Merge pull request #280 from kledmundson/LroLidar_Infrastructure (0ab8f007) · Commits · aflab / astrogeology / ISIS3

isis/src/base/objs/SpicePosition/SpicePosition.cpp

+10 −0

Original line number	Diff line number	Diff line
		@@ -1825,6 +1825,16 @@ namespace Isis {
		}


		/**
		* Return the scaled time.
		*
		* @return Scaled time.
		*/
		double SpicePosition::scaledTime() const {
		return (p_et - p_baseTime) / p_timeScale;
		}


		/**
		* Return the index of the polynomial segment that contains a given time.
		*

isis/src/base/objs/SpicePosition/SpicePosition.h

+13 −8

Original line number	Diff line number	Diff line
		@@ -181,6 +181,8 @@ namespace Isis {
		* @history 2017-08-18 Tyler Wilson, Summer Stapleton, Ian Humphrey - Added opening/closing brackets
		* to SetEphemerisTimePolyFunction() so this class compiles without warnings
		* under C++14. References #4809.
		* @history 2018-06-22 Ken Edmundson - Added scaledTime() method to return current scaled time.
		* Also removed some unnecessary semi-colons in this header file.
		*/
		class SpicePosition {
		public:
		@@ -215,14 +217,17 @@ namespace Isis {
		//! Return the current ephemeris time
		double EphemerisTime() const {
		return p_et;
		};
		}

		//! Return scaled time
		double scaledTime() const;

		const std::vector<double> &GetCenterCoordinate();

		//! Return the current J2000 position
		const std::vector<double> &Coordinate() {
		return p_coordinate;
		};
		}

		//! Return the current J2000 velocity
		const std::vector<double> &Velocity();
		@@ -230,7 +235,7 @@ namespace Isis {
		//! Return the flag indicating whether the velocity exists
		bool HasVelocity() {
		return p_hasVelocity;
		};
		}

		void LoadCache(double startTime, double endTime, int size);
		void LoadCache(double time);
		@@ -249,7 +254,7 @@ namespace Isis {
		//! Is this position cached
		bool IsCached() const {
		return (p_cache.size() > 0);
		};
		}

		void SetPolynomial(const Source type = PolyFunction);

		@@ -282,21 +287,21 @@ namespace Isis {
		//! Return the source of the position
		Source GetSource() {
		return p_source;
		};
		}

		void ComputeBaseTime();

		//! Return the base time for the position
		double GetBaseTime() {
		return p_baseTime;
		};
		}

		void SetOverrideBaseTime(double baseTime, double timeScale);

		//! Return the time scale for the position
		double GetTimeScale() {
		return p_timeScale;
		};
		}

		double DPolynomial(const int coeffIndex);

		@@ -387,6 +392,6 @@ namespace Isis {

		//! Typdef for SpicePosition QSharedPointer.
		typedef QSharedPointer<SpicePosition> SpicePositionQsp;
		};
		}

		#endif

isis/src/control/apps/jigsaw/jigsaw.cpp

+14 −1

Original line number	Diff line number	Diff line
		@@ -123,11 +123,24 @@ void IsisMain() {
		bundleSolution->outputResiduals();
		}

		// write lidar csv output file
		if (ui.GetBoolean("LIDAR_CSV")) {
		bundleSolution->outputLidarCSV();
		}

		// write updated control net
		bundleAdjustment->controlNet()->Write(ui.GetFileName("ONET"));

		// write updated lidar data file
		if (ui.WasEntered("LIDARDATA")) {
		if (ui.GetString("OLIDARFORMAT") == "JSON") {
		bundleAdjustment->lidarData()->write(ui.GetFileName("OLIDARDATA"),LidarData::Format::Json);
		}
		else {
		bundleAdjustment->lidarData()->write(ui.GetFileName("OLIDARDATA"),LidarData::Format::Binary);
		}
		}

		PvlGroup gp("JigsawResults");

		// Update the cube pointing if requested but ONLY if bundle has converged

isis/src/control/apps/jigsaw/jigsaw.xml

