Add an initial projective approximation for linescan camera (#387)

  # Nlohmann JSON

  find_package(nlohmann_json REQUIRED)

  # Eigen

  find_package(Eigen3 3.3 REQUIRED NO_MODULE)

  # ALE

  find_package(ale REQUIRED)

  set(ALE_TARGET ale::ale)

  set(ALE_BUILD_TESTS OFF)

  add_subdirectory(ale)

  set(ALE_TARGET ale)

endif(USGSCSM_EXTERNAL_DEPS)

  # Use Eigen included with ALE

  add_library (Eigen3::Eigen ALIAS eigen)

  set(EIGEN3_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/ale/eigen)

endif(USGSCSM_EXTERNAL_DEPS)

add_library(usgscsm SHARED

            src/UsgsAstroPlugin.cpp

            src/UsgsAstroLsSensorModel.cpp

            src/UsgsAstroSarSensorModel.cpp

            src/Distortion.cpp

            src/Utilities.cpp)

            src/Utilities.cpp

            src/EigenUtilities.cpp)

set_target_properties(usgscsm PROPERTIES

    VERSION ${PROJECT_VERSION}

)

set(USGSCSM_INCLUDE_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/include/usgscsm"

                         "${CMAKE_CURRENT_SOURCE_DIR}/include/")

                         "${CMAKE_CURRENT_SOURCE_DIR}/include"

                         "${EIGEN3_INCLUDE_DIR}")

target_include_directories(usgscsm

                           PUBLIC

  - ale

  - csm

  - nlohmann_json

  - eigen

#ifndef INCLUDE_USGSCSM_EIGENUTILITIES_H_

#define INCLUDE_USGSCSM_EIGENUTILITIES_H_

// Do not include Eigen header files here as those will slow down the compilation

// whereever this header file is included.

#include <csm.h>

namespace usgscsm {

// Compute the best-fitting projective transform that maps a set of 3D points

// to 2D points.

void computeBestFitProjectiveTransform(std::vector<csm::ImageCoord> const& imagePts,

                                       std::vector<csm::EcefCoord>  const& groundPts,

                                       std::vector<double> & transformCoeffs);

}

#endif  // INCLUDE_USGSCSM_EIGENUTILITIES_H_

      const std::vector<double>& adj)      // Parameter Adjustments for partials

      const;

  // The linear approximation for the sensor model is used as the starting point

  // The projective approximation for the sensor model is used as the starting point

  // for iterative rigorous calculations.

  void computeLinearApproximation(const csm::EcefCoord& gp,

  void computeProjectiveApproximation(const csm::EcefCoord& gp,

                                      csm::ImageCoord& ip) const;

  // Initial setup of the linear approximation

  void setLinearApproximation();

  // Compute the determinant of a 3x3 matrix

  double determinant3x3(double mat[9]) const;

  // Create the projective approximation to be used at each ground point

  void createProjectiveApproximation();

  // A function whose value will be 0 when the line a given ground

  // point projects into is found. The obtained line will be

  csm::NoCorrelationModel _no_corr_model;  // A way to report no correlation

                                           // between images is supported

  std::vector<double>

      _no_adjustment;  // A vector of zeros indicating no internal adjustment

  // The following support the linear approximation of the sensor model

  double _u0;

  double _du_dx;

  double _du_dy;

  double _du_dz;

  double _v0;

  double _dv_dx;

  double _dv_dy;

  double _dv_dz;

  bool _linear;  // flag indicating if linear approximation is useful.

  std::vector<double> _no_adjustment;  // A vector of zeros indicating no internal adjustment

  // Store here the projective approximation of the sensor model

  std::vector<double> m_projTransCoeffs; 

  // Flag indicating if an initial approximation is used

  bool m_useApproxInitTrans;

};

#endif  // INCLUDE_USGSCSM_USGSASTROLSSENSORMODEL_H_

#include "EigenUtilities.h"

#include <Error.h>

#include <Eigen/Dense>

#include <iostream>

// Keep these utilities separate as using Eigen in an existing source

// file results in a 50% increase in compilation time.

// Compute the best-fitting projective transform that maps a set of 3D points

// to 2D points.

// A best-fit linear transform could be created by eliminating the denominators below.

void usgscsm::computeBestFitProjectiveTransform(std::vector<csm::ImageCoord> const& imagePts,

                                                std::vector<csm::EcefCoord>  const& groundPts,

                                                std::vector<double> & transformCoeffs) {

  if (imagePts.size() != groundPts.size()) 

    throw csm::Error(csm::Error::INVALID_USE,

                     "The number of inputs and outputs must agree.",

                     "computeBestFitProjectiveTransform");

  int numPts = imagePts.size();

  if (numPts < 8)

    throw csm::Error(csm::Error::INVALID_USE,

                     "At least 8 points are needed to fit a 3D-to-2D projective transform. "

                     "Ideally more are preferred.",

                     "computeBestFitProjectiveTransform");

  int numMatRows = 2 * numPts; // there exist x and y coords for each point

  int numMatCols = 14; // Number of variables in the projective transform

  Eigen::MatrixXd M = Eigen::MatrixXd::Zero(numMatRows, numMatCols);

  Eigen::VectorXd b = Eigen::VectorXd::Zero(numMatRows);

  for (int it = 0; it < numPts; it++) {

    double x = groundPts[it].x, y = groundPts[it].y, z = groundPts[it].z;

    double r = imagePts[it].line, c = imagePts[it].samp;

    // If the solution coefficients are u0, u1, ..., must have:

    // (u0 + u1 * x + u2 * y + u3  * z) / (1 + u4  * x + u5  * y + u6  * z) = r

    // (u7 + u8 * x + u9 * y + u10 * z) / (1 + u11 * x + u12 * y + u13 * z) = c

    // Multiply by the denominators. Get a linear regression in the coefficients.

    M.row(2 * it + 0) << 1, x, y, z, -x * r, -y * r, -z * r, 0, 0, 0, 0, 0, 0, 0;

    M.row(2 * it + 1) << 0, 0, 0, 0, 0, 0, 0, 1, x, y, z, -x * c, -y * c, -z * c;

    b[2 * it + 0] = r;

    b[2 * it + 1] = c;

  }

  // Solve the over-determined system, per:

  // https://eigen.tuxfamily.org/dox/group__LeastSquares.html

  Eigen::VectorXd u = M.colPivHouseholderQr().solve(b);

  // Copy back the result to a standard vector (to avoid using Eigen too much as

  // that is slow to compile).

  transformCoeffs.resize(numMatCols);

  for (int it = 0; it < numMatCols; it++)

    transformCoeffs[it] = u[it];

  return;

}