Unverified Commit 0f065ca6 authored by Oleg Alexandrov's avatar Oleg Alexandrov Committed by GitHub
Browse files

Support for the radial + tangential distortion model (#466) 

* Implement radtan distortion and undistortion

* Modularize the distortion operation

Modularize the undistortion operation

* Sync up with ale

* Fix typo

* Add test for loading and using radtan distortion
parent f0370224

include/usgscsm/Distortion.h

+9 −7
Original line number Diff line number Diff line
@@ -6,22 +6,24 @@ 
#include <tuple>

#include <vector>



enum DistortionType { RADIAL, TRANSVERSE, KAGUYALISM, DAWNFC, LROLROCNAC, CAHVOR };

// This should be synched with the enum in ale/Distortion.h

enum DistortionType { RADIAL, TRANSVERSE, KAGUYALISM, DAWNFC, LROLROCNAC, CAHVOR, 

                     LUNARORBITER, RADTAN };



// Transverse Distortion

void distortionJacobian(double x, double y, double *jacobian,

                        const std::vector<double> opticalDistCoeffs);

// Transverse distortion Jacobian

void transverseDistortionJacobian(double x, double y, double *jacobian,

                                  std::vector<double> const& opticalDistCoeffs);



void computeTransverseDistortion(double ux, double uy, double &dx, double &dy,

                                 const std::vector<double> opticalDistCoeffs);

                                 std::vector<double> const& opticalDistCoeffs);



void removeDistortion(double dx, double dy, double &ux, double &uy,

                      const std::vector<double> opticalDistCoeffs,

                      std::vector<double> const& opticalDistCoeffs,

                      DistortionType distortionType,

                      const double tolerance = 1.0E-6);



void applyDistortion(double ux, double uy, double &dx, double &dy,

                     const std::vector<double> opticalDistCoeffs,

                     std::vector<double> const& opticalDistCoeffs,

                     DistortionType distortionType,

                     const double desiredPrecision = 1.0E-6,

                     const double tolerance = 1.0E-6);

include/usgscsm/Utilities.h

