Libmv: Fix wrong packing order of intrinsics for BA step

The order got broken when Brown distortion model has been added.
Made it so the indexing of parameters is strictly defined in the
parameter block, matching how parameters are used in the cost
function.

There is some duplication going on accessing parameters. This can
be refactored in the future, by either moving common parts packing
and cost function to an utility function in bundle.cc.
Alternatively, can introduce a public PackedIntrinsics class which
will contain a continuous block of parameters, and each of the
camera models will have API to be initialized from packed form and
to create this packed form.

The benefit of this approach over alternative solutions previously
made in the master branch or suggested in D9116 is that the specific
implementation of BA does not dictate the way how public classes need
to be organized. It is API which needs to define how implementation
goes, not the other way around.

Thanks Bastien and Ivan for the investigation!

1 files changed, 108 insertions, 12 deletions

diff --git a/intern/libmv/libmv/simple_pipeline/bundle.cc b/intern/libmv/libmv/simple_pipeline/bundle.cc
index c055846318a..ffe85c9d0c2 100644
--- a/intern/libmv/libmv/simple_pipeline/bundle.cc
+++ b/intern/libmv/libmv/simple_pipeline/bundle.cc
@@ -391,37 +391,133 @@ void BundleIntrinsicsLogMessage(const int bundle_intrinsics) {
 // 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();
 
-  int num_distortion_parameters = intrinsics.num_distortion_parameters();
-  assert(num_distortion_parameters <= NUM_DISTORTION_COEFFICIENTS);
+  // 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;
+      }
 
-  const double *distortion_parameters = intrinsics.distortion_parameters();
-  for (int i = 0; i < num_distortion_parameters; ++i) {
-    intrinsics_block[FIRST_DISTORTION_COEFFICIENT + i] =
-        distortion_parameters[i];
+    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]);
 
-  int num_distortion_parameters = intrinsics->num_distortion_parameters();
-  assert(num_distortion_parameters <= NUM_DISTORTION_COEFFICIENTS);
+  // 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;
+      }
 
-  double *distortion_parameters = intrinsics->distortion_parameters();
-  for (int i = 0; i < num_distortion_parameters; ++i) {
-    distortion_parameters[i] =
-        intrinsics_block[FIRST_DISTORTION_COEFFICIENT + i];
+    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