Factor out linescan logic in preparation for use in ASTER

src/asp/CMakeLists.txt

@@ -334,6 +334,9 @@ get_all_source_files( "PclIO/tests" ASP_PCLIO_TEST_FILES)
 set(ASP_PCLIO_LIB_DEPENDENCIES  pcl_common pcl_io_ply pcl_io
     AspCore ${VISIONWORKBENCH_LIBRARIES})
 
+# CERES and its dependencies
+set(SOLVER_LIBRARIES ${CERES_LIBRARIES} ${GLOG_LIBRARIES} ${GFLAGS_LIBRARIES})
+
 # Add all of the library subdirectories
 
 add_subdirectory(Core)

src/asp/Camera/ASTER_XML.cc

@@ -114,31 +114,27 @@ void ASTERXML::parse_xml(xercesc::DOMElement* node) {
   xercesc::DOMElement* sight_vector_node = get_node<DOMElement>(node, "SIGHT_VECTOR");
   std::string sight_vector_txt( XMLString::transcode(sight_vector_node->getTextContent()) );
   asp::read_matrix_from_string(sight_vector_txt, m_sight_mat);
-  if (m_lattice_mat.empty() || m_sight_mat.empty()     ||
-      m_lattice_mat.size()   != m_sight_mat.size()   ||
-      m_lattice_mat[0].size() != m_sight_mat[0].size() ) {
-    vw_throw( ArgumentErr() << "Inconsistent lattice point and sight vector information.\n");
+  if (m_lattice_mat.empty() || m_sight_mat.empty()  ||
+      m_lattice_mat.size()  != m_sight_mat.size()   ||
+      m_lattice_mat[0].size() != m_sight_mat[0].size()) {
+    vw_throw(ArgumentErr() 
+             << "Inconsistent lattice point and sight vector information.\n");
   }
 
   xercesc::DOMElement* world_sight_vector_node = get_node<DOMElement>(node, "WORLD_SIGHT_VECTOR");
   std::string world_sight_vector_txt(XMLString::transcode(world_sight_vector_node->getTextContent()));
   asp::read_matrix_from_string(world_sight_vector_txt, m_world_sight_mat);
   if (m_lattice_mat.empty()   || m_world_sight_mat.empty()  ||
       m_lattice_mat.size()    != m_world_sight_mat.size()   ||
-      m_lattice_mat[0].size() != m_world_sight_mat[0].size() ) {
-    vw_throw( ArgumentErr() << "Inconsistent lattice point and world sight vector information.\n");
+      m_lattice_mat[0].size() != m_world_sight_mat[0].size()) {
+    vw_throw(ArgumentErr() 
+             << "Inconsistent lattice point and world sight vector information.\n");
   }
 
-  std::vector< std::vector<vw::Vector3> > sat_pos_mat;
+  std::vector<std::vector<vw::Vector3>> sat_pos_mat;
   xercesc::DOMElement* sat_pos_node = get_node<DOMElement>(node, "SAT_POS");
   std::string sat_pos_txt( XMLString::transcode(sat_pos_node->getTextContent()) );
   asp::read_matrix_from_string(sat_pos_txt, sat_pos_mat);
-  for (size_t row = 0; row < sat_pos_mat.size(); row++) {
-    for (size_t col = 0; col < sat_pos_mat[row].size(); col++) {
-      //std::cout << sat_pos_mat[row][col] << " ";
-    }
-    //std::cout << std::endl;
-  }
   m_sat_pos = sat_pos_mat[0]; // Go from matrix with one row to a vector
 
   if (m_sat_pos.size() != m_sight_mat.size()) 

src/asp/Camera/CMakeLists.txt

@@ -1,3 +1,7 @@
 # Use wrapper function at this level to avoid code duplication
+
 add_library_wrapper(AspCamera "${ASP_CAMERA_SRC_FILES}" "${ASP_CAMERA_TEST_FILES}"
     "${ASP_CAMERA_LIB_DEPENDENCIES}")
+
+# This is needed for LinescanFit.cc. Not sure where a better place for it is.
+target_link_libraries(AspCamera ${SOLVER_LIBRARIES})

src/asp/Camera/CsmUtils.cc

@@ -19,11 +19,14 @@
 
 #include <asp/Camera/CsmModel.h>
 #include <asp/Camera/CsmUtils.h>
-#include <asp/Camera/JitterSolveUtils.h>
+#include <asp/Core/CameraTransforms.h>
+
 #include <vw/Cartography/GeoReference.h>
 #include <vw/Cartography/GeoReferenceBaseUtils.h>
 #include <vw/Cartography/GeoReferenceUtils.h>
 
+#include <usgscsm/UsgsAstroLsSensorModel.h>
+#include <usgscsm/UsgsAstroFrameSensorModel.h>
 #include <usgscsm/Utilities.h>
 
 namespace asp {
@@ -284,4 +287,139 @@ void orbitInterpExtrap(UsgsAstroLsSensorModel const * ls_model,
                     positions_out); // output
 }
 
+// See documentation in CsmUtils.h
+void populateCsmLinescan(int                          num_cams_in_image,         
+                      double                                orbit_len, 
+                      double                                velocity,
+                      double                                focal_length,
+                      vw::Vector2                   const & detector_origin,
+                      vw::Vector2i                  const & image_size,
+                      vw::cartography::Datum        const & datum, 
+                      std::string                   const & sensor_id, 
+                      std::map<int, vw::Vector3>    const & positions,
+                      std::map<int, vw::Matrix3x3>  const & cam2world,
+                      // Outputs
+                      asp::CsmModel & model) {
+
+  // Sanity checks
+  if (positions.size() != cam2world.size())
+    vw_throw(vw::ArgumentErr() << "Expecting as many positions as orientations.\n");
+ if (num_cams_in_image <= 0)
+    vw_throw(vw::ArgumentErr() << "Expecting positive number of cameras.\n");
+
+  // Do not use a precision below 1.0-e8 as then the linescan model will return junk.
+  model.m_desired_precision = asp::DEFAULT_CSM_DESIRED_PRECISION;
+  model.m_semi_major_axis = datum.semi_major_axis();
+  model.m_semi_minor_axis = datum.semi_minor_axis();
+
+  // Create the linescan model. Memory is managed by m_gm_model.
+  model.m_gm_model.reset(new UsgsAstroLsSensorModel);
+  UsgsAstroLsSensorModel* ls_model
+    = dynamic_cast<UsgsAstroLsSensorModel*>(model.m_gm_model.get());
+  if (ls_model == NULL)
+    vw::vw_throw(vw::ArgumentErr() << "Invalid initialization of the linescan model.\n");
+
+  // This performs many initializations apart from the above
+  ls_model->reset();
+
+  // Override some initializations
+  ls_model->m_nSamples         = image_size[0]; 
+  ls_model->m_nLines           = image_size[1];
+  ls_model->m_platformFlag     = 1; // Use 1, for order 8 Lagrange interpolation
+  ls_model->m_minElevation     = -10000.0; // -10 km
+  ls_model->m_maxElevation     =  10000.0; //  10 km
+  ls_model->m_focalLength      = focal_length;
+  ls_model->m_zDirection       = 1.0;
+  ls_model->m_halfSwath        = 1.0;
+  ls_model->m_sensorIdentifier = sensor_id;
+  ls_model->m_majorAxis        = model.m_semi_major_axis;
+  ls_model->m_minorAxis        = model.m_semi_minor_axis;
+
+  // The choices below are copied from the DigitalGlobe CSM linescan model.
+  // Better to keep same convention than dig deep inside UsAstroLsSensorModel.
+  // Also keep in mind that a CSM pixel has extra 0.5 added to it.
+  ls_model->m_iTransL[0]             = 0.0;  
+  ls_model->m_iTransL[1]             = 0.0;
+  ls_model->m_iTransL[2]             = 1.0;
+  ls_model->m_iTransS[0]             = 0.0;
+  ls_model->m_iTransS[1]             = 1.0;
+  ls_model->m_iTransS[2]             = 0.0;
+  ls_model->m_detectorLineOrigin     = 0.0;
+  ls_model->m_detectorSampleOrigin   = 0.0;
+  ls_model->m_detectorLineSumming    = 1.0;
+  // TODO(oalexan1): Must test with non-zero detector origin[1].
+  ls_model->m_startingDetectorLine   = detector_origin[1];
+  ls_model->m_detectorSampleSumming  = 1.0;
+  ls_model->m_startingDetectorSample = (detector_origin[0] - 0.5);
+
+  // Set the time. The first image line time is 0. The last image line time will
+  // depend on distance traveled and speed. We can have camera samples before
+  // the first and after the last image line. 
+  double beg_t = 0.0;
+  double end_t = orbit_len / velocity;
+  double dt = (end_t - beg_t) / (image_size[1] - 1.0);
+  ls_model->m_intTimeLines.push_back(1.0); // to offset CSM's quirky 0.5 additions in places
+  ls_model->m_intTimeStartTimes.push_back(beg_t);
+  ls_model->m_intTimes.push_back(dt);
+
+  // Positions and velocities. Note how, as above, there are more positions than
+  // num_cams_in_image, as they extend beyond orbital segment. So care is needed
+  // below. Time is 0 when we reach the first image line, and it is end_t at the
+  // last line. Positions before that have negative time. Time at position with
+  // index i is m_t0Ephem + i*m_dtEphem, if index 0 is for the earliest postion,
+  // but that is way before the orbital segment starting point which is the
+  // first image line.
+  int beg_pos_index = positions.begin()->first; // can be negative
+  if (beg_pos_index > 0)
+    vw::vw_throw(vw::ArgumentErr() << "First position index must be non-positive.\n");
+  ls_model->m_numPositions = 3 * positions.size(); // concatenate all coordinates
+  ls_model->m_dtEphem = (end_t - beg_t) / (num_cams_in_image - 1.0); // care here
+  ls_model->m_t0Ephem = beg_t + beg_pos_index * ls_model->m_dtEphem; // care here
+
+  ls_model->m_positions.resize(ls_model->m_numPositions);
+  ls_model->m_velocities.resize(ls_model->m_numPositions);
+  for (auto pos_it = positions.begin(); pos_it != positions.end(); pos_it++) {
+    int index = pos_it->first - beg_pos_index; // so we can start at 0
+    auto ctr = pos_it->second;
+    for (int coord = 0; coord < 3; coord++) {
+      ls_model->m_positions [3*index + coord] = ctr[coord];
+      ls_model->m_velocities[3*index + coord] = 0.0; // should not be used
+    }
+  }
+
+  // Orientations. Care with defining dt as above.
+  int beg_quat_index = cam2world.begin()->first;
+  if (beg_quat_index > 0)
+    vw::vw_throw(vw::ArgumentErr() << "First orientation index must be non-positive.\n");
+  if (beg_pos_index != beg_quat_index)
+    vw::vw_throw(vw::ArgumentErr() 
+      << "First position index must equal first orientation index.\n");
+
+  ls_model->m_numQuaternions = 4 * cam2world.size();
+  ls_model->m_dtQuat = (end_t - beg_t) / (num_cams_in_image - 1.0);
+  ls_model->m_t0Quat = beg_t + beg_quat_index * ls_model->m_dtQuat;
+
+  ls_model->m_quaternions.resize(ls_model->m_numQuaternions);
+  for (auto quat_it = cam2world.begin(); quat_it != cam2world.end(); quat_it++) {
+    int index = quat_it->first - beg_quat_index; // so we can start at 0
+
+    // Find the quaternion at this index.
+    auto c2w = quat_it->second;
+    double x, y, z, w;
+    asp::matrixToQuaternion(c2w, x, y, z, w);
+
+    // Note how we store the quaternions in the order x, y, z, w, not w, x, y, z.
+    int coord = 0;
+    ls_model->m_quaternions[4*index + coord] = x; coord++;
+    ls_model->m_quaternions[4*index + coord] = y; coord++;
+    ls_model->m_quaternions[4*index + coord] = z; coord++;
+    ls_model->m_quaternions[4*index + coord] = w; coord++;
+  }
+
+  // Re-creating the model from the state forces some operations to
+  // take place which are inaccessible otherwise.
+  std::string modelState = ls_model->getModelState();
+  ls_model->replaceModelState(modelState);
+}
+
 } // end namespace asp

src/asp/Camera/CsmUtils.h

@@ -22,20 +22,31 @@
 #ifndef __ASP_CAMERA_CSM_UTILS_H__
 #define __ASP_CAMERA_CSM_UTILS_H__
 
-#include <usgscsm/UsgsAstroLsSensorModel.h>
-#include <usgscsm/UsgsAstroFrameSensorModel.h>
+#include <vw/Math/Matrix.h>
+
 
 #include <string>
 #include <iostream>
+#include <map>
+
+class UsgsAstroFrameSensorModel;
+class UsgsAstroLsSensorModel;
 
 namespace vw {
-    namespace cartography {
-        class GeoReference;
-    }
+  namespace cartography {
+    class Datum;
+    class GeoReference;
+  }
 }
 
 namespace asp {
 
+const int NUM_XYZ_PARAMS  = 3;
+const int NUM_QUAT_PARAMS = 4;
+const int PIXEL_SIZE      = 2;
+
+class CsmModel;
+
 // Normalize quaternions in UsgsAstroLsSensorModel.
 void normalizeQuaternions(UsgsAstroLsSensorModel * ls_model);
 
@@ -44,15 +55,15 @@ void normalizeQuaternions(UsgsAstroFrameSensorModel * frame_model);
 
 // Get quaternions. This duplicates the UsgsAstroLsSensorModel function as that one is private
 void interpQuaternions(UsgsAstroLsSensorModel * ls_model, double time,
-                      double q[4]);
+                       double q[4]);
 
 // Get positions. Based on the UsgsAstroLsSensorModel code.
 void interpPositions(UsgsAstroLsSensorModel * ls_model, double time,
                      double pos[3]);
 
 // Get positions. Based on the UsgsAstroLsSensorModel code.
 void interpVelocities(UsgsAstroLsSensorModel * ls_model, double time,
-                  double vel[3]);
+                      double vel[3]);
 
 // Nearest neighbor interpolation into a sequence of vectors of length
 // vectorLength, stored one after another in valueArray. The result