+9 −0
Original line number Diff line number Diff line
@@ -65,6 +65,15 @@ void lagrangeInterp(const int &numTime, const double *valueArray, 
double brentRoot(double lowerBound, double upperBound,

                 std::function<double(double)> func, double epsilon = 1e-10);



// Use the Newton-Raphson method undistort a pixel (dx, dy), producing (ux, uy).

void newtonRaphson(double dx, double dy, double &ux, double &uy,

                    std::vector<double> const& opticalDistCoeffs,

                    DistortionType distortionType, const double tolerance,

                    std::function<void(double, double, double &, double &,

                                       std::vector<double> const&)> distortionFunction,

                    std::function<void(double, double, double *, 

                                       std::vector<double> const&)> distortionJacobian);



// Evaluate a polynomial function.

// Coefficients should be ordered least order to greatest I.E. {1, 2, 3} is 1 +

// 2x + 3x^2

src/Distortion.cpp

+101 −57
Original line number Diff line number Diff line 
#include "Distortion.h"

#include "Utilities.h"



#include <Error.h>

#include <string>



void distortionJacobian(double x, double y, double *jacobian,

                        const std::vector<double> opticalDistCoeffs) {

// Jacobian for Transverse distortion

void transverseDistortionJacobian(double x, double y, double *jacobian,

                                  std::vector<double> const& opticalDistCoeffs) {

  double d_dx[10];

  d_dx[0] = 0;

  d_dx[1] = 1;
@@ -57,7 +59,7 @@ void distortionJacobian(double x, double y, double *jacobian, 
 * tuple

 */

void computeTransverseDistortion(double ux, double uy, double &dx, double &dy,

                                 const std::vector<double> opticalDistCoeffs) {

                                 std::vector<double> const& opticalDistCoeffs) {

  double f[10];

  f[0] = 1;

  f[1] = ux;
@@ -82,8 +84,77 @@ void computeTransverseDistortion(double ux, double uy, double &dx, double &dy, 
  }

}



// Compute distorted focal plane coordinates given undistorted coordinates. Use

// the radial-tangential distortion model with 5 coefficients (k1, k2, k3 for

// radial distortion, and p1, p2 for tangential distortion). This was tested to

// give the same results as the OpenCV distortion model, by invoking the

// function cv::projectPoints() (with zero rotation, zero translation, and

// identity camera matrix). The parameters are stored in opticalDistCoeffs 

// in the order: [k1, k2, p1, p2, k3]. 

void computeRadTanDistortion(double ux, double uy, double &dx, double &dy,

                             std::vector<double> const& opticalDistCoeffs) {



  if (opticalDistCoeffs.size() != 5) {

    csm::Error::ErrorType errorType = csm::Error::INDEX_OUT_OF_RANGE;

    std::string message =

        "Distortion coefficients for the radtan distortion model must be of size 5, "

        "in the order k1, k2, p1, p2, k3. Got: " + std::to_string(opticalDistCoeffs.size());

    std::string function = "computeRadTanDistortion";

    throw csm::Error(errorType, message, function);

  }

  

  // Shorten notation

  double x = ux, y = uy; 

  double k1 = opticalDistCoeffs[0];

  double k2 = opticalDistCoeffs[1];

  double p1 = opticalDistCoeffs[2];

  double p2 = opticalDistCoeffs[3];

  double k3 = opticalDistCoeffs[4];

  

  double r2 = (x * x) + (y * y);

  

  dx = x * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)

     + (2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x));

       

  dy = y * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)

     + (p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y);

}



// Compute the jacobian for radtan distortion

void radTanDistortionJacobian(double x, double y, double *jacobian,

                              std::vector<double> const& opticalDistCoeffs) {



  double k1 = opticalDistCoeffs[0];

  double k2 = opticalDistCoeffs[1];

  double p1 = opticalDistCoeffs[2];

  double p2 = opticalDistCoeffs[3];

  double k3 = opticalDistCoeffs[4];

  

  double r2 = x * x + y * y;

  double dr2dx = 2.0 * x;

  double dr2dy = 2.0 * y;



  // dfx / dx 

  jacobian[0] = (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)    

              + x * (k1 * dr2dx + k2 * dr2dx * 2.0 * r2 + k3 * dr2dx * 3.0 * r2 * r2)

              + 2.0 * p1 * y + p2 * (dr2dx + 4.0 * x);

  

  // dfx / dy

  jacobian[1] = x * (k1 * dr2dy + k2 * dr2dy * 2.0 * r2 + k3 * dr2dy * 3.0 * r2 * r2)

              + 2.0 * p1 * x  + p2 * dr2dy;

              

  // dfy / dx

  jacobian[2] = y * (k1 * dr2dx + k2 * dr2dx * 2.0 * r2 + k3 * dr2dx * 3.0 * r2 * r2)

              + (p1 * dr2dx + 2.0 * p2 * y);

  

  // dfy / dy

  jacobian[3] = (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)

              + y * (k1 * dr2dy + k2 * dr2dy * 2.0 * r2 + k3 * dr2dy * 3.0 * r2 * r2)

              + p1 * (dr2dy + 4.0 * y) + 2.0 * p2 * x;

}



void removeDistortion(double dx, double dy, double &ux, double &uy,

                      const std::vector<double> opticalDistCoeffs,

                      std::vector<double> const& opticalDistCoeffs,

                      DistortionType distortionType, const double tolerance) {

  ux = dx;

  uy = dy;
@@ -109,55 +180,8 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, 
      // Solve the distortion equation using the Newton-Raphson method.

      // Set the error tolerance to about one millionth of a NAC pixel.

      // The maximum number of iterations of the Newton-Raphson method.

      const int maxTries = 20;



      double x;

      double y;

      double fx;

      double fy;

      double jacobian[4];



      // Initial guess at the root

      x = dx;

      y = dy;



      computeTransverseDistortion(x, y, fx, fy, opticalDistCoeffs);



      for (int count = 1;

           ((fabs(fx) + fabs(fy)) > tolerance) && (count < maxTries); count++) {

        computeTransverseDistortion(x, y, fx, fy, opticalDistCoeffs);



        fx = dx - fx;

        fy = dy - fy;



        distortionJacobian(x, y, jacobian, opticalDistCoeffs);



        // Jxx * Jyy - Jxy * Jyx

        double determinant =

            jacobian[0] * jacobian[3] - jacobian[1] * jacobian[2];

        if (fabs(determinant) < 1E-6) {

          ux = x;

          uy = y;

          //

          // Near-zero determinant. Add error handling here.

          //

          //-- Just break out and return with no convergence

          return;

        }



        x = x + (jacobian[3] * fx - jacobian[1] * fy) / determinant;

        y = y + (jacobian[0] * fy - jacobian[2] * fx) / determinant;

      }



      if ((fabs(fx) + fabs(fy)) <= tolerance) {

        // The method converged to a root.

        ux = x;

        uy = y;



        return;

      }

      // Otherwise method did not converge to a root within the maximum

      // number of iterations

      newtonRaphson(dx, dy, ux, uy, opticalDistCoeffs, distortionType, tolerance, 

                    computeTransverseDistortion, transverseDistortionJacobian);

    } break;



    case KAGUYALISM: {
@@ -314,11 +338,22 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, 
      }

    }

    break;

    

    // Compute undistorted focal plane coordinate given distorted coordinates

    // with the radtan model. See computeRadTanDistortion() for more details.

    case RADTAN:

    {

      newtonRaphson(dx, dy, ux, uy, opticalDistCoeffs, distortionType, tolerance, 

                    computeRadTanDistortion, radTanDistortionJacobian);

      

    }

    break;

    

  }

}



