BUG: fix absolute translation solver.

TODO.md

@@ -1,19 +1,3 @@
-VULKAN
-======
-- Hello Triangle tutorial.
-
-
-Bundle Adjustment Tests
-=======================
-
-Form the bundle adjustment problem.
-TODO:
-- Choose a reference camera.
-- Populate the list of cameras in BundleAdjustmentProblems:
-  As a reminder:
-  image #i -> camera #i -> BundleAdjustmentProblem.camera_parameters.row(i)
-- Initialize the absolute camera poses.
-
 Vanishing Point Detection with Radial Distortion
 ================================================
 Unsupervised Vanishing Point Detection and Camera Calibration from a Single

cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/AbsoluteTranslationSolver.hpp

@@ -11,14 +11,14 @@
 
 #pragma once
 
-#include <Eigen/Dense>
-#include <Eigen/Eigen>
+#include <DO/Sara/Core/Tensor.hpp>
 
 
 namespace DO::Sara {
 
   //! @brief Two-point solver that solves the absolute translation given a known
   //!        absolute rotaton.
+  //!        Also returns the scales for the two backprojected rays.
   //!
   //! Turns out to be very basic...
   template <typename T>
@@ -27,18 +27,23 @@ namespace DO::Sara {
     static constexpr auto num_points = 2;
     static constexpr auto num_models = 1;
 
+    using solution_type = Eigen::Vector<T, 5>;
     using translation_vector_type = Eigen::Vector3<T>;
     using scale_vector_type = Eigen::Vector2<T>;
     using model_type = std::pair<translation_vector_type, scale_vector_type>;
 
-    //! @brief Inputs are rotated world scene points and the backprojected rays.
+    using tensor_view_type = TensorView_<T, 2>;
+    using data_point_type = std::array<TensorView_<T, 2>, 2>;
+
+    //! @brief Inputs are **rotated** world scene points and the backprojected
+    //! rays.
     auto operator()(const Eigen::Matrix<T, 3, 2>& Rx,
                     const Eigen::Matrix<T, 3, 2>& y) const -> model_type
     {
-      const Eigen::Vector3<T> x0 = Rx.col(0);
-      const Eigen::Vector3<T> x1 = Rx.col(1);
-      const Eigen::Vector3<T> y0 = y.col(0);
-      const Eigen::Vector3<T> y1 = y.col(1);
+      const auto x0 = Rx.col(0);
+      const auto x1 = Rx.col(1);
+      const auto y0 = y.col(0);
+      const auto y1 = y.col(1);
 
       static const auto I3 = Eigen::Matrix3<T>::Identity();
       static const auto O3 = Eigen::Vector3<T>::Zero();
@@ -50,13 +55,74 @@ namespace DO::Sara {
       auto b = Eigen::Vector<T, 6>{};
       b << x0, x1;
 
-      const auto x = A.colPivHouseholderQr().solve(b);
-
+      const solution_type x = A.colPivHouseholderQr().solve(b);
       const translation_vector_type t = x.head(3);
       const scale_vector_type scales = x.tail(2);
+
+#if 0
+      SARA_DEBUG << "Rx =\n" << Rx << std::endl;
+      SARA_DEBUG << "y =\n" << y << std::endl;
+      SARA_DEBUG << "Rx0.cross(y0) = " << x0.cross(y0) << std::endl;
+      SARA_DEBUG << "Rx1.cross(y1) = " << x1.cross(y1) << std::endl;
+      SARA_DEBUG << "A =\n" << A << std::endl;
+      SARA_DEBUG << "b =\n" << b << std::endl;
+      SARA_CHECK(t.transpose());
+      SARA_CHECK(scales.transpose());
+#endif
+
       return {t, scales};
     }
   };
 
+  template <typename T>
+  struct AbsolutePoseSolverUsingRotationKnowledge
+  {
+    //! @brief The translation solver.
+    using translation_solver_type = AbsoluteTranslationSolver<T>;
+    using tensor_view_type = TensorView_<T, 2>;
+    using data_point_type = std::array<TensorView_<T, 2>, 2>;
+    using model_type = Eigen::Matrix<T, 3, 4>;
+
+    static constexpr auto num_points = translation_solver_type::num_points;
+    static constexpr auto num_models = translation_solver_type::num_models;
+    static_assert(num_models == 1);
+
+    translation_solver_type tsolver;
+
+    //! @brief The absolute rotation known.
+    //!
+    //! This can be set from the composition of success relative rotation.
+    Eigen::Matrix3<T> R;
+
+    inline auto operator()(const tensor_view_type& scene_points,
+                           const tensor_view_type& rays) const
+        -> std::array<model_type, num_models>
+    {
+      const auto sp_mat_ = scene_points.colmajor_view().matrix();
+
+      Eigen::Matrix<T, 3, 2> sp_mat = sp_mat_.topRows(3);
+      if (sp_mat_.cols() == 4)
+        sp_mat.array().rowwise() /= sp_mat_.array().row(3);
+
+      // Apply the global rotation to the scene points.
+      sp_mat = R * sp_mat;
+
+      const Eigen::Matrix<T, 3, 2> ray_mat = rays.colmajor_view().matrix();
+
+      // Calculate the absolute translation.
+      const auto [t, scales] = tsolver(sp_mat, ray_mat);
+
+      // Return the absolute pose.
+      auto pose = model_type{};
+      pose << R, t;
+      return {pose};
+    }
+
+    inline auto operator()(const data_point_type& X)
+        -> std::array<model_type, num_models>
+    {
+      const auto& [scene_points, backprojected_rays] = X;
+      return this->operator()(scene_points, backprojected_rays);
+    }
+  };
 }  // namespace DO::Sara
-#pragma once

cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.cpp

@@ -11,7 +11,9 @@
 
 #include <DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp>
 
+#include <DO/Sara/Core/Math/Rotation.hpp>
 #include <DO/Sara/Logging/Logger.hpp>
+#include <DO/Sara/MultiViewGeometry/MinimalSolvers/AbsoluteTranslationSolver.hpp>
 #include <DO/Sara/RANSAC/RANSACv2.hpp>
 
 
@@ -31,6 +33,48 @@ auto CameraPoseEstimator::estimate_pose(
                     debug);
 }
 
