Tracking: Implement Brown-Conrady distortion model

Implemented Brown-Conrady lens distortion model with 4 radial and
2 tangential coefficients to improve compatibility with other software,
such as Agisoft Photoscan/Metashapes, 3DF Zephir, RealityCapture,
Bentley ContextCapture, Alisevision Meshroom(opensource).

Also older programs: Bundler, CPMVS.
In general terms, most photogrammetric software.

The new model is available under the distortion model menu in Lens
settings.

For tests and demos check the original patch.

Reviewed By: sergey

Differential Revision: https://developer.blender.org/D9037

7 files changed, 389 insertions, 2 deletions

diff --git a/intern/libmv/intern/camera_intrinsics.cc b/intern/libmv/intern/camera_intrinsics.cc
index 554c4350b0a..628637e12cc 100644
--- a/intern/libmv/intern/camera_intrinsics.cc
+++ b/intern/libmv/intern/camera_intrinsics.cc
@@ -25,6 +25,7 @@ using libmv::CameraIntrinsics;
 using libmv::DivisionCameraIntrinsics;
 using libmv::PolynomialCameraIntrinsics;
 using libmv::NukeCameraIntrinsics;
+using libmv::BrownCameraIntrinsics;
 
 libmv_CameraIntrinsics *libmv_cameraIntrinsicsNew(
     const libmv_CameraIntrinsicsOptions* libmv_camera_intrinsics_options) {
@@ -64,6 +65,14 @@ libmv_CameraIntrinsics *libmv_cameraIntrinsicsCopy(
                                           *nuke_intrinsics);
         break;
       }
+    case libmv::DISTORTION_MODEL_BROWN:
+      {
+        const BrownCameraIntrinsics *brown_intrinsics =
+          static_cast<const BrownCameraIntrinsics*>(orig_intrinsics);
+        new_intrinsics = LIBMV_OBJECT_NEW(BrownCameraIntrinsics,
+                                          *brown_intrinsics);
+        break;
+      }
     default:
       assert(!"Unknown distortion model");
   }
@@ -164,6 +173,35 @@ void libmv_cameraIntrinsicsUpdate(
         break;
       }
 
+    case LIBMV_DISTORTION_MODEL_BROWN:
+      {
+        assert(camera_intrinsics->GetDistortionModelType() ==
+               libmv::DISTORTION_MODEL_BROWN);
+
+        BrownCameraIntrinsics *brown_intrinsics =
+          (BrownCameraIntrinsics *) camera_intrinsics;
+
+        double k1 = libmv_camera_intrinsics_options->brown_k1;
+        double k2 = libmv_camera_intrinsics_options->brown_k2;
+        double k3 = libmv_camera_intrinsics_options->brown_k3;
+        double k4 = libmv_camera_intrinsics_options->brown_k4;
+
+        if (brown_intrinsics->k1() != k1 ||
+            brown_intrinsics->k2() != k2 ||
+            brown_intrinsics->k3() != k3 ||
+            brown_intrinsics->k4() != k4) {
+          brown_intrinsics->SetRadialDistortion(k1, k2, k3, k4);
+        }
+
+        double p1 = libmv_camera_intrinsics_options->brown_p1;
+        double p2 = libmv_camera_intrinsics_options->brown_p2;
+
+        if (brown_intrinsics->p1() != p1 || brown_intrinsics->p2() != p2) {
+          brown_intrinsics->SetTangentialDistortion(p1, p2);
+        }
+        break;
+      }
+
     default:
       assert(!"Unknown distortion model");
   }
@@ -228,6 +266,21 @@ void libmv_cameraIntrinsicsExtractOptions(
         break;
       }
 
