Now setting measure sigmas in BundleMeasure in init() method; retrieving sigma... (aa7cab2c) · Commits · aflab / astrogeology / ISIS3

isis/src/control/objs/BundleAdjust/BundleAdjust.cpp

+32 −54

Original line number	Diff line number	Diff line
		@@ -553,13 +553,14 @@ namespace Isis {

		measure->setParentObservation(observation);
		measure->setParentImage(image);
		measure->setMeasureSigma(1.4);
		}

		point->ComputeApriori();
		}

		// add BundleLidarControlPoints
		int numLidarPoints;
		int numLidarPoints = 0;
		if (m_lidarDataSet) {
		numLidarPoints = m_lidarDataSet->points().size();
		}
		@@ -586,6 +587,7 @@ namespace Isis {

		measure->setParentObservation(observation);
		measure->setParentImage(image);
		measure->setMeasureSigma(30.0*1.4);

		lidarPoint->ComputeApriori();
		}
		@@ -1262,10 +1264,6 @@ bool BundleAdjust::formNormalEquations() {
		for (int j = 0; j < numMeasures; j++) {
		BundleMeasureQsp measure = point->at(j);

		if (measure->parentControlPoint()->id() == "Lidar3768") {
		int fred=1;
		}

		// flagged as "JigsawFail" implies this measure has been rejected
		// TODO IsRejected is obsolete -- replace code or add to ControlMeasure
		if (measure->isRejected()) {
		@@ -1554,9 +1552,6 @@ bool BundleAdjust::formNormalEquations() {
		vector<double> &nj,
		BundleControlPointQsp &bundleControlPoint) {

		if (bundleControlPoint->id().contains("Lidar3304"))
		int fred=1;

		boost::numeric::ublas::bounded_vector<double, 3> &NIC = bundleControlPoint->nicVector();
		SparseBlockRowMatrix &Q = bundleControlPoint->qMatrix();

		@@ -2388,8 +2383,6 @@ bool BundleAdjust::formNormalEquations() {

		double measuredX, computedX, measuredY, computedY;
		double deltaX, deltaY;
		double observationSigma;
		double observationWeight;

		measureCamera = measure->camera();

		@@ -2557,15 +2550,8 @@ bool BundleAdjust::formNormalEquations() {
		obsValue = deltaY / measureCamera->PixelPitch();
		m_bundleResults.addResidualsProbabilityDistributionObservation(obsValue);

		// TODO: what if camera has been subsampled, is pixel pitch computation still valid?
		observationSigma = 1.4 * measureCamera->PixelPitch();

		// TODO: lidar kluge
		if (measure->parentControlPoint()->id().contains("Lidar")) {
		observationSigma *=30.0;
		}

		observationWeight = 1.0 / observationSigma;
		double observationSigma = measure->measureSigma();
		double observationWeightSqrt = measure->measureWeightSqrt();

		if (m_bundleResults.numberMaximumLikelihoodModels()
		> m_bundleResults.maximumLikelihoodModelIndex()) {
		@@ -2575,18 +2561,18 @@ bool BundleAdjust::formNormalEquations() {
		//dynamically build the cumulative probability distribution of the R^2 residual Z Scores
		m_bundleResults.addProbabilityDistributionObservation(residualR2ZScore);
		int currentModelIndex = m_bundleResults.maximumLikelihoodModelIndex();
		observationWeight *= m_bundleResults.maximumLikelihoodModelWFunc(currentModelIndex)
		observationWeightSqrt *= m_bundleResults.maximumLikelihoodModelWFunc(currentModelIndex)
		.sqrtWeightScaler(residualR2ZScore);
		}

		// multiply coefficients by observation weight
		coeffImagePosition *= observationWeight;
		coeffImagePointing *= observationWeight;
		coeffPoint3D *= observationWeight;
		coeffRHS *= observationWeight;
		coeffImagePosition *= observationWeightSqrt;
		coeffImagePointing *= observationWeightSqrt;
		coeffPoint3D *= observationWeightSqrt;
		coeffRHS *= observationWeightSqrt;

		if (m_bundleSettings->solveTargetBody()) {
		coeffTarget *= observationWeight;
		coeffTarget *= observationWeightSqrt;
		}

		return true;
		@@ -2598,8 +2584,6 @@ bool BundleAdjust::formNormalEquations() {
		*/
		void BundleAdjust::applyParameterCorrections() {

		// qDebug() << m_imageSolution;

		int t = 0;

		// TODO - update target body parameters if in solution
		@@ -2652,7 +2636,6 @@ bool BundleAdjust::formNormalEquations() {
		double vtpvRange = 0.0;
		double weight;
		double vx, vy;
		double rmsx, rmsy, rmsxy;

		// x, y, and xy residual stats vectors
		Statistics xResiduals;
		@@ -2662,9 +2645,6 @@ bool BundleAdjust::formNormalEquations() {
		// vtpv for image coordinates
		int numObjectPoints = m_bundleControlPoints.size();

		// double vtpvTest = m_bundleControlPoints.vtpvMeasureContribution();
		// m_bundleControlPoints.rmsResiduals(rmsx, rmsy, rmsxy);

		for (int i = 0; i < numObjectPoints; i++) {

		BundleControlPointQsp point = m_bundleControlPoints.at(i);
		@@ -2673,41 +2653,39 @@ bool BundleAdjust::formNormalEquations() {
		for (int j = 0; j < numMeasures; j++) {
		const BundleMeasureQsp measure = point->at(j);

		weight = 1.4 * (measure->camera())->PixelPitch();

		// TODO: lidar kluge
		if (measure->parentControlPoint()->id().contains("Lidar")) {
		weight *=30.0;
		}

		weight = 1.0 / weight;
		weight = measure->measureWeightSqrt();
		weight *= weight;

		vx = measure->focalPlaneMeasuredX() - measure->focalPlaneComputedX();
		vy = measure->focalPlaneMeasuredY() - measure->focalPlaneComputedY();

		// TODO: Testing - correct lidar simultaneous measure by it's residual
		// if ( point->id().contains("Lidar", Qt::CaseInsensitive) ) {

		if ( point->id().contains("Lidar", Qt::CaseInsensitive) ) {
		// double sample = measure->sample();
		// double line = measure->line();

		// // in undistorted space
		// double sampleResidual = measure->sampleResidual();
		// double lineResidual = measure->lineResidual();

		// // are measure->sample, measure->line in distorted or undistorted space?
		// double newsample = measure->sample() - measure->sampleResidual();
		// double newline = measure->line() - measure->lineResidual();
		//testing re-backprojection of point

		// measure->rawMeasure()->SetCoordinate(newsample, newline);
		// Camera* camera = measure->camera();
		// get a priori surface point
		SurfacePoint surfacepoint = point->adjustedSurfacePoint();

		// camera->SetImage(newsample,newline);
		// double newx = camera->DistortionMap()->UndistortedFocalPlaneX();
		// double newy = camera->DistortionMap()->UndistortedFocalPlaneY();
		// measure->rawMeasure()->SetFocalPlaneMeasured(newx,newy);
		// }
		double newsamp = 0.0;
		double newline = 0.0;

		if (measure->camera()->SetGround(surfacepoint)) {
		newsamp = measure->camera()->Sample();
		newline = measure->camera()->Line();
		}

		measure->rawMeasure()->SetCoordinate(newsamp, newline);
		Camera* camera = measure->camera();
		camera->SetImage(newsamp,newline);
		double newx = camera->DistortionMap()->UndistortedFocalPlaneX();
		double newy = camera->DistortionMap()->UndistortedFocalPlaneY();
		measure->rawMeasure()->SetFocalPlaneMeasured(newx,newy);
		}

		// if rejected, don't include in statistics
		if (measure->isRejected()) {

isis/src/control/objs/BundleAdjust/BundleAdjust.h

+3 −0

Original line number	Diff line number	Diff line
		@@ -315,6 +315,9 @@ namespace Isis {
		* constructor in the BundleSolutionInfo class because it is derived
		* from QObject. Note that we ultimately want to return a QSharedPointer
		* instead of a raw pointer.
		* @history 2018-06-27 Ken Edmundson - Now setting measure sigmas in BundleMeasure in init()
		* method; retrieving sigma and sqrt of weight in computePartials and
		* computeVtpv methods.
		*/
		class BundleAdjust : public QObject {
		Q_OBJECT