+auto CameraPoseEstimator::estimate_pose(
+    const PointRayCorrespondenceList<double>& point_ray_pairs,
+    const CameraIntrinsicModel& camera,
+    const Eigen::Matrix3d& absolute_rotation)
+    -> std::tuple<PoseMatrix, Inlier, MinimalSample>
+{
+  _inlier_predicate.set_camera(camera);
+
+  const auto solver = AbsolutePoseSolverUsingRotationKnowledge<double>{
+      .R = absolute_rotation  //
+  };
+  auto& logger = Logger::get();
+  SARA_LOGI(logger, "Current rotation =\n{}", solver.R);
+
+  // The rotation is expressed in the camera coordinates.
+  // But the calculation is done in the automotive/aeronautics coordinate
+  // system.
+  //
+  // The z-coordinate of the camera coordinates is the x-axis of the
+  // automotive coordinates
+  //
+  // clang-format off
+  static const auto P = (Eigen::Matrix3d{} <<
+     0,  0, 1,
+    -1,  0, 0,
+     0, -1, 0
+  ).finished();
+  // clang-format on
+  const auto q_abs =
+      Eigen::Quaterniond{P * absolute_rotation.transpose() * P.transpose()};
+  const auto angles = calculate_yaw_pitch_roll(q_abs);
+  static constexpr auto degrees = 180. / M_PI;
+  SARA_LOGI(logger, "A priori yaw   = {:0.3f} deg", angles(0) * degrees);
+  SARA_LOGI(logger, "A priori pitch = {:0.3f} deg", angles(1) * degrees);
+  SARA_LOGI(logger, "A priori roll  = {:0.3f} deg", angles(2) * degrees);
+
+  static constexpr auto debug = true;
+  return v2::ransac(point_ray_pairs, solver, _inlier_predicate,
+                    _ransac_iter_max, _ransac_confidence_min, std::nullopt,
+                    debug);
+}
+
 auto CameraPoseEstimator::estimate_pose(
     const std::vector<FeatureTrack>& valid_ftracks,
     const CameraPoseGraph::Vertex pv,
@@ -147,3 +191,121 @@ auto CameraPoseEstimator::estimate_pose(
 
   return {pose, pose_estimated_successfully};
 }
+
+auto CameraPoseEstimator::estimate_pose(
+    const std::vector<FeatureTrack>& valid_ftracks,
+    const CameraPoseGraph::Vertex pv,
+    const CameraPoseEstimator::CameraIntrinsicModel& camera,
+    const PointCloudGenerator& pcg, const Eigen::Matrix3d& absolute_rotation)
+    -> std::pair<PoseMatrix, bool>
+{
+  auto& logger = Logger::get();
+
+  const auto num_ftracks = static_cast<Eigen::Index>(valid_ftracks.size());
+
+  SARA_LOGD(logger, "Applying NMS to the {} feature tracks...", num_ftracks);
+
+  auto ftracks_filtered = std::vector<FeatureTrack>{};
+  ftracks_filtered.resize(num_ftracks);
+  std::transform(
+      valid_ftracks.begin(), valid_ftracks.end(), ftracks_filtered.begin(),
+      [&pcg, pv](const FeatureTrack& ftrack) -> FeatureTrack {
+        const auto ftrack_filtered = pcg.filter_by_non_max_suppression(ftrack);
+        const auto fvertex = pcg.find_feature_vertex_at_pose(  //
+            ftrack_filtered, pv);
+        if (!fvertex.has_value())
+          throw std::runtime_error{"Error: the filtered track must contain the "
+                                   "target camera vertex!"};
+        if (ftrack_filtered.size() <= 2)
+          throw std::runtime_error{"Error: a filtered feature track can't "
+                                   "possibly have cardinality 2!"};
+        return ftrack_filtered;
+      });
+
+  // Use only feature tracks with reasonable point z-depth value.
+  auto ftrack_indices_plausible = std::vector<Eigen::Index>{};
+  for (auto t = 0; t < num_ftracks; ++t)
+  {
+    const auto& ftrack = ftracks_filtered[t];
+
+    // Fetch the 3D scene coordinates.
+    const auto scene_point_indices = pcg.list_scene_point_indices(ftrack);
+    if (scene_point_indices.empty())
+      throw std::runtime_error{
+          "Error: a feature track must be assigned a scene point index!"};
+
+    const auto coords = pcg.barycenter(scene_point_indices).coords();
+    if (coords.z() < 0 || coords.z() > 100)
+      continue;
+
+    ftrack_indices_plausible.push_back(t);
+  }
+
+  const auto num_ftracks_plausible =
+      static_cast<Eigen::Index>(ftrack_indices_plausible.size());
+  SARA_LOGD(logger, "Plausible feature tracks: {}", num_ftracks_plausible);
+
+  auto point_ray_pairs = PointRayCorrespondenceList<double>{};
+  point_ray_pairs.x.resize({num_ftracks_plausible, 3});
+  point_ray_pairs.y.resize({num_ftracks_plausible, 3});
+
+  // Data collection.
+  //
+  // 1. Collect the scene point coordinates.
+  SARA_LOGD(logger, "Retrieving scene points for each feature track...");
+  auto scene_coords = point_ray_pairs.x.colmajor_view().matrix();
+  for (auto ti = 0; ti < num_ftracks_plausible; ++ti)
+  {
+    const auto& t = ftrack_indices_plausible[ti];
+    const auto& ftrack = ftracks_filtered[t];
+
+    // Fetch the 3D scene coordinates.
+    const auto scene_point_indices = pcg.list_scene_point_indices(ftrack);
+    if (scene_point_indices.empty())
+      throw std::runtime_error{
+          "Error: a feature track must be assigned a scene point index!"};
+
+    // If there are more than one scene point index, we fetch the barycentric
+    // coordinates anyway.
+    scene_coords.col(ti) = pcg.barycenter(scene_point_indices).coords();
+  }
+
+  // 2. Collect the backprojected rays from the current camera view for each
+  //    feature track.
+  SARA_LOGD(logger,
+            "Calculating backprojected rays from camera pose [{}] for each "
+            "feature track...",
+            pv);
+  auto rays = point_ray_pairs.y.colmajor_view().matrix();
+  for (auto ti = 0; ti < num_ftracks_plausible; ++ti)
+  {
+    const auto& t = ftrack_indices_plausible[ti];
+    const auto& ftrack = ftracks_filtered[t];
+    // Calculate the backprojected ray.
+    const auto& fv = pcg.find_feature_vertex_at_pose(ftrack, pv);
+    if (!fv.has_value())
+      throw std::runtime_error{"Error: the feature track must be alive!"};
+    const auto pixel_coords = pcg.pixel_coords(*fv).cast<double>();
+    // Normalize the rays. This is important for Lambda-Twist P3P method.
+    rays.col(ti) = camera.backproject(pixel_coords).normalized();
+    if (ti < 10)
+      SARA_LOGD(logger, "Backproject point {}:\n{} -> {}", ti,
+                pixel_coords.transpose().eval(),
+                rays.col(ti).transpose().eval());
+  }
+
+  SARA_LOGD(logger, "Scene points:\n{}", scene_coords.leftCols(10).eval());
+  SARA_LOGD(logger, "Rays:\n{}", rays.leftCols(10).eval());
+
+
+  // 3. solve the PnP problem with RANSAC.
+  const auto [pose, inliers, sample_best] =
+      estimate_pose(point_ray_pairs, camera, absolute_rotation);
+  SARA_LOGD(logger, "[AbsPoseEst] Pose:\n{}", pose);
+  SARA_LOGD(logger, "[AbsPoseEst] inlier count: {}",
+            inliers.flat_array().count());
+  const auto pose_estimated_successfully =
+      inliers.flat_array().count() >= _ransac_inliers_min;
+
+  return {pose, pose_estimated_successfully};
+}

cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp

@@ -38,10 +38,10 @@ namespace DO::Sara {
     }
 
     //! @brief Set robust estimation parameters.
-    auto
-    set_estimation_params(const PixelUnit error_max = 5._px,
-                          const int ransac_iter_max = 1000,
-                          const double ransac_confidence_min = 0.99) -> void
+    auto set_estimation_params(const PixelUnit error_max = 5._px,
+                               const int ransac_iter_max = 100,
+                               const double ransac_confidence_min = 0.99)
+        -> void
     {
       _inlier_predicate.ε = error_max.value;
       _ransac_iter_max = ransac_iter_max;
@@ -54,12 +54,24 @@ namespace DO::Sara {
                        const CameraIntrinsicModel&)
         -> std::tuple<PoseMatrix, Inlier, MinimalSample>;
 
+    auto estimate_pose(const PointRayCorrespondenceList<double>&,
+                       const CameraIntrinsicModel&,
+                       const Eigen::Matrix3d& absolute_rotation)
+        -> std::tuple<PoseMatrix, Inlier, MinimalSample>;
+
     auto estimate_pose(const std::vector<FeatureTrack>&,
                        const CameraPoseGraph::Vertex,  //
                        const CameraIntrinsicModel&,    //
                        const PointCloudGenerator&)
         -> std::pair<PoseMatrix, bool>;
 
+    auto estimate_pose(const std::vector<FeatureTrack>&,
+                       const CameraPoseGraph::Vertex,  //
+                       const CameraIntrinsicModel&,    //
+                       const PointCloudGenerator&,
+                       const Eigen::Matrix3d& absolute_rotation)
+        -> std::pair<PoseMatrix, bool>;
+
   private:
     P3PSolver<double> _solver;
     CheiralPnPConsistency<CameraIntrinsicModel> _inlier_predicate;

scripts/ubuntu+cuda/notes.md

@@ -56,3 +56,14 @@ N.B.: the notes may not be entirely accurate, so please refer to the Dockerfile:
 
   More info on this page:
   [https://docs.nvidia.com/deeplearning/tensorrt/install-guide/index.html#installing]
+
+
+
+# Desperate commands
+
+```
+dpkg --force-all --configure -a
+dpkg --purge --force-depends libnettle6 (cf. this post)
+apt --fix-broken install
+apt-get -f install
+```