29 #include "boost/numeric/ublas/matrix_sparse.hpp"
30 #include "boost/numeric/ublas/vector_proxy.hpp"
31 #include "boost/lexical_cast.hpp"
33 #include "boost/numeric/ublas/io.hpp"
35 using namespace boost::numeric::ublas;
39 static void cholmod_error_handler(
int nStatus,
const char* file,
int nLineNo,
40 const char* message) {
49 gp += PvlKeyword(
"File",file);
50 gp += PvlKeyword(
"Line_Number",
toString(nLineNo));
51 gp += PvlKeyword(
"Status",
toString(nStatus));
55 errlog +=
". (See print.prt for details)";
56 throw IException(IException::Unknown, errlog,
_FILEINFO_);
60 BundleAdjust::BundleAdjust(
const QString &cnetFile,
61 const QString &cubeList,
68 m_bPrintSummary = bPrintSummary;
69 m_strCnetFileName = cnetFile;
70 m_strOutputFilePrefix =
"";
76 m_cumProRes =
new StatCumProbDistDynCalc;
77 m_cumProRes->initialize(101);
80 m_wFunc[0]=m_wFunc[1]=m_wFunc[2]=NULL;
81 m_maxLikelihoodFlag[0]=m_maxLikelihoodFlag[1]=m_maxLikelihoodFlag[2]=
false;
83 m_maxLikelihoodIndex=0;
84 m_maxLikelihoodMedianR2Residuals=0.0;
88 BundleAdjust::BundleAdjust(
const QString &cnetFile,
89 const QString &cubeList,
90 const QString &heldList,
97 m_bPrintSummary = bPrintSummary;
98 m_strCnetFileName = cnetFile;
99 m_strOutputFilePrefix =
"";
105 m_cumProRes =
new StatCumProbDistDynCalc;
106 m_cumProRes->initialize(101);
109 m_wFunc[0]=m_wFunc[1]=m_wFunc[2]=NULL;
110 m_maxLikelihoodFlag[0]=m_maxLikelihoodFlag[1]=m_maxLikelihoodFlag[2]=
false;
112 m_maxLikelihoodIndex=0;
118 bool bPrintSummary) {
122 m_pHeldSnList = NULL;
123 m_bPrintSummary = bPrintSummary;
124 m_dConvergenceThreshold = 0.;
125 m_strCnetFileName =
"";
126 m_strOutputFilePrefix =
"";
132 m_cumProRes =
new StatCumProbDistDynCalc;
133 m_cumProRes->initialize(101);
136 m_wFunc[0]=m_wFunc[1]=m_wFunc[2]=NULL;
137 m_maxLikelihoodFlag[0]=m_maxLikelihoodFlag[1]=m_maxLikelihoodFlag[2]=
false;
139 m_maxLikelihoodIndex=0;
146 bool bPrintSummary) {
150 m_pHeldSnList = &heldsnlist;
151 m_bPrintSummary = bPrintSummary;
152 m_strCnetFileName =
"";
153 m_strOutputFilePrefix =
"";
159 m_cumProRes =
new StatCumProbDistDynCalc;
160 m_cumProRes->initialize(101);
163 m_wFunc[0]=m_wFunc[1]=m_wFunc[2]=NULL;
164 m_maxLikelihoodFlag[0]=m_maxLikelihoodFlag[1]=m_maxLikelihoodFlag[2]=
false;
166 m_maxLikelihoodIndex=0;
170 BundleAdjust::~BundleAdjust() {
176 if (m_nHeldImages > 0)
177 delete m_pHeldSnList;
179 if (m_bObservationMode)
180 delete m_pObsNumList;
186 if ( m_strSolutionMethod ==
"SPARSE" )
215 m_bSimulatedData =
true;
216 m_bObservationMode =
false;
217 m_strSolutionMethod =
"SPECIALK";
218 m_pObsNumList = NULL;
220 m_dElapsedTimeErrorProp = 0.0;
221 m_dElapsedTime = 0.0;
222 m_dRejectionMultiplier = 3.;
225 m_pCnet->SetImages(*m_pSnList, progress);
228 m_pCnet->ClearJigsawRejected();
231 int nImages = m_pSnList->Size();
234 if (m_pHeldSnList != NULL) {
240 for (
int i = 0; i < nImages; i++ ) {
241 if ( m_pHeldSnList->HasSerialNumber(m_pSnList->SerialNumber(i)) )
244 m_nImageIndexMap.push_back(count);
249 for (
int i = 0; i < nImages; i++)
250 m_nImageIndexMap.push_back(i);
256 m_bSolveTwist =
true;
257 m_bSolveRadii =
false;
258 m_bUpdateCubes =
false;
259 m_bErrorPropagation =
false;
260 m_bMaxIterationsReached =
false;
261 m_cmatrixSolveType = AnglesOnly;
262 m_spacecraftPositionSolveType = Nothing;
264 m_nsolveCKDegree = m_nCKDegree;
266 m_nsolveSPKDegree = m_nSPKDegree;
267 m_nNumberCamAngleCoefSolved = 1;
268 m_nNumberCamPosCoefSolved = 1;
269 m_nUnknownParameters = 0;
270 m_bOutputStandard =
true;
272 m_bOutputResiduals =
true;
273 m_nPositionType = SpicePosition::PolyFunction;
274 m_nPointingType = SpiceRotation::PolyFunction;
275 m_bSolvePolyOverPointing =
false;
276 m_bSolvePolyOverHermite =
false;
278 m_idMinSigmaLatitude =
"";
279 m_idMaxSigmaLatitude =
"";
280 m_idMinSigmaLongitude =
"";
281 m_idMaxSigmaLongitude =
"";
282 m_idMinSigmaRadius =
"";
283 m_idMaxSigmaRadius =
"";
285 m_dmaxSigmaLatitude = 0.0;
286 m_dmaxSigmaLongitude = 0.0;
287 m_dmaxSigmaRadius = 0.0;
289 m_dminSigmaLatitude = 1.0e+12;
290 m_dminSigmaLongitude = 1.0e+12;
291 m_dminSigmaRadius = 1.0e+12;
293 m_dGlobalLatitudeAprioriSigma = 1000.0;
294 m_dGlobalLongitudeAprioriSigma = 1000.0;
295 m_dGlobalRadiusAprioriSigma = 1000.0;
307 m_dGlobalSpacecraftPositionWeight = 0.0;
308 m_dGlobalSpacecraftVelocityWeight = 0.0;
309 m_dGlobalSpacecraftAccelerationWeight = 0.0;
310 m_dGlobalCameraAnglesWeight = 0.0;
311 m_dGlobalCameraAngularVelocityWeight = 0.0;
312 m_dGlobalCameraAngularAccelerationWeight = 0.0;
314 m_dConvergenceThreshold = 1.0e-10;
315 m_nRejectedObservations = 0;
318 m_dGlobalRadiusAprioriSigma *= -1.0;
326 m_BodyRadii[0] = m_BodyRadii[1] = m_BodyRadii[2] =
Distance();
329 pCamera->
radii(m_BodyRadii);
333 if (m_BodyRadii[0] >=
Distance(0, Distance::Meters)) {
334 m_dMTR = 0.001 / m_BodyRadii[0].kilometers();
335 m_dRTM = 1.0 / m_dMTR;
362 bool BundleAdjust::validateNetwork() {
363 printf(
"Validating network...\n");
366 int nimagesWithInsufficientMeasures = 0;
367 QString msg =
"Images with one or less measures:\n";
368 int nImages = m_pSnList->Size();
369 for (
int i = 0; i < nImages; i++) {
370 int nMeasures = m_pCnet->GetNumberOfValidMeasuresInImage(m_pSnList->SerialNumber(i));
375 nimagesWithInsufficientMeasures++;
376 msg += m_pSnList->FileName(i) +
": " +
toString(nMeasures) +
"\n";
378 if ( nimagesWithInsufficientMeasures > 0 ) {
382 printf(
"Validation complete!...\n");
390 bool BundleAdjust::initializeCholMod() {
397 cholmod_start(&m_cm);
400 m_cm.error_handler = cholmod_error_handler;
404 m_cm.method[0].ordering = CHOLMOD_AMD;
407 m_SparseNormals.setNumberOfColumns( Observations() );
416 bool BundleAdjust::freeCholMod() {
418 cholmod_free_triplet(&m_pTriplet, &m_cm);
419 cholmod_free_sparse(&m_N, &m_cm);
420 cholmod_free_factor(&m_L, &m_cm);
422 cholmod_finish(&m_cm);
432 void BundleAdjust::FillPointIndexMap() {
436 m_nFixedPoints = m_nIgnoredPoints = 0;
438 int nObjectPoints = m_pCnet->GetNumPoints();
440 for (
int i = 0; i < nObjectPoints; i++) {
443 if (point->IsIgnored()) {
444 m_nPointIndexMap.push_back(-1);
449 else if (point->
GetType() == ControlPoint::Fixed) {
452 if ( m_strSolutionMethod ==
"SPECIALK" ||
453 m_strSolutionMethod ==
"SPARSE" ||
454 m_strSolutionMethod ==
"OLDSPARSE" ) {
455 m_nPointIndexMap.push_back(count);
459 m_nPointIndexMap.push_back(-1);
463 m_nPointIndexMap.push_back(count);
474 void BundleAdjust::CheckHeldList() {
475 for (
int ih = 0; ih < m_pHeldSnList->Size(); ih++) {
476 if (!(m_pSnList->HasSerialNumber(m_pHeldSnList->SerialNumber(ih)))) {
477 QString msg =
"Held image [" + m_pHeldSnList->SerialNumber(ih)
490 void BundleAdjust::ComputeNumberPartials() {
491 m_nNumImagePartials = 0;
493 if (m_cmatrixSolveType != None) {
495 m_nNumImagePartials = 2;
499 m_nNumImagePartials++;
502 m_nNumImagePartials *= m_nNumberCamAngleCoefSolved;
505 if (m_spacecraftPositionSolveType != Nothing) {
516 m_nNumImagePartials += 3.0*m_nNumberCamPosCoefSolved;
525 m_nNumPointPartials = 3;
528 if ( m_strSolutionMethod !=
"SPECIALK" &&
529 m_strSolutionMethod !=
"SPARSE" &&
530 m_strSolutionMethod !=
"OLDSPARSE" ) {
531 m_nNumPointPartials = 2;
534 m_nNumPointPartials++;
542 void BundleAdjust::ComputeImageParameterWeights() {
544 m_dImageParameterWeights.resize(m_nNumImagePartials);
545 for (
int i = 0; i < m_nNumImagePartials; i++)
546 m_dImageParameterWeights[i] = 0.0;
549 if (m_spacecraftPositionSolveType == PositionOnly) {
550 m_dImageParameterWeights[0] = m_dGlobalSpacecraftPositionWeight;
551 m_dImageParameterWeights[1] = m_dGlobalSpacecraftPositionWeight;
552 m_dImageParameterWeights[2] = m_dGlobalSpacecraftPositionWeight;
555 else if (m_spacecraftPositionSolveType == PositionVelocity) {
556 m_dImageParameterWeights[0] = m_dGlobalSpacecraftPositionWeight;
557 m_dImageParameterWeights[1] = m_dGlobalSpacecraftVelocityWeight;
558 m_dImageParameterWeights[2] = m_dGlobalSpacecraftPositionWeight;
559 m_dImageParameterWeights[3] = m_dGlobalSpacecraftVelocityWeight;
560 m_dImageParameterWeights[4] = m_dGlobalSpacecraftPositionWeight;
561 m_dImageParameterWeights[5] = m_dGlobalSpacecraftVelocityWeight;
564 else if (m_spacecraftPositionSolveType >= PositionVelocityAcceleration) {
565 m_dImageParameterWeights[0] = m_dGlobalSpacecraftPositionWeight;
566 m_dImageParameterWeights[1] = m_dGlobalSpacecraftVelocityWeight;
567 m_dImageParameterWeights[2] = m_dGlobalSpacecraftAccelerationWeight;
568 m_dImageParameterWeights[3] = m_dGlobalSpacecraftPositionWeight;
569 m_dImageParameterWeights[4] = m_dGlobalSpacecraftVelocityWeight;
570 m_dImageParameterWeights[5] = m_dGlobalSpacecraftAccelerationWeight;
571 m_dImageParameterWeights[6] = m_dGlobalSpacecraftPositionWeight;
572 m_dImageParameterWeights[7] = m_dGlobalSpacecraftVelocityWeight;
573 m_dImageParameterWeights[8] = m_dGlobalSpacecraftAccelerationWeight;
577 if (m_cmatrixSolveType == AnglesOnly) {
578 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAnglesWeight;
579 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAnglesWeight;
582 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAnglesWeight;
584 else if (m_cmatrixSolveType == AnglesVelocity) {
585 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAnglesWeight;
586 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAngularVelocityWeight;
587 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAnglesWeight;
588 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAngularVelocityWeight;
590 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAnglesWeight;
591 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAngularVelocityWeight;
594 if (m_cmatrixSolveType >= AnglesVelocityAcceleration) {
595 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAnglesWeight;
596 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAngularVelocityWeight;
597 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAngularAccelerationWeight;
598 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAnglesWeight;
599 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAngularVelocityWeight;
600 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAngularAccelerationWeight;
603 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAnglesWeight;
604 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAngularVelocityWeight;
605 m_dImageParameterWeights[nIndex++] = m_dGlobalCameraAngularAccelerationWeight;
617 void BundleAdjust::SetObservationMode(
bool observationMode) {
618 m_bObservationMode = observationMode;
620 if (!m_bObservationMode)
626 if (m_pHeldSnList == NULL)
630 for (
int ih = 0; ih < m_pHeldSnList->Size(); ih++) {
631 for (
int isn = 0; isn < m_pSnList->Size(); isn++) {
632 if (m_pHeldSnList->ObservationNumber(ih) != m_pSnList->ObservationNumber(isn))
635 if (m_pHeldSnList->HasSerialNumber(m_pSnList->SerialNumber(isn)))
638 QString msg =
"Cube file " + m_pSnList->FileName(isn)
639 +
" must be held since it is on the same observation as held cube "
640 + m_pHeldSnList->FileName(ih);
652 void BundleAdjust::SetDecompositionMethod(DecompositionMethod method) {
653 m_decompositionMethod = method;
660 void BundleAdjust::SetSolveCmatrix(CmatrixSolveType type) {
661 m_cmatrixSolveType = type;
664 case BundleAdjust::AnglesOnly:
665 m_nNumberCamAngleCoefSolved = 1;
667 case BundleAdjust::AnglesVelocity:
668 m_nNumberCamAngleCoefSolved = 2;
670 case BundleAdjust::AnglesVelocityAcceleration:
671 m_nNumberCamAngleCoefSolved = 3;
673 case BundleAdjust::CKAll:
674 m_nNumberCamAngleCoefSolved = m_nsolveCKDegree + 1;
677 m_nNumberCamAngleCoefSolved = 0;
681 m_dGlobalCameraAnglesAprioriSigma.resize(m_nNumberCamAngleCoefSolved);
682 for(
int i = 0; i < m_nNumberCamAngleCoefSolved; i++ )
683 m_dGlobalCameraAnglesAprioriSigma[i] = -1.0;
687 if (m_nNumberCamAngleCoefSolved > m_nsolveCKDegree + 1) {
688 QString msg =
"Selected SolveCameraDegree " +
toString(m_nsolveCKDegree)
689 +
" is not sufficient for the CAMSOLVE";
698 void BundleAdjust::SetSolveSpacecraftPosition(SpacecraftPositionSolveType type) {
699 m_spacecraftPositionSolveType = type;
702 case BundleAdjust::PositionOnly:
703 m_nNumberCamPosCoefSolved = 1;
705 case BundleAdjust::PositionVelocity:
706 m_nNumberCamPosCoefSolved = 2;
708 case BundleAdjust::PositionVelocityAcceleration:
709 m_nNumberCamPosCoefSolved = 3;
711 case BundleAdjust::SPKAll:
712 m_nNumberCamPosCoefSolved = m_nsolveSPKDegree + 1;
715 m_nNumberCamPosCoefSolved = 0;
719 m_dGlobalSpacecraftPositionAprioriSigma.resize(m_nNumberCamPosCoefSolved);
720 for(
int i = 0; i < m_nNumberCamPosCoefSolved; i++ )
721 m_dGlobalSpacecraftPositionAprioriSigma[i] = -1.0;
725 if (m_nNumberCamPosCoefSolved > m_nsolveSPKDegree + 1) {
726 QString msg =
"Selected SolveCameraPositionDegree " +
toString(m_nsolveSPKDegree)
727 +
" is not sufficient for the CAMSOLVE";
738 int BundleAdjust::BasisColumns() {
739 m_nImageParameters = Observations() * m_nNumImagePartials;
741 int nPointParameterColumns = m_pCnet->GetNumValidPoints() * m_nNumPointPartials;
743 if (m_strSolutionMethod !=
"SPECIALK" &&
744 m_strSolutionMethod !=
"SPARSE" &&
745 m_strSolutionMethod !=
"OLDSPARSE")
746 nPointParameterColumns -= m_nFixedPoints * m_nNumPointPartials;
748 return m_nImageParameters + nPointParameterColumns;
755 void BundleAdjust::Initialize() {
758 m_nRank = m_nNumImagePartials * Observations();
760 int n3DPoints = m_pCnet->GetNumValidPoints();
762 if ( m_decompositionMethod == SPECIALK ) {
763 m_Normals.resize(m_nRank);
765 m_Qs_SPECIALK.resize(n3DPoints);
767 else if ( m_decompositionMethod == CHOLMOD ) {
768 m_Qs_CHOLMOD.resize(n3DPoints);
771 m_nUnknownParameters = m_nRank + 3 * n3DPoints;
773 m_nRejectedObservations = 0;
775 m_Image_Solution.resize(m_nRank);
776 m_Image_Corrections.resize(m_nRank);
777 m_NICs.resize(n3DPoints);
778 m_Point_Corrections.resize(n3DPoints);
779 m_Point_Weights.resize(n3DPoints);
780 m_Point_AprioriSigmas.resize(n3DPoints);
783 for (
int i = 0; i < n3DPoints; i++) {
787 if ( m_decompositionMethod == SPECIALK )
788 m_Qs_SPECIALK[i].clear();
789 else if ( m_decompositionMethod == CHOLMOD )
790 m_Qs_CHOLMOD[i].clear();
792 m_Point_Corrections[i].clear();
793 m_Point_Weights[i].clear();
794 m_Point_AprioriSigmas[i].clear();
797 m_bConverged =
false;
801 SetSpaceCraftWeights();
805 if ( m_strSolutionMethod ==
"SPARSE" )
809 void BundleAdjust::SetSpaceCraftWeights() {
826 if ( m_nNumberCamPosCoefSolved >= 1 ) {
827 if ( m_dGlobalSpacecraftPositionAprioriSigma[0] > 0.0 ) {
828 m_dGlobalSpacecraftPositionWeight
829 = 1.0 / (m_dGlobalSpacecraftPositionAprioriSigma[0] * m_dGlobalSpacecraftPositionAprioriSigma[0] * 1.0e-6);
833 if ( m_nNumberCamPosCoefSolved >= 2 ) {
834 if ( m_dGlobalSpacecraftPositionAprioriSigma[1] > 0.0 ) {
835 m_dGlobalSpacecraftVelocityWeight
836 = 1.0 / (m_dGlobalSpacecraftPositionAprioriSigma[1] * m_dGlobalSpacecraftPositionAprioriSigma[1] * 1.0e-6);
840 if ( m_nNumberCamPosCoefSolved >= 3 ) {
841 if( m_dGlobalSpacecraftPositionAprioriSigma[2] > 0.0 ) {
842 m_dGlobalSpacecraftAccelerationWeight
843 = 1.0 / (m_dGlobalSpacecraftPositionAprioriSigma[2] * m_dGlobalSpacecraftPositionAprioriSigma[2] * 1.0e-6);
847 if ( m_nNumberCamAngleCoefSolved >= 1 ) {
848 if ( m_dGlobalCameraAnglesAprioriSigma[0] > 0.0 ) {
849 m_dGlobalCameraAnglesWeight
850 = 1.0 / (m_dGlobalCameraAnglesAprioriSigma[0] * m_dGlobalCameraAnglesAprioriSigma[0] *
DEG2RAD *
DEG2RAD);
854 if ( m_nNumberCamAngleCoefSolved >= 2 ) {
855 if ( m_dGlobalCameraAnglesAprioriSigma[1] > 0.0 ) {
856 m_dGlobalCameraAngularVelocityWeight
857 = 1.0 / (m_dGlobalCameraAnglesAprioriSigma[1] * m_dGlobalCameraAnglesAprioriSigma[1] *
DEG2RAD *
DEG2RAD);
861 if ( m_nNumberCamAngleCoefSolved >= 3 ) {
862 if( m_dGlobalCameraAnglesAprioriSigma[2] > 0.0 ) {
863 m_dGlobalCameraAngularAccelerationWeight
864 = 1.0 / (m_dGlobalCameraAnglesAprioriSigma[2] * m_dGlobalCameraAnglesAprioriSigma[2] *
DEG2RAD *
DEG2RAD);
883 bool BundleAdjust::SolveCholesky() {
898 std::vector<int> observationInitialValueIndex;
902 ComputeNumberPartials();
904 if (m_bObservationMode)
905 observationInitialValueIndex.assign(m_pObsNumList->ObservationSize(), -1);
909 for (
int i = 0; i < Images(); i++) {
912 if (m_bObservationMode) {
914 oIndex = m_pObsNumList->ObservationNumberMapIndex(oIndex);
915 iIndex = observationInitialValueIndex[oIndex];
918 if (m_cmatrixSolveType != None) {
924 if (!m_bObservationMode) {
932 SpiceRotation *pOrot = m_pCnet->Camera(iIndex)->instrumentRotation();
933 std::vector<double> anglePoly1, anglePoly2, anglePoly3;
939 pSpiceRot->
SetPolynomial(anglePoly1, anglePoly2, anglePoly3, m_nPointingType);
946 observationInitialValueIndex[oIndex] = i;
951 if (m_spacecraftPositionSolveType != Nothing) {
955 if (!m_bObservationMode) {
963 SpicePosition *pOpos = m_pCnet->Camera(iIndex)->instrumentPosition();
964 std::vector<double> posPoly1, posPoly2, posPoly3;
971 pSpicePos->
SetPolynomial(posPoly1, posPoly2, posPoly3, m_nPositionType);
978 observationInitialValueIndex[oIndex] = i;
986 ComputeImageParameterWeights();
989 m_pCnet->ComputeApriori();
991 InitializePointWeights();
999 clock_t t1 = clock();
1002 double dSigma0_previous = 0.0;
1007 printf(
"starting iteration %d\n", m_nIteration);
1008 clock_t iterationclock1 = clock();
1014 if (m_nIteration != 1) {
1015 if ( m_decompositionMethod == SPECIALK )
1017 else if ( m_decompositionMethod == CHOLMOD )
1018 m_SparseNormals.zeroBlocks();
1025 if (!formNormalEquations()) {
1026 m_bConverged =
false;
1038 if (!solveSystem()) {
1039 printf(
"solve failed!\n");
1040 m_bConverged =
false;
1051 applyParameterCorrections();
1059 dvtpv = ComputeResiduals();
1065 if ( m_bOutlierRejection ) {
1066 ComputeRejectionLimit();
1071 m_nDegreesOfFreedom =
1072 m_nObservations + (m_nConstrainedPointParameters + m_nConstrainedImageParameters) - m_nUnknownParameters;
1074 if (m_nDegreesOfFreedom > 0)
1075 m_dSigma0 = dvtpv / m_nDegreesOfFreedom;
1076 else if (m_bDeltack && m_nDegreesOfFreedom == 0)
1079 QString msg =
"Degrees of Freedom " +
toString(m_nDegreesOfFreedom)
1080 +
" is invalid (<= 0)!";
1084 m_dSigma0 = sqrt(m_dSigma0);
1086 printf(
"Iteration: %d\nSigma0: %20.10lf\n", m_nIteration, m_dSigma0);
1087 printf(
"Observations: %d\nConstrained Parameters:%d\nUnknowns: %d\nDegrees of Freedom: %d\n",
1088 m_nObservations, m_nConstrainedPointParameters, m_nUnknownParameters, m_nDegreesOfFreedom);
1091 if ( !m_bDeltack ) {
1092 if (fabs(dSigma0_previous - m_dSigma0) <= m_dConvergenceThreshold) {
1094 if (m_maxLikelihoodIndex+1 < 3 && m_maxLikelihoodFlag[m_maxLikelihoodIndex+1]) {
1095 m_maxLikelihoodIndex++;
1098 m_bLastIteration =
true;
1099 m_bConverged =
true;
1100 printf(
"Bundle has converged\n");
1107 int numimgparam = m_Image_Solution.size();
1108 for (
int ij = 0; ij < numimgparam; ij++) {
1109 if (fabs(m_Image_Solution(ij)) > m_dConvergenceThreshold)
1115 if ( nconverged == numimgparam ) {
1116 m_bConverged =
true;
1117 m_bLastIteration =
true;
1118 printf(
"Deltack Bundle has converged\n");
1123 printf(
"Maximum Likelihood Tier: %d\n",m_maxLikelihoodIndex);
1124 if (m_maxLikelihoodFlag[m_maxLikelihoodIndex]) {
1127 m_wFunc[m_maxLikelihoodIndex]->setTweakingConstant(m_cumPro->value(m_maxLikelihoodQuan[m_maxLikelihoodIndex]));
1129 m_maxLikelihoodMedianR2Residuals = m_cumPro->value(0.5);
1130 printf(
"Median of R^2 residuals: %lf\n",m_maxLikelihoodMedianR2Residuals);
1132 m_cumPro->initialize(101);
1135 clock_t iterationclock2 = clock();
1136 double dIterationTime = ((iterationclock2 - iterationclock1) / (
double)CLOCKS_PER_SEC);
1137 printf(
"End of Iteration %d\nElapsed Time: %20.10lf\n", m_nIteration, dIterationTime);
1143 if (m_nIteration >= m_nMaxIterations) {
1144 m_bMaxIterationsReached =
true;
1150 m_cumProRes->initialize(101);
1154 if ( m_decompositionMethod == CHOLMOD ) {
1155 if (!m_bConverged || (m_bConverged && !m_bErrorPropagation))
1156 cholmod_free_factor(&m_L, &m_cm);
1159 SpecialKIterationSummary();
1163 dSigma0_previous = m_dSigma0;
1166 if ( m_bConverged && m_bErrorPropagation ) {
1167 clock_t terror1 = clock();
1168 printf(
"\nStarting Error Propagation");
1170 printf(
"\n\nError Propagation Complete\n");
1171 clock_t terror2 = clock();
1172 m_dElapsedTimeErrorProp = ((terror2 - terror1) / (
double)CLOCKS_PER_SEC);
1175 clock_t t2 = clock();
1176 m_dElapsedTime = ((t2 - t1) / (
double)CLOCKS_PER_SEC);
1180 printf(
"\nGenerating report files\n");
1183 printf(
"\nBundle complete\n");
1185 SpecialKIterationSummary();
1189 QString msg =
"Need to return something here, or just change the whole darn thing? [";
1199 bool BundleAdjust::formNormalEquations() {
1200 if ( m_decompositionMethod == CHOLMOD )
1201 return formNormalEquations_CHOLMOD();
1203 return formNormalEquations_SPECIALK();
1212 bool BundleAdjust::solveSystem() {
1213 if ( m_decompositionMethod == CHOLMOD )
1214 return solveSystem_CHOLMOD();
1216 return solveSystem_SPECIALK();
1225 bool BundleAdjust::formNormalEquations_CHOLMOD() {
1232 bool bStatus =
false;
1234 m_nObservations = 0;
1235 m_nConstrainedPointParameters = 0;
1237 static matrix<double> coeff_image;
1238 static matrix<double> coeff_point3D(2, 3);
1239 static vector<double> coeff_RHS(2);
1240 static symmetric_matrix<double, upper> N22(3);
1244 static vector<double> n2(3);
1245 compressed_vector<double> n1(m_nRank);
1248 m_nj.resize(m_nRank);
1250 coeff_image.resize(2, m_nNumImagePartials);
1260 coeff_point3D.clear();
1266 int nGood3DPoints = 0;
1267 int nRejected3DPoints = 0;
1268 int nPointIndex = 0;
1270 int n3DPoints = m_pCnet->GetNumPoints();
1279 for (
int i = 0; i < n3DPoints; i++) {
1283 if ( point->IsIgnored() )
1286 if( point->IsRejected() ) {
1287 nRejected3DPoints++;
1306 int nMeasures = point->GetNumMeasures();
1308 for (
int j = 0; j < nMeasures; j++) {
1311 if ( measure->IsIgnored() )
1316 if (measure->IsRejected())
1326 if ( m_bObservationMode )
1327 nImageIndex = ImageIndex(nImageIndex)/m_nNumImagePartials;
1329 bStatus = ComputePartials_DC(coeff_image, coeff_point3D, coeff_RHS,
1340 m_nObservations += 2;
1342 formNormals1_CHOLMOD(N22, N12, n1, n2, coeff_image, coeff_point3D,
1343 coeff_RHS, nImageIndex);
1346 formNormals2_CHOLMOD(N22, N12, n2, m_nj, nPointIndex, i);
1357 formNormals3_CHOLMOD(n1, m_nj);
1363 m_nUnknownParameters = m_nRank + 3 * nGood3DPoints;
1372 bool BundleAdjust::formNormals1_CHOLMOD(symmetric_matrix<double, upper>&N22,
1374 vector<double>& n2, matrix<double>& coeff_image,
1375 matrix<double>& coeff_point3D, vector<double>& coeff_RHS,
1379 static vector<double> n1_image(m_nNumImagePartials);
1383 static symmetric_matrix<double, upper> N11(m_nNumImagePartials);
1390 N11 = prod(trans(coeff_image), coeff_image);
1394 int t = m_nNumImagePartials * nImageIndex;
1397 m_SparseNormals.InsertMatrixBlock(nImageIndex, nImageIndex,
1398 m_nNumImagePartials, m_nNumImagePartials );
1400 (*(*m_SparseNormals[nImageIndex])[nImageIndex]) += N11;
1405 static matrix<double> N12_Image(m_nNumImagePartials, 3);
1408 N12_Image = prod(trans(coeff_image), coeff_point3D);
1421 *N12[nImageIndex] += N12_Image;
1427 n1_image = prod(trans(coeff_image), coeff_RHS);
1432 for (i = 0; i < m_nNumImagePartials; i++)
1433 n1(i + t) += n1_image(i);
1436 N22 += prod(trans(coeff_point3D), coeff_point3D);
1441 n2 += prod(trans(coeff_point3D), coeff_RHS);
1452 bool BundleAdjust::formNormals2_CHOLMOD(symmetric_matrix<double, upper>&N22,
1454 int nPointIndex,
int i) {
1456 bounded_vector<double, 3>& NIC = m_NICs[nPointIndex];
1466 bounded_vector<double, 3>& weights = m_Point_Weights[nPointIndex];
1467 bounded_vector<double, 3>& corrections = m_Point_Corrections[nPointIndex];
1473 if (weights[0] > 0.0) {
1474 N22(0, 0) += weights[0];
1475 n2(0) += (-weights[0] * corrections(0));
1476 m_nConstrainedPointParameters++;
1479 if (weights[1] > 0.0) {
1480 N22(1, 1) += weights[1];
1481 n2(1) += (-weights[1] * corrections(1));
1482 m_nConstrainedPointParameters++;
1485 if (weights[2] > 0.0) {
1486 N22(2, 2) += weights[2];
1487 n2(2) += (-weights[2] * corrections(2));
1488 m_nConstrainedPointParameters++;
1513 product_ATransB(N22, N12, Q);
1523 NIC = prod(N22, n2);
1533 AmultAdd_CNZRows_CHOLMOD(-1.0, N12, Q);
1541 transA_NZ_multAdd_CHOLMOD(-1.0, Q, n2, m_nj);
1554 bool BundleAdjust::formNormals3_CHOLMOD(compressed_vector<double>& n1,
1555 vector<double>& nj) {
1559 m_nConstrainedImageParameters = 0;
1563 for (
int i = 0; i < m_SparseNormals.size(); i++ ) {
1564 matrix<double>* diagonalBlock = m_SparseNormals.getBlock(i,i);
1565 if ( !diagonalBlock )
1568 for (
int j = 0; j < m_nNumImagePartials; j++) {
1569 if (m_dImageParameterWeights[j] > 0.0) {
1570 (*diagonalBlock)(j,j) += m_dImageParameterWeights[j];
1571 m_nj[n] -= m_dImageParameterWeights[j] * m_Image_Corrections[n];
1572 m_nConstrainedImageParameters++;
1589 bool BundleAdjust::formNormalEquations_SPECIALK() {
1596 bool bStatus =
false;
1598 m_nObservations = 0;
1599 m_nConstrainedPointParameters = 0;
1601 static matrix<double> coeff_image;
1602 static matrix<double> coeff_point3D(2, 3);
1603 static vector<double> coeff_RHS(2);
1604 static symmetric_matrix<double, upper> N22(3);
1605 static matrix< double> N12(m_nRank, 3);
1606 static vector<double> n2(3);
1607 compressed_vector<double> n1(m_nRank);
1609 m_nj.resize(m_nRank);
1611 coeff_image.resize(2, m_nNumImagePartials);
1612 N12.resize(m_nRank, 3);
1620 coeff_point3D.clear();
1626 int nGood3DPoints = 0;
1627 int nRejected3DPoints = 0;
1628 int nPointIndex = 0;
1630 int n3DPoints = m_pCnet->GetNumPoints();
1637 for (
int i = 0; i < n3DPoints; i++) {
1640 if ( point->IsIgnored() )
1643 if( point->IsRejected() ) {
1644 nRejected3DPoints++;
1663 int nMeasures = point->GetNumMeasures();
1664 for (
int j = 0; j < nMeasures; j++) {
1666 if ( measure->IsIgnored() )
1671 if (measure->IsRejected())
1681 if ( m_bObservationMode )
1682 nImageIndex = ImageIndex(nImageIndex)/m_nNumImagePartials;
1684 bStatus = ComputePartials_DC(coeff_image, coeff_point3D, coeff_RHS,
1695 m_nObservations += 2;
1697 formNormals1_SPECIALK(N22, N12, n1, n2, coeff_image, coeff_point3D,
1698 coeff_RHS, nImageIndex);
1701 formNormals2_SPECIALK(N22, N12, n2, m_nj, nPointIndex, i);
1712 formNormals3_SPECIALK(n1, m_nj);
1718 m_nUnknownParameters = m_nRank + 3 * nGood3DPoints;
1727 bool BundleAdjust::formNormals1_SPECIALK(symmetric_matrix<double, upper>&N22,
1728 matrix<double>& N12, compressed_vector<double>& n1, vector<double>& n2,
1729 matrix<double>& coeff_image, matrix<double>& coeff_point3D,
1730 vector<double>& coeff_RHS,
int nImageIndex) {
1733 static vector<double> n1_image(m_nNumImagePartials);
1737 static symmetric_matrix<double, upper> N11(m_nNumImagePartials);
1744 N11 = prod(trans(coeff_image), coeff_image);
1749 int t = m_nNumImagePartials * nImageIndex;
1750 for (i = 0; i < m_nNumImagePartials; i++)
1751 for (j = i; j < m_nNumImagePartials; j++)
1752 m_Normals(i + t, j + t) += N11(i, j);
1755 static matrix<double> N12_Image(m_nNumImagePartials, 3);
1758 N12_Image = prod(trans(coeff_image), coeff_point3D);
1767 for (i = 0; i < m_nNumImagePartials; i++)
1768 for (j = 0; j < 3; j++)
1769 N12(i + t, j) += N12_Image(i, j);
1775 n1_image = prod(trans(coeff_image), coeff_RHS);
1780 for (i = 0; i < m_nNumImagePartials; i++)
1781 n1(i + t) += n1_image(i);
1784 N22 += prod(trans(coeff_point3D), coeff_point3D);
1789 n2 += prod(trans(coeff_point3D), coeff_RHS);
1800 bool BundleAdjust::formNormals2_SPECIALK(symmetric_matrix<double, upper>&N22,
1801 matrix<double>& N12, vector<double>& n2, vector<double>& nj,
1802 int nPointIndex,
int i) {
1804 bounded_vector<double, 3>& NIC = m_NICs[nPointIndex];
1805 compressed_matrix<double>& Q = m_Qs_SPECIALK[nPointIndex];
1814 bounded_vector<double, 3>& weights = m_Point_Weights[nPointIndex];
1815 bounded_vector<double, 3>& corrections = m_Point_Corrections[nPointIndex];
1821 if (weights[0] > 0.0) {
1822 N22(0, 0) += weights[0];
1823 n2(0) += (-weights[0] * corrections(0));
1824 m_nConstrainedPointParameters++;
1827 if (weights[1] > 0.0) {
1828 N22(1, 1) += weights[1];
1829 n2(1) += (-weights[1] * corrections(1));
1830 m_nConstrainedPointParameters++;
1833 if (weights[2] > 0.0) {
1834 N22(2, 2) += weights[2];
1835 n2(2) += (-weights[2] * corrections(2));
1836 m_nConstrainedPointParameters++;
1860 Q = prod(N22, trans(N12));
1869 NIC = prod(N22, n2);
1878 AmultAdd_CNZRows_SPECIALK(-1.0, N12, Q, m_Normals);
1886 transA_NZ_multAdd_SPECIALK(-1.0, Q, n2, m_nj);
1899 bool BundleAdjust::formNormals3_SPECIALK(compressed_vector<double>& n1,
1900 vector<double>& nj) {
1904 m_nConstrainedImageParameters = 0;
1908 for (
int j = 0; j < m_nNumImagePartials; j++) {
1909 if (m_dImageParameterWeights[j] > 0.0) {
1910 m_Normals(n, n) += m_dImageParameterWeights[j];
1911 m_nj[n] -= m_dImageParameterWeights[j] * m_Image_Corrections[n];
1912 m_nConstrainedImageParameters++;
1919 while (n < m_nRank);
1931 bool BundleAdjust::InitializePointWeights() {
1941 int n3DPoints = m_pCnet->GetNumPoints();
1942 int nPointIndex = 0;
1943 for (
int i = 0; i < n3DPoints; i++) {
1945 if (point->IsIgnored())
1951 SurfacePoint aprioriSurfacePoint = point->GetAprioriSurfacePoint();
1953 bounded_vector<double, 3>& weights = m_Point_Weights[nPointIndex];
1954 bounded_vector<double, 3>& apriorisigmas = m_Point_AprioriSigmas[nPointIndex];
1958 if (point->
GetType() == ControlPoint::Fixed) {
1959 weights[0] = 1.0e+50;
1960 weights[1] = 1.0e+50;
1961 weights[2] = 1.0e+50;
1963 else if (point->
GetType() == ControlPoint::Free) {
1964 if( m_dGlobalLatitudeAprioriSigma > 0.0 ) {
1965 apriorisigmas[0] = m_dGlobalLatitudeAprioriSigma;
1966 d = m_dGlobalLatitudeAprioriSigma*m_dMTR;
1967 weights[0] = 1.0/(d*d);
1969 if( m_dGlobalLongitudeAprioriSigma > 0.0 ) {
1970 apriorisigmas[1] = m_dGlobalLongitudeAprioriSigma;
1971 d = m_dGlobalLongitudeAprioriSigma*m_dMTR;
1972 weights[1] = 1.0/(d*d);
1975 weights[2] = 1.0e+50;
1977 if( m_dGlobalRadiusAprioriSigma > 0.0 ) {
1978 apriorisigmas[2] = m_dGlobalRadiusAprioriSigma;
1979 d = m_dGlobalRadiusAprioriSigma*0.001;
1980 weights[2] = 1.0/(d*d);
1984 else if (point->
GetType() == ControlPoint::Constrained) {
1985 if( point->IsLatitudeConstrained() ) {
1987 weights[0] = point->GetAprioriSurfacePoint().
GetLatWeight();
1989 else if( m_dGlobalLatitudeAprioriSigma > 0.0 ) {
1990 apriorisigmas[0] = m_dGlobalLatitudeAprioriSigma;
1991 d = m_dGlobalLatitudeAprioriSigma*m_dMTR;
1992 weights[0] = 1.0/(d*d);
1995 if( point->IsLongitudeConstrained() ) {
1997 weights[1] = point->GetAprioriSurfacePoint().
GetLonWeight();
1999 else if( m_dGlobalLongitudeAprioriSigma > 0.0 ) {
2000 apriorisigmas[1] = m_dGlobalLongitudeAprioriSigma;
2001 d = m_dGlobalLongitudeAprioriSigma*m_dMTR;
2002 weights[1] = 1.0/(d*d);
2006 weights[2] = 1.0e+50;
2008 if( point->IsRadiusConstrained() ) {
2009 apriorisigmas[2] = point->GetAprioriSurfacePoint().GetLocalRadiusSigma().
meters();
2012 else if( m_dGlobalRadiusAprioriSigma > 0.0 ) {
2013 apriorisigmas[2] = m_dGlobalRadiusAprioriSigma;
2014 d = m_dGlobalRadiusAprioriSigma*0.001;
2015 weights[2] = 1.0/(d*d);
2050 void BundleAdjust::InitializePoints() {
2051 int n3DPoints = m_pCnet->GetNumPoints();
2053 for (
int i = 0; i < n3DPoints; i++) {
2056 if (point->IsIgnored())
2059 SurfacePoint aprioriSurfacePoint = point->GetAprioriSurfacePoint();
2071 void BundleAdjust::product_AV(
double alpha, bounded_vector<double,3>& v2,
2074 QMapIterator<int, matrix<double>*> iQ(Q);
2076 while ( iQ.hasNext() ) {
2079 int ncol = iQ.key();
2081 int t = ncol*m_nNumImagePartials;
2083 v2 += alpha * prod(*(iQ.value()),subrange(v1,t,t+m_nNumImagePartials));
2095 bool BundleAdjust::product_ATransB(symmetric_matrix <double,upper>& N22,
2098 QMapIterator<int, matrix<double>*> iN12(N12);
2100 while ( iN12.hasNext() ) {
2103 int ncol = iN12.key();
2108 *(Q[ncol]) = prod(N22,trans(*(iN12.value())));
2122 void BundleAdjust::AmultAdd_CNZRows_CHOLMOD(
double alpha,
2129 QMapIterator<int, matrix<double>*> iN12(N12);
2130 QMapIterator<int, matrix<double>*> iQ(Q);
2132 while ( iN12.hasNext() ) {
2135 int nrow = iN12.key();
2137 matrix<double> *a = iN12.value();
2139 while ( iQ.hasNext() ) {
2142 int ncol = iQ.key();
2147 m_SparseNormals.InsertMatrixBlock(ncol, nrow,
2148 m_nNumImagePartials, m_nNumImagePartials );
2150 (*(*m_SparseNormals[ncol])[nrow]) -= prod(*a,*(iQ.value()));
2163 void BundleAdjust::AmultAdd_CNZRows_SPECIALK(
double alpha, matrix<double>& A, compressed_matrix<double>& B,
2164 symmetric_matrix<double, upper, column_major>& C)
2169 register int i, j, k, ii, jj;
2172 int nColsA = A.size2();
2175 std::vector<int> nz(B.nnz() / B.size1());
2178 typedef compressed_matrix<double>::iterator1 it_row;
2179 typedef compressed_matrix<double>::iterator2 it_col;
2181 it_row itr = B.begin1();
2183 for (it_col itc = itr.begin(); itc != itr.end(); ++itc) {
2184 nz[nIndex] = itc.index2();
2188 int nzlength = nz.size();
2189 for (i = 0; i < nzlength; ++i) {
2191 for (j = i; j < nzlength; ++j) {
2195 for (k = 0; k < nColsA; ++k)
2196 d += A(ii, k) * B(k, jj);
2198 C(ii, jj) += alpha * d;
2209 void BundleAdjust::transA_NZ_multAdd_CHOLMOD(
double alpha,
2215 QMapIterator<int, matrix<double>*> iQ(Q);
2217 while ( iQ.hasNext() ) {
2220 int nrow = iQ.key();
2221 matrix<double>* m = iQ.value();
2223 vector<double> v = prod(trans(*m),n2);
2225 int t = nrow*m_nNumImagePartials;
2227 for(
unsigned i = 0; i < v.size(); i++ )
2228 m_nj(t+i) += alpha*v(i);
2238 void BundleAdjust::transA_NZ_multAdd_SPECIALK(
double alpha, compressed_matrix<double>& A,
2239 vector<double>& B, vector<double>& C) {
2244 register int i, j, ii;
2247 int nRowsA = A.size1();
2250 std::vector<int> nz(A.nnz() / A.size1());
2252 typedef compressed_matrix<double>::iterator1 it_row;
2253 typedef compressed_matrix<double>::iterator2 it_col;
2255 it_row itr = A.begin1();
2257 for (it_col itc = itr.begin(); itc != itr.end(); ++itc) {
2258 nz[nIndex] = itc.index2();
2262 int nzlength = nz.size();
2263 for (i = 0; i < nzlength; ++i) {
2267 for (j = 0; j < nRowsA; ++j)
2268 d += A(j, ii) * B(j);
2280 void BundleAdjust::AmulttransBNZ(matrix<double>& A,
2281 compressed_matrix<double>& B, matrix<double> &C,
double alpha) {
2286 register int i,j,k,kk;
2289 int nRowsB = B.size1();
2292 std::vector<int> nz(B.nnz() / nRowsB);
2294 typedef compressed_matrix<double>::iterator1 it_row;
2295 typedef compressed_matrix<double>::iterator2 it_col;
2297 it_row itr = B.begin1();
2299 for (it_col itc = itr.begin(); itc != itr.end(); ++itc) {
2300 nz[nIndex] = itc.index2();
2304 int nzlength = nz.size();
2306 int nRowsA = A.size1();
2307 int nColsC = C.size2();
2309 for ( i = 0; i < nRowsA; ++i ) {
2310 for (j = 0; j < nColsC; ++j) {
2313 for (k = 0; k < nzlength; ++k) {
2315 d += A(i, kk) * B(j, kk);
2318 C(i,j) += alpha * d;
2330 void BundleAdjust::ANZmultAdd(compressed_matrix<double>& A,
2331 symmetric_matrix<double, upper, column_major>& B,
2332 matrix<double>& C,
double alpha) {
2337 register int i,j,k,kk;
2340 int nRowsA = A.size1();
2343 std::vector<int> nz(A.nnz() /nRowsA);
2345 typedef compressed_matrix<double>::iterator1 it_row;
2346 typedef compressed_matrix<double>::iterator2 it_col;
2348 it_row itr = A.begin1();
2350 for (it_col itc = itr.begin(); itc != itr.end(); ++itc) {
2351 nz[nIndex] = itc.index2();
2355 int nzlength = nz.size();
2357 int nColsC = C.size2();
2358 for ( i = 0; i < nRowsA; ++i ) {
2359 for ( j = 0; j < nColsC; ++j ) {
2362 for ( k = 0; k < nzlength; ++k ) {
2364 d += A(i, kk) * B(kk, j);
2367 C(i, j) += alpha * d;
2378 bool BundleAdjust::solveSystem_CHOLMOD() {
2381 if ( !loadCholmodTriplet() ) {
2382 QString msg =
"CHOLMOD: Failed to load Triplet matrix";
2389 m_N = cholmod_triplet_to_sparse(m_pTriplet, m_pTriplet->nnz, &m_cm);
2395 m_L = cholmod_analyze(m_N, &m_cm);
2402 cholmod_factorize(m_N, m_L, &m_cm);
2408 if ( m_cm.status == CHOLMOD_NOT_POSDEF ) {
2409 QString msg =
"matrix NOT positive-definite: failure at column "
2421 cholmod_dense *x, *b;
2424 b = cholmod_zeros (m_N->nrow, 1, m_N->xtype, &m_cm);
2427 double *px = (
double*)b->x;
2428 for (
int i = 0; i < m_nRank; i++ )
2438 x = cholmod_solve (CHOLMOD_A, m_L, b, &m_cm) ;
2449 double *sx = (
double*)x->x;
2450 for (
int i = 0; i < m_nRank; i++ )
2451 m_Image_Solution[i] = sx[i];
2454 cholmod_free_sparse(&m_N, &m_cm);
2455 cholmod_free_dense(&b, &m_cm);
2456 cholmod_free_dense(&x, &m_cm);
2466 bool BundleAdjust::loadCholmodTriplet() {
2470 if ( m_nIteration == 1 ) {
2471 int nelements = m_SparseNormals.numberOfElements();
2473 m_pTriplet = cholmod_allocate_triplet(m_nRank, m_nRank, nelements,
2474 -1, CHOLMOD_REAL, &m_cm);
2476 if ( !m_pTriplet ) {
2477 printf(
"Triplet allocation failure");
2481 m_pTriplet->nnz = 0;
2484 int* Ti = (
int*) m_pTriplet->i;
2485 int* Tj = (
int*) m_pTriplet->j;
2486 double* v = (
double*)m_pTriplet->x;
2490 int nblockcolumns = m_SparseNormals.size();
2491 for (
int ncol = 0; ncol < nblockcolumns; ncol++ ) {
2496 printf(
"SparseBlockColumnMatrix retrieval failure at column %d", ncol);
2500 QMapIterator<int, matrix<double>*> it(*sbc);
2502 while ( it.hasNext() ) {
2505 int nrow = it.key();
2507 matrix<double>* m = it.value();
2509 printf(
"matrix block retrieval failure at column %d, row %d", ncol,nrow);
2510 printf(
"Total # of block columns: %d", nblockcolumns);
2511 printf(
"Total # of blocks: %d", m_SparseNormals.numberOfBlocks());
2515 if ( ncol == nrow ) {
2516 for (
unsigned ii = 0; ii < m->size1(); ii++ ) {
2517 for (
unsigned jj = ii; jj < m->size2(); jj++) {
2518 d = m->at_element(ii,jj);
2519 int ncolindex = jj+ncol*m_nNumImagePartials;
2520 int nrowindex = ii+nrow*m_nNumImagePartials;
2522 if ( m_nIteration == 1 ) {
2523 Ti[nentries] = ncolindex;
2524 Tj[nentries] = nrowindex;
2537 for (
unsigned ii = 0; ii < m->size1(); ii++ ) {
2538 for (
unsigned jj = 0; jj < m->size2(); jj++ ) {
2539 d = m->at_element(ii,jj);
2540 int ncolindex = jj+ncol*m_nNumImagePartials;
2541 int nrowindex = ii+nrow*m_nNumImagePartials;
2543 if ( m_nIteration ==1 ) {
2544 Ti[nentries] = nrowindex;
2545 Tj[nentries] = ncolindex;
2569 bool BundleAdjust::solveSystem_SPECIALK() {
2575 if ( !CholeskyUT_NOSQR() )
2586 if (!CholeskyUT_NOSQR_BackSub(m_Normals, m_Image_Solution, m_nj))
2602 bool BundleAdjust::CholeskyUT_NOSQR() {
2604 double sum, divisor;
2608 int nRows = m_Normals.size1();
2619 for (i = 0; i < nRows; i++) {
2623 for (j = 0; j < i; j++) {
2626 d1 = m_Normals(j, i);
2629 sum += d1 * d1 * m_Normals(j, j);
2633 m_Normals(i, i) -= sum;
2637 den = m_Normals(i, i);
2638 if (fabs(den) < 1e-100)
2641 divisor = 1.0 / den;
2643 for (j = (i + 1); j < nRows; j++) {
2646 for (k = 0; k < i; k++) {
2649 d1 = m_Normals(k, j);
2653 d2 = m_Normals(k, i);
2657 sum += d1 * d2 * m_Normals(k, k);
2661 m_Normals(i, j) = (m_Normals(i, j) - sum) * divisor;
2666 for (k = 0; k < i; k++) {
2671 d2 = m_Normals(k, i);
2675 sum += d1 * d2 * m_Normals(k, k);
2678 m_nj(i) = (m_nj(i) - sum) * divisor;
2689 bool BundleAdjust::CholeskyUT_NOSQR_BackSub(symmetric_matrix<double, upper, column_major>& m,
2690 vector<double>& s, vector<double>& rhs) {
2695 int nRows = m.size1();
2697 s(nRows - 1) = rhs(nRows - 1);
2699 for (i = nRows - 2; i >= 0; i--) {
2702 for (j = i + 1; j < nRows; j++) {
2714 s(i) = rhs(i) - sum;
2727 bool BundleAdjust::CholeskyUT_NOSQR_Inverse() {
2730 double colk, tmpkj, tmpkk;
2733 symmetric_matrix<double, upper, column_major> tmp = m_Normals;
2737 vector<double> s(m_nRank);
2740 vector<double> column(m_nRank);
2744 for (i = 0; i < m_nRank; i++) {
2750 for (j = 0; j < m_nRank; j++) {
2751 div = 1.0 / tmp(j, j);
2754 for (k = 0; k < j; k++) {
2759 if (colk == 0.0 || tmpkj == 0.0 || tmpkk == 0.0)
2762 sum += colk * tmpkj * tmpkk;
2765 column(j) = (column(j) - sum) * div;
2769 if (!CholeskyUT_NOSQR_BackSub(tmp, s, column))
2774 for (j = 0; j <= i; j++)
2775 m_Normals(j, i) = s(j);
2786 bool BundleAdjust::cholmod_Inverse() {
2790 m_Normals.resize(m_nRank);
2795 b = cholmod_zeros ( m_nRank, 1, CHOLMOD_REAL, &m_cm ) ;
2796 double* pb = (
double*)b->x;
2800 for ( i = 0; i < m_nRank; i++ ) {
2805 x = cholmod_solve ( CHOLMOD_A, m_L, b, &m_cm ) ;
2810 for (j = 0; j <= i; j++)
2811 m_Normals(j, i) = px[j];
2813 cholmod_free_dense(&x,&m_cm);
2820 cholmod_free_dense(&b,&m_cm);
2831 bool BundleAdjust::Invert_3x3(symmetric_matrix<double, upper>& m) {
2835 symmetric_matrix<double, upper> c = m;
2837 den = m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1))
2838 - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0))
2839 + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0));
2842 if (fabs(den) < 1.0e-100)
2847 m(0, 0) = (c(1, 1) * c(2, 2) - c(1, 2) * c(2, 1)) * det;
2848 m(0, 1) = (c(0, 2) * c(2, 1) - c(0, 1) * c(2, 2)) * det;
2849 m(0, 2) = (c(0, 1) * c(1, 2) - c(0, 2) * c(1, 1)) * det;
2850 m(1, 1) = (c(0, 0) * c(2, 2) - c(0, 2) * c(2, 0)) * det;
2851 m(1, 2) = (c(0, 2) * c(1, 0) - c(0, 0) * c(1, 2)) * det;
2852 m(2, 2) = (c(0, 0) * c(1, 1) - c(0, 1) * c(1, 0)) * det;
2861 bool BundleAdjust::ComputePartials_DC(matrix<double>& coeff_image, matrix<double>& coeff_point3D,
2866 std::vector<double> d_lookB_WRT_LAT;
2867 std::vector<double> d_lookB_WRT_LON;
2868 std::vector<double> d_lookB_WRT_RAD;
2872 double dMeasuredx, dComputedx, dMeasuredy, dComputedy;
2873 double deltax, deltay;
2874 double dObservationSigma;
2875 double dObservationWeight;
2877 pCamera = measure.Camera();
2880 coeff_image.clear();
2881 coeff_point3D.clear();
2888 pCamera->
SetImage(measure.GetSample(), measure.GetLine());
2892 if (!(pCamera->
GroundMap()->
GetXY(point.GetAdjustedSurfacePoint(), &dComputedx, &dComputedy))) {
2893 QString msg =
"Unable to map apriori surface point for measure ";
2900 CameraGroundMap::WRT_Latitude);
2902 CameraGroundMap::WRT_Longitude);
2904 CameraGroundMap::WRT_Radius);
2912 if (m_spacecraftPositionSolveType != Nothing) {
2917 for (
int icoef = 0; icoef < m_nNumberCamPosCoefSolved; icoef++) {
2919 &coeff_image(0, nIndex),
2920 &coeff_image(1, nIndex));
2927 for (
int icoef = 0; icoef < m_nNumberCamPosCoefSolved; icoef++) {
2929 &coeff_image(0, nIndex),
2930 &coeff_image(1, nIndex));
2935 for (
int icoef = 0; icoef < m_nNumberCamPosCoefSolved; icoef++) {
2937 &coeff_image(0, nIndex),
2938 &coeff_image(1, nIndex));
2944 if (m_cmatrixSolveType != None) {
2947 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
2949 icoef, &coeff_image(0, nIndex),
2950 &coeff_image(1, nIndex));
2955 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
2957 icoef, &coeff_image(0, nIndex),
2958 &coeff_image(1, nIndex));
2963 if (m_bSolveTwist) {
2964 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
2966 icoef, &coeff_image(0, nIndex),
2967 &coeff_image(1, nIndex));
2975 &coeff_point3D(1, 0));
2977 &coeff_point3D(1, 1));
2980 &coeff_point3D(1, 2));
2984 dMeasuredx = measure.GetFocalPlaneMeasuredX();
2985 dMeasuredy = measure.GetFocalPlaneMeasuredY();
2987 deltax = dMeasuredx - dComputedx;
2988 deltay = dMeasuredy - dComputedy;
2990 coeff_RHS(0) = deltax;
2991 coeff_RHS(1) = deltay;
2993 m_cumProRes->addObs(deltax/pCamera->
PixelPitch());
2994 m_cumProRes->addObs(deltay/pCamera->
PixelPitch());
2996 dObservationSigma = 1.4 * pCamera->
PixelPitch();
2997 dObservationWeight = 1.0 / dObservationSigma;
2999 if (m_maxLikelihoodFlag[m_maxLikelihoodIndex]) {
3000 double residualR2ZScore = sqrt(deltax*deltax + deltay*deltay)/dObservationSigma/sqrt(2.0);
3001 m_cumPro->addObs(residualR2ZScore);
3005 dObservationWeight *= m_wFunc[m_maxLikelihoodIndex]->sqrtWeightScaler(residualR2ZScore);
3016 coeff_image *= dObservationWeight;
3017 coeff_point3D *= dObservationWeight;
3018 coeff_RHS *= dObservationWeight;
3020 m_Statsx.AddData(deltax);
3021 m_Statsy.AddData(deltay);
3385 double BundleAdjust::Solve() {
3386 double averageError;
3387 std::vector<int> observationInitialValueIndex;
3391 ComputeNumberPartials();
3393 ComputeImageParameterWeights();
3397 if (m_bObservationMode)
3398 observationInitialValueIndex.assign(m_pObsNumList->ObservationSize(), -1);
3400 for (
int i = 0; i < Images(); i++) {
3403 if (m_bObservationMode) {
3405 oIndex = m_pObsNumList->ObservationNumberMapIndex(oIndex);
3406 iIndex = observationInitialValueIndex[oIndex];
3409 if (m_cmatrixSolveType != None) {
3416 if (!m_bObservationMode) {
3424 SpiceRotation *pOrot = m_pCnet->Camera(iIndex)->instrumentRotation();
3425 std::vector<double> anglePoly1, anglePoly2, anglePoly3;
3431 pSpiceRot->
SetPolynomial(anglePoly1, anglePoly2, anglePoly3, m_nPointingType);
3438 observationInitialValueIndex[oIndex] = i;
3443 if (m_spacecraftPositionSolveType != Nothing) {
3447 if (!m_bObservationMode) {
3455 SpicePosition *pOpos = m_pCnet->Camera(iIndex)->instrumentPosition();
3456 std::vector<double> posPoly1, posPoly2, posPoly3;
3462 pSpicePos->
SetPolynomial(posPoly1, posPoly2, posPoly3, m_nPositionType);
3469 observationInitialValueIndex[oIndex] = i;
3476 m_pCnet->ComputeApriori();
3479 double sigmaXY, sigmaHat, sigmaX, sigmaY;
3480 sigmaXY = sigmaHat = sigmaX = sigmaY = 0.;
3483 clock_t t1 = clock();
3486 m_nBasisColumns = BasisColumns();
3487 BasisFunction basis(
"Bundle", m_nBasisColumns, m_nBasisColumns);
3488 if (m_strSolutionMethod ==
"OLDSPARSE") {
3490 m_pCnet->GetNumValidMeasures() * 2, m_nBasisColumns,
true);
3491 SetParameterWeights();
3497 m_dxKnowns.resize(m_nBasisColumns);
3498 m_dyKnowns.resize(m_nBasisColumns);
3500 double dprevious_Sigma0 = 10;
3505 while (m_nIteration < m_nMaxIterations) {
3508 m_pCnet->ComputeResiduals();
3509 m_dError = m_pCnet->GetMaximumResidual();
3510 averageError = m_pCnet->AverageResidual();
3517 IterationSummary(averageError, sigmaXY, sigmaHat, sigmaX, sigmaY);
3528 if ( m_nIteration == 0 )
3532 if (fabs(dprevious_Sigma0 - m_dSigma0) <= m_dConvergenceThreshold) {
3533 clock_t t2 = clock();
3535 m_bConverged =
true;
3537 m_dElapsedTime = ((t2 - t1) / (
double)CLOCKS_PER_SEC);
3540 GetSparseParameterCorrections();
3542 if (m_bErrorPropagation) {
3543 progress.
SetText(
"Performing Error Propagation...");
3546 clock_t terror1 = clock();
3547 if (m_pLsq->SparseErrorPropagation())
3548 SetPostBundleSigmas();
3549 clock_t terror2 = clock();
3550 m_dElapsedTimeErrorProp = ((terror2 - terror1) / (
double)CLOCKS_PER_SEC);
3554 ComputeBundleStatistics();
3560 dprevious_Sigma0 = m_dSigma0;
3562 if ( m_nIteration > 0 )
3568 int nObjectPoints = m_pCnet->GetNumPoints();
3569 for (
int i = 0; i < nObjectPoints; i++)
3574 if (m_strSolutionMethod ==
"SVD") {
3577 else if (m_strSolutionMethod ==
"QRD") {
3585 if (zeroColumn != 0) {
3587 int imageColumns = Observations() * m_nNumImagePartials;
3588 if (zeroColumn <= imageColumns) {
3589 msg =
"Solution matrix has a column of zeros which probably ";
3590 msg +=
"indicates an image with no points. Running the program, ";
3591 msg +=
"cnetcheck, before jigsaw should catch these problems.";
3594 msg =
"Solution matrix has a column of zeros which probably ";
3595 msg +=
"indicates a point with no measures. Running the program, ";
3596 msg +=
"cnetcheck, before jigsaw should catch these problems.";
3603 QString msg =
"Unable to solve in BundleAdjust, ";
3604 msg +=
"Iteration " +
toString(m_nIteration) +
" of ";
3605 msg +=
toString(m_nMaxIterations) +
", Sigma0 = ";
3606 msg +=
toString(m_dConvergenceThreshold);
3614 std::vector<double> residuals = m_pLsq->Residuals();
3615 int nresiduals = residuals.size();
3616 for (
int i = 0; i < nresiduals; i++) {
3617 m_Statsrx.AddData(residuals[i]);
3618 m_Statsry.AddData(residuals[i+1]);
3622 m_Statsrxy.AddData(&residuals[0], nresiduals);
3624 m_nObservations = m_pLsq->Knowns();
3625 m_nUnknownParameters = m_nBasisColumns;
3628 double drms_rx = sqrt(m_Statsrx.SumSquare() / (m_nObservations / 2));
3629 double drms_ry = sqrt(m_Statsry.SumSquare() / (m_nObservations / 2));
3630 double drms_rxy = sqrt(m_Statsrxy.SumSquare() / m_nObservations);
3631 double davg_rxy = m_Statsrxy.Average();
3632 printf(
"avg rxy: %20.10lf\nrms x: %20.10lf\nrms y: %20.10lf\nrms xy: %20.10lf\n", davg_rxy, drms_rx, drms_ry, drms_rxy);
3634 sigmaXY = sqrt((m_Statsx.SumSquare() + m_Statsy.SumSquare()) / m_pLsq->Knowns());
3635 m_nDegreesOfFreedom = m_pLsq->GetDegreesOfFreedom();
3636 sigmaHat = (m_nObservations - m_nBasisColumns) ?
3637 (sqrt((m_Statsx.SumSquare() + m_Statsy.SumSquare()) / m_nDegreesOfFreedom))
3640 m_dSigma0 = m_pLsq->GetSigma0();
3642 printf(
"Observations: %d Unknowns: %d\n", m_nObservations, m_nUnknownParameters);
3643 printf(
"SigmaHat: %20.10lf Sigma0: %20.10lf\n", sigmaHat, m_dSigma0);
3645 sigmaX = m_Statsx.TotalPixels() ?
3646 sqrt(m_Statsx.SumSquare() / m_Statsx.TotalPixels()) : 0.;
3647 sigmaY = m_Statsy.TotalPixels() ?
3648 sqrt(m_Statsy.SumSquare() / m_Statsy.TotalPixels()) : 0.;
3651 QString msg =
"Did not converge to Sigma0 criteria [";
3652 msg +=
toString(m_dConvergenceThreshold) +
"] in less than [";
3653 msg +=
toString(m_nMaxIterations) +
"] iterations";
3663 void BundleAdjust::GetSparseParameterCorrections() {
3665 int nValidPoints = m_pCnet->GetNumValidPoints();
3666 int nTotalPoints = m_pCnet->GetNumPoints();
3667 int nPointCorrections = 3 * nValidPoints;
3668 m_Point_Corrections.resize(nValidPoints);
3670 m_dEpsilons = m_pLsq->GetEpsilons();
3671 int nCorrections = m_dEpsilons.size();
3672 int nImageCorrections = nCorrections - nPointCorrections;
3673 m_Image_Corrections.resize(nImageCorrections);
3676 for(
int i = 0; i < nImageCorrections; i++ )
3677 m_Image_Corrections[i] = m_dEpsilons[i];
3680 int nindex = nImageCorrections;
3681 int nPointIndex = 0;
3682 for (
int i = 0; i < nTotalPoints; i++ ) {
3685 if ( point->IsIgnored() )
3688 bounded_vector<double, 3>& corrections = m_Point_Corrections[nPointIndex];
3690 corrections[0] = m_dEpsilons[nindex];
3691 corrections[1] = m_dEpsilons[nindex+1];
3692 corrections[2] = m_dEpsilons[nindex+2];
3703 void BundleAdjust::AddPartials(
int nPointIndex) {
3704 const ControlPoint *point = m_pCnet->GetPoint(nPointIndex);
3706 if (point->IsIgnored())
3712 double *px = &m_dxKnowns[0];
3713 double *py = &m_dyKnowns[0];
3716 std::vector<double> d_lookB_WRT_LAT;
3717 std::vector<double> d_lookB_WRT_LON;
3718 std::vector<double> d_lookB_WRT_RAD;
3720 std::vector<double> TC;
3721 std::vector<double> TB;
3722 std::vector<double> CJ;
3727 double dMeasuredx, dMeasuredy;
3728 double deltax, deltay;
3729 double dObservationSigma;
3730 double dObservationWeight;
3734 point->GetAdjustedSurfacePoint(),
3735 CameraGroundMap::WRT_Latitude);
3737 point->GetAdjustedSurfacePoint(),
3738 CameraGroundMap::WRT_Longitude);
3741 if (m_bSolveRadii || m_strSolutionMethod ==
"OLDSPARSE")
3743 point->GetAdjustedSurfacePoint(),
3744 CameraGroundMap::WRT_Radius);
3750 int nObservations = point->GetNumMeasures();
3751 for (
int i = 0; i < nObservations; i++) {
3753 if (measure->IsIgnored())
3757 memset(px, 0, m_nBasisColumns *
sizeof(
double));
3758 memset(py, 0, m_nBasisColumns *
sizeof(
double));
3760 pCamera = measure->Camera();
3766 if (!pCamera->
SetImage(measure->GetSample(), measure->GetLine()))
3767 printf(
"\n***Call to Camera::SetImage failed - need to handle this***\n");
3771 double dComputedx, dComputedy;
3772 if (!(pCamera->
GroundMap()->
GetXY(point->GetAdjustedSurfacePoint(), &dComputedx, &dComputedy))) {
3773 QString msg =
"Unable to map apriori surface point for measure ";
3780 nIndex = ImageIndex(nIndex);
3782 if (m_spacecraftPositionSolveType != Nothing) {
3787 for (
int icoef = 0; icoef < m_spacecraftPositionSolveType; icoef++) {
3789 &px[nIndex], &py[nIndex]);
3794 for (
int icoef = 0; icoef < m_spacecraftPositionSolveType; icoef++) {
3800 for (
int icoef = 0; icoef < m_spacecraftPositionSolveType; icoef++) {
3806 if (m_cmatrixSolveType != None) {
3809 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
3815 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
3821 if (m_bSolveTwist) {
3822 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
3830 if (point->
GetType() != ControlPoint::Fixed ||
3831 m_strSolutionMethod ==
"SPECIALK" ||
3832 m_strSolutionMethod ==
"SPARSE" ||
3833 m_strSolutionMethod ==
"OLDSPARSE") {
3834 nIndex = PointIndex(nPointIndex);
3843 if (m_bSolveRadii || m_strSolutionMethod ==
"OLDSPARSE")
3849 dMeasuredx = measure->GetFocalPlaneMeasuredX();
3850 dMeasuredy = measure->GetFocalPlaneMeasuredY();
3852 deltax = dMeasuredx - dComputedx;
3853 deltay = dMeasuredy - dComputedy;
3855 dObservationSigma = 1.4 * pCamera->
PixelPitch();
3856 dObservationWeight = 1.0 / (dObservationSigma * dObservationSigma);
3866 m_pLsq->AddKnown(m_dxKnowns, deltax, dObservationWeight);
3867 m_pLsq->AddKnown(m_dyKnowns, deltay, dObservationWeight);
3869 m_Statsx.AddData(deltax);
3870 m_Statsy.AddData(deltay);
4253 int BundleAdjust::Triangulation(
bool bDoApproximation) {
4254 int nSuccessfullyTriangulated = 0;
4259 int nControlNetPoints = m_pCnet->GetNumPoints();
4260 for (
int i = 0; i < nControlNetPoints; i++) {
4263 if (point->IsIgnored())
4264 return nSuccessfullyTriangulated;
4266 if (bDoApproximation) {
4267 ApproximatePoint_ClosestApproach(*point, i);
4273 TriangulatePoint(*point);
4279 nSuccessfullyTriangulated++;
4282 return nSuccessfullyTriangulated;
4308 bool BundleAdjust::ApproximatePoint_ClosestApproach(
const ControlPoint &rPoint,
int nIndex) {
4315 double BaseVector[3];
4316 double Camera1Position[3];
4317 double Camera2Position[3];
4318 double Camera1LookVector[3];
4319 double Camera2LookVector[3];
4320 double Camera1XCamera2[3];
4321 double ClosestPoint1[3];
4322 double ClosestPoint2[3];
4323 double AveragePoint[3];
4327 int nClosetApproaches = 0;
4332 int nObservations = rPoint.GetNumMeasures();
4333 for (
int i = 0; i < nObservations - 1; i++) {
4335 if (measure1.IsIgnored())
4339 pCamera1 = measure1.Camera();
4344 if (!pDistortionMap1)
4348 if (!pFocalPlaneMap1)
4351 pCamera1->
SetImage(measure1.GetSample(), measure1.GetLine());
4362 d = Camera1LookVector[0] * Camera1LookVector[0] +
4363 Camera1LookVector[1] * Camera1LookVector[1] +
4364 Camera1LookVector[2] * Camera1LookVector[2];
4371 Camera1LookVector[0] /= d;
4372 Camera1LookVector[1] /= d;
4373 Camera1LookVector[2] /= d;
4376 std::vector<double> dummy1(3);
4377 dummy1[0] = Camera1LookVector[0];
4378 dummy1[1] = Camera1LookVector[1];
4379 dummy1[2] = Camera1LookVector[2];
4385 Camera1LookVector[0] = dummy1[0];
4386 Camera1LookVector[1] = dummy1[1];
4387 Camera1LookVector[2] = dummy1[2];
4394 for (
int j = i + 1; j < nObservations; j++) {
4396 if (measure2.IsIgnored())
4399 pCamera2 = measure2.Camera();
4404 if (!pDistortionMap2)
4407 pCamera2->
SetImage(measure2.GetSample(), measure2.GetLine());
4416 d = Camera2LookVector[0] * Camera2LookVector[0] +
4417 Camera2LookVector[1] * Camera2LookVector[1] +
4418 Camera2LookVector[2] * Camera2LookVector[2];
4425 Camera2LookVector[0] /= d;
4426 Camera2LookVector[1] /= d;
4427 Camera2LookVector[2] /= d;
4430 dummy1[0] = Camera2LookVector[0];
4431 dummy1[1] = Camera2LookVector[1];
4432 dummy1[2] = Camera2LookVector[2];
4438 Camera2LookVector[0] = dummy1[0];
4439 Camera2LookVector[1] = dummy1[1];
4440 Camera2LookVector[2] = dummy1[2];
4443 BaseVector[0] = Camera2Position[0] - Camera1Position[0];
4444 BaseVector[1] = Camera2Position[1] - Camera1Position[1];
4445 BaseVector[2] = Camera2Position[2] - Camera1Position[2];
4448 vcrss_c(Camera1LookVector, Camera2LookVector, Camera1XCamera2);
4451 double dmag2 = Camera1XCamera2[0] * Camera1XCamera2[0] +
4452 Camera1XCamera2[1] * Camera1XCamera2[1] +
4453 Camera1XCamera2[2] * Camera1XCamera2[2];
4459 SpiceDouble dMatrix[3][3];
4460 dMatrix[0][0] = BaseVector[0];
4461 dMatrix[0][1] = BaseVector[1];
4462 dMatrix[0][2] = BaseVector[2];
4463 dMatrix[1][0] = Camera2LookVector[0];
4464 dMatrix[1][1] = Camera2LookVector[1];
4465 dMatrix[1][2] = Camera2LookVector[2];
4466 dMatrix[2][0] = Camera1XCamera2[0];
4467 dMatrix[2][1] = Camera1XCamera2[1];
4468 dMatrix[2][2] = Camera1XCamera2[2];
4470 blabla = (double)det_c(dMatrix);
4472 double t1 = blabla / dmag2;
4476 dMatrix[1][0] = Camera1LookVector[0];
4477 dMatrix[1][1] = Camera1LookVector[1];
4478 dMatrix[1][2] = Camera1LookVector[2];
4480 blabla = (double)det_c(dMatrix);
4482 double t2 = blabla / dmag2;
4490 ClosestPoint1[0] = Camera1Position[0] + t1 * Camera1LookVector[0];
4491 ClosestPoint1[1] = Camera1Position[1] + t1 * Camera1LookVector[1];
4492 ClosestPoint1[2] = Camera1Position[2] + t1 * Camera1LookVector[2];
4494 ClosestPoint2[0] = Camera2Position[0] + t2 * Camera2LookVector[0];
4495 ClosestPoint2[1] = Camera2Position[1] + t2 * Camera2LookVector[1];
4496 ClosestPoint2[2] = Camera2Position[2] + t2 * Camera2LookVector[2];
4498 AveragePoint[0] = (ClosestPoint1[0] + ClosestPoint2[0]) * 0.5;
4499 AveragePoint[1] = (ClosestPoint1[1] + ClosestPoint2[1]) * 0.5;
4500 AveragePoint[2] = (ClosestPoint1[2] + ClosestPoint2[2]) * 0.5;
4502 nClosetApproaches++;
4516 SpiceDouble lat, lon, rad;
4517 reclat_c(AveragePoint, &rad, &lon, &lat);
4524 m_pCnet->GetPoint(nIndex)->SetAdjustedSurfacePoint(
4528 Distance(rad, Distance::Kilometers)));
4549 void BundleAdjust::applyParameterCorrections() {
4550 if ( m_decompositionMethod == CHOLMOD )
4551 return applyParameterCorrections_CHOLMOD();
4553 return applyParameterCorrections_SPECIALK();
4560 void BundleAdjust::applyParameterCorrections_CHOLMOD() {
4565 int currentindex = -1;
4566 bool bsameindex =
false;
4569 int nImages = Images();
4570 for (
int i = 0; i < nImages; i++) {
4572 if ( m_nHeldImages > 0 )
4573 if ((m_pHeldSnList->HasSerialNumber(m_pSnList->SerialNumber(i))))
4577 index = ImageIndex(i);
4578 if( index == currentindex )
4583 currentindex = index;
4585 if (m_spacecraftPositionSolveType != Nothing) {
4587 std::vector<double> coefX(m_nNumberCamPosCoefSolved),
4588 coefY(m_nNumberCamPosCoefSolved),
4589 coefZ(m_nNumberCamPosCoefSolved);
4595 for (
int icoef = 0; icoef < m_nNumberCamPosCoefSolved; icoef++) {
4596 coefX[icoef] += m_Image_Solution(index);
4598 m_Image_Corrections(index) += m_Image_Solution(index);
4603 for (
int icoef = 0; icoef < m_nNumberCamPosCoefSolved; icoef++) {
4604 coefY[icoef] += m_Image_Solution(index);
4606 m_Image_Corrections(index) += m_Image_Solution(index);
4611 for (
int icoef = 0; icoef < m_nNumberCamPosCoefSolved; icoef++) {
4612 coefZ[icoef] += m_Image_Solution(index);
4614 m_Image_Corrections(index) += m_Image_Solution(index);
4618 pInstPos->
SetPolynomial(coefX, coefY, coefZ, m_nPositionType);
4621 if (m_cmatrixSolveType != None) {
4623 std::vector<double> coefRA(m_nNumberCamAngleCoefSolved),
4624 coefDEC(m_nNumberCamAngleCoefSolved),
4625 coefTWI(m_nNumberCamAngleCoefSolved);
4629 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
4630 coefRA[icoef] += m_Image_Solution(index);
4632 m_Image_Corrections(index) += m_Image_Solution(index);
4637 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
4638 coefDEC[icoef] += m_Image_Solution(index);
4640 m_Image_Corrections(index) += m_Image_Solution(index);
4644 if (m_bSolveTwist) {
4646 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
4647 coefTWI[icoef] += m_Image_Solution(index);
4649 m_Image_Corrections(index) += m_Image_Solution(index);
4654 pInstRot->
SetPolynomial(coefRA, coefDEC, coefTWI, m_nPointingType);
4659 double dLatCorr, dLongCorr, dRadCorr;
4660 int nPointIndex = 0;
4661 int nObjectPoints = m_pCnet->GetNumPoints();
4662 for (
int i = 0; i < nObjectPoints; i++) {
4664 if (point->IsIgnored())
4667 if( point->IsRejected() ) {
4673 bounded_vector<double, 3>& NIC = m_NICs[nPointIndex];
4675 bounded_vector<double, 3>& corrections = m_Point_Corrections[nPointIndex];
4687 product_AV(-1.0, NIC, Q, m_Image_Solution);
4707 dLat = -180.0 - dLat;
4708 dLon = dLon + 180.0;
4711 dLat = 180.0 - dLat;
4712 dLon = dLon + 180.0;
4714 while (dLon > 360.0) dLon = dLon - 360.0;
4715 while (dLon < 0.0) dLon = dLon + 360.0;
4717 dRad += 1000.*dRadCorr;
4722 corrections(0) += dLatCorr;
4723 corrections(1) += dLongCorr;
4724 corrections(2) += dRadCorr;
4728 SurfacePoint surfacepoint = point->GetAdjustedSurfacePoint();
4753 void BundleAdjust::applyParameterCorrections_SPECIALK() {
4758 int currentindex = -1;
4759 bool bsameindex =
false;
4762 int nImages = Images();
4763 for (
int i = 0; i < nImages; i++) {
4765 if ( m_nHeldImages > 0 )
4766 if ((m_pHeldSnList->HasSerialNumber(m_pSnList->SerialNumber(i))))
4770 index = ImageIndex(i);
4771 if( index == currentindex )
4776 currentindex = index;
4778 if (m_spacecraftPositionSolveType != Nothing) {
4780 std::vector<double> coefX(m_nNumberCamPosCoefSolved),
4781 coefY(m_nNumberCamPosCoefSolved),
4782 coefZ(m_nNumberCamPosCoefSolved);
4788 for (
int icoef = 0; icoef < m_nNumberCamPosCoefSolved; icoef++) {
4789 coefX[icoef] += m_Image_Solution(index);
4791 m_Image_Corrections(index) += m_Image_Solution(index);
4796 for (
int icoef = 0; icoef < m_nNumberCamPosCoefSolved; icoef++) {
4797 coefY[icoef] += m_Image_Solution(index);
4799 m_Image_Corrections(index) += m_Image_Solution(index);
4804 for (
int icoef = 0; icoef < m_nNumberCamPosCoefSolved; icoef++) {
4805 coefZ[icoef] += m_Image_Solution(index);
4807 m_Image_Corrections(index) += m_Image_Solution(index);
4811 pInstPos->
SetPolynomial(coefX, coefY, coefZ, m_nPositionType);
4814 if (m_cmatrixSolveType != None) {
4816 std::vector<double> coefRA(m_nNumberCamAngleCoefSolved),
4817 coefDEC(m_nNumberCamAngleCoefSolved),
4818 coefTWI(m_nNumberCamAngleCoefSolved);
4822 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
4823 coefRA[icoef] += m_Image_Solution(index);
4825 m_Image_Corrections(index) += m_Image_Solution(index);
4830 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
4831 coefDEC[icoef] += m_Image_Solution(index);
4833 m_Image_Corrections(index) += m_Image_Solution(index);
4837 if (m_bSolveTwist) {
4839 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
4840 coefTWI[icoef] += m_Image_Solution(index);
4842 m_Image_Corrections(index) += m_Image_Solution(index);
4847 pInstRot->
SetPolynomial(coefRA, coefDEC, coefTWI, m_nPointingType);
4852 double dLatCorr, dLongCorr, dRadCorr;
4853 int nPointIndex = 0;
4854 int nObjectPoints = m_pCnet->GetNumPoints();
4855 for (
int i = 0; i < nObjectPoints; i++) {
4857 if (point->IsIgnored())
4860 if( point->IsRejected() ) {
4866 bounded_vector<double, 3>& NIC = m_NICs[nPointIndex];
4867 compressed_matrix<double>& Q = m_Qs_SPECIALK[nPointIndex];
4868 bounded_vector<double, 3>& corrections = m_Point_Corrections[nPointIndex];
4879 NIC -= prod(Q, m_Image_Solution);
4899 dLat = -180.0 - dLat;
4900 dLon = dLon + 180.0;
4903 dLat = 180.0 - dLat;
4904 dLon = dLon + 180.0;
4906 while (dLon > 360.0) dLon = dLon - 360.0;
4907 while (dLon < 0.0) dLon = dLon + 360.0;
4909 dRad += 1000.*dRadCorr;
4914 corrections(0) += dLatCorr;
4915 corrections(1) += dLongCorr;
4916 corrections(2) += dRadCorr;
4920 SurfacePoint surfacepoint = point->GetAdjustedSurfacePoint();
4951 double BundleAdjust::ComputeResiduals() {
4953 double vtpv_control = 0.0;
4954 double vtpv_image = 0.0;
4968 int nObjectPoints = m_pCnet->GetNumPoints();
4969 for (
int i = 0; i < nObjectPoints; i++) {
4971 if (point->IsIgnored())
4976 int nMeasures = point->GetNumMeasures();
4977 for (
int j = 0; j < nMeasures; j++) {
4979 if (measure->IsIgnored())
4982 dWeight = 1.4 * (measure->Camera())->PixelPitch();
4983 dWeight = 1.0 / dWeight;
4986 vx = measure->GetFocalPlaneMeasuredX() - measure->GetFocalPlaneComputedX();
4987 vy = measure->GetFocalPlaneMeasuredY() - measure->GetFocalPlaneComputedY();
4992 if (measure->IsRejected())
4995 m_Statsrx.AddData(vx);
4996 m_Statsry.AddData(vy);
4997 m_Statsrxy.AddData(vx);
4998 m_Statsrxy.AddData(vy);
5002 vtpv += vx * vx * dWeight + vy * vy * dWeight;
5010 int nPointIndex = 0;
5011 for (
int i = 0; i < nObjectPoints; i++) {
5013 if ( point->IsIgnored() )
5017 bounded_vector<double, 3>& weights = m_Point_Weights[nPointIndex];
5018 bounded_vector<double, 3>& corrections = m_Point_Corrections[nPointIndex];
5024 if( weights[0] > 0.0 )
5025 vtpv_control += corrections[0] * corrections[0] * weights[0];
5026 if( weights[1] > 0.0 )
5027 vtpv_control += corrections[1] * corrections[1] * weights[1];
5028 if( weights[2] > 0.0 )
5029 vtpv_control += corrections[2] *corrections[2] * weights[2];
5039 for (
int j = 0; j < m_nNumImagePartials; j++) {
5040 if (m_dImageParameterWeights[j] > 0.0) {
5041 v = m_Image_Corrections[n];
5042 vtpv_image += v * v * m_dImageParameterWeights[j];
5049 while (n < m_nRank);
5053 vtpv = vtpv + vtpv_control + vtpv_image;
5057 m_drms_rx = m_Statsrx.Rms();
5058 m_drms_ry = m_Statsry.Rms();
5059 m_drms_rxy = m_Statsrxy.Rms();
5069 bool BundleAdjust::WrapUp() {
5073 int nObjectPoints = m_pCnet->GetNumPoints();
5074 for (
int i = 0; i < nObjectPoints; i++) {
5076 if (point->IsIgnored())
5082 ComputeBundleStatistics();
5092 bool BundleAdjust::ComputeBundleStatistics() {
5093 int nImages = Images();
5099 m_rmsImageSampleResiduals.resize(nImages);
5100 m_rmsImageLineResiduals.resize(nImages);
5101 m_rmsImageResiduals.resize(nImages);
5104 int nObservation = 0;
5105 int nObjectPoints = m_pCnet->GetNumPoints();
5106 for (
int i = 0; i < nObjectPoints; i++) {
5109 if ( point->IsIgnored() )
5112 if ( point->IsRejected() )
5115 nMeasures = point->GetNumMeasures();
5116 for (
int j = 0; j < nMeasures; j++) {
5119 if ( measure->IsIgnored() )
5122 if ( measure->IsRejected() )
5125 vsample = fabs(measure->GetSampleResidual());
5126 vline = fabs(measure->GetLineResidual());
5132 m_rmsImageSampleResiduals[nImageIndex].AddData(vsample);
5133 m_rmsImageLineResiduals[nImageIndex].AddData(vline);
5134 m_rmsImageResiduals[nImageIndex].AddData(vline);
5135 m_rmsImageResiduals[nImageIndex].AddData(vsample);
5141 if( m_bErrorPropagation )
5143 m_rmsImageXSigmas.resize(nImages);
5144 m_rmsImageYSigmas.resize(nImages);
5145 m_rmsImageZSigmas.resize(nImages);
5146 m_rmsImageRASigmas.resize(nImages);
5147 m_rmsImageDECSigmas.resize(nImages);
5148 m_rmsImageTWISTSigmas.resize(nImages);
5155 double dSigmaLat, dSigmaLong, dSigmaRadius;
5157 int nPoints = m_pCnet->GetNumPoints();
5158 for (
int i = 0; i < nPoints; i++ ) {
5161 if ( point->IsIgnored() )
5166 dSigmaRadius = point->GetAdjustedSurfacePoint().GetLocalRadiusSigma().
meters();
5168 sigmaLatitude.
AddData(dSigmaLat);
5169 sigmaLongitude.
AddData(dSigmaLong);
5170 sigmaRadius.
AddData(dSigmaRadius);
5173 if ( dSigmaLat > m_dmaxSigmaLatitude ) {
5174 m_dmaxSigmaLatitude = dSigmaLat;
5175 m_idMaxSigmaLatitude = point->
GetId();
5177 if ( dSigmaLong > m_dmaxSigmaLongitude ) {
5178 m_dmaxSigmaLongitude = dSigmaLong;
5179 m_idMaxSigmaLongitude = point->
GetId();
5181 if ( m_bSolveRadii ) {
5182 if ( dSigmaRadius > m_dmaxSigmaRadius ) {
5183 m_dmaxSigmaRadius = dSigmaRadius;
5184 m_idMaxSigmaRadius = point->
GetId();
5187 if ( dSigmaLat < m_dminSigmaLatitude ) {
5188 m_dminSigmaLatitude = dSigmaLat;
5189 m_idMinSigmaLatitude = point->
GetId();
5191 if ( dSigmaLong < m_dminSigmaLongitude ) {
5192 m_dminSigmaLongitude = dSigmaLong;
5193 m_idMinSigmaLongitude = point->
GetId();
5195 if ( m_bSolveRadii ) {
5196 if ( dSigmaRadius < m_dminSigmaRadius ) {
5197 m_dminSigmaRadius = dSigmaRadius;
5198 m_idMinSigmaRadius = point->
GetId();
5203 m_dmaxSigmaLatitude = dSigmaLat;
5204 m_dmaxSigmaLongitude = dSigmaLong;
5206 m_dminSigmaLatitude = dSigmaLat;
5207 m_dminSigmaLongitude = dSigmaLong;
5209 if ( m_bSolveRadii ) {
5210 m_dmaxSigmaRadius = dSigmaRadius;
5211 m_dminSigmaRadius = dSigmaRadius;
5216 m_drms_sigmaLat = sigmaLatitude.
Rms();
5217 m_drms_sigmaLon = sigmaLongitude.
Rms();
5218 m_drms_sigmaRad = sigmaRadius.
Rms();
5229 bool BundleAdjust::ComputeRejectionLimit() {
5232 int nResiduals = m_nObservations / 2;
5238 std::vector<double> resvectors;
5240 resvectors.resize(nResiduals);
5243 int nObservation = 0;
5244 int nObjectPoints = m_pCnet->GetNumPoints();
5245 for (
int i = 0; i < nObjectPoints; i++) {
5248 if ( point->IsIgnored() )
5251 if ( point->IsRejected() )
5254 int nMeasures = point->GetNumMeasures();
5255 for (
int j = 0; j < nMeasures; j++) {
5258 if ( measure->IsIgnored() )
5261 if ( measure->IsRejected() )
5264 vx = measure->GetSampleResidual();
5265 vy = measure->GetLineResidual();
5267 resvectors[nObservation] = sqrt(vx*vx + vy*vy);
5277 std::sort(resvectors.begin(), resvectors.end());
5288 int nmidpoint = nResiduals / 2;
5290 if ( nResiduals % 2 == 0 )
5291 median = (resvectors[nmidpoint-1] + resvectors[nmidpoint]) / 2;
5293 median = resvectors[nmidpoint];
5298 for (
int i = 0; i < nResiduals; i++)
5299 resvectors[i] = fabs(resvectors[i] - median);
5301 std::sort(resvectors.begin(), resvectors.end());
5305 if ( nResiduals % 2 == 0 )
5306 mediandev = (resvectors[nmidpoint-1] + resvectors[nmidpoint]) / 2;
5308 mediandev = resvectors[nmidpoint];
5310 std::cout <<
"median deviation: " << mediandev << std::endl;
5312 mad = 1.4826 * mediandev;
5314 std::cout <<
"mad: " << mad << std::endl;
5322 m_dRejectionLimit = median + m_dRejectionMultiplier * mad;
5324 std::cout <<
"Rejection Limit: " << m_dRejectionLimit << std::endl;
5334 bool BundleAdjust::FlagOutliers() {
5337 int ntotalrejected = 0;
5339 int nIndexMaxResidual;
5340 double dMaxResidual;
5342 double dUsedRejectionLimit = m_dRejectionLimit;
5348 int nComingBack = 0;
5350 int nObjectPoints = m_pCnet->GetNumPoints();
5351 for (
int i = 0; i < nObjectPoints; i++) {
5353 if ( point->IsIgnored() )
5359 nIndexMaxResidual = -1;
5360 dMaxResidual = -1.0;
5362 int nMeasures = point->GetNumMeasures();
5363 for (
int j = 0; j < nMeasures; j++) {
5366 if ( measure->IsIgnored() )
5369 vx = measure->GetSampleResidual();
5370 vy = measure->GetLineResidual();
5372 dSumSquares = sqrt(vx*vx + vy*vy);
5375 if ( dSumSquares <= dUsedRejectionLimit ) {
5378 if( measure->IsRejected() ) {
5379 printf(
"Coming back in: %s\r",point->
GetId().toAscii().data());
5389 if ( measure->IsRejected() ) {
5395 if ( dSumSquares > dMaxResidual ) {
5396 dMaxResidual = dSumSquares;
5397 nIndexMaxResidual = j;
5402 if ( dMaxResidual == -1.0 || dMaxResidual <= dUsedRejectionLimit ) {
5409 if ((nMeasures - (nRejected + 1)) < 2) {
5426 if( ( nMeasures-nRejected ) < 2 ) {
5428 printf(
"Rejecting Entire Point: %s\r",point->
GetId().toAscii().data());
5438 m_nRejectedObservations = 2*ntotalrejected;
5440 printf(
"\n\t Rejected Observations:%10d (Rejection Limit:%12.5lf\n",
5441 m_nRejectedObservations, dUsedRejectionLimit);
5443 std::cout <<
"Measures that came back: " << nComingBack << std::endl;
5452 bool BundleAdjust::errorPropagation() {
5453 if ( m_decompositionMethod == CHOLMOD )
5454 return errorPropagation_CHOLMOD();
5456 return errorPropagation_SPECIALK();
5465 bool BundleAdjust::errorPropagation_SPECIALK() {
5468 if ( !CholeskyUT_NOSQR_Inverse() )
5471 matrix<double> T(3, 3);
5472 matrix<double> QS(3, m_nRank);
5473 double dSigmaLat, dSigmaLong, dSigmaRadius;
5476 double dSigma02 = m_dSigma0 * m_dSigma0;
5478 int nPointIndex = 0;
5479 int nObjectPoints = m_pCnet->GetNumPoints();
5480 for (
int i = 0; i < nObjectPoints; i++) {
5482 if ( point->IsIgnored() )
5485 if ( point->IsRejected() )
5488 printf(
"\rProcessing point %d of %d",i+1,nObjectPoints);
5494 compressed_matrix<double>& Q = m_Qs_SPECIALK[nPointIndex];
5497 QS = prod(Q, m_Normals);
5500 T = prod(QS, trans(Q));
5506 dSigmaLat = SurfacePoint.GetLatSigma().
radians();
5507 dSigmaLong = SurfacePoint.GetLonSigma().
radians();
5508 dSigmaRadius = SurfacePoint.GetLocalRadiusSigma().
meters();
5516 t = dSigmaLat*dSigmaLat + T(0, 0);
5517 Distance tLatSig(sqrt(dSigma02 * t) * m_dRTM, Distance::Meters);
5519 t = dSigmaLong*dSigmaLong + T(1, 1);
5520 t = sqrt(dSigma02 * t) * m_dRTM;
5525 t = dSigmaRadius*dSigmaRadius + T(2, 2);
5526 t = sqrt(dSigma02 * t) * 1000.0;
5650 bool BundleAdjust::errorPropagation_CHOLMOD() {
5653 cholmod_free_triplet(&m_pTriplet, &m_cm);
5654 cholmod_free_sparse(&m_N, &m_cm);
5656 matrix<double> T(3, 3);
5657 double dSigmaLat, dSigmaLong, dSigmaRadius;
5660 double dSigma02 = m_dSigma0 * m_dSigma0;
5662 int nObjectPoints = m_pCnet->GetNumPoints();
5665 printf(
" Time: %s\n\n", strTime.c_str());
5668 std::vector< symmetric_matrix<double> > point_covs(nObjectPoints,symmetric_matrix<double>(3));
5669 for (
int d = 0; d < nObjectPoints; d++)
5670 point_covs[d].clear();
5673 m_Image_AdjustedSigmas.resize(m_SparseNormals.size());
5678 b = cholmod_zeros ( m_nRank, 1, CHOLMOD_REAL, &m_cm );
5679 double* pb = (
double*)b->x;
5686 int nCurrentColumn = 0;
5687 int ncolsCurrentBlockColumn = 0;
5689 int nBlockColumns = m_SparseNormals.size();
5690 for (i = 0; i < nBlockColumns; i++) {
5696 int nRows = m_SparseNormals.at(i)->numberOfRows();
5697 sbcMatrix.InsertMatrixBlock(i, nRows, ncolsCurrentBlockColumn);
5698 sbcMatrix.zeroBlocks();
5702 int nRows = m_SparseNormals.at(i)->numberOfRows();
5703 sbcMatrix.InsertMatrixBlock(i, nRows, ncolsCurrentBlockColumn);
5704 sbcMatrix.zeroBlocks();
5713 for (j = 0; j < (i+1); j++) {
5717 sbcMatrix.InsertMatrixBlock(j, nRows, ncolsCurrentBlockColumn);
5725 for (j = 0; j < ncolsCurrentBlockColumn; j++) {
5726 if ( nCurrentColumn > 0 )
5727 pb[nCurrentColumn-1] = 0.0;
5728 pb[nCurrentColumn] = 1.0;
5730 x = cholmod_solve ( CHOLMOD_A, m_L, b, &m_cm );
5735 for ( k = 0; k < sbcMatrix.size(); k++ ) {
5736 matrix<double>* matrix = sbcMatrix.value(k);
5738 int sz1 = matrix->size1();
5740 for (
int ii = 0; ii < sz1; ii++) {
5741 (*matrix)(ii,localCol) = px[ii + rp];
5743 rp += matrix->size1();
5749 cholmod_free_dense(&x,&m_cm);
5753 m_Image_AdjustedSigmas[i].resize(ncolsCurrentBlockColumn);
5754 matrix<double>* imageCovMatrix = sbcMatrix.value(i);
5755 for(
int z = 0; z < ncolsCurrentBlockColumn; z++) {
5756 m_Image_AdjustedSigmas[i][z] = (*imageCovMatrix)(z,z);
5760 int nPointIndex = 0;
5761 for (j = 0; j < nObjectPoints; j++) {
5764 if ( point->IsIgnored() )
5767 if ( point->IsRejected() )
5772 printf(
"\rError Propagation: Inverse Block %8d of %8d; Point %8d of %8d", i+1,
5773 nBlockColumns, j+1, nObjectPoints);
5782 symmetric_matrix<double>& cv = point_covs[nPointIndex];
5785 matrix<double>* qT = Q.value(i);
5793 QMapIterator<int, matrix<double>*> it(Q);
5794 while ( it.hasNext() ) {
5797 int nKey = it.key();
5802 matrix<double>* q = it.value();
5807 matrix<double>* nI = sbcMatrix.value(it.key());
5812 T = prod(*nI, trans(*qT));
5826 m_SparseNormals.wipe();
5830 printf(
"\rFilling point covariance matrices: Time %s", strTime.c_str());
5834 int nPointIndex = 0;
5835 for (j = 0; j < nObjectPoints; j++) {
5838 if ( point->IsIgnored() )
5841 if ( point->IsRejected() )
5845 printf(
"\rError Propagation: Filling point covariance matrices %8d of %8d",j+1,
5850 symmetric_matrix<double>& cv = point_covs[nPointIndex];
5856 dSigmaLat = SurfacePoint.GetLatSigma().
radians();
5857 dSigmaLong = SurfacePoint.GetLonSigma().
radians();
5858 dSigmaRadius = SurfacePoint.GetLocalRadiusSigma().
meters();
5860 t = dSigmaLat*dSigmaLat + cv(0, 0);
5861 Distance tLatSig(sqrt(dSigma02 * t) * m_dRTM, Distance::Meters);
5863 t = dSigmaLong*dSigmaLong + cv(1, 1);
5864 t = sqrt(dSigma02 * t) * m_dRTM;
5869 t = dSigmaRadius*dSigmaRadius + cv(2, 2);
5870 t = sqrt(dSigma02 * t) * 1000.0;
5894 int nImages = Images();
5895 for (
int i = 0; i < nImages; i++) {
5896 if (m_nHeldImages > 0)
5897 if ((m_pHeldSnList->HasSerialNumber(m_pSnList->SerialNumber(i))))
continue;
5901 index = ImageIndex(index);
5902 if (m_spacecraftPositionSolveType != Nothing) {
5904 std::vector<double> coefX(m_nNumberCamPosCoefSolved),
5905 coefY(m_nNumberCamPosCoefSolved),
5906 coefZ(m_nNumberCamPosCoefSolved);
5912 for (
int icoef = 0; icoef < m_nNumberCamPosCoefSolved; icoef++) {
5918 for (
int icoef = 0; icoef < m_nNumberCamPosCoefSolved; icoef++) {
5924 for (
int icoef = 0; icoef < m_nNumberCamPosCoefSolved; icoef++) {
5929 pInstPos->
SetPolynomial(coefX, coefY, coefZ, m_nPositionType);
5932 if (m_cmatrixSolveType != None) {
5934 std::vector<double> coefRA(m_nNumberCamAngleCoefSolved),
5935 coefDEC(m_nNumberCamAngleCoefSolved),
5936 coefTWI(m_nNumberCamAngleCoefSolved);
5940 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
5946 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
5951 if (m_bSolveTwist) {
5953 for (
int icoef = 0; icoef < m_nNumberCamAngleCoefSolved; icoef++) {
5959 pInstRot->
SetPolynomial(coefRA, coefDEC, coefTWI, m_nPointingType);
5964 int nObjectPoints = m_pCnet->GetNumPoints();
5965 for (
int i = 0; i < nObjectPoints; i++) {
5967 if (point->IsIgnored())
5969 if (m_strSolutionMethod !=
"SPECIALK" &&
5970 m_strSolutionMethod !=
"SPARSE" &&
5971 m_strSolutionMethod !=
"OLDSPARSE" &&
5972 point->
GetType() == ControlPoint::Fixed)
5978 int index = PointIndex(i);
5987 dLat = -180.0 - dLat;
5988 dLon = dLon + 180.0;
5991 dLat = 180.0 - dLat;
5992 dLon = dLon + 180.0;
5994 while (dLon > 360.0) dLon = dLon - 360.0;
5995 while (dLon < 0.0) dLon = dLon + 360.0;
5998 if ( m_bSolveRadii || m_strSolutionMethod ==
"OLDSPARSE") {
6006 latrec_c(dRad * 0.001,
6032 int BundleAdjust::PointIndex(
int i)
const {
6035 if (!m_bObservationMode)
6036 nIndex = Images() * m_nNumImagePartials;
6038 nIndex = Observations() * m_nNumImagePartials;
6040 nIndex += m_nPointIndexMap[i] * m_nNumPointPartials;
6050 int BundleAdjust::ImageIndex (
int i)
const {
6051 if ( !m_bObservationMode )
6052 return m_nImageIndexMap[i] * m_nNumImagePartials;
6054 return m_pObsNumList->ObservationNumberMapIndex(i) * m_nNumImagePartials;
6062 QString BundleAdjust::FileName(
int i) {
6065 return m_pSnList->FileName(i);
6073 bool BundleAdjust::IsHeld(
int i) {
6074 if ( m_nHeldImages > 0 )
6075 if ((m_pHeldSnList->HasSerialNumber(m_pSnList->SerialNumber(i))))
6086 return m_pCnet->Camera(i)->instrumentRotation()->Cache(
"InstrumentPointing");
6092 return m_pCnet->Camera(i)->instrumentPosition()->Cache(
"InstrumentPosition");
6100 int BundleAdjust::Observations()
const {
6101 if (!m_bObservationMode)
6102 return m_pSnList->
Size();
6105 return m_pObsNumList->ObservationSize();
6158 void BundleAdjust::IterationSummary(
double avErr,
double sigmaXY,
double sigmaHat,
6159 double sigmaX,
double sigmaY) {
6161 QString itlog =
"Iteration" +
toString(m_nIteration);
6172 if (m_maxLikelihoodFlag[m_maxLikelihoodIndex]) {
6174 gp +=
PvlKeyword(
"Median_of_R^2_residuals: ",
toString(m_maxLikelihoodMedianR2Residuals));
6177 std::ostringstream ostr;
6179 m_iterationSummary += QString::fromStdString(ostr.str());
6180 if (m_bPrintSummary) Application::Log(gp);
6188 void BundleAdjust::SpecialKIterationSummary() {
6191 itlog =
"Iteration" +
toString(m_nIteration) +
": Final";
6193 itlog =
"Iteration" +
toString(m_nIteration);
6198 gp +=
PvlKeyword(
"Constrained_Point_Parameters",
toString(m_nConstrainedPointParameters));
6199 gp +=
PvlKeyword(
"Constrained_Image_Parameters",
toString(m_nConstrainedImageParameters));
6204 if (m_maxLikelihoodFlag[m_maxLikelihoodIndex]) {
6206 gp +=
PvlKeyword(
"Median_of_R^2_residuals: ",
toString(m_maxLikelihoodMedianR2Residuals));
6209 if ( m_bConverged ) {
6213 if ( m_bErrorPropagation )
6217 std::ostringstream ostr;
6219 m_iterationSummary += QString::fromStdString(ostr.str());
6220 if (m_bPrintSummary) Application::Log(gp);
6230 bool BundleAdjust::SetParameterWeights() {
6232 SetSpaceCraftWeights();
6233 ComputeImageParameterWeights();
6235 m_dParameterWeights.resize(m_nBasisColumns);
6236 std::fill_n(m_dParameterWeights.begin(), m_nBasisColumns, 0.0);
6237 m_nConstrainedImageParameters = 0;
6238 m_nConstrainedPointParameters = 0;
6240 int nconstraintsperimage = std::count_if( m_dImageParameterWeights.begin(),
6241 m_dImageParameterWeights.end(),
6242 std::bind2nd(std::greater<double>(),0.0));
6245 int nCurrentIndex = -1;
6246 int nImages = m_pSnList->Size();
6247 for(
int i = 0; i < nImages; i++ ) {
6249 nWtIndex = ImageIndex(i);
6250 if ( nWtIndex == nCurrentIndex )
6253 nCurrentIndex = nWtIndex;
6255 if ( m_pHeldSnList != NULL &&
6256 (m_pHeldSnList->HasSerialNumber(m_pSnList->SerialNumber(i))) ) {
6257 std::fill_n(m_dParameterWeights.begin()+nWtIndex, m_nNumImagePartials, 1.0e+50);
6258 m_nConstrainedImageParameters += m_nNumImagePartials;
6261 std::copy(m_dImageParameterWeights.begin(),
6262 m_dImageParameterWeights.end(),
6263 m_dParameterWeights.begin()+nWtIndex);
6264 m_nConstrainedImageParameters += nconstraintsperimage;
6269 nWtIndex = m_nImageParameters;
6270 int nObjectPoints = m_pCnet->GetNumPoints();
6272 m_Point_AprioriSigmas.resize(nObjectPoints);
6275 for (
int i = 0; i < nObjectPoints; i++) {
6276 m_Point_AprioriSigmas[i].clear();
6279 int nPointIndex = 0;
6280 for (
int i = 0; i < nObjectPoints; i++) {
6282 if (point->IsIgnored())
6285 bounded_vector<double, 3>& apriorisigmas = m_Point_AprioriSigmas[nPointIndex];
6287 SurfacePoint aprioriSurfacePoint = point->GetAprioriSurfacePoint();
6291 if (point->
GetType() == ControlPoint::Fixed) {
6292 m_dParameterWeights[nWtIndex] = 1.0e+50;
6293 m_dParameterWeights[nWtIndex+1] = 1.0e+50;
6294 m_dParameterWeights[nWtIndex+2] = 1.0e+50;
6295 m_nConstrainedPointParameters += 3;
6298 if( point->IsLatitudeConstrained() ) {
6300 m_dParameterWeights[nWtIndex] = point->GetAprioriSurfacePoint().
GetLatWeight();
6301 m_nConstrainedPointParameters++;
6303 else if( m_dGlobalLatitudeAprioriSigma > 0.0 ) {
6304 apriorisigmas[0] = m_dGlobalLatitudeAprioriSigma;
6305 m_dParameterWeights[nWtIndex] = 1. / (m_dGlobalLatitudeAprioriSigma*m_dMTR);
6306 m_dParameterWeights[nWtIndex] *= m_dParameterWeights[nWtIndex];
6307 m_nConstrainedPointParameters++;
6310 if( point->IsLongitudeConstrained() ) {
6312 m_dParameterWeights[nWtIndex+1] = point->GetAprioriSurfacePoint().
GetLonWeight();
6313 m_nConstrainedPointParameters++;
6315 else if( m_dGlobalLongitudeAprioriSigma > 0.0 ) {
6316 apriorisigmas[1] = m_dGlobalLongitudeAprioriSigma;
6317 m_dParameterWeights[nWtIndex+1] = 1. / (m_dGlobalLongitudeAprioriSigma*m_dMTR);
6318 m_dParameterWeights[nWtIndex+1] *= m_dParameterWeights[nWtIndex+1];
6319 m_nConstrainedPointParameters++;
6322 if ( !m_bSolveRadii ) {
6323 m_dParameterWeights[nWtIndex+2] = 1.0e+50;
6324 m_nConstrainedPointParameters++;
6326 else if( point->IsRadiusConstrained() ) {
6327 apriorisigmas[2] = point->GetAprioriSurfacePoint().GetLocalRadiusSigma().
meters();
6329 m_nConstrainedPointParameters++;
6331 else if( m_dGlobalRadiusAprioriSigma > 0.0 ) {
6332 apriorisigmas[2] = m_dGlobalRadiusAprioriSigma;
6333 m_dParameterWeights[nWtIndex+2] = 1000. / m_dGlobalRadiusAprioriSigma;
6334 m_dParameterWeights[nWtIndex+2] *= m_dParameterWeights[nWtIndex+2];
6335 m_nConstrainedPointParameters++;
6343 m_pLsq->SetParameterWeights(m_dParameterWeights);
6346 m_pLsq->SetNumberOfConstrainedParameters(
6347 m_nConstrainedPointParameters+m_nConstrainedImageParameters);
6357 void BundleAdjust::SetPostBundleSigmas() {
6360 double dSigmaRadius;
6363 gmm::row_matrix<gmm::rsvector<double> > lsqCovMatrix = m_pLsq->GetCovarianceMatrix();
6365 int nIndex = m_nImageParameters;
6367 int nPoints = m_pCnet->GetNumPoints();
6368 for (
int i = 0; i < nPoints; i++) {
6370 if (point->IsIgnored())
6373 dSigmaLat = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
6374 dSigmaLat *= m_dRTM;
6377 dSigmaLong = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
6378 dSigmaLong *= m_dRTM * cos(point->GetAdjustedSurfacePoint().
GetLatitude().
radians());
6381 dSigmaRadius = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
6388 Distance(dSigmaLat, Distance::Meters),
6389 Distance(dSigmaLong, Distance::Meters),
6390 Distance(dSigmaRadius, Distance::Kilometers));
6400 bool BundleAdjust::Output() {
6401 if (m_bOutputStandard) {
6402 if (m_bConverged && m_bErrorPropagation)
6403 OutputWithErrorPropagation();
6405 OutputNoErrorPropagation();
6413 if (m_bOutputResiduals)
6424 bool BundleAdjust::OutputHeader(std::ofstream& fp_out) {
6429 int nImages = Images();
6430 int nValidPoints = m_pCnet->GetNumValidPoints();
6431 int nInnerConstraints = 0;
6432 int nDistanceConstraints = 0;
6433 int nDegreesOfFreedom = m_nObservations + m_nConstrainedPointParameters + m_nConstrainedImageParameters - m_nUnknownParameters;
6434 int nConvergenceCriteria = 1;
6437 sprintf(buf,
"JIGSAW: BUNDLE ADJUSTMENT\n=========================\n");
6439 sprintf(buf,
"JIGSAW (DELTACK or QTIE): BUNDLE ADJUSTMENT\n=========================\n");
6444 sprintf(buf,
"\n Network Filename: %s", m_strCnetFileName.toAscii().data());
6446 sprintf(buf,
"\n Network Id: %s", m_pCnet->GetNetworkId().toAscii().data());
6448 sprintf(buf,
"\n Network Description: %s", m_pCnet->Description().toAscii().data());
6450 sprintf(buf,
"\n Target: %s", m_pCnet->GetTarget().toAscii().data());
6452 sprintf(buf,
"\n\n Linear Units: kilometers");
6454 sprintf(buf,
"\n Angular Units: decimal degrees");
6456 sprintf(buf,
"\n\nINPUT: SOLVE OPTIONS\n====================\n");
6458 m_bObservationMode ? sprintf(buf,
"\n OBSERVATIONS: ON"):
6459 sprintf(buf,
"\n OBSERVATIONS: OFF");
6461 m_bSolveRadii ? sprintf(buf,
"\n RADIUS: ON"):
6462 sprintf(buf,
"\n RADIUS: OFF");
6464 m_bUpdateCubes ? sprintf(buf,
"\n UPDATE: YES"):
6465 sprintf(buf,
"\n UPDATE: NO");
6467 sprintf(buf,
"\n SOLUTION TYPE: %s", m_strSolutionMethod.toAscii().data());
6469 m_bErrorPropagation ? sprintf(buf,
"\n ERROR PROPAGATION: ON"):
6470 sprintf(buf,
"\n ERROR PROPAGATION: OFF");
6472 m_bOutlierRejection ? sprintf(buf,
"\n OUTLIER REJECTION: ON"):
6473 sprintf(buf,
"\n OUTLIER REJECTION: OFF");
6475 sprintf(buf,
"\n REJECTION MULTIPLIER: %lf",m_dRejectionMultiplier);
6477 sprintf(buf,
"\n\nMAXIMUM LIKELIHOOD ESTIMATION\n============================\n");
6479 for (
int tier=0;tier<3;tier++) {
6480 if (m_maxLikelihoodFlag[tier]) {
6481 sprintf(buf,
"\n Tier %d Enabled: TRUE",tier);
6483 sprintf(buf,
"\n Maximum Likelihood Model: ");
6485 m_wFunc[tier]->maximumLikelihoodModel(buf);
6487 sprintf(buf,
"\n Quantile used for tweaking constant: %lf",m_maxLikelihoodQuan[tier]);
6489 sprintf(buf,
"\n Quantile weighted R^2 Residual value: %lf",m_wFunc[tier]->tweakingConstant());
6491 sprintf(buf,
"\n Approx. weighted Residual cutoff: ");
6493 m_wFunc[tier]->weightedResidualCutoff(buf);
6495 if (tier != 2) fp_out <<
"\n";
6498 sprintf(buf,
"\n Tier %d Enabled: FALSE",tier);
6502 sprintf(buf,
"\n\nINPUT: CONVERGENCE CRITERIA\n===========================\n");
6504 sprintf(buf,
"\n SIGMA0: %e",m_dConvergenceThreshold);
6506 sprintf(buf,
"\n MAXIMUM ITERATIONS: %d",m_nMaxIterations);
6508 sprintf(buf,
"\n\nINPUT: CAMERA POINTING OPTIONS\n==============================\n");
6510 switch (m_cmatrixSolveType) {
6511 case BundleAdjust::AnglesOnly:
6512 sprintf(buf,
"\n CAMSOLVE: ANGLES");
6514 case BundleAdjust::AnglesVelocity:
6515 sprintf(buf,
"\n CAMSOLVE: ANGLES, VELOCITIES");
6517 case BundleAdjust::AnglesVelocityAcceleration:
6518 sprintf(buf,
"\n CAMSOLVE: ANGLES, VELOCITIES, ACCELERATIONS");
6520 case BundleAdjust::CKAll:
6521 sprintf(buf,
"\n CAMSOLVE: ALL POLYNOMIAL COEFFICIENTS (%d)",m_nsolveCKDegree);
6523 case BundleAdjust::None:
6524 sprintf(buf,
"\n CAMSOLVE: NONE");
6530 m_bSolveTwist ? sprintf(buf,
"\n TWIST: ON"):
6531 sprintf(buf,
"\n TWIST: OFF");
6534 m_bSolvePolyOverPointing ? sprintf(buf,
"\n POLYNOMIAL OVER EXISTING POINTING: ON"):
6535 sprintf(buf,
"\nPOLYNOMIAL OVER EXISTING POINTING : OFF");
6538 sprintf(buf,
"\n\nINPUT: SPACECRAFT OPTIONS\n=========================\n");
6540 switch (m_spacecraftPositionSolveType) {
6542 sprintf(buf,
"\n SPSOLVE: NONE");
6545 sprintf(buf,
"\n SPSOLVE: POSITION");
6547 case PositionVelocity:
6548 sprintf(buf,
"\n SPSOLVE: POSITION, VELOCITIES");
6550 case PositionVelocityAcceleration:
6551 sprintf(buf,
"\n SPSOLVE: POSITION, VELOCITIES, ACCELERATIONS");
6553 case BundleAdjust::SPKAll:
6554 sprintf(buf,
"\n CAMSOLVE: ALL POLYNOMIAL COEFFICIENTS (%d)",m_nsolveSPKDegree);
6561 m_bSolvePolyOverHermite ? sprintf(buf,
"\n POLYNOMIAL OVER HERMITE SPLINE: ON"):
6562 sprintf(buf,
"\nPOLYNOMIAL OVER HERMITE SPLINE : OFF");
6565 sprintf(buf,
"\n\nINPUT: GLOBAL IMAGE PARAMETER UNCERTAINTIES\n===========================================\n");
6567 (m_dGlobalLatitudeAprioriSigma == -1) ? sprintf(buf,
"\n POINT LATITUDE SIGMA: N/A"):
6568 sprintf(buf,
"\n POINT LATITUDE SIGMA: %lf (meters)",m_dGlobalLatitudeAprioriSigma);
6570 (m_dGlobalLongitudeAprioriSigma == -1) ? sprintf(buf,
"\n POINT LONGITUDE SIGMA: N/A"):
6571 sprintf(buf,
"\n POINT LONGITUDE SIGMA: %lf (meters)",m_dGlobalLongitudeAprioriSigma);
6573 (m_dGlobalRadiusAprioriSigma == -1) ? sprintf(buf,
"\n POINT RADIUS SIGMA: N/A"):
6574 sprintf(buf,
"\n POINT RADIUS SIGMA: %lf (meters)",m_dGlobalRadiusAprioriSigma);
6577 if (m_nNumberCamPosCoefSolved < 1 || m_dGlobalSpacecraftPositionAprioriSigma[0] == -1)
6578 sprintf(buf,
"\n SPACECRAFT POSITION SIGMA: N/A");
6580 sprintf(buf,
"\n SPACECRAFT POSITION SIGMA: %lf (meters)",m_dGlobalSpacecraftPositionAprioriSigma[0]);
6583 if (m_nNumberCamPosCoefSolved < 2 || m_dGlobalSpacecraftPositionAprioriSigma[1] == -1)
6584 sprintf(buf,
"\n SPACECRAFT VELOCITY SIGMA: N/A");
6586 sprintf(buf,
"\n SPACECRAFT VELOCITY SIGMA: %lf (m/s)",m_dGlobalSpacecraftPositionAprioriSigma[1]);
6589 if (m_nNumberCamPosCoefSolved < 3 || m_dGlobalSpacecraftPositionAprioriSigma[2] == -1)
6590 sprintf(buf,
"\n SPACECRAFT ACCELERATION SIGMA: N/A");
6592 sprintf(buf,
"\n SPACECRAFT ACCELERATION SIGMA: %lf (m/s/s)",m_dGlobalSpacecraftPositionAprioriSigma[2]);
6595 if (m_nNumberCamAngleCoefSolved < 1 || m_dGlobalCameraAnglesAprioriSigma[0] == -1)
6596 sprintf(buf,
"\n CAMERA ANGLES SIGMA: N/A");
6598 sprintf(buf,
"\n CAMERA ANGLES SIGMA: %lf (dd)",m_dGlobalCameraAnglesAprioriSigma[0]);
6601 if (m_nNumberCamAngleCoefSolved < 2 || m_dGlobalCameraAnglesAprioriSigma[1] == -1)
6602 sprintf(buf,
"\n CAMERA ANGULAR VELOCITY SIGMA: N/A");
6604 sprintf(buf,
"\n CAMERA ANGULAR VELOCITY SIGMA: %lf (dd/s)",m_dGlobalCameraAnglesAprioriSigma[1]);
6607 if (m_nNumberCamAngleCoefSolved < 3 || m_dGlobalCameraAnglesAprioriSigma[2] == -1)
6608 sprintf(buf,
"\n CAMERA ANGULAR ACCELERATION SIGMA: N/A");
6610 sprintf(buf,
"\n CAMERA ANGULAR ACCELERATION SIGMA: %lf (dd/s/s)",m_dGlobalCameraAnglesAprioriSigma[2]);
6613 sprintf(buf,
"\n\nJIGSAW: RESULTS\n===============\n");
6615 sprintf(buf,
"\n Images: %6d",nImages);
6617 sprintf(buf,
"\n Points: %6d",nValidPoints);
6619 sprintf(buf,
"\n Total Measures: %6d",(m_nObservations+m_nRejectedObservations)/2);
6621 sprintf(buf,
"\n Total Observations: %6d",m_nObservations+m_nRejectedObservations);
6623 sprintf(buf,
"\n Good Observations: %6d",m_nObservations);
6625 sprintf(buf,
"\n Rejected Observations: %6d",m_nRejectedObservations);
6628 if (m_nConstrainedPointParameters > 0) {
6629 sprintf(buf,
"\n Constrained Point Parameters: %6d",m_nConstrainedPointParameters);
6632 if (m_nConstrainedImageParameters > 0) {
6633 sprintf(buf,
"\n Constrained Image Parameters: %6d",m_nConstrainedImageParameters);
6636 sprintf(buf,
"\n Unknowns: %6d",m_nUnknownParameters);
6638 if (nInnerConstraints > 0) {
6639 sprintf(buf,
"\n Inner Constraints: %6d",nInnerConstraints);
6642 if (nDistanceConstraints > 0) {
6643 sprintf(buf,
"\n Distance Constraints: %d",nDistanceConstraints);
6647 sprintf(buf,
"\n Degrees of Freedom: %6d",nDegreesOfFreedom);
6649 sprintf(buf,
"\n Convergence Criteria: %6.3g",m_dConvergenceThreshold);
6652 if (nConvergenceCriteria == 1) {
6653 sprintf(buf,
"(Sigma0)");
6657 sprintf(buf,
"\n Iterations: %6d",m_nIteration);
6660 if (m_nIteration >= m_nMaxIterations) {
6661 sprintf(buf,
"(Maximum reached)");
6665 sprintf(buf,
"\n Sigma0: %30.20lf\n",m_dSigma0);
6667 sprintf(buf,
" Error Propagation Elapsed Time: %6.4lf (seconds)\n",m_dElapsedTimeErrorProp);
6669 sprintf(buf,
" Total Elapsed Time: %6.4lf (seconds)\n",m_dElapsedTime);
6671 if (m_nObservations+m_nRejectedObservations > 100) {
6672 sprintf(buf,
"\n Residual Percentiles:\n");
6675 for (
int bin=1;bin<34;bin++) {
6677 sprintf(buf,
" Percentile %3d: %+8.3lf Percentile %3d: %+8.3lf Percentile %3d: %+8.3lf\n",bin ,m_cumProRes->value(
double(bin )/100.0),
6678 bin+33,m_cumProRes->value(
double(bin+33)/100.0),
6679 bin+66,m_cumProRes->value(
double(bin+66)/100.0));
6684 QString msg =
"Faiiled to output residual percentiles for bundleout";
6688 sprintf(buf,
"\n Residual Box Plot:");
6690 sprintf(buf,
"\n minimum: %+8.3lf",m_cumProRes->min());
6692 sprintf(buf,
"\n Quartile 1: %+8.3lf",m_cumProRes->value(0.25));
6694 sprintf(buf,
"\n Median: %+8.3lf",m_cumProRes->value(0.50));
6696 sprintf(buf,
"\n Quartile 3: %+8.3lf",m_cumProRes->value(0.75));
6698 sprintf(buf,
"\n maximum: %+8.3lf\n",m_cumProRes->max());
6702 QString msg =
"Faiiled to output residual box plot for bundleout";
6707 sprintf(buf,
"\nIMAGE MEASURES SUMMARY\n==========================\n\n");
6711 int nRejectedMeasures;
6714 for (
int i = 0; i < nImages; i++) {
6717 double rmsSampleResiduals = m_rmsImageSampleResiduals[i].Rms();
6718 double rmsLineResiduals = m_rmsImageLineResiduals[i].Rms();
6719 double rmsLandSResiduals = m_rmsImageResiduals[i].Rms();
6721 nMeasures = m_pCnet->GetNumberOfValidMeasuresInImage(m_pSnList->SerialNumber(i));
6723 m_pCnet->GetNumberOfJigsawRejectedMeasuresInImage(m_pSnList->SerialNumber(i));
6725 nUsed = nMeasures - nRejectedMeasures;
6727 if (nUsed == nMeasures)
6728 sprintf(buf,
"%s %5d of %5d %6.3lf %6.3lf %6.3lf\n",
6729 m_pSnList->FileName(i).toAscii().data(),
6730 (nMeasures-nRejectedMeasures), nMeasures,
6731 rmsSampleResiduals,rmsLineResiduals,rmsLandSResiduals);
6733 sprintf(buf,
"%s %5d of %5d* %6.3lf %6.3lf %6.3lf\n",
6734 m_pSnList->FileName(i).toAscii().data(),
6735 (nMeasures-nRejectedMeasures), nMeasures,
6736 rmsSampleResiduals,rmsLineResiduals,rmsLandSResiduals);
6747 bool BundleAdjust::OutputWithErrorPropagation() {
6749 QString ofname(
"bundleout.txt");
6750 if( m_strOutputFilePrefix.length() != 0 )
6751 ofname = m_strOutputFilePrefix +
"_" + ofname;
6753 std::ofstream fp_out(ofname.toAscii().data(), std::ios::out);
6758 std::vector<double> coefX(m_nNumberCamPosCoefSolved);
6759 std::vector<double> coefY(m_nNumberCamPosCoefSolved);
6760 std::vector<double> coefZ(m_nNumberCamPosCoefSolved);
6761 std::vector<double> coefRA(m_nNumberCamAngleCoefSolved);
6762 std::vector<double> coefDEC(m_nNumberCamAngleCoefSolved);
6763 std::vector<double> coefTWI(m_nNumberCamAngleCoefSolved);
6764 std::vector<double> angles;
6769 int nImages = Images();
6772 bool bSolveSparse =
false;
6774 gmm::row_matrix<gmm::rsvector<double> > lsqCovMatrix;
6776 if (m_strSolutionMethod ==
"OLDSPARSE") {
6777 lsqCovMatrix = m_pLsq->GetCovarianceMatrix();
6778 bSolveSparse =
true;
6782 vector<double> vImageAdjustedSigmas;
6784 OutputHeader(fp_out);
6786 sprintf(buf,
"\nIMAGE EXTERIOR ORIENTATION\n==========================\n");
6789 for (
int i = 0; i < nImages; i++) {
6794 pCamera = m_pCnet->
Camera(i);
6799 nIndex = ImageIndex(i);
6801 if (m_decompositionMethod == CHOLMOD)
6802 vImageAdjustedSigmas = m_Image_AdjustedSigmas.at(i);
6805 if (!pSpicePosition)
6809 if (!pSpiceRotation)
6815 if (m_spacecraftPositionSolveType > 0)
6818 std::vector <double> coordinate(3);
6820 coefX.push_back(coordinate[0]);
6821 coefY.push_back(coordinate[1]);
6822 coefZ.push_back(coordinate[2]);
6825 if (m_cmatrixSolveType > 0)
6830 coefRA.push_back(angles.at(0));
6831 coefDEC.push_back(angles.at(1));
6832 coefTWI.push_back(angles.at(2));
6835 sprintf(buf,
"\nImage Full File Name: %s\n", m_pSnList->FileName(i).toAscii().data());
6837 sprintf(buf,
"\nImage Serial Number: %s\n", m_pSnList->SerialNumber(i).toAscii().data());
6839 sprintf(buf,
"\n Image Initial Total Final Initial Final\n"
6840 "Parameter Value Correction Value Accuracy Accuracy\n");
6843 int nSigmaIndex = 0;
6845 if (m_nNumberCamPosCoefSolved > 0) {
6846 char strcoeff =
'a' + m_nNumberCamPosCoefSolved -1;
6847 std::ostringstream ostr;
6848 for (
int i = 0; i < m_nNumberCamPosCoefSolved; i++) {
6850 ostr <<
" " << strcoeff;
6852 ostr <<
" " << strcoeff <<
"t";
6854 ostr << strcoeff <<
"t" << i;
6856 dSigma = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
6857 else if (m_decompositionMethod == CHOLMOD)
6858 dSigma = sqrt(vImageAdjustedSigmas[nSigmaIndex]) * m_dSigma0;
6859 else if (m_decompositionMethod == SPECIALK)
6860 dSigma = sqrt((
double)(m_Normals(nIndex, nIndex))) * m_dSigma0;
6862 sprintf(buf,
" X (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18.8lf\n",
6863 ostr.str().c_str(),coefX[i] - m_Image_Corrections(nIndex),
6864 m_Image_Corrections(nIndex), coefX[i],
6865 m_dGlobalSpacecraftPositionAprioriSigma[i], dSigma);
6868 sprintf(buf,
" (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18.8lf\n",
6869 ostr.str().c_str(),coefX[i] - m_Image_Corrections(nIndex),
6870 m_Image_Corrections(nIndex), coefX[i],
6871 m_dGlobalSpacecraftPositionAprioriSigma[i], dSigma);
6879 strcoeff =
'a' + m_nNumberCamPosCoefSolved -1;
6880 for (
int i = 0; i < m_nNumberCamPosCoefSolved; i++) {
6882 ostr <<
" " << strcoeff;
6884 ostr <<
" " << strcoeff <<
"t";
6886 ostr << strcoeff <<
"t" << i;
6888 dSigma = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
6889 else if (m_decompositionMethod == CHOLMOD)
6890 dSigma = sqrt(vImageAdjustedSigmas[nSigmaIndex]) * m_dSigma0;
6891 else if (m_decompositionMethod == SPECIALK)
6892 dSigma = sqrt((
double)(m_Normals(nIndex, nIndex))) * m_dSigma0;
6894 sprintf(buf,
" Y (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18.8lf\n",
6895 ostr.str().c_str(),coefY[i] - m_Image_Corrections(nIndex),
6896 m_Image_Corrections(nIndex), coefY[i],
6897 m_dGlobalSpacecraftPositionAprioriSigma[i], dSigma);
6900 sprintf(buf,
" (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18.8lf\n",
6901 ostr.str().c_str(),coefY[i] - m_Image_Corrections(nIndex),
6902 m_Image_Corrections(nIndex), coefY[i],
6903 m_dGlobalSpacecraftPositionAprioriSigma[i], dSigma);
6911 strcoeff =
'a' + m_nNumberCamPosCoefSolved -1;
6912 for (
int i = 0; i < m_nNumberCamPosCoefSolved; i++) {
6914 ostr <<
" " << strcoeff;
6916 ostr <<
" " << strcoeff <<
"t";
6918 ostr << strcoeff <<
"t" << i;
6920 dSigma = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
6921 else if (m_decompositionMethod == CHOLMOD)
6922 dSigma = sqrt(vImageAdjustedSigmas[nSigmaIndex]) * m_dSigma0;
6923 else if (m_decompositionMethod == SPECIALK)
6924 dSigma = sqrt((
double)(m_Normals(nIndex, nIndex))) * m_dSigma0;
6926 sprintf(buf,
" Z (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18.8lf\n",
6927 ostr.str().c_str(),coefZ[i] - m_Image_Corrections(nIndex),
6928 m_Image_Corrections(nIndex), coefZ[i],
6929 m_dGlobalSpacecraftPositionAprioriSigma[i], dSigma);
6932 sprintf(buf,
" (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18.8lf\n",
6933 ostr.str().c_str(),coefZ[i] - m_Image_Corrections(nIndex),
6934 m_Image_Corrections(nIndex), coefZ[i],
6935 m_dGlobalSpacecraftPositionAprioriSigma[i], dSigma);
6946 sprintf(buf,
" X%17.8lf%21.8lf%20.8lf%18.8lf%18s\n", coefX[0], 0.0, coefX[0], 0.0,
"N/A");
6948 sprintf(buf,
" Y%17.8lf%21.8lf%20.8lf%18.8lf%18s\n", coefY[0], 0.0, coefY[0], 0.0,
"N/A");
6950 sprintf(buf,
" Z%17.8lf%21.8lf%20.8lf%18.8lf%18s\n", coefZ[0], 0.0, coefZ[0], 0.0,
"N/A");
6954 if (m_nNumberCamAngleCoefSolved > 0) {
6955 char strcoeff =
'a' + m_nNumberCamAngleCoefSolved -1;
6956 std::ostringstream ostr;
6957 for (
int i = 0; i < m_nNumberCamAngleCoefSolved; i++) {
6959 ostr <<
" " << strcoeff;
6961 ostr <<
" " << strcoeff <<
"t";
6963 ostr << strcoeff <<
"t" << i;
6965 dSigma = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
6966 else if (m_decompositionMethod == CHOLMOD)
6967 dSigma = sqrt(vImageAdjustedSigmas[nSigmaIndex]) * m_dSigma0;
6968 else if (m_decompositionMethod == SPECIALK)
6969 dSigma = sqrt((
double)(m_Normals(nIndex, nIndex))) * m_dSigma0;
6971 sprintf(buf,
" RA (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18.8lf\n",
6972 ostr.str().c_str(),(coefRA[i] - m_Image_Corrections(nIndex)) *
RAD2DEG,
6974 m_dGlobalCameraAnglesAprioriSigma[i], dSigma *
RAD2DEG);
6977 sprintf(buf,
" (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18.8lf\n",
6978 ostr.str().c_str(),(coefRA[i] - m_Image_Corrections(nIndex)) *
RAD2DEG,
6980 m_dGlobalCameraAnglesAprioriSigma[i], dSigma *
RAD2DEG);
6988 strcoeff =
'a' + m_nNumberCamAngleCoefSolved -1;
6989 for (
int i = 0; i < m_nNumberCamAngleCoefSolved; i++) {
6991 ostr <<
" " << strcoeff;
6993 ostr <<
" " << strcoeff <<
"t";
6995 ostr << strcoeff <<
"t" << i;
6997 dSigma = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
6998 else if (m_decompositionMethod == CHOLMOD)
6999 dSigma = sqrt(vImageAdjustedSigmas[nSigmaIndex]) * m_dSigma0;
7000 else if (m_decompositionMethod == SPECIALK)
7001 dSigma = sqrt((
double)(m_Normals(nIndex, nIndex))) * m_dSigma0;
7003 sprintf(buf,
"DEC (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18.8lf\n",
7004 ostr.str().c_str(),(coefDEC[i] - m_Image_Corrections(nIndex))*
RAD2DEG,
7006 m_dGlobalCameraAnglesAprioriSigma[i], dSigma *
RAD2DEG);
7009 sprintf(buf,
" (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18.8lf\n",
7010 ostr.str().c_str(),(coefDEC[i] - m_Image_Corrections(nIndex))*
RAD2DEG,
7012 m_dGlobalCameraAnglesAprioriSigma[i], dSigma *
RAD2DEG);
7021 if (!m_bSolveTwist) {
7022 sprintf(buf,
" TWIST%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7023 coefTWI[0]*
RAD2DEG, 0.0, coefTWI[0]*RAD2DEG, 0.0,
"N/A");
7027 strcoeff =
'a' + m_nNumberCamAngleCoefSolved - 1;
7028 for (
int i = 0; i < m_nNumberCamAngleCoefSolved; i++) {
7030 ostr <<
" " << strcoeff;
7032 ostr <<
" " << strcoeff <<
"t";
7034 ostr << strcoeff <<
"t" << i;
7036 dSigma = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
7037 else if (m_decompositionMethod == CHOLMOD)
7038 dSigma = sqrt(vImageAdjustedSigmas[nSigmaIndex]) * m_dSigma0;
7039 else if (m_decompositionMethod == SPECIALK)
7040 dSigma = sqrt((
double)(m_Normals(nIndex, nIndex))) * m_dSigma0;
7042 sprintf(buf,
"TWI (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18.8lf\n",
7043 ostr.str().c_str(),(coefTWI[i] - m_Image_Corrections(nIndex))*
RAD2DEG,
7045 m_dGlobalCameraAnglesAprioriSigma[i], dSigma *
RAD2DEG);
7048 sprintf(buf,
" (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18.8lf\n",
7049 ostr.str().c_str(),(coefTWI[i] - m_Image_Corrections(nIndex))*
RAD2DEG,
7051 m_dGlobalCameraAnglesAprioriSigma[i], dSigma *
RAD2DEG);
7063 sprintf(buf,
" RA%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7064 coefRA[0]*
RAD2DEG, 0.0, coefRA[0]*RAD2DEG, 0.0,
"N/A");
7066 sprintf(buf,
" DEC%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7067 coefDEC[0]*RAD2DEG, 0.0, coefDEC[0]*RAD2DEG, 0.0,
"N/A");
7069 sprintf(buf,
" TWIST%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7070 coefTWI[0]*RAD2DEG, 0.0, coefTWI[0]*RAD2DEG, 0.0,
"N/A");
7076 sprintf(buf,
"\n\n\nPOINTS UNCERTAINTY SUMMARY\n==========================\n\n");
7078 sprintf(buf,
" RMS Sigma Latitude(m)%20.8lf\n", m_drms_sigmaLat);
7080 sprintf(buf,
" MIN Sigma Latitude(m)%20.8lf at %s\n",
7081 m_dminSigmaLatitude,m_idMinSigmaLatitude.toAscii().data());
7083 sprintf(buf,
" MAX Sigma Latitude(m)%20.8lf at %s\n\n",
7084 m_dmaxSigmaLatitude,m_idMaxSigmaLatitude.toAscii().data());
7086 sprintf(buf,
"RMS Sigma Longitude(m)%20.8lf\n", m_drms_sigmaLon);
7088 sprintf(buf,
"MIN Sigma Longitude(m)%20.8lf at %s\n",
7089 m_dminSigmaLongitude,m_idMinSigmaLongitude.toAscii().data());
7091 sprintf(buf,
"MAX Sigma Longitude(m)%20.8lf at %s\n\n",
7092 m_dmaxSigmaLongitude,m_idMaxSigmaLongitude.toAscii().data());
7094 if (m_bSolveRadii) {
7095 sprintf(buf,
" RMS Sigma Radius(m)%20.8lf\n", m_drms_sigmaRad);
7097 sprintf(buf,
" MIN Sigma Radius(m)%20.8lf at %s\n",
7098 m_dminSigmaRadius,m_idMinSigmaRadius.toAscii().data());
7100 sprintf(buf,
" MAX Sigma Radius(m)%20.8lf at %s\n",
7101 m_dmaxSigmaRadius,m_idMaxSigmaRadius.toAscii().data());
7105 sprintf(buf,
" RMS Sigma Radius(m) N/A\n");
7107 sprintf(buf,
" MIN Sigma Radius(m) N/A\n");
7109 sprintf(buf,
" MAX Sigma Radius(m) N/A\n");
7114 sprintf(buf,
"\n\nPOINTS SUMMARY\n==============\n%103sSigma Sigma Sigma\n"
7115 " Label Status Rays RMS Latitude Longitude Radius"
7116 " Latitude Longitude Radius\n",
"");
7120 double dLat, dLon, dRadius;
7121 double dSigmaLat, dSigmaLong, dSigmaRadius;
7122 double cor_lat_dd, cor_lon_dd, cor_rad_m;
7123 double cor_lat_m, cor_lon_m;
7124 double dLatInit, dLonInit, dRadiusInit;
7126 double dResidualRms;
7128 int nPointIndex = 0;
7130 int nPoints = m_pCnet->GetNumPoints();
7131 for (
int i = 0; i < nPoints; i++) {
7134 if (point->IsIgnored())
7137 nRays = point->GetNumMeasures();
7144 dSigmaRadius = point->GetAdjustedSurfacePoint().GetLocalRadiusSigma().
meters();
7147 if (point->
GetType() == ControlPoint::Fixed)
7148 strStatus =
"FIXED";
7149 else if (point->
GetType() == ControlPoint::Constrained)
7150 strStatus =
"CONSTRAINED";
7151 else if (point->
GetType() == ControlPoint::Free)
7154 strStatus =
"UNKNOWN";
7156 sprintf(buf,
"%16s%15s%5d of %d%6.2lf%16.8lf%16.8lf%16.8lf%16.8lf%16.8lf%16.8lf\n",
7157 point->
GetId().toAscii().data(), strStatus.toAscii().data(), nGoodRays, nRays,
7158 dResidualRms, dLat, dLon, dRadius * 0.001, dSigmaLat, dSigmaLong, dSigmaRadius);
7164 sprintf(buf,
"\n\nPOINTS DETAIL\n=============\n\n");
7168 for (
int i = 0; i < nPoints; i++) {
7171 if ( point->IsIgnored() )
7174 nRays = point->GetNumMeasures();
7180 dSigmaRadius = point->GetAdjustedSurfacePoint().GetLocalRadiusSigma().
meters();
7184 bounded_vector<double, 3>& corrections = m_Point_Corrections[nPointIndex];
7185 bounded_vector<double, 3>& apriorisigmas = m_Point_AprioriSigmas[nPointIndex];
7189 cor_rad_m = corrections[2] * 1000.0;
7191 cor_lat_m = corrections[0] * m_dRTM;
7192 cor_lon_m = corrections[1] * m_dRTM * cos(dLat*
Isis::DEG2RAD);
7194 dLatInit = dLat - cor_lat_dd;
7195 dLonInit = dLon - cor_lon_dd;
7196 dRadiusInit = dRadius - (corrections[2] * 1000.0);
7198 if (point->
GetType() == ControlPoint::Fixed)
7199 strStatus =
"FIXED";
7200 else if (point->
GetType() == ControlPoint::Constrained)
7201 strStatus =
"CONSTRAINED";
7202 else if (point->
GetType() == ControlPoint::Free)
7205 strStatus =
"UNKNOWN";
7207 sprintf(buf,
" Label: %s\nStatus: %s\n Rays: %d of %d\n",
7208 point->
GetId().toAscii().data(), strStatus.toAscii().data(), nGoodRays, nRays);
7211 sprintf(buf,
"\n Point Initial Total Total Final Initial Final\n"
7212 "Coordinate Value Correction Correction Value Accuracy Accuracy\n"
7213 " (dd/dd/km) (dd/dd/km) (Meters) (dd/dd/km) (Meters) (Meters)\n");
7216 sprintf(buf,
" LATITUDE%17.8lf%21.8lf%20.8lf%20.8lf%18.8lf%18.8lf\n",
7217 dLatInit, cor_lat_dd, cor_lat_m, dLat, apriorisigmas[0], dSigmaLat);
7220 sprintf(buf,
" LONGITUDE%17.8lf%21.8lf%20.8lf%20.8lf%18.8lf%18.8lf\n",
7221 dLonInit, cor_lon_dd, cor_lon_m, dLon, apriorisigmas[1], dSigmaLong);
7224 sprintf(buf,
" RADIUS%17.8lf%21.8lf%20.8lf%20.8lf%18.8lf%18.8lf\n\n",
7225 dRadiusInit * 0.001, corrections[2], cor_rad_m, dRadius * 0.001,
7226 apriorisigmas[2], dSigmaRadius);
7724 bool BundleAdjust::OutputNoErrorPropagation() {
7726 QString ofname(
"bundleout.txt");
7727 if (!m_strOutputFilePrefix.isEmpty())
7728 ofname = m_strOutputFilePrefix +
"_" + ofname;
7730 std::ofstream fp_out(ofname.toAscii().data(), std::ios::out);
7737 std::vector<double> coefX(m_nNumberCamPosCoefSolved);
7738 std::vector<double> coefY(m_nNumberCamPosCoefSolved);
7739 std::vector<double> coefZ(m_nNumberCamPosCoefSolved);
7740 std::vector<double> coefRA(m_nNumberCamAngleCoefSolved);
7741 std::vector<double> coefDEC(m_nNumberCamAngleCoefSolved);
7742 std::vector<double> coefTWI(m_nNumberCamAngleCoefSolved);
7743 std::vector<double> angles;
7748 int nImages = Images();
7750 OutputHeader(fp_out);
7752 sprintf(buf,
"\nIMAGE EXTERIOR ORIENTATION ***J2000***\n======================================\n");
7755 for (
int i = 0; i < nImages; i++) {
7759 pCamera = m_pCnet->
Camera(i);
7763 nIndex = ImageIndex(i);
7766 if (!pSpicePosition)
7770 if (!pSpiceRotation)
7780 if (m_spacecraftPositionSolveType > 0)
7784 std::vector <double> coordinate(3);
7786 coefX.push_back(coordinate[0]);
7787 coefY.push_back(coordinate[1]);
7788 coefZ.push_back(coordinate[2]);
7791 if (m_cmatrixSolveType > 0) {
7797 coefRA.push_back(angles.at(0));
7798 coefDEC.push_back(angles.at(1));
7799 coefTWI.push_back(angles.at(2));
7802 sprintf(buf,
"\nImage Full File Name: %s\n", m_pSnList->FileName(i).toAscii().data());
7804 sprintf(buf,
"\n Image Serial Number: %s\n", m_pSnList->SerialNumber(i).toAscii().data());
7806 sprintf(buf,
"\n Image Initial Total Final Initial Final\n"
7807 "Parameter Value Correction Value Accuracy Accuracy\n");
7810 if (m_nNumberCamPosCoefSolved > 0) {
7811 char strcoeff =
'a' + m_nNumberCamPosCoefSolved - 1;
7812 std::ostringstream ostr;
7813 for (
int i = 0; i < m_nNumberCamPosCoefSolved; i++) {
7815 ostr <<
" " << strcoeff;
7817 ostr <<
" " << strcoeff <<
"t";
7819 ostr << strcoeff <<
"t" << i;
7821 sprintf(buf,
" X (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7822 ostr.str().c_str(), coefX[i] - m_Image_Corrections(nIndex),
7823 m_Image_Corrections(nIndex), coefX[i],
7824 m_dGlobalSpacecraftPositionAprioriSigma[i],
"N/A");
7827 sprintf(buf,
" (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7828 ostr.str().c_str(), coefX[i] - m_Image_Corrections(nIndex),
7829 m_Image_Corrections(nIndex), coefX[i],
7830 m_dGlobalSpacecraftPositionAprioriSigma[i],
"N/A");
7837 strcoeff =
'a' + m_nNumberCamPosCoefSolved - 1;
7838 for (
int i = 0; i < m_nNumberCamPosCoefSolved; i++) {
7840 ostr <<
" " << strcoeff;
7842 ostr <<
" " << strcoeff <<
"t";
7844 ostr << strcoeff <<
"t" << i;
7846 sprintf(buf,
" Y (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7847 ostr.str().c_str(), coefY[i] - m_Image_Corrections(nIndex),
7848 m_Image_Corrections(nIndex), coefY[i],
7849 m_dGlobalSpacecraftPositionAprioriSigma[i],
"N/A");
7852 sprintf(buf,
" (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7853 ostr.str().c_str(), coefY[i] - m_Image_Corrections(nIndex),
7854 m_Image_Corrections(nIndex), coefY[i],
7855 m_dGlobalSpacecraftPositionAprioriSigma[i],
"N/A");
7862 strcoeff =
'a' + m_nNumberCamPosCoefSolved - 1;
7863 for (
int i = 0; i < m_nNumberCamPosCoefSolved; i++) {
7865 ostr <<
" " << strcoeff;
7867 ostr <<
" " << strcoeff <<
"t";
7869 ostr << strcoeff <<
"t" << i;
7871 sprintf(buf,
" Z (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7872 ostr.str().c_str(), coefZ[i] - m_Image_Corrections(nIndex),
7873 m_Image_Corrections(nIndex), coefZ[i],
7874 m_dGlobalSpacecraftPositionAprioriSigma[i],
"N/A");
7877 sprintf(buf,
" (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7878 ostr.str().c_str(), coefZ[i] - m_Image_Corrections(nIndex),
7879 m_Image_Corrections(nIndex), coefZ[i],
7880 m_dGlobalSpacecraftPositionAprioriSigma[i],
"N/A");
7889 sprintf(buf,
" X%17.8lf%21.8lf%20.8lf%18s%18s\n", coefX[0], 0.0, coefX[0],
"N/A",
"N/A");
7891 sprintf(buf,
" Y%17.8lf%21.8lf%20.8lf%18s%18s\n", coefY[0], 0.0, coefY[0],
"N/A",
"N/A");
7893 sprintf(buf,
" Z%17.8lf%21.8lf%20.8lf%18s%18s\n", coefZ[0], 0.0, coefZ[0],
"N/A",
"N/A");
7897 if (m_nNumberCamAngleCoefSolved > 0) {
7898 char strcoeff =
'a' + m_nNumberCamAngleCoefSolved - 1;
7899 std::ostringstream ostr;
7900 for (
int i = 0; i < m_nNumberCamAngleCoefSolved; i++) {
7902 ostr <<
" " << strcoeff;
7904 ostr <<
" " << strcoeff <<
"t";
7906 ostr << strcoeff <<
"t" << i;
7908 sprintf(buf,
" RA (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7909 ostr.str().c_str(),(coefRA[i] - m_Image_Corrections(nIndex)) *
RAD2DEG,
7911 m_dGlobalCameraAnglesAprioriSigma[i],
"N/A");
7914 sprintf(buf,
" (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7915 ostr.str().c_str(),(coefRA[i] - m_Image_Corrections(nIndex)) *
RAD2DEG,
7917 m_dGlobalCameraAnglesAprioriSigma[i],
"N/A");
7924 strcoeff =
'a' + m_nNumberCamAngleCoefSolved - 1;
7925 for (
int i = 0; i < m_nNumberCamAngleCoefSolved; i++) {
7927 ostr <<
" " << strcoeff;
7929 ostr <<
" " << strcoeff <<
"t";
7931 ostr << strcoeff <<
"t" << i;
7933 sprintf(buf,
"DEC (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7934 ostr.str().c_str(),(coefDEC[i] - m_Image_Corrections(nIndex))*
RAD2DEG,
7936 m_dGlobalCameraAnglesAprioriSigma[i],
"N/A");
7939 sprintf(buf,
" (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7940 ostr.str().c_str(),(coefDEC[i] - m_Image_Corrections(nIndex))*
RAD2DEG,
7942 m_dGlobalCameraAnglesAprioriSigma[i],
"N/A");
7949 if (!m_bSolveTwist) {
7950 sprintf(buf,
" TWIST%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7951 coefTWI[0]*
RAD2DEG, 0.0, coefTWI[0]*RAD2DEG, 0.0,
"N/A");
7955 strcoeff =
'a' + m_nNumberCamAngleCoefSolved - 1;
7956 for (
int i = 0; i < m_nNumberCamAngleCoefSolved; i++) {
7958 ostr <<
" " << strcoeff;
7960 ostr <<
" " << strcoeff <<
"t";
7962 ostr << strcoeff <<
"t" << i;
7964 sprintf(buf,
"TWI (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7965 ostr.str().c_str(),(coefTWI[i] - m_Image_Corrections(nIndex))*
RAD2DEG,
7967 m_dGlobalCameraAnglesAprioriSigma[i],
"N/A");
7970 sprintf(buf,
" (%s)%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7971 ostr.str().c_str(),(coefTWI[i] - m_Image_Corrections(nIndex))*
RAD2DEG,
7973 m_dGlobalCameraAnglesAprioriSigma[i],
"N/A");
7983 sprintf(buf,
" RA%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7984 coefRA[0]*
RAD2DEG, 0.0, coefRA[0]*RAD2DEG, 0.0,
"N/A");
7986 sprintf(buf,
" DEC%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7987 coefDEC[0]*RAD2DEG, 0.0, coefDEC[0]*RAD2DEG, 0.0,
"N/A");
7989 sprintf(buf,
" TWIST%17.8lf%21.8lf%20.8lf%18.8lf%18s\n",
7990 coefTWI[0]*RAD2DEG, 0.0, coefTWI[0]*RAD2DEG, 0.0,
"N/A");
7998 sprintf(buf,
"\nPOINTS SUMMARY\n==============\n%99sSigma Sigma Sigma\n"
7999 " Label Status Rays RMS Latitude Longitude Radius"
8000 " Latitude Longitude Radius\n",
"");
8004 double dResidualRms;
8005 double dLat,dLon,dRadius;
8006 double cor_lat_dd,cor_lon_dd,cor_rad_m;
8007 double cor_lat_m,cor_lon_m;
8008 double dLatInit,dLonInit,dRadiusInit;
8013 int nPoints = m_pCnet->GetNumPoints();
8014 for (
int i = 0; i < nPoints; i++) {
8017 if (point->IsIgnored())
8020 nRays = point->GetNumMeasures();
8027 if (point->
GetType() == ControlPoint::Fixed)
8028 strStatus =
"FIXED";
8029 else if (point->
GetType() == ControlPoint::Constrained)
8030 strStatus =
"CONSTRAINED";
8031 else if (point->
GetType() == ControlPoint::Free)
8034 strStatus =
"UNKNOWN";
8036 sprintf(buf,
"%16s%12s%4d of %d%6.2lf%16.8lf%16.8lf%16.8lf%11s%16s%16s\n",
8037 point->
GetId().toAscii().data(), strStatus.toAscii().data(), nGoodRays, nRays,
8038 dResidualRms, dLat, dLon, dRadius * 0.001,
"N/A",
"N/A",
"N/A");
8043 sprintf(buf,
"\n\nPOINTS DETAIL\n=============\n\n");
8046 int nPointIndex = 0;
8047 for (
int i = 0; i < nPoints; i++) {
8050 if (point->IsIgnored())
8053 nRays = point->GetNumMeasures();
8060 bounded_vector<double,3>& corrections = m_Point_Corrections[nPointIndex];
8061 bounded_vector<double,3>& apriorisigmas = m_Point_AprioriSigmas[nPointIndex];
8065 cor_rad_m = corrections[2]*1000.0;
8067 cor_lat_m = corrections[0]*m_dRTM;
8070 dLatInit = dLat-cor_lat_dd;
8071 dLonInit = dLon-cor_lon_dd;
8072 dRadiusInit = dRadius-(corrections[2]*1000.0);
8074 if (point->
GetType() == ControlPoint::Fixed)
8075 strStatus =
"FIXED";
8076 else if (point->
GetType() == ControlPoint::Constrained)
8077 strStatus =
"CONSTRAINED";
8078 else if (point->
GetType() == ControlPoint::Free)
8081 strStatus =
"UNKNOWN";
8083 sprintf(buf,
" Label: %s\nStatus: %s\n Rays: %d of %d\n",
8084 point->
GetId().toAscii().data(),strStatus.toAscii().data(),nGoodRays,nRays);
8087 sprintf(buf,
"\n Point Initial Total Total Final Initial Final\n"
8088 "Coordinate Value Correction Correction Value Accuracy Accuracy\n"
8089 " (dd/dd/km) (dd/dd/km) (Meters) (dd/dd/km) (Meters) (Meters)\n");
8092 sprintf(buf,
" LATITUDE%17.8lf%21.8lf%20.8lf%20.8lf%18.8lf%18s\n",
8093 dLatInit, cor_lat_dd, cor_lat_m, dLat, apriorisigmas[0],
"N/A");
8096 sprintf(buf,
" LONGITUDE%17.8lf%21.8lf%20.8lf%20.8lf%18.8lf%18s\n",
8097 dLonInit, cor_lon_dd, cor_lon_m, dLon, apriorisigmas[1],
"N/A");
8100 sprintf(buf,
" RADIUS%17.8lf%21.8lf%20.8lf%20.8lf%18.8lf%18s\n\n",
8101 dRadiusInit*0.001, corrections[2], cor_rad_m, dRadius*0.001, apriorisigmas[2],
"N/A");
8117 bool BundleAdjust::OutputPointsCSV() {
8120 QString ofname(
"bundleout_points.csv");
8121 if (!m_strOutputFilePrefix.isEmpty())
8122 ofname = m_strOutputFilePrefix +
"_" + ofname;
8124 std::ofstream fp_out(ofname.toAscii().data(), std::ios::out);
8128 int nPoints = m_pCnet->GetNumPoints();
8130 double dLat, dLon, dRadius;
8132 double dSigmaLat, dSigmaLong, dSigmaRadius;
8137 int nMeasures, nRejectedMeasures;
8138 double dResidualRms;
8141 if (m_bErrorPropagation) {
8142 sprintf(buf,
"Point,Point,Accepted,Rejected,Residual,3-d,3-d,3-d,Sigma,"
8143 "Sigma,Sigma,Correction,Correction,Correction,Coordinate,"
8144 "Coordinate,Coordinate\nID,,,,,Latitude,Longitude,Radius,"
8145 "Latitude,Longitude,Radius,Latitude,Longitude,Radius,X,Y,Z\n"
8146 "Label,Status,Measures,Measures,RMS,(dd),(dd),(km),(m),(m),(m),"
8147 "(m),(m),(m),(km),(km),(km)\n");
8150 sprintf(buf,
"Point,Point,Accepted,Rejected,Residual,3-d,3-d,3-d,"
8151 "Correction,Correction,Correction,Coordinate,Coordinate,"
8152 "Coordinate\n,,,,,Latitude,Longitude,Radius,Latitude,"
8153 "Longitude,Radius,X,Y,Z\nLabel,Status,Measures,Measures,"
8154 "RMS,(dd),(dd),(km),(m),(m),(m),(km),(km),(km)\n");
8158 int nPointIndex = 0;
8159 for (
int i = 0; i < nPoints; i++) {
8165 if (point->IsIgnored() || point->IsRejected())
8171 dX = point->GetAdjustedSurfacePoint().GetX().
kilometers();
8172 dY = point->GetAdjustedSurfacePoint().GetY().
kilometers();
8173 dZ = point->GetAdjustedSurfacePoint().GetZ().
kilometers();
8174 nMeasures = point->GetNumMeasures();
8179 bounded_vector<double,3>& corrections = m_Point_Corrections[nPointIndex];
8180 cor_lat_m = corrections[0]*m_dRTM;
8182 cor_rad_m = corrections[2]*1000.0;
8184 if (point->
GetType() == ControlPoint::Fixed)
8185 strStatus =
"FIXED";
8186 else if (point->
GetType() == ControlPoint::Constrained)
8187 strStatus =
"CONSTRAINED";
8188 else if (point->
GetType() == ControlPoint::Free)
8191 strStatus =
"UNKNOWN";
8193 if (m_bErrorPropagation) {
8196 dSigmaRadius = point->GetAdjustedSurfacePoint().GetLocalRadiusSigma().
meters();
8198 sprintf(buf,
"%s,%s,%d,%d,%6.2lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf\n",
8199 point->
GetId().toAscii().data(), strStatus.toAscii().data(), nMeasures, nRejectedMeasures, dResidualRms, dLat, dLon, dRadius,
8200 dSigmaLat, dSigmaLong, dSigmaRadius, cor_lat_m, cor_lon_m, cor_rad_m, dX, dY, dZ);
8203 sprintf(buf,
"%s,%s,%d,%d,%6.2lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf\n",
8204 point->
GetId().toAscii().data(), strStatus.toAscii().data(), nMeasures, nRejectedMeasures, dResidualRms, dLat, dLon, dRadius,
8205 cor_lat_m, cor_lon_m, cor_rad_m, dX, dY, dZ);
8222 bool BundleAdjust::OutputResiduals() {
8225 QString ofname(
"residuals.csv");
8226 if (!m_strOutputFilePrefix.isEmpty())
8227 ofname = m_strOutputFilePrefix +
"_" + ofname;
8229 std::ofstream fp_out(ofname.toAscii().data(), std::ios::out);
8234 sprintf(buf,
",,,x image,y image,Measured,Measured,sample,line,Residual Vector\n");
8236 sprintf(buf,
"Point,Image,Image,coordinate,coordinate,Sample,Line,residual,residual,Magnitude\n");
8238 sprintf(buf,
"Label,Filename,Serial Number,(mm),(mm),(pixels),(pixels),(pixels),(pixels),(pixels),Rejected\n");
8245 int nObjectPoints = m_pCnet->GetNumPoints();
8246 for (
int i = 0; i < nObjectPoints; i++) {
8248 if (point->IsIgnored())
8251 int nObservations = point->GetNumMeasures();
8252 for (
int j = 0; j < nObservations; j++) {
8254 if (measure->IsIgnored())
8257 Camera *pCamera = measure->Camera();
8264 if (measure->IsRejected())
8265 sprintf(buf,
"%s,%s,%s,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,*\n",
8266 point->
GetId().toAscii().data(), m_pSnList->FileName(nImageIndex).toAscii().data(), m_pSnList->SerialNumber(nImageIndex).toAscii().data(),
8267 measure->GetFocalPlaneMeasuredX(), measure->GetFocalPlaneMeasuredY(), measure->GetSample(),
8268 measure->GetLine(), measure->GetSampleResidual(), measure->GetLineResidual(),
8271 sprintf(buf,
"%s,%s,%s,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf,%16.8lf\n",
8272 point->
GetId().toAscii().data(), m_pSnList->FileName(nImageIndex).toAscii().data(), m_pSnList->SerialNumber(nImageIndex).toAscii().data(),
8273 measure->GetFocalPlaneMeasuredX(), measure->GetFocalPlaneMeasuredY(), measure->GetSample(),
8274 measure->GetLine(), measure->GetSampleResidual(), measure->GetLineResidual(), measure->
GetResidualMagnitude());
8287 bool BundleAdjust::OutputImagesCSV() {
8290 QString ofname(
"bundleout_images.csv");
8291 if (!m_strOutputFilePrefix.isEmpty())
8292 ofname = m_strOutputFilePrefix +
"_" + ofname;
8294 std::ofstream fp_out(ofname.toAscii().data(), std::ios::out);
8299 std::vector<QString> output_columns;
8301 output_columns.push_back(
"Image,");
8303 output_columns.push_back(
"rms,");
8304 output_columns.push_back(
"rms,");
8305 output_columns.push_back(
"rms,");
8307 char strcoeff =
'a' + m_nNumberCamPosCoefSolved -1;
8308 std::ostringstream ostr;
8310 if (m_nNumberCamPosCoefSolved > 0)
8311 ncoeff = m_nNumberCamPosCoefSolved;
8313 for (
int i = 0; i < ncoeff; i++) {
8317 ostr << strcoeff <<
"t";
8319 ostr << strcoeff <<
"t" << i;
8320 for (
int j = 0; j < 5; j++) {
8322 output_columns.push_back(
"X,");
8325 str += ostr.str().c_str();
8327 output_columns.push_back(str);
8333 strcoeff =
'a' + m_nNumberCamPosCoefSolved - 1;
8334 for (
int i = 0; i < ncoeff; i++) {
8338 ostr << strcoeff <<
"t";
8340 ostr << strcoeff <<
"t" << i;
8341 for (
int j = 0; j < 5; j++) {
8343 output_columns.push_back(
"Y,");
8346 str += ostr.str().c_str();
8348 output_columns.push_back(str);
8354 strcoeff =
'a' + m_nNumberCamPosCoefSolved - 1;
8355 for (
int i = 0; i < ncoeff; i++) {
8359 ostr << strcoeff <<
"t";
8361 ostr << strcoeff <<
"t" << i;
8362 for (
int j = 0; j < 5; j++) {
8364 output_columns.push_back(
"Z,");
8368 str += ostr.str().c_str();
8370 output_columns.push_back(str);
8379 strcoeff =
'a' + m_nNumberCamAngleCoefSolved - 1;
8380 for (
int i = 0; i < m_nNumberCamAngleCoefSolved; i++) {
8384 ostr << strcoeff <<
"t";
8386 ostr << strcoeff <<
"t" << i;
8387 for (
int j = 0; j < 5; j++) {
8388 if (m_nNumberCamAngleCoefSolved == 1)
8389 output_columns.push_back(
"RA,");
8391 QString str =
"RA(";
8392 str += ostr.str().c_str();
8394 output_columns.push_back(str);
8400 strcoeff =
'a' + m_nNumberCamAngleCoefSolved - 1;
8401 for (
int i = 0; i < m_nNumberCamAngleCoefSolved; i++) {
8405 ostr << strcoeff <<
"t";
8407 ostr << strcoeff <<
"t" << i;
8408 for (
int j = 0; j < 5; j++) {
8409 if (m_nNumberCamAngleCoefSolved == 1)
8410 output_columns.push_back(
"DEC,");
8412 QString str =
"DEC(";
8413 str += ostr.str().c_str();
8415 output_columns.push_back(str);
8421 strcoeff =
'a' + m_nNumberCamAngleCoefSolved - 1;
8422 for (
int i = 0; i < m_nNumberCamAngleCoefSolved; i++) {
8426 ostr << strcoeff <<
"t";
8428 ostr << strcoeff <<
"t" << i;
8429 for (
int j = 0; j < 5; j++) {
8430 if (m_nNumberCamAngleCoefSolved == 1 || !m_bSolveTwist) {
8431 output_columns.push_back(
"TWIST,");
8434 QString str =
"TWIST(";
8435 str += ostr.str().c_str();
8437 output_columns.push_back(str);
8447 int ncolumns = output_columns.size();
8448 for (
int i = 0; i < ncolumns; i++) {
8449 QString str = output_columns.at(i);
8450 sprintf(buf,
"%s", (
const char*)str.toAscii().data());
8456 output_columns.clear();
8457 output_columns.push_back(
"Filename,");
8459 output_columns.push_back(
"sample res,");
8460 output_columns.push_back(
"line res,");
8461 output_columns.push_back(
"total res,");
8464 if (m_nNumberCamPosCoefSolved)
8465 nparams = 3 * m_nNumberCamPosCoefSolved;
8467 int numCameraAnglesSolved = 2;
8468 if (m_bSolveTwist) numCameraAnglesSolved++;
8469 nparams += numCameraAnglesSolved*m_nNumberCamAngleCoefSolved;
8470 if (!m_bSolveTwist) nparams += 1;
8471 for (
int i = 0; i < nparams; i++) {
8472 output_columns.push_back(
"Initial,");
8473 output_columns.push_back(
"Correction,");
8474 output_columns.push_back(
"Final,");
8475 output_columns.push_back(
"Apriori Sigma,");
8476 output_columns.push_back(
"Adj Sigma,");
8480 ncolumns = output_columns.size();
8481 for (
int i = 0; i < ncolumns; i++) {
8482 QString str = output_columns.at(i);
8483 sprintf(buf,
"%s", (
const char*)str.toAscii().data());
8493 int nImages = Images();
8496 bool bSolveSparse =
false;
8498 std::vector<double> coefX(m_nNumberCamPosCoefSolved);
8499 std::vector<double> coefY(m_nNumberCamPosCoefSolved);
8500 std::vector<double> coefZ(m_nNumberCamPosCoefSolved);
8501 std::vector<double> coefRA(m_nNumberCamAngleCoefSolved);
8502 std::vector<double> coefDEC(m_nNumberCamAngleCoefSolved);
8503 std::vector<double> coefTWI(m_nNumberCamAngleCoefSolved);
8504 std::vector<double> angles;
8506 output_columns.clear();
8508 gmm::row_matrix<gmm::rsvector<double> > lsqCovMatrix;
8509 if (m_strSolutionMethod ==
"OLDSPARSE" && m_bErrorPropagation) {
8511 lsqCovMatrix = m_pLsq->GetCovarianceMatrix();
8512 bSolveSparse =
true;
8516 vector<double> vImageAdjustedSigmas;
8518 std::vector<double> BFP(3);
8520 for (
int i = 0; i < nImages; i++) {
8526 pCamera = m_pCnet->
Camera(i);
8532 nIndex = ImageIndex(i) ;
8534 if (m_bErrorPropagation && m_bConverged && (m_decompositionMethod == CHOLMOD))
8535 vImageAdjustedSigmas = m_Image_AdjustedSigmas.at(i);
8538 if (!pSpicePosition)
8542 if (!pSpiceRotation)
8549 if (m_spacecraftPositionSolveType > 0)
8552 std::vector <double> coordinate(3);
8555 coefX.push_back(coordinate[0]);
8556 coefY.push_back(coordinate[1]);
8557 coefZ.push_back(coordinate[2]);
8560 if (m_cmatrixSolveType > 0)
8567 coefRA.push_back(angles.at(0));
8568 coefDEC.push_back(angles.at(1));
8569 coefTWI.push_back(angles.at(2));
8573 output_columns.clear();
8576 output_columns.push_back(m_pSnList->FileName(i).toAscii().data());
8579 output_columns.push_back(
8580 toString(m_rmsImageSampleResiduals[i].Rms()));
8581 output_columns.push_back(
8582 toString(m_rmsImageLineResiduals[i].Rms()));
8583 output_columns.push_back(
8584 toString(m_rmsImageResiduals[i].Rms()));
8586 int nSigmaIndex = 0;
8587 if (m_nNumberCamPosCoefSolved > 0) {
8588 for (
int i = 0; i < m_nNumberCamPosCoefSolved; i++) {
8590 if (m_bErrorPropagation && m_bConverged) {
8592 dSigma = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
8593 else if (m_decompositionMethod == CHOLMOD)
8594 dSigma = sqrt(vImageAdjustedSigmas[nSigmaIndex]) * m_dSigma0;
8595 else if (m_decompositionMethod == SPECIALK)
8596 dSigma = sqrt((
double)(m_Normals(nIndex, nIndex))) * m_dSigma0;
8599 output_columns.push_back(
8600 toString(coefX[0] - m_Image_Corrections(nIndex)));
8601 output_columns.push_back(
8602 toString(m_Image_Corrections(nIndex)));
8603 output_columns.push_back(
8605 output_columns.push_back(
8606 toString(m_dGlobalSpacecraftPositionAprioriSigma[i]));
8608 if (m_bErrorPropagation && m_bConverged)
8609 output_columns.push_back(
8612 output_columns.push_back(
"N/A");
8616 for (
int i = 0; i < m_nNumberCamPosCoefSolved; i++) {
8618 if (m_bErrorPropagation && m_bConverged) {
8620 dSigma = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
8621 else if (m_decompositionMethod == CHOLMOD)
8622 dSigma = sqrt(vImageAdjustedSigmas[nSigmaIndex]) * m_dSigma0;
8623 else if (m_decompositionMethod == SPECIALK)
8624 dSigma = sqrt((
double)(m_Normals(nIndex, nIndex))) * m_dSigma0;
8627 output_columns.push_back(
8628 toString(coefY[0] - m_Image_Corrections(nIndex)));
8629 output_columns.push_back(
8630 toString(m_Image_Corrections(nIndex)));
8631 output_columns.push_back(
8633 output_columns.push_back(
8634 toString(m_dGlobalSpacecraftPositionAprioriSigma[i]));
8636 if (m_bErrorPropagation && m_bConverged)
8637 output_columns.push_back(
8640 output_columns.push_back(
"N/A");
8644 for (
int i = 0; i < m_nNumberCamPosCoefSolved; i++) {
8646 if (m_bErrorPropagation && m_bConverged) {
8648 dSigma = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
8649 else if (m_decompositionMethod == CHOLMOD)
8650 dSigma = sqrt(vImageAdjustedSigmas[nSigmaIndex]) * m_dSigma0;
8651 else if (m_decompositionMethod == SPECIALK)
8652 dSigma = sqrt((
double)(m_Normals(nIndex, nIndex))) * m_dSigma0;
8655 output_columns.push_back(
8656 toString(coefZ[0] - m_Image_Corrections(nIndex)));
8657 output_columns.push_back(
8658 toString(m_Image_Corrections(nIndex)));
8659 output_columns.push_back(
8661 output_columns.push_back(
8662 toString(m_dGlobalSpacecraftPositionAprioriSigma[i]));
8664 if (m_bErrorPropagation && m_bConverged)
8668 output_columns.push_back(
"N/A");
8674 output_columns.push_back(
toString(coefX[0]));
8675 output_columns.push_back(
toString(0));
8676 output_columns.push_back(
toString(coefX[0]));
8677 output_columns.push_back(
toString(0));
8678 output_columns.push_back(
"N/A");
8679 output_columns.push_back(
toString(coefY[0]));
8680 output_columns.push_back(
toString(0));
8681 output_columns.push_back(
toString(coefY[0]));
8682 output_columns.push_back(
toString(0));
8683 output_columns.push_back(
"N/A");
8684 output_columns.push_back(
toString(coefZ[0]));
8685 output_columns.push_back(
toString(0));
8686 output_columns.push_back(
toString(coefZ[0]));
8687 output_columns.push_back(
toString(0));
8688 output_columns.push_back(
"N/A");
8691 if (m_nNumberCamAngleCoefSolved > 0) {
8692 for (
int i = 0; i < m_nNumberCamAngleCoefSolved; i++) {
8694 if (m_bErrorPropagation && m_bConverged) {
8696 dSigma = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
8697 else if (m_decompositionMethod == CHOLMOD)
8698 dSigma = sqrt(vImageAdjustedSigmas[nSigmaIndex]) * m_dSigma0;
8699 else if (m_decompositionMethod == SPECIALK)
8700 dSigma = sqrt((
double)(m_Normals(nIndex, nIndex))) * m_dSigma0;
8704 ((coefRA[i] - m_Image_Corrections(nIndex)) *
RAD2DEG));
8706 (m_Image_Corrections(nIndex) * RAD2DEG));
8708 (coefRA[i] * RAD2DEG));
8710 m_dGlobalCameraAnglesAprioriSigma[i]));
8712 if (m_bErrorPropagation && m_bConverged)
8714 (dSigma * RAD2DEG));
8716 output_columns.push_back(
"N/A");
8720 for (
int i = 0; i < m_nNumberCamAngleCoefSolved; i++) {
8722 if (m_bErrorPropagation && m_bConverged) {
8724 dSigma = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
8725 else if (m_decompositionMethod == CHOLMOD)
8726 dSigma = sqrt(vImageAdjustedSigmas[nSigmaIndex]) * m_dSigma0;
8727 else if (m_decompositionMethod == SPECIALK)
8728 dSigma = sqrt((
double)(m_Normals(nIndex, nIndex))) * m_dSigma0;
8732 ((coefDEC[i] - m_Image_Corrections(nIndex)) *
RAD2DEG));
8734 (m_Image_Corrections(nIndex) * RAD2DEG));
8736 (coefDEC[i] * RAD2DEG));
8738 (m_dGlobalCameraAnglesAprioriSigma[i]));
8740 if (m_bErrorPropagation && m_bConverged)
8742 (dSigma * RAD2DEG));
8744 output_columns.push_back(
"N/A");
8748 if (!m_bSolveTwist) {
8751 output_columns.push_back(
toString(0.0));
8753 (coefTWI[0]*RAD2DEG));
8754 output_columns.push_back(
toString(0.0));
8755 output_columns.push_back(
"N/A");
8758 for (
int i = 0; i < m_nNumberCamAngleCoefSolved; i++) {
8760 if (m_bErrorPropagation && m_bConverged) {
8762 dSigma = sqrt((
double)(lsqCovMatrix(nIndex, nIndex)));
8763 else if (m_decompositionMethod == CHOLMOD)
8764 dSigma = sqrt(vImageAdjustedSigmas[nSigmaIndex]) * m_dSigma0;
8765 else if (m_decompositionMethod == SPECIALK)
8766 dSigma = sqrt((
double)(m_Normals(nIndex, nIndex))) * m_dSigma0;
8770 ((coefTWI[i] - m_Image_Corrections(nIndex)) *
RAD2DEG));
8772 (m_Image_Corrections(nIndex) * RAD2DEG));
8774 (coefTWI[i] * RAD2DEG));
8776 (m_dGlobalCameraAnglesAprioriSigma[i]));
8778 if (m_bErrorPropagation && m_bConverged)
8780 (dSigma * RAD2DEG));
8782 output_columns.push_back(
"N/A");
8792 output_columns.push_back(
toString(0.0));
8794 (coefRA[0]*RAD2DEG));
8795 output_columns.push_back(
toString(0.0));
8796 output_columns.push_back(
"N/A");
8798 (coefDEC[0]*RAD2DEG));
8799 output_columns.push_back(
toString(0.0));
8801 (coefDEC[0]*RAD2DEG));
8802 output_columns.push_back(
toString(0.0));
8803 output_columns.push_back(
"N/A");
8805 (coefTWI[0]*RAD2DEG));
8806 output_columns.push_back(
toString(0.0));
8808 (coefTWI[0]*RAD2DEG));
8809 output_columns.push_back(
toString(0.0));
8810 output_columns.push_back(
"N/A");
8814 int ncolumns = output_columns.size();
8815 for (
int i = 0; i < ncolumns; i++) {
8816 QString str = output_columns.at(i);
8819 sprintf(buf,
"%s,", (
const char*)str.toAscii().data());
8821 sprintf(buf,
"%s", (
const char*)str.toAscii().data());
8838 void BundleAdjust::SetSolutionMethod(QString str) {
8839 m_strSolutionMethod = str;
8840 FillPointIndexMap();
8848 m_wFunc[0]=m_wFunc[1]=m_wFunc[2]=NULL;
8849 m_maxLikelihoodFlag[0]=m_maxLikelihoodFlag[1]=m_maxLikelihoodFlag[2]=
false;
8850 if (models.size() == 0) {
8856 for (
int i=0;i<models.size() && i<3;i++) {
8857 m_maxLikelihoodFlag[i] =
true;
8859 if (models[i].compare(
"HUBER") == 0)
8860 m_wFunc[i]->setModel(MaximumLikelihoodWFunctions::Huber);
8861 else if (models[i].compare(
"HUBER_MODIFIED") == 0)
8862 m_wFunc[i]->setModel(MaximumLikelihoodWFunctions::HuberModified);
8863 else if (models[i].compare(
"WELSCH") == 0)
8864 m_wFunc[i]->setModel(MaximumLikelihoodWFunctions::Welsch);
8865 else if (models[i].compare(
"CHEN") == 0)
8866 m_wFunc[i]->setModel(MaximumLikelihoodWFunctions::Chen);
8868 QString msg =
"Unsuported Maximum Likelihood estimation model: " + models[i] +
"\n";
8869 m_maxLikelihoodFlag[i] =
false;
8874 for (
int i=0;i<quantiles.size() && i<3;i++)
8875 m_maxLikelihoodQuan[i] = quantiles[i];
8879 m_maxLikelihoodIndex=0;