void applyDistortion(double ux, double uy, double &dx, double &dy,

                     const std::vector<double> opticalDistCoeffs,

                     std::vector<double> const& opticalDistCoeffs,

                     DistortionType distortionType,

                     const double desiredPrecision, const double tolerance) {

  dx = ux;
@@ -451,9 +486,9 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, 
      }

    } break;



    // The dawn distortion model is "reversed" from other distortion models so

    // the apply function computes distorted coordinates as a

    // fn(undistorted coordinates)

    // The dawn distortion model is "reversed" from other distortion models. 

    // The apply function computes distorted coordinates as a

    // function of undistorted coordinates.

    case DAWNFC: {

      double r2;
@@ -594,5 +629,14 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, 
      }

    }

    break;

    

    // Compute distorted focal plane coordinate given undistorted coordinates

    // with the RADTAN model. See computeRadTanDistortion() for more details.

    case RADTAN:

    {

      computeRadTanDistortion(ux, uy, dx, dy, opticalDistCoeffs);  

    }  

    break;

    

  }

}

src/Utilities.cpp

+82 −2
Original line number Diff line number Diff line
@@ -401,6 +401,57 @@ double brentRoot(double lowerBound, double upperBound, 
  return nextPoint;

}



// Use the Newton-Raphson method undistort a pixel (dx, dy), producing (ux, uy).

void newtonRaphson(double dx, double dy, double &ux, double &uy,

                    std::vector<double> const& opticalDistCoeffs,

                    DistortionType distortionType, const double tolerance,

                    std::function<void(double, double, double &, double &,

                                       std::vector<double> const&)> distortionFunction,

                    std::function<void(double, double, double *, 

                                       std::vector<double> const&)> distortionJacobian) {



  const int maxTries = 20;



  double x, y, fx, fy, jacobian[4];



  // Initial guess for the root

  x = dx;

  y = dy;



  distortionFunction(x, y, fx, fy, opticalDistCoeffs);



  for (int count = 1;

        ((fabs(fx) + fabs(fy)) > tolerance) && (count < maxTries); count++) {

    distortionFunction(x, y, fx, fy, opticalDistCoeffs);



    fx = dx - fx;

    fy = dy - fy;



    distortionJacobian(x, y, jacobian, opticalDistCoeffs);



    // Jxx * Jyy - Jxy * Jyx

    double determinant =

        jacobian[0] * jacobian[3] - jacobian[1] * jacobian[2];

    if (fabs(determinant) < 1e-6) {

      ux = x;

      uy = y;

      // Near-zero determinant. Cannot continue. Return most recent result.

      return;

    }



    x = x + (jacobian[3] * fx - jacobian[1] * fy) / determinant;

    y = y + (jacobian[0] * fy - jacobian[2] * fx) / determinant;

  }



  if ((fabs(fx) + fabs(fy)) <= tolerance) {

    // The method converged to a root.

    ux = x;

    uy = y;



    return;

  }

}



