Feature/ten point radial distortion fundamental matrix

include/theia/theia.h

@@ -181,6 +181,8 @@
 #include "theia/sfm/pose/relative_pose_from_two_points_with_known_rotation.h"
 #include "theia/sfm/pose/seven_point_fundamental_matrix.h"
 #include "theia/sfm/pose/sim_transform_partial_rotation.h"
+#include "theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.h"
+#include "theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix_helper.h"
 #include "theia/sfm/pose/test_util.h"
 #include "theia/sfm/pose/three_point_relative_pose_partial_rotation.h"
 #include "theia/sfm/pose/two_point_pose_partial_rotation.h"

src/theia/CMakeLists.txt

@@ -166,6 +166,8 @@ set(THEIA_SRC
   sfm/pose/relative_pose_from_two_points_with_known_rotation.cc
   sfm/pose/seven_point_fundamental_matrix.cc
   sfm/pose/sim_transform_partial_rotation.cc
+  sfm/pose/ten_point_radial_distortion_fundamental_matrix.cc
+  sfm/pose/ten_point_radial_distortion_fundamental_matrix_helper.cc
   sfm/pose/test_util.cc
   sfm/pose/three_point_relative_pose_partial_rotation.cc
   sfm/pose/two_point_pose_partial_rotation.cc
@@ -328,6 +330,7 @@ if (BUILD_TESTING)
   gtest(sfm/pose/relative_pose_from_two_points_with_known_rotation)
   gtest(sfm/pose/seven_point_fundamental_matrix)
   gtest(sfm/pose/sim_transform_partial_rotation)
+  gtest(sfm/pose/ten_point_radial_distortion_fundamental_matrix)
   gtest(sfm/pose/three_point_relative_pose_partial_rotation)
   gtest(sfm/pose/two_point_pose_partial_rotation)
   gtest(sfm/reconstruction)

src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.cc

@@ -0,0 +1,152 @@
+// Copyright (C) 2019 The Regents of the University of California (Regents).
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above
+//       copyright notice, this list of conditions and the following
+//       disclaimer in the documentation and/or other materials provided
+//       with the distribution.
+//
+//     * Neither the name of The Regents or University of California nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Please contact the author of this library if you have any questions.
+// Author: Chris Sweeney ([email protected])
+
+// This file was created by Steffen Urban ([email protected]) or
+// company address ([email protected])
+// January 2019
+
+#include "theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.h"
+#include "theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix_helper.h"
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <Eigen/Jacobi>
+
+namespace theia {
+
+using Eigen::Matrix;
+using Eigen::MatrixXd;
+using Eigen::Vector3d;
+using Eigen::Vector2d;
+using Eigen::Matrix3d;
+using Eigen::Vector4d;
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+using Vector10d = Eigen::Matrix<double, 10, 1>;
+using Matrix102d = Eigen::Matrix<double, 10, 2>;
+using Matrix210d = Eigen::Matrix<double, 2, 10>;
+
+bool TenPointRadialDistortionFundamentalMatrix(
+    const std::vector<Eigen::Vector2d>& normalized_feature_points_left,
+    const std::vector<Eigen::Vector2d>& normalized_feature_points_right,
+    std::vector<RadialFundamentalMatrixResult>* results,
+    double lmin, double lmax) {
+
+  Matrix102d X;
+  Matrix102d U;
+  for (int i = 0; i < 10; ++i) {
+    X.row(i) = normalized_feature_points_left[i];
+    U.row(i) = normalized_feature_points_right[i];
+  }
+
+  Vector10d Z1;
+  Vector10d Z2;
+  Matrix<double, 10, 16> A;
+  Z1.array() =
+      X.col(0).array() * X.col(0).array() + X.col(1).array() * X.col(1).array();
+  Z2.array() =
+      U.col(0).array() * U.col(0).array() + U.col(1).array() * U.col(1).array();
+
+  A.col(0).array() = X.col(0).array() * U.col(0).array();
+  A.col(1).array() = X.col(0).array() * U.col(1).array();
+  A.col(2).array() = X.col(1).array() * U.col(0).array();
+  A.col(3).array() = X.col(1).array() * U.col(1).array();
+  A.col(4).array() = U.col(0).array() * Z1.array();
+  A.col(5).array() = U.col(0).array();
+  A.col(6).array() = U.col(1).array() * Z1.array();
+  A.col(7).array() = U.col(1).array();
+  A.col(8).array() = X.col(0).array() * Z2.array();
+  A.col(9).array() = X.col(0).array();
+  A.col(10).array() = X.col(1).array() * Z2.array();
+  A.col(11).array() = X.col(1).array();
+  A.col(12).array() = Z1.array() * Z2.array();
+  A.col(13).array() = Z1.array();
+  A.col(14).array() = Z2.array();
+  A.col(15).fill(1.0);
+
+  const Matrix<double, 10, 6> Mr =
+      A.block<10, 10>(0, 0).lu().solve(A.block<10, 6>(0, 10));
+
+  Matrix<double, 29, 1> params;
+
+  params << Mr(5, 0), Mr(5, 1), -Mr(4, 0), -Mr(4, 1), Mr(5, 2), Mr(5, 3),
+      Mr(5, 4) - Mr(4, 2), Mr(5, 5) - Mr(4, 3), -Mr(4, 4), -Mr(4, 5), Mr(7, 0),
+      Mr(7, 1), -Mr(6, 0), -Mr(6, 1), Mr(7, 2), Mr(7, 3), Mr(7, 4) - Mr(6, 2),
+      Mr(7, 5) - Mr(6, 3), -Mr(6, 4), -Mr(6, 5), Mr(9, 0), Mr(9, 1) - Mr(8, 0),
+      -Mr(8, 1), Mr(9, 2), Mr(9, 4), Mr(9, 3) - Mr(8, 2), -Mr(8, 3),
+      Mr(9, 5) - Mr(8, 4), -Mr(8, 5);
+
+  Matrix210d Ls;
+  int nsols = TenPointRadialDistortionFundamentalMatrixHelper(params, Ls);
+
+  if (nsols > 0) {
+    Vector4d m1;
+    Vector6d m2;
+    Vector6d m3;
+    Vector10d b;
+
+    b << Mr(5, 0), Mr(5, 1), -Mr(4, 0), -Mr(4, 1), Mr(5, 2), Mr(5, 3),
+        Mr(5, 4) - Mr(4, 2), Mr(5, 5) - Mr(4, 3), -Mr(4, 4), -Mr(4, 5);
+
+    for (int i = 0; i < nsols; i++) {
+      const double l1 = Ls(0, i);
+      const double l2 = Ls(1, i);
+
+      if (l1 < lmin || l2 < lmin || l1 > lmax || l2 > lmax) {
+        continue;
+      }
+
+      const double l1l1 = l1 * l1;
+      const double l1l2 = l1 * l2;
+      double f23;
+
+      m1 << l1l2, l1, l2, 1;
+      m2 << l1l2 * l1, l1l1, l1l2, l1, l2, 1;
+      f23 = -b.block<6, 1>(4, 0).dot(m2) / b.block<4, 1>(0, 0).dot(m1);
+      m3 << l2 * f23, f23, l1l2, l1, l2, 1;
+
+      RadialFundamentalMatrixResult res;
+      res.l1 = l1;
+      res.l2 = l2;
+      res.F <<
+          m3.dot(-Mr.row(0)), m3.dot(-Mr.row(1)), m3.dot(-Mr.row(9)),
+          m3.dot(-Mr.row(2)), m3.dot(-Mr.row(3)), f23,
+          m3.dot(-Mr.row(5)), m3.dot(-Mr.row(7)), 1.0;
+      results->push_back(res);
+    }
+
+    return true;
+  } else
+    return false;
+}
+}  // theia namespace

src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.h

@@ -0,0 +1,79 @@
+// Copyright (C) 2019 The Regents of the University of California (Regents).
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above
+//       copyright notice, this list of conditions and the following
+//       disclaimer in the documentation and/or other materials provided
+//       with the distribution.
+//
+//     * Neither the name of The Regents or University of California nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Please contact the author of this library if you have any questions.
+// Author: Chris Sweeney ([email protected])
+
+// This file was created by Steffen Urban ([email protected]) or
+// company address ([email protected])
+// January 2019
+
+// This solver is taken from http://cmp.felk.cvut.cz/~hellej1/ and implements
+// the ten point (two-sided) radial distortion fundamental matrix solver
+// (H10_l1l2).
+// The corresponding paper is:
+// "Efficient Solution to the Epipolar Geometry for Radially Distorted Cameras",
+// Zuzana Kukelova et al. ICCV 2015
+
+#ifndef THEIA_SFM_POSE_TEN_POINT_RADIAL_DISTORTION_FUNDAMENTAL_MATRIX_H_
+#define THEIA_SFM_POSE_TEN_POINT_RADIAL_DISTORTION_FUNDAMENTAL_MATRIX_H_
+
+#include <Eigen/Core>
+#include <vector>
+
+#include "theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.h"
+
+namespace theia {
+
+struct RadialFundamentalMatrixResult {
+  Eigen::Matrix3d F;
+  double l1;
+  double l2;
+};
+
+// Input:
+//   normalized_feature_points_left  - ten normalized image positions (inv(K)*p)
+//   normalized_feature_points_right - ten normalized image positions (inv(K)*p)
+//   lmin - minimum radial distortion (can be used to speed up ransac loops)
+//   lmax - maximum radial distortion (can be used to speed up ransac loops)
+// Output: bool - returns true if at least one solution was found
+// Return: RadialFundamentalMatrixResult - struct that contains fundamental
+// matrix and a radial
+// distortion parameter for each image (i.e. two-sided, called: H10_l1l2 in the
+// paper)
+bool TenPointRadialDistortionFundamentalMatrix(
+    const std::vector<Eigen::Vector2d>& normalized_feature_points_left,
+    const std::vector<Eigen::Vector2d>& normalized_feature_points_right,
+    std::vector<RadialFundamentalMatrixResult>* results, double lmin = -5.0,
+    double lmax = 0.0);
+}
+
+#endif