+62 −0

Original line number	Diff line number	Diff line
		@@ -260,6 +260,9 @@
		BundleSolutionInfo object. Am also deleting this pointer because jigsaw.cpp takes
		ownership from BundleAdjust.
		</change>
		<change name="Ken Edmundson" date="2018-06-22">
		Added output of lidar adjusted csv file and output of adjusted lidar data file.
		</change>
		</history>

		<groups>
		@@ -314,6 +317,7 @@
		*.net
		</filter>
		</parameter>

		<parameter name="ONET">
		<type>filename</type>
		<fileMode>output</fileMode>
		@@ -329,6 +333,7 @@
		*.net
		</filter>
		</parameter>

		<parameter name="LIDARDATA">
		<type>filename</type>
		<internalDefault>none</internalDefault>
		@@ -340,11 +345,56 @@
		This file is a <def>lidar point data</def> generated from <b>lrolola2isis.cpp</b>.
		It contains <def>lidar control point</def>s and associated measures for simultaneous images.
		</description>
		<exclusions>
		</exclusions>
		<filter>
		.dat .json
		</filter>
		</parameter>

		<parameter name="OLIDARDATA">
		<type>filename</type>
		<internalDefault>none</internalDefault>
		<fileMode>output</fileMode>
		<brief>
		Output lidar data file
		</brief>
		<description>
		This output file contains the adjusted <def>lidar data</def> with
		the final coordinates of the <def>lidar point</def>s and residuals for each
		measurement.
		</description>
		<filter>
		.dat .json
		</filter>
		</parameter>

		<parameter name="OLIDARFORMAT">
		<type>string</type>
		<brief> Output lidar data file format</brief>
		<default>
		<item>BINARY</item>
		</default>
		<description>
		Output lidar data file format.
		</description>
		<list>
		<option value="BINARY">
		<brief> Output lidar data in binary format</brief>
		<description>
		Output lidar data in binary format.
		</description>
		</option>

		<option value="JSON">
		<brief> Output lidar data in json format </brief>
		<description>
		Output lidar data in json format.
		</description>
		</option>
		</list>
		</parameter>

		<!-- Turning off USEPVL and SC_PARAMETERS for now. Since we are now using observation numbers
		to obtain the BundleObservationSolveSettings for a BundleObservation (instead of inst. id),
		this would require using a PVL that contains names of lists of images to group by.
		@@ -1248,6 +1298,18 @@
		<item>yes</item>
		</default>
		</parameter>

		<parameter name="LIDAR_CSV">
		<brief> Outputs lidar data to csv file - lidar.csv</brief>
		<description>
		Selection of this parameter flags output of lidar data
		points to a csv file
		</description>
		<type>boolean</type>
		<default>
		<item>no</item>
		</default>
		</parameter>
		</group>

		</groups>

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

+82 −144

