Skip to content

Commit

Permalink
New groundToImage method (#260)
Browse files Browse the repository at this point in the history 
* Added line and point comp and distance

* Finished linearization

* Fixed removing wrong line

* More fixes

* First pass at detector coords

* Added line coefficient computation

* Updated for the line search algorithem

* Added final touches to the groundToImage algorithm

* Removed old groundtoimage code

* Removed some prints

* Somewhat working groundToImage stuffs

* Fixed orbital ls

* Fixed old line

* Added warnings, moved things around, added some prints

* More prints

* Fixed sensor angles

* Removed constAngular tests

* Commented out new algorithm section and removed prints

* Updated expected precision to be the default precision passed to groundtoimage

* Updated tests to check for precision

* Addressed PR feedback

* Added old logger message back in

* Removed utlity code from algorithm

* Changes based on feedback and other small comment based changes

Co-authored-by: Jesse Mapel <jam826@nau.edu>
  • Loading branch information
@acpaquette @jessemapel
acpaquette and jessemapel committed Jan 17, 2020
1 parent b2ea3a2 commit 0bddf1d
Show file tree
Hide file tree
Showing 9 changed files with 136 additions and 583 deletions.
10 changes: 2 additions & 8 deletions include/usgscsm/UsgsAstroLsSensorModel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,11 +108,6 @@ class UsgsAstroLsSensorModel : public csm::RasterGM, virtual public csm::Settabl
std::vector<double> m_positions;
std::vector<double> m_velocities;
std::vector<double> m_quaternions;
std::vector<int> m_detectorNodes;
std::vector<double> m_detectorXCoords;
std::vector<double> m_detectorYCoords;
std::vector<double> m_detectorLineCoeffs;
double m_averageDetectorSize;
std::vector<double> m_currentParameterValue;
std::vector<csm::param::Type> m_parameterType;
csm::EcefCoord m_referencePointXyz;
Expand Down Expand Up @@ -1043,11 +1038,10 @@ class UsgsAstroLsSensorModel : public csm::RasterGM, virtual public csm::Settabl

// Computes the imaging locus that would view a ground point at a specific
// time. Computationally, this is the opposite of losToEcf.
csm::ImageCoord computeViewingPixel(
std::vector<double> computeDetectorView(
const double& time, // The time to use the EO at
const csm::EcefCoord& groundPoint, // The ground coordinate
const std::vector<double>& adj, // Parameter Adjustments for partials
const double& desiredPrecision // Desired precision for distortion inversion
const std::vector<double>& adj // Parameter Adjustments for partials
) const;

// The linear approximation for the sensor model is used as the starting point
Expand Down
19 changes: 0 additions & 19 deletions include/usgscsm/Utilities.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,25 +11,6 @@
#include <json/json.hpp>

#include <Warning.h>

double distanceToLine(double x, double y,
double a, double b, double c);

double distanceToPlane(double x, double y, double z,
double a, double b, double c, double d);

void line(double x1, double y1, double x2, double y2,
double &a, double &b, double &c);

void plane(double x0, double y0, double z0,
double v1x, double v1y, double v1z,
double v2x, double v2y, double v2z,
double &a, double &b, double &c, double &d);

std::vector<int> fitLinearApproximation(const std::vector<double> &x,
const std::vector<double> &y,
double tolerance);

// methods pulled out of los2ecf and computeViewingPixel

// for now, put everything in here.
Expand Down
250 changes: 71 additions & 179 deletions src/UsgsAstroLsSensorModel.cpp
View file

Large diffs are not rendered by default.

96 changes: 0 additions & 96 deletions src/Utilities.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,102 +7,6 @@

using json = nlohmann::json;

// Calculate the signed distance from a 2D point to a line given in standard form
//
// --NOTE-- This function assumes that the coefficients of the line have been reduced
// so that sqrt(a^2 + b^2) = 1
double distanceToLine(double x, double y,
double a, double b, double c) {
return a*x + b*y + c;
}

// Calculate the distance from a 3D point to a plane given in standard form
//
// --NOTE-- This function assumes that the coefficients of the plane have been reduced
// so that sqrt(a^2 + b^2 + c^2) = 1
double distanceToPlane(double x, double y, double z,
double a, double b, double c, double d) {
return std::abs(a*x + b*y + c*z + d);
}

// Calculate the standard form coefficients of a line that passes through two points
//
// --NOTE-- The coefficients have been reduced such that sqrt(a^2 + b^2) = 1
void line(double x1, double y1, double x2, double y2,
double &a, double &b, double &c) {
a = y1 - y2;
b = x2 - x1;
c = x1*y2 - x2*y1;
double scale = sqrt(a*a + b*b);
a /= scale;
b /= scale;
c /= scale;
}

// Calculate the standard form coefficients of a plane that passes through a point
// and contains two vectors.
//
// --NOTE-- The coefficients have been reduced such that sqrt(a^2 + b^2 + c^2) = 1
void plane(double x0, double y0, double z0,
double v1x, double v1y, double v1z,
double v2x, double v2y, double v2z,
double &a, double &b, double &c, double &d) {
// Compute normal vector and scale
a = v1y*v2z - v1z*v2y;
b = v1z*v2x - v1x*v2z;
c = v1x*v2y - v1y*v2x;
double scale = sqrt(a*a + b*b + c*c);
a /= scale;
b /= scale;
c /= scale;
// Shift to point
d = - (a*x0 + b*y0 + c*z0);
}



// Fit a piecewise-linear approximations to 2D data within a tolerance
//
// Returns a vector of node indices
std::vector<int> fitLinearApproximation(const std::vector<double> &x,
const std::vector<double> &y,
double tolerance) {
std::vector<int> nodes;
nodes.push_back(0);

std::stack<std::pair<int, int>> workStack;
workStack.push(std::make_pair(0, x.size() - 1));
while (!workStack.empty()) {
std::pair<int, int> range = workStack.top();
workStack.pop();
double a, b, c;
line(x[range.first], y[range.first], x[range.second], y[range.second], a, b, c);
double maxError = 0;
int maxIndex = (range.second + range.first) / 2;
for (int i = range.first + 1; i < range.second; i++) {
double error = std::abs(distanceToLine(x[i], y[i], a, b, c));
if (error > maxError) {
maxError = error;
maxIndex = i;
}
}
// If the max error is greater than the tolerance, split at the largest error
// Do not split if the range only contains two nodes
if (maxError > tolerance && range.second - range.first > 1) {
// Append the second range and then the first range
// so that nodes are added in the same order they came in
workStack.push(std::make_pair(maxIndex, range.second));
workStack.push(std::make_pair(range.first, maxIndex));
}
else {
// segment is good so append last point to nodes
nodes.push_back(range.second);
}
}

return nodes;
}

// Calculates a rotation matrix from Euler angles
// in - euler[3]
// out - rotationMatrix[9]
Expand Down
27 changes: 0 additions & 27 deletions tests/Fixtures.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -240,33 +240,6 @@ class ConstVelocityLineScanSensorModel : public ::testing::Test {
}
};

class ConstAngularVelocityLineScanSensorModel : public ::testing::Test {
protected:
csm::Isd isd;
UsgsAstroLsSensorModel *sensorModel;

void SetUp() override {
sensorModel = NULL;

isd.setFilename("data/constAngularVelocityLineScan.img");
UsgsAstroPlugin cameraPlugin;

csm::Model *model = cameraPlugin.constructModelFromISD(
isd,
"USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL");
sensorModel = dynamic_cast<UsgsAstroLsSensorModel *>(model);

ASSERT_NE(sensorModel, nullptr);
}

void TearDown() override {
if (sensorModel) {
delete sensorModel;
sensorModel = NULL;
}
}
};

class OrbitalLineScanSensorModel : public ::testing::Test {
protected:
csm::Isd isd;
Expand Down
53 changes: 15 additions & 38 deletions tests/LineScanCameraTests.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,12 +34,14 @@ TEST_F(ConstVelocityLineScanSensorModel, Center) {
}

TEST_F(ConstVelocityLineScanSensorModel, Inversion) {
double achievedPrecision;
csm::ImageCoord imagePt(8.5, 8);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
csm::ImageCoord imageReprojPt = sensorModel->groundToImage(groundPt);
csm::ImageCoord imageReprojPt = sensorModel->groundToImage(groundPt, 0.001, &achievedPrecision);

EXPECT_DOUBLE_EQ(imagePt.line, imageReprojPt.line);
EXPECT_DOUBLE_EQ(imagePt.samp, imageReprojPt.samp);
EXPECT_LT(achievedPrecision, 0.001);
EXPECT_NEAR(imagePt.line, imageReprojPt.line, 1e-3);
EXPECT_NEAR(imagePt.samp, imageReprojPt.samp, 1e-3);
}

TEST_F(ConstVelocityLineScanSensorModel, OffBody) {
Expand Down Expand Up @@ -73,39 +75,12 @@ TEST_F(ConstVelocityLineScanSensorModel, RemoteImageLocus) {
EXPECT_NEAR(locus.point.z, 0.0, 1e-12);
}

// Pan tests

TEST_F(ConstAngularVelocityLineScanSensorModel, Center) {
csm::ImageCoord imagePt(8.5, 8.0);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
EXPECT_DOUBLE_EQ(groundPt.x, 10.0);
EXPECT_DOUBLE_EQ(groundPt.y, 0.0);
EXPECT_DOUBLE_EQ(groundPt.z, 0.0);
}

TEST_F(ConstAngularVelocityLineScanSensorModel, Inversion) {
csm::ImageCoord imagePt(8.5, 8);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
csm::ImageCoord imageReprojPt = sensorModel->groundToImage(groundPt);

EXPECT_DOUBLE_EQ(imagePt.line, imageReprojPt.line);
EXPECT_DOUBLE_EQ(imagePt.samp, imageReprojPt.samp);
}

TEST_F(ConstAngularVelocityLineScanSensorModel, OffBody) {
csm::ImageCoord imagePt(4.5, 4.0);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
EXPECT_NEAR(groundPt.x, 4.15414478, 1e-8);
EXPECT_NEAR(groundPt.y, -7.98311067, 1e-8);
EXPECT_NEAR(groundPt.z, 63.82129588, 1e-8);
}

TEST_F(ConstVelocityLineScanSensorModel, calculateAttitudeCorrection) {
std::vector<double> adj;
double attCorr[9];
adj.resize(15, 0);
// Pi/2 with simply compensating for member variable m_flyingHeight in UsgsAstroLsSensorModel
adj[7] = (M_PI / 2) * 990.0496255790623081338708;
adj[7] = (M_PI / 2) * sensorModel->m_flyingHeight;
sensorModel->calculateAttitudeCorrection(999.5, adj, attCorr);

// EXPECT_NEARs are used here instead of EXPECT_DOUBLE_EQs because index 0 and 8 of the matrix
Expand Down Expand Up @@ -138,9 +113,9 @@ TEST_F(OrbitalLineScanSensorModel, getIlluminationDirectionStationary) {

// Calculate expected sun direction
// These are the ground point coordinates minus constant sun positions.
double expected_x = 999899.680000017;
double expected_y = -100;
double expected_z = -899.99991466668735;
double expected_x = groundPt.x - sensorModel->m_sunPosition[0];
double expected_y = groundPt.y - sensorModel->m_sunPosition[1];
double expected_z = groundPt.z - sensorModel->m_sunPosition[2];

//normalize
double scale = sqrt((expected_x * expected_x) + (expected_y * expected_y) + (expected_z * expected_z));
Expand Down Expand Up @@ -194,19 +169,21 @@ TEST_F(OrbitalLineScanSensorModel, getSunPositionStationary){
TEST_F(OrbitalLineScanSensorModel, Center) {
csm::ImageCoord imagePt(8.5, 8.0);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
EXPECT_DOUBLE_EQ(groundPt.x, 999999.680000017);
EXPECT_DOUBLE_EQ(groundPt.x, 999999.70975015126);
EXPECT_DOUBLE_EQ(groundPt.y, 0.0);
EXPECT_DOUBLE_EQ(groundPt.z, -799.99991466668735);
EXPECT_DOUBLE_EQ(groundPt.z, -761.90525203960692);
}

TEST_F(OrbitalLineScanSensorModel, Inversion) {
double achievedPrecision;
for (double line = 0.5; line < 16; line++) {
csm::ImageCoord imagePt(line, 8);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
csm::ImageCoord imageReprojPt = sensorModel->groundToImage(groundPt);
csm::ImageCoord imageReprojPt = sensorModel->groundToImage(groundPt, 0.001, &achievedPrecision);

// groundToImage has a default precision of 0.001m and each pixel is 100m
// so we should be within 0.1 pixels
EXPECT_LT(achievedPrecision, 0.001);
EXPECT_NEAR(imagePt.line, imageReprojPt.line, 0.1);
EXPECT_NEAR(imagePt.samp, imageReprojPt.samp, 0.1);
}
Expand Down Expand Up @@ -238,7 +215,7 @@ TEST_F(OrbitalLineScanSensorModel, InversionHeight) {
TEST_F(OrbitalLineScanSensorModel, InversionReallyHigh) {
for (double line = 0.5; line < 16; line++) {
csm::ImageCoord imagePt(line, 8);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 49000.0);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 4900.0);
csm::ImageCoord imageReprojPt = sensorModel->groundToImage(groundPt);

// groundToImage has a default precision of 0.001m and each pixel is 2m
Expand Down
76 changes: 0 additions & 76 deletions tests/UtilitiesTests.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,82 +11,6 @@

using json = nlohmann::json;

TEST(UtilitiesTests, distanceToLine) {
// Line passing through (0, 1) and (2,2)
double a = -1.0 / sqrt(5);
double b = 2.0 / sqrt(5);
double c = -2.0 / sqrt(5);
// Point on line
EXPECT_NEAR(distanceToLine(4.0, 3.0, a, b, c), 0, 1e-12);
// Point above line
EXPECT_DOUBLE_EQ(distanceToLine(1.0, 4.0, a, b, c), sqrt(5));
// Point below line
EXPECT_DOUBLE_EQ(distanceToLine(4.5, 2.0, a, b, c), -sqrt(5) / 2.0);
}

TEST(UtilitiesTests, line) {
double x1 = 0.0;
double y1 = 1.0;
double x2 = 2.0;
double y2 = 2.0;
double a, b, c;
line(x1, y1, x2, y2, a, b, c);
EXPECT_DOUBLE_EQ(a, -1.0 / sqrt(5));
EXPECT_DOUBLE_EQ(b, 2.0 / sqrt(5));
EXPECT_DOUBLE_EQ(c, -2.0 / sqrt(5));
}

TEST(UtilitiesTests, distanceToPlane) {
// Plane passing through (0, 0, 1), (0, 2, 0) and (3, 0, 0)
double a = 2.0 / 7.0;
double b = 3.0 / 7.0;
double c = 6.0 / 7.0;
double d = -12.0 / 7.0;
// Points on plane
EXPECT_NEAR(distanceToPlane(0.0, 0.0, 2.0, a, b, c, d), 0.0, 1e-12);
EXPECT_NEAR(distanceToPlane(0.0, 4.0, 0.0, a, b, c, d), 0.0, 1e-12);
EXPECT_NEAR(distanceToPlane(6.0, 0.0, 0.0, a, b, c, d), 0.0, 1e-12);
// Points off plane
EXPECT_DOUBLE_EQ(distanceToPlane(0.0, 0.0, 0.0, a, b, c, d), 12.0 / 7.0);
EXPECT_DOUBLE_EQ(distanceToPlane(0.0, 0.0, 1.0, a, b, c, d), 6.0 / 7.0);
EXPECT_DOUBLE_EQ(distanceToPlane(0.0, 2.0, 0.0, a, b, c, d), 6.0 / 7.0);
EXPECT_DOUBLE_EQ(distanceToPlane(3.0, 0.0, 0.0, a, b, c, d), 6.0 / 7.0);
}

TEST(UtilitiesTests, plane) {
double x0 = 0.0;
double y0 = 0.0;
double z0 = 2.0;
double v1x = 0.0;
double v1y = 4.0;
double v1z = -2.0;
double v2x = 6.0;
double v2y = 0.0;
double v2z = -2.0;
double a, b, c, d;
plane(x0, y0, z0, v1x, v1y, v1z, v2x, v2y, v2z, a, b, c, d);
EXPECT_DOUBLE_EQ(a, -2.0 / 7.0);
EXPECT_DOUBLE_EQ(b, -3.0 / 7.0);
EXPECT_DOUBLE_EQ(c, -6.0 / 7.0);
EXPECT_DOUBLE_EQ(d, 12.0 / 7.0);
}

TEST(UtilitiesTests, fitLinearApproximation) {
std::vector<double> x = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
std::vector<double> y = {1, 0, 5, 7, -2, 1, 2, 2, 1, 0, 1};

std::vector<int> nodes = fitLinearApproximation(x, y, 1.0);
ASSERT_EQ(nodes.size(), 7);
EXPECT_EQ(nodes[0], 0);
EXPECT_EQ(nodes[1], 1);
EXPECT_EQ(nodes[2], 3);
EXPECT_EQ(nodes[3], 4);
EXPECT_EQ(nodes[4], 6);
EXPECT_EQ(nodes[5], 9);
EXPECT_EQ(nodes[6], 10);
}


TEST(UtilitiesTests, calculateRotationMatrixFromEuler) {
double euler[3], rotationMatrix[9];
euler[0] = 0;
Expand Down
Loading

0 comments on commit 0bddf1d

Please sign in to comment.