9 #include <QCoreApplication>
15 #include <boost/lexical_cast.hpp>
16 #include <boost/numeric/ublas/io.hpp>
17 #include <boost/numeric/ublas/matrix_sparse.hpp>
18 #include <boost/numeric/ublas/vector_proxy.hpp>
37 #include "ImageList.h"
47 using namespace boost::numeric::ublas;
65 const char* message) {
79 errlog +=
". (See print.prt for details)";
95 const QString &cnetFile,
96 const QString &cubeList,
103 m_printSummary = printSummary;
105 m_cnetFileName = cnetFile;
107 m_bundleResults.setOutputControlNet(m_controlNet);
109 m_bundleSettings = bundleSettings;
110 m_bundleTargetBody = bundleSettings->bundleTargetBody();
135 m_printSummary = printSummary;
137 m_cnetFileName = cnetFile;
139 m_bundleResults.setOutputControlNet(m_controlNet);
140 m_serialNumberList = &snlist;
141 m_bundleSettings = bundleSettings;
142 m_bundleTargetBody = bundleSettings->bundleTargetBody();
167 m_printSummary = printSummary;
171 m_bundleResults.setOutputControlNet(m_controlNet);
172 m_serialNumberList = &snlist;
173 m_bundleSettings = bundleSettings;
174 m_bundleTargetBody = bundleSettings->bundleTargetBody();
198 m_printSummary = printSummary;
202 m_bundleResults.setOutputControlNet(m_controlNet);
203 m_serialNumberList = &snlist;
204 m_bundleSettings = bundleSettings;
205 m_bundleTargetBody = bundleSettings->bundleTargetBody();
221 const QString &cubeList,
224 m_printSummary = printSummary;
228 m_bundleResults.setOutputControlNet(m_controlNet);
230 m_bundleSettings = bundleSettings;
231 m_bundleTargetBody = bundleSettings->bundleTargetBody();
250 m_bundleSettings = bundleSettings;
255 m_bundleResults.setOutputControlNet(m_controlNet);
262 foreach (
Image *image, *imgList) {
268 m_bundleTargetBody = bundleSettings->bundleTargetBody();
270 m_printSummary = printSummary;
273 m_cnetFileName = control.
fileName();
287 BundleAdjust::~BundleAdjust() {
290 delete m_serialNumberList;
293 freeCHOLMODLibraryVariables();
335 m_radiansToMeters = 0.0;
336 m_metersToRadians = 0.0;
338 m_iterationSummary =
"";
344 m_controlNet->SetImages(*m_serialNumberList, progress);
347 m_controlNet->ClearJigsawRejected();
350 int numImages = m_serialNumberList->size();
353 m_normalInverse.clear();
355 m_imageSolution.clear();
360 m_cholmodNormal = NULL;
361 m_cholmodTriplet = NULL;
369 m_bodyRadii[0] = m_bodyRadii[1] = m_bodyRadii[2] =
Distance();
372 cnetCamera->
radii(m_bodyRadii);
374 if (m_bodyRadii[0] >=
Distance(0, Distance::Meters)) {
375 m_metersToRadians = 0.001 / m_bodyRadii[0].kilometers();
376 m_radiansToMeters = 1.0 / m_metersToRadians;
377 m_bundleResults.setRadiansToMeters(m_radiansToMeters);
384 for (
int i = 0; i < numImages; i++) {
387 QString observationNumber = m_serialNumberList->observationNumber(i);
388 QString instrumentId = m_serialNumberList->spacecraftInstrumentId(i);
389 QString serialNumber = m_serialNumberList->serialNumber(i);
390 QString fileName = m_serialNumberList->fileName(i);
394 BundleImageQsp image = BundleImageQsp(
new BundleImage(camera, serialNumber, fileName));
397 QString msg =
"In BundleAdjust::init(): image " + fileName +
"is null." +
"\n";
402 m_bundleObservations.addNew(image, observationNumber, instrumentId, m_bundleSettings);
405 QString msg =
"In BundleAdjust::init(): observation "
406 + observationNumber +
"is null." +
"\n";
413 m_bundleObservations.initializeExteriorOrientation();
415 if (m_bundleSettings->solveTargetBody()) {
416 m_bundleObservations.initializeBodyRotation();
420 int numControlPoints = m_controlNet->GetNumPoints();
421 for (
int i = 0; i < numControlPoints; i++) {
423 if (point->IsIgnored()) {
428 m_bundleControlPoints.append(bundleControlPoint);
430 bundleControlPoint->setWeights(m_bundleSettings, m_metersToRadians);
434 int numMeasures = bundleControlPoint->size();
435 for (
int j=0; j < numMeasures; j++) {
437 QString cubeSerialNumber = measure->cubeSerialNumber();
440 m_bundleObservations.observationByCubeSerialNumber(cubeSerialNumber);
441 BundleImageQsp image = observation->imageByCubeSerialNumber(cubeSerialNumber);
443 measure->setParentObservation(observation);
444 measure->setParentImage(image);
458 if (m_bundleSettings->validateNetwork()) {
461 m_bundleResults.maximumLikelihoodSetUp(m_bundleSettings->maximumLikelihoodEstimatorModels());
477 m_rank = m_bundleObservations.numberParameters();
479 if (m_bundleSettings->solveTargetBody()) {
480 m_rank += m_bundleSettings->numberTargetBodyParameters();
483 int num3DPoints = m_bundleControlPoints.size();
485 m_bundleResults.setNumberUnknownParameters(m_rank + 3 * num3DPoints);
487 m_imageSolution.resize(m_rank);
491 initializeCHOLMODLibraryVariables();
515 bool BundleAdjust::validateNetwork() {
516 outputBundleStatus(
"\nValidating network...");
518 int imagesWithInsufficientMeasures = 0;
519 QString msg =
"Images with one or less measures:\n";
521 int numObservations = m_bundleObservations.size();
522 for (
int i = 0; i < numObservations; i++) {
523 int numImages = m_bundleObservations.at(i)->size();
524 for (
int j = 0; j < numImages; j++) {
525 BundleImageQsp bundleImage = m_bundleObservations.at(i)->at(j);
526 int numMeasures = m_controlNet->
527 GetNumberOfValidMeasuresInImage(bundleImage->serialNumber());
529 if (numMeasures > 1) {
533 imagesWithInsufficientMeasures++;
534 msg += bundleImage->fileName() +
": " +
toString(numMeasures) +
"\n";
538 if ( imagesWithInsufficientMeasures > 0 ) {
542 outputBundleStatus(
"Validation complete!...");
553 bool BundleAdjust::initializeCHOLMODLibraryVariables() {
559 m_cholmodTriplet = NULL;
561 cholmod_start(&m_cholmodCommon);
567 m_cholmodCommon.nmethods = 1;
568 m_cholmodCommon.method[0].ordering = CHOLMOD_AMD;
571 if (m_bundleSettings->solveTargetBody()) {
572 m_sparseNormals.setNumberOfColumns(m_bundleObservations.size()+1);
575 m_sparseNormals.setNumberOfColumns(m_bundleObservations.size());
590 bool BundleAdjust::freeCHOLMODLibraryVariables() {
592 cholmod_free_triplet(&m_cholmodTriplet, &m_cholmodCommon);
593 cholmod_free_sparse(&m_cholmodNormal, &m_cholmodCommon);
594 cholmod_free_factor(&m_L, &m_cholmodCommon);
596 cholmod_finish(&m_cholmodCommon);
613 return bundleSolveInformation();
621 void BundleAdjust::abortBundle() {
640 bool BundleAdjust::solveCholesky() {
658 m_controlNet->ComputeApriori();
662 if (m_bundleTargetBody && m_bundleTargetBody->solveMeanRadius()) {
663 int numControlPoints = m_bundleControlPoints.size();
664 for (
int i = 0; i < numControlPoints; i++) {
666 SurfacePoint surfacepoint = point->adjustedSurfacePoint();
670 point->setAdjustedSurfacePoint(surfacepoint);
674 if (m_bundleTargetBody && m_bundleTargetBody->solveTriaxialRadii()) {
675 int numControlPoints = m_bundleControlPoints.size();
676 for (
int i = 0; i < numControlPoints; i++) {
678 SurfacePoint surfacepoint = point->adjustedSurfacePoint();
684 point->setAdjustedSurfacePoint(surfacepoint);
690 double previousSigma0 = 0.0;
693 clock_t solveStartClock = clock();
697 emit iterationUpdate(m_iteration, m_bundleResults.sigma0());
701 m_bundleResults.setConverged(
false);
702 emit statusUpdate(
"\n aborting...");
708 emit statusUpdate( QString(
"starting iteration %1\n").arg(m_iteration) );
710 clock_t iterationStartClock = clock();
713 if (m_iteration != 1) {
714 m_sparseNormals.zeroBlocks();
718 if (!formNormalEquations()) {
719 m_bundleResults.setConverged(
false);
725 m_bundleResults.setConverged(
false);
726 emit statusUpdate(
"\n aborting...");
733 if (!solveSystem()) {
734 printf(
"solve failed!\n");
735 m_bundleResults.setConverged(
false);
741 m_bundleResults.setConverged(
false);
742 emit statusUpdate(
"\n aborting...");
749 applyParameterCorrections();
753 m_bundleResults.setConverged(
false);
754 emit statusUpdate(
"\n aborting...");
761 vtpv = computeResiduals();
764 if ( m_bundleSettings->outlierRejection() ) {
765 computeRejectionLimit();
771 m_bundleResults.setConverged(
false);
772 emit statusUpdate(
"\n aborting...");
779 m_bundleResults.computeSigma0(vtpv, m_bundleSettings->convergenceCriteria());
786 emit statusUpdate(QString(
"Iteration: %1")
788 emit statusUpdate(QString(
"Sigma0: %1")
789 .arg(m_bundleResults.sigma0(),
793 emit statusUpdate(QString(
"Observations: %1")
794 .arg(m_bundleResults.numberObservations()));
795 emit statusUpdate(QString(
"Constrained Parameters:%1")
796 .arg(m_bundleResults.numberConstrainedPointParameters()));
797 emit statusUpdate(QString(
"Unknowns: %1")
798 .arg(m_bundleResults.numberUnknownParameters()));
799 emit statusUpdate(QString(
"Degrees of Freedom: %1")
800 .arg(m_bundleResults.degreesOfFreedom()));
801 emit iterationUpdate(m_iteration, m_bundleResults.sigma0());
804 if (m_bundleSettings->convergenceCriteria() == BundleSettings::Sigma0) {
805 if (fabs(previousSigma0 - m_bundleResults.sigma0())
806 <= m_bundleSettings->convergenceCriteriaThreshold()) {
811 if (m_bundleResults.maximumLikelihoodModelIndex()
812 < m_bundleResults.numberMaximumLikelihoodModels() - 1
813 && m_bundleResults.maximumLikelihoodModelIndex()
817 if (m_bundleResults.numberMaximumLikelihoodModels()
818 > m_bundleResults.maximumLikelihoodModelIndex() + 1) {
821 m_bundleResults.incrementMaximumLikelihoodModelIndex();
825 m_bundleResults.setConverged(
true);
826 emit statusUpdate(
"Bundle has converged\n");
833 int numConvergedParams = 0;
834 int numImgParams = m_imageSolution.size();
835 for (
int ij = 0; ij < numImgParams; ij++) {
836 if (fabs(m_imageSolution(ij)) > m_bundleSettings->convergenceCriteriaThreshold()) {
840 numConvergedParams++;
843 if ( numConvergedParams == numImgParams ) {
844 m_bundleResults.setConverged(
true);
845 emit statusUpdate(
"Bundle has converged");
850 m_bundleResults.printMaximumLikelihoodTierInformation();
851 clock_t iterationStopClock = clock();
852 double iterationTime = (iterationStopClock - iterationStartClock)
853 / (
double)CLOCKS_PER_SEC;
854 emit statusUpdate( QString(
"End of Iteration %1").arg(m_iteration) );
855 emit statusUpdate( QString(
"Elapsed Time: %1").arg(iterationTime,
861 if (m_iteration >= m_bundleSettings->convergenceCriteriaMaximumIterations()) {
867 if (!m_bundleResults.converged()) {
868 m_bundleResults.initializeResidualsProbabilityDistribution(101);
875 if (!m_bundleResults.converged() || !m_bundleSettings->errorPropagation()) {
876 cholmod_free_factor(&m_L, &m_cholmodCommon);
884 previousSigma0 = m_bundleResults.sigma0();
887 if (m_bundleResults.converged() && m_bundleSettings->errorPropagation()) {
888 clock_t errorPropStartClock = clock();
889 printf(
"Starting Error Propagation");
891 emit statusUpdate(
"\n\nError Propagation Complete");
892 clock_t errorPropStopClock = clock();
893 m_bundleResults.setElapsedTimeErrorProp((errorPropStopClock - errorPropStartClock)
894 / (
double)CLOCKS_PER_SEC);
897 clock_t solveStopClock = clock();
898 m_bundleResults.setElapsedTime((solveStopClock - solveStartClock)
899 / (
double)CLOCKS_PER_SEC);
903 m_bundleResults.setIterations(m_iteration);
904 m_bundleResults.setObservations(m_bundleObservations);
905 m_bundleResults.setBundleControlPoints(m_bundleControlPoints);
908 emit resultsReady(results);
910 emit statusUpdate(
"\nBundle Complete");
915 m_bundleResults.setConverged(
false);
916 emit statusUpdate(
"\n aborting...");
918 QString msg =
"Could not solve bundle adjust.";
949 bool BundleAdjust::formNormalEquations() {
952 m_bundleResults.setNumberObservations(0);
953 m_bundleResults.resetNumberConstrainedPointParameters();
960 static boost::numeric::ublas::symmetric_matrix<double, upper> N22(3);
963 boost::numeric::ublas::compressed_vector<double> n1(m_rank);
965 m_RHS.resize(m_rank);
969 if (m_bundleSettings->solveTargetBody()) {
970 int numTargetBodyParameters = m_bundleSettings->numberTargetBodyParameters();
972 coeffTarget.resize(2,numTargetBodyParameters);
981 coeffPoint3D.clear();
987 int numGood3DPoints = 0;
988 int numRejected3DPoints = 0;
990 int num3DPoints = m_bundleControlPoints.size();
994 for (
int i = 0; i < num3DPoints; i++) {
998 if (point->isRejected()) {
999 numRejected3DPoints++;
1012 int numMeasures = point->size();
1013 for (
int j = 0; j < numMeasures; j++) {
1018 if (measure->isRejected()) {
1022 status = computePartials(coeffTarget, coeffImage, coeffPoint3D, coeffRHS, *measure,
1032 int numObs = m_bundleResults.numberObservations();
1033 m_bundleResults.setNumberObservations(numObs + 2);
1034 formMeasureNormals(N22, N12, n1, n2, coeffTarget, coeffImage, coeffPoint3D, coeffRHS,
1035 measure->observationIndex());
1038 formPointNormals(N22, N12, n2, m_RHS, point);
1048 formWeightedNormals(n1, m_RHS);
1051 m_bundleResults.setNumberUnknownParameters(m_rank + 3 * numGood3DPoints);
1076 bool BundleAdjust::formMeasureNormals(symmetric_matrix<double, upper>&N22,
1078 compressed_vector<double> &n1,
1080 matrix<double> &coeffTarget,
1081 matrix<double> &coeffImage,
1082 matrix<double> &coeffPoint3D,
1083 vector<double> &coeffRHS,
1084 int observationIndex) {
1086 static symmetric_matrix<double, upper> N11;
1087 static matrix<double> N11TargetImage;
1089 int blockIndex = observationIndex;
1092 int numTargetPartials = coeffTarget.size2();
1093 if (m_bundleSettings->solveTargetBody()) {
1096 static vector<double> n1Target(numTargetPartials);
1097 n1Target.resize(numTargetPartials);
1101 N11.resize(numTargetPartials);
1104 N11 = prod(trans(coeffTarget), coeffTarget);
1107 m_sparseNormals.insertMatrixBlock(0, 0, numTargetPartials, numTargetPartials);
1109 (*(*m_sparseNormals[0])[0]) += N11;
1112 N11TargetImage.resize(numTargetPartials, coeffImage.size2());
1113 N11TargetImage.clear();
1114 N11TargetImage = prod(trans(coeffTarget),coeffImage);
1116 m_sparseNormals.insertMatrixBlock(observationIndex+1, 0,
1117 numTargetPartials, coeffImage.size2());
1118 (*(*m_sparseNormals[observationIndex+1])[0]) += N11TargetImage;
1121 static matrix<double> N12Target(numTargetPartials, 3);
1124 N12Target = prod(trans(coeffTarget), coeffPoint3D);
1128 *N12[0] += N12Target;
1131 n1Target = prod(trans(coeffTarget), coeffRHS);
1134 for (
int i = 0; i < numTargetPartials; i++) {
1135 n1(i) += n1Target(i);
1144 int numImagePartials = coeffImage.size2();
1147 n1Image.resize(numImagePartials);
1151 N11.resize(numImagePartials);
1154 N11 = prod(trans(coeffImage), coeffImage);
1158 for (
int a = 0; a < observationIndex; a++) {
1160 t += observation->numberParameters();
1163 t += numTargetPartials;
1166 m_sparseNormals.insertMatrixBlock(blockIndex, blockIndex,
1167 numImagePartials, numImagePartials);
1169 (*(*m_sparseNormals[blockIndex])[blockIndex]) += N11;
1172 static matrix<double> N12Image(numImagePartials, 3);
1173 N12Image.resize(numImagePartials, 3);
1176 N12Image = prod(trans(coeffImage), coeffPoint3D);
1180 *N12[blockIndex] += N12Image;
1183 n1Image = prod(trans(coeffImage), coeffRHS);
1188 for (
int i = 0; i < numImagePartials; i++) {
1189 n1(i + t) += n1Image(i);
1193 N22 += prod(trans(coeffPoint3D), coeffPoint3D);
1196 n2 += prod(trans(coeffPoint3D), coeffRHS);
1219 bool BundleAdjust::formPointNormals(symmetric_matrix<double, upper>&N22,
1225 boost::numeric::ublas::bounded_vector<double, 3> &NIC = bundleControlPoint->nicVector();
1232 boost::numeric::ublas::bounded_vector<double, 3> &weights
1233 = bundleControlPoint->weights();
1234 boost::numeric::ublas::bounded_vector<double, 3> &corrections
1235 = bundleControlPoint->corrections();
1237 if (weights(0) > 0.0) {
1238 N22(0,0) += weights(0);
1239 n2(0) += (-weights(0) * corrections(0));
1240 m_bundleResults.incrementNumberConstrainedPointParameters(1);
1243 if (weights(1) > 0.0) {
1244 N22(1,1) += weights(1);
1245 n2(1) += (-weights(1) * corrections(1));
1246 m_bundleResults.incrementNumberConstrainedPointParameters(1);
1249 if (weights(2) > 0.0) {
1250 N22(2,2) += weights(2);
1251 n2(2) += (-weights(2) * corrections(2));
1252 m_bundleResults.incrementNumberConstrainedPointParameters(1);
1261 bundleControlPoint->setAdjustedSurfacePoint(SurfacePoint);
1265 productATransB(N22, N12, Q);
1268 NIC = prod(N22, n2);
1275 accumProductAlphaAB(-1.0, Q, n2, nj);
1292 bool BundleAdjust::formWeightedNormals(compressed_vector<double> &n1,
1293 vector<double> &nj) {
1295 m_bundleResults.resetNumberConstrainedImageParameters();
1299 for (
int i = 0; i < m_sparseNormals.size(); i++) {
1301 if ( !diagonalBlock )
1304 if (m_bundleSettings->solveTargetBody() && i == 0) {
1305 m_bundleResults.resetNumberConstrainedTargetParameters();
1308 vector<double> weights = m_bundleTargetBody->parameterWeights();
1309 vector<double> corrections = m_bundleTargetBody->parameterCorrections();
1311 int blockSize = diagonalBlock->size1();
1312 for (
int j = 0; j < blockSize; j++) {
1313 if (weights[j] > 0.0) {
1314 (*diagonalBlock)(j,j) += weights[j];
1315 nj[n] -= weights[j] * corrections(j);
1316 m_bundleResults.incrementNumberConstrainedTargetParameters(1);
1325 if (m_bundleSettings->solveTargetBody()) {
1326 observation = m_bundleObservations.at(i-1);
1329 observation = m_bundleObservations.at(i);
1335 int blockSize = diagonalBlock->size1();
1336 for (
int j = 0; j < blockSize; j++) {
1337 if (weights(j) > 0.0) {
1338 (*diagonalBlock)(j,j) += weights(j);
1339 nj[n] -= weights(j) * corrections(j);
1340 m_bundleResults.incrementNumberConstrainedImageParameters(1);
1362 void BundleAdjust::productAlphaAV(
double alpha, bounded_vector<double,3> &v2,
1364 vector<double> &v1) {
1366 QMapIterator< int, LinearAlgebra::Matrix * > Qit(Q);
1368 int subrangeStart, subrangeEnd;
1370 while ( Qit.hasNext() ) {
1373 int columnIndex = Qit.key();
1375 subrangeStart = m_sparseNormals.getLeadingColumnsForBlock(columnIndex);
1376 subrangeEnd = subrangeStart + Qit.value()->size2();
1378 v2 += alpha * prod(*(Qit.value()),subrange(v1,subrangeStart,subrangeEnd));
1392 bool BundleAdjust::productATransB(symmetric_matrix <double,upper> &N22,
1396 QMapIterator< int, LinearAlgebra::Matrix * > N12it(N12);
1398 while ( N12it.hasNext() ) {
1401 int rowIndex = N12it.key();
1406 *(Q[rowIndex]) = prod(N22,trans(*(N12it.value())));
1425 QMapIterator<int, LinearAlgebra::Matrix*> N12it(N12);
1426 QMapIterator<int, LinearAlgebra::Matrix*> Qit(Q);
1429 while ( N12it.hasNext() ) {
1432 int rowIndex = N12it.key();
1435 while ( Qit.hasNext() ) {
1438 int columnIndex = Qit.key();
1440 if ( rowIndex > columnIndex ) {
1447 m_sparseNormals.insertMatrixBlock(columnIndex, rowIndex,
1448 N12block->size1(), Qblock->size2());
1450 (*(*m_sparseNormals[columnIndex])[rowIndex]) -= prod(*N12block,*Qblock);
1467 void BundleAdjust::accumProductAlphaAB(
double alpha,
1470 vector<double> &nj) {
1476 int numTargetParameters = m_bundleSettings->numberTargetBodyParameters();
1478 QMapIterator<int, LinearAlgebra::Matrix*> Qit(Q);
1480 while ( Qit.hasNext() ) {
1483 int columnIndex = Qit.key();
1489 for (
int observationIndex = 0; observationIndex < columnIndex; observationIndex++) {
1490 if (numTargetParameters > 0 && observationIndex == 0) {
1491 numParams += numTargetParameters;
1494 if (numTargetParameters > 0 ) {
1496 numParams += observation->numberParameters();
1500 numParams += observation->numberParameters();
1505 for (
unsigned i = 0; i < blockProduct.size(); i++) {
1506 nj(numParams+i) += alpha*blockProduct(i);
1521 bool BundleAdjust::solveSystem() {
1524 if ( !loadCholmodTriplet() ) {
1525 QString msg =
"CHOLMOD: Failed to load Triplet matrix";
1530 m_cholmodNormal = cholmod_triplet_to_sparse(m_cholmodTriplet,
1531 m_cholmodTriplet->nnz,
1536 m_L = cholmod_analyze(m_cholmodNormal, &m_cholmodCommon);
1540 cholmod_factorize(m_cholmodNormal, m_L, &m_cholmodCommon);
1543 if (m_cholmodCommon.status == CHOLMOD_NOT_POSDEF) {
1544 QString msg =
"Matrix NOT positive-definite: failure at column " +
toString((
int) m_L->minor);
1552 cholmod_dense *x, *b;
1555 b = cholmod_zeros(m_cholmodNormal->nrow, 1, m_cholmodNormal->xtype, &m_cholmodCommon);
1558 double *px = (
double*)b->x;
1559 for (
int i = 0; i < m_rank; i++) {
1564 x = cholmod_solve(CHOLMOD_A, m_L, b, &m_cholmodCommon);
1567 double *sx = (
double*)x->x;
1568 for (
int i = 0; i < m_rank; i++) {
1569 m_imageSolution[i] = sx[i];
1573 cholmod_free_sparse(&m_cholmodNormal, &m_cholmodCommon);
1574 cholmod_free_dense(&b, &m_cholmodCommon);
1575 cholmod_free_dense(&x, &m_cholmodCommon);
1592 bool BundleAdjust::loadCholmodTriplet() {
1594 if ( m_iteration == 1 ) {
1595 int numElements = m_sparseNormals.numberOfElements();
1596 m_cholmodTriplet = cholmod_allocate_triplet(m_rank, m_rank, numElements,
1597 -1, CHOLMOD_REAL, &m_cholmodCommon);
1599 if ( !m_cholmodTriplet ) {
1600 printf(
"Triplet allocation failure");
1604 m_cholmodTriplet->nnz = 0;
1607 int *tripletColumns = (
int*) m_cholmodTriplet->i;
1608 int *tripletRows = (
int*) m_cholmodTriplet->j;
1609 double *tripletValues = (
double*)m_cholmodTriplet->x;
1615 int numBlockcolumns = m_sparseNormals.size();
1616 for (
int columnIndex = 0; columnIndex < numBlockcolumns; columnIndex++) {
1620 if ( !normalsColumn ) {
1621 printf(
"SparseBlockColumnMatrix retrieval failure at column %d", columnIndex);
1625 int numLeadingColumns = m_sparseNormals.getLeadingColumnsForBlock(columnIndex);
1627 QMapIterator< int, LinearAlgebra::Matrix * > it(*normalsColumn);
1629 while ( it.hasNext() ) {
1632 int rowIndex = it.key();
1634 int numLeadingRows = m_sparseNormals.getLeadingRowsForBlock(rowIndex);
1637 if ( !normalsBlock ) {
1638 printf(
"matrix block retrieval failure at column %d, row %d", columnIndex, rowIndex);
1639 printf(
"Total # of block columns: %d", numBlockcolumns);
1640 printf(
"Total # of blocks: %d", m_sparseNormals.numberOfBlocks());
1644 if ( columnIndex == rowIndex ) {
1645 for (
unsigned ii = 0; ii < normalsBlock->size1(); ii++) {
1646 for (
unsigned jj = ii; jj < normalsBlock->size2(); jj++) {
1647 entryValue = normalsBlock->at_element(ii,jj);
1648 int entryColumnIndex = jj + numLeadingColumns;
1649 int entryRowIndex = ii + numLeadingRows;
1651 if ( m_iteration == 1 ) {
1652 tripletColumns[numEntries] = entryColumnIndex;
1653 tripletRows[numEntries] = entryRowIndex;
1654 m_cholmodTriplet->nnz++;
1657 tripletValues[numEntries] = entryValue;
1664 for (
unsigned ii = 0; ii < normalsBlock->size1(); ii++) {
1665 for (
unsigned jj = 0; jj < normalsBlock->size2(); jj++) {
1666 entryValue = normalsBlock->at_element(ii,jj);
1667 int entryColumnIndex = jj + numLeadingColumns;
1668 int entryRowIndex = ii + numLeadingRows;
1670 if ( m_iteration ==1 ) {
1671 tripletColumns[numEntries] = entryRowIndex;
1672 tripletRows[numEntries] = entryColumnIndex;
1673 m_cholmodTriplet->nnz++;
1676 tripletValues[numEntries] = entryValue;
1697 bool BundleAdjust::cholmodInverse() {
1701 m_normalInverse.resize(m_rank);
1706 b = cholmod_zeros ( m_rank, 1, CHOLMOD_REAL, &m_cholmodCommon ) ;
1707 double *pb = (
double*)b->x;
1711 for (i = 0; i < m_rank; i++) {
1717 x = cholmod_solve ( CHOLMOD_A, m_L, b, &m_cholmodCommon ) ;
1721 for (j = 0; j <= i; j++) {
1722 m_normalInverse(j, i) = px[j];
1725 cholmod_free_dense(&x,&m_cholmodCommon);
1728 cholmod_free_dense(&b,&m_cholmodCommon);
1746 bool BundleAdjust::invert3x3(symmetric_matrix<double, upper> &m) {
1750 boost::numeric::ublas::symmetric_matrix< double, upper > c = m;
1752 den = m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1))
1753 - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0))
1754 + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0));
1757 if (fabs(den) < 1.0e-100) {
1763 m(0, 0) = (c(1, 1) * c(2, 2) - c(1, 2) * c(2, 1)) * det;
1764 m(0, 1) = (c(0, 2) * c(2, 1) - c(0, 1) * c(2, 2)) * det;
1765 m(0, 2) = (c(0, 1) * c(1, 2) - c(0, 2) * c(1, 1)) * det;
1766 m(1, 1) = (c(0, 0) * c(2, 2) - c(0, 2) * c(2, 0)) * det;
1767 m(1, 2) = (c(0, 2) * c(1, 0) - c(0, 0) * c(1, 2)) * det;
1768 m(2, 2) = (c(0, 0) * c(1, 1) - c(0, 1) * c(1, 0)) * det;
1793 bool BundleAdjust::computePartials(matrix<double> &coeffTarget,
1794 matrix<double> &coeffImage,
1795 matrix<double> &coeffPoint3D,
1796 vector<double> &coeffRHS,
1801 std::vector<double> lookBWRTLat;
1802 std::vector<double> lookBWRTLon;
1803 std::vector<double> lookBWRTRad;
1805 Camera *measureCamera = NULL;
1807 double measuredX, computedX, measuredY, computedY;
1808 double deltaX, deltaY;
1809 double observationSigma;
1810 double observationWeight;
1812 measureCamera = measure.
camera();
1818 int numImagePartials = observation->numberParameters();
1819 coeffImage.resize(2,numImagePartials);
1822 if (m_bundleSettings->solveTargetBody()) {
1823 coeffTarget.clear();
1827 coeffPoint3D.clear();
1844 &computedX, &computedY))) {
1845 QString msg =
"Unable to map apriori surface point for measure ";
1852 CameraGroundMap::WRT_Latitude);
1854 CameraGroundMap::WRT_Longitude);
1856 CameraGroundMap::WRT_Radius);
1859 if (m_bundleSettings->solveTargetBody() && m_bundleSettings->solvePoleRA()) {
1861 &coeffTarget(0, index),
1862 &coeffTarget(1, index));
1866 if (m_bundleSettings->solveTargetBody() && m_bundleSettings->solvePoleRAVelocity()) {
1868 &coeffTarget(0, index),
1869 &coeffTarget(1, index));
1873 if (m_bundleSettings->solveTargetBody() && m_bundleSettings->solvePoleDec()) {
1875 &coeffTarget(0, index),
1876 &coeffTarget(1, index));
1880 if (m_bundleSettings->solveTargetBody() && m_bundleSettings->solvePoleDecVelocity()) {
1882 &coeffTarget(0, index),
1883 &coeffTarget(1, index));
1887 if (m_bundleSettings->solveTargetBody() && m_bundleSettings->solvePM()) {
1889 &coeffTarget(0, index),
1890 &coeffTarget(1, index));
1894 if (m_bundleSettings->solveTargetBody() && m_bundleSettings->solvePMVelocity()) {
1896 &coeffTarget(0, index),
1897 &coeffTarget(1, index));
1901 if (m_bundleSettings->solveTargetBody() && m_bundleTargetBody->solveMeanRadius()) {
1902 std::vector<double> lookBWRTMeanRadius =
1904 m_bundleTargetBody->meanRadius());
1907 &coeffTarget(1, index));
1911 if (m_bundleSettings->solveTargetBody() && m_bundleTargetBody->solveTriaxialRadii()) {
1913 std::vector<double> lookBWRTRadiusA =
1915 CameraGroundMap::WRT_MajorAxis);
1918 &coeffTarget(1, index));
1921 std::vector<double> lookBWRTRadiusB =
1923 CameraGroundMap::WRT_MinorAxis);
1926 &coeffTarget(1, index));
1929 std::vector<double> lookBWRTRadiusC =
1931 CameraGroundMap::WRT_PolarAxis);
1934 &coeffTarget(1, index));
1940 if (observationSolveSettings->instrumentPositionSolveOption() !=
1941 BundleObservationSolveSettings::NoPositionFactors) {
1943 int numCamPositionCoefficients =
1944 observationSolveSettings->numberCameraPositionCoefficientsSolved();
1948 for (
int cameraCoef = 0; cameraCoef < numCamPositionCoefficients; cameraCoef++) {
1950 &coeffImage(0, index),
1951 &coeffImage(1, index));
1956 for (
int cameraCoef = 0; cameraCoef < numCamPositionCoefficients; cameraCoef++) {
1958 &coeffImage(0, index),
1959 &coeffImage(1, index));
1964 for (
int cameraCoef = 0; cameraCoef < numCamPositionCoefficients; cameraCoef++) {
1966 &coeffImage(0, index),
1967 &coeffImage(1, index));
1973 if (observationSolveSettings->instrumentPointingSolveOption() !=
1974 BundleObservationSolveSettings::NoPointingFactors) {
1976 int numCamAngleCoefficients =
1977 observationSolveSettings->numberCameraAngleCoefficientsSolved();
1980 for (
int cameraCoef = 0; cameraCoef < numCamAngleCoefficients; cameraCoef++) {
1982 cameraCoef, &coeffImage(0, index),
1983 &coeffImage(1, index));
1988 for (
int cameraCoef = 0; cameraCoef < numCamAngleCoefficients; cameraCoef++) {
1990 cameraCoef, &coeffImage(0, index),
1991 &coeffImage(1, index));
1996 if (observationSolveSettings->solveTwist()) {
1997 for (
int cameraCoef = 0; cameraCoef < numCamAngleCoefficients; cameraCoef++) {
1999 cameraCoef, &coeffImage(0, index),
2000 &coeffImage(1, index));
2008 &coeffPoint3D(0, 0),
2009 &coeffPoint3D(1, 0));
2011 &coeffPoint3D(0, 1),
2012 &coeffPoint3D(1, 1));
2014 &coeffPoint3D(0, 2),
2015 &coeffPoint3D(1, 2));
2021 deltaX = measuredX - computedX;
2022 deltaY = measuredY - computedY;
2024 coeffRHS(0) = deltaX;
2025 coeffRHS(1) = deltaY;
2028 double obsValue = deltaX / measureCamera->
PixelPitch();
2029 m_bundleResults.addResidualsProbabilityDistributionObservation(obsValue);
2031 obsValue = deltaY / measureCamera->
PixelPitch();
2032 m_bundleResults.addResidualsProbabilityDistributionObservation(obsValue);
2034 observationSigma = 1.4 * measureCamera->
PixelPitch();
2035 observationWeight = 1.0 / observationSigma;
2037 if (m_bundleResults.numberMaximumLikelihoodModels()
2038 > m_bundleResults.maximumLikelihoodModelIndex()) {
2040 double residualR2ZScore
2041 = sqrt(deltaX * deltaX + deltaY * deltaY) / observationSigma / sqrt(2.0);
2043 m_bundleResults.addProbabilityDistributionObservation(residualR2ZScore);
2044 int currentModelIndex = m_bundleResults.maximumLikelihoodModelIndex();
2045 observationWeight *= m_bundleResults.maximumLikelihoodModelWFunc(currentModelIndex)
2046 .sqrtWeightScaler(residualR2ZScore);
2050 coeffImage *= observationWeight;
2051 coeffPoint3D *= observationWeight;
2052 coeffRHS *= observationWeight;
2054 if (m_bundleSettings->solveTargetBody()) {
2055 coeffTarget *= observationWeight;
2065 void BundleAdjust::applyParameterCorrections() {
2071 if (m_bundleSettings->solveTargetBody()) {
2072 int numTargetBodyParameters = m_bundleTargetBody->numberParameters();
2074 m_bundleTargetBody->applyParameterCorrections(subrange(m_imageSolution,0,
2075 numTargetBodyParameters));
2077 t += numTargetBodyParameters;
2081 int numObservations = m_bundleObservations.size();
2082 for (
int i = 0; i < numObservations; i++) {
2085 int numParameters = observation->numberParameters();
2087 observation->applyParameterCorrections(subrange(m_imageSolution,t,t+numParameters));
2089 if (m_bundleSettings->solveTargetBody()) {
2090 observation->updateBodyRotation();
2102 double latCorrection, lonCorrection, radCorrection;
2104 int numControlPoints = m_bundleControlPoints.size();
2105 for (
int i = 0; i < numControlPoints; i++) {
2108 if (point->isRejected()) {
2114 boost::numeric::ublas::bounded_vector< double, 3 > &NIC = point->nicVector();
2116 boost::numeric::ublas::bounded_vector< double, 3 > &corrections = point->corrections();
2120 productAlphaAV(-1.0, NIC, Q, m_imageSolution);
2123 latCorrection = NIC(0);
2124 lonCorrection = NIC(1);
2125 radCorrection = NIC(2);
2127 SurfacePoint surfacepoint = point->adjustedSurfacePoint();
2133 pointLat +=
RAD2DEG * latCorrection;
2134 pointLon +=
RAD2DEG * lonCorrection;
2138 if (pointLat < -90.0) {
2139 pointLat = -180.0 - pointLat;
2140 pointLon = pointLon + 180.0;
2142 if (pointLat > 90.0) {
2143 pointLat = 180.0 - pointLat;
2144 pointLon = pointLon + 180.0;
2146 while (pointLon > 360.0) {
2147 pointLon = pointLon - 360.0;
2149 while (pointLon < 0.0) {
2150 pointLon = pointLon + 360.0;
2153 pointRad += 1000.*radCorrection;
2156 corrections(0) += latCorrection;
2157 corrections(1) += lonCorrection;
2158 corrections(2) += radCorrection;
2162 if (m_bundleTargetBody && (m_bundleTargetBody->solveMeanRadius()
2163 || m_bundleTargetBody->solveTriaxialRadii())) {
2164 if (m_bundleTargetBody->solveMeanRadius()) {
2167 m_bundleTargetBody->meanRadius());
2169 else if (m_bundleTargetBody->solveTriaxialRadii()) {
2170 Distance localRadius = m_bundleTargetBody->
2171 localRadius(
Latitude(pointLat, Angle::Degrees),
2181 Distance(pointRad, Distance::Meters));
2184 point->setAdjustedSurfacePoint(surfacepoint);
2203 double BundleAdjust::computeResiduals() {
2205 double vtpvControl = 0.0;
2206 double vtpvImage = 0.0;
2211 m_xResiduals.Reset();
2212 m_yResiduals.Reset();
2213 m_xyResiduals.Reset();
2216 int numObjectPoints = m_bundleControlPoints.size();
2218 for (
int i = 0; i < numObjectPoints; i++) {
2222 point->computeResiduals();
2224 int numMeasures = point->numberOfMeasures();
2225 for (
int j = 0; j < numMeasures; j++) {
2228 weight = 1.4 * (measure->camera())->PixelPitch();
2229 weight = 1.0 / weight;
2232 vx = measure->focalPlaneMeasuredX() - measure->focalPlaneComputedX();
2233 vy = measure->focalPlaneMeasuredY() - measure->focalPlaneComputedY();
2236 if (measure->isRejected()) {
2240 m_xResiduals.AddData(vx);
2241 m_yResiduals.AddData(vy);
2242 m_xyResiduals.AddData(vx);
2243 m_xyResiduals.AddData(vy);
2245 vtpv += vx * vx * weight + vy * vy * weight;
2251 for (
int i = 0; i < numObjectPoints; i++) {
2255 boost::numeric::ublas::bounded_vector<double, 3> weights = bundleControlPoint->weights();
2256 boost::numeric::ublas::bounded_vector<double, 3> corrections =
2257 bundleControlPoint->corrections();
2259 if ( weights(0) > 0.0 ) {
2260 vtpvControl += corrections(0) * corrections(0) * weights(0);
2262 if ( weights(1) > 0.0 ) {
2263 vtpvControl += corrections(1) * corrections(1) * weights(1);
2265 if ( weights(2) > 0.0 ) {
2266 vtpvControl += corrections(2) * corrections(2) * weights(2);
2273 for (
int i = 0; i < m_bundleObservations.size(); i++) {
2280 for (
int j = 0; j < (int)corrections.size(); j++) {
2281 if (weights[j] > 0.0) {
2283 vtpvImage += v * v * weights[j];
2289 double vtpvTargetBody = 0.0;
2290 if ( m_bundleTargetBody) {
2291 vtpvTargetBody = m_bundleTargetBody->vtpv();
2294 vtpv = vtpv + vtpvControl + vtpvImage + vtpvTargetBody;
2298 m_bundleResults.setRmsXYResiduals(m_xResiduals.Rms(), m_yResiduals.Rms(), m_xyResiduals.Rms());
2310 bool BundleAdjust::wrapUp() {
2314 for (
int i = 0; i < m_bundleControlPoints.size(); i++) {
2316 point->computeResiduals();
2319 computeBundleStatistics();
2339 bool BundleAdjust::computeRejectionLimit() {
2342 int numResiduals = m_bundleResults.numberObservations() / 2;
2344 std::vector<double> residuals;
2346 residuals.resize(numResiduals);
2349 int residualIndex = 0;
2350 int numObjectPoints = m_bundleControlPoints.size();
2351 for (
int i = 0; i < numObjectPoints; i++) {
2355 if ( point->isRejected() ) {
2359 int numMeasures = point->numberOfMeasures();
2360 for (
int j = 0; j < numMeasures; j++) {
2364 if ( measure->isRejected() ) {
2368 vx = measure->sampleResidual();
2369 vy = measure->lineResidual();
2371 residuals[residualIndex] = sqrt(vx*vx + vy*vy);
2378 std::sort(residuals.begin(), residuals.end());
2384 int midpointIndex = numResiduals / 2;
2386 if ( numResiduals % 2 == 0 ) {
2387 median = (residuals[midpointIndex - 1] + residuals[midpointIndex]) / 2;
2390 median = residuals[midpointIndex];
2394 for (
int i = 0; i < numResiduals; i++) {
2395 residuals[i] = fabs(residuals[i] - median);
2398 std::sort(residuals.begin(), residuals.end());
2400 if ( numResiduals % 2 == 0 ) {
2401 medianDev = (residuals[midpointIndex - 1] + residuals[midpointIndex]) / 2;
2404 medianDev = residuals[midpointIndex];
2407 std::cout <<
"median deviation: " << medianDev << std::endl;
2409 mad = 1.4826 * medianDev;
2411 std::cout <<
"mad: " << mad <<
"\n";
2413 m_bundleResults.setRejectionLimit(median
2414 + m_bundleSettings->outlierRejectionMultiplier() * mad);
2416 std::cout <<
"Rejection Limit: " << m_bundleResults.rejectionLimit() << std::endl;
2429 bool BundleAdjust::flagOutliers() {
2432 int totalNumRejected = 0;
2434 int maxResidualIndex;
2437 double usedRejectionLimit = m_bundleResults.rejectionLimit();
2441 int numComingBack = 0;
2443 int numObjectPoints = m_bundleControlPoints.size();
2444 for (
int i = 0; i < numObjectPoints; i++) {
2447 point->zeroNumberOfRejectedMeasures();
2450 maxResidualIndex = -1;
2453 int numMeasures = point->numberOfMeasures();
2454 for (
int j = 0; j < numMeasures; j++) {
2458 vx = measure->sampleResidual();
2459 vy = measure->lineResidual();
2461 sumSquares = sqrt(vx*vx + vy*vy);
2464 if ( sumSquares <= usedRejectionLimit ) {
2467 if ( measure->isRejected() ) {
2468 printf(
"Coming back in: %s\r",point->id().toLatin1().data());
2470 m_controlNet->DecrementNumberOfRejectedMeasuresInImage(measure->cubeSerialNumber());
2473 measure->setRejected(
false);
2478 if ( measure->isRejected() ) {
2484 if ( sumSquares > maxResidual ) {
2485 maxResidual = sumSquares;
2486 maxResidualIndex = j;
2491 if ( maxResidual == -1.0 || maxResidual <= usedRejectionLimit ) {
2492 point->setNumberOfRejectedMeasures(numRejected);
2498 if ((numMeasures - (numRejected + 1)) < 2) {
2499 point->setNumberOfRejectedMeasures(numRejected);
2508 rejected->setRejected(
true);
2510 point->setNumberOfRejectedMeasures(numRejected);
2511 m_controlNet->IncrementNumberOfRejectedMeasuresInImage(rejected->cubeSerialNumber());
2515 if ( ( numMeasures-numRejected ) < 2 ) {
2516 point->setRejected(
true);
2517 printf(
"Rejecting Entire Point: %s\r",point->id().toLatin1().data());
2520 point->setRejected(
false);
2524 int numberRejectedObservations = 2*totalNumRejected;
2526 printf(
"\nRejected Observations:%10d (Rejection Limit:%12.5lf)\n",
2527 numberRejectedObservations, usedRejectionLimit);
2528 m_bundleResults.setNumberRejectedObservations(numberRejectedObservations);
2530 std::cout <<
"Measures that came back: " << numComingBack <<
"\n" << std::endl;
2550 bool BundleAdjust::errorPropagation() {
2553 cholmod_free_triplet(&m_cholmodTriplet, &m_cholmodCommon);
2554 cholmod_free_sparse(&m_cholmodNormal, &m_cholmodCommon);
2557 double sigmaLat, sigmaLon, sigmaRad;
2560 double sigma0Squared = m_bundleResults.sigma0() * m_bundleResults.sigma0();
2562 int numObjectPoints = m_bundleControlPoints.size();
2564 std::string currentTime = iTime::CurrentLocalTime().toLatin1().data();
2565 printf(
" Time: %s\n\n", currentTime.c_str());
2568 std::vector< symmetric_matrix<double> > pointCovariances(numObjectPoints,
2569 symmetric_matrix<double>(3));
2570 for (
int d = 0; d < numObjectPoints; d++) {
2571 pointCovariances[d].clear();
2577 b = cholmod_zeros ( m_rank, 1, CHOLMOD_REAL, &m_cholmodCommon );
2578 double *pb = (
double*)b->x;
2585 FileName matrixFile(m_bundleSettings->outputFilePrefix() +
"inverseMatrix.dat");
2589 QFile matrixOutput(matrixFile.expanded());
2592 if (m_bundleSettings->createInverseMatrix()) {
2594 matrixOutput.open(QIODevice::WriteOnly);
2596 QDataStream outStream(&matrixOutput);
2599 int columnIndex = 0;
2601 int numBlockColumns = m_sparseNormals.size();
2603 for (i = 0; i < numBlockColumns; i++) {
2609 int numRows = m_sparseNormals.at(i)->numberOfRows();
2610 inverseMatrix.insertMatrixBlock(i, numRows, numColumns);
2611 inverseMatrix.zeroBlocks();
2615 int numRows = m_sparseNormals.at(i)->numberOfRows();
2616 inverseMatrix.insertMatrixBlock(i, numRows, numColumns);
2617 inverseMatrix.zeroBlocks();
2623 inverseMatrix.wipe();
2626 for (j = 0; j < (i+1); j++) {
2630 inverseMatrix.insertMatrixBlock(j, numRows, numColumns);
2638 for (j = 0; j < numColumns; j++) {
2639 if ( columnIndex > 0 ) {
2640 pb[columnIndex- 1] = 0.0;
2642 pb[columnIndex] = 1.0;
2644 x = cholmod_solve ( CHOLMOD_A, m_L, b, &m_cholmodCommon );
2649 for (k = 0; k < inverseMatrix.size(); k++) {
2652 int sz1 = matrix->size1();
2654 for (
int ii = 0; ii < sz1; ii++) {
2655 (*matrix)(ii,localCol) = px[ii + rp];
2657 rp += matrix->size1();
2663 cholmod_free_dense(&x,&m_cholmodCommon);
2667 if (m_bundleSettings->solveTargetBody() && i == 0) {
2668 vector< double > &adjustedSigmas = m_bundleTargetBody->adjustedSigmas();
2669 matrix< double > *targetCovMatrix = inverseMatrix.value(i);
2671 for (
int z = 0; z < numColumns; z++)
2672 adjustedSigmas[z] = sqrt((*targetCovMatrix)(z,z))*m_bundleResults.sigma0();
2677 if (m_bundleSettings->solveTargetBody()) {
2678 observation = m_bundleObservations.at(i-1);
2681 observation = m_bundleObservations.at(i);
2683 vector< double > &adjustedSigmas = observation->adjustedSigmas();
2684 matrix< double > *imageCovMatrix = inverseMatrix.value(i);
2685 for (
int z = 0; z < numColumns; z++) {
2686 adjustedSigmas[z] = sqrt((*imageCovMatrix)(z,z))*m_bundleResults.sigma0();
2691 if (m_bundleSettings->createInverseMatrix()) {
2692 outStream << inverseMatrix;
2697 for (j = 0; j < numObjectPoints; j++) {
2700 if ( point->isRejected() ) {
2706 printf(
"\rError Propagation: Inverse Block %8d of %8d; Point %8d of %8d", i+1,
2707 numBlockColumns, j+1, numObjectPoints);
2709 emit iterationUpdate(i+1, j+1);
2718 boost::numeric::ublas::symmetric_matrix<double> &covariance = pointCovariances[pointIndex];
2729 QMapIterator< int, LinearAlgebra::Matrix * > it(Q);
2730 while ( it.hasNext() ) {
2733 int nKey = it.key();
2741 if ( !secondQBlock ) {
2747 if ( !inverseBlock ) {
2751 T = prod(*inverseBlock, trans(*firstQBlock));
2752 T = prod(*secondQBlock,T);
2762 catch (std::exception &e) {
2764 QString msg =
"Input data and settings are not sufficiently stable "
2765 "for error propagation.";
2773 if (m_bundleSettings->createInverseMatrix()) {
2775 matrixOutput.close();
2777 m_bundleResults.setCorrMatCovFileName(matrixFile);
2781 m_sparseNormals.wipe();
2784 cholmod_free_dense(&b,&m_cholmodCommon);
2788 printf(
"\rFilling point covariance matrices: Time %s", currentTime.c_str());
2793 for (j = 0; j < numObjectPoints; j++) {
2797 if ( point->isRejected() ) {
2802 printf(
"\rError Propagation: Filling point covariance matrices %8d of %8d\r",j+1,
2807 boost::numeric::ublas::symmetric_matrix<double> &covariance = pointCovariances[pointIndex];
2813 sigmaLat = SurfacePoint.GetLatSigma().
radians();
2814 sigmaLon = SurfacePoint.GetLonSigma().
radians();
2815 sigmaRad = SurfacePoint.GetLocalRadiusSigma().
meters();
2817 t = sigmaLat * sigmaLat + covariance(0, 0);
2818 Distance latSigmaDistance(sqrt(sigma0Squared * t) * m_radiansToMeters, Distance::Meters);
2820 t = sigmaLon * sigmaLon + covariance(1, 1);
2821 t = sqrt(sigma0Squared * t) * m_radiansToMeters;
2823 t * cos(point->adjustedSurfacePoint().GetLatitude().radians()),
2826 t = sigmaRad*sigmaRad + covariance(2, 2);
2827 t = sqrt(sigma0Squared * t) * 1000.0;
2832 point->setAdjustedSurfacePoint(SurfacePoint);
2847 return m_controlNet;
2857 return m_serialNumberList;
2866 int BundleAdjust::numberOfImages()
const {
2867 return m_serialNumberList->
size();
2879 QString BundleAdjust::fileName(
int i) {
2880 return m_serialNumberList->fileName(i);
2889 double BundleAdjust::iteration()
const {
2902 return m_controlNet->Camera(i)->instrumentRotation()->Cache(
"InstrumentPointing");
2914 return m_controlNet->Camera(i)->instrumentPosition()->Cache(
"InstrumentPosition");
2926 void BundleAdjust::iterationSummary() {
2927 QString iterationNumber;
2929 if ( m_bundleResults.converged() ) {
2930 iterationNumber =
"Iteration" +
toString(m_iteration) +
": Final";
2933 iterationNumber =
"Iteration" +
toString(m_iteration);
2936 PvlGroup summaryGroup(iterationNumber);
2939 toString( m_bundleResults.sigma0() ) );
2941 toString( m_bundleResults.numberObservations() ) );
2942 summaryGroup +=
PvlKeyword(
"Constrained_Point_Parameters",
2943 toString( m_bundleResults.numberConstrainedPointParameters() ) );
2944 summaryGroup +=
PvlKeyword(
"Constrained_Image_Parameters",
2945 toString( m_bundleResults.numberConstrainedImageParameters() ) );
2946 if (m_bundleSettings->bundleTargetBody()) {
2947 summaryGroup +=
PvlKeyword(
"Constrained_Target_Parameters",
2948 toString( m_bundleResults.numberConstrainedTargetParameters() ) );
2950 summaryGroup +=
PvlKeyword(
"Unknown_Parameters",
2951 toString( m_bundleResults.numberUnknownParameters() ) );
2952 summaryGroup +=
PvlKeyword(
"Degrees_of_Freedom",
2953 toString( m_bundleResults.degreesOfFreedom() ) );
2954 summaryGroup +=
PvlKeyword(
"Rejected_Measures",
2955 toString( m_bundleResults.numberRejectedObservations()/2) );
2957 if ( m_bundleResults.numberMaximumLikelihoodModels() >
2958 m_bundleResults.maximumLikelihoodModelIndex() ) {
2961 summaryGroup +=
PvlKeyword(
"Maximum_Likelihood_Tier: ",
2962 toString( m_bundleResults.maximumLikelihoodModelIndex() ) );
2963 summaryGroup +=
PvlKeyword(
"Median_of_R^2_residuals: ",
2964 toString( m_bundleResults.maximumLikelihoodMedianR2Residuals() ) );
2967 if ( m_bundleResults.converged() ) {
2968 summaryGroup +=
PvlKeyword(
"Converged",
"TRUE");
2969 summaryGroup +=
PvlKeyword(
"TotalElapsedTime",
toString( m_bundleResults.elapsedTime() ) );
2971 if (m_bundleSettings->errorPropagation()) {
2972 summaryGroup +=
PvlKeyword(
"ErrorPropagationElapsedTime",
2973 toString( m_bundleResults.elapsedTimeErrorProp() ) );
2977 std::ostringstream ostr;
2978 ostr << summaryGroup << std::endl;
2979 m_iterationSummary += QString::fromStdString( ostr.str() );
2980 if (m_printSummary) {
2981 Application::Log(summaryGroup);
2991 bool BundleAdjust::isConverged() {
2992 return m_bundleResults.converged();
3003 QString BundleAdjust::iterationSummaryGroup()
const {
3004 return m_iterationSummary;
3017 void BundleAdjust::outputBundleStatus(QString status) {
3019 printf(
"%s", status.toStdString().c_str());
3059 bool BundleAdjust::computeBundleStatistics() {
3064 int numberImages = m_serialNumberList->size();
3069 int numObjectPoints = m_bundleControlPoints.size();
3070 for (
int i = 0; i < numObjectPoints; i++) {
3074 if (point->isRejected()) {
3078 int numMeasures = point->numberOfMeasures();
3079 for (
int j = 0; j < numMeasures; j++) {
3083 if (measure->isRejected()) {
3087 double sampleResidual = fabs(measure->sampleResidual());
3088 double lineResidual = fabs(measure->lineResidual());
3091 int imageIndex = m_serialNumberList->serialNumberIndex(measure->cubeSerialNumber());
3094 rmsImageSampleResiduals[imageIndex].AddData(sampleResidual);
3095 rmsImageLineResiduals[imageIndex].AddData(lineResidual);
3096 rmsImageResiduals[imageIndex].AddData(lineResidual);
3097 rmsImageResiduals[imageIndex].AddData(sampleResidual);
3102 if (m_bundleSettings->errorPropagation()) {
3106 QString minSigmaLatPointId =
"";
3109 QString maxSigmaLatPointId =
"";
3112 QString minSigmaLonPointId =
"";
3115 QString maxSigmaLonPointId =
"";
3118 QString minSigmaRadPointId =
"";
3121 QString maxSigmaRadPointId =
"";
3128 Distance sigmaLatDist, sigmaLonDist, sigmaRadDist;
3130 int numPoints = m_bundleControlPoints.size();
3132 for (
int i = 0; i < numPoints; i++) {
3136 maxSigmaLatDist = point->adjustedSurfacePoint().GetLatSigmaDistance();;
3137 minSigmaLatDist = maxSigmaLatDist;
3139 maxSigmaLonDist = point->adjustedSurfacePoint().GetLonSigmaDistance();;
3140 minSigmaLonDist = maxSigmaLonDist;
3142 maxSigmaLatPointId = point->id();
3143 maxSigmaLonPointId = maxSigmaLatPointId;
3144 minSigmaLatPointId = maxSigmaLatPointId;
3145 minSigmaLonPointId = maxSigmaLatPointId;
3147 if (m_bundleSettings->solveRadius()) {
3148 maxSigmaRadDist = point->adjustedSurfacePoint().GetLocalRadiusSigma();
3149 minSigmaRadDist = maxSigmaRadDist;
3151 maxSigmaRadPointId = maxSigmaLatPointId;
3152 minSigmaRadPointId = maxSigmaLatPointId;
3157 for (
int i = 0; i < numPoints; i++) {
3161 sigmaLatDist = point->adjustedSurfacePoint().GetLatSigmaDistance();
3162 sigmaLonDist = point->adjustedSurfacePoint().GetLonSigmaDistance();
3163 sigmaRadDist = point->adjustedSurfacePoint().GetLocalRadiusSigma();
3169 if (sigmaLatDist > maxSigmaLatDist) {
3170 maxSigmaLatDist = sigmaLatDist;
3171 maxSigmaLatPointId = point->id();
3173 if (sigmaLonDist > maxSigmaLonDist) {
3174 maxSigmaLonDist = sigmaLonDist;
3175 maxSigmaLonPointId = point->id();
3177 if (m_bundleSettings->solveRadius()) {
3178 if (sigmaRadDist > maxSigmaRadDist) {
3179 maxSigmaRadDist = sigmaRadDist;
3180 maxSigmaRadPointId = point->id();
3183 if (sigmaLatDist < minSigmaLatDist) {
3184 minSigmaLatDist = sigmaLatDist;
3185 minSigmaLatPointId = point->id();
3187 if (sigmaLonDist < minSigmaLonDist) {
3188 minSigmaLonDist = sigmaLonDist;
3189 minSigmaLonPointId = point->id();
3191 if (m_bundleSettings->solveRadius()) {
3192 if (sigmaRadDist < minSigmaRadDist) {
3193 minSigmaRadDist = sigmaRadDist;
3194 minSigmaRadPointId = point->id();
3200 m_bundleResults.resizeSigmaStatisticsVectors(numberImages);
3202 m_bundleResults.setSigmaLatitudeRange(minSigmaLatDist, maxSigmaLatDist,
3203 minSigmaLatPointId, maxSigmaLatPointId);
3205 m_bundleResults.setSigmaLongitudeRange(minSigmaLonDist, maxSigmaLonDist,
3206 minSigmaLonPointId, maxSigmaLonPointId);
3208 m_bundleResults.setSigmaRadiusRange(minSigmaRadDist, maxSigmaRadDist,
3209 minSigmaRadPointId, maxSigmaRadPointId);
3211 m_bundleResults.setRmsFromSigmaStatistics(sigmaLatStats.
Rms(),
3212 sigmaLonStats.
Rms(),
3213 sigmaRadStats.
Rms());
3215 m_bundleResults.setRmsImageResidualLists(rmsImageLineResiduals.toList(),
3216 rmsImageSampleResiduals.toList(),
3217 rmsImageResiduals.toList());
This class defines a body-fixed surface point.
This represents an ISIS control net in a project-based GUI interface.
void setRunTime(QString runTime)
Sets the run time.
virtual CameraType GetCameraType() const =0
Returns the type of camera that was created.
std::vector< double > EllipsoidPartial(SurfacePoint spoint, PartialType raxis)
Compute derivative of focal plane coordinate w/r to one of the ellipsoidal radii (a, b, or c)
Internalizes a list of images and allows for operations on the entire list.
QSharedPointer< BundleSettings > BundleSettingsQsp
Definition for a BundleSettingsQsp, a shared pointer to a BundleSettings object.
static QString CurrentLocalTime()
Returns the current local time This time is taken directly from the system clock, so if the system cl...
double degrees() const
Get the angle in units of Degrees.
File name manipulation and expansion.
QSharedPointer< ControlNet > ControlNetQsp
This typedef is for future implementation of target body.
QSharedPointer< BundleObservation > parentBundleObservation()
Accesses the parent BundleObservation for this bundle measure.
Distance GetLocalRadius() const
Return the radius of the surface point.
Container class for BundleAdjustment results.
double radians() const
Convert an angle to a double.
Camera(Cube &cube)
Constructs the Camera object.
void add(const QString &filename, bool def2filename=false)
Adds a new filename / serial number pair to the SerialNumberList.
int size() const
How many serial number / filename combos are in the list.
QString fileName() const
Get the file name of the cube that this image represents.
virtual bool GetXY(const SurfacePoint &spoint, double *cudx, double *cudy)
Compute undistorted focal plane coordinate from ground position using current Spice from SetImage cal...
static void cholmodErrorHandler(int nStatus, const char *file, int nLineNo, const char *message)
Custom error handler for CHOLMOD.
QSharedPointer< BundleObservationSolveSettings > BundleObservationSolveSettingsQsp
Definition for BundleObservationSolveSettingsQsp, a QSharedPointer to a < BundleObservationSolveSet...
virtual bool GetdXYdTOrientation(const SpiceRotation::PartialType varType, int coefIndex, double *cudx, double *cudy)
Compute derivative of focal plane coordinate w/r to target body using current state.
void radii(Distance r[3]) const
Returns the radii of the body in km.
double Rms() const
Computes and returns the rms.
double focalPlaneMeasuredX() const
Accesses the measured focal plane x value for this control measure //TODO verify? ...
void zeroBlocks()
Sets all elements of all matrix blocks to zero.
ErrorType errorType() const
Returns the source of the error for this exception.
This class is designed to encapsulate the concept of a Latitude.
double line() const
Accesses the current line measurement for this control measure.
Unless noted otherwise, the portions of Isis written by the USGS are public domain.
A container class for a ControlMeasure.
void ResetLocalRadius(const Distance &radius)
This method resets the local radius of a SurfacePoint.
boost::numeric::ublas::matrix< double > Matrix
Definition for an Isis::LinearAlgebra::Matrix of doubles.
QString toString(bool boolToConvert)
Global function to convert a boolean to a string.
Unless noted otherwise, the portions of Isis written by the USGS are public domain.
Distance measurement, usually in meters.
QSharedPointer< BundleObservation > BundleObservationQsp
Typdef for BundleObservation QSharedPointer.
Longitude GetLongitude() const
Return the body-fixed longitude for the surface point.
void wipe()
Deletes all pointer elements and removes them from the map.
bool SetImage(const double sample, const double line)
Sets the sample/line values of the image to get the lat/lon values.
This class is used to accumulate statistics on double arrays.
This class is designed to encapsulate the concept of a Longitude.
This class hold image information that BundleAdjust needs to run correctly.Definition for a BundleIma...
boost::numeric::ublas::vector< double > Vector
Definition for an Isis::LinearAlgebra::Vector of doubles.
SurfacePoint adjustedSurfacePoint() const
Accesses the adjusted SurfacePoint associated with this BundleControlPoint.
Program progress reporter.
double sample() const
Accesses the current sample measurement for this control measure.
virtual bool GetdXYdPoint(std::vector< double > d_lookB, double *cudx, double *cudy)
Compute derivative of focal plane coordinate w/r to ground point using current state.
double meters() const
Get the distance in meters.
int numberOfRows()
Returns total number of rows in map (this needs to be clarified and maybe rewritten).
std::vector< double > MeanRadiusPartial(SurfacePoint spoint, Distance meanRadius)
Compute derivative of focal plane coordinate w/r to mean of the ellipsoidal radii (a...
CameraGroundMap * GroundMap()
Returns a pointer to the CameraGroundMap object.
Latitude GetLatitude() const
Return the body-fixed latitude for the surface point.
double focalPlaneMeasuredY() const
Accesses the measured focal plane y value for this control measure //TODO verify? ...
virtual bool GetdXYdPosition(const SpicePosition::PartialType varType, int coefIndex, double *cudx, double *cudy)
Compute derivative w/r to position of focal plane coordinate from ground position using current Spice...
Contains multiple PvlContainers.
bool insertMatrixBlock(int nRowBlock, int nRows, int nCols)
Inserts a "newed" LinearAlgebra::Matrix pointer of size (nRows, nCols) into the map with the block ro...
#define _FILEINFO_
Macro for the filename and line number.
A single keyword-value pair.
This class holds information about a control point that BundleAdjust needs to run corretly...
This represents a cube in a project-based GUI interface.
QString fileName() const
Access the name of the control network file associated with this Control.
QString cubeSerialNumber() const
Accesses the serial number of the cube containing this control measure.
const QSharedPointer< BundleObservationSolveSettings > observationSolveSettings()
Accesses the parent observation's solve settings.
virtual bool GetdXYdOrientation(const SpiceRotation::PartialType varType, int coefIndex, double *cudx, double *cudy)
Compute derivative of focal plane coordinate w/r to instrument using current state from SetImage call...
int numberOfColumns()
Returns total number of columns in map (NOTE: NOT the number of matrix blocks).
Unless noted otherwise, the portions of Isis written by the USGS are public domain.
Unless noted otherwise, the portions of Isis written by the USGS are public domain.
void SetSphericalMatrix(const boost::numeric::ublas::symmetric_matrix< double, boost::numeric::ublas::upper > &covar)
Set spherical covariance matrix.
Class for storing Table blobs information.
double PixelPitch() const
Returns the pixel pitch.
QSharedPointer< BundleMeasure > BundleMeasureQsp
Definition for BundleMeasureQsp, a shared pointer to a BundleMeasure.
std::vector< double > PointPartial(SurfacePoint spoint, PartialType wrt)
Compute derivative with respect to indicated variable of conversion function from lat/lon/rad to rect...
bool insertMatrixBlock(int nColumnBlock, int nRows, int nCols)
Inserts a "newed" LinearAlgebra::Matrix pointer of size (nRows, nCols) into the map with the block co...
void SetSphericalSigmasDistance(const Distance &latSigma, const Distance &lonSigma, const Distance &radiusSigma)
Set the spherical sigmas (in meters) into the spherical variance/covariance matrix.
Camera * camera() const
Accesses the associated camera for this bundle measure.
QString id() const
Accesses the Point ID associated with this BundleControlPoint.
QSharedPointer< BundleControlPoint > BundleControlPointQsp
Definition for BundleControlPointQSP, a shared pointer to a BundleControlPoint.
void AddData(const double *data, const unsigned int count)
Add an array of doubles to the accumulators and counters.
Serial Number list generator.
void SetSphericalCoordinates(const Latitude &lat, const Longitude &lon, const Distance &radius)
Update spherical coordinates (lat/lon/radius)
const double RAD2DEG(57.29577951308232087679815481)
Multiplier for converting from radians to degrees.
Unless noted otherwise, the portions of Isis written by the USGS are public domain.