Commit d5c49d98 authored by Oleg Alexandrov's avatar Oleg Alexandrov
Browse files

Implement radtan distortion and undistortion

parent ce4d6a19

include/usgscsm/Distortion.h

+4 −4
Original line number Diff line number Diff line
@@ -6,10 +6,10 @@ 
#include <tuple>

#include <vector>



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

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



// Transverse Distortion

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

// Transverse distortion Jacobian

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

                                  const std::vector<double> opticalDistCoeffs);



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

src/Distortion.cpp

+212 −10
Original line number Diff line number Diff line
@@ -3,7 +3,8 @@ 
#include <Error.h>

#include <string>



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

// Jacobian for Transverse distortion

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

                                  const std::vector<double> opticalDistCoeffs) {

  double d_dx[10];

  d_dx[0] = 0;
@@ -82,6 +83,138 @@ 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. "

        "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;



#if 0

// Check the partial derivatives with numerical differentiation              

  {

    double eps = 1e-4;

    double y0 = y;

    y = y0 + eps;

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

      

    double dx1 = x * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)

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



    double dy1 = y * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)

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



    y = y0 - eps;

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



    double dx2 = x * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)

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

              

    double dy2 = y * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)

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



    std::cout << "--numerical1 is " << (dx1-dx2)/(2*eps) << " " << jacobian[1] << std::endl;

    std::cout << "diff1 is " << (dx1-dx2)/(2*eps) - jacobian[1] << std::endl;

                  

    std::cout << "--numerical3 is " << (dy1-dy2)/(2*eps) << " " << jacobian[3] << std::endl;

    std::cout << "diff3 is " << (dy1-dy2)/(2*eps) - jacobian[3] << std::endl;

    

    y = y0;

  }



  {

    double eps = 1e-4;

    double x0 = x;

    x = x0 + eps;

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

      

    double dx1 = x * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)

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



    double dy1 = y * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)

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



    x = x0 - eps;

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



    double dx2 = x * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)

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

              

    double dy2 = y * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)

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



    std::cout << "--numerical0 is " << (dx1-dx2)/(2*eps) << " " << jacobian[0] << std::endl;

    std::cout << "diff0 is " << (dx1-dx2)/(2*eps) - jacobian[0] << std::endl;

                  

    std::cout << "--numerical2 is " << (dy1-dy2)/(2*eps) << " " << jacobian[2] << std::endl;

    std::cout << "diff2 is " << (dy1-dy2)/(2*eps) - jacobian[2] << std::endl;

    

    x = x0;

  }

#endif  

}



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

                      const std::vector<double> opticalDistCoeffs,

                      DistortionType distortionType, const double tolerance) {
@@ -132,7 +265,7 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, 
        fx = dx - fx;

        fy = dy - fy;



        distortionJacobian(x, y, jacobian, opticalDistCoeffs);

        transverseDistortionJacobian(x, y, jacobian, opticalDistCoeffs);



        // Jxx * Jyy - Jxy * Jyx

        double determinant =
@@ -316,6 +449,66 @@ 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:

    {

      // 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;



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



      for (int count = 1;

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

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



        fx = dx - fx;

        fy = dy - fy;



        radTanDistortionJacobian(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

    }

    break;

    

  }

}
@@ -327,7 +520,7 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, 
  dy = uy;



  switch (distortionType) {

    // Compute undistorted focal plane coordinate given a distorted

    // Compute distorted focal plane coordinate given undistorted

    // focal plane coordinate. This case works by iteratively adding distortion

    // until the new distorted point, r, undistorts to within a tolerance of the

    // original point, rp.
@@ -454,9 +647,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;
@@ -468,7 +661,7 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, 


    // The LRO LROC NAC distortion model uses an iterative approach to go from

    // undistorted x,y to distorted x,y

    // Algorithum adapted from ISIS3 LRONarrowAngleDistortionMap.cpp

    // Algorithm adapted from ISIS3 LRONarrowAngleDistortionMap.cpp

    case LROLROCNAC: {

      double yt = uy;
@@ -491,9 +684,9 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, 


      double dk1 = opticalDistCoeffs[0];



      // Owing to the odd distotion model employed in this senser if |y| is >

      // Owing to the odd distortion model employed in this sensor if |y| is >

      // 116.881145553046 then there is no root to find.  Further, the greatest

      // y that any measure on the sensor will acutally distort to is less

      // y that any measure on the sensor will actually distort to is less

      // than 20.  Thus, if any distorted measure is greater that that skip the

      // iterations.  The points isn't in the cube, and exactly how far outside

      // the cube is irrelevant.  Just let the camera model know its not in the
@@ -597,5 +790,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;

    

  }

}

tests/DistortionTests.cpp

+29 −4
Original line number Diff line number Diff line
@@ -19,7 +19,7 @@ TEST_P(ImageCoordParameterizedTest, JacobianTest) { 


  csm::ImageCoord imagePt = GetParam();

  double jacobian[4];

  distortionJacobian(imagePt.samp, imagePt.line, jacobian,

  transverseDistortionJacobian(imagePt.samp, imagePt.line, jacobian,

                               transverseDistortionCoeffs);



  // Jxx * Jyy - Jxy * Jyx
@@ -36,7 +36,7 @@ TEST(Transverse, Jacobian1) { 
      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0};



  double jacobian[4];

  distortionJacobian(imagePt.samp, imagePt.line, jacobian,

  transverseDistortionJacobian(imagePt.samp, imagePt.line, jacobian,

                               transverseDistortionCoeffs);



  EXPECT_NEAR(jacobian[0], 56.25, 1e-8);
@@ -380,3 +380,28 @@ TEST_P(CoeffOffsetParameterizedTest, InverseOnesCoeffsCahvorTest) 
  EXPECT_NEAR(dx, 4, 1e-8);

  EXPECT_NEAR(dy, 0, 1e-8);

}



INSTANTIATE_TEST_SUITE_P(RadTanInversionTest, RadTanTest,

                         ::testing::Values(std::vector<double>(0, 0)));



TEST_P(RadTanTest, RadTanInversionTest)

{



  // Initialize radtan distortion coefficients (k1, k2, p1, p2, k3)  

  std::vector<double> distCoeffs= {0.000031, -0.000056, 1.3e-5, -1.7e-6, 2.9e-8};

  

  double ux = 5.0, uy = 6.0; 

  

  // Compute distortion 

  double dx, dy;

  applyDistortion(ux, uy, dx, dy, distCoeffs, DistortionType::RADTAN, 1e-8, 1e-8);

  

  // Remove distortion (undistort)

  double ux2, uy2;

  removeDistortion(dx, dy, ux2, uy2, distCoeffs, DistortionType::RADTAN, 1e-8);

  

  EXPECT_NEAR(dx, 4.0010785450000004, 1e-8);

  EXPECT_NEAR(dy, 4.8022116940000013, 1e-8);

  EXPECT_NEAR(ux2, ux, 1e-8);

  EXPECT_NEAR(uy2, uy, 1e-8);

}

tests/Fixtures.h

+3 −0
Original line number Diff line number Diff line
@@ -180,6 +180,9 @@ class ImageCoordParameterizedTest 
class CoeffOffsetParameterizedTest

    : public ::testing::TestWithParam<std::vector<double>> {};



class RadTanTest

    : public ::testing::TestWithParam<std::vector<double>> {};



class FramerParameterizedTest

    : public ::testing::TestWithParam<csm::ImageCoord> {

 protected: