From 1c4832d808c0f001c0f8975a6fb7a29aef7a28f7 Mon Sep 17 00:00:00 2001
From: Oleg Alexandrov <oleg.alexandrov@gmail.com>
Date: Tue, 3 May 2022 11:17:51 -0700
Subject: [PATCH] Add an initial projective approximation for linescan camera
(#387)
* Add an initial projective approximation for linescan camera
linescan model: Minor adjustment to initial guess
Minor tweak
* Edding Eigen to environment.yml
* Fix a header to make Windows happy
* Remove old code and add unit test
* Add the unit tests
---
CMakeLists.txt | 14 +-
environment.yml | 1 +
include/usgscsm/EigenUtilities.h | 17 ++
include/usgscsm/UsgsAstroLsSensorModel.h | 35 ++--
src/EigenUtilities.cpp | 66 +++++++
src/UsgsAstroLsSensorModel.cpp | 231 ++++++++---------------
tests/CMakeLists.txt | 3 +-
tests/EigenUtilitiesTests.cpp | 46 +++++
8 files changed, 233 insertions(+), 180 deletions(-)
create mode 100644 include/usgscsm/EigenUtilities.h
create mode 100644 src/EigenUtilities.cpp
create mode 100644 tests/EigenUtilitiesTests.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 57ca167..76df1fd 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -32,6 +32,9 @@ if(USGSCSM_EXTERNAL_DEPS)
# 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)
@@ -53,8 +56,11 @@ else()
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
@@ -63,7 +69,8 @@ add_library(usgscsm SHARED
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}
@@ -71,7 +78,8 @@ set_target_properties(usgscsm PROPERTIES
)
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
diff --git a/environment.yml b/environment.yml
index ccb7e68..680db84 100644
--- a/environment.yml
+++ b/environment.yml
@@ -8,3 +8,4 @@ dependencies:
- ale
- csm
- nlohmann_json
+ - eigen
diff --git a/include/usgscsm/EigenUtilities.h b/include/usgscsm/EigenUtilities.h
new file mode 100644
index 0000000..41ed21c
--- /dev/null
+++ b/include/usgscsm/EigenUtilities.h
@@ -0,0 +1,17 @@
+#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_
diff --git a/include/usgscsm/UsgsAstroLsSensorModel.h b/include/usgscsm/UsgsAstroLsSensorModel.h
index ec99db0..ec0e6c6 100644
--- a/include/usgscsm/UsgsAstroLsSensorModel.h
+++ b/include/usgscsm/UsgsAstroLsSensorModel.h
@@ -968,16 +968,13 @@ class UsgsAstroLsSensorModel : public csm::RasterGM,
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,
- 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;
+ void computeProjectiveApproximation(const csm::EcefCoord& gp,
+ csm::ImageCoord& ip) 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
@@ -988,19 +985,13 @@ class UsgsAstroLsSensorModel : public csm::RasterGM,
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_
diff --git a/src/EigenUtilities.cpp b/src/EigenUtilities.cpp
new file mode 100644
index 0000000..81115ce
--- /dev/null
+++ b/src/EigenUtilities.cpp
@@ -0,0 +1,66 @@
+#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;
+}
diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
index b3c21c6..479b8e7 100644
--- a/src/UsgsAstroLsSensorModel.cpp
+++ b/src/UsgsAstroLsSensorModel.cpp
@@ -20,6 +20,7 @@
#include "UsgsAstroLsSensorModel.h"
#include "Distortion.h"
#include "Utilities.h"
+#include "EigenUtilities.h"
#include <float.h>
#include <math.h>
@@ -270,9 +271,10 @@ void UsgsAstroLsSensorModel::replaceModelState(const std::string& stateString) {
}
try {
- setLinearApproximation();
+ // Approximate the ground-to-image function with a projective transform
+ createProjectiveApproximation();
} catch (...) {
- _linear = false;
+ m_useApproxInitTrans = false;
}
}
@@ -478,15 +480,7 @@ void UsgsAstroLsSensorModel::applyTransformToState(ale::Rotation const& r, ale::
//***************************************************************************
void UsgsAstroLsSensorModel::reset() {
MESSAGE_LOG("Running reset()")
- _linear = false; // default until a linear model is made
- _u0 = 0.0;
- _du_dx = 0.0;
- _du_dy = 0.0;
- _du_dz = 0.0;
- _v0 = 0.0;
- _dv_dx = 0.0;
- _dv_dy = 0.0;
- _dv_dz = 0.0;
+ m_useApproxInitTrans = false; // default until an initial approximation is found
_no_adjustment.assign(UsgsAstroLsSensorModel::NUM_PARAMETERS, 0.0);
@@ -680,7 +674,7 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(
csm::WarningList* warnings) const {
csm::ImageCoord approxPt;
- computeLinearApproximation(groundPt, approxPt);
+ computeProjectiveApproximation(groundPt, approxPt);
// Search for the (line, sample) coordinate that views a given
// ground point. Set this up as a root-finding problem and use the
@@ -688,7 +682,7 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(
int count = 0;
double t0 = 0.0;
double lineErr0 = calcDetectorLineErr(t0, approxPt, groundPt, adj);
- double t1 = lineErr0 * 0.1; // need to stay close or else there's overshooting
+ double t1 = 0.1;
double lineErr1 = calcDetectorLineErr(t1, approxPt, groundPt, adj);
while (std::abs(lineErr1) > desiredPrecision && ++count < 15) {
@@ -2111,16 +2105,34 @@ std::vector<double> UsgsAstroLsSensorModel::computeDetectorView(
}
//***************************************************************************
-// UsgsAstroLineScannerSensorModel::computeLinearApproximation
+// UsgsAstroLineScannerSensorModel::computeProjectiveApproximation
//***************************************************************************
-void UsgsAstroLsSensorModel::computeLinearApproximation(
- const csm::EcefCoord& gp, csm::ImageCoord& ip) const {
- if (_linear) {
- ip.line = _u0 + _du_dx * gp.x + _du_dy * gp.y + _du_dz * gp.z;
- ip.samp = _v0 + _dv_dx * gp.x + _dv_dy * gp.y + _dv_dz * gp.z;
+void UsgsAstroLsSensorModel::computeProjectiveApproximation(const csm::EcefCoord& gp,
+ csm::ImageCoord& ip) const {
+ if (m_useApproxInitTrans) {
+ std::vector<double> const& u = m_projTransCoeffs; // alias, to save on typing
- // Since this is valid only over image,
- // don't let result go beyond the image border.
+ double x = gp.x, y = gp.y, z = gp.z;
+ double line_den = 1 + u[4] * x + u[5] * y + u[6] * z;
+ double samp_den = 1 + u[11] * x + u[12] * y + u[13] * z;
+
+ // Sanity checks. Ensure we don't divide by 0 and that the numbers are valid.
+ if (line_den == 0.0 || std::isnan(line_den) || std::isinf(line_den) ||
+ samp_den == 0.0 || std::isnan(samp_den) || std::isinf(samp_den)) {
+
+ ip.line = m_nLines / 2.0;
+ ip.samp = m_nSamples / 2.0;
+ MESSAGE_LOG("Computing initial guess with constant approx line/2 and sample/2");
+
+ return;
+ }
+
+ // Apply the formula
+ ip.line = (u[0] + u[1] * x + u[2] * y + u[3] * z) / line_den;
+ ip.samp = (u[7] + u[8] * x + u[9] * y + u[10] * z) / samp_den;
+
+ // Since this is valid only over the image,
+ // don't let the result go beyond the image border.
double numRows = m_nLines;
double numCols = m_nSamples;
if (ip.line < 0.0) ip.line = 0.0;
@@ -2128,25 +2140,25 @@ void UsgsAstroLsSensorModel::computeLinearApproximation(
if (ip.samp < 0.0) ip.samp = 0.0;
if (ip.samp > numCols) ip.samp = numCols;
- MESSAGE_LOG(
- "Computing computeLinearApproximation"
- "with linear approximation")
+
+ MESSAGE_LOG("Computing initial guess with projective approximation");
} else {
ip.line = m_nLines / 2.0;
ip.samp = m_nSamples / 2.0;
- MESSAGE_LOG(
- "Computing computeLinearApproximation"
- "nonlinear approx line/2 and sample/2")
+ MESSAGE_LOG("Computing initial guess with constant approx line/2 and sample/2");
}
}
//***************************************************************************
-// UsgsAstroLineScannerSensorModel::setLinearApproximation
+// UsgsAstroLineScannerSensorModel::createProjectiveApproximation
//***************************************************************************
-void UsgsAstroLsSensorModel::setLinearApproximation() {
- MESSAGE_LOG("Calculating setLinearApproximation")
- double u_factors[4] = {0.0, 0.0, 1.0, 1.0};
- double v_factors[4] = {0.0, 1.0, 0.0, 1.0};
+void UsgsAstroLsSensorModel::createProjectiveApproximation() {
+ MESSAGE_LOG("Calculating createProjectiveApproximation");
+
+ // Use 9 points (9*4 eventual matrix rows) as we need to fit 14 variables.
+ const int numPts = 9;
+ double u_factors[numPts] = {0.0, 0.0, 0.0, 0.5, 0.5, 0.5, 1.0, 1.0, 1.0};
+ double v_factors[numPts] = {0.0, 0.5, 1.0, 0.0, 0.5, 1.0, 0.0, 0.5, 1.0};
csm::EcefCoord refPt = getReferencePoint();
@@ -2154,135 +2166,46 @@ void UsgsAstroLsSensorModel::setLinearApproximation() {
double height = computeEllipsoidElevation(
refPt.x, refPt.y, refPt.z, m_majorAxis, m_minorAxis, desired_precision);
if (std::isnan(height)) {
- MESSAGE_LOG(
- "setLinearApproximation: computeElevation of"
- "reference point {} {} {} with desired precision"
- "{} returned nan height; nonlinear",
- refPt.x, refPt.y, refPt.z, desired_precision)
- _linear = false;
- return;
- }
- MESSAGE_LOG(
- "setLinearApproximation: computeElevation of"
- "reference point {} {} {} with desired precision"
- "{} returned {} height",
- refPt.x, refPt.y, refPt.z, desired_precision, height)
-
- double numRows = m_nLines;
- double numCols = m_nSamples;
-
- csm::ImageCoord imagePt;
- csm::EcefCoord gp[8];
-
- int i;
- for (i = 0; i < 4; i++) {
- imagePt.line = u_factors[i] * numRows;
- imagePt.samp = v_factors[i] * numCols;
- gp[i] = imageToGround(imagePt, height);
- }
-
- double delz = 100.0;
- height += delz;
- for (i = 0; i < 4; i++) {
- imagePt.line = u_factors[i] * numRows;
- imagePt.samp = v_factors[i] * numCols;
- gp[i + 4] = imageToGround(imagePt, height);
- }
-
- csm::EcefCoord d_du;
- d_du.x = ((gp[2].x + gp[3].x + gp[6].x + gp[7].x) -
- (gp[0].x + gp[1].x + gp[4].x + gp[5].x)) /
- numRows / 4.0;
- d_du.y = ((gp[2].y + gp[3].y + gp[6].y + gp[7].y) -
- (gp[0].y + gp[1].y + gp[4].y + gp[5].y)) /
- numRows / 4.0;
- d_du.z = ((gp[2].z + gp[3].z + gp[6].z + gp[7].z) -
- (gp[0].z + gp[1].z + gp[4].z + gp[5].z)) /
- numRows / 4.0;
-
- csm::EcefCoord d_dv;
- d_dv.x = ((gp[1].x + gp[3].x + gp[5].x + gp[7].x) -
- (gp[0].x + gp[2].x + gp[4].x + gp[6].x)) /
- numCols / 4.0;
- d_dv.y = ((gp[1].y + gp[3].y + gp[5].y + gp[7].y) -
- (gp[0].y + gp[2].y + gp[4].y + gp[6].y)) /
- numCols / 4.0;
- d_dv.z = ((gp[1].z + gp[3].z + gp[5].z + gp[7].z) -
- (gp[0].z + gp[2].z + gp[4].z + gp[6].z)) /
- numCols / 4.0;
-
- double mat3x3[9];
-
- mat3x3[0] = d_du.x;
- mat3x3[1] = d_dv.x;
- mat3x3[2] = 1.0;
- mat3x3[3] = d_du.y;
- mat3x3[4] = d_dv.y;
- mat3x3[5] = 1.0;
- mat3x3[6] = d_du.z;
- mat3x3[7] = d_dv.z;
- mat3x3[8] = 1.0;
-
- double denom = determinant3x3(mat3x3);
-
- // Can not get derivatives this way
- if (fabs(denom) < 1.0e-8) {
- MESSAGE_LOG(
- "setLinearApproximation: determinant3x3 of"
- "matrix of partials is {}; nonlinear",
- denom)
- _linear = false;
+ MESSAGE_LOG("createProjectiveApproximation: computeElevation of"
+ "reference point {} {} {} with desired precision"
+ "{} returned nan height; nonprojective",
+ refPt.x, refPt.y, refPt.z, desired_precision);
+ m_useApproxInitTrans = false;
return;
}
+ MESSAGE_LOG("createProjectiveApproximation: computeElevation of"
+ "reference point {} {} {} with desired precision"
+ "{} returned {} height",
+ refPt.x, refPt.y, refPt.z, desired_precision, height);
- mat3x3[0] = 1.0;
- mat3x3[3] = 0.0;
- mat3x3[6] = 0.0;
- _du_dx = determinant3x3(mat3x3) / denom;
-
- mat3x3[0] = 0.0;
- mat3x3[3] = 1.0;
- mat3x3[6] = 0.0;
- _du_dy = determinant3x3(mat3x3) / denom;
-
- mat3x3[0] = 0.0;
- mat3x3[3] = 0.0;
- mat3x3[6] = 1.0;
- _du_dz = determinant3x3(mat3x3) / denom;
-
- mat3x3[0] = d_du.x;
- mat3x3[3] = d_du.y;
- mat3x3[6] = d_du.z;
-
- mat3x3[1] = 1.0;
- mat3x3[4] = 0.0;
- mat3x3[7] = 0.0;
- _dv_dx = determinant3x3(mat3x3) / denom;
-
- mat3x3[1] = 0.0;
- mat3x3[4] = 1.0;
- mat3x3[7] = 0.0;
- _dv_dy = determinant3x3(mat3x3) / denom;
+ double numImageRows = m_nLines;
+ double numImageCols = m_nSamples;
- mat3x3[1] = 0.0;
- mat3x3[4] = 0.0;
- mat3x3[7] = 1.0;
- _dv_dz = determinant3x3(mat3x3) / denom;
+ std::vector<csm::ImageCoord> ip(2 * numPts);
+ std::vector<csm::EcefCoord> gp(2 * numPts);
- _u0 = -gp[0].x * _du_dx - gp[0].y * _du_dy - gp[0].z * _du_dz;
- _v0 = -gp[0].x * _dv_dx - gp[0].y * _dv_dy - gp[0].z * _dv_dz;
+ // Sample at two heights above the ellipsoid in order to get a
+ // reliable estimate of the relationship between image points and
+ // ground points.
+
+ for (int i = 0; i < numPts; i++) {
+ ip[i].line = u_factors[i] * numImageRows;
+ ip[i].samp = v_factors[i] * numImageCols;
+ gp[i] = imageToGround(ip[i], height);
+ }
- _linear = true;
- MESSAGE_LOG("Completed setLinearApproximation")
-}
+ double delta_z = 100.0;
+ height += delta_z;
+ for (int i = 0; i < numPts; i++) {
+ ip[i + numPts].line = u_factors[i] * numImageRows;
+ ip[i + numPts].samp = v_factors[i] * numImageCols;
+ gp[i + numPts] = imageToGround(ip[i + numPts], height);
+ }
-//***************************************************************************
-// UsgsAstroLineScannerSensorModel::determinant3x3
-//***************************************************************************
-double UsgsAstroLsSensorModel::determinant3x3(double mat[9]) const {
- return mat[0] * (mat[4] * mat[8] - mat[7] * mat[5]) -
- mat[1] * (mat[3] * mat[8] - mat[6] * mat[5]) +
- mat[2] * (mat[3] * mat[7] - mat[6] * mat[4]);
+ usgscsm::computeBestFitProjectiveTransform(ip, gp, m_projTransCoeffs);
+ m_useApproxInitTrans = true;
+
+ MESSAGE_LOG("Completed createProjectiveApproximation");
}
//***************************************************************************
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index bdf7d95..1b10252 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -9,7 +9,8 @@ add_executable(runCSMCameraModelTests
DistortionTests.cpp
SarTests.cpp
ISDParsingTests.cpp
- UtilitiesTests.cpp)
+ UtilitiesTests.cpp
+ EigenUtilitiesTests.cpp)
if(WIN32)
option(CMAKE_USE_WIN32_THREADS_INIT "using WIN32 threads" ON)
option(gtest_disable_pthreads "Disable uses of pthreads in gtest." ON)
diff --git a/tests/EigenUtilitiesTests.cpp b/tests/EigenUtilitiesTests.cpp
new file mode 100644
index 0000000..7b9b8ea
--- /dev/null
+++ b/tests/EigenUtilitiesTests.cpp
@@ -0,0 +1,46 @@
+#include "EigenUtilities.h"
+
+#include <gtest/gtest.h>
+#include <cmath>
+
+// See if fitting a projective transform to points which were created
+// with such a transform to start with can recover it.
+TEST(EigenUtilitiesTests, createProjectiveApproximation) {
+
+ // Create a projective transform with coeffs stored in a vector.
+ // The exact values are not important.
+ std::vector<double> u(14);
+ for (size_t it = 0; it < u.size(); it++)
+ u[it] = sin(it); // why not
+
+ // Create inputs and outputs
+ std::vector<csm::ImageCoord> imagePts;
+ std::vector<csm::EcefCoord> groundPts;
+ for (int r = 0; r < 5; r++) {
+ for (int c = 0; c < 5; c++) {
+
+ // Create some 3D points. The precise values are not important
+ // as long as they are well-distributed
+ double x = c, y = r, z = 0.3 * x + 2 * y + 0.04 * x * y;
+
+ // Find corresponding 2D points
+ double p = (u[0] + u[1] * x + u[2] * y + u[3] * z) / (1 + u[4] * x + u[5] * y + u[6] * z);
+ double q = (u[7] + u[8] * x + u[9] * y + u[10] * z) / (1 + u[11] * x + u[12] * y + u[13] * z);
+
+ imagePts.push_back(csm::ImageCoord(p, q));
+ groundPts.push_back(csm::EcefCoord(x, y, z));
+ }
+ }
+
+ // Find the transform
+ std::vector<double> transformCoeffs;
+ usgscsm::computeBestFitProjectiveTransform(imagePts, groundPts, transformCoeffs);
+
+ // Verify that we recover the transform
+ double err = 0.0;
+ for (size_t it = 0; it < u.size(); it++)
+ err = std::max(err, std::abs(u[it] - transformCoeffs[it]));
+
+ EXPECT_NEAR(0.0, err, 1e-12);
+}
+
--
GitLab