Libmv: Add generic class for packed intrinsics

This is a common class which can be used in all sort of minimization
problems which needs camera intrinsics as a parameter block.

Currently unused, but will replace a lot of hard-coded logic in the
bundle adjustment code.

5 files changed, 275 insertions, 0 deletions

diff --git a/intern/libmv/CMakeLists.txt b/intern/libmv/CMakeLists.txt
index 3e2ca8329a4..b372cfe92bb 100644
--- a/intern/libmv/CMakeLists.txt
+++ b/intern/libmv/CMakeLists.txt
@@ -111,6 +111,7 @@ if(WITH_LIBMV)
     libmv/simple_pipeline/intersect.cc
     libmv/simple_pipeline/keyframe_selection.cc
     libmv/simple_pipeline/modal_solver.cc
+    libmv/simple_pipeline/packed_intrinsics.cc
     libmv/simple_pipeline/pipeline.cc
     libmv/simple_pipeline/reconstruction.cc
     libmv/simple_pipeline/reconstruction_scale.cc
@@ -192,6 +193,7 @@ if(WITH_LIBMV)
     libmv/simple_pipeline/intersect.h
     libmv/simple_pipeline/keyframe_selection.h
     libmv/simple_pipeline/modal_solver.h
+    libmv/simple_pipeline/packed_intrinsics.h
     libmv/simple_pipeline/pipeline.h
     libmv/simple_pipeline/reconstruction.h
     libmv/simple_pipeline/reconstruction_scale.h
diff --git a/intern/libmv/libmv/simple_pipeline/camera_intrinsics.cc b/intern/libmv/libmv/simple_pipeline/camera_intrinsics.cc
index 052714bbb3e..ccb6e3d34c8 100644
--- a/intern/libmv/libmv/simple_pipeline/camera_intrinsics.cc
+++ b/intern/libmv/libmv/simple_pipeline/camera_intrinsics.cc
@@ -22,6 +22,7 @@
 
 #include "libmv/logging/logging.h"
 #include "libmv/simple_pipeline/distortion_models.h"
+#include "libmv/simple_pipeline/packed_intrinsics.h"
 
 namespace libmv {
 
@@ -131,6 +132,20 @@ void CameraIntrinsics::ResetLookupGrids() {
   undistort_.Reset();
 }
 
+void CameraIntrinsics::Pack(PackedIntrinsics* packed_intrinsics) const {
+  packed_intrinsics->SetFocalLength(focal_length());
+  packed_intrinsics->SetPrincipalPoint(principal_point_x(),
+                                       principal_point_y());
+}
+
+void CameraIntrinsics::Unpack(const PackedIntrinsics& packed_intrinsics) {
+  SetFocalLength(packed_intrinsics.GetFocalLength(),
+                 packed_intrinsics.GetFocalLength());
+
+  SetPrincipalPoint(packed_intrinsics.GetPrincipalPointX(),
+                    packed_intrinsics.GetPrincipalPointY());
+}
+
 // Polynomial model.
 
 PolynomialCameraIntrinsics::PolynomialCameraIntrinsics()
@@ -195,6 +210,30 @@ void PolynomialCameraIntrinsics::InvertIntrinsics(
                                   normalized_y);
 }
 
+void PolynomialCameraIntrinsics::Pack(
+    PackedIntrinsics* packed_intrinsics) const {
+  CameraIntrinsics::Pack(packed_intrinsics);
+
+  packed_intrinsics->SetK1(k1());
+  packed_intrinsics->SetK2(k2());
+  packed_intrinsics->SetK3(k3());
+
+  packed_intrinsics->SetP1(p1());
+  packed_intrinsics->SetP2(p2());
+}
+
+void PolynomialCameraIntrinsics::Unpack(
+    const PackedIntrinsics& packed_intrinsics) {
+  CameraIntrinsics::Unpack(packed_intrinsics);
+
+  SetRadialDistortion(packed_intrinsics.GetK1(),
+                      packed_intrinsics.GetK2(),
+                      packed_intrinsics.GetK3());
+
+  SetTangentialDistortion(packed_intrinsics.GetP1(),
+                          packed_intrinsics.GetP2());
+}
+
 // Division model.
 
 DivisionCameraIntrinsics::DivisionCameraIntrinsics()
@@ -245,6 +284,21 @@ void DivisionCameraIntrinsics::InvertIntrinsics(double image_x,
                                 normalized_y);
 }
 
+void DivisionCameraIntrinsics::Pack(
+    PackedIntrinsics* packed_intrinsics) const {
+  CameraIntrinsics::Pack(packed_intrinsics);
+
+  packed_intrinsics->SetK1(k1());
+  packed_intrinsics->SetK2(k2());
+}
+
+void DivisionCameraIntrinsics::Unpack(
+    const PackedIntrinsics& packed_intrinsics) {
+  CameraIntrinsics::Unpack(packed_intrinsics);
+
+  SetDistortion(packed_intrinsics.GetK1(), packed_intrinsics.GetK2());
+}
+
 // Nuke model.
 
 NukeCameraIntrinsics::NukeCameraIntrinsics()