+    case libmv::DISTORTION_MODEL_BROWN:
+      {
+        const BrownCameraIntrinsics *brown_intrinsics =
+          static_cast<const BrownCameraIntrinsics *>(camera_intrinsics);
+        camera_intrinsics_options->distortion_model =
+          LIBMV_DISTORTION_MODEL_BROWN;
+        camera_intrinsics_options->brown_k1 = brown_intrinsics->k1();
+        camera_intrinsics_options->brown_k2 = brown_intrinsics->k2();
+        camera_intrinsics_options->brown_k3 = brown_intrinsics->k3();
+        camera_intrinsics_options->brown_k4 = brown_intrinsics->k4();
+        camera_intrinsics_options->brown_p1 = brown_intrinsics->p1();
+        camera_intrinsics_options->brown_p2 = brown_intrinsics->p2();
+        break;
+      }
+
     default:
       assert(!"Unknown distortion model");
   }
@@ -366,6 +419,23 @@ static void libmv_cameraIntrinsicsFillFromOptions(
         break;
       }
 
+    case LIBMV_DISTORTION_MODEL_BROWN:
+      {
+        BrownCameraIntrinsics *brown_intrinsics =
+          static_cast<BrownCameraIntrinsics*>(camera_intrinsics);
+
+        brown_intrinsics->SetRadialDistortion(
+            camera_intrinsics_options->brown_k1,
+            camera_intrinsics_options->brown_k2,
+            camera_intrinsics_options->brown_k3,
+            camera_intrinsics_options->brown_k4);
+        brown_intrinsics->SetTangentialDistortion(
+          camera_intrinsics_options->brown_p1,
+          camera_intrinsics_options->brown_p2);
+
+        break;
+      }
+
     default:
       assert(!"Unknown distortion model");
   }
@@ -384,6 +454,9 @@ CameraIntrinsics* libmv_cameraIntrinsicsCreateFromOptions(
     case LIBMV_DISTORTION_MODEL_NUKE:
       camera_intrinsics = LIBMV_OBJECT_NEW(NukeCameraIntrinsics);
       break;
+    case LIBMV_DISTORTION_MODEL_BROWN:
+      camera_intrinsics = LIBMV_OBJECT_NEW(BrownCameraIntrinsics);
+      break;
     default:
       assert(!"Unknown distortion model");
   }
