Merge branch 'maint-remove-v1-code' into 'master'

MAINT: remove old camera code

See merge request oddkiva/sara!460

cpp/drafts/Calibration/Utilities.hpp

@@ -14,122 +14,49 @@
 #include <drafts/Calibration/Chessboard.hpp>
 
 #include <DO/Sara/Graphics.hpp>
-#include <DO/Sara/MultiViewGeometry/Camera/OmnidirectionalCamera.hpp>
+#include <DO/Sara/MultiViewGeometry/Camera/v2/OmnidirectionalCamera.hpp>
 #include <DO/Sara/MultiViewGeometry/Camera/v2/PinholeCamera.hpp>
 #include <DO/Sara/MultiViewGeometry/PnP/LambdaTwist.hpp>
 
 
 namespace DO::Sara {
 
-  inline auto init_calibration_matrix(const int w, const int h)
-      -> Eigen::Matrix3d
+  template <typename CameraIntrinsicParameters>
+  inline auto init_calibration_matrix(CameraIntrinsicParameters& camera,
+                                      const int w, const int h) -> void
   {
-    const auto d = static_cast<double>(std::max(w, h));
+    using T = typename CameraIntrinsicParameters::T;
+    const auto d = static_cast<T>(std::max(w, h));
     const auto f = 0.5 * d;
 
-    // clang-format off
-  const auto K = (Eigen::Matrix3d{} <<
-    f, 0, w * 0.5,
-    0, f, h * 0.5,
-    0, 0,       1
-  ).finished();
-    // clang-format on
-
-    return K;
-  }
-
-  inline auto estimate_pose_with_p3p(const ChessboardCorners& cb,
-                                     const OmnidirectionalCamera<double>& K)
-      -> std::optional<Eigen::Matrix<double, 3, 4>>
-  {
-    auto points = Eigen::Matrix3d{};
-    auto rays = Eigen::Matrix3d{};
-
-    SARA_DEBUG << "Filling points and rays for P3P..." << std::endl;
-    auto xs = std::array{0, 1, 0};
-    auto ys = std::array{0, 0, 1};
-    for (auto n = 0; n < 3; ++n)
-    {
-      const auto& x = xs[n];
-      const auto& y = ys[n];
-      const Eigen::Vector2d xn = cb.image_point(x, y).cast<double>();
-      if (is_nan(xn))
-        continue;
-
-      points.col(n) = cb.scene_point(x, y);
-      rays.col(n) = K.backproject(xn).normalized();
-    }
-    if (is_nan(points) || is_nan(rays))
-      return std::nullopt;
-
-    SARA_DEBUG << "Solving P3P..." << std::endl;
-    SARA_DEBUG << "Points =\n" << points << std::endl;
-    SARA_DEBUG << "Rays   =\n" << rays << std::endl;
-    const auto poses = solve_p3p(points, rays);
-    if (poses.empty())
-      return std::nullopt;
-
-    // Find the best poses.
-    SARA_DEBUG << "Determining the best pose..." << std::endl;
-    auto errors = std::vector<double>{};
-    for (const auto& pose : poses)
-    {
-      auto error = 0;
-
-      auto n = 0;
-      for (auto y = 0; y < cb.height(); ++y)
-      {
-        for (auto x = 0; x < cb.width(); ++x)
-        {
-          auto x0 = cb.image_point(x, y);
-          if (is_nan(x0))
-            continue;
-
-          const auto& R = pose.topLeftCorner<3, 3>();
-          const auto& t = pose.col(3);
-          const auto X = (R * cb.scene_point(x, y) + t);
-
-
-          const Eigen::Vector2f x1 = K.project(X).cast<float>();
-          error += (x1 - x0).squaredNorm();
-          ++n;
-        }
-      }
-
-      errors.emplace_back(error / n);
-    }
-
-    const auto best_pose_index =
-        std::min_element(errors.begin(), errors.end()) - errors.begin();
-    const auto& best_pose = poses[best_pose_index];
-    SARA_DEBUG << "Best pose:\n" << best_pose << std::endl;
-
-    return best_pose;
+    camera.fx() = f;
+    camera.fy() = f;
+    camera.shear() = 0;
+    camera.u0() = static_cast<T>(w * 0.5);
+    camera.v0() = static_cast<T>(h * 0.5);
   }
 
-  inline auto estimate_pose_with_p3p(const ChessboardCorners& cb,
-                                     const Eigen::Matrix3d& K)
+  template <typename CameraIntrinsicParameters>
+  auto estimate_pose_with_p3p(const ChessboardCorners& chessboard,
+                              const CameraIntrinsicParameters& camera)
       -> std::optional<Eigen::Matrix<double, 3, 4>>
   {
     auto points = Eigen::Matrix3d{};
     auto rays = Eigen::Matrix3d{};
 
-    const Eigen::Matrix3d K_inv = K.inverse();
-
     SARA_DEBUG << "Filling points and rays for P3P..." << std::endl;
     auto xs = std::array{0, 1, 0};
     auto ys = std::array{0, 0, 1};
     for (auto n = 0; n < 3; ++n)
     {
       const auto& x = xs[n];
       const auto& y = ys[n];
-      const Eigen::Vector3d xn =
-          cb.image_point(x, y).homogeneous().cast<double>();
+      const Eigen::Vector2d xn = chessboard.image_point(x, y).cast<double>();
       if (is_nan(xn))
         continue;
 
-      points.col(n) = cb.scene_point(x, y);
-      rays.col(n) = (K_inv * xn).normalized();
+      points.col(n) = chessboard.scene_point(x, y);
+      rays.col(n) = camera.backproject(xn).normalized();
     }
     if (is_nan(points) || is_nan(rays))
       return std::nullopt;
@@ -149,19 +76,19 @@ namespace DO::Sara {
       auto error = 0;
 
       auto n = 0;
-      for (auto y = 0; y < cb.height(); ++y)
+      for (auto y = 0; y < chessboard.height(); ++y)
       {
-        for (auto x = 0; x < cb.width(); ++x)
+        for (auto x = 0; x < chessboard.width(); ++x)
         {
-          auto x0 = cb.image_point(x, y);
+          auto x0 = chessboard.image_point(x, y);
           if (is_nan(x0))
             continue;
 
           const auto& R = pose.topLeftCorner<3, 3>();
           const auto& t = pose.col(3);
+          const auto X = (R * chessboard.scene_point(x, y) + t);
 
-          const Eigen::Vector2f x1 =
-              (K * (R * cb.scene_point(x, y) + t)).hnormalized().cast<float>();
+          const Eigen::Vector2f x1 = camera.project(X).template cast<float>();
           error += (x1 - x0).squaredNorm();
           ++n;
         }
@@ -178,7 +105,6 @@ namespace DO::Sara {
     return best_pose;
   }
 
-
   template <typename CameraModel>
   inline auto inspect(ImageView<Rgb8>& image,               //
                       const ChessboardCorners& chessboard,  //

cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp

@@ -45,30 +45,30 @@ class ChessboardCalibrationProblem
     _camera_type = camera_type;
   }
 
-  auto initialize_intrinsics(const Eigen::Matrix3d& K, const Eigen::Vector3d& k,
-                             const Eigen::Vector2d& p,  //
-                             const double xi) -> void
+  auto
+  initialize_intrinsics(const sara::v2::OmnidirectionalCamera<double>& camera)
+      -> void
   {
     // fx
-    _intrinsics[0] = K(0, 0);
+    _intrinsics[0] = camera.fx();
     // fy (NORMALIZED)
-    _intrinsics[1] = K(1, 1) / K(0, 0);
+    _intrinsics[1] = camera.fy() / camera.fx();
     // shear (NORMALIZED)
-    _intrinsics[2] = K(0, 1) / K(0, 0);
+    _intrinsics[2] = camera.shear() / camera.fx();
     // principal point (u0, v0)
-    _intrinsics[3] = K(0, 2);
-    _intrinsics[4] = K(1, 2);
+    _intrinsics[3] = camera.u0();
+    _intrinsics[4] = camera.v0();
 
     // Mirror parameter: xi
-    _intrinsics[5] = xi;
+    _intrinsics[5] = camera.xi();
 
     // k
-    _intrinsics[6] = k(0);
-    _intrinsics[7] = k(1);
-    _intrinsics[8] = k(2);
+    _intrinsics[6] = camera.k()(0);
+    _intrinsics[7] = camera.k()(1);
+    _intrinsics[8] = camera.k()(2);
     // p
-    _intrinsics[9] = p(0);
-    _intrinsics[10] = p(1);
+    _intrinsics[9] = camera.p()(0);
+    _intrinsics[10] = camera.p()(1);
   }
 
   auto add(const sara::ChessboardCorners& chessboard, const Eigen::Matrix3d& R,
@@ -256,7 +256,7 @@ class ChessboardCalibrationProblem
 #endif
   }
 
-  auto copy_camera_intrinsics(sara::OmnidirectionalCamera<double>& camera)
+  auto copy_camera_intrinsics(sara::v2::OmnidirectionalCamera<double>& camera)
       -> void
   {
     // Copy back the final parameter to the omnidirectional camera parameter
@@ -270,6 +270,11 @@ class ChessboardCalibrationProblem
     const auto shear = fx * alpha;
     const auto& u0 = mutable_intrinsics()[3];
     const auto& v0 = mutable_intrinsics()[4];
+    camera.fx() = fx;
+    camera.fy() = fy;
+    camera.shear() = shear;
+    camera.u0() = u0;
+    camera.v0() = v0;
 
     // Mirror parameter.
     const auto& xi = mutable_intrinsics()[5];
@@ -280,16 +285,9 @@ class ChessboardCalibrationProblem
     const auto& k2 = mutable_intrinsics()[8];
     const auto& p0 = mutable_intrinsics()[9];
     const auto& p1 = mutable_intrinsics()[10];
-    // clang-format off
-    auto K = Eigen::Matrix3d{};
-    K << fx, shear, u0,
-          0,    fy, v0,
-          0,     0,  1;
-    // clang-format on
-    camera.set_calibration_matrix(K);
-    camera.radial_distortion_coefficients << k0, k1, k2;
-    camera.tangential_distortion_coefficients << p0, p1;
-    camera.xi = xi;
+    camera.k() << k0, k1, k2;
+    camera.p() << p0, p1;
+    camera.xi() = xi;
   }
 
 private:
@@ -346,18 +344,15 @@ auto sara_graphics_main(int argc, char** argv) -> int
   auto chessboards = std::vector<sara::ChessboardCorners>{};
 
   // Initialize the calibration matrix.
-  auto camera = sara::OmnidirectionalCamera<double>{};
-  camera.K = sara::init_calibration_matrix(frame.width(), frame.height());
-  camera.K_inverse = camera.K.inverse();
-  camera.radial_distortion_coefficients.setZero();
-  camera.tangential_distortion_coefficients.setZero();
-  camera.xi = 1;
+  auto camera = sara::v2::OmnidirectionalCamera<double>{};
+  sara::init_calibration_matrix(camera, frame.width(), frame.height());
+  camera.k().setZero();
+  camera.p().setZero();
+  camera.xi() = 1;
 
   // Initialize the calibration problem.
   auto calibration_problem = ChessboardCalibrationProblem{};
-  calibration_problem.initialize_intrinsics(
-      camera.K, camera.radial_distortion_coefficients,
-      camera.tangential_distortion_coefficients, camera.xi);
+  calibration_problem.initialize_intrinsics(camera);
 
   auto selected_frames = std::vector<sara::Image<sara::Rgb8>>{};
   sara::create_window(frame.sizes());
@@ -453,13 +448,14 @@ auto sara_graphics_main(int argc, char** argv) -> int
 
     calibration_problem.copy_camera_intrinsics(camera);
 
-    SARA_DEBUG << "K =\n" << camera.K << std::endl;
-    SARA_DEBUG << "k = " << camera.radial_distortion_coefficients.transpose()
-               << std::endl;
-    SARA_DEBUG << "p = "
-               << camera.tangential_distortion_coefficients.transpose()
-               << std::endl;
-    SARA_DEBUG << "xi = " << camera.xi << std::endl;
+    SARA_DEBUG << "fx = " << camera.fx() << std::endl;
+    SARA_DEBUG << "fy = " << camera.fy() << std::endl;
+    SARA_DEBUG << "shear = " << camera.shear() << std::endl;
+    SARA_DEBUG << "u0 = " << camera.u0() << std::endl;
+    SARA_DEBUG << "v0 = " << camera.v0() << std::endl;
+    SARA_DEBUG << "k = " << camera.k().transpose() << std::endl;
+    SARA_DEBUG << "p = " << camera.p().transpose() << std::endl;
+    SARA_DEBUG << "xi = " << camera.xi() << std::endl;
   }
 
   for (auto i = 0u; i < chessboards.size(); ++i)

cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp

@@ -33,18 +33,18 @@ class ChessboardCalibrationData
   static constexpr auto extrinsic_parameter_count =
       sara::PinholeCameraReprojectionError::extrinsic_parameter_count;
 
-  auto initialize_intrinsics(const Eigen::Matrix3d& K) -> void
+  auto initialize_intrinsics(const sara::v2::PinholeCamera<double>& camera) -> void
   {
     // fx
-    _intrinsics.fx() = K(0, 0);
+    _intrinsics.fx() = camera.fx();
     // fy (NORMALIZED)
-    _intrinsics.fy() = K(1, 1) / K(0, 0);
+    _intrinsics.fy() = camera.fy() / camera.fx();
     // shear (NORMALIZED)
-    _intrinsics.shear() = K(0, 1) / K(0, 0);
+    _intrinsics.shear() = camera.shear() / camera.fx();
     // u0
-    _intrinsics.u0() = K(0, 2);
+    _intrinsics.u0() = camera.u0();
     // v0
-    _intrinsics.v0() = K(1, 2);
+    _intrinsics.v0() = camera.v0();
   }
 
   auto add(const sara::ChessboardCorners& chessboard,  //
@@ -308,12 +308,12 @@ auto sara_graphics_main(int argc, char** argv) -> int
   auto chessboards = std::vector<sara::ChessboardCorners>{};
 
   // Initialize the calibration matrix.
-  const auto K_initial =
-      sara::init_calibration_matrix(frame.width(), frame.height());
+  auto camera = sara::v2::PinholeCamera<double>{};
+  sara::init_calibration_matrix(camera, frame.width(), frame.height());
 
   // Initialize the calibration problem.
   auto calibration_data = ChessboardCalibrationData{};
-  calibration_data.initialize_intrinsics(K_initial);
+  calibration_data.initialize_intrinsics(camera);
 
   auto selected_frames = std::vector<sara::Image<sara::Rgb8>>{};
   sara::create_window(frame.sizes());
@@ -340,7 +340,7 @@ auto sara_graphics_main(int argc, char** argv) -> int
       auto frame_copy = sara::Image<sara::Rgb8>{frame};
       draw_chessboard(frame_copy, chessboard);
 
-      const auto pose = estimate_pose_with_p3p(chessboard, K_initial);
+      const auto pose = estimate_pose_with_p3p(chessboard, camera);
       if (pose == std::nullopt || sara::is_nan(*pose))
         continue;
 
@@ -352,13 +352,6 @@ auto sara_graphics_main(int argc, char** argv) -> int
       calibration_data.add(chessboard, R, t);
 
       // inspect(frame_copy, chessboard, K_initial, Rs[0], ts[0]);
-      auto camera = sara::v2::PinholeCamera<double>();
-      camera.fx() = K_initial(0, 0);
-      camera.fy() = K_initial(1, 1);
-      camera.shear() = K_initial(0, 1);
-      camera.u0() = K_initial(0, 2);
-      camera.v0() = K_initial(1, 2);
-
       inspect(frame_copy, chessboard, camera, R, t);
       sara::draw_text(frame_copy, 80, 80, "Chessboard: FOUND!", sara::White8,
                       60, 0, false, true);
@@ -391,7 +384,6 @@ auto sara_graphics_main(int argc, char** argv) -> int
   // "my guess is that this is because the LBFGS direction is poor and
   // re-starting the solver resets it to an identity matrix."
   auto convergence = false;
-  auto camera = sara::v2::PinholeCamera<double>{};
   for (auto i = 0; i < 20 && !convergence; ++i)
   {
     SARA_DEBUG << "Solving Ceres Problem..." << std::endl;

cpp/drafts/ChessboardDetection/examples/Utilities/ImageOrVideoReader.cpp

@@ -1,6 +1,6 @@
 #include "ImageOrVideoReader.hpp"
 
-#include <boost/filesystem.hpp>
+#include <filesystem>
 
 
 namespace DO::Sara {
@@ -14,7 +14,7 @@ namespace DO::Sara {
 
   auto ImageOrVideoReader::open(const std::string& path) -> void
   {
-    namespace fs = boost::filesystem;
+    namespace fs = std::filesystem;
     if (fs::path{path}.extension().string() == ".png")
     {
       _path = path;