Original line number	Diff line number	Diff line
		@@ -132,6 +132,7 @@ namespace Isis {
		* @param bundleSettings A shared pointer to the BundleSettings to be used.
		* @param cnetFile The filename of the control network to be used.
		* @param cubeList The list of filenames of the cubes to be adjusted.
		* @param lidarDataFile Lidar point dataset filename.
		* @param printSummary If summaries should be printed each iteration.
		*/
		BundleAdjust::BundleAdjust(BundleSettingsQsp bundleSettings,
		@@ -157,13 +158,15 @@ namespace Isis {

		// read lidar point data file
		try {
		m_lidarDataSet.read(lidarDataFile);
		m_lidarDataSet = LidarDataQsp( new LidarData());
		m_lidarDataSet->read(lidarDataFile);
		}
		catch (IException &e) {
		throw;
		}

		m_bundleResults.setOutputControlNet(m_controlNet);
		m_bundleResults.setOutputLidarData(m_lidarDataSet);
		m_serialNumberList = new SerialNumberList(cubeList);
		m_bundleSettings = bundleSettings;
		m_bundleTargetBody = bundleSettings->bundleTargetBody();
		@@ -433,7 +436,9 @@ namespace Isis {
		// RENAME????????????
		m_controlNet->SetImages(*m_serialNumberList, progress);

		m_lidarDataSet.SetImages(*m_serialNumberList, progress);
		if (m_lidarDataSet) {
		m_lidarDataSet->SetImages(*m_serialNumberList, progress);
		}

		// clear JigsawRejected flags
		m_controlNet->ClearJigsawRejected();
		@@ -554,9 +559,12 @@ namespace Isis {
		}

		// add BundleLidarControlPoints
		int numLidarPoints = m_lidarDataSet.points().size();
		int numLidarPoints;
		if (m_lidarDataSet) {
		numLidarPoints = m_lidarDataSet->points().size();
		}
		for (int i = 0; i < numLidarPoints; i++) {
		LidarControlPointQsp lidarPoint = m_lidarDataSet.points().at(i);
		LidarControlPointQsp lidarPoint = m_lidarDataSet->points().at(i);
		if (lidarPoint->IsIgnored()) {
		continue;
		}
		@@ -717,55 +725,20 @@ namespace Isis {


		/**
		* Initialize Normal Equations matrix (m_sparseNormals).
		*
		* @return bool.
		* @brief Free CHOLMOD library variables.
		*
		* @todo Ken We are explicitly setting the start column for each SparseBlockColumn in the normal
		* equations matrix below. Is it possible to make the m_sparseNormals matrix smart
		* enough to set the start column of a column block automatically when it is added?
		* Frees m_cholmodTriplet, m_cholmodNormal, and m_L.
		* Calls cholmod_finish when complete.
		*
		* @return bool If the CHOLMOD library successfully cleaned up.
		*/
		bool BundleAdjust::initializeNormalEquationsMatrix() {

		int nBlockColumns = m_bundleObservations.numberPolynomialSegments();

		if (m_bundleSettings->solveTargetBody())
		nBlockColumns += 1;

		m_sparseNormals.setNumberOfColumns(nBlockColumns);

		m_sparseNormals.at(0)->setStartColumn(0);

		int nParameters = 0;
		int blockColumn = 0;
		if (m_bundleSettings->solveTargetBody()) {
		nParameters += m_bundleSettings->numberTargetBodyParameters();
		blockColumn = 1;
		}

		for (int i = 0; i < m_bundleObservations.size(); i++) {
		int positionParameters =
		m_bundleObservations.at(i)->numberPositionParametersPerSegment();
		bool BundleAdjust::freeCHOLMODLibraryVariables() {

		int pointingParameters =
		m_bundleObservations.at(i)->numberPointingParametersPerSegment();
		cholmod_free_triplet(&m_cholmodTriplet, &m_cholmodCommon);
		cholmod_free_sparse(&m_cholmodNormal, &m_cholmodCommon);
		cholmod_free_factor(&m_L, &m_cholmodCommon);

		int positionSegments = m_bundleObservations.at(i)->numberPolynomialPositionSegments();
		for (int j = 0; j < positionSegments; j++) {
		m_sparseNormals.at(blockColumn)->setStartColumn(nParameters);
		m_sparseNormals.at(blockColumn)->setObservationIndex(i);
		nParameters += positionParameters;
		blockColumn++;
		}
		int pointingSegments = m_bundleObservations.at(i)->numberPolynomialPointingSegments();
		for (int j = 0; j < pointingSegments; j++) {
		m_sparseNormals.at(blockColumn)->setStartColumn(nParameters);
		m_sparseNormals.at(blockColumn)->setObservationIndex(i);
		nParameters += pointingParameters;
		blockColumn++;
		}
		}
		cholmod_finish(&m_cholmodCommon);

		return true;
		}
		@@ -781,7 +754,6 @@ namespace Isis {
		* enough to set the start column of a column block automatically when it is added?
		*
		*/
		/*
		bool BundleAdjust::initializeNormalEquationsMatrix() {

		int nBlockColumns = m_bundleObservations.numberPolynomialSegments();
		@@ -794,19 +766,12 @@ namespace Isis {
		m_sparseNormals.at(0)->setStartColumn(0);

		int nParameters = 0;
		int blockColumn = 0;
		if (m_bundleSettings->solveTargetBody()) {
		nParameters += m_bundleSettings->numberTargetBodyParameters();
		m_sparseNormals.at(1)->setStartColumn(nParameters);

		int observation = 0;
		for (int i = 2; i < nBlockColumns; i++) {
		nParameters += m_bundleObservations.at(observation)->numberParameters();
		m_sparseNormals.at(i)->setStartColumn(nParameters);
		observation++;
		}
		blockColumn = 1;
		}
		else {
		int blockColumn = 0;

		for (int i = 0; i < m_bundleObservations.size(); i++) {
		int positionParameters =
		m_bundleObservations.at(i)->numberPositionParametersPerSegment();
		@@ -829,27 +794,6 @@ namespace Isis {
		blockColumn++;
		}
		}
		}

		return true;
		}
		*/

		/**
		* @brief Free CHOLMOD library variables.
		*
		* Frees m_cholmodTriplet, m_cholmodNormal, and m_L.
		* Calls cholmod_finish when complete.
		*
		* @return bool If the CHOLMOD library successfully cleaned up.
		*/
		bool BundleAdjust::freeCHOLMODLibraryVariables() {

		cholmod_free_triplet(&m_cholmodTriplet, &m_cholmodCommon);
		cholmod_free_sparse(&m_cholmodNormal, &m_cholmodCommon);
		cholmod_free_factor(&m_L, &m_cholmodCommon);

		cholmod_finish(&m_cholmodCommon);

		return true;
		}
		@@ -1157,7 +1101,7 @@ namespace Isis {
		m_bundleResults.initializeResidualsProbabilityDistribution(101);
		}
		// TODO: is this necessary ???
		// probably all ready initialized to 101 nodes in bundle settings constructor...
		// probably already initialized to 101 nodes in bundle settings constructor...

		// if we're using CHOLMOD and still going, release cholmod_factor
		// (if we don't, memory leaks will occur), otherwise we need it for error propagation
		@@ -1222,6 +1166,7 @@ namespace Isis {
		BundleSolutionInfo *BundleAdjust::bundleSolveInformation() {
		BundleSolutionInfo *bundleSolutionInfo = new BundleSolutionInfo(m_bundleSettings,
		FileName(m_cnetFileName),
		FileName(m_lidarFileName),
		m_bundleResults,
		imageLists());
		bundleSolutionInfo->setRunTime("");
		@@ -1231,8 +1176,9 @@ namespace Isis {

		/**
		* Form the least-squares normal equations matrix.
		*
		* Each BundleControlPoint stores its Q matrix and NIC vector.
		* The covariance matrix for each point will be stored in its adjusted surface point.
		* Limiting error portion of each points covariance matrix is stored in its adjusted surface point.
		*
		* @return bool
		*
		@@ -1241,7 +1187,6 @@ namespace Isis {
		* @see formWeightedNormals()
		*/
		bool BundleAdjust::formNormalEquations() {
		// bool BundleAdjust::formPhotoNormals() {
		bool status = false;

		m_bundleResults.setNumberObservations(0);
		@@ -1442,7 +1387,7 @@ bool BundleAdjust::formNormalEquations() {
		*
		* @return bool If the matrices were successfully formed.
		*
		* @see BundleAdjust formNormalEquations
		* @see formNormalEquations()
		*/
		bool BundleAdjust::formMeasureNormals(LinearAlgebra::MatrixUpperTriangular &N22,
		SparseBlockColumnMatrix &N12,
		@@ -1609,6 +1554,9 @@ 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();

		@@ -1775,35 +1723,6 @@ bool BundleAdjust::formNormalEquations() {
		}


		/**
		* Perform the matrix multiplication v2 = alpha ( Q x v1 ).
		*
		* @param alpha A constant multiplier.
		* @param v2 The output vector.
		* @param Q A sparse block matrix.
		* @param v1 A vector.
		*/
		void BundleAdjust::productAlphaAV(double alpha, bounded_vector<double,3> &v2,
		SparseBlockRowMatrix &Q,
		vector<double> &v1) {

		QMapIterator< int, LinearAlgebra::Matrix * > Qit(Q);

		int subrangeStart, subrangeEnd;

		while ( Qit.hasNext() ) {
		Qit.next();

		int columnIndex = Qit.key();

		subrangeStart = m_sparseNormals.at(columnIndex)->startColumn();
		subrangeEnd = subrangeStart + Qit.value()->size2();

		v2 += alpha * prod(*(Qit.value()),subrange(v1,subrangeStart,subrangeEnd));
		}
		}


		/**
		* Add piecewise polynomial continuity constraints to normals equations
		*
		@@ -2494,7 +2413,7 @@ bool BundleAdjust::formNormalEquations() {
		// we set the measures polynomial segment indices and position and pointing matrix blocks
		// once only, in the first iteration.
		// NOTE: for time dependent sensors, Camera::SetImage MUST be called prior to
		// setPolySegementIndices
		// setPolySegmentIndices
		// TODO: should we do this in initialization? But SetImage would have to be called there for
		// time dependent sensors.
		if (m_iteration == 1) {
		@@ -2643,7 +2562,7 @@ bool BundleAdjust::formNormalEquations() {

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

		observationWeight = 1.0 / observationSigma;
		@@ -2679,7 +2598,7 @@ bool BundleAdjust::formNormalEquations() {
		*/
		void BundleAdjust::applyParameterCorrections() {

		qDebug() << m_imageSolution;
		// qDebug() << m_imageSolution;

		int t = 0;

		@@ -2743,7 +2662,7 @@ bool BundleAdjust::formNormalEquations() {
		// vtpv for image coordinates
		int numObjectPoints = m_bundleControlPoints.size();

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

		for (int i = 0; i < numObjectPoints; i++) {
		@@ -2758,7 +2677,7 @@ bool BundleAdjust::formNormalEquations() {

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

		weight = 1.0 / weight;
		@@ -2768,18 +2687,27 @@ bool BundleAdjust::formNormalEquations() {
		vy = measure->focalPlaneMeasuredY() - measure->focalPlaneComputedY();

		// TODO: Testing - correct lidar simultaneous measure by it's residual
		if ( point->id().contains("Lidar", Qt::CaseInsensitive) ) {
		double newsample = measure->sample() - measure->sampleResidual();
		double newline = measure->line() - measure->lineResidual();
		// if ( point->id().contains("Lidar", Qt::CaseInsensitive) ) {

		measure->rawMeasure()->SetCoordinate(newsample, newline);
		Camera* camera = measure->camera();
		// double sample = measure->sample();
		// double line = measure->line();

		camera->SetImage(newsample,newline);
		double newx = camera->DistortionMap()->UndistortedFocalPlaneX();
		double newy = camera->DistortionMap()->UndistortedFocalPlaneY();
		measure->rawMeasure()->SetFocalPlaneMeasured(newx,newy);
		}
		// // 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();

		// measure->rawMeasure()->SetCoordinate(newsample, newline);
		// Camera* camera = measure->camera();

		// camera->SetImage(newsample,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()) {
		@@ -3415,6 +3343,16 @@ bool BundleAdjust::formNormalEquations() {
		}


		/**
		* Returns a pointer to the output lidar data file.
		*
		* @return LidarDataQsp A shared pointer to the output lidar data file.
		*/
		LidarDataQsp BundleAdjust::lidarData() {
		return m_lidarDataSet;
		}


		/**
		* Returns a pointer to the serial number list.
		*
		@@ -3580,7 +3518,7 @@ bool BundleAdjust::formNormalEquations() {
		* @param status The bundle status string to output.
		*
		* @internal
		* @history 2016-12-01 Ian Humphrey - Added %s as first paramter to prevent a
		* @history 2016-12-01 Ian Humphrey - Added %s as first parameter to prevent a
		* -Wformat-security warning during the build.
		*/
		void BundleAdjust::outputBundleStatus(QString status) {