diff --git a/intern/libmv/intern/camera_intrinsics.h b/intern/libmv/intern/camera_intrinsics.h
index b3d259893bd..eb6176770ec 100644
--- a/intern/libmv/intern/camera_intrinsics.h
+++ b/intern/libmv/intern/camera_intrinsics.h
@@ -30,6 +30,7 @@ enum {
   LIBMV_DISTORTION_MODEL_POLYNOMIAL = 0,
   LIBMV_DISTORTION_MODEL_DIVISION = 1,
   LIBMV_DISTORTION_MODEL_NUKE = 2,
+  LIBMV_DISTORTION_MODEL_BROWN = 3,
 };
 
 typedef struct libmv_CameraIntrinsicsOptions {
@@ -49,6 +50,10 @@ typedef struct libmv_CameraIntrinsicsOptions {
 
   // Nuke distortion model.
   double nuke_k1, nuke_k2;
+
+  // Brown-Conrady distortion model.
+  double brown_k1, brown_k2, brown_k3, brown_k4;
+  double brown_p1, brown_p2;
 } libmv_CameraIntrinsicsOptions;
 
 libmv_CameraIntrinsics *libmv_cameraIntrinsicsNew(
diff --git a/intern/libmv/libmv/simple_pipeline/bundle.cc b/intern/libmv/libmv/simple_pipeline/bundle.cc
index 22ab0cdf864..c055846318a 100644
--- a/intern/libmv/libmv/simple_pipeline/bundle.cc
+++ b/intern/libmv/libmv/simple_pipeline/bundle.cc
@@ -50,6 +50,7 @@ enum {
   OFFSET_K1,
   OFFSET_K2,
   OFFSET_K3,
+  OFFSET_K4,
   OFFSET_P1,
   OFFSET_P2,
 
@@ -135,6 +136,26 @@ void ApplyDistortionModelUsingIntrinsicsBlock(
         LOG(FATAL) << "Unsupported distortion model.";
         return;
       }
+
+    case DISTORTION_MODEL_BROWN:
+      {
+        const T& k1 = intrinsics_block[OFFSET_K1];
+        const T& k2 = intrinsics_block[OFFSET_K2];
+        const T& k3 = intrinsics_block[OFFSET_K3];
+        const T& k4 = intrinsics_block[OFFSET_K4];
+        const T& p1 = intrinsics_block[OFFSET_P1];
+        const T& p2 = intrinsics_block[OFFSET_P2];
+
+        ApplyBrownDistortionModel(focal_length,
+                                  focal_length,
+                                  principal_point_x,
+                                  principal_point_y,
+                                  k1, k2, k3, k4,
+                                  p1, p2,
+                                  normalized_x, normalized_y,
+                                  distorted_x, distorted_y);
+        return;
+      }
   }
 
   LOG(FATAL) << "Unknown distortion model.";
@@ -168,6 +189,7 @@ void InvertDistortionModelUsingIntrinsicsBlock(
   switch (invariant_intrinsics->GetDistortionModelType()) {
     case DISTORTION_MODEL_POLYNOMIAL:
     case DISTORTION_MODEL_DIVISION:
+    case DISTORTION_MODEL_BROWN:
       LOG(FATAL) << "Unsupported distortion model.";
       return;
 
@@ -783,8 +805,9 @@ void EuclideanBundleCommonIntrinsics(
     MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P2,   OFFSET_P2);
 #undef MAYBE_SET_CONSTANT
 
-    // Always set K3 constant, it's not used at the moment.
+    // Always set K3 and K4 constant, it's not used at the moment.
     constant_intrinsics.push_back(OFFSET_K3);
+    constant_intrinsics.push_back(OFFSET_K4);
 
     ceres::SubsetParameterization *subset_parameterization =
       new ceres::SubsetParameterization(OFFSET_MAX, constant_intrinsics);
diff --git a/intern/libmv/libmv/simple_pipeline/camera_intrinsics.cc b/intern/libmv/libmv/simple_pipeline/camera_intrinsics.cc
index c7c827fcd66..052714bbb3e 100644
--- a/intern/libmv/libmv/simple_pipeline/camera_intrinsics.cc
+++ b/intern/libmv/libmv/simple_pipeline/camera_intrinsics.cc
@@ -296,6 +296,72 @@ void NukeCameraIntrinsics::InvertIntrinsics(double image_x,
                             normalized_y);
 }
 
+// Brown model.
+
+BrownCameraIntrinsics::BrownCameraIntrinsics()
+    : CameraIntrinsics() {
+  SetRadialDistortion(0.0, 0.0, 0.0, 0.0);
+  SetTangentialDistortion(0.0, 0.0);
+}
+
+BrownCameraIntrinsics::BrownCameraIntrinsics(
+    const BrownCameraIntrinsics &from)
+    : CameraIntrinsics(from) {
+  SetRadialDistortion(from.k1(), from.k2(), from.k3(), from.k4());
+  SetTangentialDistortion(from.p1(), from.p2());
+}
+
+void BrownCameraIntrinsics::SetRadialDistortion(double k1,
+                                                double k2,
+                                                double k3,
+                                                double k4) {
+  parameters_[OFFSET_K1] = k1;
+  parameters_[OFFSET_K2] = k2;
+  parameters_[OFFSET_K3] = k3;
+  parameters_[OFFSET_K4] = k4;
+  ResetLookupGrids();
+}
+
+void BrownCameraIntrinsics::SetTangentialDistortion(double p1,
+                                                    double p2) {
+  parameters_[OFFSET_P1] = p1;
+  parameters_[OFFSET_P2] = p2;
+  ResetLookupGrids();
+}
+
+void BrownCameraIntrinsics::ApplyIntrinsics(double normalized_x,
+                                            double normalized_y,
+                                            double *image_x,
+                                            double *image_y) const {
+  ApplyBrownDistortionModel(focal_length_x(),
+                            focal_length_y(),
+                            principal_point_x(),
+                            principal_point_y(),
+                            k1(), k2(), k3(), k4(),
+                            p1(), p2(),
+                            normalized_x,
+                            normalized_y,
+                            image_x,
+                            image_y);
+}
+
+void BrownCameraIntrinsics::InvertIntrinsics(
+    double image_x,
+    double image_y,
+    double *normalized_x,
+    double *normalized_y) const {
+  InvertBrownDistortionModel(focal_length_x(),
+                             focal_length_y(),
+                             principal_point_x(),
+                             principal_point_y(),
+                             k1(), k2(), k3(), k4(),
+                             p1(), p2(),
+                             image_x,
+                             image_y,
+                             normalized_x,
+                             normalized_y);
+}
+
 std::ostream& operator <<(std::ostream &os,
                           const CameraIntrinsics &intrinsics) {
   if (intrinsics.focal_length_x() == intrinsics.focal_length_x()) {
@@ -344,6 +410,18 @@ std::ostream& operator <<(std::ostream &os,
         PRINT_NONZERO_COEFFICIENT(nuke_intrinsics, k2);
         break;
       }
+    case DISTORTION_MODEL_BROWN:
+      {
+        const BrownCameraIntrinsics *brown_intrinsics =
+            static_cast<const BrownCameraIntrinsics *>(&intrinsics);
+        PRINT_NONZERO_COEFFICIENT(brown_intrinsics, k1);
+        PRINT_NONZERO_COEFFICIENT(brown_intrinsics, k2);
+        PRINT_NONZERO_COEFFICIENT(brown_intrinsics, k3);
+        PRINT_NONZERO_COEFFICIENT(brown_intrinsics, k4);
+        PRINT_NONZERO_COEFFICIENT(brown_intrinsics, p1);
+        PRINT_NONZERO_COEFFICIENT(brown_intrinsics, p2);
+        break;
+      }
     default:
       LOG(FATAL) << "Unknown distortion model.";
   }
diff --git a/intern/libmv/libmv/simple_pipeline/camera_intrinsics.h b/intern/libmv/libmv/simple_pipeline/camera_intrinsics.h
index 782fd56c54c..cf0bdb76ccb 100644
--- a/intern/libmv/libmv/simple_pipeline/camera_intrinsics.h
+++ b/intern/libmv/libmv/simple_pipeline/camera_intrinsics.h
@@ -447,6 +447,71 @@ class NukeCameraIntrinsics : public CameraIntrinsics {
   double parameters_[NUM_PARAMETERS];
 };
 
+class BrownCameraIntrinsics : public CameraIntrinsics {
+ public:
+  // This constants defines an offset of corresponding coefficients
+  // in the parameters_ array.
+  enum {
+    OFFSET_K1,
+    OFFSET_K2,
+    OFFSET_K3,
+    OFFSET_K4,
+    OFFSET_P1,
+    OFFSET_P2,
+
+    // This defines the size of array which we need to have in order
+    // to store all the coefficients.
+    NUM_PARAMETERS,
+  };
+
+  BrownCameraIntrinsics();
+  BrownCameraIntrinsics(const BrownCameraIntrinsics &from);
+
+  DistortionModelType GetDistortionModelType() const {
+    return DISTORTION_MODEL_BROWN;
+  }
+
+  int num_distortion_parameters() const { return NUM_PARAMETERS; }
+  double *distortion_parameters() { return parameters_; };
+  const double *distortion_parameters() const { return parameters_; };
+
+  double k1() const { return parameters_[OFFSET_K1]; }
+  double k2() const { return parameters_[OFFSET_K2]; }
+  double k3() const { return parameters_[OFFSET_K3]; }
+  double k4() const { return parameters_[OFFSET_K4]; }
+  double p1() const { return parameters_[OFFSET_P1]; }
+  double p2() const { return parameters_[OFFSET_P2]; }
+
+  // Set radial distortion coeffcients.
+  void SetRadialDistortion(double k1, double k2, double k3, double k4);
+
+  // Set tangential distortion coeffcients.
+  void SetTangentialDistortion(double p1, double p2);
+
+  // Apply camera intrinsics to the normalized point to get image coordinates.
+  //
+  // This applies the lens distortion to a point which is in normalized
+  // camera coordinates (i.e. the principal point is at (0, 0)) to get image
+  // coordinates in pixels.
+  void ApplyIntrinsics(double normalized_x,
+                       double normalized_y,
+                       double *image_x,
+                       double *image_y) const;
+
+  // Invert camera intrinsics on the image point to get normalized coordinates.
+  //
+  // This reverses the effect of lens distortion on a point which is in image
+  // coordinates to get normalized camera coordinates.
+  void InvertIntrinsics(double image_x,
+                        double image_y,
+                        double *normalized_x,
+                        double *normalized_y) const;
+
+ private:
+  double parameters_[NUM_PARAMETERS];
+};
+
+
 /// A human-readable representation of the camera intrinsic parameters.
 std::ostream& operator <<(std::ostream &os,
                           const CameraIntrinsics &intrinsics);
diff --git a/intern/libmv/libmv/simple_pipeline/distortion_models.cc b/intern/libmv/libmv/simple_pipeline/distortion_models.cc
index c069fc6f623..f602234b630 100644
--- a/intern/libmv/libmv/simple_pipeline/distortion_models.cc
+++ b/intern/libmv/libmv/simple_pipeline/distortion_models.cc
@@ -117,6 +117,56 @@ struct InvertDivisionIntrinsicsCostFunction {
   double x_, y_;
 };
 
+struct InvertBrownIntrinsicsCostFunction {
+ public:
+  typedef Vec2 FMatrixType;
+  typedef Vec2 XMatrixType;
+
+  InvertBrownIntrinsicsCostFunction(const double focal_length_x,
+                                    const double focal_length_y,
+                                    const double principal_point_x,
+                                    const double principal_point_y,
+                                    const double k1,
+                                    const double k2,
+                                    const double k3,
+                                    const double k4,
+                                    const double p1,
+                                    const double p2,
+                                    const double image_x,
+                                    const double image_y)
+    : focal_length_x_(focal_length_x),
+      focal_length_y_(focal_length_y),
+      principal_point_x_(principal_point_x),
+      principal_point_y_(principal_point_y),
+      k1_(k1), k2_(k2), k3_(k3), k4_(k4),
+      p1_(p1), p2_(p2),
+      x_(image_x), y_(image_y) {}
+
+  Vec2 operator()(const Vec2 &u) const {
+    double xx, yy;
+
+    ApplyBrownDistortionModel(focal_length_x_,
+                              focal_length_y_,
+                              principal_point_x_,
+                              principal_point_y_,
+                              k1_, k2_, k3_, k4_,
+                              p1_, p2_,
+                              u(0), u(1),
+                              &xx, &yy);
+
+    Vec2 fx;
+    fx << (xx - x_), (yy - y_);
+    return fx;
+  }
+  double focal_length_x_;
+  double focal_length_y_;
+  double principal_point_x_;
+  double principal_point_y_;
+  double k1_, k2_, k3_, k4_;
+  double p1_, p2_;
+  double x_, y_;
+};
+
 }  // namespace
 
 void InvertPolynomialDistortionModel(const double focal_length_x,
@@ -194,6 +244,46 @@ void InvertDivisionDistortionModel(const double focal_length_x,
   *normalized_y = normalized(1);
 }
 
+void InvertBrownDistortionModel(const double focal_length_x,
+                                const double focal_length_y,
+                                const double principal_point_x,
+                                const double principal_point_y,
+                                const double k1,
+                                const double k2,
+                                const double k3,
+                                const double k4,
+                                const double p1,
+                                const double p2,
+                                const double image_x,
+                                const double image_y,
+                                double *normalized_x,
+                                double *normalized_y) {
+  // Compute the initial guess. For a camera with no distortion, this will also
+  // be the final answer; the LM iteration will terminate immediately.
+  Vec2 normalized;
+  normalized(0) = (image_x - principal_point_x) / focal_length_x;
+  normalized(1) = (image_y - principal_point_y) / focal_length_y;
+
+  typedef LevenbergMarquardt<InvertBrownIntrinsicsCostFunction> Solver;
+
+  InvertBrownIntrinsicsCostFunction intrinsics_cost(focal_length_x,
+                                                    focal_length_y,
+                                                    principal_point_x,
+                                                    principal_point_y,
+                                                    k1, k2, k3, k4,
+                                                    p1, p2,
+                                                    image_x, image_y);
+  Solver::SolverParameters params;
+  Solver solver(intrinsics_cost);
+
+  /*Solver::Results results =*/ solver.minimize(params, &normalized);
+
+  // TODO(keir): Better error handling.
+
+  *normalized_x = normalized(0);
+  *normalized_y = normalized(1);
+}
+
 struct ApplyNukeIntrinsicsCostFunction {
  public:
   typedef Vec2 FMatrixType;
diff --git a/intern/libmv/libmv/simple_pipeline/distortion_models.h b/intern/libmv/libmv/simple_pipeline/distortion_models.h
index 6ba351d729d..51300477956 100644
--- a/intern/libmv/libmv/simple_pipeline/distortion_models.h
+++ b/intern/libmv/libmv/simple_pipeline/distortion_models.h
@@ -29,6 +29,7 @@ enum DistortionModelType {
   DISTORTION_MODEL_POLYNOMIAL,
   DISTORTION_MODEL_DIVISION,
   DISTORTION_MODEL_NUKE,
+  DISTORTION_MODEL_BROWN,
 };
 
 // Invert camera intrinsics on the image point to get normalized coordinates.
@@ -202,6 +203,58 @@ void ApplyNukeDistortionModel(const double focal_length_x,
                               double *image_x,
                               double *image_y);
 
-}  // namespace libmv
+// Invert camera intrinsics on the image point to get normalized coordinates.
+// This inverts the radial lens distortion to a point which is in image pixel
+// coordinates to get normalized coordinates.
+void InvertBrownDistortionModel(const double focal_length_x,
+                                const double focal_length_y,
+                                const double principal_point_x,
+                                const double principal_point_y,
+                                const double k1,
+                                const double k2,
+                                const double k3,
+                                const double k4,
+                                const double p1,
+                                const double p2,
+                                const double image_x,
+                                const double image_y,
+                                double *normalized_x,
+                                double *normalized_y);
+
+template <typename T>
+inline void ApplyBrownDistortionModel(const T &focal_length_x,
+                                      const T &focal_length_y,
+                                      const T &principal_point_x,
+                                      const T &principal_point_y,
+                                      const T &k1,
+                                      const T &k2,
+                                      const T &k3,
+                                      const T &k4,
+                                      const T &p1,
+                                      const T &p2,
+                                      const T &normalized_x,
+                                      const T &normalized_y,
+                                      T *image_x,
+                                      T *image_y) {
+  T x = normalized_x;
+  T y = normalized_y;
+
+  // Apply distortion to the normalized points to get (xd, yd).
+  T x2 = x * x;
+  T y2 = y * y;
+  T xy2 = T(2) * x * y;
+  T r2 = x2 + y2;
+  T r_coeff = T(1) + (((k4 * r2 + k3) * r2 + k2) * r2 + k1) * r2;
+  T tx = p1 * (r2 + T(2) * x2) + p2 * xy2;
+  T ty = p2 * (r2 + T(2) * y2) + p1 * xy2;
+  T xd = x * r_coeff + tx;
+  T yd = y * r_coeff + ty;
+
+  // Apply focal length and principal point to get the final image coordinates.
+  *image_x = focal_length_x * xd + principal_point_x;
+  *image_y = focal_length_y * yd + principal_point_y;
+} // namespace libmv
+
+}
 
 #endif  // LIBMV_SIMPLE_PIPELINE_DISTORTION_MODELS_H_