Libmv: Refactor camera intrinsics parameter block

Use the newly introduced packed intrinsics, which allows to remove
code which was initializing parameters block based on distortion
model type.

Now such initialization is done by a particular implementation of
a distortion model.

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

1 files changed, 69 insertions, 206 deletions

diff --git a/intern/libmv/libmv/simple_pipeline/bundle.cc b/intern/libmv/libmv/simple_pipeline/bundle.cc
index d06942c230b..3df39ab8936 100644
--- a/intern/libmv/libmv/simple_pipeline/bundle.cc
+++ b/intern/libmv/libmv/simple_pipeline/bundle.cc
@@ -35,34 +35,10 @@
 #include "libmv/simple_pipeline/reconstruction.h"
 #include "libmv/simple_pipeline/tracks.h"
 #include "libmv/simple_pipeline/distortion_models.h"
+#include "libmv/simple_pipeline/packed_intrinsics.h"
 
 namespace libmv {
 
-// The intrinsics need to get combined into a single parameter block; use these
-// enums to index instead of numeric constants.
-enum {
-  // Camera calibration values.
-  OFFSET_FOCAL_LENGTH,
-  OFFSET_PRINCIPAL_POINT_X,
-  OFFSET_PRINCIPAL_POINT_Y,
-
-  // Distortion model coefficients.
-  OFFSET_K1,
-  OFFSET_K2,
-  OFFSET_K3,
-  OFFSET_K4,
-  OFFSET_P1,
-  OFFSET_P2,
-
-  // Maximal possible offset.
-  OFFSET_MAX,
-};
-
-#define FIRST_DISTORTION_COEFFICIENT OFFSET_K1
-#define LAST_DISTORTION_COEFFICIENT OFFSET_P2
-#define NUM_DISTORTION_COEFFICIENTS  \
-  (LAST_DISTORTION_COEFFICIENT - FIRST_DISTORTION_COEFFICIENT + 1)
-
 namespace {
 
 bool NeedUseInvertIntrinsicsPipeline(const CameraIntrinsics *intrinsics) {
@@ -90,20 +66,23 @@ void ApplyDistortionModelUsingIntrinsicsBlock(
     const T& normalized_x, const T& normalized_y,
     T* distorted_x, T* distorted_y) {
   // Unpack the intrinsics.
-  const T& focal_length      = intrinsics_block[OFFSET_FOCAL_LENGTH];
-  const T& principal_point_x = intrinsics_block[OFFSET_PRINCIPAL_POINT_X];
-  const T& principal_point_y = intrinsics_block[OFFSET_PRINCIPAL_POINT_Y];
+  const T& focal_length =
+      intrinsics_block[PackedIntrinsics::OFFSET_FOCAL_LENGTH];
+  const T& principal_point_x =
+      intrinsics_block[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_X];
+  const T& principal_point_y =
+      intrinsics_block[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_Y];
 
   // TODO(keir): Do early bailouts for zero distortion; these are expensive
   // jet operations.
   switch (invariant_intrinsics->GetDistortionModelType()) {
     case DISTORTION_MODEL_POLYNOMIAL:
       {
-        const T& k1 = intrinsics_block[OFFSET_K1];
-        const T& k2 = intrinsics_block[OFFSET_K2];
-        const T& k3 = intrinsics_block[OFFSET_K3];
-        const T& p1 = intrinsics_block[OFFSET_P1];
-        const T& p2 = intrinsics_block[OFFSET_P2];
+        const T& k1 = intrinsics_block[PackedIntrinsics::OFFSET_K1];
+        const T& k2 = intrinsics_block[PackedIntrinsics::OFFSET_K2];
+        const T& k3 = intrinsics_block[PackedIntrinsics::OFFSET_K3];
+        const T& p1 = intrinsics_block[PackedIntrinsics::OFFSET_P1];
+        const T& p2 = intrinsics_block[PackedIntrinsics::OFFSET_P2];
 
         ApplyPolynomialDistortionModel(focal_length,
                                        focal_length,
@@ -118,8 +97,8 @@ void ApplyDistortionModelUsingIntrinsicsBlock(
 
     case DISTORTION_MODEL_DIVISION:
       {
-        const T& k1 = intrinsics_block[OFFSET_K1];
-        const T& k2 = intrinsics_block[OFFSET_K2];
+        const T& k1 = intrinsics_block[PackedIntrinsics::OFFSET_K1];
+        const T& k2 = intrinsics_block[PackedIntrinsics::OFFSET_K2];
 
         ApplyDivisionDistortionModel(focal_length,
                                      focal_length,
@@ -139,12 +118,12 @@ void ApplyDistortionModelUsingIntrinsicsBlock(
 
     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];
+        const T& k1 = intrinsics_block[PackedIntrinsics::OFFSET_K1];
+        const T& k2 = intrinsics_block[PackedIntrinsics::OFFSET_K2];
+        const T& k3 = intrinsics_block[PackedIntrinsics::OFFSET_K3];
+        const T& k4 = intrinsics_block[PackedIntrinsics::OFFSET_K4];
+        const T& p1 = intrinsics_block[PackedIntrinsics::OFFSET_P1];
+        const T& p2 = intrinsics_block[PackedIntrinsics::OFFSET_P2];
 
         ApplyBrownDistortionModel(focal_length,
                                   focal_length,
@@ -180,9 +159,12 @@ void InvertDistortionModelUsingIntrinsicsBlock(
     const T& image_x, const T& image_y,
     T* normalized_x, T* normalized_y) {
   // Unpack the intrinsics.
-  const T& focal_length      = intrinsics_block[OFFSET_FOCAL_LENGTH];
-  const T& principal_point_x = intrinsics_block[OFFSET_PRINCIPAL_POINT_X];
-  const T& principal_point_y = intrinsics_block[OFFSET_PRINCIPAL_POINT_Y];
+  const T& focal_length =
+      intrinsics_block[PackedIntrinsics::OFFSET_FOCAL_LENGTH];
+  const T& principal_point_x =
+      intrinsics_block[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_X];
+  const T& principal_point_y =
+      intrinsics_block[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_Y];
 
   // TODO(keir): Do early bailouts for zero distortion; these are expensive
   // jet operations.
@@ -195,8 +177,8 @@ void InvertDistortionModelUsingIntrinsicsBlock(
 
     case DISTORTION_MODEL_NUKE:
       {
-        const T& k1 = intrinsics_block[OFFSET_K1];
-        const T& k2 = intrinsics_block[OFFSET_K2];
+        const T& k1 = intrinsics_block[PackedIntrinsics::OFFSET_K1];
+        const T& k2 = intrinsics_block[PackedIntrinsics::OFFSET_K2];
 
         InvertNukeDistortionModel(focal_length,
                                   focal_length,
@@ -219,9 +201,12 @@ void NormalizedToImageSpace(const T* const intrinsics_block,
                             const T& normalized_x, const T& normalized_y,
                             T* image_x, T* image_y) {
   // Unpack the intrinsics.
-  const T& focal_length      = intrinsics_block[OFFSET_FOCAL_LENGTH];
-  const T& principal_point_x = intrinsics_block[OFFSET_PRINCIPAL_POINT_X];
-  const T& principal_point_y = intrinsics_block[OFFSET_PRINCIPAL_POINT_Y];
+  const T& focal_length =
+      intrinsics_block[PackedIntrinsics::OFFSET_FOCAL_LENGTH];
+  const T& principal_point_x =
+      intrinsics_block[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_X];
+  const T& principal_point_y =
+      intrinsics_block[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_Y];
 
   *image_x = normalized_x * focal_length + principal_point_x;
   *image_y = normalized_y * focal_length + principal_point_y;
@@ -310,9 +295,12 @@ struct ReprojectionErrorInvertIntrinsics {
                   const T* const X,    // Point coordinates 3x1.
                   T* residuals) const {
     // Unpack the intrinsics.
-    const T& focal_length      = intrinsics[OFFSET_FOCAL_LENGTH];
-    const T& principal_point_x = intrinsics[OFFSET_PRINCIPAL_POINT_X];
-    const T& principal_point_y = intrinsics[OFFSET_PRINCIPAL_POINT_Y];
+    const T& focal_length =
+        intrinsics[PackedIntrinsics::OFFSET_FOCAL_LENGTH];
+    const T& principal_point_x =
+        intrinsics[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_X];
+    const T& principal_point_y =
+        intrinsics[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_Y];
 
     // Compute projective coordinates: x = RX + t.
     T x[3];
@@ -387,139 +375,6 @@ void BundleIntrinsicsLogMessage(const int bundle_intrinsics) {
   }
 }
 
-// Pack intrinsics from object to an array for easier
-// and faster minimization.
-void PackIntrinisicsIntoArray(const CameraIntrinsics &intrinsics,
-                              double intrinsics_block[OFFSET_MAX]) {
-  // Pack common intrinsics part.
-  intrinsics_block[OFFSET_FOCAL_LENGTH]       = intrinsics.focal_length();
-  intrinsics_block[OFFSET_PRINCIPAL_POINT_X]  = intrinsics.principal_point_x();
-  intrinsics_block[OFFSET_PRINCIPAL_POINT_Y]  = intrinsics.principal_point_y();
-
-  // Per-model intrinsics block.
-  //
-  // The goal here is to get named parameters from the intrinsics object and
-  // place them into well-defined position within the intrinsics block. This
-  // simplifies logic of marking parameters constant.
-  //
-  // TODO(sergey): The code is very much similar to what is goping on in the
-  // cost functors. With some templates and helper functions it will be
-  // possible to reduce level of duplication.
-  switch (intrinsics.GetDistortionModelType()) {
-    case DISTORTION_MODEL_POLYNOMIAL:
-      {
-        const PolynomialCameraIntrinsics& polynomial_intrinsics =
-            static_cast<const PolynomialCameraIntrinsics&>(intrinsics);
-        intrinsics_block[OFFSET_K1] = polynomial_intrinsics.k1();
-        intrinsics_block[OFFSET_K2] = polynomial_intrinsics.k2();
-        intrinsics_block[OFFSET_K3] = polynomial_intrinsics.k3();
-        intrinsics_block[OFFSET_P1] = polynomial_intrinsics.p1();
-        intrinsics_block[OFFSET_P2] = polynomial_intrinsics.p2();
-        return;
-      }
-
-    case DISTORTION_MODEL_DIVISION:
-      {
-        const DivisionCameraIntrinsics& division_intrinsics =
-            static_cast<const DivisionCameraIntrinsics&>(intrinsics);
-        intrinsics_block[OFFSET_K1] = division_intrinsics.k1();
-        intrinsics_block[OFFSET_K2] = division_intrinsics.k2();
-        return;
-      }
-
-    case DISTORTION_MODEL_NUKE:
-      {
-        const NukeCameraIntrinsics& nuke_intrinsics =
-            static_cast<const NukeCameraIntrinsics&>(intrinsics);
-        intrinsics_block[OFFSET_K1] = nuke_intrinsics.k1();
-        intrinsics_block[OFFSET_K2] = nuke_intrinsics.k2();
-        return;
-      }
-
-    case DISTORTION_MODEL_BROWN:
-      {
-        const BrownCameraIntrinsics& brown_intrinsics =
-            static_cast<const BrownCameraIntrinsics&>(intrinsics);
-        intrinsics_block[OFFSET_K1] = brown_intrinsics.k1();
-        intrinsics_block[OFFSET_K2] = brown_intrinsics.k2();
-        intrinsics_block[OFFSET_K3] = brown_intrinsics.k3();
-        intrinsics_block[OFFSET_K4] = brown_intrinsics.k4();
-        intrinsics_block[OFFSET_P1] = brown_intrinsics.p1();
-        intrinsics_block[OFFSET_P2] = brown_intrinsics.p2();
-        return;
-      }
-  }
-
-  LOG(FATAL) << "Unknown distortion model.";
-}
-
-// Unpack intrinsics back from an array to an object.
-void UnpackIntrinsicsFromArray(const double intrinsics_block[OFFSET_MAX],
-                               CameraIntrinsics *intrinsics) {
-  // Unpack common intrinsics part.
-  intrinsics->SetFocalLength(intrinsics_block[OFFSET_FOCAL_LENGTH],
-                             intrinsics_block[OFFSET_FOCAL_LENGTH]);
-
-  intrinsics->SetPrincipalPoint(intrinsics_block[OFFSET_PRINCIPAL_POINT_X],
-                                intrinsics_block[OFFSET_PRINCIPAL_POINT_Y]);
-
-  // Per-model intrinsics block.
-  //
-  // The goal here is to get named parameters from the intrinsics object and
-  // place them into well-defined position within the intrinsics block. This
-  // simplifies logic of marking parameters constant.
-  //
-  // TODO(sergey): The code is very much similar to what is goping on in the
-  // cost functors. With some templates and helper functions it will be
-  // possible to reduce level of duplication.
-  switch (intrinsics->GetDistortionModelType()) {
-    case DISTORTION_MODEL_POLYNOMIAL:
-      {
-        PolynomialCameraIntrinsics* polynomial_intrinsics =
-            static_cast<PolynomialCameraIntrinsics*>(intrinsics);
-        polynomial_intrinsics->SetRadialDistortion(intrinsics_block[OFFSET_K1],
-                                                   intrinsics_block[OFFSET_K2],
-                                                   intrinsics_block[OFFSET_K3]);
-        polynomial_intrinsics->SetTangentialDistortion(
-            intrinsics_block[OFFSET_P1], intrinsics_block[OFFSET_P2]);
-        return;
-      }
-
-    case DISTORTION_MODEL_DIVISION:
-      {
-        DivisionCameraIntrinsics* division_intrinsics =
-            static_cast<DivisionCameraIntrinsics*>(intrinsics);
-        division_intrinsics->SetDistortion(intrinsics_block[OFFSET_K1],
-                                           intrinsics_block[OFFSET_K2]);
-        return;
-      }
-
-    case DISTORTION_MODEL_NUKE:
-      {
-        NukeCameraIntrinsics* nuke_intrinsics =
-            static_cast<NukeCameraIntrinsics*>(intrinsics);
-        nuke_intrinsics->SetDistortion(intrinsics_block[OFFSET_K1],
-                                       intrinsics_block[OFFSET_K2]);
-        return;
-      }
-
-    case DISTORTION_MODEL_BROWN:
-      {
-        BrownCameraIntrinsics* brown_intrinsics =
-            static_cast<BrownCameraIntrinsics*>(intrinsics);
-        brown_intrinsics->SetRadialDistortion(intrinsics_block[OFFSET_K1],
-                                              intrinsics_block[OFFSET_K2],
-                                              intrinsics_block[OFFSET_K3],
-                                              intrinsics_block[OFFSET_K4]);
-        brown_intrinsics->SetTangentialDistortion(intrinsics_block[OFFSET_P1],
-                                                  intrinsics_block[OFFSET_P2]);
-        return;
-      }
-  }
-
-  LOG(FATAL) << "Unknown distortion model.";
-}
-
 // Get a vector of camera's rotations denoted by angle axis
 // conjuncted with translations into single block
 //
@@ -656,12 +511,12 @@ template<typename CostFunction>
 void AddResidualBlockToProblemImpl(const CameraIntrinsics *invariant_intrinsics,
                                    double observed_x, double observed_y,
                                    double weight,
-                                   double intrinsics_block[OFFSET_MAX],
+                                   double *intrinsics_block,
                                    double *camera_R_t,
                                    EuclideanPoint *point,
                                    ceres::Problem* problem) {
   problem->AddResidualBlock(new ceres::AutoDiffCostFunction<
-      CostFunction, 2, OFFSET_MAX, 6, 3>(
+      CostFunction, 2, PackedIntrinsics::NUM_PARAMETERS, 6, 3>(
           new CostFunction(
               invariant_intrinsics,
               observed_x, observed_y,
@@ -675,7 +530,7 @@ void AddResidualBlockToProblemImpl(const CameraIntrinsics *invariant_intrinsics,
 void AddResidualBlockToProblem(const CameraIntrinsics *invariant_intrinsics,
                                const Marker &marker,
                                double marker_weight,
-                               double intrinsics_block[OFFSET_MAX],
+                               double* intrinsics_block,
                                double *camera_R_t,
                                EuclideanPoint *point,
                                ceres::Problem* problem) {
@@ -712,7 +567,7 @@ void AddResidualBlockToProblem(const CameraIntrinsics *invariant_intrinsics,
 void EuclideanBundlePointsOnly(const CameraIntrinsics *invariant_intrinsics,
                                const vector<Marker> &markers,
                                map<int, Vec6> &all_cameras_R_t,
-                               double intrinsics_block[OFFSET_MAX],
+                               double* intrinsics_block,
                                EuclideanReconstruction *reconstruction) {
   ceres::Problem::Options problem_options;
   ceres::Problem problem(problem_options);
@@ -792,11 +647,11 @@ void EuclideanBundleCommonIntrinsics(
   // N-th element denotes whether track N is a constant zero-weighted track.
   vector<bool> zero_weight_tracks_flags(tracks.MaxTrack() + 1, true);
 
-  // Residual blocks with 10 parameters are unwieldly with Ceres, so pack the
-  // intrinsics into a single block and rely on local parameterizations to
-  // control which intrinsics are allowed to vary.
-  double intrinsics_block[OFFSET_MAX];
-  PackIntrinisicsIntoArray(*intrinsics, intrinsics_block);
+  // Pack all intrinsics parameters into a single block and rely on local
+  // parameterizations to control which intrinsics are allowed to vary.
+  PackedIntrinsics packed_intrinsics;
+  intrinsics->Pack(&packed_intrinsics);
+  double* intrinsics_block = packed_intrinsics.GetParametersBlock();
 
   // Convert cameras rotations to angle axis and merge with translation
   // into single parameter block for maximal minimization speed.
@@ -892,21 +747,28 @@ void EuclideanBundleCommonIntrinsics(
     if (!(bundle_intrinsics & bundle_enum)) { \
       constant_intrinsics.push_back(offset); \
     }
-    MAYBE_SET_CONSTANT(BUNDLE_FOCAL_LENGTH,    OFFSET_FOCAL_LENGTH);
-    MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_X);
-    MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_Y);
-    MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K1,       OFFSET_K1);
-    MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K2,       OFFSET_K2);
-    MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P1,   OFFSET_P1);
-    MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P2,   OFFSET_P2);
+    MAYBE_SET_CONSTANT(BUNDLE_FOCAL_LENGTH,
+                       PackedIntrinsics::OFFSET_FOCAL_LENGTH);
+    MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT,
+                       PackedIntrinsics::OFFSET_PRINCIPAL_POINT_X);
+    MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT,
+                       PackedIntrinsics::OFFSET_PRINCIPAL_POINT_Y);
+    MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K1,       PackedIntrinsics::OFFSET_K1);
+    MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K2,       PackedIntrinsics::OFFSET_K2);
+    MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P1,   PackedIntrinsics::OFFSET_P1);
+    MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P2,   PackedIntrinsics::OFFSET_P2);
 #undef MAYBE_SET_CONSTANT
 
     // 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);
+    constant_intrinsics.push_back(PackedIntrinsics::OFFSET_K3);
+    constant_intrinsics.push_back(PackedIntrinsics::OFFSET_K4);
+
+   // TODO(sergey): Mark all parameters which are not used by the distortion
+   // model as constant.
 
     ceres::SubsetParameterization *subset_parameterization =
-      new ceres::SubsetParameterization(OFFSET_MAX, constant_intrinsics);
+      new ceres::SubsetParameterization(PackedIntrinsics::NUM_PARAMETERS,
+                                        constant_intrinsics);
 
     problem.SetParameterization(intrinsics_block, subset_parameterization);
   }
@@ -931,8 +793,9 @@ void EuclideanBundleCommonIntrinsics(
   UnpackCamerasRotationAndTranslation(all_cameras_R_t, reconstruction);
 
   // Copy intrinsics back.
-  if (bundle_intrinsics != BUNDLE_NO_INTRINSICS)
-    UnpackIntrinsicsFromArray(intrinsics_block, intrinsics);
+  if (bundle_intrinsics != BUNDLE_NO_INTRINSICS) {
+    intrinsics->Unpack(packed_intrinsics);
+  }
 
   LG << "Final intrinsics: " << *intrinsics;