Added remaining information to LidarData necessary for the bundle adjustment. ... (fae5ed6b) · Commits · aflab / astrogeology / ISIS3

isis/src/base/objs/LidarData/LidarData.cpp

+125 −8

Original line number	Diff line number	Diff line
		@@ -26,6 +26,7 @@
		#include "SerialNumberList.h"
		#include "SurfacePoint.h"

		using namespace boost::numeric::ublas;

		namespace Isis {

		@@ -210,9 +211,72 @@ namespace Isis {
		lcp->setTime(iTime(time));
		lcp->setRange(range);
		lcp->setSigmaRange(sigmaRange);

		if (pointObject.contains("aprioriMatrix") &&
		pointObject["aprioriMatrix"].isArray()) {
		QJsonArray aprioriMatrixArray = pointObject["apriorMatrix"].toArray();
		boost::numeric::ublas::symmetric_matrix<double, upper> aprioriMatrix(3);
		aprioriMatrix.clear();
		aprioriMatrix(0, 0) = aprioriMatrixArray[0].toDouble();
		aprioriMatrix(0, 1) = aprioriMatrixArray[1].toDouble();
		aprioriMatrix(0, 2) = aprioriMatrixArray[2].toDouble();
		aprioriMatrix(1, 1) = aprioriMatrixArray[3].toDouble();
		aprioriMatrix(1, 2) = aprioriMatrixArray[4].toDouble();
		aprioriMatrix(2, 2) = aprioriMatrixArray[5].toDouble();

		lcp->SetAprioriSurfacePoint(SurfacePoint(Latitude(latitude, Angle::Units::Degrees),
		Longitude(longitude, Angle::Units::Degrees),
		Distance(radius, Distance::Units::Kilometers),
		aprioriMatrix));
		lcp->SetType(ControlPoint::Constrained);
		}
		else {
		lcp->SetAprioriSurfacePoint(SurfacePoint(Latitude(latitude, Angle::Units::Degrees),
		Longitude(longitude, Angle::Units::Degrees),
		Distance(radius, Distance::Units::Kilometers)));
		}

		// Set the adjusted surface point if it exists
		if (pointObject.contains("adjustedLatitude") &&
		pointObject["adjustedLatitude"].isDouble() &&
		pointObject.contains("adjustedLongitude") &&
		pointObject["adjustedLongitude"].isDouble() &&
		pointObject.contains("adjustedRadius") &&
		pointObject["adjustedRadius"].isDouble()) {

		double adjustedLatitude = 0.0;
		adjustedLatitude = pointObject["adjustedLatitude"].toDouble();

		double adjustedLongitude = 0.0;
		adjustedLongitude = pointObject["adjustedLongitude"].toDouble();

		double adjustedRadius = 0.0;
		adjustedRadius = pointObject["radius"].toDouble();

		if (pointObject.contains("adjustedMatrix") &&
		pointObject["adjustedMatrix"].isArray()) {
		QJsonArray adjustedMatrixArray = pointObject["adjustedMatrix"].toArray();
		boost::numeric::ublas::symmetric_matrix<double, upper> adjustedMatrix(3);
		adjustedMatrix.clear();
		adjustedMatrix(0, 0) = adjustedMatrixArray[0].toDouble();
		adjustedMatrix(0, 1) = adjustedMatrixArray[1].toDouble();
		adjustedMatrix(0, 2) = adjustedMatrixArray[2].toDouble();
		adjustedMatrix(1, 1) = adjustedMatrixArray[3].toDouble();
		adjustedMatrix(1, 2) = adjustedMatrixArray[4].toDouble();
		adjustedMatrix(2, 2) = adjustedMatrixArray[5].toDouble();

		lcp->SetAdjustedSurfacePoint(SurfacePoint(Latitude(adjustedLatitude, Angle::Units::Degrees),
		Longitude(adjustedLongitude, Angle::Units::Degrees),
		Distance(adjustedRadius, Distance::Units::Kilometers),
		adjustedMatrix));
		lcp->SetType(ControlPoint::Constrained);
		}
		else {
		lcp->SetAdjustedSurfacePoint(SurfacePoint(Latitude(adjustedLatitude, Angle::Units::Degrees),
		Longitude(adjustedLongitude, Angle::Units::Degrees),
		Distance(adjustedRadius, Distance::Units::Kilometers)));
		}
		}

		if (pointObject.contains("simultaneousImages") &&
		pointObject["simultaneousImages"].isArray()) {
		@@ -305,12 +369,65 @@ namespace Isis {
		pointObject["range"] = lcp->range();
		pointObject["sigmaRange"] = lcp->sigmaRange();
		pointObject["time"] = lcp->time().Et();
		// Serialize the lat/lon/radius (AprioriSurfacePoint)

		// Serialize the AprioriSurfacePoint
		SurfacePoint aprioriSurfacePoint = lcp->GetAprioriSurfacePoint();
		if (aprioriSurfacePoint.Valid()) {
		pointObject["latitude"] = aprioriSurfacePoint.GetLatitude().planetocentric(Angle::Units::Degrees);
		pointObject["longitude"] = aprioriSurfacePoint.GetLongitude().positiveEast(Angle::Units::Degrees);
		pointObject["radius"] = aprioriSurfacePoint.GetLocalRadius().kilometers();

		// Serialize the apriori matrix
		symmetric_matrix<double, upper> aprioriMatrix = aprioriSurfacePoint.GetSphericalMatrix();
		QJsonArray aprioriMatrixArray;
		aprioriMatrixArray += toString(aprioriMatrix(0, 0));
		aprioriMatrixArray += toString(aprioriMatrix(0, 1));
		aprioriMatrixArray += toString(aprioriMatrix(0, 2));
		aprioriMatrixArray += toString(aprioriMatrix(1, 1));
		aprioriMatrixArray += toString(aprioriMatrix(1, 2));
		aprioriMatrixArray += toString(aprioriMatrix(2, 2));

		// If the covariance matrix has a value, add it to the PVL point.
		if ( aprioriMatrix(0, 0) != 0.0
		\|\| aprioriMatrix(0, 1) != 0.0
		\|\| aprioriMatrix(0, 2) != 0.0
		\|\| aprioriMatrix(1, 1) != 0.0
		\|\| aprioriMatrix(1, 2) != 0.0
		\|\| aprioriMatrix(2, 2) != 0.0 ) {
		pointObject["aprioriMatrix"] = aprioriMatrixArray;
		}
		}

		// Serialize the AdjustedSurfacePoint
		SurfacePoint adjustedSurfacePoint = lcp->GetAdjustedSurfacePoint();
		if (adjustedSurfacePoint.Valid()) {
		pointObject["adjustedLatitude"] =
		adjustedSurfacePoint.GetLatitude().planetocentric(Angle::Units::Degrees);
		pointObject["adjustedLongitude"] =
		adjustedSurfacePoint.GetLongitude().positiveEast(Angle::Units::Degrees);
		pointObject["adjustedRadius"] = adjustedSurfacePoint.GetLocalRadius().kilometers();

		// Serialize the adjusted matrix
		symmetric_matrix<double, upper> adjustedMatrix = adjustedSurfacePoint.GetSphericalMatrix();
		QJsonArray adjustedMatrixArray;
		adjustedMatrixArray += toString(adjustedMatrix(0, 0));
		adjustedMatrixArray += toString(adjustedMatrix(0, 1));
		adjustedMatrixArray += toString(adjustedMatrix(0, 2));
		adjustedMatrixArray += toString(adjustedMatrix(1, 1));
		adjustedMatrixArray += toString(adjustedMatrix(1, 2));
		adjustedMatrixArray += toString(adjustedMatrix(2, 2));

		// If the covariance matrix has a value, add it to the PVL point.
		if ( adjustedMatrix(0, 0) != 0.0
		\|\| adjustedMatrix(0, 1) != 0.0
		\|\| adjustedMatrix(0, 2) != 0.0
		\|\| adjustedMatrix(1, 1) != 0.0
		\|\| adjustedMatrix(1, 2) != 0.0
		\|\| adjustedMatrix(2, 2) != 0.0 ) {
		pointObject["adjustedMatrix"] = adjustedMatrixArray;
		}
		}

		// Serialize the list of simultaneous images
		QJsonArray simultaneousArray;
		foreach (QString sn, lcp->snSimultaneous()) {

isis/src/base/objs/LidarData/LidarData.h

+8 −0

Original line number	Diff line number	Diff line
		@@ -8,6 +8,9 @@
		#include <QString>
		#include <QVector>

		#include "boost/numeric/ublas/symmetric.hpp"
		#include "boost/numeric/ublas/io.hpp"

		class QJsonObject;

		namespace Isis {
		@@ -31,6 +34,11 @@ namespace Isis {
		* @history 2018-02-03 Ian Humphrey - Renamed read to readCsv. read() and write()
		* methods support JSON or binary serialization. Added
		* documentation to new Format enumeration.
		* @history 2018-03-19 Debbie A. Cook - Added simultaneousImages,
		* apriori variance/covariance matrix, adjusted point coordinates,
		* and adjusted variance/covariance matrix to the read and
		* write methods. Ref #5343.
		*
		*/
		class LidarData {

isis/src/base/objs/Target/Target.cpp

+31 −7

Original line number	Diff line number	Diff line
		@@ -65,6 +65,21 @@ namespace Isis {

		PvlGroup &inst = lab.findGroup("Instrument", Pvl::Traverse);
		m_name = new QString;

		// * TODO *
		// Is TargetName a required keyword in the Isis labels? If so then if this call fails, the class
		// should throw an error. If not then what are the alternatives? Each alternative should be
		// attempted and an error thrown in the case all options fail. Basically this value should be
		// loaded when the objected is created, or the object should not be created. How can we
		// tell if the request for the TargetName keyword fails to return anything? Should the
		// NaifSpkCode, if it exists, override the code associated with the target name in all cases
		// and not just he Sky case? The trykey settings below will not work for getting m_bodyCode.
		// The frame code is totally different from the body code usually. SpkCode may work, because
		// the spk will contail information describing the bode and not its frame.
		//
		// Basically, this constructor should load the specified values, including body code and radii,
		// or throw an error.

		*m_name = inst["TargetName"][0];
		QString trykey = "NaifIkCode";

		@@ -197,14 +212,23 @@ namespace Isis {
		catch (IException &e) {
		try {
		if (m_spice) {
		code = m_spice->getInteger("BODY_FRAME_CODE", 0);
		code = m_spice->getInteger("NAIF_BODY_CODE", 0); // Changed from BODY_FRAME_CODE
		// which is wrong. It is a totally different code and not used to label the radii.
		// Stuart says some missions use the frame code to label radii. Do we need
		// to code for both? Discuss this with Stuart when he returns. 03-19-2018 DAC
		return code;
		}
		else if (lab.hasObject("NaifKeywords")
		&& lab.findObject("NaifKeywords").hasKeyword("BODY_FRAME_CODE") ) {
		code = int(lab.findObject("NaifKeywords").findKeyword("BODY_FRAME_CODE"));
		return code;
		}
		// Add this check for NAIF_SPK_CODE if NAIF_BODY_CODE fails. Why not just call NaifBodyCode()
		// or NaifSpkCode()?
		}
		// else if (lab.hasObject("NaifKeywords")
		// && lab.findObject("NaifKeywords").hasKeyword("NAIF_BODY_CODE") ) {
		// // * TODO *
		// // Change from BODY_FRAME_CODE which is not the same as body code. DAC 3-7-18
		// // I don't think this keyword is used in naifkeywords. Maybe this should search the kernels group.
		// // For now comment this section out, because it isn't doing anything.
		// code = int(lab.findObject("NaifKeywords").findKeyword("NAIF_BODY_CODE"));
		// return code;
		// }
		else {
		throw IException(e,
		IException::Unknown,

isis/src/control/objs/LidarControlPoint/LidarControlPoint.h

+2 −0

Original line number	Diff line number	Diff line
		@@ -42,6 +42,7 @@ namespace Isis {
		* @internal
		* @history 2018-01-29 Makayla Shepherd Original version
		* @history 2018-02-09 Ken Edmundson Added typedef forLidarControlPointQsp
		* @history 2018-03-18 Debbie A. Cook Added Simultaneous measures
		*/

		class LidarControlPoint : public ControlPoint {
		@@ -57,6 +58,7 @@ namespace Isis {
		ControlPoint::Status setTime(iTime time);
		ControlPoint::Status addSimultaneous(QString newSerial);


		double range();
		double sigmaRange();
		iTime time();

isis/src/lro/apps/lrolola2isis/lrolola2isis.cpp

+50 −11

Original line number	Diff line number	Diff line
		@@ -17,6 +17,7 @@
		#include "LidarControlPoint.h"
		#include "LidarData.h"
		#include "Longitude.h"
		#include "Target.h"
		#include "SerialNumberList.h"
		#include "UserInterface.h"

		@@ -38,6 +39,10 @@ void IsisMain() {
		FileName dataFile = ui.GetFileName("FROM");
		SerialNumberList cubeList = SerialNumberList(ui.GetFileName("CUBES"));
		double threshold = ui.GetDouble("THRESHOLD");
		double rangeSigma = ui.GetDouble("POINT_RANGE_SIGMA");
		double latSigma = ui.GetDouble("POINT_LATITUDE_SIGMA");
		double lonSigma = ui.GetDouble("POINT_LONGITUDE_SIGMA");
		double radiusSigma = ui.GetDouble("POINT_RADIUS_SIGMA");

		QList<LidarCube> images;

		@@ -60,6 +65,10 @@ void IsisMain() {
		lidarDataFile.read(dataFile.expanded());
		LidarData lidarDataSet;
		CubeManager cubeMgr;
		Distance majorAx;
		Distance minorAx;
		Distance polarAx;
		bool setSurfacePointRadii = true;

		// Set up an automatic id generator for the point ids
		ID pointId = ID(ui.GetString("POINTID"));
		@@ -73,15 +82,21 @@ void IsisMain() {
		Longitude lon(row[1].toDouble(), Angle::Units::Degrees);
		Distance radius(row[3].toDouble(), Distance::Units::Kilometers);
		double range = row[4].toDouble();
		double sigma = 0; //TODO figure out how/where to calculate this
		// QString quality = row[]; //TODO figure out how/where to find this value

		LidarControlPoint *lidarPoint = new LidarControlPoint;
		lidarPoint->SetId(pointId.Next());
		lidarPoint->setTime(time);
		lidarPoint->setRange(range);
		lidarPoint->setSigmaRange(sigma);
		lidarPoint->SetAprioriSurfacePoint(SurfacePoint(lat, lon, radius));
		lidarPoint->setSigmaRange(rangeSigma);

		// Just set the point coordinates for now. We need to wait until we set
		// the target radii to be able to set the coordinate sigmas. The sigmas
		// will be converted to angles and the target radii are needed to do that.
		SurfacePoint spoint(lat, lon, radius);
		// lidarPoint->SetAprioriSurfacePoint(SurfacePoint(lat, lon, radius));

		bool setSurfacePointRadii = true;

		for (int j = 0; j < images.size(); j++) {
		Cube *cube = cubeMgr.OpenCube(images[j].name.expanded());
		@@ -102,6 +117,38 @@ void IsisMain() {
		measure->SetCoordinate(camera->Sample(), camera->Line());
		measure->SetCubeSerialNumber(images[j].sn);

		if (setSurfacePointRadii) {
		// Get the radii and set the radii in the SurfacePoint
		std::vector<Distance> targetRadii;
		targetRadii = camera->target()->radii();
		majorAx = targetRadii[0];
		minorAx = targetRadii[1];
		polarAx = targetRadii[2];
		setSurfacePointRadii = false;
		spoint.SetRadii(majorAx, minorAx, polarAx);
		spoint.SetSphericalSigmasDistance(
		Distance(latSigma, Distance::Units::Meters),
		Distance(lonSigma, Distance::Units::Meters),
		Distance(radiusSigma, Distance::Units::Meters));
		lidarPoint->SetAprioriSurfacePoint(spoint);
		// if (camera->target()->shape()->hasValidTarget()) {
		// targetRadii = camera->target()->shape()->targetRadii();
		// }
		// else {
		// QString msg = "Valid target not defined in shape model ";
		// throw IException(IException::Unknown, msg, _FILEINFO_);
		// }

		// targid = camera->SpkTargetId();
		// Distance targetRadii[3];
		// camera0>getDouble(
		// camera->radii(targetRadii);
		// majorAx = targetRadii[0];
		// minorAx = targetRadii[1];
		// polarAx = targetRadii[2];
		// setSurfacePointRadii = false;
		}

		lidarPoint->Add(measure);
		if (time >= images[j].startTime && time <= images[j].endTime) {
		QString newSerial = measure->GetCubeSerialNumber();
		@@ -121,14 +168,6 @@ void IsisMain() {
		}
		}
		// end image loop
		// Debug lines
		// if (lidarPoint->snSimultaneous().size() > 0) {
		// std::cout << "Number of simultaneous images is " <<
		// lidarPoint->snSimultaneous().size() << std::endl;
		// std::cout << "Row index = " << i << std::endl;
		// std::cout << " sn[0] = " << lidarPoint->snSimultaneous()[0] << std::endl;
		// }
		// End debug lines

		if (lidarPoint->GetNumMeasures() <= 0) {
		continue;