double evaluatePolynomial(const std::vector<double> &coeffs, double x) {

  if (coeffs.empty()) {

    throw std::invalid_argument("Polynomial coeffs must be non-empty.");
@@ -1009,9 +1060,9 @@ double getSemiMinorRadius(json isd, csm::WarningList *list) { 
// type. Defaults to transverse

DistortionType getDistortionModel(json isd, csm::WarningList *list) {

  try {

    json distoriton_subset = isd.at("optical_distortion");

    json distortion_subset = isd.at("optical_distortion");



    json::iterator it = distoriton_subset.begin();

    json::iterator it = distortion_subset.begin();



    std::string distortion = (std::string)it.key();
@@ -1025,6 +1076,12 @@ DistortionType getDistortionModel(json isd, csm::WarningList *list) { 
      return DistortionType::DAWNFC;

    } else if (distortion.compare("lrolrocnac") == 0) {

      return DistortionType::LROLROCNAC;

    } else if (distortion.compare("cahvor") == 0) {

      return DistortionType::CAHVOR;

    } else if (distortion.compare("lunarorbiter") == 0) {

      return DistortionType::LUNARORBITER;

    } else if (distortion.compare("radtan") == 0) {

      return DistortionType::RADTAN;

    }

  } catch (...) {

    if (list) {
@@ -1054,6 +1111,10 @@ DistortionType getDistortionModel(int aleDistortionModel, 
      return DistortionType::LROLROCNAC;

    }else if (aleDistortionType == ale::DistortionType::CAHVOR) {

      return DistortionType::CAHVOR;

    }else if (aleDistortionType == ale::DistortionType::LUNARORBITER) {

      return DistortionType::LUNARORBITER;

    }else if (aleDistortionType == ale::DistortionType::RADTAN) {

      return DistortionType::RADTAN;

    }

  } catch (...) {

    if (list) {
@@ -1212,7 +1273,26 @@ std::vector<double> getDistortionCoeffs(json isd, csm::WarningList *list) { 
        coefficients = std::vector<double>(6, 0.0);

      }

    } break;

    case DistortionType::RADTAN: {

      try {

        coefficients = isd.at("optical_distortion")

                           .at("radtan")

                           .at("coefficients")

                           .get<std::vector<double>>();



        return coefficients;

      } catch (...) {

        if (list) {

          list->push_back(csm::Warning(

              csm::Warning::DATA_NOT_AVAILABLE,

              "Could not parse the radtan distortion model coefficients.",

              "Utilities::getDistortion()"));

        }

        coefficients = std::vector<double>(5, 0.0);

      }

    } break;

  }

  

  if (list) {

    list->push_back(

        csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,

tests/CMakeLists.txt

+5 −1
Original line number Diff line number Diff line
@@ -33,7 +33,6 @@ add_test(NAME test_usgscsm_cam_test_load_state 
    COMMAND usgscsm_cam_test --model model_state.json --output-model-state model_state2.json

    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/tests)





# 3. Save model state from .sup file

add_test(NAME test_usgscsm_cam_test_save_sup_state

    COMMAND usgscsm_cam_test --model data/gxp_model_file.sup --output-model-state gxp_model_state.json
@@ -43,4 +42,9 @@ add_test(NAME test_usgscsm_cam_test_load_sup_state 
    COMMAND usgscsm_cam_test --model gxp_model_state.json --output-model-state gxp_model_state2.json

    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/tests)



# 5. Test the radtan model (this distortion model gets added to a DawnFC sensor)

add_test(NAME test_usgscsm_cam_test_radtan

    COMMAND usgscsm_cam_test --model data/dawnfc_radtan.json  --sample-rate 100 --output-model-state dawnfc_radtan_model_state.json

    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/tests)

    

gtest_discover_tests(runCSMCameraModelTests WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/tests)