Isis 3.0 Programmer Reference
Back | Home
BundleAdjust.cpp
1 #include "BundleAdjust.h"
2 
3 // std lib
4 #include <iomanip>
5 #include <iostream>
6 #include <sstream>
7 
8 // qt lib
9 #include <QCoreApplication>
10 #include <QDebug>
11 #include <QFile>
12 #include <QMutex>
13 
14 // boost lib
15 #include <boost/lexical_cast.hpp>
16 #include <boost/numeric/ublas/io.hpp>
17 #include <boost/numeric/ublas/matrix_sparse.hpp>
18 #include <boost/numeric/ublas/vector_proxy.hpp>
19 
20 // Isis lib
21 #include "Application.h"
22 #include "BundleObservation.h"
24 #include "BundleResults.h"
25 #include "BundleSettings.h"
26 #include "BundleSolutionInfo.h"
27 #include "BundleTargetBody.h"
28 #include "Camera.h"
29 #include "CameraDetectorMap.h"
30 #include "CameraDistortionMap.h"
31 #include "CameraFocalPlaneMap.h"
32 #include "CameraGroundMap.h"
33 #include "Control.h"
34 #include "ControlPoint.h"
35 #include "CorrelationMatrix.h"
36 #include "Distance.h"
37 #include "ImageList.h"
38 #include "iTime.h"
39 #include "Latitude.h"
40 #include "Longitude.h"
42 #include "SpecialPixel.h"
43 #include "StatCumProbDistDynCalc.h"
44 #include "SurfacePoint.h"
45 #include "Target.h"
46 
47 using namespace boost::numeric::ublas;
48 using namespace Isis;
49 
50 namespace Isis {
51 
52 
62  static void cholmodErrorHandler(int nStatus,
63  const char* file,
64  int nLineNo,
65  const char* message) {
66  QString errlog;
67 
68  errlog = "SPARSE: ";
69  errlog += message;
70 
71  PvlGroup gp(errlog);
72 
73  gp += PvlKeyword("File",file);
74  gp += PvlKeyword("Line_Number", toString(nLineNo));
75  gp += PvlKeyword("Status", toString(nStatus));
76 
77 // Application::Log(gp);
78 
79  errlog += ". (See print.prt for details)";
80 
81 // throw IException(IException::Unknown, errlog, _FILEINFO_);
82  }
83 
84 
94  BundleAdjust::BundleAdjust(BundleSettingsQsp bundleSettings,
95  const QString &cnetFile,
96  const QString &cubeList,
97  bool printSummary) {
98  m_abort = false;
99  Progress progress;
100  // initialize constructor dependent settings...
101  // m_printSummary, m_cleanUp, m_cnetFileName, m_controlNet,
102  // m_serialNumberList, m_bundleSettings
103  m_printSummary = printSummary;
104  m_cleanUp = true;
105  m_cnetFileName = cnetFile;
106  m_controlNet = ControlNetQsp( new ControlNet(cnetFile, &progress) );
107  m_bundleResults.setOutputControlNet(m_controlNet);
108  m_serialNumberList = new SerialNumberList(cubeList);
109  m_bundleSettings = bundleSettings;
110  m_bundleTargetBody = bundleSettings->bundleTargetBody();
111 
112  init(&progress);
113  }
114 
115 
126  BundleAdjust::BundleAdjust(BundleSettingsQsp bundleSettings,
127  QString &cnetFile,
128  SerialNumberList &snlist,
129  bool printSummary) {
130  // initialize constructor dependent settings...
131  // m_printSummary, m_cleanUp, m_cnetFileName, m_controlNet,
132  // m_serialNumberList, m_bundleSettings
133  m_abort = false;
134  Progress progress;
135  m_printSummary = printSummary;
136  m_cleanUp = false;
137  m_cnetFileName = cnetFile;
138  m_controlNet = ControlNetQsp( new ControlNet(cnetFile, &progress) );
139  m_bundleResults.setOutputControlNet(m_controlNet);
140  m_serialNumberList = &snlist;
141  m_bundleSettings = bundleSettings;
142  m_bundleTargetBody = bundleSettings->bundleTargetBody();
143 
144  init();
145  }
146 
147 
158  BundleAdjust::BundleAdjust(BundleSettingsQsp bundleSettings,
159  Control &cnet,
160  SerialNumberList &snlist,
161  bool printSummary) {
162  // initialize constructor dependent settings...
163  // m_printSummary, m_cleanUp, m_cnetFileName, m_controlNet,
164  // m_serialNumberList, m_bundleSettings
165  m_abort = false;
166  Progress progress;
167  m_printSummary = printSummary;
168  m_cleanUp = false;
169  m_cnetFileName = cnet.fileName();
170  m_controlNet = ControlNetQsp( new ControlNet(cnet.fileName(), &progress) );
171  m_bundleResults.setOutputControlNet(m_controlNet);
172  m_serialNumberList = &snlist;
173  m_bundleSettings = bundleSettings;
174  m_bundleTargetBody = bundleSettings->bundleTargetBody();
175 
176  init();
177  }
178 
179 
190  BundleAdjust::BundleAdjust(BundleSettingsQsp bundleSettings,
191  ControlNet &cnet,
192  SerialNumberList &snlist,
193  bool printSummary) {
194  // initialize constructor dependent settings...
195  // m_printSummary, m_cleanUp, m_cnetFileName, m_controlNet,
196  // m_serialNumberList, m_bundleSettings
197  m_abort = false;
198  m_printSummary = printSummary;
199  m_cleanUp = false;
200  m_cnetFileName = "";
201  m_controlNet = ControlNetQsp( new ControlNet(cnet) );
202  m_bundleResults.setOutputControlNet(m_controlNet);
203  m_serialNumberList = &snlist;
204  m_bundleSettings = bundleSettings;
205  m_bundleTargetBody = bundleSettings->bundleTargetBody();
206 
207  init();
208  }
209 
210 
219  BundleAdjust::BundleAdjust(BundleSettingsQsp bundleSettings,
220  ControlNetQsp cnet,
221  const QString &cubeList,
222  bool printSummary) {
223  m_abort = false;
224  m_printSummary = printSummary;
225  m_cleanUp = false;
226  m_cnetFileName = "";
227  m_controlNet = cnet;
228  m_bundleResults.setOutputControlNet(m_controlNet);
229  m_serialNumberList = new SerialNumberList(cubeList);
230  m_bundleSettings = bundleSettings;
231  m_bundleTargetBody = bundleSettings->bundleTargetBody();
232 
233  init();
234  }
235 
236 
246  BundleAdjust::BundleAdjust(BundleSettingsQsp bundleSettings,
247  Control &control,
248  QList<ImageList *> imgLists,
249  bool printSummary) {
250  m_bundleSettings = bundleSettings;
251 
252  m_abort = false;
253  Progress progress;
254  m_controlNet = ControlNetQsp( new ControlNet(control.fileName(), &progress) );
255  m_bundleResults.setOutputControlNet(m_controlNet);
256 
257  // this is too slow and we need to get rid of the serial number list anyway
258  // should be unnecessary as Image class has serial number
259  // could hang on to image list until creating BundleObservations?
260  m_serialNumberList = new SerialNumberList;
261  foreach (ImageList *imgList, imgLists) {
262  foreach (Image *image, *imgList) {
263  m_serialNumberList->add(image->fileName());
264 // m_serialNumberList->add(image->serialNumber(), image->fileName());
265  }
266  }
267 
268  m_bundleTargetBody = bundleSettings->bundleTargetBody();
269 
270  m_printSummary = printSummary;
271 
272  m_cleanUp = false;
273  m_cnetFileName = control.fileName();
274 
275  init();
276  }
277 
278 
287  BundleAdjust::~BundleAdjust() {
288  // If we have ownership
289  if (m_cleanUp) {
290  delete m_serialNumberList;
291  }
292 
293  freeCHOLMODLibraryVariables();
294 
295  }
296 
297 
329  void BundleAdjust::init(Progress *progress) {
330  // initialize
331  //
332  // JWB
333  // - some of these not originally initialized.. better values???
334  m_iteration = 0;
335  m_radiansToMeters = 0.0;
336  m_metersToRadians = 0.0;
337  m_rank = 0;
338  m_iterationSummary = "";
339 
340  // Get the cameras set up for all images
341  // NOTE - THIS IS NOT THE SAME AS "setImage" as called in BundleAdjust::computePartials
342  // this call only does initializations; sets measure's camera pointer, etc
343  // RENAME????????????
344  m_controlNet->SetImages(*m_serialNumberList, progress);
345 
346  // clear JigsawRejected flags
347  m_controlNet->ClearJigsawRejected();
348 
349  // initialize held variables
350  int numImages = m_serialNumberList->size();
351 
352  // matrix stuff
353  m_normalInverse.clear();
354  m_RHS.clear();
355  m_imageSolution.clear();
356 
357  // we don't want to call initializeCHOLMODLibraryVariables() here since mRank=0
358  // m_cholmodCommon, m_sparseNormals are not initialized
359  m_L = NULL;
360  m_cholmodNormal = NULL;
361  m_cholmodTriplet = NULL;
362 
363  // should we initialize objects m_xResiduals, m_yResiduals, m_xyResiduals
364 
365  // (must be a smarter way)
366  // get target body radii and body specific conversion factors between radians and meters.
367  // need validity checks and different conversion factors for lat and long
368  // initialize m_bodyRadii
369  m_bodyRadii[0] = m_bodyRadii[1] = m_bodyRadii[2] = Distance();
370  Camera *cnetCamera = m_controlNet->Camera(0);
371  if (cnetCamera) {
372  cnetCamera->radii(m_bodyRadii); // meters
373 
374  if (m_bodyRadii[0] >= Distance(0, Distance::Meters)) {
375  m_metersToRadians = 0.001 / m_bodyRadii[0].kilometers(); // at equator
376  m_radiansToMeters = 1.0 / m_metersToRadians;
377  m_bundleResults.setRadiansToMeters(m_radiansToMeters);
378  }
379  }
380 
381  // TESTING
382  // TODO: code below should go into a separate method???
383  // set up BundleObservations and assign solve settings for each from BundleSettings class
384  for (int i = 0; i < numImages; i++) {
385 
386  Camera *camera = m_controlNet->Camera(i);
387  QString observationNumber = m_serialNumberList->observationNumber(i);
388  QString instrumentId = m_serialNumberList->spacecraftInstrumentId(i);
389  QString serialNumber = m_serialNumberList->serialNumber(i);
390  QString fileName = m_serialNumberList->fileName(i);
391 
392  // create a new BundleImage and add to new (or existing if observation mode is on)
393  // BundleObservation
394  BundleImageQsp image = BundleImageQsp(new BundleImage(camera, serialNumber, fileName));
395 
396  if (!image) {
397  QString msg = "In BundleAdjust::init(): image " + fileName + "is null." + "\n";
398  throw IException(IException::Programmer, msg, _FILEINFO_);
399  }
400 
401  BundleObservationQsp observation =
402  m_bundleObservations.addNew(image, observationNumber, instrumentId, m_bundleSettings);
403 
404  if (!observation) {
405  QString msg = "In BundleAdjust::init(): observation "
406  + observationNumber + "is null." + "\n";
407  throw IException(IException::Programmer, msg, _FILEINFO_);
408  }
409  }
410 
411  // initialize exterior orientation (spice) for all BundleImages in all BundleObservations
412  // TODO!!! - should these initializations just be done when we add the new observation above?
413  m_bundleObservations.initializeExteriorOrientation();
414 
415  if (m_bundleSettings->solveTargetBody()) {
416  m_bundleObservations.initializeBodyRotation();
417  }
418 
419  // set up vector of BundleControlPoints
420  int numControlPoints = m_controlNet->GetNumPoints();
421  for (int i = 0; i < numControlPoints; i++) {
422  ControlPoint *point = m_controlNet->GetPoint(i);
423  if (point->IsIgnored()) {
424  continue;
425  }
426 
427  BundleControlPointQsp bundleControlPoint(new BundleControlPoint(point));
428  m_bundleControlPoints.append(bundleControlPoint);
429 
430  bundleControlPoint->setWeights(m_bundleSettings, m_metersToRadians);
431 
432  // set parent observation for each BundleMeasure
433 
434  int numMeasures = bundleControlPoint->size();
435  for (int j=0; j < numMeasures; j++) {
436  BundleMeasureQsp measure = bundleControlPoint->at(j);
437  QString cubeSerialNumber = measure->cubeSerialNumber();
438 
439  BundleObservationQsp observation =
440  m_bundleObservations.observationByCubeSerialNumber(cubeSerialNumber);
441  BundleImageQsp image = observation->imageByCubeSerialNumber(cubeSerialNumber);
442 
443  measure->setParentObservation(observation);
444  measure->setParentImage(image);
445  }
446  }
447 
448  //===========================================================================================//
449  //==== Use the bundle settings to initialize more member variables and set up solutions =====//
450  //===========================================================================================//
451 
452  // TODO: Need to have some validation code to make sure everything is
453  // on the up-and-up with the control network. Add checks for multiple
454  // networks, images without any points, and points on images removed from
455  // the control net (when we start adding software to remove points with high
456  // residuals) and ?. For "deltack" a single measure on a point is allowed
457  // so skip the test.
458  if (m_bundleSettings->validateNetwork()) {
459  validateNetwork();
460  }
461  m_bundleResults.maximumLikelihoodSetUp(m_bundleSettings->maximumLikelihoodEstimatorModels());
462 
463  //===========================================================================================//
464  //=============== End Bundle Settings =======================================================//
465  //===========================================================================================//
466 
467  //===========================================================================================//
468  //======================== initialize matrices and more parameters ==========================//
469  //===========================================================================================//
470 
471  // size of reduced normals matrix
472 
473  // TODO
474  // this should be determined from BundleSettings
475  // m_rank will be the sum of observation, target, and self-cal parameters
476  // TODO
477  m_rank = m_bundleObservations.numberParameters();
478 
479  if (m_bundleSettings->solveTargetBody()) {
480  m_rank += m_bundleSettings->numberTargetBodyParameters();
481  }
482 
483  int num3DPoints = m_bundleControlPoints.size();
484 
485  m_bundleResults.setNumberUnknownParameters(m_rank + 3 * num3DPoints);
486 
487  m_imageSolution.resize(m_rank);
488 
489  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
490  // initializations for cholmod
491  initializeCHOLMODLibraryVariables();
492 
493  }
494 
495 
515  bool BundleAdjust::validateNetwork() {
516  outputBundleStatus("\nValidating network...");
517 
518  int imagesWithInsufficientMeasures = 0;
519  QString msg = "Images with one or less measures:\n";
520 
521  int numObservations = m_bundleObservations.size();
522  for (int i = 0; i < numObservations; i++) {
523  int numImages = m_bundleObservations.at(i)->size();
524  for (int j = 0; j < numImages; j++) {
525  BundleImageQsp bundleImage = m_bundleObservations.at(i)->at(j);
526  int numMeasures = m_controlNet->
527  GetNumberOfValidMeasuresInImage(bundleImage->serialNumber());
528 
529  if (numMeasures > 1) {
530  continue;
531  }
532 
533  imagesWithInsufficientMeasures++;
534  msg += bundleImage->fileName() + ": " + toString(numMeasures) + "\n";
535  }
536  }
537 
538  if ( imagesWithInsufficientMeasures > 0 ) {
539  throw IException(IException::User, msg, _FILEINFO_);
540  }
541 
542  outputBundleStatus("Validation complete!...");
543 
544  return true;
545  }
546 
553  bool BundleAdjust::initializeCHOLMODLibraryVariables() {
554 
555  if ( m_rank <= 0 ) {
556  return false;
557  }
558 
559  m_cholmodTriplet = NULL;
560 
561  cholmod_start(&m_cholmodCommon);
562 
563  // set user-defined cholmod error handler
564  m_cholmodCommon.error_handler = cholmodErrorHandler;
565 
566  // testing not using metis
567  m_cholmodCommon.nmethods = 1;
568  m_cholmodCommon.method[0].ordering = CHOLMOD_AMD;
569 
570  // set size of sparse block normal equations matrix
571  if (m_bundleSettings->solveTargetBody()) {
572  m_sparseNormals.setNumberOfColumns(m_bundleObservations.size()+1);
573  }
574  else {
575  m_sparseNormals.setNumberOfColumns(m_bundleObservations.size());
576  }
577 
578  return true;
579  }
580 
581 
590  bool BundleAdjust::freeCHOLMODLibraryVariables() {
591 
592  cholmod_free_triplet(&m_cholmodTriplet, &m_cholmodCommon);
593  cholmod_free_sparse(&m_cholmodNormal, &m_cholmodCommon);
594  cholmod_free_factor(&m_L, &m_cholmodCommon);
595 
596  cholmod_finish(&m_cholmodCommon);
597 
598  return true;
599  }
600 
601 
611  BundleSolutionInfo BundleAdjust::solveCholeskyBR() {
612  solveCholesky();
613  return bundleSolveInformation();
614  }
615 
616 
621  void BundleAdjust::abortBundle() {
622  m_abort = true;
623  }
624 
625 
640  bool BundleAdjust::solveCholesky() {
641  try {
642 
643  // throw error if a frame camera is included AND
644  // if m_bundleSettings->solveInstrumentPositionOverHermiteSpline()
645  // is set to true (can only use for line scan or radar)
646  // if (m_bundleSettings->solveInstrumentPositionOverHermiteSpline() == true) {
647  // int numImages = images();
648  // for (int i = 0; i < numImages; i++) {
649  // if (m_controlNet->Camera(i)->GetCameraType() == 0) {
650  // QString msg = "At least one sensor is a frame camera. "
651  // "Spacecraft Option OVERHERMITE is not valid for frame cameras\n";
652  // throw IException(IException::User, msg, _FILEINFO_);
653  // }
654  // }
655  // }
656 
657  // Compute the apriori lat/lons for each nonheld point
658  m_controlNet->ComputeApriori(); // original location
659 
660  // ken testing - if solving for target mean radius, set point radius to current mean radius
661  // if solving for triaxial radii, set point radius to local radius
662  if (m_bundleTargetBody && m_bundleTargetBody->solveMeanRadius()) {
663  int numControlPoints = m_bundleControlPoints.size();
664  for (int i = 0; i < numControlPoints; i++) {
665  BundleControlPointQsp point = m_bundleControlPoints.at(i);
666  SurfacePoint surfacepoint = point->adjustedSurfacePoint();
667 
668  surfacepoint.ResetLocalRadius(m_bundleTargetBody->meanRadius());
669 
670  point->setAdjustedSurfacePoint(surfacepoint);
671  }
672  }
673 
674  if (m_bundleTargetBody && m_bundleTargetBody->solveTriaxialRadii()) {
675  int numControlPoints = m_bundleControlPoints.size();
676  for (int i = 0; i < numControlPoints; i++) {
677  BundleControlPointQsp point = m_bundleControlPoints.at(i);
678  SurfacePoint surfacepoint = point->adjustedSurfacePoint();
679 
680  Distance localRadius = m_bundleTargetBody->localRadius(surfacepoint.GetLatitude(),
681  surfacepoint.GetLongitude());
682  surfacepoint.ResetLocalRadius(localRadius);
683 
684  point->setAdjustedSurfacePoint(surfacepoint);
685  }
686  }
687 
688  m_iteration = 1;
689  double vtpv = 0.0;
690  double previousSigma0 = 0.0;
691 
692  // start the clock
693  clock_t solveStartClock = clock();
694 
695  for (;;) {
696 
697  emit iterationUpdate(m_iteration, m_bundleResults.sigma0());
698 
699  // testing
700  if (m_abort) {
701  m_bundleResults.setConverged(false);
702  emit statusUpdate("\n aborting...");
703  emit finished();
704  return false;
705  }
706  // testing
707 
708  emit statusUpdate( QString("starting iteration %1\n").arg(m_iteration) );
709 
710  clock_t iterationStartClock = clock();
711 
712  // zero normals (after iteration 0)
713  if (m_iteration != 1) {
714  m_sparseNormals.zeroBlocks();
715  }
716 
717  // form normal equations
718  if (!formNormalEquations()) {
719  m_bundleResults.setConverged(false);
720  break;
721  }
722 
723  // testing
724  if (m_abort) {
725  m_bundleResults.setConverged(false);
726  emit statusUpdate("\n aborting...");
727  emit finished();
728  return false;
729  }
730  // testing
731 
732  // solve the system
733  if (!solveSystem()) {
734  printf("solve failed!\n");
735  m_bundleResults.setConverged(false);
736  break;
737  }
738 
739  // testing
740  if (m_abort) {
741  m_bundleResults.setConverged(false);
742  emit statusUpdate("\n aborting...");
743  emit finished();
744  return false;
745  }
746  // testing
747 
748  // apply parameter corrections
749  applyParameterCorrections();
750 
751  // testing
752  if (m_abort) {
753  m_bundleResults.setConverged(false);
754  emit statusUpdate("\n aborting...");
755  emit finished();
756  return false;
757  }
758  // testing
759 
760  // compute residuals
761  vtpv = computeResiduals();
762 
763  // flag outliers
764  if ( m_bundleSettings->outlierRejection() ) {
765  computeRejectionLimit();
766  flagOutliers();
767  }
768 
769  // testing
770  if (m_abort) {
771  m_bundleResults.setConverged(false);
772  emit statusUpdate("\n aborting...");
773  emit finished();
774  return false;
775  }
776  // testing
777 
778  // variance of unit weight (also reference variance, variance factor, etc.)
779  m_bundleResults.computeSigma0(vtpv, m_bundleSettings->convergenceCriteria());
780 
781  // Set up formatting for status updates with doubles (e.g. Sigma0, Elapsed Time)
782  int fieldWidth = 20;
783  char format = 'f';
784  int precision = 10;
785 
786  emit statusUpdate(QString("Iteration: %1")
787  .arg(m_iteration));
788  emit statusUpdate(QString("Sigma0: %1")
789  .arg(m_bundleResults.sigma0(),
790  fieldWidth,
791  format,
792  precision));
793  emit statusUpdate(QString("Observations: %1")
794  .arg(m_bundleResults.numberObservations()));
795  emit statusUpdate(QString("Constrained Parameters:%1")
796  .arg(m_bundleResults.numberConstrainedPointParameters()));
797  emit statusUpdate(QString("Unknowns: %1")
798  .arg(m_bundleResults.numberUnknownParameters()));
799  emit statusUpdate(QString("Degrees of Freedom: %1")
800  .arg(m_bundleResults.degreesOfFreedom()));
801  emit iterationUpdate(m_iteration, m_bundleResults.sigma0());
802 
803  // check for convergence
804  if (m_bundleSettings->convergenceCriteria() == BundleSettings::Sigma0) {
805  if (fabs(previousSigma0 - m_bundleResults.sigma0())
806  <= m_bundleSettings->convergenceCriteriaThreshold()) {
807  // convergence detected
808 
809  // if maximum likelihood tiers are being processed,
810  // check to see if there's another tier to go.
811  if (m_bundleResults.maximumLikelihoodModelIndex()
812  < m_bundleResults.numberMaximumLikelihoodModels() - 1
813  && m_bundleResults.maximumLikelihoodModelIndex()
814  < 2) {
815  // TODO is this second condition redundant???
816  // should BundleResults require num models <= 3, so num models - 1 <= 2
817  if (m_bundleResults.numberMaximumLikelihoodModels()
818  > m_bundleResults.maximumLikelihoodModelIndex() + 1) {
819 
820  // If there is another tier left we will increment the index.
821  m_bundleResults.incrementMaximumLikelihoodModelIndex();
822  }
823  }
824  else { // otherwise iterations are complete
825  m_bundleResults.setConverged(true);
826  emit statusUpdate("Bundle has converged\n");
827  break;
828  }
829  }
830  }
831  else {
832  // bundleSettings.convergenceCriteria() == BundleSettings::ParameterCorrections
833  int numConvergedParams = 0;
834  int numImgParams = m_imageSolution.size();
835  for (int ij = 0; ij < numImgParams; ij++) {
836  if (fabs(m_imageSolution(ij)) > m_bundleSettings->convergenceCriteriaThreshold()) {
837  break;
838  }
839  else
840  numConvergedParams++;
841  }
842 
843  if ( numConvergedParams == numImgParams ) {
844  m_bundleResults.setConverged(true);
845  emit statusUpdate("Bundle has converged");
846  break;
847  }
848  }
849 
850  m_bundleResults.printMaximumLikelihoodTierInformation();
851  clock_t iterationStopClock = clock();
852  double iterationTime = (iterationStopClock - iterationStartClock)
853  / (double)CLOCKS_PER_SEC;
854  emit statusUpdate( QString("End of Iteration %1").arg(m_iteration) );
855  emit statusUpdate( QString("Elapsed Time: %1").arg(iterationTime,
856  fieldWidth,
857  format,
858  precision) );
859 
860  // check for maximum iterations
861  if (m_iteration >= m_bundleSettings->convergenceCriteriaMaximumIterations()) {
862  break;
863  }
864 
865  // restart the dynamic calculation of the cumulative probility distribution of residuals
866  // (in unweighted pixels) --so it will be up to date for the next iteration
867  if (!m_bundleResults.converged()) {
868  m_bundleResults.initializeResidualsProbabilityDistribution(101);
869  }
870  // TODO: is this necessary ???
871  // probably all ready initialized to 101 nodes in bundle settings constructor...
872 
873  // if we're using CHOLMOD and still going, release cholmod_factor
874  // (if we don't, memory leaks will occur), otherwise we need it for error propagation
875  if (!m_bundleResults.converged() || !m_bundleSettings->errorPropagation()) {
876  cholmod_free_factor(&m_L, &m_cholmodCommon);
877  }
878 
879 
880  iterationSummary();
881 
882  m_iteration++;
883 
884  previousSigma0 = m_bundleResults.sigma0();
885  }
886 
887  if (m_bundleResults.converged() && m_bundleSettings->errorPropagation()) {
888  clock_t errorPropStartClock = clock();
889  printf("Starting Error Propagation");
890  errorPropagation();
891  emit statusUpdate("\n\nError Propagation Complete");
892  clock_t errorPropStopClock = clock();
893  m_bundleResults.setElapsedTimeErrorProp((errorPropStopClock - errorPropStartClock)
894  / (double)CLOCKS_PER_SEC);
895  }
896 
897  clock_t solveStopClock = clock();
898  m_bundleResults.setElapsedTime((solveStopClock - solveStartClock)
899  / (double)CLOCKS_PER_SEC);
900 
901  wrapUp();
902 
903  m_bundleResults.setIterations(m_iteration);
904  m_bundleResults.setObservations(m_bundleObservations);
905  m_bundleResults.setBundleControlPoints(m_bundleControlPoints);
906 
907  BundleSolutionInfo *results = new BundleSolutionInfo(bundleSolveInformation());
908  emit resultsReady(results);
909 
910  emit statusUpdate("\nBundle Complete");
911 
912  iterationSummary();
913  }
914  catch (IException &e) {
915  m_bundleResults.setConverged(false);
916  emit statusUpdate("\n aborting...");
917  emit finished();
918  QString msg = "Could not solve bundle adjust.";
919  throw IException(e, e.errorType(), msg, _FILEINFO_);
920  }
921 
922  return true;
923  }
924 
925 
931  BundleSolutionInfo BundleAdjust::bundleSolveInformation() {
932  BundleSolutionInfo results(m_bundleSettings, FileName(m_cnetFileName), m_bundleResults);
933  results.setRunTime("");
934  return results;
935  }
936 
937 
949  bool BundleAdjust::formNormalEquations() {
950  bool status = false;
951 
952  m_bundleResults.setNumberObservations(0);// ???
953  m_bundleResults.resetNumberConstrainedPointParameters();//???
954 
955  // Initialize auxilary matrices and vectors.
956  static LinearAlgebra::Matrix coeffTarget;
957  static LinearAlgebra::Matrix coeffImage;
958  static LinearAlgebra::Matrix coeffPoint3D(2, 3);
959  static LinearAlgebra::Vector coeffRHS(2);
960  static boost::numeric::ublas::symmetric_matrix<double, upper> N22(3);
962  static LinearAlgebra::Vector n2(3);
963  boost::numeric::ublas::compressed_vector<double> n1(m_rank);
964 
965  m_RHS.resize(m_rank);
966 
967  // if solving for target body parameters, set size of coeffTarget
968  // (note this size will not change through the adjustment).
969  if (m_bundleSettings->solveTargetBody()) {
970  int numTargetBodyParameters = m_bundleSettings->numberTargetBodyParameters();
971  // TODO make sure numTargetBodyParameters is greater than 0
972  coeffTarget.resize(2,numTargetBodyParameters);
973  }
974 
975  // clear N12, n1, and nj
976  N12.clear();
977  n1.clear();
978  m_RHS.clear();
979 
980  // clear static matrices
981  coeffPoint3D.clear();
982  coeffRHS.clear();
983  N22.clear();
984  n2.clear();
985 
986  // loop over 3D points
987  int numGood3DPoints = 0;
988  int numRejected3DPoints = 0;
989  int pointIndex = 0;
990  int num3DPoints = m_bundleControlPoints.size();
991 
992  printf("\n");
993 
994  for (int i = 0; i < num3DPoints; i++) {
995 
996  BundleControlPointQsp point = m_bundleControlPoints.at(i);
997 
998  if (point->isRejected()) {
999  numRejected3DPoints++;
1000 
1001  pointIndex++;
1002  continue;
1003  }
1004 
1005  if ( i != 0 ) {
1006  N22.clear();
1007  N12.wipe();
1008  n2.clear();
1009  }
1010 
1011  // loop over measures for this point
1012  int numMeasures = point->size();
1013  for (int j = 0; j < numMeasures; j++) {
1014  BundleMeasureQsp measure = point->at(j);
1015 
1016  // flagged as "JigsawFail" implies this measure has been rejected
1017  // TODO IsRejected is obsolete -- replace code or add to ControlMeasure
1018  if (measure->isRejected()) {
1019  continue;
1020  }
1021 
1022  status = computePartials(coeffTarget, coeffImage, coeffPoint3D, coeffRHS, *measure,
1023  *point);
1024 
1025  if (!status) {
1026  // TODO should status be set back to true? JAM
1027  // TODO this measure should be flagged as rejected.
1028  continue;
1029  }
1030 
1031  // update number of observations
1032  int numObs = m_bundleResults.numberObservations();
1033  m_bundleResults.setNumberObservations(numObs + 2);
1034  formMeasureNormals(N22, N12, n1, n2, coeffTarget, coeffImage, coeffPoint3D, coeffRHS,
1035  measure->observationIndex());
1036  } // end loop over this points measures
1037 
1038  formPointNormals(N22, N12, n2, m_RHS, point);
1039 
1040  pointIndex++;
1041 
1042  numGood3DPoints++;
1043 
1044  } // end loop over 3D points
1045 
1046  // finally, form the reduced normal equations
1047 
1048  formWeightedNormals(n1, m_RHS);
1049 
1050  // update number of unknown parameters
1051  m_bundleResults.setNumberUnknownParameters(m_rank + 3 * numGood3DPoints);
1052 
1053  return status;
1054 }
1055 
1056 
1076  bool BundleAdjust::formMeasureNormals(symmetric_matrix<double, upper>&N22,
1078  compressed_vector<double> &n1,
1079  vector<double> &n2,
1080  matrix<double> &coeffTarget,
1081  matrix<double> &coeffImage,
1082  matrix<double> &coeffPoint3D,
1083  vector<double> &coeffRHS,
1084  int observationIndex) {
1085 
1086  static symmetric_matrix<double, upper> N11;
1087  static matrix<double> N11TargetImage;
1088 
1089  int blockIndex = observationIndex;
1090 
1091  // if we are solving for target body parameters
1092  int numTargetPartials = coeffTarget.size2();
1093  if (m_bundleSettings->solveTargetBody()) {
1094  blockIndex++;
1095 
1096  static vector<double> n1Target(numTargetPartials);
1097  n1Target.resize(numTargetPartials);
1098  n1Target.clear();
1099 
1100  // form N11 (normals for target body)
1101  N11.resize(numTargetPartials);
1102  N11.clear();
1103 
1104  N11 = prod(trans(coeffTarget), coeffTarget);
1105 
1106  // insert submatrix at column, row
1107  m_sparseNormals.insertMatrixBlock(0, 0, numTargetPartials, numTargetPartials);
1108 
1109  (*(*m_sparseNormals[0])[0]) += N11;
1110 
1111  // form portion of N11 between target and image
1112  N11TargetImage.resize(numTargetPartials, coeffImage.size2());
1113  N11TargetImage.clear();
1114  N11TargetImage = prod(trans(coeffTarget),coeffImage);
1115 
1116  m_sparseNormals.insertMatrixBlock(observationIndex+1, 0,
1117  numTargetPartials, coeffImage.size2());
1118  (*(*m_sparseNormals[observationIndex+1])[0]) += N11TargetImage;
1119 
1120  // form N12 target portion
1121  static matrix<double> N12Target(numTargetPartials, 3);
1122  N12Target.clear();
1123 
1124  N12Target = prod(trans(coeffTarget), coeffPoint3D);
1125 
1126  // insert N12Target into N12
1127  N12.insertMatrixBlock(0, numTargetPartials, 3);
1128  *N12[0] += N12Target;
1129 
1130  // form n1Target
1131  n1Target = prod(trans(coeffTarget), coeffRHS);
1132 
1133  // insert n1Target into n1
1134  for (int i = 0; i < numTargetPartials; i++) {
1135  n1(i) += n1Target(i);
1136  }
1137  }
1138 
1139 
1140 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ below is ok (2015-06-03)
1141 // TODO - if solving for target (and/or self-cal) have to use not observationIndex below but
1142 // observationIndex plus 1 or 2
1143 
1144  int numImagePartials = coeffImage.size2();
1145 
1146  static LinearAlgebra::Vector n1Image(numImagePartials);
1147  n1Image.resize(numImagePartials);
1148  n1Image.clear();
1149 
1150  // form N11 (normals for photo)
1151  N11.resize(numImagePartials);
1152  N11.clear();
1153 
1154  N11 = prod(trans(coeffImage), coeffImage);
1155 
1156  int t = 0;
1157  //testing
1158  for (int a = 0; a < observationIndex; a++) {
1159  BundleObservationQsp observation = m_bundleObservations.at(a);
1160  t += observation->numberParameters();
1161  }
1162  // account for target parameters
1163  t += numTargetPartials;
1164 
1165  // insert submatrix at column, row
1166  m_sparseNormals.insertMatrixBlock(blockIndex, blockIndex,
1167  numImagePartials, numImagePartials);
1168 
1169  (*(*m_sparseNormals[blockIndex])[blockIndex]) += N11;
1170 
1171  // form N12Image
1172  static matrix<double> N12Image(numImagePartials, 3);
1173  N12Image.resize(numImagePartials, 3);
1174  N12Image.clear();
1175 
1176  N12Image = prod(trans(coeffImage), coeffPoint3D);
1177 
1178  // insert N12Image into N12
1179  N12.insertMatrixBlock(blockIndex, numImagePartials, 3);
1180  *N12[blockIndex] += N12Image;
1181 
1182  // form n1
1183  n1Image = prod(trans(coeffImage), coeffRHS);
1184 
1185  // insert n1Image into n1
1186  // TODO - MUST ACCOUNT FOR TARGET BODY PARAMETERS
1187  // WHEN INSERTING INTO n1 HERE!!!!!
1188  for (int i = 0; i < numImagePartials; i++) {
1189  n1(i + t) += n1Image(i);
1190  }
1191 
1192  // form N22
1193  N22 += prod(trans(coeffPoint3D), coeffPoint3D);
1194 
1195  // form n2
1196  n2 += prod(trans(coeffPoint3D), coeffRHS);
1197 
1198  return true;
1199  }
1200 
1201 
1219  bool BundleAdjust::formPointNormals(symmetric_matrix<double, upper>&N22,
1221  vector<double> &n2,
1222  vector<double> &nj,
1223  BundleControlPointQsp &bundleControlPoint) {
1224 
1225  boost::numeric::ublas::bounded_vector<double, 3> &NIC = bundleControlPoint->nicVector();
1226  SparseBlockRowMatrix &Q = bundleControlPoint->cholmodQMatrix();
1227 
1228  NIC.clear();
1229  Q.zeroBlocks();
1230 
1231  // weighting of 3D point parameters
1232  boost::numeric::ublas::bounded_vector<double, 3> &weights
1233  = bundleControlPoint->weights();
1234  boost::numeric::ublas::bounded_vector<double, 3> &corrections
1235  = bundleControlPoint->corrections();
1236 
1237  if (weights(0) > 0.0) {
1238  N22(0,0) += weights(0);
1239  n2(0) += (-weights(0) * corrections(0));
1240  m_bundleResults.incrementNumberConstrainedPointParameters(1);
1241  }
1242 
1243  if (weights(1) > 0.0) {
1244  N22(1,1) += weights(1);
1245  n2(1) += (-weights(1) * corrections(1));
1246  m_bundleResults.incrementNumberConstrainedPointParameters(1);
1247  }
1248 
1249  if (weights(2) > 0.0) {
1250  N22(2,2) += weights(2);
1251  n2(2) += (-weights(2) * corrections(2));
1252  m_bundleResults.incrementNumberConstrainedPointParameters(1);
1253  }
1254 
1255  // invert N22
1256  invert3x3(N22);
1257 
1258  // save upper triangular covariance matrix for error propagation
1259  SurfacePoint SurfacePoint = bundleControlPoint->adjustedSurfacePoint();
1260  SurfacePoint.SetSphericalMatrix(N22);
1261  bundleControlPoint->setAdjustedSurfacePoint(SurfacePoint);
1262 
1263  // form Q (this is N22{-1} * N12{T})
1264  // Q = prod(N22, trans(N12));
1265  productATransB(N22, N12, Q);
1266 
1267  // form product of N22(inverse) and n2; store in NIC
1268  NIC = prod(N22, n2);
1269 
1270  // accumulate -R directly into reduced normal equations
1271  productAB(N12, Q);
1272 
1273  // accumulate -nj
1274  // nj -= prod(trans(Q),n2);
1275  accumProductAlphaAB(-1.0, Q, n2, nj);
1276 
1277  return true;
1278  }
1279 
1280 
1292  bool BundleAdjust::formWeightedNormals(compressed_vector<double> &n1,
1293  vector<double> &nj) {
1294 
1295  m_bundleResults.resetNumberConstrainedImageParameters();
1296 
1297  int n = 0;
1298 
1299  for (int i = 0; i < m_sparseNormals.size(); i++) {
1300  LinearAlgebra::Matrix *diagonalBlock = m_sparseNormals.getBlock(i, i);
1301  if ( !diagonalBlock )
1302  continue;
1303 
1304  if (m_bundleSettings->solveTargetBody() && i == 0) {
1305  m_bundleResults.resetNumberConstrainedTargetParameters();
1306 
1307  // get parameter weights for target body
1308  vector<double> weights = m_bundleTargetBody->parameterWeights();
1309  vector<double> corrections = m_bundleTargetBody->parameterCorrections();
1310 
1311  int blockSize = diagonalBlock->size1();
1312  for (int j = 0; j < blockSize; j++) {
1313  if (weights[j] > 0.0) {
1314  (*diagonalBlock)(j,j) += weights[j];
1315  nj[n] -= weights[j] * corrections(j);
1316  m_bundleResults.incrementNumberConstrainedTargetParameters(1);
1317  }
1318  n++;
1319  }
1320  }
1321  else {
1322  BundleObservationQsp observation;
1323 
1324  // get parameter weights for this observation
1325  if (m_bundleSettings->solveTargetBody()) {
1326  observation = m_bundleObservations.at(i-1);
1327  }
1328  else {
1329  observation = m_bundleObservations.at(i);
1330  }
1331 
1332  LinearAlgebra::Vector weights = observation->parameterWeights();
1333  LinearAlgebra::Vector corrections = observation->parameterCorrections();
1334 
1335  int blockSize = diagonalBlock->size1();
1336  for (int j = 0; j < blockSize; j++) {
1337  if (weights(j) > 0.0) {
1338  (*diagonalBlock)(j,j) += weights(j);
1339  nj[n] -= weights(j) * corrections(j);
1340  m_bundleResults.incrementNumberConstrainedImageParameters(1);
1341  }
1342  n++;
1343  }
1344  }
1345  }
1346 
1347  // add n1 to nj
1348  nj += n1;
1349 
1350  return true;
1351  }
1352 
1353 
1362  void BundleAdjust::productAlphaAV(double alpha, bounded_vector<double,3> &v2,
1364  vector<double> &v1) {
1365 
1366  QMapIterator< int, LinearAlgebra::Matrix * > Qit(Q);
1367 
1368  int subrangeStart, subrangeEnd;
1369 
1370  while ( Qit.hasNext() ) {
1371  Qit.next();
1372 
1373  int columnIndex = Qit.key();
1374 
1375  subrangeStart = m_sparseNormals.getLeadingColumnsForBlock(columnIndex);
1376  subrangeEnd = subrangeStart + Qit.value()->size2();
1377 
1378  v2 += alpha * prod(*(Qit.value()),subrange(v1,subrangeStart,subrangeEnd));
1379  }
1380  }
1381 
1382 
1392  bool BundleAdjust::productATransB(symmetric_matrix <double,upper> &N22,
1394  SparseBlockRowMatrix &Q) {
1395 
1396  QMapIterator< int, LinearAlgebra::Matrix * > N12it(N12);
1397 
1398  while ( N12it.hasNext() ) {
1399  N12it.next();
1400 
1401  int rowIndex = N12it.key();
1402 
1403  // insert submatrix in Q at block "rowIndex"
1404  Q.insertMatrixBlock(rowIndex, 3, N12it.value()->size1());
1405 
1406  *(Q[rowIndex]) = prod(N22,trans(*(N12it.value())));
1407  }
1408 
1409  return true;
1410  }
1411 
1412 
1422  void BundleAdjust::productAB(SparseBlockColumnMatrix &N12,
1423  SparseBlockRowMatrix &Q) {
1424  // iterators for N12 and Q
1425  QMapIterator<int, LinearAlgebra::Matrix*> N12it(N12);
1426  QMapIterator<int, LinearAlgebra::Matrix*> Qit(Q);
1427 
1428  // now multiply blocks and subtract from m_sparseNormals
1429  while ( N12it.hasNext() ) {
1430  N12it.next();
1431 
1432  int rowIndex = N12it.key();
1433  LinearAlgebra::Matrix *N12block = N12it.value();
1434 
1435  while ( Qit.hasNext() ) {
1436  Qit.next();
1437 
1438  int columnIndex = Qit.key();
1439 
1440  if ( rowIndex > columnIndex ) {
1441  continue;
1442  }
1443 
1444  LinearAlgebra::Matrix *Qblock = Qit.value();
1445 
1446  // insert submatrix at column, row
1447  m_sparseNormals.insertMatrixBlock(columnIndex, rowIndex,
1448  N12block->size1(), Qblock->size2());
1449 
1450  (*(*m_sparseNormals[columnIndex])[rowIndex]) -= prod(*N12block,*Qblock);
1451  }
1452  Qit.toFront();
1453  }
1454  }
1455 
1456 
1467  void BundleAdjust::accumProductAlphaAB(double alpha,
1469  vector<double> &n2,
1470  vector<double> &nj) {
1471 
1472  if (alpha == 0.0) {
1473  return;
1474  }
1475 
1476  int numTargetParameters = m_bundleSettings->numberTargetBodyParameters();
1477 
1478  QMapIterator<int, LinearAlgebra::Matrix*> Qit(Q);
1479 
1480  while ( Qit.hasNext() ) {
1481  Qit.next();
1482 
1483  int columnIndex = Qit.key();
1484  LinearAlgebra::Matrix *Qblock = Qit.value();
1485 
1486  LinearAlgebra::Vector blockProduct = prod(trans(*Qblock),n2);
1487 
1488  int numParams = 0;
1489  for (int observationIndex = 0; observationIndex < columnIndex; observationIndex++) {
1490  if (numTargetParameters > 0 && observationIndex == 0) {
1491  numParams += numTargetParameters;
1492  }
1493  else {
1494  if (numTargetParameters > 0 ) {
1495  BundleObservationQsp observation = m_bundleObservations.at(observationIndex-1);
1496  numParams += observation->numberParameters();
1497  }
1498  else {
1499  BundleObservationQsp observation = m_bundleObservations.at(observationIndex);
1500  numParams += observation->numberParameters();
1501  }
1502  }
1503  }
1504 
1505  for (unsigned i = 0; i < blockProduct.size(); i++) {
1506  nj(numParams+i) += alpha*blockProduct(i);
1507  }
1508  }
1509  }
1510 
1511 
1521  bool BundleAdjust::solveSystem() {
1522 
1523  // load cholmod triplet
1524  if ( !loadCholmodTriplet() ) {
1525  QString msg = "CHOLMOD: Failed to load Triplet matrix";
1526  throw IException(IException::Programmer, msg, _FILEINFO_);
1527  }
1528 
1529  // convert triplet to sparse matrix
1530  m_cholmodNormal = cholmod_triplet_to_sparse(m_cholmodTriplet,
1531  m_cholmodTriplet->nnz,
1532  &m_cholmodCommon);
1533 
1534  // analyze matrix
1535  // TODO should we analyze just 1st iteration?
1536  m_L = cholmod_analyze(m_cholmodNormal, &m_cholmodCommon);
1537 
1538  // create cholmod cholesky factor
1539  // CHOLMOD will choose LLT or LDLT decomposition based on the characteristics of the matrix.
1540  cholmod_factorize(m_cholmodNormal, m_L, &m_cholmodCommon);
1541 
1542  // check for "matrix not positive definite" error
1543  if (m_cholmodCommon.status == CHOLMOD_NOT_POSDEF) {
1544  QString msg = "Matrix NOT positive-definite: failure at column " + toString((int) m_L->minor);
1545 // throw IException(IException::User, msg, _FILEINFO_);
1546  error(msg);
1547  emit(finished());
1548  return false;
1549  }
1550 
1551  // cholmod solution and right-hand side vectors
1552  cholmod_dense *x, *b;
1553 
1554  // initialize right-hand side vector
1555  b = cholmod_zeros(m_cholmodNormal->nrow, 1, m_cholmodNormal->xtype, &m_cholmodCommon);
1556 
1557  // copy right-hand side vector into b
1558  double *px = (double*)b->x;
1559  for (int i = 0; i < m_rank; i++) {
1560  px[i] = m_RHS[i];
1561  }
1562 
1563  // cholmod solve
1564  x = cholmod_solve(CHOLMOD_A, m_L, b, &m_cholmodCommon);
1565 
1566  // copy solution vector x out into m_imageSolution
1567  double *sx = (double*)x->x;
1568  for (int i = 0; i < m_rank; i++) {
1569  m_imageSolution[i] = sx[i];
1570  }
1571 
1572  // free cholmod structures
1573  cholmod_free_sparse(&m_cholmodNormal, &m_cholmodCommon);
1574  cholmod_free_dense(&b, &m_cholmodCommon);
1575  cholmod_free_dense(&x, &m_cholmodCommon);
1576 
1577  return true;
1578  }
1579 
1580 
1592  bool BundleAdjust::loadCholmodTriplet() {
1593 
1594  if ( m_iteration == 1 ) {
1595  int numElements = m_sparseNormals.numberOfElements();
1596  m_cholmodTriplet = cholmod_allocate_triplet(m_rank, m_rank, numElements,
1597  -1, CHOLMOD_REAL, &m_cholmodCommon);
1598 
1599  if ( !m_cholmodTriplet ) {
1600  printf("Triplet allocation failure");
1601  return false;
1602  }
1603 
1604  m_cholmodTriplet->nnz = 0;
1605  }
1606 
1607  int *tripletColumns = (int*) m_cholmodTriplet->i;
1608  int *tripletRows = (int*) m_cholmodTriplet->j;
1609  double *tripletValues = (double*)m_cholmodTriplet->x;
1610 
1611  double entryValue;
1612 
1613  int numEntries = 0;
1614 
1615  int numBlockcolumns = m_sparseNormals.size();
1616  for (int columnIndex = 0; columnIndex < numBlockcolumns; columnIndex++) {
1617 
1618  SparseBlockColumnMatrix *normalsColumn = m_sparseNormals[columnIndex];
1619 
1620  if ( !normalsColumn ) {
1621  printf("SparseBlockColumnMatrix retrieval failure at column %d", columnIndex);
1622  return false;
1623  }
1624 
1625  int numLeadingColumns = m_sparseNormals.getLeadingColumnsForBlock(columnIndex);
1626 
1627  QMapIterator< int, LinearAlgebra::Matrix * > it(*normalsColumn);
1628 
1629  while ( it.hasNext() ) {
1630  it.next();
1631 
1632  int rowIndex = it.key();
1633 
1634  int numLeadingRows = m_sparseNormals.getLeadingRowsForBlock(rowIndex);
1635 
1636  LinearAlgebra::Matrix *normalsBlock = it.value();
1637  if ( !normalsBlock ) {
1638  printf("matrix block retrieval failure at column %d, row %d", columnIndex, rowIndex);
1639  printf("Total # of block columns: %d", numBlockcolumns);
1640  printf("Total # of blocks: %d", m_sparseNormals.numberOfBlocks());
1641  return false;
1642  }
1643 
1644  if ( columnIndex == rowIndex ) { // diagonal block (upper-triangular)
1645  for (unsigned ii = 0; ii < normalsBlock->size1(); ii++) {
1646  for (unsigned jj = ii; jj < normalsBlock->size2(); jj++) {
1647  entryValue = normalsBlock->at_element(ii,jj);
1648  int entryColumnIndex = jj + numLeadingColumns;
1649  int entryRowIndex = ii + numLeadingRows;
1650 
1651  if ( m_iteration == 1 ) {
1652  tripletColumns[numEntries] = entryColumnIndex;
1653  tripletRows[numEntries] = entryRowIndex;
1654  m_cholmodTriplet->nnz++;
1655  }
1656 
1657  tripletValues[numEntries] = entryValue;
1658 
1659  numEntries++;
1660  }
1661  }
1662  }
1663  else { // off-diagonal block (square)
1664  for (unsigned ii = 0; ii < normalsBlock->size1(); ii++) {
1665  for (unsigned jj = 0; jj < normalsBlock->size2(); jj++) {
1666  entryValue = normalsBlock->at_element(ii,jj);
1667  int entryColumnIndex = jj + numLeadingColumns;
1668  int entryRowIndex = ii + numLeadingRows;
1669 
1670  if ( m_iteration ==1 ) {
1671  tripletColumns[numEntries] = entryRowIndex;
1672  tripletRows[numEntries] = entryColumnIndex;
1673  m_cholmodTriplet->nnz++;
1674  }
1675 
1676  tripletValues[numEntries] = entryValue;
1677 
1678  numEntries++;
1679  }
1680  }
1681  }
1682  }
1683  }
1684 
1685  return true;
1686  }
1687 
1688 
1697  bool BundleAdjust::cholmodInverse() {
1698  int i, j;
1699 
1700  // allocate matrix inverse
1701  m_normalInverse.resize(m_rank);
1702 
1703  cholmod_dense *x; // solution vector
1704  cholmod_dense *b; // right-hand side (column vectors of identity)
1705 
1706  b = cholmod_zeros ( m_rank, 1, CHOLMOD_REAL, &m_cholmodCommon ) ;
1707  double *pb = (double*)b->x;
1708 
1709  double *px = NULL;
1710 
1711  for (i = 0; i < m_rank; i++) {
1712  if ( i > 0 ) {
1713  pb[i-1] = 0.0;
1714  }
1715  pb[i] = 1.0;
1716 
1717  x = cholmod_solve ( CHOLMOD_A, m_L, b, &m_cholmodCommon ) ;
1718  px = (double*)x->x;
1719 
1720  // store solution in corresponding column of inverse
1721  for (j = 0; j <= i; j++) {
1722  m_normalInverse(j, i) = px[j];
1723  }
1724 
1725  cholmod_free_dense(&x,&m_cholmodCommon);
1726  }
1727 
1728  cholmod_free_dense(&b,&m_cholmodCommon);
1729 
1730  return true;
1731  }
1732 
1733 
1746  bool BundleAdjust::invert3x3(symmetric_matrix<double, upper> &m) {
1747  double det;
1748  double den;
1749 
1750  boost::numeric::ublas::symmetric_matrix< double, upper > c = m;
1751 
1752  den = m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1))
1753  - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0))
1754  + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0));
1755 
1756  // check for divide by zero
1757  if (fabs(den) < 1.0e-100) {
1758  return false;
1759  }
1760 
1761  det = 1.0 / den;
1762 
1763  m(0, 0) = (c(1, 1) * c(2, 2) - c(1, 2) * c(2, 1)) * det;
1764  m(0, 1) = (c(0, 2) * c(2, 1) - c(0, 1) * c(2, 2)) * det;
1765  m(0, 2) = (c(0, 1) * c(1, 2) - c(0, 2) * c(1, 1)) * det;
1766  m(1, 1) = (c(0, 0) * c(2, 2) - c(0, 2) * c(2, 0)) * det;
1767  m(1, 2) = (c(0, 2) * c(1, 0) - c(0, 0) * c(1, 2)) * det;
1768  m(2, 2) = (c(0, 0) * c(1, 1) - c(0, 1) * c(1, 0)) * det;
1769 
1770  return true;
1771  }
1772 
1773 
1793  bool BundleAdjust::computePartials(matrix<double> &coeffTarget,
1794  matrix<double> &coeffImage,
1795  matrix<double> &coeffPoint3D,
1796  vector<double> &coeffRHS,
1797  BundleMeasure &measure,
1798  BundleControlPoint &point) {
1799 
1800  // additional vectors
1801  std::vector<double> lookBWRTLat;
1802  std::vector<double> lookBWRTLon;
1803  std::vector<double> lookBWRTRad;
1804 
1805  Camera *measureCamera = NULL;
1806 
1807  double measuredX, computedX, measuredY, computedY;
1808  double deltaX, deltaY;
1809  double observationSigma;
1810  double observationWeight;
1811 
1812  measureCamera = measure.camera();
1813 
1814  const BundleObservationSolveSettingsQsp observationSolveSettings =
1815  measure.observationSolveSettings();
1816  BundleObservationQsp observation = measure.parentBundleObservation();
1817 
1818  int numImagePartials = observation->numberParameters();
1819  coeffImage.resize(2,numImagePartials);
1820 
1821  // clear partial derivative matrices and vectors
1822  if (m_bundleSettings->solveTargetBody()) {
1823  coeffTarget.clear();
1824  }
1825 
1826  coeffImage.clear();
1827  coeffPoint3D.clear();
1828  coeffRHS.clear();
1829 
1830  // no need to call SetImage for framing camera ( CameraType = 0 )
1831  if (measureCamera->GetCameraType() != 0) {
1832  // Set the Spice to the measured point
1833  // TODO - can we explain this better?
1834  measureCamera->SetImage(measure.sample(), measure.line());
1835  }
1836 
1837  // REMOVE
1838  SurfacePoint surfacePoint = point.adjustedSurfacePoint();
1839  // REMOVE
1840 
1841  // Compute the look vector in instrument coordinates based on time of observation and apriori
1842  // lat/lon/radius
1843  if (!(measureCamera->GroundMap()->GetXY(point.adjustedSurfacePoint(),
1844  &computedX, &computedY))) {
1845  QString msg = "Unable to map apriori surface point for measure ";
1846  msg += measure.cubeSerialNumber() + " on point " + point.id() + " into focal plane";
1847  throw IException(IException::User, msg, _FILEINFO_);
1848  }
1849 
1850  // partials for fixed point w/r lat, long, radius in Body-Fixed
1851  lookBWRTLat = measureCamera->GroundMap()->PointPartial(point.adjustedSurfacePoint(),
1852  CameraGroundMap::WRT_Latitude);
1853  lookBWRTLon = measureCamera->GroundMap()->PointPartial(point.adjustedSurfacePoint(),
1854  CameraGroundMap::WRT_Longitude);
1855  lookBWRTRad = measureCamera->GroundMap()->PointPartial(point.adjustedSurfacePoint(),
1856  CameraGroundMap::WRT_Radius);
1857 
1858  int index = 0;
1859  if (m_bundleSettings->solveTargetBody() && m_bundleSettings->solvePoleRA()) {
1860  measureCamera->GroundMap()->GetdXYdTOrientation(SpiceRotation::WRT_RightAscension, 0,
1861  &coeffTarget(0, index),
1862  &coeffTarget(1, index));
1863  index++;
1864  }
1865 
1866  if (m_bundleSettings->solveTargetBody() && m_bundleSettings->solvePoleRAVelocity()) {
1867  measureCamera->GroundMap()->GetdXYdTOrientation(SpiceRotation::WRT_RightAscension, 1,
1868  &coeffTarget(0, index),
1869  &coeffTarget(1, index));
1870  index++;
1871  }
1872 
1873  if (m_bundleSettings->solveTargetBody() && m_bundleSettings->solvePoleDec()) {
1874  measureCamera->GroundMap()->GetdXYdTOrientation(SpiceRotation::WRT_Declination, 0,
1875  &coeffTarget(0, index),
1876  &coeffTarget(1, index));
1877  index++;
1878  }
1879 
1880  if (m_bundleSettings->solveTargetBody() && m_bundleSettings->solvePoleDecVelocity()) {
1881  measureCamera->GroundMap()->GetdXYdTOrientation(SpiceRotation::WRT_Declination, 1,
1882  &coeffTarget(0, index),
1883  &coeffTarget(1, index));
1884  index++;
1885  }
1886 
1887  if (m_bundleSettings->solveTargetBody() && m_bundleSettings->solvePM()) {
1888  measureCamera->GroundMap()->GetdXYdTOrientation(SpiceRotation::WRT_Twist, 0,
1889  &coeffTarget(0, index),
1890  &coeffTarget(1, index));
1891  index++;
1892  }
1893 
1894  if (m_bundleSettings->solveTargetBody() && m_bundleSettings->solvePMVelocity()) {
1895  measureCamera->GroundMap()->GetdXYdTOrientation(SpiceRotation::WRT_Twist, 1,
1896  &coeffTarget(0, index),
1897  &coeffTarget(1, index));
1898  index++;
1899  }
1900 
1901  if (m_bundleSettings->solveTargetBody() && m_bundleTargetBody->solveMeanRadius()) {
1902  std::vector<double> lookBWRTMeanRadius =
1903  measureCamera->GroundMap()->MeanRadiusPartial(surfacePoint,
1904  m_bundleTargetBody->meanRadius());
1905 
1906  measureCamera->GroundMap()->GetdXYdPoint(lookBWRTMeanRadius, &coeffTarget(0, index),
1907  &coeffTarget(1, index));
1908  index++;
1909  }
1910 
1911  if (m_bundleSettings->solveTargetBody() && m_bundleTargetBody->solveTriaxialRadii()) {
1912 
1913  std::vector<double> lookBWRTRadiusA =
1914  measureCamera->GroundMap()->EllipsoidPartial(surfacePoint,
1915  CameraGroundMap::WRT_MajorAxis);
1916 
1917  measureCamera->GroundMap()->GetdXYdPoint(lookBWRTRadiusA, &coeffTarget(0, index),
1918  &coeffTarget(1, index));
1919  index++;
1920 
1921  std::vector<double> lookBWRTRadiusB =
1922  measureCamera->GroundMap()->EllipsoidPartial(surfacePoint,
1923  CameraGroundMap::WRT_MinorAxis);
1924 
1925  measureCamera->GroundMap()->GetdXYdPoint(lookBWRTRadiusB, &coeffTarget(0, index),
1926  &coeffTarget(1, index));
1927  index++;
1928 
1929  std::vector<double> lookBWRTRadiusC =
1930  measureCamera->GroundMap()->EllipsoidPartial(surfacePoint,
1931  CameraGroundMap::WRT_PolarAxis);
1932 
1933  measureCamera->GroundMap()->GetdXYdPoint(lookBWRTRadiusC, &coeffTarget(0, index),
1934  &coeffTarget(1, index));
1935  index++;
1936  }
1937 
1938  index = 0;
1939 
1940  if (observationSolveSettings->instrumentPositionSolveOption() !=
1941  BundleObservationSolveSettings::NoPositionFactors) {
1942 
1943  int numCamPositionCoefficients =
1944  observationSolveSettings->numberCameraPositionCoefficientsSolved();
1945 
1946  // Add the partial for the x coordinate of the position (differentiating
1947  // point(x,y,z) - spacecraftPosition(x,y,z) in J2000
1948  for (int cameraCoef = 0; cameraCoef < numCamPositionCoefficients; cameraCoef++) {
1949  measureCamera->GroundMap()->GetdXYdPosition(SpicePosition::WRT_X, cameraCoef,
1950  &coeffImage(0, index),
1951  &coeffImage(1, index));
1952  index++;
1953  }
1954 
1955  // Add the partial for the y coordinate of the position
1956  for (int cameraCoef = 0; cameraCoef < numCamPositionCoefficients; cameraCoef++) {
1957  measureCamera->GroundMap()->GetdXYdPosition(SpicePosition::WRT_Y, cameraCoef,
1958  &coeffImage(0, index),
1959  &coeffImage(1, index));
1960  index++;
1961  }
1962 
1963  // Add the partial for the z coordinate of the position
1964  for (int cameraCoef = 0; cameraCoef < numCamPositionCoefficients; cameraCoef++) {
1965  measureCamera->GroundMap()->GetdXYdPosition(SpicePosition::WRT_Z, cameraCoef,
1966  &coeffImage(0, index),
1967  &coeffImage(1, index));
1968  index++;
1969  }
1970 
1971  }
1972 
1973  if (observationSolveSettings->instrumentPointingSolveOption() !=
1974  BundleObservationSolveSettings::NoPointingFactors) {
1975 
1976  int numCamAngleCoefficients =
1977  observationSolveSettings->numberCameraAngleCoefficientsSolved();
1978 
1979  // Add the partials for ra
1980  for (int cameraCoef = 0; cameraCoef < numCamAngleCoefficients; cameraCoef++) {
1981  measureCamera->GroundMap()->GetdXYdOrientation(SpiceRotation::WRT_RightAscension,
1982  cameraCoef, &coeffImage(0, index),
1983  &coeffImage(1, index));
1984  index++;
1985  }
1986 
1987  // Add the partials for dec
1988  for (int cameraCoef = 0; cameraCoef < numCamAngleCoefficients; cameraCoef++) {
1989  measureCamera->GroundMap()->GetdXYdOrientation(SpiceRotation::WRT_Declination,
1990  cameraCoef, &coeffImage(0, index),
1991  &coeffImage(1, index));
1992  index++;
1993  }
1994 
1995  // Add the partial for twist if necessary
1996  if (observationSolveSettings->solveTwist()) {
1997  for (int cameraCoef = 0; cameraCoef < numCamAngleCoefficients; cameraCoef++) {
1998  measureCamera->GroundMap()->GetdXYdOrientation(SpiceRotation::WRT_Twist,
1999  cameraCoef, &coeffImage(0, index),
2000  &coeffImage(1, index));
2001  index++;
2002  }
2003  }
2004  }
2005 
2006  // partials for 3D point
2007  measureCamera->GroundMap()->GetdXYdPoint(lookBWRTLat,
2008  &coeffPoint3D(0, 0),
2009  &coeffPoint3D(1, 0));
2010  measureCamera->GroundMap()->GetdXYdPoint(lookBWRTLon,
2011  &coeffPoint3D(0, 1),
2012  &coeffPoint3D(1, 1));
2013  measureCamera->GroundMap()->GetdXYdPoint(lookBWRTRad,
2014  &coeffPoint3D(0, 2),
2015  &coeffPoint3D(1, 2));
2016 
2017  // right-hand side (measured - computed)
2018  measuredX = measure.focalPlaneMeasuredX();
2019  measuredY = measure.focalPlaneMeasuredY();
2020 
2021  deltaX = measuredX - computedX;
2022  deltaY = measuredY - computedY;
2023 
2024  coeffRHS(0) = deltaX;
2025  coeffRHS(1) = deltaY;
2026 
2027  // residual prob distribution is calculated even if there is no maximum likelihood estimation
2028  double obsValue = deltaX / measureCamera->PixelPitch();
2029  m_bundleResults.addResidualsProbabilityDistributionObservation(obsValue);
2030 
2031  obsValue = deltaY / measureCamera->PixelPitch();
2032  m_bundleResults.addResidualsProbabilityDistributionObservation(obsValue);
2033 
2034  observationSigma = 1.4 * measureCamera->PixelPitch();
2035  observationWeight = 1.0 / observationSigma;
2036 
2037  if (m_bundleResults.numberMaximumLikelihoodModels()
2038  > m_bundleResults.maximumLikelihoodModelIndex()) {
2039  // if maximum likelihood estimation is being used
2040  double residualR2ZScore
2041  = sqrt(deltaX * deltaX + deltaY * deltaY) / observationSigma / sqrt(2.0);
2042  //dynamically build the cumulative probability distribution of the R^2 residual Z Scores
2043  m_bundleResults.addProbabilityDistributionObservation(residualR2ZScore);
2044  int currentModelIndex = m_bundleResults.maximumLikelihoodModelIndex();
2045  observationWeight *= m_bundleResults.maximumLikelihoodModelWFunc(currentModelIndex)
2046  .sqrtWeightScaler(residualR2ZScore);
2047  }
2048 
2049  // multiply coefficients by observation weight
2050  coeffImage *= observationWeight;
2051  coeffPoint3D *= observationWeight;
2052  coeffRHS *= observationWeight;
2053 
2054  if (m_bundleSettings->solveTargetBody()) {
2055  coeffTarget *= observationWeight;
2056  }
2057 
2058  return true;
2059  }
2060 
2061 
2065  void BundleAdjust::applyParameterCorrections() {
2066 
2067  int t = 0;
2068 
2069  // TODO - update target body parameters if in solution
2070  // note these come before BundleObservation parameters in normal equations matrix
2071  if (m_bundleSettings->solveTargetBody()) {
2072  int numTargetBodyParameters = m_bundleTargetBody->numberParameters();
2073 
2074  m_bundleTargetBody->applyParameterCorrections(subrange(m_imageSolution,0,
2075  numTargetBodyParameters));
2076 
2077  t += numTargetBodyParameters;
2078  }
2079 
2080  // Update spice for each BundleObservation
2081  int numObservations = m_bundleObservations.size();
2082  for (int i = 0; i < numObservations; i++) {
2083  BundleObservationQsp observation = m_bundleObservations.at(i);
2084 
2085  int numParameters = observation->numberParameters();
2086 
2087  observation->applyParameterCorrections(subrange(m_imageSolution,t,t+numParameters));
2088 
2089  if (m_bundleSettings->solveTargetBody()) {
2090  observation->updateBodyRotation();
2091  }
2092 
2093  t += numParameters;
2094  }
2095 
2096  // TODO: CHECK - do we need point index in case of rejected points????
2097 
2098  // TODO: Below code should move into BundleControlPoint->updateParameterCorrections
2099  // except, what about the productAlphaAV method?
2100 
2101  // Update lat/lon for each control point
2102  double latCorrection, lonCorrection, radCorrection;
2103  int pointIndex = 0;
2104  int numControlPoints = m_bundleControlPoints.size();
2105  for (int i = 0; i < numControlPoints; i++) {
2106  BundleControlPointQsp point = m_bundleControlPoints.at(i);
2107 
2108  if (point->isRejected()) {
2109  pointIndex++;
2110  continue;
2111  }
2112 
2113  // get NIC, Q, and correction vector for this point
2114  boost::numeric::ublas::bounded_vector< double, 3 > &NIC = point->nicVector();
2115  SparseBlockRowMatrix &Q = point->cholmodQMatrix();
2116  boost::numeric::ublas::bounded_vector< double, 3 > &corrections = point->corrections();
2117 
2118  // subtract product of Q and nj from NIC
2119  // NIC -= prod(Q, m_imageSolution);
2120  productAlphaAV(-1.0, NIC, Q, m_imageSolution);
2121 
2122  // get point parameter corrections
2123  latCorrection = NIC(0);
2124  lonCorrection = NIC(1);
2125  radCorrection = NIC(2);
2126 
2127  SurfacePoint surfacepoint = point->adjustedSurfacePoint();
2128 
2129  double pointLat = surfacepoint.GetLatitude().degrees();
2130  double pointLon = surfacepoint.GetLongitude().degrees();
2131  double pointRad = surfacepoint.GetLocalRadius().meters();
2132 
2133  pointLat += RAD2DEG * latCorrection;
2134  pointLon += RAD2DEG * lonCorrection;
2135 
2136  // Make sure updated values are still in valid range.
2137  // TODO What is the valid lon range?
2138  if (pointLat < -90.0) {
2139  pointLat = -180.0 - pointLat;
2140  pointLon = pointLon + 180.0;
2141  }
2142  if (pointLat > 90.0) {
2143  pointLat = 180.0 - pointLat;
2144  pointLon = pointLon + 180.0;
2145  }
2146  while (pointLon > 360.0) {
2147  pointLon = pointLon - 360.0;
2148  }
2149  while (pointLon < 0.0) {
2150  pointLon = pointLon + 360.0;
2151  }
2152 
2153  pointRad += 1000.*radCorrection;
2154 
2155  // sum and save corrections
2156  corrections(0) += latCorrection;
2157  corrections(1) += lonCorrection;
2158  corrections(2) += radCorrection;
2159 
2160  // ken testing - if solving for target body mean radius, set radius to current
2161  // mean radius value
2162  if (m_bundleTargetBody && (m_bundleTargetBody->solveMeanRadius()
2163  || m_bundleTargetBody->solveTriaxialRadii())) {
2164  if (m_bundleTargetBody->solveMeanRadius()) {
2165  surfacepoint.SetSphericalCoordinates(Latitude(pointLat, Angle::Degrees),
2166  Longitude(pointLon, Angle::Degrees),
2167  m_bundleTargetBody->meanRadius());
2168  }
2169  else if (m_bundleTargetBody->solveTriaxialRadii()) {
2170  Distance localRadius = m_bundleTargetBody->
2171  localRadius(Latitude(pointLat, Angle::Degrees),
2172  Longitude(pointLon, Angle::Degrees));
2173  surfacepoint.SetSphericalCoordinates(Latitude(pointLat, Angle::Degrees),
2174  Longitude(pointLon, Angle::Degrees),
2175  localRadius);
2176  }
2177  }
2178  else {
2179  surfacepoint.SetSphericalCoordinates(Latitude(pointLat, Angle::Degrees),
2180  Longitude(pointLon, Angle::Degrees),
2181  Distance(pointRad, Distance::Meters));
2182  }
2183 
2184  point->setAdjustedSurfacePoint(surfacepoint);
2185 
2186  pointIndex++;
2187 
2188  } // end loop over point corrections
2189  }
2190 
2191 
2203  double BundleAdjust::computeResiduals() {
2204  double vtpv = 0.0;
2205  double vtpvControl = 0.0;
2206  double vtpvImage = 0.0;
2207  double weight;
2208  double v, vx, vy;
2209 
2210  // clear residual stats vectors
2211  m_xResiduals.Reset();
2212  m_yResiduals.Reset();
2213  m_xyResiduals.Reset();
2214 
2215  // vtpv for image coordinates
2216  int numObjectPoints = m_bundleControlPoints.size();
2217 
2218  for (int i = 0; i < numObjectPoints; i++) {
2219 
2220  BundleControlPointQsp point = m_bundleControlPoints.at(i);
2221 
2222  point->computeResiduals();
2223 
2224  int numMeasures = point->numberOfMeasures();
2225  for (int j = 0; j < numMeasures; j++) {
2226  const BundleMeasureQsp measure = point->at(j);
2227 
2228  weight = 1.4 * (measure->camera())->PixelPitch();
2229  weight = 1.0 / weight;
2230  weight *= weight;
2231 
2232  vx = measure->focalPlaneMeasuredX() - measure->focalPlaneComputedX();
2233  vy = measure->focalPlaneMeasuredY() - measure->focalPlaneComputedY();
2234 
2235  // if rejected, don't include in statistics
2236  if (measure->isRejected()) {
2237  continue;
2238  }
2239 
2240  m_xResiduals.AddData(vx);
2241  m_yResiduals.AddData(vy);
2242  m_xyResiduals.AddData(vx);
2243  m_xyResiduals.AddData(vy);
2244 
2245  vtpv += vx * vx * weight + vy * vy * weight;
2246  }
2247  }
2248 
2249  // add vtpv from constrained 3D points
2250  int pointIndex = 0;
2251  for (int i = 0; i < numObjectPoints; i++) {
2252  BundleControlPointQsp bundleControlPoint = m_bundleControlPoints.at(i);
2253 
2254  // get weight and correction vector for this point
2255  boost::numeric::ublas::bounded_vector<double, 3> weights = bundleControlPoint->weights();
2256  boost::numeric::ublas::bounded_vector<double, 3> corrections =
2257  bundleControlPoint->corrections();
2258 
2259  if ( weights(0) > 0.0 ) {
2260  vtpvControl += corrections(0) * corrections(0) * weights(0);
2261  }
2262  if ( weights(1) > 0.0 ) {
2263  vtpvControl += corrections(1) * corrections(1) * weights(1);
2264  }
2265  if ( weights(2) > 0.0 ) {
2266  vtpvControl += corrections(2) * corrections(2) * weights(2);
2267  }
2268 
2269  pointIndex++;
2270  }
2271 
2272  // add vtpv from constrained image parameters
2273  for (int i = 0; i < m_bundleObservations.size(); i++) {
2274  BundleObservationQsp observation = m_bundleObservations.at(i);
2275 
2276  // get weight and correction vector for this observation
2277  const LinearAlgebra::Vector &weights = observation->parameterWeights();
2278  const LinearAlgebra::Vector &corrections = observation->parameterCorrections();
2279 
2280  for (int j = 0; j < (int)corrections.size(); j++) {
2281  if (weights[j] > 0.0) {
2282  v = corrections[j];
2283  vtpvImage += v * v * weights[j];
2284  }
2285  }
2286  }
2287 
2288  // TODO - add vtpv from constrained target body parameters
2289  double vtpvTargetBody = 0.0;
2290  if ( m_bundleTargetBody) {
2291  vtpvTargetBody = m_bundleTargetBody->vtpv();
2292  }
2293 
2294  vtpv = vtpv + vtpvControl + vtpvImage + vtpvTargetBody;
2295 
2296  // Compute rms for all image coordinate residuals
2297  // separately for x, y, then x and y together
2298  m_bundleResults.setRmsXYResiduals(m_xResiduals.Rms(), m_yResiduals.Rms(), m_xyResiduals.Rms());
2299 
2300  return vtpv;
2301  }
2302 
2303 
2310  bool BundleAdjust::wrapUp() {
2311  // compute residuals in pixels
2312 
2313  // vtpv for image coordinates
2314  for (int i = 0; i < m_bundleControlPoints.size(); i++) {
2315  BundleControlPointQsp point = m_bundleControlPoints.at(i);
2316  point->computeResiduals();
2317  }
2318 
2319  computeBundleStatistics();
2320 
2321  return true;
2322  }
2323 
2324 
2339  bool BundleAdjust::computeRejectionLimit() {
2340  double vx, vy;
2341 
2342  int numResiduals = m_bundleResults.numberObservations() / 2;
2343 
2344  std::vector<double> residuals;
2345 
2346  residuals.resize(numResiduals);
2347 
2348  // load magnitude (squared) of residual vector
2349  int residualIndex = 0;
2350  int numObjectPoints = m_bundleControlPoints.size();
2351  for (int i = 0; i < numObjectPoints; i++) {
2352 
2353  const BundleControlPointQsp point = m_bundleControlPoints.at(i);
2354 
2355  if ( point->isRejected() ) {
2356  continue;
2357  }
2358 
2359  int numMeasures = point->numberOfMeasures();
2360  for (int j = 0; j < numMeasures; j++) {
2361 
2362  const BundleMeasureQsp measure = point->at(j);
2363 
2364  if ( measure->isRejected() ) {
2365  continue;
2366  }
2367 
2368  vx = measure->sampleResidual();
2369  vy = measure->lineResidual();
2370 
2371  residuals[residualIndex] = sqrt(vx*vx + vy*vy);
2372 
2373  residualIndex++;
2374  }
2375  }
2376 
2377  // sort vectors
2378  std::sort(residuals.begin(), residuals.end());
2379 
2380  double median;
2381  double medianDev;
2382  double mad;
2383 
2384  int midpointIndex = numResiduals / 2;
2385 
2386  if ( numResiduals % 2 == 0 ) {
2387  median = (residuals[midpointIndex - 1] + residuals[midpointIndex]) / 2;
2388  }
2389  else {
2390  median = residuals[midpointIndex];
2391  }
2392 
2393  // compute M.A.D.
2394  for (int i = 0; i < numResiduals; i++) {
2395  residuals[i] = fabs(residuals[i] - median);
2396  }
2397 
2398  std::sort(residuals.begin(), residuals.end());
2399 
2400  if ( numResiduals % 2 == 0 ) {
2401  medianDev = (residuals[midpointIndex - 1] + residuals[midpointIndex]) / 2;
2402  }
2403  else {
2404  medianDev = residuals[midpointIndex];
2405  }
2406 
2407  std::cout << "median deviation: " << medianDev << std::endl;
2408 
2409  mad = 1.4826 * medianDev;
2410 
2411  std::cout << "mad: " << mad << "\n";
2412 
2413  m_bundleResults.setRejectionLimit(median
2414  + m_bundleSettings->outlierRejectionMultiplier() * mad);
2415 
2416  std::cout << "Rejection Limit: " << m_bundleResults.rejectionLimit() << std::endl;
2417 
2418  return true;
2419  }
2420 
2421 
2429  bool BundleAdjust::flagOutliers() {
2430  double vx, vy;
2431  int numRejected;
2432  int totalNumRejected = 0;
2433 
2434  int maxResidualIndex;
2435  double maxResidual;
2436  double sumSquares;
2437  double usedRejectionLimit = m_bundleResults.rejectionLimit();
2438 
2439  // TODO What to do if usedRejectionLimit is too low?
2440 
2441  int numComingBack = 0;
2442 
2443  int numObjectPoints = m_bundleControlPoints.size();
2444  for (int i = 0; i < numObjectPoints; i++) {
2445  BundleControlPointQsp point = m_bundleControlPoints.at(i);
2446 
2447  point->zeroNumberOfRejectedMeasures();
2448 
2449  numRejected = 0;
2450  maxResidualIndex = -1;
2451  maxResidual = -1.0;
2452 
2453  int numMeasures = point->numberOfMeasures();
2454  for (int j = 0; j < numMeasures; j++) {
2455 
2456  BundleMeasureQsp measure = point->at(j);
2457 
2458  vx = measure->sampleResidual();
2459  vy = measure->lineResidual();
2460 
2461  sumSquares = sqrt(vx*vx + vy*vy);
2462 
2463  // measure is good
2464  if ( sumSquares <= usedRejectionLimit ) {
2465 
2466  // was it previously rejected?
2467  if ( measure->isRejected() ) {
2468  printf("Coming back in: %s\r",point->id().toLatin1().data());
2469  numComingBack++;
2470  m_controlNet->DecrementNumberOfRejectedMeasuresInImage(measure->cubeSerialNumber());
2471  }
2472 
2473  measure->setRejected(false);
2474  continue;
2475  }
2476 
2477  // if it's still rejected, skip it
2478  if ( measure->isRejected() ) {
2479  numRejected++;
2480  totalNumRejected++;
2481  continue;
2482  }
2483 
2484  if ( sumSquares > maxResidual ) {
2485  maxResidual = sumSquares;
2486  maxResidualIndex = j;
2487  }
2488  }
2489 
2490  // no observations above the current rejection limit for this 3D point
2491  if ( maxResidual == -1.0 || maxResidual <= usedRejectionLimit ) {
2492  point->setNumberOfRejectedMeasures(numRejected);
2493  continue;
2494  }
2495 
2496  // this is another kluge - if we only have two observations
2497  // we won't reject (for now)
2498  if ((numMeasures - (numRejected + 1)) < 2) {
2499  point->setNumberOfRejectedMeasures(numRejected);
2500  continue;
2501  }
2502 
2503  // otherwise, we have at least one observation
2504  // for this point whose residual is above the
2505  // current rejection limit - we'll flag the
2506  // worst of these as rejected
2507  BundleMeasureQsp rejected = point->at(maxResidualIndex);
2508  rejected->setRejected(true);
2509  numRejected++;
2510  point->setNumberOfRejectedMeasures(numRejected);
2511  m_controlNet->IncrementNumberOfRejectedMeasuresInImage(rejected->cubeSerialNumber());
2512  totalNumRejected++;
2513 
2514  // do we still have sufficient remaining observations for this 3D point?
2515  if ( ( numMeasures-numRejected ) < 2 ) {
2516  point->setRejected(true);
2517  printf("Rejecting Entire Point: %s\r",point->id().toLatin1().data());
2518  }
2519  else
2520  point->setRejected(false);
2521 
2522  }
2523 
2524  int numberRejectedObservations = 2*totalNumRejected;
2525 
2526  printf("\nRejected Observations:%10d (Rejection Limit:%12.5lf)\n",
2527  numberRejectedObservations, usedRejectionLimit);
2528  m_bundleResults.setNumberRejectedObservations(numberRejectedObservations);
2529 
2530  std::cout << "Measures that came back: " << numComingBack << "\n" << std::endl;
2531 
2532  return true;
2533  }
2534 
2535 
2550  bool BundleAdjust::errorPropagation() {
2551 
2552  // free unneeded memory
2553  cholmod_free_triplet(&m_cholmodTriplet, &m_cholmodCommon);
2554  cholmod_free_sparse(&m_cholmodNormal, &m_cholmodCommon);
2555 
2556  LinearAlgebra::Matrix T(3, 3);
2557  double sigmaLat, sigmaLon, sigmaRad;
2558  double t;
2559 
2560  double sigma0Squared = m_bundleResults.sigma0() * m_bundleResults.sigma0();
2561 
2562  int numObjectPoints = m_bundleControlPoints.size();
2563 
2564  std::string currentTime = iTime::CurrentLocalTime().toLatin1().data();
2565  printf(" Time: %s\n\n", currentTime.c_str());
2566 
2567  // create and initialize array of 3x3 matrices for all object points
2568  std::vector< symmetric_matrix<double> > pointCovariances(numObjectPoints,
2569  symmetric_matrix<double>(3));
2570  for (int d = 0; d < numObjectPoints; d++) {
2571  pointCovariances[d].clear();
2572  }
2573 
2574  cholmod_dense *x; // solution vector
2575  cholmod_dense *b; // right-hand side (column vectors of identity)
2576 
2577  b = cholmod_zeros ( m_rank, 1, CHOLMOD_REAL, &m_cholmodCommon );
2578  double *pb = (double*)b->x;
2579 
2580  double *px = NULL;
2581 
2582  SparseBlockColumnMatrix inverseMatrix;
2583 
2584  // Create unique file name
2585  FileName matrixFile(m_bundleSettings->outputFilePrefix() + "inverseMatrix.dat");
2586  //???FileName matrixFile = FileName::createTempFile(m_bundleSettings.outputFilePrefix()
2587  //??? + "inverseMatrix.dat");
2588  // Create file handle
2589  QFile matrixOutput(matrixFile.expanded());
2590 
2591  // Check to see if creating the inverse correlation matrix is turned on
2592  if (m_bundleSettings->createInverseMatrix()) {
2593  // Open file to write to
2594  matrixOutput.open(QIODevice::WriteOnly);
2595  }
2596  QDataStream outStream(&matrixOutput);
2597 
2598  int i, j, k;
2599  int columnIndex = 0;
2600  int numColumns = 0;
2601  int numBlockColumns = m_sparseNormals.size();
2602 
2603  for (i = 0; i < numBlockColumns; i++) {
2604 
2605  // columns in this column block
2606  SparseBlockColumnMatrix *normalsColumn = m_sparseNormals.at(i);
2607  if (i == 0) {
2608  numColumns = normalsColumn->numberOfColumns();
2609  int numRows = m_sparseNormals.at(i)->numberOfRows();
2610  inverseMatrix.insertMatrixBlock(i, numRows, numColumns);
2611  inverseMatrix.zeroBlocks();
2612  }
2613  else {
2614  if (normalsColumn->numberOfColumns() == numColumns) {
2615  int numRows = m_sparseNormals.at(i)->numberOfRows();
2616  inverseMatrix.insertMatrixBlock(i, numRows, numColumns);
2617  inverseMatrix.zeroBlocks();
2618  }
2619  else {
2620  numColumns = normalsColumn->numberOfColumns();
2621 
2622  // reset inverseMatrix
2623  inverseMatrix.wipe();
2624 
2625  // insert blocks
2626  for (j = 0; j < (i+1); j++) {
2627  SparseBlockColumnMatrix *normalsRow = m_sparseNormals.at(j);
2628  int numRows = normalsRow->numberOfRows();
2629 
2630  inverseMatrix.insertMatrixBlock(j, numRows, numColumns);
2631  }
2632  }
2633  }
2634 
2635  int localCol = 0;
2636 
2637  // solve for inverse for nCols
2638  for (j = 0; j < numColumns; j++) {
2639  if ( columnIndex > 0 ) {
2640  pb[columnIndex- 1] = 0.0;
2641  }
2642  pb[columnIndex] = 1.0;
2643 
2644  x = cholmod_solve ( CHOLMOD_A, m_L, b, &m_cholmodCommon );
2645  px = (double*)x->x;
2646  int rp = 0;
2647 
2648  // store solution in corresponding column of inverse
2649  for (k = 0; k < inverseMatrix.size(); k++) {
2650  LinearAlgebra::Matrix *matrix = inverseMatrix.value(k);
2651 
2652  int sz1 = matrix->size1();
2653 
2654  for (int ii = 0; ii < sz1; ii++) {
2655  (*matrix)(ii,localCol) = px[ii + rp];
2656  }
2657  rp += matrix->size1();
2658  }
2659 
2660  columnIndex++;
2661  localCol++;
2662 
2663  cholmod_free_dense(&x,&m_cholmodCommon);
2664  }
2665 
2666  // save adjusted target body sigmas if solving for target
2667  if (m_bundleSettings->solveTargetBody() && i == 0) {
2668  vector< double > &adjustedSigmas = m_bundleTargetBody->adjustedSigmas();
2669  matrix< double > *targetCovMatrix = inverseMatrix.value(i);
2670 
2671  for (int z = 0; z < numColumns; z++)
2672  adjustedSigmas[z] = sqrt((*targetCovMatrix)(z,z))*m_bundleResults.sigma0();
2673  }
2674  // save adjusted image sigmas
2675  else {
2676  BundleObservationQsp observation;
2677  if (m_bundleSettings->solveTargetBody()) {
2678  observation = m_bundleObservations.at(i-1);
2679  }
2680  else {
2681  observation = m_bundleObservations.at(i);
2682  }
2683  vector< double > &adjustedSigmas = observation->adjustedSigmas();
2684  matrix< double > *imageCovMatrix = inverseMatrix.value(i);
2685  for ( int z = 0; z < numColumns; z++) {
2686  adjustedSigmas[z] = sqrt((*imageCovMatrix)(z,z))*m_bundleResults.sigma0();
2687  }
2688  }
2689 
2690  // Output the inverse matrix if requested
2691  if (m_bundleSettings->createInverseMatrix()) {
2692  outStream << inverseMatrix;
2693  }
2694 
2695  // now loop over all object points to sum contributions into 3x3 point covariance matrix
2696  int pointIndex = 0;
2697  for (j = 0; j < numObjectPoints; j++) {
2698 
2699  BundleControlPointQsp point = m_bundleControlPoints.at(pointIndex);
2700  if ( point->isRejected() ) {
2701  continue;
2702  }
2703 
2704  // only update point every 100 points
2705  if (j%100 == 0) {
2706  printf("\rError Propagation: Inverse Block %8d of %8d; Point %8d of %8d", i+1,
2707  numBlockColumns, j+1, numObjectPoints);
2708 
2709  emit iterationUpdate(i+1, j+1);
2710  }
2711 
2712  // get corresponding Q matrix
2713  SparseBlockRowMatrix Q = point->cholmodQMatrix();
2714 
2715  T.clear();
2716 
2717  // get corresponding point covariance matrix
2718  boost::numeric::ublas::symmetric_matrix<double> &covariance = pointCovariances[pointIndex];
2719 
2720  // get firstQBlock - index i is the key into Q for firstQBlock
2721  LinearAlgebra::Matrix *firstQBlock = Q.value(i);
2722  if (!firstQBlock) {
2723  pointIndex++;
2724  continue;
2725  }
2726 
2727  // iterate over Q
2728  // secondQBlock is current map value
2729  QMapIterator< int, LinearAlgebra::Matrix * > it(Q);
2730  while ( it.hasNext() ) {
2731  it.next();
2732 
2733  int nKey = it.key();
2734 
2735  if (it.key() > i) {
2736  break;
2737  }
2738 
2739  LinearAlgebra::Matrix *secondQBlock = it.value();
2740 
2741  if ( !secondQBlock ) {// should never be NULL
2742  continue;
2743  }
2744 
2745  LinearAlgebra::Matrix *inverseBlock = inverseMatrix.value(it.key());
2746 
2747  if ( !inverseBlock ) {// should never be NULL
2748  continue;
2749  }
2750 
2751  T = prod(*inverseBlock, trans(*firstQBlock));
2752  T = prod(*secondQBlock,T);
2753 
2754  if (nKey != i) {
2755  T += trans(T);
2756  }
2757 
2758  try {
2759  covariance += T;
2760  }
2761 
2762  catch (std::exception &e) {
2763  printf("\n\n");
2764  QString msg = "Input data and settings are not sufficiently stable "
2765  "for error propagation.";
2766  throw IException(IException::User, msg, _FILEINFO_);
2767  }
2768  }
2769  pointIndex++;
2770  }
2771  }
2772 
2773  if (m_bundleSettings->createInverseMatrix()) {
2774  // Close the file.
2775  matrixOutput.close();
2776  // Save the location of the "covariance" matrix
2777  m_bundleResults.setCorrMatCovFileName(matrixFile);
2778  }
2779 
2780  // can free sparse normals now
2781  m_sparseNormals.wipe();
2782 
2783  // free b (right-hand side vector
2784  cholmod_free_dense(&b,&m_cholmodCommon);
2785 
2786  printf("\n\n");
2787  currentTime = Isis::iTime::CurrentLocalTime().toLatin1().data();
2788  printf("\rFilling point covariance matrices: Time %s", currentTime.c_str());
2789  printf("\n\n");
2790 
2791  // now loop over points again and set final covariance stuff
2792  int pointIndex = 0;
2793  for (j = 0; j < numObjectPoints; j++) {
2794 
2795  BundleControlPointQsp point = m_bundleControlPoints.at(pointIndex);
2796 
2797  if ( point->isRejected() ) {
2798  continue;
2799  }
2800 
2801  if (j%100 == 0) {
2802  printf("\rError Propagation: Filling point covariance matrices %8d of %8d\r",j+1,
2803  numObjectPoints);
2804  }
2805 
2806  // get corresponding point covariance matrix
2807  boost::numeric::ublas::symmetric_matrix<double> &covariance = pointCovariances[pointIndex];
2808 
2809  // Ask Ken what is happening here...Setting just the sigmas is not very accurate
2810  // Shouldn't we be updating and setting the matrix??? TODO
2811  SurfacePoint SurfacePoint = point->adjustedSurfacePoint();
2812 
2813  sigmaLat = SurfacePoint.GetLatSigma().radians();
2814  sigmaLon = SurfacePoint.GetLonSigma().radians();
2815  sigmaRad = SurfacePoint.GetLocalRadiusSigma().meters();
2816 
2817  t = sigmaLat * sigmaLat + covariance(0, 0);
2818  Distance latSigmaDistance(sqrt(sigma0Squared * t) * m_radiansToMeters, Distance::Meters);
2819 
2820  t = sigmaLon * sigmaLon + covariance(1, 1);
2821  t = sqrt(sigma0Squared * t) * m_radiansToMeters;
2822  Distance lonSigmaDistance(
2823  t * cos(point->adjustedSurfacePoint().GetLatitude().radians()),
2824  Distance::Meters);
2825 
2826  t = sigmaRad*sigmaRad + covariance(2, 2);
2827  t = sqrt(sigma0Squared * t) * 1000.0;
2828 
2829  SurfacePoint.SetSphericalSigmasDistance(latSigmaDistance, lonSigmaDistance,
2830  Distance(t, Distance::Meters));
2831 
2832  point->setAdjustedSurfacePoint(SurfacePoint);
2833 
2834  pointIndex++;
2835  }
2836 
2837  return true;
2838  }
2839 
2840 
2846  ControlNetQsp BundleAdjust::controlNet() {
2847  return m_controlNet;
2848  }
2849 
2850 
2856  SerialNumberList *BundleAdjust::serialNumberList() {
2857  return m_serialNumberList;
2858  }
2859 
2860 
2866  int BundleAdjust::numberOfImages() const {
2867  return m_serialNumberList->size();
2868  }
2869 
2870 
2878  // TODO: probably don't need this, can get from BundleObservation
2879  QString BundleAdjust::fileName(int i) {
2880  return m_serialNumberList->fileName(i);
2881  }
2882 
2883 
2889  double BundleAdjust::iteration() const {
2890  return m_iteration;
2891  }
2892 
2893 
2901  Table BundleAdjust::cMatrix(int i) {
2902  return m_controlNet->Camera(i)->instrumentRotation()->Cache("InstrumentPointing");
2903  }
2904 
2913  Table BundleAdjust::spVector(int i) {
2914  return m_controlNet->Camera(i)->instrumentPosition()->Cache("InstrumentPosition");
2915  }
2916 
2917 
2926  void BundleAdjust::iterationSummary() {
2927  QString iterationNumber;
2928 
2929  if ( m_bundleResults.converged() ) {
2930  iterationNumber = "Iteration" + toString(m_iteration) + ": Final";
2931  }
2932  else {
2933  iterationNumber = "Iteration" + toString(m_iteration);
2934  }
2935 
2936  PvlGroup summaryGroup(iterationNumber);
2937 
2938  summaryGroup += PvlKeyword("Sigma0",
2939  toString( m_bundleResults.sigma0() ) );
2940  summaryGroup += PvlKeyword("Observations",
2941  toString( m_bundleResults.numberObservations() ) );
2942  summaryGroup += PvlKeyword("Constrained_Point_Parameters",
2943  toString( m_bundleResults.numberConstrainedPointParameters() ) );
2944  summaryGroup += PvlKeyword("Constrained_Image_Parameters",
2945  toString( m_bundleResults.numberConstrainedImageParameters() ) );
2946  if (m_bundleSettings->bundleTargetBody()) {
2947  summaryGroup += PvlKeyword("Constrained_Target_Parameters",
2948  toString( m_bundleResults.numberConstrainedTargetParameters() ) );
2949  }
2950  summaryGroup += PvlKeyword("Unknown_Parameters",
2951  toString( m_bundleResults.numberUnknownParameters() ) );
2952  summaryGroup += PvlKeyword("Degrees_of_Freedom",
2953  toString( m_bundleResults.degreesOfFreedom() ) );
2954  summaryGroup += PvlKeyword( "Rejected_Measures",
2955  toString( m_bundleResults.numberRejectedObservations()/2) );
2956 
2957  if ( m_bundleResults.numberMaximumLikelihoodModels() >
2958  m_bundleResults.maximumLikelihoodModelIndex() ) {
2959  // if maximum likelihood estimation is being used
2960 
2961  summaryGroup += PvlKeyword("Maximum_Likelihood_Tier: ",
2962  toString( m_bundleResults.maximumLikelihoodModelIndex() ) );
2963  summaryGroup += PvlKeyword("Median_of_R^2_residuals: ",
2964  toString( m_bundleResults.maximumLikelihoodMedianR2Residuals() ) );
2965  }
2966 
2967  if ( m_bundleResults.converged() ) {
2968  summaryGroup += PvlKeyword("Converged", "TRUE");
2969  summaryGroup += PvlKeyword("TotalElapsedTime", toString( m_bundleResults.elapsedTime() ) );
2970 
2971  if (m_bundleSettings->errorPropagation()) {
2972  summaryGroup += PvlKeyword("ErrorPropagationElapsedTime",
2973  toString( m_bundleResults.elapsedTimeErrorProp() ) );
2974  }
2975  }
2976 
2977  std::ostringstream ostr;
2978  ostr << summaryGroup << std::endl;
2979  m_iterationSummary += QString::fromStdString( ostr.str() );
2980  if (m_printSummary) {
2981  Application::Log(summaryGroup);
2982  }
2983  }
2984 
2985 
2991  bool BundleAdjust::isConverged() {
2992  return m_bundleResults.converged();
2993  }
2994 
2995 
3003  QString BundleAdjust::iterationSummaryGroup() const {
3004  return m_iterationSummary;
3005  }
3006 
3007 
3017  void BundleAdjust::outputBundleStatus(QString status) {
3018  status += "\n";
3019  printf("%s", status.toStdString().c_str());
3020  }
3021 
3022 
3023 
3059  bool BundleAdjust::computeBundleStatistics() {
3060 
3061  // use qvectors so that we can set the size.
3062  // this will be useful later when adding data.
3063  // data may added out of index order
3064  int numberImages = m_serialNumberList->size();
3065  QVector<Statistics> rmsImageSampleResiduals(numberImages);
3066  QVector<Statistics> rmsImageLineResiduals(numberImages);
3067  QVector<Statistics> rmsImageResiduals(numberImages);
3068 
3069  int numObjectPoints = m_bundleControlPoints.size();
3070  for (int i = 0; i < numObjectPoints; i++) {
3071 
3072  const BundleControlPointQsp point = m_bundleControlPoints.at(i);
3073 
3074  if (point->isRejected()) {
3075  continue;
3076  }
3077 
3078  int numMeasures = point->numberOfMeasures();
3079  for (int j = 0; j < numMeasures; j++) {
3080 
3081  const BundleMeasureQsp measure = point->at(j);
3082 
3083  if (measure->isRejected()) {
3084  continue;
3085  }
3086 
3087  double sampleResidual = fabs(measure->sampleResidual());
3088  double lineResidual = fabs(measure->lineResidual());
3089 
3090  // Determine the index for this measure's serial number
3091  int imageIndex = m_serialNumberList->serialNumberIndex(measure->cubeSerialNumber());
3092 
3093  // add residual data to the statistics object at the appropriate serial number index
3094  rmsImageSampleResiduals[imageIndex].AddData(sampleResidual);
3095  rmsImageLineResiduals[imageIndex].AddData(lineResidual);
3096  rmsImageResiduals[imageIndex].AddData(lineResidual);
3097  rmsImageResiduals[imageIndex].AddData(sampleResidual);
3098  }
3099  }
3100 
3101 
3102  if (m_bundleSettings->errorPropagation()) {
3103 
3104  // initialize lat/lon/rad boundaries
3105  Distance minSigmaLatDist;
3106  QString minSigmaLatPointId = "";
3107 
3108  Distance maxSigmaLatDist;
3109  QString maxSigmaLatPointId = "";
3110 
3111  Distance minSigmaLonDist;
3112  QString minSigmaLonPointId = "";
3113 
3114  Distance maxSigmaLonDist;
3115  QString maxSigmaLonPointId = "";
3116 
3117  Distance minSigmaRadDist;
3118  QString minSigmaRadPointId = "";
3119 
3120  Distance maxSigmaRadDist;
3121  QString maxSigmaRadPointId = "";
3122 
3123  // compute stats for point sigmas
3124  Statistics sigmaLatStats;
3125  Statistics sigmaLonStats;
3126  Statistics sigmaRadStats;
3127 
3128  Distance sigmaLatDist, sigmaLonDist, sigmaRadDist;
3129 
3130  int numPoints = m_bundleControlPoints.size();
3131  // initialize max and min values to those from first valid point
3132  for (int i = 0; i < numPoints; i++) {
3133 
3134  const BundleControlPointQsp point = m_bundleControlPoints.at(i);
3135 
3136  maxSigmaLatDist = point->adjustedSurfacePoint().GetLatSigmaDistance();;
3137  minSigmaLatDist = maxSigmaLatDist;
3138 
3139  maxSigmaLonDist = point->adjustedSurfacePoint().GetLonSigmaDistance();;
3140  minSigmaLonDist = maxSigmaLonDist;
3141 
3142  maxSigmaLatPointId = point->id();
3143  maxSigmaLonPointId = maxSigmaLatPointId;
3144  minSigmaLatPointId = maxSigmaLatPointId;
3145  minSigmaLonPointId = maxSigmaLatPointId;
3146 
3147  if (m_bundleSettings->solveRadius()) {
3148  maxSigmaRadDist = point->adjustedSurfacePoint().GetLocalRadiusSigma();
3149  minSigmaRadDist = maxSigmaRadDist;
3150 
3151  maxSigmaRadPointId = maxSigmaLatPointId;
3152  minSigmaRadPointId = maxSigmaLatPointId;
3153  }
3154  break;
3155  }
3156 
3157  for (int i = 0; i < numPoints; i++) {
3158 
3159  const BundleControlPointQsp point = m_bundleControlPoints.at(i);
3160 
3161  sigmaLatDist = point->adjustedSurfacePoint().GetLatSigmaDistance();
3162  sigmaLonDist = point->adjustedSurfacePoint().GetLonSigmaDistance();
3163  sigmaRadDist = point->adjustedSurfacePoint().GetLocalRadiusSigma();
3164 
3165  sigmaLatStats.AddData(sigmaLatDist.meters());
3166  sigmaLonStats.AddData(sigmaLonDist.meters());
3167  sigmaRadStats.AddData(sigmaRadDist.meters());
3168 
3169  if (sigmaLatDist > maxSigmaLatDist) {
3170  maxSigmaLatDist = sigmaLatDist;
3171  maxSigmaLatPointId = point->id();
3172  }
3173  if (sigmaLonDist > maxSigmaLonDist) {
3174  maxSigmaLonDist = sigmaLonDist;
3175  maxSigmaLonPointId = point->id();
3176  }
3177  if (m_bundleSettings->solveRadius()) {
3178  if (sigmaRadDist > maxSigmaRadDist) {
3179  maxSigmaRadDist = sigmaRadDist;
3180  maxSigmaRadPointId = point->id();
3181  }
3182  }
3183  if (sigmaLatDist < minSigmaLatDist) {
3184  minSigmaLatDist = sigmaLatDist;
3185  minSigmaLatPointId = point->id();
3186  }
3187  if (sigmaLonDist < minSigmaLonDist) {
3188  minSigmaLonDist = sigmaLonDist;
3189  minSigmaLonPointId = point->id();
3190  }
3191  if (m_bundleSettings->solveRadius()) {
3192  if (sigmaRadDist < minSigmaRadDist) {
3193  minSigmaRadDist = sigmaRadDist;
3194  minSigmaRadPointId = point->id();
3195  }
3196  }
3197  }
3198 
3199  // update bundle results
3200  m_bundleResults.resizeSigmaStatisticsVectors(numberImages);
3201 
3202  m_bundleResults.setSigmaLatitudeRange(minSigmaLatDist, maxSigmaLatDist,
3203  minSigmaLatPointId, maxSigmaLatPointId);
3204 
3205  m_bundleResults.setSigmaLongitudeRange(minSigmaLonDist, maxSigmaLonDist,
3206  minSigmaLonPointId, maxSigmaLonPointId);
3207 
3208  m_bundleResults.setSigmaRadiusRange(minSigmaRadDist, maxSigmaRadDist,
3209  minSigmaRadPointId, maxSigmaRadPointId);
3210 
3211  m_bundleResults.setRmsFromSigmaStatistics(sigmaLatStats.Rms(),
3212  sigmaLonStats.Rms(),
3213  sigmaRadStats.Rms());
3214  }
3215  m_bundleResults.setRmsImageResidualLists(rmsImageLineResiduals.toList(),
3216  rmsImageSampleResiduals.toList(),
3217  rmsImageResiduals.toList());
3218 
3219  return true;
3220  }
3221 
3222 }
This class defines a body-fixed surface point.
Definition: SurfacePoint.h:86
This represents an ISIS control net in a project-based GUI interface.
Definition: Control.h:57
void setRunTime(QString runTime)
Sets the run time.
virtual CameraType GetCameraType() const =0
Returns the type of camera that was created.
std::vector< double > EllipsoidPartial(SurfacePoint spoint, PartialType raxis)
Compute derivative of focal plane coordinate w/r to one of the ellipsoidal radii (a, b, or c)
Internalizes a list of images and allows for operations on the entire list.
Definition: ImageList.h:44
QSharedPointer< BundleSettings > BundleSettingsQsp
Definition for a BundleSettingsQsp, a shared pointer to a BundleSettings object.
SparseBlockColumnMatrix.
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:519
double degrees() const
Get the angle in units of Degrees.
Definition: Angle.h:245
File name manipulation and expansion.
Definition: FileName.h:111
QSharedPointer< ControlNet > ControlNetQsp
This typedef is for future implementation of target body.
Definition: ControlNet.h:446
QSharedPointer< BundleObservation > parentBundleObservation()
Accesses the parent BundleObservation for this bundle measure.
Distance GetLocalRadius() const
Return the radius of the surface point.
Container class for BundleAdjustment results.
double radians() const
Convert an angle to a double.
Definition: Angle.h:239
Camera(Cube &cube)
Constructs the Camera object.
Definition: Camera.cpp:70
void add(const QString &filename, bool def2filename=false)
Adds a new filename / serial number pair to the SerialNumberList.
int size() const
How many serial number / filename combos are in the list.
QString fileName() const
Get the file name of the cube that this image represents.
Definition: Image.cpp:305
virtual bool GetXY(const SurfacePoint &spoint, double *cudx, double *cudy)
Compute undistorted focal plane coordinate from ground position using current Spice from SetImage cal...
static void cholmodErrorHandler(int nStatus, const char *file, int nLineNo, const char *message)
Custom error handler for CHOLMOD.
QSharedPointer< BundleObservationSolveSettings > BundleObservationSolveSettingsQsp
Definition for BundleObservationSolveSettingsQsp, a QSharedPointer to a &lt; BundleObservationSolveSet...
virtual bool GetdXYdTOrientation(const SpiceRotation::PartialType varType, int coefIndex, double *cudx, double *cudy)
Compute derivative of focal plane coordinate w/r to target body using current state.
void radii(Distance r[3]) const
Returns the radii of the body in km.
Definition: Spice.cpp:850
double Rms() const
Computes and returns the rms.
Definition: Statistics.cpp:382
double focalPlaneMeasuredX() const
Accesses the measured focal plane x value for this control measure //TODO verify? ...
void zeroBlocks()
Sets all elements of all matrix blocks to zero.
ErrorType errorType() const
Returns the source of the error for this exception.
Definition: IException.cpp:446
This class is designed to encapsulate the concept of a Latitude.
Definition: Latitude.h:59
double line() const
Accesses the current line measurement for this control measure.
Unless noted otherwise, the portions of Isis written by the USGS are public domain.
A container class for a ControlMeasure.
Definition: BundleMeasure.h:69
void ResetLocalRadius(const Distance &radius)
This method resets the local radius of a SurfacePoint.
boost::numeric::ublas::matrix< double > Matrix
Definition for an Isis::LinearAlgebra::Matrix of doubles.
QString toString(bool boolToConvert)
Global function to convert a boolean to a string.
Definition: IString.cpp:226
Unless noted otherwise, the portions of Isis written by the USGS are public domain.
Distance measurement, usually in meters.
Definition: Distance.h:47
QSharedPointer< BundleObservation > BundleObservationQsp
Typdef for BundleObservation QSharedPointer.
Longitude GetLongitude() const
Return the body-fixed longitude for the surface point.
void wipe()
Deletes all pointer elements and removes them from the map.
bool SetImage(const double sample, const double line)
Sets the sample/line values of the image to get the lat/lon values.
Definition: Camera.cpp:166
This class is used to accumulate statistics on double arrays.
Definition: Statistics.h:109
This class is designed to encapsulate the concept of a Longitude.
Definition: Longitude.h:52
This class hold image information that BundleAdjust needs to run correctly.Definition for a BundleIma...
Definition: BundleImage.h:51
boost::numeric::ublas::vector< double > Vector
Definition for an Isis::LinearAlgebra::Vector of doubles.
SurfacePoint adjustedSurfacePoint() const
Accesses the adjusted SurfacePoint associated with this BundleControlPoint.
Program progress reporter.
Definition: Progress.h:58
double sample() const
Accesses the current sample measurement for this control measure.
virtual bool GetdXYdPoint(std::vector< double > d_lookB, double *cudx, double *cudy)
Compute derivative of focal plane coordinate w/r to ground point using current state.
double meters() const
Get the distance in meters.
Definition: Distance.cpp:97
a control network
Definition: ControlNet.h:207
int numberOfRows()
Returns total number of rows in map (this needs to be clarified and maybe rewritten).
std::vector< double > MeanRadiusPartial(SurfacePoint spoint, Distance meanRadius)
Compute derivative of focal plane coordinate w/r to mean of the ellipsoidal radii (a...
CameraGroundMap * GroundMap()
Returns a pointer to the CameraGroundMap object.
Definition: Camera.cpp:2916
Latitude GetLatitude() const
Return the body-fixed latitude for the surface point.
double focalPlaneMeasuredY() const
Accesses the measured focal plane y value for this control measure //TODO verify? ...
virtual bool GetdXYdPosition(const SpicePosition::PartialType varType, int coefIndex, double *cudx, double *cudy)
Compute derivative w/r to position of focal plane coordinate from ground position using current Spice...
Contains multiple PvlContainers.
Definition: PvlGroup.h:57
bool insertMatrixBlock(int nRowBlock, int nRows, int nCols)
Inserts a &quot;newed&quot; LinearAlgebra::Matrix pointer of size (nRows, nCols) into the map with the block ro...
#define _FILEINFO_
Macro for the filename and line number.
Definition: IException.h:38
A single keyword-value pair.
Definition: PvlKeyword.h:98
This class holds information about a control point that BundleAdjust needs to run corretly...
A single control point.
Definition: ControlPoint.h:339
This represents a cube in a project-based GUI interface.
Definition: Image.h:91
QString fileName() const
Access the name of the control network file associated with this Control.
Definition: Control.cpp:171
QString cubeSerialNumber() const
Accesses the serial number of the cube containing this control measure.
const QSharedPointer< BundleObservationSolveSettings > observationSolveSettings()
Accesses the parent observation&#39;s solve settings.
virtual bool GetdXYdOrientation(const SpiceRotation::PartialType varType, int coefIndex, double *cudx, double *cudy)
Compute derivative of focal plane coordinate w/r to instrument using current state from SetImage call...
int numberOfColumns()
Returns total number of columns in map (NOTE: NOT the number of matrix blocks).
Unless noted otherwise, the portions of Isis written by the USGS are public domain.
Unless noted otherwise, the portions of Isis written by the USGS are public domain.
void SetSphericalMatrix(const boost::numeric::ublas::symmetric_matrix< double, boost::numeric::ublas::upper > &covar)
Set spherical covariance matrix.
Class for storing Table blobs information.
Definition: Table.h:74
double PixelPitch() const
Returns the pixel pitch.
Definition: Camera.cpp:2804
QSharedPointer< BundleMeasure > BundleMeasureQsp
Definition for BundleMeasureQsp, a shared pointer to a BundleMeasure.
Isis exception class.
Definition: IException.h:99
std::vector< double > PointPartial(SurfacePoint spoint, PartialType wrt)
Compute derivative with respect to indicated variable of conversion function from lat/lon/rad to rect...
bool insertMatrixBlock(int nColumnBlock, int nRows, int nCols)
Inserts a &quot;newed&quot; LinearAlgebra::Matrix pointer of size (nRows, nCols) into the map with the block co...
void SetSphericalSigmasDistance(const Distance &latSigma, const Distance &lonSigma, const Distance &radiusSigma)
Set the spherical sigmas (in meters) into the spherical variance/covariance matrix.
Camera * camera() const
Accesses the associated camera for this bundle measure.
QString id() const
Accesses the Point ID associated with this BundleControlPoint.
QSharedPointer< BundleControlPoint > BundleControlPointQsp
Definition for BundleControlPointQSP, a shared pointer to a BundleControlPoint.
void AddData(const double *data, const unsigned int count)
Add an array of doubles to the accumulators and counters.
Definition: Statistics.cpp:158
Serial Number list generator.
void SetSphericalCoordinates(const Latitude &lat, const Longitude &lon, const Distance &radius)
Update spherical coordinates (lat/lon/radius)
const double RAD2DEG(57.29577951308232087679815481)
Multiplier for converting from radians to degrees.
SparseBlockRowMatrix.
Unless noted otherwise, the portions of Isis written by the USGS are public domain.

U.S. Department of the Interior | U.S. Geological Survey
ISIS | Privacy & Disclaimers | Astrogeology Research Program
To contact us, please post comments and questions on the ISIS Support Center
File Modified: 07/12/2023 23:14:58