libmv: bundle new upstream version from own branch with rigid registration implementation

Currently not used in blender code but is needed for some current work.

9 files changed, 358 insertions, 18 deletions

diff --git a/extern/libmv/CMakeLists.txt b/extern/libmv/CMakeLists.txt
index 51f254264b6..02723b64b62 100644
--- a/extern/libmv/CMakeLists.txt
+++ b/extern/libmv/CMakeLists.txt
@@ -72,6 +72,7 @@ set(SRC
 	libmv/simple_pipeline/pipeline.cc
 	libmv/simple_pipeline/reconstruction.cc
 	libmv/simple_pipeline/resect.cc
+	libmv/simple_pipeline/rigid_registration.cc
 	libmv/simple_pipeline/tracks.cc
 	libmv/tracking/brute_region_tracker.cc
 	libmv/tracking/esm_region_tracker.cc
@@ -128,6 +129,7 @@ set(SRC
 	libmv/simple_pipeline/pipeline.h
 	libmv/simple_pipeline/reconstruction.h
 	libmv/simple_pipeline/resect.h
+	libmv/simple_pipeline/rigid_registration.h
 	libmv/simple_pipeline/tracks.h
 	libmv/tracking/brute_region_tracker.h
 	libmv/tracking/esm_region_tracker.h
diff --git a/extern/libmv/ChangeLog b/extern/libmv/ChangeLog
index ac5a404bafa..33068bddf90 100644
--- a/extern/libmv/ChangeLog
+++ b/extern/libmv/ChangeLog
@@ -1,3 +1,36 @@
+commit fa3842e472e3b9c789e47bf6d8f592aa40a84f16
+Author: Sergey Sharybin <sergey.vfx@gmail.com>
+Date:   Thu Apr 12 12:32:48 2012 +0600
+
+    implementation of some basic algorithms for point cloud orientation:
+    
+    - Implementation of rigid registration algorithm which searches transformation
+      form one point cloud to another assuming that points in this clouds are
+      already paired (points with the same index in different clouds belongs to
+      the same pair) which minimizes average distance between points in pairs.
+    
+      Algorithm uses Levenberg-Marquardt solver to find such transformation.
+    
+      Supports registration of rotation-scale-transform (which is probably most
+      common usage) and rotation only (which might be useful for basic modal
+      tripod solver).
+    
+    - Implementation of Iterative-Point-Clouds algorithm which searches
+      transformation from one arbitrary point cloud to another  making
+      points as closest to each other as possible.
+    
+      This algorithm doesn't require points be initially paired, but for
+      good result clouds should have rough initial orientation. If they're
+      arbitrary oriented from the very beginning, algorithm might fail
+      producing good resold.
+    
+      Iteration is based on building pairs of closest to each other points
+      and registering rigid transformation between them which incrementally
+      constructs final result.
+    
+    TODO: building pairs might be speedup a lot using data structures like
+          AABB trees, K-D trees or so.
+
 commit 9618d9a1d48bb3c28da605d9027f57a74f462785
 Author: Sergey Sharybin <sergey.vfx@gmail.com>
 Date:   Wed Apr 11 14:17:14 2012 +0600
@@ -493,11 +526,3 @@ Author: Matthias Fauconneau <matthias.fauconneau@gmail.com>
 Date:   Fri Aug 19 14:59:24 2011 +0200
 
     Expose regularization parameters (areaPenalty and conditionPenalty) in API.
-
-commit 3e84ae5fbac10451d4935418f6281a90cedace11
-Author: Matthias Fauconneau <matthias.fauconneau@gmail.com>
-Date:   Fri Aug 19 14:19:27 2011 +0200
-
-    Add LaplaceFilter.
-    Add regularization in affine SAD Tracker (keep constant area and good condition number).
-    UI: Better track display (+enable line antialiasing).
diff --git a/extern/libmv/files.txt b/extern/libmv/files.txt
index 426a573de4e..1e564d3a2f2 100644
--- a/extern/libmv/files.txt
+++ b/extern/libmv/files.txt
@@ -48,6 +48,8 @@ libmv/simple_pipeline/reconstruction.cc
 libmv/simple_pipeline/reconstruction.h
 libmv/simple_pipeline/resect.cc
 libmv/simple_pipeline/resect.h
+libmv/simple_pipeline/rigid_registration.cc
+libmv/simple_pipeline/rigid_registration.h
 libmv/simple_pipeline/tracks.cc
 libmv/simple_pipeline/tracks.h
 libmv/tracking/brute_region_tracker.cc
diff --git a/extern/libmv/libmv-capi.cpp b/extern/libmv/libmv-capi.cpp
index 7fd4bfdd9d1..e4708e5907d 100644
--- a/extern/libmv/libmv-capi.cpp
+++ b/extern/libmv/libmv-capi.cpp
@@ -36,6 +36,8 @@
 
 #include "Math/v3d_optimization.h"
 
+#include "libmv/numeric/numeric.h"
+
 #include "libmv/tracking/esm_region_tracker.h"
 #include "libmv/tracking/brute_region_tracker.h"
 #include "libmv/tracking/hybrid_region_tracker.h"
@@ -51,6 +53,7 @@
 #include "libmv/simple_pipeline/detect.h"
 #include "libmv/simple_pipeline/pipeline.h"
 #include "libmv/simple_pipeline/camera_intrinsics.h"
+#include "libmv/simple_pipeline/rigid_registration.h"
 
 #include <stdlib.h>
 #include <assert.h>
@@ -838,3 +841,56 @@ void libmv_InvertIntrinsics(double focal_length, double principal_x, double prin
 		intrinsics.InvertIntrinsics(x, y, x1, y1);
 	}
 }
+
+/* ************ point clouds ************ */
+
+void libmvTransformToMat4(libmv::Mat3 &R, libmv::Vec3 &S, libmv::Vec3 &t, double M[4][4])
+{
+	for (int j = 0; j < 3; ++j)
+		for (int k = 0; k < 3; ++k)
+			M[j][k] = R(k, j) * S(j);
+
+	for (int i = 0; i < 3; ++i) {
+		M[3][0] = t(0);
+		M[3][1] = t(1);
+		M[3][2] = t(2);
+
+		M[0][3] = M[1][3] = M[2][3] = 0;
+	}
+
+	M[3][3] = 1.0;
+}
+
+void libmv_rigidRegistration(float (*reference_points)[3], float (*points)[3], int total_points,
+                             int use_scale, int use_translation, double M[4][4])
+{
+	libmv::Mat3 R;
+	libmv::Vec3 S;
+	libmv::Vec3 t;
+	libmv::vector<libmv::Vec3> reference_points_vector, points_vector;
+
+	for (int i = 0; i < total_points; i++) {
+		reference_points_vector.push_back(libmv::Vec3(reference_points[i][0],
+		                                              reference_points[i][1],
+		                                              reference_points[i][2]));
+
+		points_vector.push_back(libmv::Vec3(points[i][0],
+		                                    points[i][1],
+		                                    points[i][2]));
+	}
+
+	if (use_scale && use_translation) {
+		libmv::RigidRegistration(reference_points_vector, points_vector, R, S, t);
+	}
+	else if (use_translation) {
+		S = libmv::Vec3(1.0, 1.0, 1.0);
+		libmv::RigidRegistration(reference_points_vector, points_vector, R, t);
+	}
+	else {
+		S = libmv::Vec3(1.0, 1.0, 1.0);
+		t = libmv::Vec3::Zero();
+		libmv::RigidRegistration(reference_points_vector, points_vector, R);
+	}
+
+	libmvTransformToMat4(R, S, t, M);
+}
diff --git a/extern/libmv/libmv-capi.h b/extern/libmv/libmv-capi.h
index e5728188afb..01019832374 100644
--- a/extern/libmv/libmv-capi.h
+++ b/extern/libmv/libmv-capi.h
@@ -129,6 +129,10 @@ void libmv_applyCameraIntrinsics(double focal_length, double principal_x, double
 void libmv_InvertIntrinsics(double focal_length, double principal_x, double principal_y, double k1, double k2, double k3,
 			double x, double y, double *x1, double *y1);
 
+/* point clouds */
+void libmv_rigidRegistration(float (*reference_points)[3], float (*points)[3], int total_points,
+                             int use_scale, int use_translation, double M[4][4]);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/extern/libmv/libmv/numeric/numeric.h b/extern/libmv/libmv/numeric/numeric.h
index bb7f9b364ef..ad707fb76f2 100644
--- a/extern/libmv/libmv/numeric/numeric.h
+++ b/extern/libmv/libmv/numeric/numeric.h
@@ -474,6 +474,17 @@ inline Mat23 SkewMatMinimal(const Vec2 &x) {
           1, 0, -x(0);
   return skew;
 }
+
+/// Returns the rotaiton matrix built from given vector of euler angles
+inline Mat3 RotationFromEulerVector(Vec3 euler_vector) {
+  double theta = euler_vector.norm();
+  if (theta == 0.0) {
+    return Mat3::Identity();
+  }
+  Vec3 w = euler_vector / theta;
+  Mat3 w_hat = CrossProductMatrix(w);
+  return Mat3::Identity() + w_hat*sin(theta) + w_hat*w_hat*(1 - cos(theta));
+}
 } // namespace libmv
 
 #endif  // LIBMV_NUMERIC_NUMERIC_H
diff --git a/extern/libmv/libmv/simple_pipeline/resect.cc b/extern/libmv/libmv/simple_pipeline/resect.cc
index 6e71c3c7206..b30d959b512 100644
--- a/extern/libmv/libmv/simple_pipeline/resect.cc
+++ b/extern/libmv/libmv/simple_pipeline/resect.cc
@@ -42,16 +42,6 @@ Mat2X PointMatrixFromMarkers(const vector<Marker> &markers) {
   return points;
 }
 
-Mat3 RotationFromEulerVector(Vec3 euler_vector) {
-  double theta = euler_vector.norm();
-  if (theta == 0.0) {
-    return Mat3::Identity();
-  }
-  Vec3 w = euler_vector / theta;
-  Mat3 w_hat = CrossProductMatrix(w);
-  return Mat3::Identity() + w_hat*sin(theta) + w_hat*w_hat*(1 - cos(theta));
-}
-
 // Uses an incremental rotation:
 //
 //   x = R' * R * X + t;
diff --git a/extern/libmv/libmv/simple_pipeline/rigid_registration.cc b/extern/libmv/libmv/simple_pipeline/rigid_registration.cc
new file mode 100644
index 00000000000..f7cb1e66e7d
--- /dev/null
+++ b/extern/libmv/libmv/simple_pipeline/rigid_registration.cc
@@ -0,0 +1,182 @@
+// Copyright (c) 2012 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/rigid_registration.h"
+#include "libmv/numeric/levenberg_marquardt.h"
+
+namespace libmv {
+
+template<class RigidTransformation>
+struct RigidRegistrationCostFunction {
+ public:
+  typedef Vec FMatrixType;
+  typedef RigidTransformation XMatrixType;
+
+  RigidRegistrationCostFunction(const vector<Vec3> &reference_points,
+                                const vector<Vec3> &points):
+    reference_points_(reference_points),
+    points_(points) {}
+
+  Vec CalculateResiduals(const Mat3 &R,
+                         const Vec3 &S,
+                         const Vec3 &t) const {
+    Vec residuals(points_.size());
+    residuals.setZero();
+
+    // Convert scale vector to matrix
+    Mat3 SMat = Mat3::Identity();
+    SMat(0, 0) *= S(0);
+    SMat(1, 1) *= S(1);
+    SMat(2, 2) *= S(2);
+
+    for (int i = 0; i < points_.size(); i++) {
+      Vec3 transformed_point = R * SMat * points_[i] + t;
+
+      double norm = (transformed_point - reference_points_[i]).norm();
+
+      residuals(i) = norm * norm;
+    }
+
+    return residuals;
+  }
+
+  Vec operator()(const Vec9 &RSt) const {
+    Mat3 R = RotationFromEulerVector(RSt.head<3>());
+    Vec3 S = RSt.segment<3>(3);
+    Vec3 t = RSt.tail<3>();
+
+    return CalculateResiduals(R, S, t);
+  }
+
+  Vec operator()(const Vec3 &euler) const {
+    Mat3 R = RotationFromEulerVector(euler);
+
+    return CalculateResiduals(R, Vec3(1.0, 1.0, 1.0), Vec3::Zero());
+  }
+
+  Vec operator()(const Vec6 &Rt) const {
+    Mat3 R = RotationFromEulerVector(Rt.head<3>());
+    Vec3 t = Rt.tail<3>();
+
+    return CalculateResiduals(R, Vec3(1.0, 1.0, 1.0), t);
+  }
+
+ private:
+  vector<Vec3> reference_points_;
+  vector<Vec3> points_;
+};
+
+static double RigidRegistrationError(const vector<Vec3> &reference_points,
+                                     const vector<Vec3> &points,
+                                     const Mat3 &R,
+                                     const Vec3 &S,
+                                     const Vec3 &t) {
+  double error = 0.0;
+
+  Mat3 SMat = Mat3::Identity();
+  SMat(0, 0) *= S(0);
+  SMat(1, 1) *= S(1);
+  SMat(2, 2) *= S(2);
+
+  for (int i = 0; i < points.size(); i++) {
+    Vec3 new_point = R * SMat * points[i] + t;
+
+    double norm = (new_point - reference_points[i]).norm();
+    error += norm * norm;
+  }
+  error /= points.size();
+
+  return error;
+}
+
+double RigidRegistration(const vector<Vec3> &reference_points,
+                         const vector<Vec3> &points,
+                         Mat3 &R,
+                         Vec3 &S,
+                         Vec3 &t) {
+  typedef LevenbergMarquardt<RigidRegistrationCostFunction <Vec9> > Solver;
+
+  RigidRegistrationCostFunction<Vec9> rigidregistration_cost(reference_points, points);
+  Solver solver(rigidregistration_cost);
+
+  Vec9 RSt = Vec9::Zero();
+
+  RSt(3) = RSt(4) = RSt(5) = 1.0;
+
+  Solver::SolverParameters params;
+  /*Solver::Results results = */ solver.minimize(params, &RSt);
+  /* TODO(sergey): better error handling here */
+
+  LG << "Rigid registration completed, rotation is:" << RSt.head<3>().transpose()
+    << ", scale is " << RSt.segment<3>(3).transpose()
+    << ", translation is " << RSt.tail<3>().transpose();
+
+  // Decompose individual rotation and translation
+  R = RotationFromEulerVector(RSt.head<3>());
+  S = RSt.segment<3>(3);
+  t = RSt.tail<3>();
+
+  return RigidRegistrationError(reference_points, points, R, S, t);
+}
+
+double RigidRegistration(const vector<Vec3> &reference_points,
+                         const vector<Vec3> &points,
+                         Mat3 &R,
+                         Vec3 &t) {
+  typedef LevenbergMarquardt<RigidRegistrationCostFunction <Vec6> > Solver;
+
+  RigidRegistrationCostFunction<Vec6> rigidregistration_cost(reference_points, points);
+  Solver solver(rigidregistration_cost);
+
+  Vec6 Rt = Vec6::Zero();
+  Solver::SolverParameters params;
+  /*Solver::Results results = */solver.minimize(params, &Rt);
+  /* TODO(sergey): better error handling here */
+
+  LG << "Rigid registration completed, rotation is:" << Rt.head<3>().transpose()
+    << ", translation is " << Rt.tail<3>().transpose();
+
+  R = RotationFromEulerVector(Rt.head<3>());
+  t = Rt.tail<3>();
+
+  return RigidRegistrationError(reference_points, points, R, Vec3(1.0, 1.0, 1.0), t);
+}
+
+double RigidRegistration(const vector<Vec3> &reference_points,
+                         const vector<Vec3> &points,
+                         Mat3 &R) {
+  typedef LevenbergMarquardt<RigidRegistrationCostFunction <Vec3> > Solver;
+
+  RigidRegistrationCostFunction<Vec3> rigidregistration_cost(reference_points, points);
+  Solver solver(rigidregistration_cost);
+
+  Vec3 euler = Vec3::Zero();
+  Solver::SolverParameters params;
+  /*Solver::Results results = */solver.minimize(params, &euler);
+  /* TODO(sergey): better error handling here */
+
+  LG << "Rigid registration completed, rotation is:" << euler.transpose();
+
+  R = RotationFromEulerVector(euler);
+
+  return RigidRegistrationError(reference_points, points, R, Vec3(1.0, 1.0, 1.0), Vec3::Zero());
+}
+
+}  // namespace libmv
diff --git a/extern/libmv/libmv/simple_pipeline/rigid_registration.h b/extern/libmv/libmv/simple_pipeline/rigid_registration.h
new file mode 100644
index 00000000000..21ea57af507
--- /dev/null
+++ b/extern/libmv/libmv/simple_pipeline/rigid_registration.h
@@ -0,0 +1,68 @@
+// Copyright (c) 2012 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_RIGID_REGISTRATION_H_
+#define LIBMV_SIMPLE_PIPELINE_RIGID_REGISTRATION_H_
+
+#include "libmv/base/vector.h"
+#include "libmv/numeric/numeric.h"
+
+namespace libmv {
+
+/*!
+    Searched for an affine transformation of rigid 3D object defined by it's
+    vertices positions from it's current state called points to it's desired
+    state called reference points.
+
+    Returns rotation matrix, per-component scale vector and translation which
+    transforms points to the mot close state to reference_points.
+
+    Return value is an average squared distance between reference state
+    and transformed one.
+
+    Minimzation of distance between point pairs is used to register such a
+    rigid transformation and algorithm assumes that pairs of points are
+    defined by point's index in a vector, so points with the same index
+    belongs to the same pair.
+ */
+double RigidRegistration(const vector<Vec3> &reference_points,
+                         const vector<Vec3> &points,
+                         Mat3 &R,
+                         Vec3 &S,
+                         Vec3 &t);
+
+/*!
+ * Same as RigidRegistration but provides registration of rotation and translation only
+ */
+double RigidRegistration(const vector<Vec3> &reference_points,
+                         const vector<Vec3> &points,
+                         Mat3 &R,
+                         Vec3 &t);
+
+/*!
+ * Same as RigidRegistration but provides registration of rotation only
+ */
+double RigidRegistration(const vector<Vec3> &reference_points,
+                         const vector<Vec3> &points,
+                         Mat3 &R);
+
+}  // namespace libmv
+
+#endif  // LIBMV_SIMPLE_PIPELINE_RIGID_REGISTRATION_H_