Merge pull request #302 from kledmundson/LroLidar_Infrastructure (561bc942) · Commits · aflab / astrogeology / ISIS3

isis/src/control/objs/BundleConstraint/BundleLidarRangeConstraint.cpp

+46 −27

Original line number	Diff line number	Diff line
		@@ -27,6 +27,14 @@ namespace Isis {

		m_scaledTime = 0.0;
		m_rangeObserved = m_lidarControlPoint->range();

		if (m_rangeObserved <= 0) {
		QString msg ="In BundleLidarRangeConstraint::BundleLidarRangeConstraint():"
		"observed range for lidar point must be positive (Point Id: "
		+ measure->parentControlPoint()->id() + ")\n.";
		throw IException(IException::Programmer, msg, _FILEINFO_);
		}

		m_rangeObservedSigma = m_lidarControlPoint->sigmaRange() * 0.001; // converting from m to km

		if (m_rangeObservedSigma <= 0) {
		@@ -39,10 +47,6 @@ namespace Isis {
		m_rangeObservedWeightSqrt = 1.0/m_rangeObservedSigma;
		m_adjustedSigma = 0.0;

		m_coeff_range_image.resize(1, m_bundleObservation->numberPositionParameters());
		m_coeff_range_point3D.resize(1,3);
		m_coeff_range_RHS.resize(1);

		m_pointBodyFixed.resize(3);
		m_camPositionJ2K.resize(3);
		m_camPositionBodyFixed.resize(3);
		@@ -129,6 +133,11 @@ namespace Isis {
		double dZ = m_camPositionBodyFixed[2] - m_pointBodyFixed[2];
		m_rangeComputed = sqrt(dXdX+dYdY+dZ*dZ);

		if (m_rangeComputed <= 0.0) {
		QString msg = "In BundleLidarRangeConstraint::update(): m_rangeComputed must be positive\n";
		throw IException(IException::Programmer, msg, _FILEINFO_);
		}

		// compute current contribution to vtpv from spacecraft-to-lidar point constraint equation
		// vtpv is the weight sum of squares of the residuals
		double t = m_rangeObserved - m_rangeComputed;
		@@ -150,11 +159,21 @@ namespace Isis {
		return false;
		}

		static LinearAlgebra::Matrix coeff_range_image; //!< Partials w/r to image position
		static LinearAlgebra::Matrix coeff_range_point3D(1,3); //!< Partials w/r to lidar point
		static LinearAlgebra::Vector coeff_range_RHS(1); //!< Right hand side of normals

		int positionBlockIndex = measure->positionNormalsBlockIndex();

		m_coeff_range_image.clear();
		m_coeff_range_point3D.clear();
		m_coeff_range_RHS.clear();
		// resize coeff_range_image matrix if necessary
		int numPositionParameters = m_bundleObservation->numberPositionParameters();
		if ((int) coeff_range_image.size2() != numPositionParameters) {
		coeff_range_image.resize(1, numPositionParameters, false);
		}

		coeff_range_image.clear();
		coeff_range_point3D.clear();
		coeff_range_RHS.clear();

		// compute partial derivatives for camstation-to-range point condition

		@@ -183,26 +202,26 @@ namespace Isis {

		// partials with respect to camera body fixed X polynomial coefficients
		double c = 1.0;
		int numPositionParameters = m_bundleObservation->numberPositionParameters()/3;
		int numPositionParametersPerCoordinate = numPositionParameters/3;
		int index = 0;
		for (int i = 0; i < numPositionParameters; i++) {
		m_coeff_range_image(0,index) = b1*c;
		for (int i = 0; i < numPositionParametersPerCoordinate; i++) {
		coeff_range_image(0,index) = b1*c;
		c *= m_scaledTime;
		index++;
		}

		// partials with respect to camera body fixed Y polynomial coefficients
		c = 1.0;
		for (int i = 0; i < numPositionParameters; i++) {
		m_coeff_range_image(0,index) = b2*c;
		for (int i = 0; i < numPositionParametersPerCoordinate; i++) {
		coeff_range_image(0,index) = b2*c;
		c *= m_scaledTime;
		index++;
		}

		// partials with respect to camera body fixed Z polynomial coefficients
		c = 1.0;
		for (int i = 0; i < numPositionParameters; i++) {
		m_coeff_range_image(0,index) = b3*c;
		for (int i = 0; i < numPositionParametersPerCoordinate; i++) {
		coeff_range_image(0,index) = b3*c;
		c *= m_scaledTime;
		index++;
		}
		@@ -219,20 +238,20 @@ namespace Isis {
		double coslon = cos(lon);

		// partials with respect to point latitude, longitude, and radius
		m_coeff_range_point3D(0,0)
		coeff_range_point3D(0,0)
		= radius(-sinlatcoslona1 - sinlatsinlona2 + coslata3)/m_rangeComputed;
		m_coeff_range_point3D(0,1)
		coeff_range_point3D(0,1)
		= radius(-coslatsinlona1 + coslatcoslon*a2)/m_rangeComputed;
		m_coeff_range_point3D(0,2)
		coeff_range_point3D(0,2)
		= (coslatcoslona1 + coslatsinlona2 + sinlat*a3)/m_rangeComputed;

		// right hand side (observed distance - computed distance)
		m_coeff_range_RHS(0) = m_rangeObserved - m_rangeComputed;
		coeff_range_RHS(0) = m_rangeObserved - m_rangeComputed;

		// multiply coefficients by observation weight
		m_coeff_range_image *= m_rangeObservedWeightSqrt;
		m_coeff_range_point3D *= m_rangeObservedWeightSqrt;
		m_coeff_range_RHS *= m_rangeObservedWeightSqrt;
		coeff_range_image *= m_rangeObservedWeightSqrt;
		coeff_range_point3D *= m_rangeObservedWeightSqrt;
		coeff_range_RHS *= m_rangeObservedWeightSqrt;

		// form matrices to be added to normal equation auxiliaries
		// TODO: be careful about need to resize if if different images have different numbers of
		@@ -240,23 +259,23 @@ namespace Isis {

		// add range condition contribution to N11 portion of normal equations matrix
		((normalsMatrix[positionBlockIndex])[positionBlockIndex])
		+= prod(trans(m_coeff_range_image), m_coeff_range_image);
		+= prod(trans(coeff_range_image), coeff_range_image);

		// add range condition contribution to N12 portion of normal equations matrix
		*N12[positionBlockIndex] += prod(trans(m_coeff_range_image), m_coeff_range_point3D);
		*N12[positionBlockIndex] += prod(trans(coeff_range_image), coeff_range_point3D);

		// contribution to n1 vector
		int startColumn = normalsMatrix.at(positionBlockIndex)->startColumn();
		vector_range<LinearAlgebra::VectorCompressed >
		n1_range (n1, range (startColumn, m_coeff_range_image.size2()));
		n1_range (n1, range (startColumn, coeff_range_image.size2()));

		n1_range += prod(trans(m_coeff_range_image), m_coeff_range_RHS);
		n1_range += prod(trans(coeff_range_image), coeff_range_RHS);

		// form N22
		N22 += prod(trans(m_coeff_range_point3D), m_coeff_range_point3D);
		N22 += prod(trans(coeff_range_point3D), coeff_range_point3D);

		// contribution to n2 vector
		n2 += prod(trans(m_coeff_range_point3D), m_coeff_range_RHS);
		n2 += prod(trans(coeff_range_point3D), coeff_range_RHS);

		return true;
		}

isis/src/control/objs/BundleConstraint/BundleLidarRangeConstraint.h

+9 −7

Original line number	Diff line number	Diff line
		@@ -44,6 +44,13 @@ namespace Isis {
		* @internal
		* @history 2018-04-13 Ken Edmundson - Original version.
		* @history 2018-06-27 Ken Edmundson - Code clean up.
		* @history 2018-06-28 Ken Edmundson - Removed partial derivative matrices as member variables
		* and now declare them as static in the applyConstraint
		* method. Now coeff_range_image matrix is resized if number
		* of image parameters changes. This is consistent with
		* BundleAdjust::computePartials. Added IExceptions to
		* verify m_rangeObserved and m_rangeComputed are positive.
		*
		*/
		class BundleLidarRangeConstraint : public BundleConstraint {
		public:
		@@ -84,12 +91,7 @@ namespace Isis {
		image using the image's current exterior
		orientation (SPICE). The "measure" is
		corrected in each iteration of the bundle
		adjustement by it's residuals.*/
		// TODO: should partial matrices be static?
		// NOTE: they will be different size for different images
		LinearAlgebra::Matrix m_coeff_range_image; //!< Partials w/respect to image position
		LinearAlgebra::Matrix m_coeff_range_point3D; //!< Partials w/respect to lidar point
		LinearAlgebra::Vector m_coeff_range_RHS; //!< Right hand side of normals
		adjustment by it's residuals.*/

		std::vector<double> m_pointBodyFixed; //!< Body fixed coordinates of lidar point
		std::vector<double> m_camPositionJ2K; //!< J2K coordinates of camera
		@@ -107,6 +109,6 @@ namespace Isis {

		//! Typdef for BundleLidarRangeConstraint QSharedPointer.
		typedef QSharedPointer<BundleLidarRangeConstraint> BundleLidarRangeConstraintQsp;
		};
		}

		#endif