@@ -296,6 +350,21 @@ void NukeCameraIntrinsics::InvertIntrinsics(double image_x,
                             normalized_y);
 }
 
+void NukeCameraIntrinsics::Pack(
+    PackedIntrinsics* packed_intrinsics) const {
+  CameraIntrinsics::Pack(packed_intrinsics);
+
+  packed_intrinsics->SetK1(k1());
+  packed_intrinsics->SetK2(k2());
+}
+
+void NukeCameraIntrinsics::Unpack(
+    const PackedIntrinsics& packed_intrinsics) {
+  CameraIntrinsics::Unpack(packed_intrinsics);
+
+  SetDistortion(packed_intrinsics.GetK1(), packed_intrinsics.GetK2());
+}
+
 // Brown model.
 
 BrownCameraIntrinsics::BrownCameraIntrinsics()
@@ -362,6 +431,32 @@ void BrownCameraIntrinsics::InvertIntrinsics(
                              normalized_y);
 }
 
+void BrownCameraIntrinsics::Pack(
+    PackedIntrinsics* packed_intrinsics) const {
+  CameraIntrinsics::Pack(packed_intrinsics);
+
+  packed_intrinsics->SetK1(k1());
+  packed_intrinsics->SetK2(k2());
+  packed_intrinsics->SetK3(k3());
+  packed_intrinsics->SetK4(k4());
+
+  packed_intrinsics->SetP1(p1());
+  packed_intrinsics->SetP2(p2());
+}
+
+void BrownCameraIntrinsics::Unpack(
+    const PackedIntrinsics& packed_intrinsics) {
+  CameraIntrinsics::Unpack(packed_intrinsics);
+
+  SetRadialDistortion(packed_intrinsics.GetK1(),
+                      packed_intrinsics.GetK2(),
+                      packed_intrinsics.GetK3(),
+                      packed_intrinsics.GetK4());
+
+  SetTangentialDistortion(packed_intrinsics.GetP1(),
+                          packed_intrinsics.GetP2());
+}
+
 std::ostream& operator <<(std::ostream &os,
                           const CameraIntrinsics &intrinsics) {
   if (intrinsics.focal_length_x() == intrinsics.focal_length_x()) {
diff --git a/intern/libmv/libmv/simple_pipeline/camera_intrinsics.h b/intern/libmv/libmv/simple_pipeline/camera_intrinsics.h
index 90bbc592815..30b0f1abf7b 100644
--- a/intern/libmv/libmv/simple_pipeline/camera_intrinsics.h
+++ b/intern/libmv/libmv/simple_pipeline/camera_intrinsics.h
@@ -32,6 +32,7 @@
 namespace libmv {
 
 class CameraIntrinsics;
+class PackedIntrinsics;
 
 namespace internal {
 
@@ -193,6 +194,9 @@ class CameraIntrinsics {
                                 double *normalized_x,
                                 double *normalized_y) const = 0;
 
+  virtual void Pack(PackedIntrinsics* packed_intrinsics) const;
+  virtual void Unpack(const PackedIntrinsics& packed_intrinsics);
+
   // Distort an image using the current camera instrinsics
   //
   // The distorted image is computed in output_buffer using samples from
@@ -323,6 +327,9 @@ class PolynomialCameraIntrinsics : public CameraIntrinsics {
                         double *normalized_x,
                         double *normalized_y) const;
 
+  virtual void Pack(PackedIntrinsics* packed_intrinsics) const override;
+  virtual void Unpack(const PackedIntrinsics& packed_intrinsics) override;
+
  private:
   // OpenCV's distortion model with third order polynomial radial distortion
   // terms and second order tangential distortion. The distortion is applied to
@@ -376,6 +383,9 @@ class DivisionCameraIntrinsics : public CameraIntrinsics {
                         double *normalized_x,
                         double *normalized_y) const;
 
+  virtual void Pack(PackedIntrinsics* packed_intrinsics) const override;
+  virtual void Unpack(const PackedIntrinsics& packed_intrinsics) override;
+
  private:
   // Double-parameter division distortion model.
   double parameters_[NUM_PARAMETERS];
@@ -426,6 +436,9 @@ class NukeCameraIntrinsics : public CameraIntrinsics {
                         double *normalized_x,
                         double *normalized_y) const;
 
+  virtual void Pack(PackedIntrinsics* packed_intrinsics) const override;
+  virtual void Unpack(const PackedIntrinsics& packed_intrinsics) override;
+
  private:
   // Double-parameter division distortion model.
   double parameters_[NUM_PARAMETERS];
@@ -487,6 +500,9 @@ class BrownCameraIntrinsics : public CameraIntrinsics {
                         double *normalized_x,
                         double *normalized_y) const;
 
+  virtual void Pack(PackedIntrinsics* packed_intrinsics) const override;
+  virtual void Unpack(const PackedIntrinsics& packed_intrinsics) override;
+
  private:
   double parameters_[NUM_PARAMETERS];
 };
diff --git a/intern/libmv/libmv/simple_pipeline/packed_intrinsics.cc b/intern/libmv/libmv/simple_pipeline/packed_intrinsics.cc
new file mode 100644
index 00000000000..e9d7c2e78be
--- /dev/null
+++ b/intern/libmv/libmv/simple_pipeline/packed_intrinsics.cc
@@ -0,0 +1,58 @@
+// Copyright (c) 2020 libmv authors.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+
+#include "libmv/simple_pipeline/packed_intrinsics.h"
+
+namespace libmv {
+
+PackedIntrinsics::PackedIntrinsics() {
+  parameters_.fill(0.0);
+  known_parameters_.fill(false);
+}
+
+void PackedIntrinsics::SetFocalLength(double focal_length) {
+  SetParameter(OFFSET_FOCAL_LENGTH, focal_length);
+}
+double PackedIntrinsics::GetFocalLength() const {
+  return GetParameter(OFFSET_FOCAL_LENGTH);
+}
+
+void PackedIntrinsics::SetPrincipalPoint(double x, double y) {
+  SetParameter(OFFSET_PRINCIPAL_POINT_X, x);
+  SetParameter(OFFSET_PRINCIPAL_POINT_Y, y);
+}
+double PackedIntrinsics::GetPrincipalPointX() const {
+  return GetParameter(OFFSET_PRINCIPAL_POINT_X);
+}
+double PackedIntrinsics::GetPrincipalPointY() const {
+  return GetParameter(OFFSET_PRINCIPAL_POINT_Y);
+}
+
+void PackedIntrinsics::SetParameter(int index, double value) {
+  parameters_.at(index) = value;
+  known_parameters_.at(index) = value;
+}
+double PackedIntrinsics::GetParameter(int index) const {
+  // TODO(sergey): Consider adding a check for whether the parameter is known.
+
+  return parameters_.at(index);
+}
+
+}  // namespace libmv
diff --git a/intern/libmv/libmv/simple_pipeline/packed_intrinsics.h b/intern/libmv/libmv/simple_pipeline/packed_intrinsics.h
new file mode 100644
index 00000000000..e551fe74800
--- /dev/null
+++ b/intern/libmv/libmv/simple_pipeline/packed_intrinsics.h
@@ -0,0 +1,104 @@
+// Copyright (c) 2020 libmv authors.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+
+#ifndef LIBMV_SIMPLE_PIPELINE_PACKED_INTRINSICS_H_
+#define LIBMV_SIMPLE_PIPELINE_PACKED_INTRINSICS_H_
+
+#include "libmv/base/array.h"
+
+namespace libmv {
+
+// Intrinsics parameters packed into a single continuous block of memory.
+// Used in cases like minimization problems which involves camera intrinsics
+// as a minimizing parameters.
+//
+// It keeps track of which parameters has been specified explicitly, which
+// allows to mark parameters which are not used by distortion model as constant,
+// which improves minimization quality.
+class PackedIntrinsics {
+ public:
+  // Offsets of corresponding parameters in the array of all parameters.
+  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,
+  
+    // Number of parameters which are to be stored in the block.
+    NUM_PARAMETERS,
+  };
+
+  PackedIntrinsics();
+
+  void SetFocalLength(double focal_length);
+  double GetFocalLength() const;
+
+  void SetPrincipalPoint(double x, double y);
+  double GetPrincipalPointX() const;
+  double GetPrincipalPointY() const;
+
+  // TODO(sergey): Consider adding vectorized (Vec2) accessors for the principal
+  // point.
+
+#define DEFINE_PARAMETER(parameter_name)                                       \
+  void Set ## parameter_name(double value) {                                   \
+    SetParameter(OFFSET_ ## parameter_name, value);                            \
+  }                                                                            \
+  double Get ## parameter_name() const {                                       \
+    return GetParameter(OFFSET_ ## parameter_name);                            \
+  }                                                                            \
+
+  DEFINE_PARAMETER(K1)
+  DEFINE_PARAMETER(K2)
+  DEFINE_PARAMETER(K3)
+  DEFINE_PARAMETER(K4)
+
+  DEFINE_PARAMETER(P1)
+  DEFINE_PARAMETER(P2)
+
+#undef DEFINE_PARAMETER
+
+  double* GetParametersBlock() { return parameters_.data(); }
+  const double* GetParametersBlock() const { return parameters_.data(); }
+
+ private:
+  void SetParameter(int index, double value);
+  double GetParameter(int index) const;
+
+  // All intrinsics parameters packed into a single block.
+  // Use OFFSET_FOO indexes to access corresponding values.
+  array<double,  NUM_PARAMETERS> parameters_;
+
+  // Indexed by parameter offset, set to truth if the value of the parameter is
+  // explicitly specified.
+  array<bool,  NUM_PARAMETERS> known_parameters_;
+};
+
+}  // namespace libmv
+
+#endif  // LIBMV_SIMPLE_PIPELINE_PACKED_INTRINSICS_H_