@@ -68,6 +79,29 @@ void orbitInterpExtrap(UsgsAstroLsSensorModel const * ls_model,
                        vw::cartography::GeoReference const& geo,
                        std::vector<double> & positions_out);
 
+// Populate the CSM model with the given camera positions and orientations. Note
+// that num_cams_in_image is the number of cameras within the desired orbital segment
+// of length orbit_len, and also the number of cameras for which we have
+// image lines. We will have extra cameras beyond that segment to make it 
+// easy to interpolate the camera position and orientation at any time and also to 
+// solve for jitter. The indices in positions and cam2world can go beyond
+// [0, num_cams_in_image). When it is in this interval, we are recording
+// image lines.
+// TODO(oalexan1): This is not generic enough. 
+// TODO(oalexan1): Use this in LinescanDGModel.cc.
+void populateCsmLinescan(int                                  num_cams_in_image,         
+                         double                               orbit_len, 
+                         double                               velocity,
+                         double                               focal_length,
+                         vw::Vector2                  const & detector_origin,
+                         vw::Vector2i                 const & image_size,
+                         vw::cartography::Datum       const & datum, 
+                         std::string                  const & sensor_id, 
+                         std::map<int, vw::Vector3>   const & positions,
+                         std::map<int, vw::Matrix3x3> const & cam2world,
+                         // Outputs
+                         asp::CsmModel & model);
+
 } // end namespace asp
 
 #endif//__ASP_CAMERA_CSM_UTILS_H__

