Merge pull request #609 from kledmundson/LroLidar_Infrastructure (2a5afe5b) · Commits · aflab / astrogeology / ISIS3

isis/src/base/objs/SpiceRotation/SpiceRotation.cpp

+8 −2

Original line number	Diff line number	Diff line
		@@ -1681,6 +1681,8 @@ namespace Isis {
		// Otherwise, fit the polynomial.
		else {
		m_polynomial = fitPolynomial(p_degree, m_segments);
		// TODO: ask Debbie about placement of next statement...
		p_degreeApplied = true;
		}

		p_source = type;
		@@ -2064,7 +2066,7 @@ namespace Isis {
		double derivative;
		double time = (p_et - p_baseTime) / p_timeScale;

		if (coeffIndex > 0 && coeffIndex <= p_degree) {
		if (coeffIndex > 0 && coeffIndex <= m_polynomial.degree()) {
		derivative = pow(time, coeffIndex);
		}
		else if (coeffIndex == 0) {
		@@ -2074,7 +2076,7 @@ namespace Isis {
		QString msg = "Unable to evaluate the derivative of the SPICE rotation fit polynomial for "
		"the given coefficient index [" + toString(coeffIndex) + "]. "
		"Index is negative or exceeds degree of polynomial ["
		+ toString(p_degree) + "]";
		+ toString(m_polynomial.degree()) + "]";
		throw IException(IException::Programmer, msg, _FILEINFO_);
		}
		return derivative;
		@@ -2277,6 +2279,8 @@ namespace Isis {
		// If polynomials have not been applied yet then simply set the degree and return
		if (!p_degreeApplied) {
		p_degree = degree;
		// TODO: verify
		m_polynomial.setDegree(degree);
		}

		// Otherwise adjust the degree.
		@@ -2300,6 +2304,8 @@ namespace Isis {

		// Change the polynomial degree
		m_polynomial.setDegree(degree);
		// TODO: verify next line
		p_degree = degree;

		// Reset the coefficients
		for (int i = 0; i < m_segments; i++) {

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

+13 −5

Original line number	Diff line number	Diff line
		@@ -507,6 +507,11 @@ namespace Isis {
		}
		}

		// initialize exterior orientation (spice) for all BundleImages in all BundleObservations
		// TODO!!! - should these initializations just be done when we add the new observation above?
		m_bundleObservations.initializeExteriorOrientation();


		if (m_bundleSettings->solveTargetBody()) {
		m_bundleObservations.setBodyRotation();
		}
		@@ -545,7 +550,8 @@ namespace Isis {
		if (m_lidarDataSet) {
		numLidarPoints = m_lidarDataSet->points().size();
		}
		for (int i = 0; i < numLidarPoints; i++) {
		for (int i =
		0; i < numLidarPoints; i++) {
		LidarControlPointQsp lidarPoint = m_lidarDataSet->points().at(i);
		if (lidarPoint->IsIgnored()) {
		continue;
		@@ -1431,7 +1437,7 @@ namespace Isis {

		// form N12 target portion
		N12.insertMatrixBlock(0, numTargetPartials, 3);
		*N12[0] += prod(trans(coeffTarget), coeffPoint3D);;
		*N12[0] += prod(trans(coeffTarget), coeffPoint3D);

		// contribution to n1 vector
		vector_range<LinearAlgebra::VectorCompressed >
		@@ -1475,7 +1481,7 @@ namespace Isis {
		// solving for pointing
		if (pointingBlockIndex >= 0) {

		// insert submatrix into normal equations at positionBlockIndex, positionBlockIndex
		// insert submatrix into normal equations at pointingBlockIndex, pointingBlockIndex
		// if block is already there, no insertion is made
		m_sparseNormals.insertMatrixBlock(pointingBlockIndex, pointingBlockIndex,
		coeffImagePointing.size2(), coeffImagePointing.size2());
		@@ -2533,7 +2539,8 @@ namespace Isis {
		}
		}

		measure.parentBundleObservation()->computePartials(coeffImagePosition, coeffImagePointing);
		measure.parentBundleObservation()->computePartials(coeffImagePosition, coeffImagePointing,
		*measureCamera);

		// Complete partials calculations for 3D point (latitudinal or rectangular)
		measureCamera->GroundMap()->GetdXYdPoint(lookBWRTCoord1,
		@@ -2626,7 +2633,8 @@ namespace Isis {

		// Apply parameter corrections for control points
		// TODO: can we do this faster by threading with QtConcurrent::run?
		m_bundleControlPoints.applyParameterCorrections(m_sparseNormals, m_imageSolution);
		m_bundleControlPoints.applyParameterCorrections(m_sparseNormals, m_imageSolution,
		m_bundleTargetBody);
		}

isis/src/control/objs/BundleUtilities/BundleControlPoint.cpp

+3 −83

Original line number	Diff line number	Diff line
		@@ -355,8 +355,8 @@ namespace Isis {
		* @param factor The unit conversion factor to use on lat and lon rad or x/y/z km.
		* @param target The BundleTargetBody.
		*/
		void BundleControlPoint::applyParameterCorrections(LinearAlgebra::Vector imageSolution,
		SparseBlockMatrix &sparseNormals,
		void BundleControlPoint::applyParameterCorrections(SparseBlockMatrix &sparseNormals,
		LinearAlgebra::Vector imageSolution,
		const BundleTargetBodyQsp target) {
		if (!isRejected()) {

		@@ -1151,86 +1151,6 @@ QString BundleControlPoint::formatCoordAprioriSigmaString(SurfacePoint::CoordInd
		}


		/**
		* Apply point parameter corrections for in Bundle Adjustment.
		*
		* @param normalsMatrix Normal equations matrix.
		* @param imageSolution Current iteration solution vector for image parameters.
		*
		*/
		void BundleControlPoint::applyParameterCorrections(SparseBlockMatrix &normalsMatrix,
		LinearAlgebra::Vector &imageSolution) {

		// subtract product of Q and nj from NIC
		productAlphaAV(-1.0, normalsMatrix, imageSolution);

		SurfacePoint surfacepoint = adjustedSurfacePoint();

		double pointLat = surfacepoint.GetLatitude().degrees();
		double pointLon = surfacepoint.GetLongitude().degrees();
		double pointRad = surfacepoint.GetLocalRadius().meters();

		pointLat += RAD2DEG * m_nicVector(0);
		pointLon += RAD2DEG * m_nicVector(1);

		// Make sure updated values are still in valid range.
		// TODO What is the valid lon range?
		if (pointLat < -90.0) {
		pointLat = -180.0 - pointLat;
		pointLon = pointLon + 180.0;
		}
		if (pointLat > 90.0) {
		pointLat = 180.0 - pointLat;
		pointLon = pointLon + 180.0;
		}
		while (pointLon > 360.0) {
		pointLon = pointLon - 360.0;
		}
		while (pointLon < 0.0) {
		pointLon = pointLon + 360.0;
		}

		pointRad += 1000.*m_nicVector(2);

		// sum and save corrections
		m_corrections += m_nicVector;

		/*
		// ken testing - if solving for target body mean radius, set radius to current
		// mean radius value
		// TODO: What if this point is FIXED or CONSTRAINED?
		// feels wrong in that case?
		if (m_bundleTargetBody && (m_bundleTargetBody->solveMeanRadius()
		\|\| m_bundleTargetBody->solveTriaxialRadii())) {
		if (m_bundleTargetBody->solveMeanRadius()) {
		surfacepoint.SetSphericalCoordinates(Latitude(pointLat, Angle::Degrees),
		Longitude(pointLon, Angle::Degrees),
		m_bundleTargetBody->meanRadius());
		}
		else if (m_bundleTargetBody->solveTriaxialRadii()) {
		Distance localRadius = m_bundleTargetBody->
		localRadius(Latitude(pointLat, Angle::Degrees),
		Longitude(pointLon, Angle::Degrees));
		surfacepoint.SetSphericalCoordinates(Latitude(pointLat, Angle::Degrees),
		Longitude(pointLon, Angle::Degrees),
		localRadius);
		}
		}
		else {
		surfacepoint.SetSphericalCoordinates(Latitude(pointLat, Angle::Degrees),
		Longitude(pointLon, Angle::Degrees),
		Distance(pointRad, Distance::Meters));
		}
		*/

		surfacepoint.SetSphericalCoordinates(Latitude(pointLat, Angle::Degrees),
		Longitude(pointLon, Angle::Degrees),
		Distance(pointRad, Distance::Meters));

		setAdjustedSurfacePoint(surfacepoint);
		}


		/**
		* Perform the matrix multiplication v2 = alpha ( Q x v1 ).
		*

isis/src/control/objs/BundleUtilities/BundleControlPoint.h

+3 −4

Original line number	Diff line number	Diff line
		@@ -136,8 +136,6 @@ namespace Isis {
		void setSigmaWeightFromGlobals(double gSigma, int index, double cFactor);
		void zeroNumberOfRejectedMeasures();
		void productAlphaAV(double alpha, SparseBlockMatrix &sparseMatrix, LinearAlgebra::Vector &v1);
		void applyParameterCorrections(LinearAlgebra::Vector imageSolution,
		SparseBlockMatrix &sparseNormals, const BundleTargetBodyQsp target);

		// accessors
		ControlPoint *rawControlPoint() const;
		@@ -173,8 +171,9 @@ namespace Isis {
		QString formatCoordAdjustedSigmaString(SurfacePoint::CoordIndex, int fieldWidth, int precision,
		bool errorPropagation) const;

		virtual void applyParameterCorrections(SparseBlockMatrix &normalsMatrix,
		LinearAlgebra::Vector &imageSolution);
		virtual void applyParameterCorrections(SparseBlockMatrix &sparseNormals,
		LinearAlgebra::Vector imageSolution,
		const BundleTargetBodyQsp target);

		virtual double measureSigma();
		virtual double measureWeight();

isis/src/control/objs/BundleUtilities/BundleControlPointVector.cpp

+4 −2

Original line number	Diff line number	Diff line
		@@ -4,6 +4,7 @@
		#include <QFutureWatcher>
		#include <QtConcurrentRun>

		#include "BundleTargetBody.h"
		#include "IException.h"

		namespace Isis {
		@@ -60,9 +61,10 @@ namespace Isis {
		*
		*/
		void BundleControlPointVector::applyParameterCorrections(SparseBlockMatrix &normalsMatrix,
		LinearAlgebra::Vector &imageSolution) {
		LinearAlgebra::Vector &imageSolution,
		const BundleTargetBodyQsp target) {
		for (int i = 0; i < size(); i++) {
		at(i)->applyParameterCorrections(normalsMatrix, imageSolution);
		at(i)->applyParameterCorrections(normalsMatrix, imageSolution, target);
		}
		}