|
Isis 3.0 Developer's Reference (API) |
Home |
00001 #ifndef BundleAdjust_h 00002 #define BundleAdjust_h 00003 00027 #include <QObject> // parent class 00028 00029 #include <vector> 00030 #include <fstream> 00031 00032 #include "ControlNet.h" 00033 #include "SerialNumberList.h" 00034 #include "ObservationNumberList.h" 00035 #include "Camera.h" 00036 #include "Statistics.h" 00037 #include "SpicePosition.h" 00038 #include "Progress.h" 00039 #include "CameraGroundMap.h" 00040 #include "ControlMeasure.h" 00041 #include "SparseBlockMatrix.h" 00042 00043 #include <CHOLMOD/cholmod.h> 00044 #include <CHOLMOD/UFconfig.h> 00045 00046 template< typename T > class QList; 00047 template< typename A, typename B > class QMap; 00048 00049 #if !defined(__sun__) 00050 #include "gmm/gmm.h" 00051 #endif 00052 00053 namespace Isis { 00054 class LeastSquares; 00055 class BasisFunction; 00056 00141 class BundleAdjust { 00142 public: 00143 BundleAdjust(const std::string &cnetFile, const std::string &cubeList, 00144 bool printSummary = true); 00145 BundleAdjust(const std::string &cnetFile, const std::string &cubeList, 00146 const std::string &heldList, bool printSummary = true); 00147 BundleAdjust(Isis::ControlNet &cnet, Isis::SerialNumberList &snlist, 00148 bool printSummary = true); 00149 BundleAdjust(Isis::ControlNet &cnet, Isis::SerialNumberList &snlist, 00150 Isis::SerialNumberList &heldsnlist, bool printSummary = true); 00151 ~BundleAdjust(); 00152 00153 bool ReadSCSigmas(const std::string &scsigmasList); 00154 00155 double Solve(); 00156 bool SolveCholesky(); 00157 00158 Isis::ControlNet *ControlNet() { return m_pCnet; } 00159 00160 Isis::SerialNumberList *SerialNumberList() { return m_pSnList; } 00161 int Images() const { return m_pSnList->Size(); } 00162 int Observations() const; 00163 std::string Filename(int index); 00164 bool IsHeld(int index); 00165 Table Cmatrix(int index); 00166 Table SpVector(int index); 00167 00168 void SetSolveTwist(bool solve) { m_bSolveTwist = solve; ComputeNumberPartials(); } 00169 void SetSolveRadii(bool solve) { m_bSolveRadii = solve; } 00170 void SetErrorPropagation(bool b) { m_bErrorPropagation = b; } 00171 void SetOutlierRejection(bool b) { m_bOutlierRejection = b; } 00172 void SetRejectionMultiplier(double d) { m_dRejectionMultiplier = d; } 00173 00174 void SetGlobalLatitudeAprioriSigma(double d) { m_dGlobalLatitudeAprioriSigma = d; } 00175 void SetGlobalLongitudeAprioriSigma(double d) { m_dGlobalLongitudeAprioriSigma = d; } 00176 void SetGlobalRadiiAprioriSigma(double d) { m_dGlobalRadiusAprioriSigma = d; } 00177 00178 // void SetGlobalSurfaceXAprioriSigma(double d) { m_dGlobalSurfaceXAprioriSigma = d; } 00179 // void SetGlobalSurfaceYAprioriSigma(double d) { m_dGlobalSurfaceYAprioriSigma = d; } 00180 // void SetGlobalSurfaceZAprioriSigma(double d) { m_dGlobalSurfaceZAprioriSigma = d; } 00181 // 00182 void SetGlobalSpacecraftPositionAprioriSigma(double d) { m_dGlobalSpacecraftPositionAprioriSigma = d; } 00183 void SetGlobalSpacecraftVelocityAprioriSigma(double d) { m_dGlobalSpacecraftVelocityAprioriSigma = d; } 00184 void SetGlobalSpacecraftAccelerationAprioriSigma(double d) { m_dGlobalSpacecraftAccelerationAprioriSigma = d; } 00185 00186 void SetGlobalCameraAnglesAprioriSigma(double d) { if( m_nNumberCameraCoefSolved < 1 ) return; m_dGlobalCameraAnglesAprioriSigma[0] = d; } 00187 void SetGlobalCameraAngularVelocityAprioriSigma(double d) { if( m_nNumberCameraCoefSolved < 2 ) return; m_dGlobalCameraAnglesAprioriSigma[1] = d; } 00188 void SetGlobalCameraAngularAccelerationAprioriSigma(double d) { if( m_nNumberCameraCoefSolved < 3 ) return; m_dGlobalCameraAnglesAprioriSigma[2] = d; } 00189 00190 // void SetGlobalCameraAngularVelocityAprioriSigma(double d) { m_dGlobalCameraAngularVelocityAprioriSigma = d; } 00191 // void SetGlobalCameraAngularAccelerationAprioriSigma(double d) { m_dGlobalCameraAngularAccelerationAprioriSigma = d; } 00192 00193 void SetStandardOutput(bool b) { m_bOutputStandard = b; } 00194 void SetCSVOutput(bool b) { m_bOutputCSV = b; } 00195 void SetResidualOutput(bool b) { m_bOutputResiduals = b; } 00196 void SetOutputFilePrefix(const std::string &str) { m_strOutputFilePrefix = str; } 00197 00198 enum DecompositionMethod { 00199 NoneSelected, 00200 SPECIALK, 00201 CHOLMOD, 00202 }; 00203 00204 enum CmatrixSolveType { 00205 None, 00206 AnglesOnly, 00207 AnglesVelocity, 00208 AnglesVelocityAcceleration, 00209 All 00210 }; 00211 00212 enum SpacecraftPositionSolveType { 00213 Nothing, 00214 PositionOnly, 00215 PositionVelocity, 00216 PositionVelocityAcceleration 00217 }; 00218 00219 struct SpacecraftWeights { 00220 std::string SpacecraftName; 00221 std::string InstrumentId; 00222 std::vector<double> weights; 00223 }; 00224 00225 void SetDecompositionMethod(DecompositionMethod method); 00226 00227 void SetSolveCmatrix(CmatrixSolveType type); 00228 void SetSolveSpacecraftPosition(SpacecraftPositionSolveType type) { m_spacecraftPositionSolveType = type; } 00229 00231 void SetCkDegree(int degree) { m_nckDegree = degree; } 00232 00234 void SetSolveCamDegree(int degree) { m_nsolveCamDegree = degree; } 00235 00236 int BasisColumns(); 00237 int ComputeConstrainedParameters(); 00238 00239 double Error() const { return m_dError; } 00240 double Iteration() const { return m_nIteration; } 00241 00242 // int HeldPoints() const { return m_nHeldPoints; } 00243 int IgnoredPoints() const { return m_nIgnoredPoints; } 00244 int FixedPoints() const { return m_nFixedPoints; } 00245 void SetObservationMode(bool bObservationMode); 00246 00247 void SetConvergenceThreshold(double d) { m_dConvergenceThreshold = d; } 00248 void SetMaxIterations(int n) { m_nMaxIterations = n; } 00249 void SetSolutionMethod(std::string str); 00250 00251 void GetSparseParameterCorrections(); 00252 00253 bool IsConverged() { return m_bConverged; } 00254 QString IterationSummaryGroup () const { 00255 return m_iterationSummary; 00256 } 00257 00258 private: 00259 00260 void Init(Progress *progress = 0); 00261 bool validateNetwork(); 00262 00263 void ComputeNumberPartials(); 00264 void ComputeImageParameterWeights(); 00265 void SetSpaceCraftWeights(); 00266 00267 void AddPartials(int nPointIndex); 00268 void FillPointIndexMap(); 00269 void Update(BasisFunction &basis); 00270 00271 int PointIndex(int i) const; 00272 int ImageIndex(int i) const; 00273 00274 void CheckHeldList(); 00275 void ApplyHeldList(); 00276 00277 // triangulation functions 00278 int Triangulation(bool bDoApproximation = false); 00279 bool ApproximatePoint_ClosestApproach(const ControlPoint &rpoint, int nIndex); 00280 bool TriangulatePoint(const ControlPoint &rpoint); 00281 bool TriangulationPartials(); 00282 00283 bool SetParameterWeights(); 00284 void SetPostBundleSigmas(); 00285 00286 // output functions 00287 void IterationSummary(double avErr, double sigmaXY, double sigmaHat, 00288 double sigmaX, double sigmaY); 00289 void SpecialKIterationSummary(); 00290 bool Output(); 00291 bool OutputHeader(std::ofstream& fp_out); 00292 bool OutputWithErrorPropagation(); 00293 bool OutputNoErrorPropagation(); 00294 bool OutputPointsCSV(); 00295 bool OutputImagesCSV(); 00296 bool OutputResiduals(); 00297 bool WrapUp(); 00298 bool ComputeBundleStatistics(); 00299 00301 bool m_bSolveTwist; 00302 bool m_bSolveRadii; 00303 bool m_bObservationMode; 00304 bool m_bErrorPropagation; 00305 bool m_bOutlierRejection; 00306 double m_dRejectionMultiplier; 00307 bool m_bPrintSummary; 00308 bool m_bOutputStandard; 00309 bool m_bOutputCSV; 00310 bool m_bOutputResiduals; 00311 bool m_bCleanUp; 00312 bool m_bSimulatedData; 00313 bool m_bLastIteration; 00314 bool m_bMaxIterationsReached; 00315 bool m_bDeltack; 00316 // This will become obsolete once we have a dedicated resection class. 00317 00318 int m_nIteration; 00319 int m_nMaxIterations; 00320 int m_nNumImagePartials; 00321 int m_nNumPointPartials; 00322 int m_nObservations; 00323 int m_nRejectedObservations; 00324 int m_nImageParameters; 00325 int m_nPointParameters; 00326 int m_nConstrainedPointParameters; 00327 int m_nConstrainedImageParameters; 00328 int m_nDegreesOfFreedom; 00329 int m_nHeldPoints; 00330 int m_nFixedPoints; 00331 int m_nIgnoredPoints; 00332 int m_nHeldImages; 00333 int m_nHeldObservations; 00334 int m_nckDegree; 00335 int m_nsolveCamDegree; 00336 int m_nNumberCameraCoefSolved; 00337 int m_nUnknownParameters; 00338 int m_nBasisColumns; 00339 00340 std::vector<int> m_nPointIndexMap; 00341 std::vector<int> m_nImageIndexMap; 00342 00343 double m_dError; 00344 double m_dConvergenceThreshold; 00345 double m_dElapsedTime; 00346 double m_dElapsedTimeErrorProp; 00347 double m_dSigma0; 00348 double m_drms_rx; 00349 double m_drms_ry; 00350 double m_drms_rxy; 00351 double m_drms_sigmaLat; 00352 double m_drms_sigmaLon; 00353 double m_drms_sigmaRad; 00354 double m_dminSigmaLatitude; 00355 std::string m_idMinSigmaLatitude; 00356 double m_dmaxSigmaLatitude; 00357 std::string m_idMaxSigmaLatitude; 00358 double m_dminSigmaLongitude; 00359 std::string m_idMinSigmaLongitude; 00360 double m_dmaxSigmaLongitude; 00361 std::string m_idMaxSigmaLongitude; 00362 double m_dminSigmaRadius; 00363 std::string m_idMinSigmaRadius; 00364 double m_dmaxSigmaRadius; 00365 std::string m_idMaxSigmaRadius; 00366 00367 double m_dRejectionLimit; 00368 00371 00373 double m_dGlobalLatitudeAprioriSigma; 00374 double m_dGlobalLongitudeAprioriSigma; 00375 double m_dGlobalRadiusAprioriSigma; 00376 double m_dGlobalSurfaceXAprioriSigma; 00377 double m_dGlobalSurfaceYAprioriSigma; 00378 double m_dGlobalSurfaceZAprioriSigma; 00379 00380 double m_dGlobalSpacecraftPositionAprioriSigma; 00381 double m_dGlobalSpacecraftVelocityAprioriSigma; 00382 double m_dGlobalSpacecraftAccelerationAprioriSigma; 00383 00384 std::vector<double> m_dGlobalCameraAnglesAprioriSigma; 00385 // double m_dGlobalCameraAnglesAprioriSigma; //!< camera angles apriori sigmas 00386 // double m_dGlobalCameraAngularVelocityAprioriSigma; //!< camera angular velocities apriori sigmas 00387 // double m_dGlobalCameraAngularAccelerationAprioriSigma; //!< camera angular accelerations apriori sigmas 00388 00389 double m_dGlobalSpacecraftPositionWeight; 00390 double m_dGlobalSpacecraftVelocityWeight; 00391 double m_dGlobalSpacecraftAccelerationWeight; 00392 double m_dGlobalCameraAnglesWeight; 00393 double m_dGlobalCameraAngularVelocityWeight; 00394 double m_dGlobalCameraAngularAccelerationWeight; 00395 00396 std::vector<double> m_dImageParameterWeights; 00397 00398 00399 double m_dRTM; 00400 double m_dMTR; 00401 Distance m_BodyRadii[3]; 00402 00403 std::vector<double> m_dEpsilons; 00404 std::vector<double> m_dParameterWeights; 00405 00406 std::vector<double> m_dxKnowns; 00407 std::vector<double> m_dyKnowns; 00408 00409 std::string m_strCnetFilename; 00410 std::string m_strSolutionMethod; 00411 std::string m_strOutputFilePrefix; 00412 00414 Isis::LeastSquares *m_pLsq; 00415 Isis::ControlNet *m_pCnet; 00416 Isis::SerialNumberList *m_pSnList; 00417 Isis::SerialNumberList *m_pHeldSnList; 00418 Isis::ObservationNumberList *m_pObsNumList; 00419 00421 Statistics m_Statsx; 00422 Statistics m_Statsy; 00423 Statistics m_Statsrx; 00424 Statistics m_Statsry; 00425 Statistics m_Statsrxy; 00426 00427 std::vector<Statistics> m_rmsImageSampleResiduals; 00428 std::vector<Statistics> m_rmsImageLineResiduals; 00429 std::vector<Statistics> m_rmsImageResiduals; 00430 std::vector<Statistics> m_rmsImageXSigmas; 00431 std::vector<Statistics> m_rmsImageYSigmas; 00432 std::vector<Statistics> m_rmsImageZSigmas; 00433 std::vector<Statistics> m_rmsImageRASigmas; 00434 std::vector<Statistics> m_rmsImageDECSigmas; 00435 std::vector<Statistics> m_rmsImageTWISTSigmas; 00436 00437 DecompositionMethod m_decompositionMethod; 00438 00439 CmatrixSolveType m_cmatrixSolveType; 00440 SpacecraftPositionSolveType m_spacecraftPositionSolveType; 00441 00442 std::vector<SpacecraftWeights> m_SCWeights; 00443 00444 // beyond this place (there be dragons) all refers to the folded bundle solution (referred to as 'SpecialK' 00445 // in the interim; there is no dependence on the least-squares class 00446 00447 private: 00448 int m_nRank; 00449 00450 bool m_bConverged; 00451 bool m_bError; 00452 00453 boost::numeric::ublas::symmetric_matrix < double, 00454 boost::numeric::ublas::upper, boost::numeric::ublas::column_major > 00455 m_Normals; 00456 // symmetric_matrix<double,lower> m_Normals; //!< reduced normal equations matrix 00457 boost::numeric::ublas::vector< double > m_nj; 00458 00460 std::vector< boost::numeric::ublas::compressed_matrix< double> > m_Qs_SPECIALK; 00461 std::vector< SparseBlockRowMatrix > m_Qs_CHOLMOD; 00462 00463 // vector<bounded_vector<double,3> > m_NICs; //!< array of NICs (see Brown, 1976) 00465 std::vector< boost::numeric::ublas::bounded_vector< double, 3 > > m_NICs; 00466 00467 boost::numeric::ublas::vector<double> m_Image_Corrections; 00468 boost::numeric::ublas::vector<double> m_Image_Solution; 00469 00470 // vector<bounded_vector<double,3> > m_Point_Corrections; //!< vector of corrections to 3D point parameter 00471 std::vector< boost::numeric::ublas::bounded_vector< double, 3 > > m_Point_Corrections; 00472 std::vector< boost::numeric::ublas::bounded_vector< double, 3 > > m_Point_AprioriSigmas; 00473 std::vector< boost::numeric::ublas::bounded_vector< double, 3 > > m_Point_Weights; 00474 00475 void Initialize(); 00476 bool InitializePointWeights(); 00477 void InitializePoints(); 00478 00479 QString m_iterationSummary; 00480 00481 bool formNormalEquations(); 00482 void applyParameterCorrections(); 00483 bool errorPropagation(); 00484 bool solveSystem(); 00485 00486 // solution, error propagation, and matrix methods for cholmod approach 00487 bool formNormalEquations_CHOLMOD(); 00488 00489 bool formNormals1_CHOLMOD(boost::numeric::ublas::symmetric_matrix<double, boost::numeric::ublas::upper>&N22, 00490 SparseBlockColumnMatrix& N12, 00491 boost::numeric::ublas::compressed_vector<double>& n1, 00492 boost::numeric::ublas::vector<double>& n2, 00493 boost::numeric::ublas::matrix<double>& coeff_image, 00494 boost::numeric::ublas::matrix<double>& coeff_point3D, 00495 boost::numeric::ublas::vector<double>& coeff_RHS, int nImageIndex); 00496 00497 bool formNormals2_CHOLMOD(boost::numeric::ublas::symmetric_matrix<double, boost::numeric::ublas::upper>&N22, 00498 SparseBlockColumnMatrix& N12, 00499 boost::numeric::ublas::vector<double>& n2, 00500 boost::numeric::ublas::vector<double>& nj, int nPointIndex, int i); 00501 00502 bool formNormals3_CHOLMOD(boost::numeric::ublas::compressed_vector<double>& n1, 00503 boost::numeric::ublas::vector<double>& nj); 00504 00505 bool solveSystem_CHOLMOD(); 00506 00507 void AmultAdd_CNZRows_CHOLMOD(double alpha, SparseBlockColumnMatrix& A, 00508 SparseBlockRowMatrix& B); 00509 00510 void transA_NZ_multAdd_CHOLMOD(double alpha, SparseBlockRowMatrix &A, 00511 boost::numeric::ublas::vector< double > &B, 00512 boost::numeric::ublas::vector< double > &C); 00513 00514 void applyParameterCorrections_CHOLMOD(); 00515 00516 bool errorPropagation_CHOLMOD(); 00517 00518 // solution, error propagation, and matrix methods for specialk approach 00519 // TODO: this may be able to go away if I can verify cholmod behaviour for 00520 // a truly dense matrix 00521 bool formNormalEquations_SPECIALK(); 00522 00523 bool formNormals1_SPECIALK(boost::numeric::ublas::symmetric_matrix<double, boost::numeric::ublas::upper>&N22, 00524 boost::numeric::ublas::matrix<double>& N12, 00525 boost::numeric::ublas::compressed_vector<double>& n1, 00526 boost::numeric::ublas::vector<double>& n2, 00527 boost::numeric::ublas::matrix<double>& coeff_image, 00528 boost::numeric::ublas::matrix<double>& coeff_point3D, 00529 boost::numeric::ublas::vector<double>& coeff_RHS, int nImageIndex); 00530 00531 bool formNormals2_SPECIALK(boost::numeric::ublas::symmetric_matrix<double, boost::numeric::ublas::upper>&N22, 00532 boost::numeric::ublas::matrix<double>& N12, 00533 boost::numeric::ublas::vector<double>& n2, 00534 boost::numeric::ublas::vector<double>& nj, int nPointIndex, int i); 00535 00536 bool formNormals3_SPECIALK(boost::numeric::ublas::compressed_vector<double>& n1, 00537 boost::numeric::ublas::vector<double>& nj); 00538 00539 bool solveSystem_SPECIALK(); 00540 00541 void AmultAdd_CNZRows_SPECIALK(double alpha, 00542 boost::numeric::ublas::matrix< double > &A, 00543 boost::numeric::ublas::compressed_matrix< double > &B, 00544 boost::numeric::ublas::symmetric_matrix < double, 00545 boost::numeric::ublas::upper, 00546 boost::numeric::ublas::column_major > &C); 00547 00548 void transA_NZ_multAdd_SPECIALK(double alpha, 00549 boost::numeric::ublas::compressed_matrix< double > &A, 00550 boost::numeric::ublas::vector< double > &B, 00551 boost::numeric::ublas::vector< double > &C); 00552 00553 void applyParameterCorrections_SPECIALK(); 00554 00555 bool errorPropagation_SPECIALK(); 00556 00557 bool CholeskyUT_NOSQR(); 00558 bool CholeskyUT_NOSQR_Inverse(); 00559 bool CholeskyUT_NOSQR_BackSub( 00560 boost::numeric::ublas::symmetric_matrix < double, 00561 boost::numeric::ublas::upper, 00562 boost::numeric::ublas::column_major > &m, 00563 boost::numeric::ublas::vector< double > &s, 00564 boost::numeric::ublas::vector< double > &rhs); 00565 00566 bool ComputePartials_DC(boost::numeric::ublas::matrix<double>& coeff_image, 00567 boost::numeric::ublas::matrix<double>& coeff_point3D, 00568 boost::numeric::ublas::vector<double>& coeff_RHS, 00569 const ControlMeasure &measure, const ControlPoint &point); 00570 00571 // bool CholeskyUT_NOSQR_BackSub(symmetric_matrix<double,lower>& m, vector<double>& s, vector<double>& rhs); 00572 double ComputeResiduals(); 00573 00574 // dedicated matrix functions 00575 void AmulttransBNZ(boost::numeric::ublas::matrix<double>& A, 00576 boost::numeric::ublas::compressed_matrix<double>& B, 00577 boost::numeric::ublas::matrix<double> &C, double alpha=1.0); 00578 void ANZmultAdd(boost::numeric::ublas::compressed_matrix<double>& A, 00579 boost::numeric::ublas::symmetric_matrix < double, 00580 boost::numeric::ublas::upper,boost::numeric::ublas::column_major >& B, 00581 boost::numeric::ublas::matrix<double>& C, double alpha=1.0) ; 00582 00583 bool Invert_3x3(boost::numeric::ublas::symmetric_matrix < double, 00584 boost::numeric::ublas::upper > &m); 00585 00586 bool product_ATransB(boost::numeric::ublas::symmetric_matrix < double, 00587 boost::numeric::ublas::upper > &N22, SparseBlockColumnMatrix& N12, 00588 SparseBlockRowMatrix& Q); 00589 void product_AV(double alpha, boost::numeric::ublas::bounded_vector< double,3 >& v2, 00590 SparseBlockRowMatrix& Q, boost::numeric::ublas::vector< double >& v1); 00591 00592 bool ComputeRejectionLimit(); 00593 bool FlagOutliers(); 00594 00595 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 00596 // variables and methods for cholmod 00597 cholmod_common m_cm; 00598 cholmod_factor *m_L; 00599 cholmod_sparse* m_N; 00600 00601 SparseBlockMatrix m_SparseNormals; 00602 cholmod_triplet* m_pTriplet; 00603 00604 bool initializeCholMod(); 00605 bool freeCholMod(); 00606 bool cholmod_Inverse(); 00607 bool loadCholmodTriplet(); 00608 }; 00609 00610 } 00611 00612 #endif 00613