Unverified Commit e9c53955 authored by Jesse Mapel's avatar Jesse Mapel Committed by GitHub
Browse files

Adding lidar support in the Bundle Adjustment (#4878) 



* Lidar points (#4709)

* Added LidarControlPoint

* Hand merge Lidar changes to ControlPoint

* Hand merge changes to ControlPoint for Lidar

* Minor build fix

* Fixed negation

* Moved LidarControlPoint test to gtest

* BundleObservationVector; initial commit

* Adding lrolola2isis (#4712)

* Adding lrolola2isis; initial commit

* created new data for old tests; all passing

* adding assesstfor future documentation examples

* removing test data

* Lrolola2isis gtest conversion (#4723)

* initial gtest commit

* removing c_str call

* Update Bundle Utility classes for LiDAR work (#4716)

* Initial utilities hand merge

* Clean up for tests

* Fixed disclaimer for new files

* Removed BundleConstraint

* Moved Constraint into utilities folder

* Fixed header and SpciePosition test

* Moved BundleResults unit test to GTest

* Moved BundleSolutionInfo test to gtest

* BundleControlPoint gtest

* Added constrained point test

* updated BundleResults saving and test

* Updated BundleSolutionInfo Test

* Initial Lidar Network fixture (#4875)

* Initial Lidar Network fixture

* Initial FunctionalTestJigsawLidar

* Added data tests for lidar

* Compiling BundleAdjust

* In progress debug

* Jigsaw tests passing

* Passing tests

Co-authored-by: default avatarAustin Sanders <austinsanders1993@gmail.com>

* Added lidar fixtures

* Added lidar fixtures

* Added namespace to json

* Fixed Sigma0Computation test

Co-authored-by: default avatarladoramkershner <ladoramkershner@gmail.com>
Co-authored-by: default avatarLauren Adoram-Kershner <42873279+ladoramkershner@users.noreply.github.com>
Co-authored-by: default avatarAustin Sanders <austinsanders1993@gmail.com>
Co-authored-by: default avatarAustin Sanders <arsanders@usgs.gov>
parent de865f67

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

0 → 100644
+699 −0

File added.

Preview size limit exceeded, changes collapsed.

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

0 → 100644
+102 −0
Original line number Diff line number Diff line 
#ifndef LidarData_h

#define LidarData_h



#include <QHash>

#include <QList>

#include <QMap>

#include <QPointer>

#include <QString>

#include <QVector>



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

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



#include "LidarControlPoint.h"



class QJsonObject;



namespace Isis {

  class Camera;

  class ControlMeasure;

  class ControlNet;

  class FileName;

  class Progress;

  class SerialNumberList;



  /**

   * LidarData class.

   *

   * @author 2018-01-29 Ian Humphrey

   *

   * @internal

   *   @history 2018-01-29 Ian Humphrey - original version.

   *   @history 2018-01-31 Tyler Wilson - Implemented Lidar::read(Filename &).

   *   @history 2018-01-31 Ian Humphrey - Added insert method to insert a

   *                           LidarControlPoint into the LidarData. Added

   *                           documentation for m_points.

   *   @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.

   *   @history 2018-06-14 Ken Edmundson - Added typedef for QSharedPointer to LidarData object.

   *   @history 2019-02-23 Debbie A. Cook - Added sorting option to points() method with default

   *                           being to not sort. References #5343.

   *

   */

  class LidarData {



    public:

      /** Enumerates the file formats for serializing the LidarData class. */

      enum Format {

        Binary, /**< Serializes to a binary (QByteArray) .dat file. */

        Json,    /**< Serializes to a JSON .json file. */

        Test    /**< Serializes to an ordered JSON .json file for comparing to truth data. */

      };



      LidarData();



      void insert(QSharedPointer<LidarControlPoint> point);



      QSharedPointer<LidarControlPoint> point(QString pointId) const;

      QList< QSharedPointer<LidarControlPoint> > points(bool sort = false) const;



      void SetImages(SerialNumberList &list, Progress *progress = 0);

      void SetImages(ControlNet &controlNet, Progress *progress = 0);



      // Serialization methods for LidarData

      void read(FileName);

      void write(FileName, Format);



      int numberLidarPoints();

      int numberSimultaneousMeasures();

      int numberAsynchronousMeasures();

      int numberMeasures();



      bool ValidateSerialNumber(QString serialNumber) const;

      QList< ControlMeasure * > GetMeasuresInCube(QString serialNumber);

      QList< ControlMeasure * > GetValidMeasuresInCube(QString serialNumber);

      int GetNumberOfValidMeasuresInImage(const QString &serialNumber);

      int GetNumberOfJigsawRejectedMeasuresInImage(const QString &serialNumber);



    private:

      QHash<QString, QSharedPointer <LidarControlPoint> > m_points; //!< hash of LidarControlPoints



                                                                    //!< maps between serial# and...

      QMap<QString, Isis::Camera *> p_cameraMap;                    //!< camera

      QMap<QString, int> p_cameraValidMeasuresMap;                  //!< #measures

      QMap<QString, int> p_cameraRejectedMeasuresMap;               //!< #rejected measures



      QVector<Isis::Camera *> p_cameraList;                         //!< vector of image# to camera



      int m_numSimultaneousMeasures;

      int m_numAsynchronousMeasures;

  };



  // typedefs

  //! Definition for a shared pointer to a LidarData object.

  typedef QSharedPointer<LidarData> LidarDataQsp;

}

#endif

isis/src/base/objs/LidarData/LidarData.truth

0 → 100644
+522 −0
Original line number Diff line number Diff line 
Testing default constructor... 

	number of points: 0



Testing insert(QSharedPointer<LidarControlPoint>)... 

	number of points: 1

	name of point:    testLidarControlPoint

	time of point:    2018-01-31T14:05:00.1233999



Testing write(FileName)... 

json



json

dat

./test.dat

Testing read(FileName) from binary data... 

LidarData:

	LidarControlPoint:

		id: testLidarControlPoint1

		latitude:  51

		longitude: 101

		radius:    1000

		range:     65

		sigmaRange:0.1

		time:      2018-01-31T14:06:00.1233999

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  51.2

		adjustedLongitude: 101.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 1

			SN:     SN_1-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 1

			SN:     SN_1-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint2

		latitude:  52

		longitude: 102

		radius:    1000

		range:     75

		sigmaRange:0.1

		time:      2018-01-31T14:07:00.1233999

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  52.2

		adjustedLongitude: 102.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 2

			SN:     SN_2-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 2

			SN:     SN_2-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint3

		latitude:  53

		longitude: 103

		radius:    1000

		range:     85

		sigmaRange:0.1

		time:      2018-01-31T14:08:00.1233999

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  53.2

		adjustedLongitude: 103.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 3

			SN:     SN_3-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 3

			SN:     SN_3-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint4

		latitude:  54

		longitude: 104

		radius:    1000

		range:     95

		sigmaRange:0.1

		time:      2018-01-31T14:09:00.1233999

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  54.2

		adjustedLongitude: 104.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 4

			SN:     SN_4-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 4

			SN:     SN_4-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint5

		latitude:  55

		longitude: 105

		radius:    1000

		range:     105

		sigmaRange:0.1

		time:      2018-01-31T14:10:00.1233999

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  55.2

		adjustedLongitude: 105.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 5

			SN:     SN_5-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 5

			SN:     SN_5-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint6

		latitude:  56

		longitude: 106

		radius:    1000

		range:     115

		sigmaRange:0.1

		time:      2018-01-31T14:11:00.1233999

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  56.2

		adjustedLongitude: 106.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 6

			SN:     SN_6-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 6

			SN:     SN_6-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint7

		latitude:  57

		longitude: 107

		radius:    1000

		range:     125

		sigmaRange:0.1

		time:      2018-01-31T14:12:00.1233998

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  57.2

		adjustedLongitude: 107.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 7

			SN:     SN_7-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 7

			SN:     SN_7-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint8

		latitude:  58

		longitude: 108

		radius:    1000

		range:     135

		sigmaRange:0.1

		time:      2018-01-31T14:13:00.1233998

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  58.2

		adjustedLongitude: 108.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 8

			SN:     SN_8-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 8

			SN:     SN_8-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint9

		latitude:  59

		longitude: 109

		radius:    1000

		range:     145

		sigmaRange:0.1

		time:      2018-01-31T14:14:00.1233998

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  59.2

		adjustedLongitude: 109.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 9

			SN:     SN_9-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 9

			SN:     SN_9-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint10

		latitude:  60

		longitude: 110

		radius:    1000

		range:     155

		sigmaRange:0.1

		time:      2018-01-31T14:15:00.1233998

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  60.2

		adjustedLongitude: 110.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 10

			SN:     SN_10-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 10

			SN:     SN_10-1

		#END_ControlMeasure.

	#END_LidarControlPoint.







Testing read(FileName) from JSON data... 

LidarData:

	LidarControlPoint:

		id: testLidarControlPoint1

		latitude:  51

		longitude: 101

		radius:    1000

		range:     65

		sigmaRange:0.1

		time:      2018-01-31T14:06:00.1233999

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  51.2

		adjustedLongitude: 101.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 1

			SN:     SN_1-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 1

			SN:     SN_1-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint2

		latitude:  52

		longitude: 102

		radius:    1000

		range:     75

		sigmaRange:0.1

		time:      2018-01-31T14:07:00.1233999

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  52.2

		adjustedLongitude: 102.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 2

			SN:     SN_2-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 2

			SN:     SN_2-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint3

		latitude:  53

		longitude: 103

		radius:    1000

		range:     85

		sigmaRange:0.1

		time:      2018-01-31T14:08:00.1233999

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  53.2

		adjustedLongitude: 103.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 3

			SN:     SN_3-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 3

			SN:     SN_3-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint4

		latitude:  54

		longitude: 104

		radius:    1000

		range:     95

		sigmaRange:0.1

		time:      2018-01-31T14:09:00.1233999

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  54.2

		adjustedLongitude: 104.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 4

			SN:     SN_4-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 4

			SN:     SN_4-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint5

		latitude:  55

		longitude: 105

		radius:    1000

		range:     105

		sigmaRange:0.1

		time:      2018-01-31T14:10:00.1233999

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  55.2

		adjustedLongitude: 105.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 5

			SN:     SN_5-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 5

			SN:     SN_5-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint6

		latitude:  56

		longitude: 106

		radius:    1000

		range:     115

		sigmaRange:0.1

		time:      2018-01-31T14:11:00.1233999

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  56.2

		adjustedLongitude: 106.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 6

			SN:     SN_6-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 6

			SN:     SN_6-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint7

		latitude:  57

		longitude: 107

		radius:    1000

		range:     125

		sigmaRange:0.1

		time:      2018-01-31T14:12:00.1233998

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  57.2

		adjustedLongitude: 107.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 7

			SN:     SN_7-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 7

			SN:     SN_7-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint8

		latitude:  58

		longitude: 108

		radius:    1000

		range:     135

		sigmaRange:0.1

		time:      2018-01-31T14:13:00.1233998

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  58.2

		adjustedLongitude: 108.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 8

			SN:     SN_8-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 8

			SN:     SN_8-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint9

		latitude:  59

		longitude: 109

		radius:    1000

		range:     145

		sigmaRange:0.1

		time:      2018-01-31T14:14:00.1233998

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  59.2

		adjustedLongitude: 109.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 9

			SN:     SN_9-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 9

			SN:     SN_9-1

		#END_ControlMeasure.

	#END_LidarControlPoint.



	LidarControlPoint:

		id: testLidarControlPoint10

		latitude:  60

		longitude: 110

		radius:    1000

		range:     155

		sigmaRange:0.1

		time:      2018-01-31T14:15:00.1233998

		matrix:      [3,3]((0.01,0,0),(0,0.0121,0),(0,0,100))

		adjustedLatitude:  60.2

		adjustedLongitude: 110.1

		adjustedRadius:    1000

		adjustedMatrix:      [3,3]((0.02,0,0),(0,0.0144,0),(0,0,81))

		ControlMeasure: 

			line:   0

			sample: 10

			SN:     SN_10-0

		#END_ControlMeasure.

		ControlMeasure: 

			line:   1

			sample: 10

			SN:     SN_10-1

		#END_ControlMeasure.

	#END_LidarControlPoint.

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

0 → 100644
+221 −0
Original line number Diff line number Diff line 
#include "LidarData.h"



#include <QDebug>

#include <QString>



#include "Angle.h"

#include "ControlMeasure.h"

#include "Distance.h"

#include "FileName.h"

#include "iTime.h"

#include "Latitude.h"

#include "LidarControlPoint.h"

#include "Longitude.h"

#include "Preference.h"

#include "SurfacePoint.h"



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



using namespace std;

using namespace Isis;

using namespace boost::numeric::ublas;



void print(const LidarData &lidarData);



/**

 * Unit test for the LIDARData class.

 *

 * @internal

 *   @history 2018-01-29 Ian Humphrey - original version.

 *   @history 2018-01-31 Ian Humphrey - added tests for points() and insert().

 *   @history 2018-03-28 Debbie A Cook - added apriori surface point and 

 *                               adjusted surface point to the LidarData test.  Also

 *                               added a sort of the control points before outputting

 *                               them to compare to make sure they are in a 

 *                               consistent order.

 */

int main(int argc, char *argv[]) {

  // Set up our unit test preferences

  Preference::Preferences(true);



  // LidarData data;

  // FileName csvFile("RDR_98E100E_60N62NPointPerRow_csv_table-original.csv");

  // data.readCsv(csvFile);



  // Test LidarData()

  cout << "Testing default constructor... " << endl;

  LidarData defaultData;

  cout << "\tnumber of points: " << defaultData.points().size() << endl;

  cout << endl;



  // // Test LidarData(FileName)

  // cout << "Testing LidarData(FileName)... " << endl;

  // cout << endl;



  // Test insert(QSharedPointer<LidarControlPoint>)

  cout << "Testing insert(QSharedPointer<LidarControlPoint>)... " << endl;

  iTime time("2018-01-31T14:05:00.1234");

  double range = 55.0;

  double sigmaRange = 0.1;

  QSharedPointer<LidarControlPoint> lcp =

      QSharedPointer<LidarControlPoint>(new LidarControlPoint());

  lcp->setTime(time);

  iTime check = lcp->time();

  lcp->setRange(range);

  lcp->setSigmaRange(sigmaRange);

  lcp->SetId("testLidarControlPoint");

  defaultData.insert(lcp);

  cout << "\tnumber of points: " << defaultData.points().size() << endl;

  cout << "\tname of point:    " << defaultData.points().first()->GetId() << endl;

  cout << "\ttime of point:    " << defaultData.points().first()->time().UTC() << endl;

  cout << endl;



  // Test write() JSON format

  cout << "Testing write(FileName)... " << endl;

  LidarData mockData;

  double lat, lon, rad;

  lat = 50.0;

  lon = 100.0;

  rad = 1000.0;

  double latAd = 50.2;

  double lonAd = 100.1;

  double radAd = 1001.0;

  boost::numeric::ublas::symmetric_matrix<double, upper> aprioriMatrix(3);

  aprioriMatrix.clear();

  aprioriMatrix(0, 0) = .01;

  aprioriMatrix(1, 1) = 0.0121;

  aprioriMatrix(2, 2) = 100.;

  boost::numeric::ublas::symmetric_matrix<double, upper> adjustedMatrix(3);

  adjustedMatrix.clear();

  adjustedMatrix(0, 0) = .02;

  adjustedMatrix(1, 1) = 0.0144;

  adjustedMatrix(2, 2) = 81.;

  

  for (int i = 1; i < 11; i++) {

    time += 60.0;

    range += 10.0;

    lcp = QSharedPointer<LidarControlPoint>(new LidarControlPoint());

    lcp->setTime(time);

    lcp->setRange(range);

    lcp->setSigmaRange(sigmaRange);

    lcp->SetId("testLidarControlPoint" + QString::number(i));

    lat += 1.0;

    lon += 1.0;

    SurfacePoint sp(Latitude(lat, Angle::Units::Degrees),

                    Longitude(lon, Angle::Units::Degrees),

                    Distance(rad, Distance::Units::Kilometers));

    latAd += 1.0;

    lonAd += 1.0;

    SurfacePoint sp2(Latitude(latAd, Angle::Units::Degrees),

                    Longitude(lonAd, Angle::Units::Degrees),

                    Distance(radAd, Distance::Units::Kilometers));

    sp.SetSphericalMatrix(aprioriMatrix);

    lcp->SetAprioriSurfacePoint(sp);

    sp2.SetSphericalMatrix(adjustedMatrix);

    lcp->SetAdjustedSurfacePoint(sp2);

    for (int j = 0; j < 2; j++) {

      ControlMeasure *measure = new ControlMeasure();

      measure->SetCoordinate((double) i, (double) j);

      measure->SetCubeSerialNumber("SN_" + QString::number(i) + "-" + QString::number(j));

      lcp->Add(measure);

    }

    mockData.insert(lcp);

  }

  FileName outputFile("./test.json");

  cout << outputFile.extension() << endl;

  mockData.write(outputFile, LidarData::Json);

  cout << endl;



  // Test write() with no data



  // Test read() with no data



  // Test write() binary format

  cout << outputFile.extension() << endl;

  outputFile = outputFile.setExtension("dat");

  cout << outputFile.extension() << endl;

  cout << outputFile.expanded().toStdString() << endl;

  mockData.write(outputFile, LidarData::Binary);



  // Test read() binary format

  cout << "Testing read(FileName) from binary data... " << endl;

  LidarData fromBinary;

  fromBinary.read(outputFile);

  print(fromBinary);

  cout << endl;



  // Test read()

  cout << "Testing read(FileName) from JSON data... " << endl;

  LidarData fromJson;

  outputFile = outputFile.setExtension("json");

  fromJson.read(outputFile);

  print(fromJson);

  cout << endl;



  // Clean up

  remove("test.dat");

  remove("test.json");



}





// int cmp (QSharedPointer<LidarControlPoint> lcp1, QSharedPointer<LidarControlPoint> lcp2) {

int cmpLessThan (QSharedPointer<LidarControlPoint> lcp1, QSharedPointer<LidarControlPoint> lcp2) {

  // Trim off the id name and only keep the number to compare

  int num1 = ((lcp1->GetId()).remove(0, 21)).toInt();

  int num2 = ((lcp2->GetId()).remove(0, 21)).toInt();

  return num1 < num2;

}





void print(const LidarData &lidarData) {

  QList< QSharedPointer<LidarControlPoint> > points = lidarData.points();

  

  // Order the control points so test runs will list points in a consistent order

  qSort(points.begin(), points.end(), cmpLessThan);

  

  std::cout << "LidarData:" << std::endl;

  foreach (QSharedPointer<LidarControlPoint> point, points) {

    std::cout << "\tLidarControlPoint:" << std::endl;

    std::cout << "\t\tid: " << point->GetId() << std::endl;;



    // Print the apriori surface point

    SurfacePoint sp = point->GetAprioriSurfacePoint();

    double lat, lon, rad;

    lat = sp.GetLatitude().planetocentric(Angle::Units::Degrees);

    lon = sp.GetLongitude().positiveEast(Angle::Units::Degrees);

    rad = sp.GetLocalRadius().kilometers();

    symmetric_matrix<double, upper> aprioriMatrix = sp.GetSphericalMatrix();

    std::cout << "\t\tlatitude:  " << lat << std::endl;

    std::cout << "\t\tlongitude: " << lon << std::endl;

    std::cout << "\t\tradius:    " << rad << std::endl;

    std::cout << "\t\trange:     " << point->range() << std::endl;

    std::cout << "\t\tsigmaRange:" << point->sigmaRange() << std::endl;

    // std::cout << "\t\ttime:      " << point->time().Et() << std::endl;

    std::cout << "\t\ttime:      " << point->time().UTC() << std::endl;

    std::cout << "\t\tmatrix:      " << aprioriMatrix << std::endl;



    // Print the adjusted surface point

    SurfacePoint sp2 = point->GetAdjustedSurfacePoint();

    double lat2, lon2, rad2;

    lat2 = sp2.GetLatitude().planetocentric(Angle::Units::Degrees);

    lon2 = sp2.GetLongitude().positiveEast(Angle::Units::Degrees);

    rad2 = sp2.GetLocalRadius().kilometers();

    symmetric_matrix<double, upper> adjustedMatrix = sp2.GetSphericalMatrix();

    std::cout << "\t\tadjustedLatitude:  " << lat2 << std::endl;

    std::cout << "\t\tadjustedLongitude: " << lon2 << std::endl;

    std::cout << "\t\tadjustedRadius:    " << rad2 << std::endl;

    std::cout << "\t\tadjustedMatrix:      " << adjustedMatrix << std::endl;

    

    QList<ControlMeasure *> measures = point->getMeasures();

    foreach (ControlMeasure *measure, measures) {

      std::cout << "\t\tControlMeasure: " << std::endl;

      std::cout << "\t\t\tline:   " << measure->GetLine() << std::endl;

      std::cout << "\t\t\tsample: " << measure->GetSample() << std::endl;

      std::cout << "\t\t\tSN:     " << measure->GetCubeSerialNumber() << std::endl;

      std::cout << "\t\t#END_ControlMeasure." << std::endl;

    }

    std::cout << "\t#END_LidarControlPoint." << std::endl << std::endl;

  }

  std::cout << std::endl;

}