Loading [MathJax]/jax/output/NativeMML/config.js
Isis 3 Programmer Reference
BundleAdjust.cpp
1
6
7/* SPDX-License-Identifier: CC0-1.0 */
8
9#include "BundleAdjust.h"
10
11// std lib
12#include <iomanip>
13#include <iostream>
14#include <sstream>
15
16// qt lib
17#include <QCoreApplication>
18#include <QDebug>
19#include <QFile>
20#include <QMutex>
21
22// boost lib
23#include <boost/lexical_cast.hpp>
24#include <boost/numeric/ublas/io.hpp>
25#include <boost/numeric/ublas/matrix_proxy.hpp>
26#include <boost/numeric/ublas/matrix_sparse.hpp>
27#include <boost/numeric/ublas/vector_proxy.hpp>
28
29// Isis lib
30#include "Application.h"
31#include "BundleLidarControlPoint.h"
32#include "BundleMeasure.h"
33#include "BundleObservation.h"
34#include "IsisBundleObservation.h"
35#include "BundleObservationSolveSettings.h"
36#include "BundleResults.h"
37#include "BundleSettings.h"
38#include "BundleSolutionInfo.h"
39#include "BundleTargetBody.h"
40#include "Camera.h"
41#include "CameraDetectorMap.h"
42#include "CameraDistortionMap.h"
43#include "CameraFocalPlaneMap.h"
44#include "CameraGroundMap.h"
45#include "CSMCamera.h"
46#include "Control.h"
47#include "ControlPoint.h"
48#include "CorrelationMatrix.h"
49#include "Distance.h"
50#include "ImageList.h"
51#include "iTime.h"
52#include "Latitude.h"
53#include "LidarControlPoint.h"
54#include "Longitude.h"
55#include "MaximumLikelihoodWFunctions.h"
56#include "SpecialPixel.h"
57#include "StatCumProbDistDynCalc.h"
58#include "SurfacePoint.h"
59#include "Target.h"
60
61using namespace boost::numeric::ublas;
62
63namespace Isis {
64
65
75 static void cholmodErrorHandler(int nStatus,
76 const char* file,
77 int nLineNo,
78 const char* message) {
79 QString errlog;
80
81 errlog = "SPARSE: ";
82 errlog += message;
83
84 PvlGroup gp(errlog);
85
86 gp += PvlKeyword("File",file);
87 gp += PvlKeyword("Line_Number", toString(nLineNo));
88 gp += PvlKeyword("Status", toString(nStatus));
89
90// Application::Log(gp);
91
92 errlog += ". (See print.prt for details)";
93
94// throw IException(IException::Unknown, errlog, _FILEINFO_);
95 }
96
97
108 const QString &cnetFile,
109 const QString &cubeList,
110 bool printSummary) {
111 m_abort = false;
112 Progress progress;
113 // initialize constructor dependent settings...
114 // m_printSummary, m_cleanUp, m_cnetFileName, m_controlNet,
115 // m_serialNumberList, m_bundleSettings
116 m_printSummary = printSummary;
117 m_cleanUp = true;
118 m_cnetFileName = cnetFile;
119 try {
120 m_controlNet = ControlNetQsp( new ControlNet(cnetFile, &progress,
121 bundleSettings->controlPointCoordTypeReports()) );
122 }
123 catch (IException &e) {
124 throw;
125 }
126 m_bundleResults.setOutputControlNet(m_controlNet);
127 m_serialNumberList = new SerialNumberList(cubeList);
128 m_bundleSettings = bundleSettings;
129 m_bundleTargetBody = bundleSettings->bundleTargetBody();
130
131 init(&progress);
132 }
133
134
146 const QString &cnetFile,
147 const QString &cubeList,
148 const QString &lidarDataFile,
149 bool printSummary) {
150 m_abort = false;
151 Progress progress;
152 // initialize constructor dependent settings...
153 // m_printSummary, m_cleanUp, m_cnetFileName, m_controlNet,
154 // m_serialNumberList, m_bundleSettings
155 m_printSummary = printSummary;
156 m_cleanUp = true;
157 m_cnetFileName = cnetFile;
158 m_lidarFileName = lidarDataFile;
159 try {
160 m_controlNet = ControlNetQsp( new ControlNet(cnetFile, &progress) );
161 }
162 catch (IException &e) {
163 throw;
164 }
165
166 // read lidar point data file
167 try {
169 m_lidarDataSet->read(lidarDataFile);
170 }
171 catch (IException &e) {
172 throw;
173 }
174
175 m_bundleResults.setOutputControlNet(m_controlNet);
176 m_bundleResults.setOutputLidarData(m_lidarDataSet);
177 m_serialNumberList = new SerialNumberList(cubeList);
178 m_bundleSettings = bundleSettings;
179 m_bundleTargetBody = bundleSettings->bundleTargetBody();
180
181 init(&progress);
182 }
183
184
196 QString &cnetFile,
197 SerialNumberList &snlist,
198 bool printSummary) {
199 // initialize constructor dependent settings...
200 // m_printSummary, m_cleanUp, m_cnetFileName, m_controlNet,
201 // m_serialNumberList, m_bundleSettings
202 m_abort = false;
203 Progress progress;
204 m_printSummary = printSummary;
205 m_cleanUp = false;
206 m_cnetFileName = cnetFile;
207 try {
208 m_controlNet = ControlNetQsp( new ControlNet(cnetFile, &progress) );
209 }
210 catch (IException &e) {
211 throw;
212 }
213 m_bundleResults.setOutputControlNet(m_controlNet);
214 m_serialNumberList = &snlist;
215 m_bundleSettings = bundleSettings;
216 m_bundleTargetBody = bundleSettings->bundleTargetBody();
217
218 init();
219 }
220
221
233 Control &cnet,
234 SerialNumberList &snlist,
235 bool printSummary) {
236 // initialize constructor dependent settings...
237 // m_printSummary, m_cleanUp, m_cnetFileName, m_controlNet,
238 // m_serialNumberList, m_bundleSettings
239 m_abort = false;
240 Progress progress;
241 m_printSummary = printSummary;
242 m_cleanUp = false;
243 m_cnetFileName = cnet.fileName();
244 try {
245 m_controlNet = ControlNetQsp( new ControlNet(cnet.fileName(), &progress) );
246 }
247 catch (IException &e) {
248 throw;
249 }
250 m_bundleResults.setOutputControlNet(m_controlNet);
251 m_serialNumberList = &snlist;
252 m_bundleSettings = bundleSettings;
253 m_bundleTargetBody = bundleSettings->bundleTargetBody();
254
255 init();
256 }
257
258
270 ControlNet &cnet,
271 SerialNumberList &snlist,
272 bool printSummary) {
273 // initialize constructor dependent settings...
274 // m_printSummary, m_cleanUp, m_cnetFileName, m_controlNet,
275 // m_serialNumberList, m_bundleSettings
276 m_abort = false;
277 m_printSummary = printSummary;
278 m_cleanUp = false;
279 m_cnetFileName = "";
280 try {
282 }
283 catch (IException &e) {
284 throw;
285 }
286 m_bundleResults.setOutputControlNet(m_controlNet);
287 m_serialNumberList = &snlist;
288 m_bundleSettings = bundleSettings;
289 m_bundleTargetBody = bundleSettings->bundleTargetBody();
290
291 init();
292 }
293
294
304 ControlNetQsp cnet,
305 const QString &cubeList,
306 bool printSummary) {
307 m_abort = false;
308 m_printSummary = printSummary;
309 m_cleanUp = false;
310 m_cnetFileName = "";
311 try {
312 m_controlNet = cnet;
313 }
314 catch (IException &e) {
315 throw;
316 }
317 m_bundleResults.setOutputControlNet(m_controlNet);
318 m_serialNumberList = new SerialNumberList(cubeList);
319 m_bundleSettings = bundleSettings;
320 m_bundleTargetBody = bundleSettings->bundleTargetBody();
321
322 init();
323 }
324
325
336 Control &control,
337 QList<ImageList *> imgLists,
338 bool printSummary) {
339 m_bundleSettings = bundleSettings;
340
341 m_abort = false;
342 try {
343 m_controlNet = ControlNetQsp( new ControlNet(control.fileName()) );
344 }
345 catch (IException &e) {
346 throw;
347 }
348 m_bundleResults.setOutputControlNet(m_controlNet);
349
350 m_imageLists = imgLists;
351
352 // this is too slow and we need to get rid of the serial number list anyway
353 // should be unnecessary as Image class has serial number
354 // could hang on to image list until creating BundleObservations?
356
357 foreach (ImageList *imgList, imgLists) {
358 foreach (Image *image, *imgList) {
359 m_serialNumberList->add(image->fileName());
360// m_serialNumberList->add(image->serialNumber(), image->fileName());
361 }
362 }
363
364 m_bundleTargetBody = bundleSettings->bundleTargetBody();
365
366 m_printSummary = printSummary;
367
368 m_cleanUp = false;
369 m_cnetFileName = control.fileName();
370
371 init();
372 }
373
374
384 // If we have ownership
385 if (m_cleanUp) {
386 delete m_serialNumberList;
387 }
388
390
391 }
392
393
425 void BundleAdjust::init(Progress *progress) {
426 emit(statusUpdate("Initialization"));
428
429 // initialize
430 //
431 // JWB
432 // - some of these not originally initialized.. better values???
433 m_iteration = 0;
434 m_rank = 0;
435 m_iterationSummary = "";
436
437 // Get the cameras set up for all images
438 // NOTE - THIS IS NOT THE SAME AS "setImage" as called in BundleAdjust::computePartials
439 // this call only does initializations; sets measure's camera pointer, etc
440 // RENAME????????????
441 m_controlNet->SetImages(*m_serialNumberList, progress);
442
443 if (m_lidarDataSet) {
444 // TODO: (KLE) document why we're (at the moment) required to use an existing control net to
445 // SetImages for the lidar data set. In my opinion this is a major drawback to this
446 // implementation, and a really good argument for a control net design that allows
447 // multiple point sources in the same net (e.g. photogrammetric, lidar, and other?
448 // types).
449 m_lidarDataSet->SetImages(*(m_controlNet.data()), progress);
450 }
451
452 // clear JigsawRejected flags
453 m_controlNet->ClearJigsawRejected();
454
455 // initialize held variables
456 int numImages = m_serialNumberList->size();
457
458 // matrix stuff
459 m_normalInverse.clear();
460 m_RHS.clear();
461 m_imageSolution.clear();
462
463 // we don't want to call initializeCHOLMODLibraryVariables() here since mRank=0
464 // m_cholmodCommon, m_sparseNormals are not initialized
465 m_L = NULL;
466 m_cholmodNormal = NULL;
467 m_cholmodTriplet = NULL;
468
469 // set up BundleObservations and assign solve settings for each from BundleSettings class
470 for (int i = 0; i < numImages; i++) {
471 Camera *camera = m_controlNet->Camera(i);
472 QString observationNumber = m_serialNumberList->observationNumber(i);
473 QString instrumentId = m_serialNumberList->spacecraftInstrumentId(i);
474 QString serialNumber = m_serialNumberList->serialNumber(i);
475 QString fileName = m_serialNumberList->fileName(i);
476
477 // If any camera is initialized via CSMInit, but no csm solve options are specified, fail early.
478 if (camera->GetCameraType() == Camera::Csm && m_bundleSettings->observationSolveSettings(observationNumber).csmSolveOption() == 0){
479 QString msg = fileName + " camera was initialized using CSMInit, so jigsaw must use CSM parameters." +
480 " Please refer to documentation for more information." + "\n";
481 throw IException(IException::User, msg, _FILEINFO_);
482 }
483
484 // create a new BundleImage and add to new (or existing if observation mode is on)
485 // BundleObservation
486 BundleImageQsp image = BundleImageQsp(new BundleImage(camera, serialNumber, fileName));
487
488 if (!image) {
489 QString msg = "In BundleAdjust::init(): image " + fileName + "is null." + "\n";
490 throw IException(IException::Programmer, msg, _FILEINFO_);
491 }
492
493 BundleObservationQsp observation =
494 m_bundleObservations.addNew(image, observationNumber, instrumentId, m_bundleSettings);
495
496 if (!observation) {
497 QString msg = "In BundleAdjust::init(): observation "
498 + observationNumber + "is null." + "\n";
499 throw IException(IException::Programmer, msg, _FILEINFO_);
500 }
501 }
502
503 // set up vector of BundleControlPoints
504 int numControlPoints = m_controlNet->GetNumPoints();
505 for (int i = 0; i < numControlPoints; i++) {
506 ControlPoint *point = m_controlNet->GetPoint(i);
507 if (point->IsIgnored()) {
508 continue;
509 }
510
511 BundleControlPointQsp bundleControlPoint(new BundleControlPoint
512 (m_bundleSettings, point));
513 m_bundleControlPoints.append(bundleControlPoint);
514
515 // set parent observation for each BundleMeasure
516 int numMeasures = bundleControlPoint->size();
517 for (int j = 0; j < numMeasures; j++) {
518 BundleMeasureQsp measure = bundleControlPoint->at(j);
519 QString cubeSerialNumber = measure->cubeSerialNumber();
520
521 BundleObservationQsp observation =
522 m_bundleObservations.observationByCubeSerialNumber(cubeSerialNumber);
523 BundleImageQsp image = observation->imageByCubeSerialNumber(cubeSerialNumber);
524
525 measure->setParentObservation(observation);
526 measure->setParentImage(image);
527 measure->setSigma(1.4);
528 }
529
530 point->ComputeApriori();
531 }
532
533 // set up vector of BundleLidarControlPoints
534 int numLidarPoints = 0;
535 if (m_lidarDataSet) {
536 numLidarPoints = m_lidarDataSet->points().size();
537 }
538 for (int i = 0; i < numLidarPoints; i++) {
539 LidarControlPointQsp lidarPoint = m_lidarDataSet->points().at(i);
540 if (lidarPoint->IsIgnored()) {
541 continue;
542 }
543
545 lidarPoint));
546 m_bundleLidarControlPoints.append(bundleLidarPoint);
547
548 // set parent observation for each BundleMeasure
549 int numMeasures = bundleLidarPoint->size();
550 for (int j = 0; j < numMeasures; j++) {
551 BundleMeasureQsp measure = bundleLidarPoint->at(j);
552 QString cubeSerialNumber = measure->cubeSerialNumber();
553
554 BundleObservationQsp observation =
555 m_bundleObservations.observationByCubeSerialNumber(cubeSerialNumber);
556 BundleImageQsp image = observation->imageByCubeSerialNumber(cubeSerialNumber);
557
558 measure->setParentObservation(observation);
559 measure->setParentImage(image);
560 measure->setSigma(30.0);
561 }
562
563 // WHY ARE WE CALLING COMPUTE APRIORI FOR LIDAR POINTS?
564 // ANSWER: Because the ::computeApriori method is also setting the focal plane measures, see
565 // line 916 in ControlPoint.Constrained_Point_Parameters
566 // This really stinks, maybe we should be setting the focal plane measures here, as part of
567 // the BundleAdjust::init method? Or better yet as part of the BundleControlPoint constructor?
568 // Currently have a kluge in the ControlPoint::setApriori method to not update the coordinates
569 // of lidar points.
570 // Also, maybe we could address Brents constant complaint about points where we can't get a
571 // lat or lon due to bad SPICE causing the bundle to fail.
572 lidarPoint->ComputeApriori();
573
574 // initialize range constraints
575 bundleLidarPoint->initializeRangeConstraints();
576 }
577
578 //===========================================================================================//
579 //==== Use the bundle settings to initialize more member variables and set up solutions =====//
580 //===========================================================================================//
581
582 // TODO: Need to have some validation code to make sure everything is
583 // on the up-and-up with the control network. Add checks for multiple
584 // networks, images without any points, and points on images removed from
585 // the control net (when we start adding software to remove points with high
586 // residuals) and ?. For "deltack" a single measure on a point is allowed
587 // so skip the test.
588 if (m_bundleSettings->validateNetwork()) {
590 }
591 m_bundleResults.maximumLikelihoodSetUp(m_bundleSettings->maximumLikelihoodEstimatorModels());
592
593 //===========================================================================================//
594 //=============== End Bundle Settings =======================================================//
595 //===========================================================================================//
596
597 //===========================================================================================//
598 //======================== initialize matrices and more parameters ==========================//
599 //===========================================================================================//
600
601 // size of reduced normals matrix
602
603 // TODO
604 // this should be determined from BundleSettings
605 // m_rank will be the sum of observation, target, and self-cal parameters
606 // TODO
607 m_rank = m_bundleObservations.numberParameters();
608
609 if (m_bundleSettings->solveTargetBody()) {
610 m_rank += m_bundleSettings->numberTargetBodyParameters();
611
612 if (m_bundleTargetBody->solveMeanRadius() || m_bundleTargetBody->solveTriaxialRadii()) {
613 outputBundleStatus("Warning: Solving for the target body radii (triaxial or mean) "
614 "is NOT possible and likely increases error in the solve.\n");
615 }
616
617 if (m_bundleTargetBody->solveMeanRadius()){
618 // Check if MeanRadiusValue is abnormal compared to observation
619 bool isMeanRadiusValid = true;
620 double localRadius, aprioriRadius;
621
622 // Arbitrary control point containing an observed localRadius
624 SurfacePoint surfacepoint = point->adjustedSurfacePoint();
625
626 localRadius = surfacepoint.GetLocalRadius().meters();
627 aprioriRadius = m_bundleTargetBody->meanRadius().meters();
628
629 // Ensure aprioriRadius is within some threshold
630 // Considered potentially inaccurate if it's off by atleast a factor of two
631 if (aprioriRadius >= 2 * localRadius || aprioriRadius <= localRadius / 2) {
632 isMeanRadiusValid = false;
633 }
634
635 // Warn user for abnormal MeanRadiusValue
636 if (!isMeanRadiusValid) {
637 outputBundleStatus("Warning: User-entered MeanRadiusValue appears to be inaccurate. "
638 "This can cause a bundle failure.\n");
639 }
640 }
641 }
642
643 int num3DPoints = m_bundleControlPoints.size() + m_bundleLidarControlPoints.size();
644
645 m_bundleResults.setNumberUnknownParameters(m_rank + 3 * num3DPoints);
646
647 m_imageSolution.resize(m_rank);
648
649 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
650 // initializations for cholmod
652
653 // initialize normal equations matrix
655 }
656
657
678
679 outputBundleStatus("\nValidating network...");
680
681 int imagesWithInsufficientMeasures = 0;
682 QString msg = "Images with one or less measures:\n";
683 int numObservations = m_bundleObservations.size();
684 for (int i = 0; i < numObservations; i++) {
685 int numImages = m_bundleObservations.at(i)->size();
686 for (int j = 0; j < numImages; j++) {
687 BundleImageQsp bundleImage = m_bundleObservations.at(i)->at(j);
688 int numMeasures = m_controlNet->
689 GetNumberOfValidMeasuresInImage(bundleImage->serialNumber());
690
691 if (numMeasures > 1) {
692 continue;
693 }
694
695 imagesWithInsufficientMeasures++;
696 msg += bundleImage->fileName() + ": " + toString(numMeasures) + "\n";
697 }
698 }
699
700 if ( imagesWithInsufficientMeasures > 0 ) {
701 throw IException(IException::User, msg, _FILEINFO_);
702 }
703
704 outputBundleStatus("\nValidation complete!...\n");
705
706 return true;
707 }
708
709
717 if ( m_rank <= 0 ) {
718 return false;
719 }
720
721 m_cholmodTriplet = NULL;
722
723 cholmod_l_start(&m_cholmodCommon);
724
725 // set user-defined cholmod error handler
726 m_cholmodCommon.error_handler = cholmodErrorHandler;
727
728 // testing not using metis
729 m_cholmodCommon.nmethods = 1;
730 m_cholmodCommon.method[0].ordering = CHOLMOD_AMD;
731
732 return true;
733 }
734
735
745
746 cholmod_l_free_triplet(&m_cholmodTriplet, &m_cholmodCommon);
747 cholmod_l_free_sparse(&m_cholmodNormal, &m_cholmodCommon);
748 cholmod_l_free_factor(&m_L, &m_cholmodCommon);
749
750 cholmod_l_finish(&m_cholmodCommon);
751
752 return true;
753 }
754
755
767
768 int nBlockColumns = m_bundleObservations.size();
769
770 if (m_bundleSettings->solveTargetBody())
771 nBlockColumns += 1;
772
773 m_sparseNormals.setNumberOfColumns(nBlockColumns);
774
775 m_sparseNormals.at(0)->setStartColumn(0);
776
777 int nParameters = 0;
778 if (m_bundleSettings->solveTargetBody()) {
779 nParameters += m_bundleSettings->numberTargetBodyParameters();
780 m_sparseNormals.at(1)->setStartColumn(nParameters);
781
782 int observation = 0;
783 for (int i = 2; i < nBlockColumns; i++) {
784 nParameters += m_bundleObservations.at(observation)->numberParameters();
785 m_sparseNormals.at(i)->setStartColumn(nParameters);
786 observation++;
787 }
788 }
789 else {
790 for (int i = 0; i < nBlockColumns; i++) {
791 m_sparseNormals.at(i)->setStartColumn(nParameters);
792 nParameters += m_bundleObservations.at(i)->numberParameters();
793 }
794 }
795
796 return true;
797 }
798
799
813
814
820 m_abort = true;
821 }
822
823
839 emit(statusBarUpdate("Solving"));
840 try {
841
842 // throw error if a frame camera is included AND
843 // if m_bundleSettings->solveInstrumentPositionOverHermiteSpline()
844 // is set to true (can only use for line scan or radar)
845 // if (m_bundleSettings->solveInstrumentPositionOverHermiteSpline() == true) {
846 // int numImages = images();
847 // for (int i = 0; i < numImages; i++) {
848 // if (m_controlNet->Camera(i)->GetCameraType() == 0) {
849 // QString msg = "At least one sensor is a frame camera. "
850 // "Spacecraft Option OVERHERMITE is not valid for frame cameras\n";
851 // throw IException(IException::User, msg, _FILEINFO_);
852 // }
853 // }
854 // }
855
856 // ken testing - if solving for target mean radius, set point radius to current mean radius
857 // if solving for triaxial radii, set point radius to local radius
858 if (m_bundleTargetBody && m_bundleTargetBody->solveMeanRadius()) {
859 int numControlPoints = m_bundleControlPoints.size();
860 for (int i = 0; i < numControlPoints; i++) {
862 SurfacePoint surfacepoint = point->adjustedSurfacePoint();
863
864 surfacepoint.ResetLocalRadius(m_bundleTargetBody->meanRadius());
865
866 point->setAdjustedSurfacePoint(surfacepoint);
867 }
868 }
869
870 // Only use target body solution options when using Latitudinal coordinates
871 if (m_bundleTargetBody && m_bundleTargetBody->solveTriaxialRadii()
872 && m_bundleSettings->controlPointCoordTypeBundle() == SurfacePoint::Latitudinal) {
873 int numControlPoints = m_bundleControlPoints.size();
874 for (int i = 0; i < numControlPoints; i++) {
876 SurfacePoint surfacepoint = point->adjustedSurfacePoint();
877
878 Distance localRadius = m_bundleTargetBody->localRadius(surfacepoint.GetLatitude(),
879 surfacepoint.GetLongitude());
880 surfacepoint.ResetLocalRadius(localRadius);
881
882 point->setAdjustedSurfacePoint(surfacepoint);
883 }
884 }
885
886 // Beginning of iterations
887 m_iteration = 1;
888 double vtpv = 0.0;
889 double previousSigma0 = 0.0;
890
891 // start the clock
892 clock_t solveStartClock = clock();
893
894 for (;;) {
895
896 emit iterationUpdate(m_iteration);
897
898 // testing
899 if (m_abort) {
900 m_bundleResults.setConverged(false);
901 emit statusUpdate("\n aborting...");
902 emit finished();
903 return false;
904 }
905 // testing
906
907 emit statusUpdate( QString("\nstarting iteration %1\n").arg(m_iteration) );
908
909 clock_t iterationStartClock = clock();
910
911 // zero normals (after iteration 0)
912 if (m_iteration != 1) {
913 m_sparseNormals.zeroBlocks();
914 }
915
916 // form normal equations -- computePartials is called in here.
917 if (!formNormalEquations()) {
918 m_bundleResults.setConverged(false);
919 break;
920 }
921
922 // testing
923 if (m_abort) {
924 m_bundleResults.setConverged(false);
925 emit statusUpdate("\n aborting...");
926 emit finished();
927 return false;
928 }
929 // testing
930
931 // solve the system
932 if (!solveSystem()) {
933 outputBundleStatus("\nsolve failed!");
934 m_bundleResults.setConverged(false);
935 break;
936 }
937
938 // testing
939 if (m_abort) {
940 m_bundleResults.setConverged(false);
941 emit statusUpdate("\n aborting...");
942 emit finished();
943 return false;
944 }
945 // testing
946
947 // apply parameter corrections
949
950 // testing
951 if (m_abort) {
952 m_bundleResults.setConverged(false);
953 emit statusUpdate("\n aborting...");
954 emit finished();
955 return false;
956 }
957 // testing
958
959 // compute residuals
960 emit(statusBarUpdate("Computing Residuals"));
962
963 // compute vtpv (weighted sum of squares of residuals)
964 vtpv = computeVtpv();
965
966 // flag outliers
967 if ( m_bundleSettings->outlierRejection() ) {
969 flagOutliers();
970 }
971
972 // testing
973 if (m_abort) {
974 m_bundleResults.setConverged(false);
975 emit statusUpdate("\n aborting...");
976 emit finished();
977 return false;
978 }
979 // testing
980
981 // Sigma0 (or "sigma nought") is the standard deviation of an observation of unit weight.
982 // Sigma0^2 is the variance of an observation of unit weight (also reference variance or
983 // variance factor).
984 m_bundleResults.computeSigma0(vtpv, m_bundleSettings->convergenceCriteria());
985
986 // Set up formatting for status updates with doubles (e.g. Sigma0, Elapsed Time)
987 int fieldWidth = 20;
988 char format = 'f';
989 int precision = 10;
990
991 emit statusUpdate(QString("Iteration: %1 \n")
992 .arg(m_iteration));
993 emit statusUpdate(QString("Sigma0: %1 \n")
994 .arg(m_bundleResults.sigma0(),
995 fieldWidth,
996 format,
997 precision));
998 emit statusUpdate(QString("Observations: %1 \n")
999 .arg(m_bundleResults.numberObservations()));
1000 emit statusUpdate(QString("Constrained Parameters:%1 \n")
1001 .arg(m_bundleResults.numberConstrainedPointParameters()));
1002 emit statusUpdate(QString("Unknowns: %1 \n")
1003 .arg(m_bundleResults.numberUnknownParameters()));
1004 emit statusUpdate(QString("Degrees of Freedom: %1 \n")
1005 .arg(m_bundleResults.degreesOfFreedom()));
1006 emit iterationUpdate(m_iteration);
1007
1008 // check for convergence
1009 if (m_bundleSettings->convergenceCriteria() == BundleSettings::Sigma0) {
1010 if (fabs(previousSigma0 - m_bundleResults.sigma0())
1011 <= m_bundleSettings->convergenceCriteriaThreshold()) {
1012 // convergence detected
1013
1014 // if maximum likelihood tiers are being processed,
1015 // check to see if there's another tier to go.
1016 if (m_bundleResults.maximumLikelihoodModelIndex()
1017 < m_bundleResults.numberMaximumLikelihoodModels() - 1
1018 && m_bundleResults.maximumLikelihoodModelIndex()
1019 < 2) {
1020 // TODO is this second condition redundant???
1021 // should BundleResults require num models <= 3, so num models - 1 <= 2
1022 if (m_bundleResults.numberMaximumLikelihoodModels()
1023 > m_bundleResults.maximumLikelihoodModelIndex() + 1) {
1024
1025 // If there is another tier left we will increment the index.
1026 m_bundleResults.incrementMaximumLikelihoodModelIndex();
1027 }
1028 }
1029 else { // otherwise iterations are complete
1030 m_bundleResults.setConverged(true);
1031 emit statusUpdate("Bundle has converged\n");
1032 emit statusBarUpdate("Converged");
1033
1034 clock_t iterationStopClock = clock();
1035 m_iterationTime = (iterationStopClock - iterationStartClock)
1036 / (double)CLOCKS_PER_SEC;
1037 break;
1038 }
1039 }
1040 }
1041 else {
1042 // bundleSettings.convergenceCriteria() == BundleSettings::ParameterCorrections
1043 int numConvergedParams = 0;
1044 int numImgParams = m_imageSolution.size();
1045 for (int ij = 0; ij < numImgParams; ij++) {
1046 if (fabs(m_imageSolution(ij)) > m_bundleSettings->convergenceCriteriaThreshold()) {
1047 break;
1048 }
1049 else
1050 numConvergedParams++;
1051 }
1052
1053 if ( numConvergedParams == numImgParams ) {
1054 m_bundleResults.setConverged(true);
1055 emit statusUpdate("Bundle has converged\n");
1056 emit statusBarUpdate("Converged");
1057 break;
1058 }
1059 }
1060
1061 m_bundleResults.printMaximumLikelihoodTierInformation();
1062 clock_t iterationStopClock = clock();
1063 m_iterationTime = (iterationStopClock - iterationStartClock)
1064 / (double)CLOCKS_PER_SEC;
1065 emit statusUpdate( QString("End of Iteration %1 \n").arg(m_iteration) );
1066 emit statusUpdate( QString("Elapsed Time: %1 \n").arg(m_iterationTime,
1067 fieldWidth,
1068 format,
1069 precision) );
1070
1071 // check for maximum iterations
1072 if (m_iteration >= m_bundleSettings->convergenceCriteriaMaximumIterations()) {
1073 emit(statusBarUpdate("Max Iterations Reached"));
1074 break;
1075 }
1076
1077 // restart the dynamic calculation of the cumulative probility distribution of residuals
1078 // (in unweighted pixels) --so it will be up to date for the next iteration
1079 if (!m_bundleResults.converged()) {
1080 m_bundleResults.initializeResidualsProbabilityDistribution(101);
1081 }
1082
1083 // if we're using CHOLMOD and still going, release cholmod_factor
1084 // (if we don't, memory leaks will occur), otherwise we need it for error propagation
1085 if (!m_bundleResults.converged() || !m_bundleSettings->errorPropagation()) {
1086 cholmod_l_free_factor(&m_L, &m_cholmodCommon);
1087 }
1088
1090
1091 m_iteration++;
1092
1093 previousSigma0 = m_bundleResults.sigma0();
1094 } // end of bundle iteration loop
1095
1096 if (m_bundleResults.converged() && m_bundleSettings->errorPropagation()) {
1097 clock_t errorPropStartClock = clock();
1098
1099 outputBundleStatus("\nStarting Error Propagation");
1100
1102 emit statusUpdate("\n\nError Propagation Complete\n");
1103 clock_t errorPropStopClock = clock();
1104 m_bundleResults.setElapsedTimeErrorProp((errorPropStopClock - errorPropStartClock)
1105 / (double)CLOCKS_PER_SEC);
1106 }
1107
1108 clock_t solveStopClock = clock();
1109 m_bundleResults.setElapsedTime((solveStopClock - solveStartClock)
1110 / (double)CLOCKS_PER_SEC);
1111
1113
1114 m_bundleResults.setIterations(m_iteration);
1115 m_bundleResults.setObservations(m_bundleObservations);
1116 m_bundleResults.setBundleControlPoints(m_bundleControlPoints);
1117
1118 if (!m_bundleLidarControlPoints.isEmpty()) {
1119 m_bundleResults.setBundleLidarPoints(m_bundleLidarControlPoints);
1120 }
1121
1122 emit resultsReady(bundleSolveInformation());
1123
1124 emit statusUpdate("\nBundle Complete\n");
1125
1127 }
1128 catch (IException &e) {
1129 m_bundleResults.setConverged(false);
1130 emit statusUpdate("\n aborting...");
1131 emit statusBarUpdate("Failed to Converge");
1132 emit finished();
1133 QString msg = "Could not solve bundle adjust.";
1134 throw IException(e, e.errorType(), msg, _FILEINFO_);
1135 }
1136
1137 emit finished();
1138 return true;
1139 }
1140
1141
1146
1147 // residuals for photogrammetric measures
1148 emit(statusBarUpdate("Computing Measure Residuals"));
1149 for (int i = 0; i < m_bundleControlPoints.size(); i++) {
1150 m_bundleControlPoints.at(i)->computeResiduals();
1151 }
1152
1153 // residuals for lidar measures
1154 if (!m_bundleLidarControlPoints.isEmpty()) {
1155 emit(statusBarUpdate("Computing Lidar Measure Residuals"));
1156 for (int i = 0; i < m_bundleLidarControlPoints.size(); i++) {
1157 m_bundleLidarControlPoints.at(i)->computeResiduals();
1158 }
1159 }
1160 }
1161
1162
1172 BundleSolutionInfo *bundleSolutionInfo = new BundleSolutionInfo(m_bundleSettings,
1176 imageLists());
1177 bundleSolutionInfo->setRunTime("");
1178 return bundleSolutionInfo;
1179 }
1180
1181
1194 emit(statusBarUpdate("Forming Normal Equations"));
1195 bool status = false;
1196
1197 // Initialize auxilary matrices and vectors.
1198 static LinearAlgebra::Matrix coeffTarget;
1199 static LinearAlgebra::Matrix coeffImage;
1200 static LinearAlgebra::Matrix coeffPoint3D(2, 3);
1201 static LinearAlgebra::Vector coeffRHS(2);
1204 static LinearAlgebra::Vector n2(3);
1206
1207 m_RHS.resize(m_rank);
1208
1209 // if solving for target body parameters, set size of coeffTarget
1210 // (note this size will not change through the adjustment).
1211 if (m_bundleSettings->solveTargetBody()) {
1212 int numTargetBodyParameters = m_bundleSettings->numberTargetBodyParameters();
1213 // TODO make sure numTargetBodyParameters is greater than 0
1214 coeffTarget.resize(2,numTargetBodyParameters);
1215 }
1216
1217 // clear N12, n1, and nj
1218 N12.clear();
1219 n1.clear();
1220 m_RHS.clear();
1221
1222 // clear static matrices
1223 coeffPoint3D.clear();
1224 coeffRHS.clear();
1225 N22.clear();
1226 n2.clear();
1227
1228 // loop over 3D points
1229 int numObservations = 0;
1230 int numGood3DPoints = 0;
1231 int numConstrainedCoordinates = 0;
1232 int num3DPoints = m_bundleControlPoints.size();
1233
1234 outputBundleStatus("\n\n");
1235
1236 for (int i = 0; i < num3DPoints; i++) {
1237 emit(pointUpdate(i+1));
1239
1240 if (point->isRejected()) {
1241 continue;
1242 }
1243
1244 if ( i != 0 ) {
1245 N22.clear();
1246 N12.wipe();
1247 n2.clear();
1248 }
1249
1250 // loop over measures for this point
1251 int numMeasures = point->size();
1252 for (int j = 0; j < numMeasures; j++) {
1253 BundleMeasureQsp measure = point->at(j);
1254
1255 // flagged as "JigsawFail" implies this measure has been rejected
1256 // TODO IsRejected is obsolete -- replace code or add to ControlMeasure
1257 if (measure->isRejected()) {
1258 continue;
1259 }
1260
1261 status = computePartials(coeffTarget, coeffImage, coeffPoint3D, coeffRHS, *measure,
1262 *point);
1263
1264 if (!status) {
1265 // TODO should status be set back to true? JAM
1266 // TODO this measure should be flagged as rejected.
1267 continue;
1268 }
1269
1270 // increment number of observations
1271 numObservations += 2;
1272
1273 formMeasureNormals(N22, N12, n1, n2, coeffTarget, coeffImage, coeffPoint3D, coeffRHS,
1274 measure->observationIndex());
1275
1276 } // end loop over this points measures
1277
1278 numConstrainedCoordinates += formPointNormals(N22, N12, n2, m_RHS, point);
1279
1280 numGood3DPoints++;
1281 } // end loop over 3D points
1282
1283 m_bundleResults.setNumberConstrainedPointParameters(numConstrainedCoordinates);
1284 m_bundleResults.setNumberImageObservations(numObservations);
1285
1286 numObservations = 0;
1287 numConstrainedCoordinates = 0;
1288
1289 // loop over lidar points
1290 int numLidarPoints = m_bundleLidarControlPoints.size();
1292
1293
1294 for (int i = 0; i < numLidarPoints; i++) {
1295 emit(pointUpdate(i+1));
1297
1298 if (point->isRejected()) {
1299 continue;
1300 }
1301
1302 N22.clear();
1303 N12.wipe();
1304 n2.clear();
1305
1306 // loop over measures for this point
1307 int numMeasures = point->size();
1308 for (int j = 0; j < numMeasures; j++) {
1309 BundleMeasureQsp measure = point->at(j);
1310
1311 if (measure->isRejected()) {
1312 continue;
1313 }
1314
1315 status = computePartials(coeffTarget, coeffImage, coeffPoint3D, coeffRHS, *measure,
1316 *point);
1317
1318 if (!status) {
1319 // TODO this measure should be flagged as rejected.
1320 continue;
1321 }
1322
1323 // increment number of lidar image "measurement" observations
1324 numObservations += 2;
1325
1326 formMeasureNormals(N22, N12, n1, n2, coeffTarget, coeffImage, coeffPoint3D, coeffRHS,
1327 measure->observationIndex());
1328
1329 } // end loop over this points measures
1330
1331 m_numLidarConstraints += point->applyLidarRangeConstraints(m_sparseNormals, N22, N12, n1, n2);
1332
1333 numConstrainedCoordinates += formLidarPointNormals(N22, N12, n2, m_RHS, point);
1334 } // end loop over lidar 3D points
1335
1336 m_bundleResults.setNumberLidarRangeConstraints(m_numLidarConstraints);
1337 m_bundleResults.setNumberConstrainedLidarPointParameters(numConstrainedCoordinates);
1338 m_bundleResults.setNumberLidarImageObservations(numObservations);
1339
1340 // form the reduced normal equations
1342
1343 // update number of unknown parameters
1344 m_bundleResults.setNumberUnknownParameters(m_rank + 3 * numGood3DPoints);
1345
1346 return status;
1347}
1348
1349
1373 LinearAlgebra::Matrix &coeffTarget,
1374 LinearAlgebra::Matrix &coeffImage,
1375 LinearAlgebra::Matrix &coeffPoint3D,
1376 LinearAlgebra::Vector &coeffRHS,
1377 int observationIndex) {
1378
1379 int blockIndex = observationIndex;
1380
1381 // if we are solving for target body parameters
1382 if (m_bundleSettings->solveTargetBody()) {
1383 int numTargetPartials = coeffTarget.size2();
1384 blockIndex++;
1385
1386 // insert submatrix at column, row
1387 m_sparseNormals.insertMatrixBlock(0, 0, numTargetPartials, numTargetPartials);
1388
1389 // contribution to N11 matrix for target body
1390 (*(*m_sparseNormals[0])[0]) += prod(trans(coeffTarget), coeffTarget);
1391
1392 m_sparseNormals.insertMatrixBlock(blockIndex, 0,
1393 numTargetPartials, coeffImage.size2());
1394 (*(*m_sparseNormals[blockIndex])[0]) += prod(trans(coeffTarget),coeffImage);
1395
1396 // insert N12 target into N12
1397 N12.insertMatrixBlock(0, numTargetPartials, 3);
1398 *N12[0] += prod(trans(coeffTarget), coeffPoint3D);
1399
1400 // contribution to n1 vector
1401 vector_range<LinearAlgebra::VectorCompressed> n1_range(n1, range(0, numTargetPartials));
1402
1403 n1_range += prod(trans(coeffTarget), coeffRHS);
1404 }
1405
1406
1407 int numImagePartials = coeffImage.size2();
1408
1409 // insert submatrix at column, row
1410 m_sparseNormals.insertMatrixBlock(blockIndex, blockIndex,
1411 numImagePartials, numImagePartials);
1412
1413 (*(*m_sparseNormals[blockIndex])[blockIndex]) += prod(trans(coeffImage), coeffImage);
1414
1415 // insert N12Image into N12
1416 N12.insertMatrixBlock(blockIndex, numImagePartials, 3);
1417 *N12[blockIndex] += prod(trans(coeffImage), coeffPoint3D);
1418
1419 // insert n1Image into n1
1420 vector_range<LinearAlgebra::VectorCompressed> vr(
1421 n1,
1422 range(
1423 m_sparseNormals.at(blockIndex)->startColumn(),
1424 m_sparseNormals.at(blockIndex)->startColumn() + numImagePartials));
1425
1426 vr += prod(trans(coeffImage), coeffRHS);
1427
1428 // form N22 matrix
1429 N22 += prod(trans(coeffPoint3D), coeffPoint3D);
1430
1431 // form n2 vector
1432 n2 += prod(trans(coeffPoint3D), coeffRHS);
1433
1434 return true;
1435 }
1436
1437
1455 int BundleAdjust::formPointNormals(symmetric_matrix<double, upper>&N22,
1457 vector<double> &n2,
1458 vector<double> &nj,
1459 BundleControlPointQsp &bundleControlPoint) {
1460
1461 boost::numeric::ublas::bounded_vector<double, 3> &NIC = bundleControlPoint->nicVector();
1462 SparseBlockRowMatrix &Q = bundleControlPoint->cholmodQMatrix();
1463
1464 NIC.clear();
1465 Q.zeroBlocks();
1466
1467 int numConstrainedCoordinates = 0;
1468
1469 // weighting of 3D point parameters
1470 // Make sure weights are in the units corresponding to the bundle coordinate type
1471 boost::numeric::ublas::bounded_vector<double, 3> &weights
1472 = bundleControlPoint->weights();
1473 boost::numeric::ublas::bounded_vector<double, 3> &corrections
1474 = bundleControlPoint->corrections();
1475
1476 if (weights(0) > 0.0) {
1477 N22(0,0) += weights(0);
1478 n2(0) += (-weights(0) * corrections(0));
1479 numConstrainedCoordinates++;
1480 }
1481
1482 if (weights(1) > 0.0) {
1483 N22(1,1) += weights(1);
1484 n2(1) += (-weights(1) * corrections(1));
1485 numConstrainedCoordinates++;
1486 }
1487
1488 if (weights(2) > 0.0) {
1489 N22(2,2) += weights(2);
1490 n2(2) += (-weights(2) * corrections(2));
1491 numConstrainedCoordinates++;
1492 }
1493
1494 // invert N22
1495 invert3x3(N22);
1496
1497 // save upper triangular covariance matrix for error propagation
1498 SurfacePoint SurfacePoint = bundleControlPoint->adjustedSurfacePoint();
1499 SurfacePoint.SetMatrix(m_bundleSettings->controlPointCoordTypeBundle(), N22);
1500 bundleControlPoint->setAdjustedSurfacePoint(SurfacePoint);
1501
1502 // form Q (this is N22{-1} * N12{T})
1503 productATransB(N22, N12, Q);
1504
1505 // form product of N22(inverse) and n2; store in NIC
1506 NIC = prod(N22, n2);
1507
1508 // accumulate -R directly into reduced normal equations
1509 productAB(N12, Q);
1510
1511 // accumulate -nj
1512 accumProductAlphaAB(-1.0, Q, n2, nj);
1513
1514 return numConstrainedCoordinates;
1515 }
1516
1517
1535 int BundleAdjust::formLidarPointNormals(symmetric_matrix<double, upper>&N22,
1537 vector<double> &n2,
1538 vector<double> &nj,
1539 BundleLidarControlPointQsp &bundleLidarControlPoint) {
1540
1541 boost::numeric::ublas::bounded_vector<double, 3> &NIC = bundleLidarControlPoint->nicVector();
1542 SparseBlockRowMatrix &Q = bundleLidarControlPoint->cholmodQMatrix();
1543
1544 NIC.clear();
1545 Q.zeroBlocks();
1546
1547 int numConstrainedCoordinates = 0;
1548
1549 // weighting of 3D point parameters
1550 // Make sure weights are in the units corresponding to the bundle coordinate type
1551 boost::numeric::ublas::bounded_vector<double, 3> &weights
1552 = bundleLidarControlPoint->weights();
1553 boost::numeric::ublas::bounded_vector<double, 3> &corrections
1554 = bundleLidarControlPoint->corrections();
1555
1556 if (weights(0) > 0.0) {
1557 N22(0,0) += weights(0);
1558 n2(0) += (-weights(0) * corrections(0));
1559 numConstrainedCoordinates++;
1560 }
1561
1562 if (weights(1) > 0.0) {
1563 N22(1,1) += weights(1);
1564 n2(1) += (-weights(1) * corrections(1));
1565 numConstrainedCoordinates++;
1566 }
1567
1568 if (weights(2) > 0.0) {
1569 N22(2,2) += weights(2);
1570 n2(2) += (-weights(2) * corrections(2));
1571 numConstrainedCoordinates++;
1572 }
1573
1574 // invert N22
1575 invert3x3(N22);
1576
1577 // save upper triangular covariance matrix for error propagation
1578 SurfacePoint SurfacePoint = bundleLidarControlPoint->adjustedSurfacePoint();
1579 SurfacePoint.SetMatrix(m_bundleSettings->controlPointCoordTypeBundle(), N22);
1580 bundleLidarControlPoint->setAdjustedSurfacePoint(SurfacePoint);
1581
1582 // form Q (this is N22{-1} * N12{T})
1583 productATransB(N22, N12, Q);
1584
1585 // form product of N22(inverse) and n2; store in NIC
1586 NIC = prod(N22, n2);
1587
1588 // accumulate -R directly into reduced normal equations
1589 productAB(N12, Q);
1590
1591 // accumulate -nj
1592 accumProductAlphaAB(-1.0, Q, n2, nj);
1593
1594 return numConstrainedCoordinates;
1595 }
1596
1597
1616 bool BundleAdjust::formWeightedNormals(compressed_vector<double> &n1,
1617 vector<double> &nj) {
1618
1619 m_bundleResults.resetNumberConstrainedImageParameters();
1620
1621 int n = 0;
1622
1623 for (int i = 0; i < m_sparseNormals.size(); i++) {
1624 LinearAlgebra::Matrix *diagonalBlock = m_sparseNormals.getBlock(i, i);
1625 if ( !diagonalBlock )
1626 continue;
1627
1628 if (m_bundleSettings->solveTargetBody() && i == 0) {
1629 m_bundleResults.resetNumberConstrainedTargetParameters();
1630
1631 // get parameter weights for target body
1632 vector<double> weights = m_bundleTargetBody->parameterWeights();
1633 vector<double> corrections = m_bundleTargetBody->parameterCorrections();
1634
1635 int blockSize = diagonalBlock->size1();
1636 for (int j = 0; j < blockSize; j++) {
1637 if (weights[j] > 0.0) {
1638 (*diagonalBlock)(j,j) += weights[j];
1639 nj[n] -= weights[j] * corrections(j);
1640 m_bundleResults.incrementNumberConstrainedTargetParameters(1);
1641 }
1642 n++;
1643 }
1644 }
1645 else {
1646 BundleObservationQsp observation;
1647 if (m_bundleSettings->solveTargetBody()) {
1648 // If we are solving for taget body the observation index is off by 1
1649 observation = m_bundleObservations.at(i-1);
1650 }
1651 else {
1652 observation = m_bundleObservations.at(i);
1653 }
1654
1655 // get parameter weights for this observation
1656 LinearAlgebra::Vector weights = observation->parameterWeights();
1657 LinearAlgebra::Vector corrections = observation->parameterCorrections();
1658
1659 int blockSize = diagonalBlock->size1();
1660 for (int j = 0; j < blockSize; j++) {
1661 if (weights(j) > 0.0) {
1662 (*diagonalBlock)(j,j) += weights(j);
1663 nj[n] -= weights(j) * corrections(j);
1664 m_bundleResults.incrementNumberConstrainedImageParameters(1);
1665 }
1666 n++;
1667 }
1668 }
1669 }
1670
1671 // add n1 to nj
1672 nj += n1;
1673
1674 return true;
1675 }
1676
1677
1687 bool BundleAdjust::productATransB(symmetric_matrix <double,upper> &N22,
1690
1691 QMapIterator< int, LinearAlgebra::Matrix * > N12it(N12);
1692
1693 while ( N12it.hasNext() ) {
1694 N12it.next();
1695
1696 int rowIndex = N12it.key();
1697
1698 // insert submatrix in Q at block "rowIndex"
1699 Q.insertMatrixBlock(rowIndex, 3, N12it.value()->size1());
1700
1701 *(Q[rowIndex]) = prod(N22,trans(*(N12it.value())));
1702 }
1703
1704 return true;
1705 }
1706
1707
1719 // iterators for N12 and Q
1720 QMapIterator<int, LinearAlgebra::Matrix*> N12it(N12);
1721 QMapIterator<int, LinearAlgebra::Matrix*> Qit(Q);
1722
1723 // now multiply blocks and subtract from m_sparseNormals
1724 while ( N12it.hasNext() ) {
1725 N12it.next();
1726
1727 int rowIndex = N12it.key();
1728 LinearAlgebra::Matrix *N12block = N12it.value();
1729
1730 while ( Qit.hasNext() ) {
1731 Qit.next();
1732
1733 int columnIndex = Qit.key();
1734
1735 if ( rowIndex > columnIndex ) {
1736 continue;
1737 }
1738
1739 LinearAlgebra::Matrix *Qblock = Qit.value();
1740
1741 // insert submatrix at column, row
1742 m_sparseNormals.insertMatrixBlock(columnIndex, rowIndex,
1743 N12block->size1(), Qblock->size2());
1744
1745 (*(*m_sparseNormals[columnIndex])[rowIndex]) -= prod(*N12block,*Qblock);
1746 }
1747 Qit.toFront();
1748 }
1749 }
1750
1751
1764 vector<double> &n2,
1765 vector<double> &nj) {
1766
1767 if (alpha == 0.0) {
1768 return;
1769 }
1770
1771 int numParams;
1772
1773 QMapIterator<int, LinearAlgebra::Matrix*> Qit(Q);
1774
1775 while ( Qit.hasNext() ) {
1776 Qit.next();
1777
1778 int columnIndex = Qit.key();
1779 LinearAlgebra::Matrix *Qblock = Qit.value();
1780
1781 LinearAlgebra::Vector blockProduct = prod(trans(*Qblock),n2);
1782
1783 numParams = m_sparseNormals.at(columnIndex)->startColumn();
1784
1785 for (unsigned i = 0; i < blockProduct.size(); i++) {
1786 nj(numParams+i) += alpha*blockProduct(i);
1787 }
1788 }
1789 }
1790
1791
1802
1803 // load cholmod triplet
1804 if ( !loadCholmodTriplet() ) {
1805 QString msg = "CHOLMOD: Failed to load Triplet matrix";
1806 throw IException(IException::Programmer, msg, _FILEINFO_);
1807 }
1808
1809 // convert triplet to sparse matrix
1810 m_cholmodNormal = cholmod_l_triplet_to_sparse(m_cholmodTriplet,
1811 m_cholmodTriplet->nnz,
1812 &m_cholmodCommon);
1813
1814 // analyze matrix
1815 // TODO should we analyze just 1st iteration?
1816 m_L = cholmod_l_analyze(m_cholmodNormal, &m_cholmodCommon);
1817
1818 // create cholmod cholesky factor
1819 // CHOLMOD will choose LLT or LDLT decomposition based on the characteristics of the matrix.
1820 cholmod_l_factorize(m_cholmodNormal, m_L, &m_cholmodCommon);
1821
1822 // check for "matrix not positive definite" error
1823 if (m_cholmodCommon.status == CHOLMOD_NOT_POSDEF) {
1824 QString msg = "Matrix NOT positive-definite: failure at column " + toString((int) m_L->minor);
1825// throw IException(IException::User, msg, _FILEINFO_);
1826 error(msg);
1827 emit(finished());
1828 return false;
1829 }
1830
1831 // cholmod solution and right-hand side vectors
1832 cholmod_dense *x, *b;
1833
1834 // initialize right-hand side vector
1835 b = cholmod_l_zeros(m_cholmodNormal->nrow, 1, m_cholmodNormal->xtype, &m_cholmodCommon);
1836
1837 // copy right-hand side vector into b
1838 double *px = (double*)b->x;
1839 for (int i = 0; i < m_rank; i++) {
1840 px[i] = m_RHS[i];
1841 }
1842
1843 // cholmod solve
1844 x = cholmod_l_solve(CHOLMOD_A, m_L, b, &m_cholmodCommon);
1845
1846 // copy solution vector x out into m_imageSolution
1847 double *sx = (double*)x->x;
1848 for (int i = 0; i < m_rank; i++) {
1849 m_imageSolution[i] = sx[i];
1850 }
1851
1852 // free cholmod structures
1853 cholmod_l_free_sparse(&m_cholmodNormal, &m_cholmodCommon);
1854 cholmod_l_free_dense(&b, &m_cholmodCommon);
1855 cholmod_l_free_dense(&x, &m_cholmodCommon);
1856
1857 return true;
1858 }
1859
1860
1873
1874 if ( m_iteration == 1 ) {
1875 int numElements = m_sparseNormals.numberOfElements();
1876 m_cholmodTriplet = cholmod_l_allocate_triplet(m_rank, m_rank, numElements,
1877 -1, CHOLMOD_REAL, &m_cholmodCommon);
1878
1879 if ( !m_cholmodTriplet ) {
1880 outputBundleStatus("\nTriplet allocation failure\n");
1881 return false;
1882 }
1883 m_cholmodTriplet->nnz = 0;
1884 }
1885
1886 long *tripletColumns = (long*) m_cholmodTriplet->i;
1887 long *tripletRows = (long*) m_cholmodTriplet->j;
1888 double *tripletValues = (double*)m_cholmodTriplet->x;
1889
1890 double entryValue;
1891
1892 long numEntries = 0;
1893
1894 long numBlockcolumns = m_sparseNormals.size();
1895 for (int columnIndex = 0; columnIndex < numBlockcolumns; columnIndex++) {
1896
1897 SparseBlockColumnMatrix *normalsColumn = m_sparseNormals[columnIndex];
1898
1899 if ( !normalsColumn ) {
1900 QString status = "\nSparseBlockColumnMatrix retrieval failure at column " +
1901 QString::number(columnIndex);
1902 outputBundleStatus(status);
1903 return false;
1904 }
1905
1906 int numLeadingColumns = normalsColumn->startColumn();
1907
1908 QMapIterator< int, LinearAlgebra::Matrix * > it(*normalsColumn);
1909
1910 while ( it.hasNext() ) {
1911 it.next();
1912
1913 int rowIndex = it.key();
1914
1915 // note: as the normal equations matrix is symmetric, the # of leading rows for a block is
1916 // equal to the # of leading columns for a block column at the "rowIndex" position
1917 int numLeadingRows = m_sparseNormals.at(rowIndex)->startColumn();
1918
1919 LinearAlgebra::Matrix *normalsBlock = it.value();
1920 if ( !normalsBlock ) {
1921 QString status = "\nmatrix block retrieval failure at column ";
1922 status.append(columnIndex);
1923 status.append(", row ");
1924 status.append(rowIndex);
1925 outputBundleStatus(status);
1926 status = "Total # of block columns: " + QString::number(numBlockcolumns);
1927 outputBundleStatus(status);
1928 status = "Total # of blocks: " + QString::number(m_sparseNormals.numberOfBlocks());
1929 outputBundleStatus(status);
1930 return false;
1931 }
1932
1933 if ( columnIndex == rowIndex ) { // diagonal block (upper-triangular)
1934 for (unsigned ii = 0; ii < normalsBlock->size1(); ii++) {
1935 for (unsigned jj = ii; jj < normalsBlock->size2(); jj++) {
1936 entryValue = normalsBlock->at_element(ii,jj);
1937 int entryColumnIndex = jj + numLeadingColumns;
1938 int entryRowIndex = ii + numLeadingRows;
1939
1940 if ( m_iteration == 1 ) {
1941 tripletColumns[numEntries] = entryColumnIndex;
1942 tripletRows[numEntries] = entryRowIndex;
1943 m_cholmodTriplet->nnz++;
1944 }
1945
1946 tripletValues[numEntries] = entryValue;
1947
1948 numEntries++;
1949 }
1950 }
1951 }
1952 else { // off-diagonal block (square)
1953 for (unsigned ii = 0; ii < normalsBlock->size1(); ii++) {
1954 for (unsigned jj = 0; jj < normalsBlock->size2(); jj++) {
1955 entryValue = normalsBlock->at_element(ii,jj);
1956 int entryColumnIndex = jj + numLeadingColumns;
1957 int entryRowIndex = ii + numLeadingRows;
1958
1959 if ( m_iteration ==1 ) {
1960 tripletColumns[numEntries] = entryRowIndex;
1961 tripletRows[numEntries] = entryColumnIndex;
1962 m_cholmodTriplet->nnz++;
1963 }
1964
1965 tripletValues[numEntries] = entryValue;
1966
1967 numEntries++;
1968 }
1969 }
1970 }
1971 }
1972 }
1973
1974 return true;
1975 }
1976
1977
1991 double det;
1992 double den;
1993
1994 boost::numeric::ublas::symmetric_matrix< double, upper > c = m;
1995
1996 den = m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1))
1997 - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0))
1998 + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0));
1999
2000 // check for divide by zero
2001 if (fabs(den) < 1.0e-100) {
2002 return false;
2003 }
2004
2005 det = 1.0 / den;
2006
2007 m(0, 0) = (c(1, 1) * c(2, 2) - c(1, 2) * c(2, 1)) * det;
2008 m(0, 1) = (c(0, 2) * c(2, 1) - c(0, 1) * c(2, 2)) * det;
2009 m(0, 2) = (c(0, 1) * c(1, 2) - c(0, 2) * c(1, 1)) * det;
2010 m(1, 1) = (c(0, 0) * c(2, 2) - c(0, 2) * c(2, 0)) * det;
2011 m(1, 2) = (c(0, 2) * c(1, 0) - c(0, 0) * c(1, 2)) * det;
2012 m(2, 2) = (c(0, 0) * c(1, 1) - c(0, 1) * c(1, 0)) * det;
2013
2014 return true;
2015 }
2016
2017
2037 bool BundleAdjust::computePartials(matrix<double> &coeffTarget,
2038 matrix<double> &coeffImage,
2039 matrix<double> &coeffPoint3D,
2040 vector<double> &coeffRHS,
2041 BundleMeasure &measure,
2042 BundleControlPoint &point) {
2043
2044 Camera *measureCamera = measure.camera();
2045 BundleObservationQsp observation = measure.parentBundleObservation();
2046
2047 int numImagePartials = observation->numberParameters();
2048
2049 // we're saving the number of image partials in m_previousNumberImagePartials
2050 // to compare to the previous computePartials call to avoid unnecessary resizing of the
2051 // coeffImage matrix
2052 if (numImagePartials != m_previousNumberImagePartials) {
2053 coeffImage.resize(2,numImagePartials);
2054 m_previousNumberImagePartials = numImagePartials;
2055 }
2056
2057 // No need to call SetImage for framing camera
2058 if (measureCamera->GetCameraType() != Camera::Framing) {
2059 // Set the Spice to the measured point. A framing camera exposes the entire image at one time.
2060 // It will have a single set of Spice for the entire image. Scanning cameras may populate a single
2061 // image with multiple exposures, each with a unique set of Spice. SetImage needs to be called
2062 // repeatedly for these images to point to the Spice for the current pixel.
2063 measureCamera->SetImage(measure.sample(), measure.line());
2064 }
2065
2066 // CSM Cameras do not have a ground map
2067 if (measureCamera->GetCameraType() != Camera::Csm) {
2068 // Compute the look vector in instrument coordinates based on time of observation and apriori
2069 // lat/lon/radius. As of 05/15/2019, this call no longer does the back-of-planet test. An optional
2070 // bool argument was added CameraGroundMap::GetXY to turn off the test.
2071 double computedX, computedY;
2072 if (!(measureCamera->GroundMap()->GetXY(point.adjustedSurfacePoint(),
2073 &computedX, &computedY, false))) {
2074 QString msg = "Unable to map apriori surface point for measure ";
2075 msg += measure.cubeSerialNumber() + " on point " + point.id() + " into focal plane";
2076 throw IException(IException::User, msg, _FILEINFO_);
2077 }
2078 }
2079
2080 if (m_bundleSettings->solveTargetBody()) {
2081 observation->computeTargetPartials(coeffTarget, measure, m_bundleSettings, m_bundleTargetBody);
2082 }
2083
2084 observation->computeImagePartials(coeffImage, measure);
2085
2086 // Complete partials calculations for 3D point (latitudinal or rectangular)
2087 // Retrieve the coordinate type (latitudinal or rectangular) and compute the partials for
2088 // the fixed point with respect to each coordinate in Body-Fixed
2089 SurfacePoint::CoordinateType coordType = m_bundleSettings->controlPointCoordTypeBundle();
2090 observation->computePoint3DPartials(coeffPoint3D, measure, coordType);
2091
2092 // right-hand side (measured - computed)
2093 observation->computeRHSPartials(coeffRHS, measure);
2094
2095 double deltaX = coeffRHS(0);
2096 double deltaY = coeffRHS(1);
2097
2098 m_bundleResults.addResidualsProbabilityDistributionObservation(observation->computeObservationValue(measure, deltaX));
2099 m_bundleResults.addResidualsProbabilityDistributionObservation(observation->computeObservationValue(measure, deltaY));
2100
2101 if (m_bundleResults.numberMaximumLikelihoodModels()
2102 > m_bundleResults.maximumLikelihoodModelIndex()) {
2103 // If maximum likelihood estimation is being used
2104 double residualR2ZScore = sqrt(deltaX * deltaX + deltaY * deltaY) / sqrt(2.0);
2105
2106 // Dynamically build the cumulative probability distribution of the R^2 residual Z Scores
2107 m_bundleResults.addProbabilityDistributionObservation(residualR2ZScore);
2108
2109 int currentModelIndex = m_bundleResults.maximumLikelihoodModelIndex();
2110 double observationWeight = m_bundleResults.maximumLikelihoodModelWFunc(currentModelIndex)
2111 .sqrtWeightScaler(residualR2ZScore);
2112 coeffImage *= observationWeight;
2113 coeffPoint3D *= observationWeight;
2114 coeffRHS *= observationWeight;
2115
2116 if (m_bundleSettings->solveTargetBody()) {
2117 coeffTarget *= observationWeight;
2118 }
2119 }
2120
2121 return true;
2122 }
2123
2124
2129 emit(statusBarUpdate("Updating Parameters"));
2130 int t = 0;
2131
2132 // TODO - update target body parameters if in solution
2133 // note these come before BundleObservation parameters in normal equations matrix
2134 if (m_bundleSettings->solveTargetBody()) {
2135 int numTargetBodyParameters = m_bundleTargetBody->numberParameters();
2136
2137 m_bundleTargetBody->applyParameterCorrections(subrange(m_imageSolution,0,
2138 numTargetBodyParameters));
2139
2140 t += numTargetBodyParameters;
2141 }
2142
2143 // Update spice for each BundleObservation
2144 int numObservations = m_bundleObservations.size();
2145 for (int i = 0; i < numObservations; i++) {
2146 BundleObservationQsp observation = m_bundleObservations.at(i);
2147
2148 int numParameters = observation->numberParameters();
2149
2150 observation->applyParameterCorrections(subrange(m_imageSolution,t,t+numParameters));
2151
2152 if (m_bundleSettings->solveTargetBody()) {
2153 // TODO: needs to be updated for ISIS vs. CSM CSM has no updateBodyRotation]
2154 // TODO: this is no good.
2155 QSharedPointer<IsisBundleObservation> isisObservation = qSharedPointerDynamicCast<IsisBundleObservation>(observation);
2156 isisObservation->updateBodyRotation();
2157 }
2158
2159 t += numParameters;
2160 }
2161
2162 // Apply corrections for photogrammetric control points
2163 for (int i = 0; i < m_bundleControlPoints.size(); i++) {
2165
2166 if (point->isRejected()) {
2167 continue;
2168 }
2169
2170 point->applyParameterCorrections(m_imageSolution, m_sparseNormals,
2171 m_bundleTargetBody);
2172
2173 }
2174
2175 // Apply corrections for lidar points
2176 for (int i = 0; i < m_bundleLidarControlPoints.size(); i++) {
2178
2179 if (point->isRejected()) {
2180 continue;
2181 }
2182
2183 point->applyParameterCorrections(m_imageSolution, m_sparseNormals,
2184 m_bundleTargetBody);
2185
2186 }
2187 }
2188
2189
2197 emit(statusBarUpdate("Computing Residuals"));
2198 double vtpv = 0.0;
2199
2200 // x, y, and xy residual stats vectors
2201 Statistics xResiduals;
2202 Statistics yResiduals;
2203 Statistics xyResiduals;
2204
2205 // vtpv for photo measures
2206 for (int i = 0; i < m_bundleControlPoints.size(); i++) {
2207 vtpv += m_bundleControlPoints.at(i)->vtpvMeasures();
2208 vtpv += m_bundleControlPoints.at(i)->vtpv();
2209 }
2210
2211 // vtpv for lidar measures
2212 for (int i = 0; i < m_bundleLidarControlPoints.size(); i++) {
2213 vtpv += m_bundleLidarControlPoints.at(i)->vtpvMeasures();
2214 vtpv += m_bundleLidarControlPoints.at(i)->vtpv();
2215 vtpv += m_bundleLidarControlPoints.at(i)->vtpvRangeContribution();
2216 }
2217
2218 // vtpv for image parameters
2219 for (int i = 0; i < m_bundleObservations.size(); i++) {
2220 vtpv += m_bundleObservations.at(i)->vtpv();
2221 }
2222
2223 // vtpv for target body parameters
2224 if ( m_bundleTargetBody) {
2225 vtpv += m_bundleTargetBody->vtpv();
2226 }
2227
2228
2229 // Compute rms for all image coordinate residuals
2230 // separately for x, y, then x and y together
2231 m_bundleResults.setRmsXYResiduals(xResiduals.Rms(), yResiduals.Rms(), xyResiduals.Rms());
2232
2233 return vtpv;
2234 }
2235
2236
2252 double vx, vy;
2253
2254 int numResiduals = m_bundleResults.numberObservations() / 2;
2255
2256 std::vector<double> residuals;
2257
2258 residuals.resize(numResiduals);
2259
2260 // load magnitude (squared) of residual vector
2261 int residualIndex = 0;
2262 int numObjectPoints = m_bundleControlPoints.size();
2263 for (int i = 0; i < numObjectPoints; i++) {
2264
2265 const BundleControlPointQsp point = m_bundleControlPoints.at(i);
2266
2267 if ( point->isRejected() ) {
2268 continue;
2269 }
2270
2271 int numMeasures = point->numberOfMeasures();
2272 for (int j = 0; j < numMeasures; j++) {
2273
2274 const BundleMeasureQsp measure = point->at(j);
2275
2276 if ( measure->isRejected() ) {
2277 continue;
2278 }
2279
2280 vx = measure->sampleResidual();
2281 vy = measure->lineResidual();
2282
2283 residuals[residualIndex] = sqrt(vx*vx + vy*vy);
2284
2285 residualIndex++;
2286 }
2287 }
2288
2289 // sort vectors
2290 std::sort(residuals.begin(), residuals.end());
2291
2292 double median;
2293 double medianDev;
2294 double mad;
2295
2296 int midpointIndex = numResiduals / 2;
2297
2298 if ( numResiduals % 2 == 0 ) {
2299 median = (residuals[midpointIndex - 1] + residuals[midpointIndex]) / 2;
2300 }
2301 else {
2302 median = residuals[midpointIndex];
2303 }
2304
2305 // compute M.A.D.
2306 for (int i = 0; i < numResiduals; i++) {
2307 residuals[i] = fabs(residuals[i] - median);
2308 }
2309
2310 std::sort(residuals.begin(), residuals.end());
2311
2312 if ( numResiduals % 2 == 0 ) {
2313 medianDev = (residuals[midpointIndex - 1] + residuals[midpointIndex]) / 2;
2314 }
2315 else {
2316 medianDev = residuals[midpointIndex];
2317 }
2318
2319 QString status = "\nmedian deviation: ";
2320 status.append(QString("%1").arg(medianDev));
2321 status.append("\n");
2322 outputBundleStatus(status);
2323
2324 mad = 1.4826 * medianDev;
2325
2326 status = "\nmad: ";
2327 status.append(QString("%1").arg(mad));
2328 status.append("\n");
2329 outputBundleStatus(status);
2330
2331 m_bundleResults.setRejectionLimit(median
2332 + m_bundleSettings->outlierRejectionMultiplier() * mad);
2333
2334 status = "\nRejection Limit: ";
2335 status.append(QString("%1").arg(m_bundleResults.rejectionLimit()));
2336 status.append("\n");
2337 outputBundleStatus(status);
2338
2339 return true;
2340 }
2341
2342
2351 double vx, vy;
2352 int numRejected;
2353 int totalNumRejected = 0;
2354
2355 int maxResidualIndex;
2356 double maxResidual;
2357 double sumSquares;
2358 double usedRejectionLimit = m_bundleResults.rejectionLimit();
2359
2360 // TODO What to do if usedRejectionLimit is too low?
2361
2362 int numComingBack = 0;
2363
2364 int numObjectPoints = m_bundleControlPoints.size();
2365
2366 outputBundleStatus("\n");
2367 for (int i = 0; i < numObjectPoints; i++) {
2369
2370 point->zeroNumberOfRejectedMeasures();
2371
2372 numRejected = 0;
2373 maxResidualIndex = -1;
2374 maxResidual = -1.0;
2375
2376 int numMeasures = point->numberOfMeasures();
2377 for (int j = 0; j < numMeasures; j++) {
2378
2379 BundleMeasureQsp measure = point->at(j);
2380
2381 vx = measure->sampleResidual();
2382 vy = measure->lineResidual();
2383
2384 sumSquares = sqrt(vx*vx + vy*vy);
2385
2386 // measure is good
2387 if ( sumSquares <= usedRejectionLimit ) {
2388
2389 // was it previously rejected?
2390 if ( measure->isRejected() ) {
2391 QString status = "Coming back in: ";
2392 status.append(QString("%1").arg(point->id().toLatin1().data()));
2393 status.append("\r");
2394 outputBundleStatus(status);
2395 numComingBack++;
2396 m_controlNet->DecrementNumberOfRejectedMeasuresInImage(measure->cubeSerialNumber());
2397 }
2398
2399 measure->setRejected(false);
2400 continue;
2401 }
2402
2403 // if it's still rejected, skip it
2404 if ( measure->isRejected() ) {
2405 numRejected++;
2406 totalNumRejected++;
2407 continue;
2408 }
2409
2410 if ( sumSquares > maxResidual ) {
2411 maxResidual = sumSquares;
2412 maxResidualIndex = j;
2413 }
2414 }
2415
2416 // no observations above the current rejection limit for this 3D point
2417 if ( maxResidual == -1.0 || maxResidual <= usedRejectionLimit ) {
2418 point->setNumberOfRejectedMeasures(numRejected);
2419 continue;
2420 }
2421
2422 // this is another kluge - if we only have two observations
2423 // we won't reject (for now)
2424 if ((numMeasures - (numRejected + 1)) < 2) {
2425 point->setNumberOfRejectedMeasures(numRejected);
2426 continue;
2427 }
2428
2429 // otherwise, we have at least one observation
2430 // for this point whose residual is above the
2431 // current rejection limit - we'll flag the
2432 // worst of these as rejected
2433 BundleMeasureQsp rejected = point->at(maxResidualIndex);
2434 rejected->setRejected(true);
2435 numRejected++;
2436 point->setNumberOfRejectedMeasures(numRejected);
2437 m_controlNet->IncrementNumberOfRejectedMeasuresInImage(rejected->cubeSerialNumber());
2438 totalNumRejected++;
2439
2440 // do we still have sufficient remaining observations for this 3D point?
2441 if ( ( numMeasures-numRejected ) < 2 ) {
2442 point->setRejected(true);
2443 QString status = "Rejecting Entire Point: ";
2444 status.append(QString("%1").arg(point->id().toLatin1().data()));
2445 status.append("\r");
2446 outputBundleStatus(status);
2447 }
2448 else
2449 point->setRejected(false);
2450
2451 }
2452
2453 int numberRejectedObservations = 2*totalNumRejected;
2454
2455 QString status = "\nRejected Observations:";
2456 status.append(QString("%1").arg(numberRejectedObservations));
2457 status.append(" (Rejection Limit:");
2458 status.append(QString("%1").arg(usedRejectionLimit));
2459 status.append(")\n");
2460 outputBundleStatus(status);
2461
2462 m_bundleResults.setNumberRejectedObservations(numberRejectedObservations);
2463
2464 status = "\nMeasures that came back: ";
2465 status.append(QString("%1").arg(numComingBack));
2466 status.append("\n");
2467 outputBundleStatus(status);
2468
2469 return true;
2470 }
2471
2472
2481
2482 if (m_imageLists.count() > 0) {
2483 return m_imageLists;
2484 }
2485 else if (m_serialNumberList->size() > 0) {
2486 ImageList *imgList = new ImageList;
2487 try {
2488 for (int i = 0; i < m_serialNumberList->size(); i++) {
2489 Image *image = new Image(m_serialNumberList->fileName(i));
2490 imgList->append(image);
2491 image->closeCube();
2492 }
2493 m_imageLists.append(imgList);
2494 }
2495 catch (IException &e) {
2496 QString msg = "Invalid image in serial number list\n";
2497 throw IException(IException::Programmer, msg, _FILEINFO_);
2498 }
2499 }
2500 else {
2501 QString msg = "No images used in bundle adjust\n";
2502 throw IException(IException::Programmer, msg, _FILEINFO_);
2503 }
2504
2505 return m_imageLists;
2506 }
2507
2508
2530 emit(statusBarUpdate("Error Propagation"));
2531 // free unneeded memory
2532 cholmod_l_free_triplet(&m_cholmodTriplet, &m_cholmodCommon);
2533 cholmod_l_free_sparse(&m_cholmodNormal, &m_cholmodCommon);
2534
2535 LinearAlgebra::Matrix T(3, 3);
2536 // *** TODO ***
2537 // Can any of the control point specific code be moved to BundleControlPoint?
2538
2539 double sigma0Squared = m_bundleResults.sigma0() * m_bundleResults.sigma0();
2540
2541 int numObjectPoints = m_bundleControlPoints.size();
2542
2543 std::string currentTime = iTime::CurrentLocalTime().toLatin1().data();
2544
2545 QString status = " Time: ";
2546 status.append(currentTime.c_str());
2547 status.append("\n\n");
2548 outputBundleStatus(status);
2549
2550 // create and initialize array of 3x3 matrices for all object points
2551 std::vector< symmetric_matrix<double> > pointCovariances(numObjectPoints,
2552 symmetric_matrix<double>(3));
2553 for (int d = 0; d < numObjectPoints; d++) {
2554 pointCovariances[d].clear();
2555 }
2556
2557 cholmod_dense *x; // solution vector
2558 cholmod_dense *b; // right-hand side (column vectors of identity)
2559
2560 b = cholmod_l_zeros ( m_rank, 1, CHOLMOD_REAL, &m_cholmodCommon );
2561 double *pb = (double*)b->x;
2562
2563 double *px = NULL;
2564
2565 SparseBlockColumnMatrix inverseMatrix;
2566
2567 // Create unique file name
2568 FileName matrixFile(m_bundleSettings->outputFilePrefix() + "inverseMatrix.dat");
2569 //???FileName matrixFile = FileName::createTempFile(m_bundleSettings.outputFilePrefix()
2570 //??? + "inverseMatrix.dat");
2571 // Create file handle
2572 QFile matrixOutput(matrixFile.expanded());
2573
2574 // Check to see if creating the inverse correlation matrix is turned on
2575 if (m_bundleSettings->createInverseMatrix()) {
2576 // Open file to write to
2577 matrixOutput.open(QIODevice::WriteOnly);
2578 }
2579 QDataStream outStream(&matrixOutput);
2580
2581 int i, j, k;
2582 int columnIndex = 0;
2583 int numColumns = 0;
2584 int numBlockColumns = m_sparseNormals.size();
2585 for (i = 0; i < numBlockColumns; i++) {
2586
2587 // columns in this column block
2588 SparseBlockColumnMatrix *normalsColumn = m_sparseNormals.at(i);
2589 if (i == 0) {
2590 numColumns = normalsColumn->numberOfColumns();
2591 int numRows = normalsColumn->numberOfRows();
2592 inverseMatrix.insertMatrixBlock(i, numRows, numColumns);
2593 inverseMatrix.zeroBlocks();
2594 }
2595 else {
2596 if (normalsColumn->numberOfColumns() == numColumns) {
2597 int numRows = normalsColumn->numberOfRows();
2598 inverseMatrix.insertMatrixBlock(i, numRows, numColumns);
2599 inverseMatrix.zeroBlocks();
2600 }
2601 else {
2602 numColumns = normalsColumn->numberOfColumns();
2603
2604 // reset inverseMatrix
2605 inverseMatrix.wipe();
2606
2607 // insert blocks
2608 for (j = 0; j < (i+1); j++) {
2609 SparseBlockColumnMatrix *normalsRow = m_sparseNormals.at(j);
2610 int numRows = normalsRow->numberOfRows();
2611
2612 inverseMatrix.insertMatrixBlock(j, numRows, numColumns);
2613 }
2614 }
2615 }
2616
2617 int localCol = 0;
2618
2619 // solve for inverse for nCols
2620 for (j = 0; j < numColumns; j++) {
2621 if ( columnIndex > 0 ) {
2622 pb[columnIndex - 1] = 0.0;
2623 }
2624 pb[columnIndex] = 1.0;
2625
2626 x = cholmod_l_solve ( CHOLMOD_A, m_L, b, &m_cholmodCommon );
2627 px = (double*)x->x;
2628 int rp = 0;
2629
2630 // store solution in corresponding column of inverse
2631 for (k = 0; k < inverseMatrix.size(); k++) {
2632 LinearAlgebra::Matrix *matrix = inverseMatrix.value(k);
2633
2634 int sz1 = matrix->size1();
2635
2636 for (int ii = 0; ii < sz1; ii++) {
2637 (*matrix)(ii,localCol) = px[ii + rp];
2638 }
2639 rp += matrix->size1();
2640 }
2641
2642 columnIndex++;
2643 localCol++;
2644
2645 cholmod_l_free_dense(&x,&m_cholmodCommon);
2646 }
2647
2648 // save adjusted target body sigmas if solving for target
2649 if (m_bundleSettings->solveTargetBody() && i == 0) {
2650 vector< double > &adjustedSigmas = m_bundleTargetBody->adjustedSigmas();
2651 matrix< double > *targetCovMatrix = inverseMatrix.value(i);
2652
2653 for (int z = 0; z < numColumns; z++)
2654 adjustedSigmas[z] = sqrt((*targetCovMatrix)(z,z))*m_bundleResults.sigma0();
2655 }
2656 // save adjusted image sigmas
2657 else {
2658 BundleObservationQsp observation;
2659 if (m_bundleSettings->solveTargetBody()) {
2660 observation = m_bundleObservations.at(i-1);
2661 }
2662 else {
2663 observation = m_bundleObservations.at(i);
2664 }
2665 vector< double > &adjustedSigmas = observation->adjustedSigmas();
2666 matrix< double > *imageCovMatrix = inverseMatrix.value(i);
2667 for ( int z = 0; z < numColumns; z++) {
2668 adjustedSigmas[z] = sqrt((*imageCovMatrix)(z,z))*m_bundleResults.sigma0();
2669 }
2670 }
2671
2672 // Output the inverse matrix if requested
2673 if (m_bundleSettings->createInverseMatrix()) {
2674 outStream << inverseMatrix;
2675 }
2676
2677 // now loop over all object points to sum contributions into 3x3 point covariance matrix
2678 int pointIndex = 0;
2679 for (j = 0; j < numObjectPoints; j++) {
2680 emit(pointUpdate(j+1));
2681 BundleControlPointQsp point = m_bundleControlPoints.at(pointIndex);
2682 if ( point->isRejected() ) {
2683 continue;
2684 }
2685
2686 // only update point every 100 points
2687 if (j%100 == 0) {
2688 QString status = "\rError Propagation: Inverse Block ";
2689 status.append(QString::number(i+1));
2690 status.append(" of ");
2691 status.append(QString::number(numBlockColumns));
2692 status.append("; Point ");
2693 status.append(QString::number(j+1));
2694 status.append(" of ");
2695 status.append(QString::number(numObjectPoints));
2696 outputBundleStatus(status);
2697 }
2698
2699 // get corresponding Q matrix
2700 // NOTE: we are getting a reference to the Q matrix stored
2701 // in the BundleControlPoint for speed (without the & it is dirt slow)
2702 SparseBlockRowMatrix &Q = point->cholmodQMatrix();
2703
2704 T.clear();
2705
2706 // get corresponding point covariance matrix
2707 boost::numeric::ublas::symmetric_matrix<double> &covariance = pointCovariances[pointIndex];
2708
2709 // get firstQBlock - index i is the key into Q for firstQBlock
2710 LinearAlgebra::Matrix *firstQBlock = Q.value(i);
2711 if (!firstQBlock) {
2712 pointIndex++;
2713 continue;
2714 }
2715
2716 // iterate over Q
2717 // secondQBlock is current map value
2718 QMapIterator< int, LinearAlgebra::Matrix * > it(Q);
2719 while ( it.hasNext() ) {
2720 it.next();
2721
2722 int nKey = it.key();
2723
2724 if (it.key() > i) {
2725 break;
2726 }
2727
2728 LinearAlgebra::Matrix *secondQBlock = it.value();
2729
2730 if ( !secondQBlock ) {// should never be NULL
2731 continue;
2732 }
2733
2734 LinearAlgebra::Matrix *inverseBlock = inverseMatrix.value(it.key());
2735
2736 if ( !inverseBlock ) {// should never be NULL
2737 continue;
2738 }
2739
2740 T = prod(*inverseBlock, trans(*firstQBlock));
2741 T = prod(*secondQBlock,T);
2742
2743 if (nKey != i) {
2744 T += trans(T);
2745 }
2746
2747 try {
2748 covariance += T;
2749 }
2750
2751 catch (std::exception &e) {
2752 outputBundleStatus("\n\n");
2753 QString msg = "Input data and settings are not sufficiently stable "
2754 "for error propagation.";
2755 throw IException(IException::User, msg, _FILEINFO_);
2756 }
2757 }
2758 pointIndex++;
2759 }
2760 }
2761
2762 if (m_bundleSettings->createInverseMatrix()) {
2763 // Close the file.
2764 matrixOutput.close();
2765 // Save the location of the "covariance" matrix
2766 m_bundleResults.setCorrMatCovFileName(matrixFile);
2767 }
2768
2769 // can free sparse normals now
2770 m_sparseNormals.wipe();
2771
2772 // free b (right-hand side vector)
2773 cholmod_l_free_dense(&b,&m_cholmodCommon);
2774
2775 outputBundleStatus("\n\n");
2776
2777 currentTime = Isis::iTime::CurrentLocalTime().toLatin1().data();
2778
2779 status = "\rFilling point covariance matrices: Time ";
2780 status.append(currentTime.c_str());
2781 outputBundleStatus(status);
2782 outputBundleStatus("\n\n");
2783
2784 // now loop over points again and set final covariance stuff
2785 // *** TODO *** Can this loop go into BundleControlPoint
2786 int pointIndex = 0;
2787 for (j = 0; j < numObjectPoints; j++) {
2788
2789 BundleControlPointQsp point = m_bundleControlPoints.at(pointIndex);
2790
2791 if ( point->isRejected() ) {
2792 continue;
2793 }
2794
2795 if (j%100 == 0) {
2796 status = "\rError Propagation: Filling point covariance matrices ";
2797 status.append(QString("%1").arg(j+1));
2798 status.append(" of ");
2799 status.append(QString("%1").arg(numObjectPoints));
2800 status.append("\r");
2801 outputBundleStatus(status);
2802 }
2803
2804 // get corresponding point covariance matrix
2805 boost::numeric::ublas::symmetric_matrix<double> &covariance = pointCovariances[pointIndex];
2806
2807 // Update and reset the matrix
2808 // Get the Limiting Error Propagation uncertainties: sigmas for coordinate 1, 2, and 3 in meters
2809 //
2810 SurfacePoint SurfacePoint = point->adjustedSurfacePoint();
2811
2812 // Get the TEP by adding the corresponding members of pCovar and covariance
2813 boost::numeric::ublas::symmetric_matrix <double,boost::numeric::ublas::upper> pCovar;
2814
2815 if (m_bundleSettings->controlPointCoordTypeBundle() == SurfacePoint::Latitudinal) {
2816 pCovar = SurfacePoint.GetSphericalMatrix(SurfacePoint::Kilometers);
2817 }
2818 else {
2819 // Assume Rectangular coordinates
2820 pCovar = SurfacePoint.GetRectangularMatrix(SurfacePoint::Kilometers);
2821 }
2822 pCovar += covariance;
2823 pCovar *= sigma0Squared;
2824
2825 // debug lines
2826 // if (j < 3) {
2827 // std::cout << " Adjusted surface point ..." << std::endl;
2828 // std:: cout << " sigmaLat (radians) = " << sqrt(pCovar(0,0)) << std::endl;
2829 // std:: cout << " sigmaLon (radians) = " << sqrt(pCovar(1,1)) << std::endl;
2830 // std:: cout << " sigmaRad (km) = " << sqrt(pCovar(2,2)) << std::endl;
2831 // std::cout << " Adjusted matrix = " << std::endl;
2832 // std::cout << " " << pCovar(0,0) << " " << pCovar(0,1) << " "
2833 // << pCovar(0,2) << std::endl;
2834 // std::cout << " " << pCovar(1,0) << " " << pCovar(1,1) << " "
2835 // << pCovar(1,2) << std::endl;
2836 // std::cout << " " << pCovar(2,0) << " " << pCovar(2,1) << " "
2837 // << pCovar(2,2) << std::endl;
2838 // }
2839 // end debug
2840
2841 // Distance units are km**2
2842 SurfacePoint.SetMatrix(m_bundleSettings->controlPointCoordTypeBundle(),pCovar);
2843 point->setAdjustedSurfacePoint(SurfacePoint);
2844 // // debug lines
2845 // if (j < 3) {
2846 // boost::numeric::ublas::symmetric_matrix <double,boost::numeric::ublas::upper> recCovar;
2847 // recCovar = SurfacePoint.GetRectangularMatrix(SurfacePoint::Meters);
2848 // std:: cout << " sigmaLat (meters) = " <<
2849 // point->adjustedSurfacePoint().GetSigmaDistance(SurfacePoint::Latitudinal,
2850 // SurfacePoint::One).meters() << std::endl;
2851 // std:: cout << " sigmaLon (meters) = " <<
2852 // point->adjustedSurfacePoint().GetSigmaDistance(SurfacePoint::Latitudinal,
2853 // SurfacePoint::Two).meters() << std::endl;
2854 // std:: cout << " sigmaRad (km) = " << sqrt(pCovar(2,2)) << std::endl;
2855 // std::cout << "Rectangular matrix with radius in meters" << std::endl;
2856 // std::cout << " " << recCovar(0,0) << " " << recCovar(0,1) << " "
2857 // << recCovar(0,2) << std::endl;
2858 // std::cout << " " << recCovar(1,0) << " " << recCovar(1,1) << " "
2859 // << recCovar(1,2) << std::endl;
2860 // std::cout << " " << recCovar(2,0) << " " << recCovar(2,1) << " "
2861 // << recCovar(2,2) << std::endl;
2862 // }
2863 // // end debug
2864
2865 pointIndex++;
2866 }
2867
2868 return true;
2869 }
2870
2871
2880
2881
2890
2891
2900
2901
2908 return m_serialNumberList->size();
2909 }
2910
2911
2921 QString BundleAdjust::fileName(int i) {
2922 return m_serialNumberList->fileName(i);
2923 }
2924
2925
2932 return m_iteration;
2933 }
2934
2935
2947 return m_controlNet->Camera(i)->instrumentRotation()->Cache("InstrumentPointing");
2948 }
2949
2950
2962 return m_controlNet->Camera(i)->instrumentPosition()->Cache("InstrumentPosition");
2963 }
2964
2965
2975 Camera *imageCam = m_controlNet->Camera(i);
2976 if (imageCam->GetCameraType() != Camera::Csm) {
2977 QString msg = "Cannot get model state for image [" + toString(i) +
2978 "] because it is not a CSM camera model.";
2979 throw IException(IException::Programmer, msg, _FILEINFO_);
2980 }
2981
2982 CSMCamera *csmCamera = dynamic_cast<CSMCamera*>(imageCam);
2983 return csmCamera->getModelState();
2984 }
2985
2986
2996 QString iterationNumber;
2997
2998 if ( m_bundleResults.converged() ) {
2999 iterationNumber = "Iteration" + toString(m_iteration) + ": Final";
3000 }
3001 else {
3002 iterationNumber = "Iteration" + toString(m_iteration);
3003 }
3004
3005 PvlGroup summaryGroup(iterationNumber);
3006
3007 summaryGroup += PvlKeyword("Sigma0",
3008 toString( m_bundleResults.sigma0() ) );
3009 summaryGroup += PvlKeyword("Observations",
3010 toString( m_bundleResults.numberObservations() ) );
3011 summaryGroup += PvlKeyword("Constrained_Point_Parameters",
3012 toString( m_bundleResults.numberConstrainedPointParameters() ) );
3013 summaryGroup += PvlKeyword("Constrained_Image_Parameters",
3014 toString( m_bundleResults.numberConstrainedImageParameters() ) );
3015 if (m_bundleSettings->bundleTargetBody()) {
3016 summaryGroup += PvlKeyword("Constrained_Target_Parameters",
3017 toString( m_bundleResults.numberConstrainedTargetParameters() ) );
3018 }
3019 summaryGroup += PvlKeyword("Unknown_Parameters",
3020 toString( m_bundleResults.numberUnknownParameters() ) );
3021 summaryGroup += PvlKeyword("Degrees_of_Freedom",
3022 toString( m_bundleResults.degreesOfFreedom() ) );
3023 summaryGroup += PvlKeyword( "Rejected_Measures",
3024 toString( m_bundleResults.numberRejectedObservations()/2) );
3025
3026 if ( m_bundleResults.numberMaximumLikelihoodModels() >
3027 m_bundleResults.maximumLikelihoodModelIndex() ) {
3028 // if maximum likelihood estimation is being used
3029
3030 summaryGroup += PvlKeyword("Maximum_Likelihood_Tier: ",
3031 toString( m_bundleResults.maximumLikelihoodModelIndex() ) );
3032 summaryGroup += PvlKeyword("Median_of_R^2_residuals: ",
3033 toString( m_bundleResults.maximumLikelihoodMedianR2Residuals() ) );
3034 }
3035
3036 if ( m_bundleResults.converged() ) {
3037 summaryGroup += PvlKeyword("Converged", "TRUE");
3038 summaryGroup += PvlKeyword("TotalElapsedTime", toString( m_bundleResults.elapsedTime() ) );
3039
3040 if (m_bundleSettings->errorPropagation()) {
3041 summaryGroup += PvlKeyword("ErrorPropagationElapsedTime",
3042 toString( m_bundleResults.elapsedTimeErrorProp() ) );
3043 }
3044 }
3045 else {
3046 summaryGroup += PvlKeyword("Elapsed_Time",
3048 }
3049
3050 std::ostringstream ostr;
3051 ostr << summaryGroup << std::endl;
3052 m_iterationSummary += QString::fromStdString( ostr.str() );
3053
3054 if (m_printSummary && iApp != NULL) {
3055 Application::Log(summaryGroup);
3056 }
3057 else {
3058 std::cout << summaryGroup << std::endl;
3059 }
3060 }
3061
3062
3069 return m_bundleResults.converged();
3070 }
3071
3072
3079 return m_abort;
3080 }
3081
3082
3091 return m_iterationSummary;
3092 }
3093
3094
3105 if (iApp == NULL) { // in a function call
3106 printf("%s", status.toStdString().c_str());
3107 }
3108 else if (QCoreApplication::applicationName() != "ipce") {
3109 printf("%s", status.toStdString().c_str());
3110 }
3111 }
3112
3113
3114
3151
3152 // use qvectors so that we can set the size.
3153 // this will be useful later when adding data.
3154 // data may added out of index order
3155 int numberImages = m_serialNumberList->size();
3156 QVector<Statistics> rmsImageSampleResiduals(numberImages);
3157 QVector<Statistics> rmsImageLineResiduals(numberImages);
3158 QVector<Statistics> rmsImageResiduals(numberImages);
3159
3160 int numObjectPoints = m_bundleControlPoints.size();
3161 for (int i = 0; i < numObjectPoints; i++) {
3162
3163 const BundleControlPointQsp point = m_bundleControlPoints.at(i);
3164
3165 if (point->isRejected()) {
3166 continue;
3167 }
3168
3169 int numMeasures = point->numberOfMeasures();
3170 for (int j = 0; j < numMeasures; j++) {
3171
3172 const BundleMeasureQsp measure = point->at(j);
3173
3174 if (measure->isRejected()) {
3175 continue;
3176 }
3177
3178 double sampleResidual = fabs(measure->sampleResidual());
3179 double lineResidual = fabs(measure->lineResidual());
3180
3181 // Determine the index for this measure's serial number
3182 int imageIndex = m_serialNumberList->serialNumberIndex(measure->cubeSerialNumber());
3183
3184 // add residual data to the statistics object at the appropriate serial number index
3185 rmsImageSampleResiduals[imageIndex].AddData(sampleResidual);
3186 rmsImageLineResiduals[imageIndex].AddData(lineResidual);
3187 rmsImageResiduals[imageIndex].AddData(lineResidual);
3188 rmsImageResiduals[imageIndex].AddData(sampleResidual);
3189 }
3190 }
3191
3192 QVector<Statistics> rmsLidarImageSampleResiduals(numberImages);
3193 QVector<Statistics> rmsLidarImageLineResiduals(numberImages);
3194 QVector<Statistics> rmsLidarImageResiduals(numberImages);
3195
3196
3197 int numLidarPoints = m_bundleLidarControlPoints.size();
3198 for (int i = 0; i < numLidarPoints; i++) {
3199
3201
3202 if (point->isRejected()) {
3203 continue;
3204 }
3205
3206 int numMeasures = point->numberOfMeasures();
3207 for (int j = 0; j < numMeasures; j++) {
3208
3209 const BundleMeasureQsp measure = point->at(j);
3210
3211 if (measure->isRejected()) {
3212 continue;
3213 }
3214
3215 double sampleResidual = fabs(measure->sampleResidual());
3216 double lineResidual = fabs(measure->lineResidual());
3217
3218 // Determine the index for this measure's serial number
3219 int imageIndex = m_serialNumberList->serialNumberIndex(measure->cubeSerialNumber());
3220
3221 // add residual data to the statistics object at the appropriate serial number index
3222 rmsLidarImageSampleResiduals[imageIndex].AddData(sampleResidual);
3223 rmsLidarImageLineResiduals[imageIndex].AddData(lineResidual);
3224 rmsLidarImageResiduals[imageIndex].AddData(lineResidual);
3225 rmsLidarImageResiduals[imageIndex].AddData(sampleResidual);
3226 }
3227 }
3228
3229 if (m_bundleSettings->errorPropagation()) {
3230
3231 // initialize body-fixed coordinate boundaries
3232
3233 // Latitude or X
3234 Distance minSigmaCoord1Dist;
3235 QString minSigmaCoord1PointId = "";
3236
3237 Distance maxSigmaCoord1Dist;
3238 QString maxSigmaCoord1PointId = "";
3239
3240 // Longitude or Y
3241 Distance minSigmaCoord2Dist;
3242 QString minSigmaCoord2PointId = "";
3243
3244 Distance maxSigmaCoord2Dist;
3245 QString maxSigmaCoord2PointId = "";
3246
3247 // Radius or Z
3248 Distance minSigmaCoord3Dist;
3249 QString minSigmaCoord3PointId = "";
3250
3251 Distance maxSigmaCoord3Dist;
3252 QString maxSigmaCoord3PointId = "";
3253
3254 // compute stats for point sigmas
3255 Statistics sigmaCoord1Stats;
3256 Statistics sigmaCoord2Stats;
3257 Statistics sigmaCoord3Stats;
3258
3259 Distance sigmaCoord1Dist, sigmaCoord2Dist, sigmaCoord3Dist;
3260 SurfacePoint::CoordinateType reportType = m_bundleSettings->controlPointCoordTypeReports();
3261 SurfacePoint::CoordinateType bundleType = m_bundleSettings->controlPointCoordTypeBundle();
3262
3263 // we report statistics on coordinate 3 (Radius or Z) UNLESS
3264 // bundle and report types are BOTH Latitudinal AND Radius is OFF
3265 bool reportCoord3Stats = true;
3266 if (bundleType == SurfacePoint::Latitudinal &&
3267 reportType == SurfacePoint::Latitudinal &&
3268 m_bundleSettings->solveRadius() == false) {
3269 reportCoord3Stats = false;
3270 }
3271
3272 int numPoints = m_bundleControlPoints.size();
3273 // initialize max and min values to those from first valid point
3274 for (int i = 0; i < numPoints; i++) {
3275
3276 const BundleControlPointQsp point = m_bundleControlPoints.at(i);
3277
3278 maxSigmaCoord1Dist = point->adjustedSurfacePoint().GetSigmaDistance(reportType,
3279 SurfacePoint::One);
3280 minSigmaCoord1Dist = maxSigmaCoord1Dist;
3281
3282 maxSigmaCoord2Dist = point->adjustedSurfacePoint().GetSigmaDistance(reportType,
3283 SurfacePoint::Two);
3284 minSigmaCoord2Dist = maxSigmaCoord2Dist;
3285
3286 maxSigmaCoord1PointId = point->id();
3287 maxSigmaCoord2PointId = maxSigmaCoord1PointId;
3288 minSigmaCoord1PointId = maxSigmaCoord1PointId;
3289 minSigmaCoord2PointId = maxSigmaCoord1PointId;
3290
3291 // Get stats for coordinate 3 if used
3292 if (reportCoord3Stats) {
3293 maxSigmaCoord3Dist = point->adjustedSurfacePoint().GetSigmaDistance(reportType,
3294 SurfacePoint::Three);
3295 minSigmaCoord3Dist = maxSigmaCoord3Dist;
3296
3297 maxSigmaCoord3PointId = maxSigmaCoord1PointId;
3298 minSigmaCoord3PointId = maxSigmaCoord1PointId;
3299 }
3300 break;
3301 }
3302
3303 for (int i = 0; i < numPoints; i++) {
3304
3305 const BundleControlPointQsp point = m_bundleControlPoints.at(i);
3306
3307 sigmaCoord1Dist = point->adjustedSurfacePoint().GetSigmaDistance(reportType,
3308 SurfacePoint::One);
3309 sigmaCoord2Dist = point->adjustedSurfacePoint().GetSigmaDistance(reportType,
3310 SurfacePoint::Two);
3311 sigmaCoord3Dist = point->adjustedSurfacePoint().GetSigmaDistance(reportType,
3312 SurfacePoint::Three);
3313
3314 sigmaCoord1Stats.AddData(sigmaCoord1Dist.meters());
3315 sigmaCoord2Stats.AddData(sigmaCoord2Dist.meters());
3316 sigmaCoord3Stats.AddData(sigmaCoord3Dist.meters());
3317
3318 if (sigmaCoord1Dist > maxSigmaCoord1Dist) {
3319 maxSigmaCoord1Dist = sigmaCoord1Dist;
3320 maxSigmaCoord1PointId = point->id();
3321 }
3322 if (sigmaCoord2Dist > maxSigmaCoord2Dist) {
3323 maxSigmaCoord2Dist = sigmaCoord2Dist;
3324 maxSigmaCoord2PointId = point->id();
3325 }
3326 if (reportCoord3Stats) {
3327 if (sigmaCoord3Dist > maxSigmaCoord3Dist) {
3328 maxSigmaCoord3Dist = sigmaCoord3Dist;
3329 maxSigmaCoord3PointId = point->id();
3330 }
3331 }
3332 if (sigmaCoord1Dist < minSigmaCoord1Dist) {
3333 minSigmaCoord1Dist = sigmaCoord1Dist;
3334 minSigmaCoord1PointId = point->id();
3335 }
3336 if (sigmaCoord2Dist < minSigmaCoord2Dist) {
3337 minSigmaCoord2Dist = sigmaCoord2Dist;
3338 minSigmaCoord2PointId = point->id();
3339 }
3340 if (reportCoord3Stats) {
3341 if (sigmaCoord3Dist < minSigmaCoord3Dist) {
3342 minSigmaCoord3Dist = sigmaCoord3Dist;
3343 minSigmaCoord3PointId = point->id();
3344 }
3345 }
3346 }
3347
3348 // update bundle results
3349 m_bundleResults.resizeSigmaStatisticsVectors(numberImages);
3350
3351 m_bundleResults.setSigmaCoord1Range(minSigmaCoord1Dist, maxSigmaCoord1Dist,
3352 minSigmaCoord1PointId, maxSigmaCoord1PointId);
3353
3354 m_bundleResults.setSigmaCoord2Range(minSigmaCoord2Dist, maxSigmaCoord2Dist,
3355 minSigmaCoord2PointId, maxSigmaCoord2PointId);
3356
3357 m_bundleResults.setSigmaCoord3Range(minSigmaCoord3Dist, maxSigmaCoord3Dist,
3358 minSigmaCoord3PointId, maxSigmaCoord3PointId);
3359
3360 m_bundleResults.setRmsFromSigmaStatistics(sigmaCoord1Stats.Rms(),
3361 sigmaCoord2Stats.Rms(),
3362 sigmaCoord3Stats.Rms());
3363 }
3364 m_bundleResults.setRmsImageResidualLists(rmsImageLineResiduals.toList(),
3365 rmsImageSampleResiduals.toList(),
3366 rmsImageResiduals.toList());
3367
3368 m_bundleResults.setRmsLidarImageResidualLists(rmsLidarImageLineResiduals.toList(),
3369 rmsLidarImageSampleResiduals.toList(),
3370 rmsLidarImageResiduals.toList());
3371 return true;
3372 }
3373
3374}
static void Log(PvlGroup &results)
Writes Pvl results to sessionlog and printfile.
QString modelState(int index)
Return the updated model state for the ith cube in the cube list given to the constructor.
bool m_cleanUp
!< If the iteration summaries should be output to the log file.
int m_rank
!< If the serial number lists need to be deleted by the destructor.
SparseBlockMatrix m_sparseNormals
!< The right hand side of the normal equations.
int numberOfImages() const
Returns the number of images.
void productAB(SparseBlockColumnMatrix &A, SparseBlockRowMatrix &B)
Perform the matrix multiplication C = N12 x Q.
bool solveCholesky()
Compute the least squares bundle adjustment solution using Cholesky decomposition.
bool computeBundleStatistics()
Compute Bundle statistics and store them in m_bundleResults.
double m_iterationTime
Time for last iteration.
int formLidarPointNormals(LinearAlgebra::MatrixUpperTriangular &N22, SparseBlockColumnMatrix &N12, LinearAlgebra::Vector &n2, LinearAlgebra::Vector &nj, BundleLidarControlPointQsp &point)
Compute the Q matrix and NIC vector for a control point.
bool formNormalEquations()
Form the least-squares normal equations matrix via cholmod.
SerialNumberList * serialNumberList()
Returns a pointer to the serial number list.
void init(Progress *progress=0)
Initialize all solution parameters.
bool computePartials(LinearAlgebra::Matrix &coeffTarget, LinearAlgebra::Matrix &coeffImage, LinearAlgebra::Matrix &coeffPoint3D, LinearAlgebra::Vector &coeffRHS, BundleMeasure &measure, BundleControlPoint &point)
Compute partial derivatives and weighted residuals for a measure.
bool isConverged()
Returns if the BundleAdjust converged.
QString fileName(int index)
Return the ith filename in the cube list file given to constructor.
void abortBundle()
Flag to abort when bundle is threaded.
QString m_lidarFileName
Input lidar point filename.
int m_previousNumberImagePartials
!< The image parameter solution vector.
double iteration() const
Returns what iteration the BundleAdjust is currently on.
void outputBundleStatus(QString status)
Slot for deltack and jigsaw to output the bundle status.
bool flagOutliers()
Flags outlier measures and control points.
QVector< BundleControlPointQsp > m_bundleControlPoints
Vector of control points.
LidarDataQsp lidarData()
Returns a pointer to the output lidar data file.
bool isAborted()
Returns if the BundleAdjust has been aborted.
BundleSettingsQsp m_bundleSettings
Contains the solve settings.
bool invert3x3(LinearAlgebra::MatrixUpperTriangular &m)
Dedicated quick inverse of 3x3 matrix.
int m_numLidarConstraints
TODO: temp.
Table cMatrix(int index)
Return the updated instrument pointing table for the ith cube in the cube list given to the construct...
double computeVtpv()
Computes vtpv, the weighted sum of squares of residuals.
QList< ImageList * > imageLists()
This method returns the image list used in the bundle adjust.
void applyParameterCorrections()
Apply parameter corrections for current iteration.
bool initializeCHOLMODLibraryVariables()
Initializations for CHOLMOD sparse matrix package.
boost::numeric::ublas::symmetric_matrix< double, boost::numeric::ublas::upper, boost::numeric::ublas::column_major > m_normalInverse
!< The lists of images used in the bundle.
LinearAlgebra::Vector m_imageSolution
!< The lower triangular L matrix from Cholesky decomposition.
cholmod_triplet * m_cholmodTriplet
!< The sparse block normal equations matrix.
ControlNetQsp m_controlNet
Output control net.
bool validateNetwork()
control network validation - on the very real chance that the net has not been checked before running...
SerialNumberList * m_serialNumberList
!< Vector of observations.
bool loadCholmodTriplet()
Load sparse normal equations matrix into CHOLMOD triplet.
LidarDataQsp m_lidarDataSet
Output lidar data.
cholmod_factor * m_L
!< The CHOLMOD sparse normal equations matrix used by cholmod_factorize to solve the system.
bool solveSystem()
Compute the solution to the normal equations using the CHOLMOD library.
BundleAdjust(BundleSettingsQsp bundleSettings, const QString &cnetFile, const QString &cubeList, bool printSummary=true)
Construct a BundleAdjust object from the given settings, control network file, and cube list.
void accumProductAlphaAB(double alpha, SparseBlockRowMatrix &A, LinearAlgebra::Vector &B, LinearAlgebra::Vector &C)
Performs the matrix multiplication nj = nj + alpha (Q x n2).
bool initializeNormalEquationsMatrix()
Initialize Normal Equations matrix (m_sparseNormals).
QString iterationSummaryGroup() const
Returns the iteration summary string.
LinearAlgebra::Vector m_RHS
!< Contains object parameters, statistics, and workspace used by the CHOLMOD library.
bool errorPropagation()
Error propagation for solution.
BundleResults m_bundleResults
Stores the results of the bundle adjust.
QString m_cnetFileName
The control net filename.
bool formMeasureNormals(LinearAlgebra::MatrixUpperTriangular &N22, SparseBlockColumnMatrix &N12, LinearAlgebra::VectorCompressed &n1, LinearAlgebra::Vector &n2, LinearAlgebra::Matrix &coeffTarget, LinearAlgebra::Matrix &coeffImage, LinearAlgebra::Matrix &coeffPoint3D, LinearAlgebra::Vector &coeffRHS, int observationIndex)
Form the auxilary normal equation matrices for a measure.
BundleSolutionInfo * bundleSolveInformation()
Create a BundleSolutionInfo containing the settings and results from the bundle adjustment.
int formPointNormals(LinearAlgebra::MatrixUpperTriangular &N22, SparseBlockColumnMatrix &N12, LinearAlgebra::Vector &n2, LinearAlgebra::Vector &nj, BundleControlPointQsp &point)
Compute the Q matrix and NIC vector for a control point.
bool m_abort
!< Contains information about the target body.
BundleLidarPointVector m_bundleLidarControlPoints
Vector of lidar points.
void computeResiduals()
Compute image measure residuals.
bool m_printSummary
!< Summary of the most recently completed iteration.
bool computeRejectionLimit()
Compute rejection limit.
bool productATransB(LinearAlgebra::MatrixUpperTriangular &N22, SparseBlockColumnMatrix &N12, SparseBlockRowMatrix &Q)
Perform the matrix multiplication Q = N22 x N12(transpose)
void iterationSummary()
Creates an iteration summary and an iteration group for the solution summary.
int m_iteration
The current iteration.
bool freeCHOLMODLibraryVariables()
Free CHOLMOD library variables.
bool formWeightedNormals(LinearAlgebra::VectorCompressed &n1, LinearAlgebra::Vector &nj)
Apply weighting for spacecraft position, velocity, acceleration and camera angles,...
BundleSolutionInfo * solveCholeskyBR()
Compute the least squares bundle adjustment solution using Cholesky decomposition.
ControlNetQsp controlNet()
Returns a pointer to the output control network.
cholmod_sparse * m_cholmodNormal
!< The CHOLMOD triplet representation of the sparse normal equations matrix.
~BundleAdjust()
Destroys BundleAdjust object, deallocates pointers (if we have ownership), and frees variables from c...
Table spVector(int index)
Return the updated instrument position table for the ith cube in the cube list given to the construct...
This class holds information about a control point that BundleAdjust needs to run correctly.
SurfacePoint adjustedSurfacePoint() const
Accesses the adjusted SurfacePoint associated with this BundleControlPoint.
QString id() const
Accesses the Point ID associated with this BundleControlPoint.
This class hold image information that BundleAdjust needs to run correctly.Definition for a BundleIma...
Definition BundleImage.h:36
This class holds information about a lidar control point that BundleAdjust requires.
A container class for a ControlMeasure.
QSharedPointer< BundleObservation > parentBundleObservation()
Accesses the parent BundleObservation for this bundle measure.
double sample() const
Accesses the current sample measurement for this control measure.
double line() const
Accesses the current line measurement for this control measure.
QString cubeSerialNumber() const
Accesses the serial number of the cube containing this control measure.
Camera * camera() const
Accesses the associated camera for this bundle measure.
@ Sigma0
The value of sigma0 will be used to determine that the bundle adjustment has converged.
Container class for BundleAdjustment results.
void setRunTime(QString runTime)
Sets the run time, and the name if a name is not already set.
QString getModelState() const
Get the CSM Model state string to re-create the CSM Model.
virtual bool GetXY(const SurfacePoint &spoint, double *cudx, double *cudy, bool test=true)
Compute undistorted focal plane coordinate from ground position using current Spice from SetImage cal...
@ Framing
Framing Camera.
Definition Camera.h:359
@ Csm
Community Sensor Model Camera.
Definition Camera.h:365
virtual bool SetImage(const double sample, const double line)
Sets the sample/line values of the image to get the lat/lon values.
Definition Camera.cpp:156
virtual CameraType GetCameraType() const =0
Returns the type of camera that was created.
CameraGroundMap * GroundMap()
Returns a pointer to the CameraGroundMap object.
Definition Camera.cpp:2886
This represents an ISIS control net in a project-based GUI interface.
Definition Control.h:65
QString fileName() const
Access the name of the control network file associated with this Control.
Definition Control.cpp:252
a control network
Definition ControlNet.h:258
A single control point.
Status ComputeApriori()
Computes a priori lat/lon/radius point coordinates by determining the average lat/lon/radius of all m...
Distance measurement, usually in meters.
Definition Distance.h:34
double meters() const
Get the distance in meters.
Definition Distance.cpp:85
File name manipulation and expansion.
Definition FileName.h:100
QString expanded() const
Returns a QString of the full file name including the file path, excluding the attributes.
Definition FileName.cpp:196
Isis exception class.
Definition IException.h:91
@ User
A type of error that could only have occurred due to a mistake on the user's part (e....
Definition IException.h:126
@ Programmer
This error is for when a programmer made an API call that was illegal.
Definition IException.h:146
ErrorType errorType() const
Returns the source of the error for this exception.
This represents a cube in a project-based GUI interface.
Definition Image.h:105
void closeCube()
Cleans up the Cube pointer.
Definition Image.cpp:282
QString fileName() const
Get the file name of the cube that this image represents.
Definition Image.cpp:315
Internalizes a list of images and allows for operations on the entire list.
Definition ImageList.h:53
void append(Image *const &value)
Appends an image to the image list.
LidarData class.
Definition LidarData.h:49
boost::numeric::ublas::compressed_vector< double > VectorCompressed
Definition for an Isis::LinearAlgebra::VectorCompressed of doubles.
boost::numeric::ublas::vector< double > Vector
Definition for an Isis::LinearAlgebra::Vector of doubles.
boost::numeric::ublas::matrix< double > Matrix
Definition for an Isis::LinearAlgebra::Matrix of doubles.
boost::numeric::ublas::symmetric_matrix< double, boost::numeric::ublas::upper > MatrixUpperTriangular
Definition for an Isis::LinearAlgebra::MatrixUpperTriangular of doubles with an upper configuration.
Program progress reporter.
Definition Progress.h:42
Contains multiple PvlContainers.
Definition PvlGroup.h:41
A single keyword-value pair.
Definition PvlKeyword.h:87
Serial Number list generator.
SparseBlockColumnMatrix.
void wipe()
Deletes all pointer elements and removes them from the map.
int startColumn() const
Sets starting column for block in full matrix.
void zeroBlocks()
Sets all elements of all matrix blocks to zero.
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...
int numberOfColumns()
Returns total number of columns in map (NOTE: NOT the number of matrix blocks).
int numberOfRows()
Returns total number of rows in map (this needs to be clarified and maybe rewritten).
void zeroBlocks()
Sets all elements of all matrix blocks to zero.
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...
This class is used to accumulate statistics on double arrays.
Definition Statistics.h:93
void AddData(const double *data, const unsigned int count)
Add an array of doubles to the accumulators and counters.
double Rms() const
Computes and returns the rms.
This class defines a body-fixed surface point.
CoordinateType
Defines the coordinate typ, units, and coordinate index for some of the output methods.
@ Latitudinal
Planetocentric latitudinal (lat/lon/rad) coordinates.
void SetMatrix(CoordinateType type, const boost::numeric::ublas::symmetric_matrix< double, boost::numeric::ublas::upper > &covar)
Set the covariance matrix.
Latitude GetLatitude() const
Return the body-fixed latitude for the surface point.
Longitude GetLongitude() const
Return the body-fixed longitude for the surface point.
void ResetLocalRadius(const Distance &radius)
This method resets the local radius of a SurfacePoint.
Distance GetLocalRadius() const
Return the radius of the surface point.
Class for storing Table blobs information.
Definition Table.h:61
static QString CurrentLocalTime()
Returns the current local time This time is taken directly from the system clock, so if the system cl...
Definition iTime.cpp:520
This is free and unencumbered software released into the public domain.
This is free and unencumbered software released into the public domain.
Definition Calculator.h:18
This is free and unencumbered software released into the public domain.
Definition Apollo.h:16
QSharedPointer< LidarData > LidarDataQsp
Definition for a shared pointer to a LidarData object.
Definition LidarData.h:100
QString toString(bool boolToConvert)
Global function to convert a boolean to a string.
Definition IString.cpp:211
QSharedPointer< BundleLidarControlPoint > BundleLidarControlPointQsp
QSharedPointer to a BundleLidarControlPoint.
QSharedPointer< BundleSettings > BundleSettingsQsp
Definition for a BundleSettingsQsp, a shared pointer to a BundleSettings object.
QSharedPointer< BundleObservation > BundleObservationQsp
Typdef for BundleObservation QSharedPointer.
static void cholmodErrorHandler(int nStatus, const char *file, int nLineNo, const char *message)
Custom error handler for CHOLMOD.
QSharedPointer< ControlNet > ControlNetQsp
Typedef for QSharedPointer to control network. This typedef is for future implementation of target bo...
Definition ControlNet.h:485
QSharedPointer< LidarControlPoint > LidarControlPointQsp
Definition for a shared pointer to a LidarControlPoint.
QSharedPointer< BundleControlPoint > BundleControlPointQsp
Definition for BundleControlPointQSP, a shared pointer to a BundleControlPoint.
QSharedPointer< BundleMeasure > BundleMeasureQsp
Definition for BundleMeasureQsp, a shared pointer to a BundleMeasure.