src/asp/Camera/JitterSolveCostFuns.h

@@ -26,6 +26,7 @@
 
 #include <asp/Camera/CsmModel.h>
 #include <asp/Camera/JitterSolveUtils.h>
+#include <asp/Camera/CsmUtils.h>
 
 #include <vw/Cartography/GeoReference.h>
 

src/asp/Camera/JitterSolveUtils.cc

@@ -17,6 +17,7 @@
 
 // Low-level functions used in jitter_solve.cc.
 
+#include <asp/Camera/CsmUtils.h>
 #include <asp/Camera/JitterSolveUtils.h>
 #include <asp/Core/Common.h>
 

src/asp/Camera/JitterSolveUtils.h

@@ -32,10 +32,6 @@
 
 namespace asp {
 
-const int NUM_XYZ_PARAMS  = 3;
-const int NUM_QUAT_PARAMS = 4;
-const int PIXEL_SIZE      = 2;
-
 // If several images are acquired in quick succession along the same orbit and
 // stored in the same list, record this structure by grouping them together.
 // Each element in the input vector below is either a standalone image, then it

src/asp/Camera/LinescanDGModel.cc

@@ -248,6 +248,9 @@ DGCameraModel::DGCameraModel
 }
 
 // This is a lengthy function that does many initializations  
+// TODO(oalexan1): Replace this with a call to populateCsmLinescan()
+// from CsmUtils.cc, but that will need some refactoring first to handle
+// this case. 
 void DGCameraModel::populateCsmModel() {
 
   // Using a desired precision of 1e-8 will result in about this much
@@ -314,6 +317,7 @@ void DGCameraModel::populateCsmModel() {
   // only 0.5 to tlc[i].first, further down. Looks to produce similar
   // but not quite the same result, but there was no time for a lot of
   // testing doing it that way.
+  std::cout << "m_detector_origin = " << m_detector_origin << std::endl;
   m_ls_model->m_startingDetectorLine   = m_detector_origin[1];
   m_ls_model->m_detectorSampleSumming  = 1.0;
   m_ls_model->m_startingDetectorSample = (m_detector_origin[0] - 0.5);