Merge branch 'master' into maint-small-improvements

cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp

@@ -10,6 +10,7 @@
 // ========================================================================== //
 
 #include <DO/Kalpana/EasyGL.hpp>
+#include <DO/Kalpana/EasyGL/Objects/Camera.hpp>
 #include <DO/Kalpana/EasyGL/Objects/ColoredPointCloud.hpp>
 #include <DO/Kalpana/EasyGL/Objects/TexturedImage.hpp>
 #include <DO/Kalpana/EasyGL/Objects/TexturedQuad.hpp>
@@ -41,6 +42,9 @@ namespace sara = DO::Sara;
 namespace k = DO::Kalpana;
 namespace kgl = DO::Kalpana::GL;
 
+using sara::operator""_m;
+using sara::operator""_deg;
+
 
 class SingleWindowApp
 {
@@ -101,6 +105,8 @@ class SingleWindowApp
     // Current projection matrix
     _projection = _video_viewport.orthographic_projection();
     _point_cloud_projection = _point_cloud_viewport.orthographic_projection();
+    // _point_cloud_projection =
+    //     _point_cloud_viewport.perspective(120.f, 1e-6f, 1e3f);
 
     // Background color.
     glClearColor(0.2f, 0.3f, 0.3f, 1.0f);
@@ -118,6 +124,9 @@ class SingleWindowApp
     {
       if (!_pause)
       {
+        if (_quit)
+          break;
+
         if (!_pipeline.read())
           break;
 
@@ -135,6 +144,9 @@ class SingleWindowApp
         }
       }
 
+      if (_quit)
+        break;
+
       // Clear the color buffer and the buffer testing.
       glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 
@@ -220,6 +232,9 @@ class SingleWindowApp
 
   auto render_point_cloud() -> void
   {
+    if (_pipeline.point_cloud().empty())
+      return;
+
     glViewport(_point_cloud_viewport.x(), _point_cloud_viewport.y(),
                _point_cloud_viewport.width(), _point_cloud_viewport.height());
 
@@ -233,13 +248,11 @@ class SingleWindowApp
       0,  0, -1;
     // clang-format on
 
-    // Update the model view matrix.
-    const Eigen::Matrix4f model_view = Eigen::Matrix4f::Identity();
-
     // Render the point cloud.
+    const Eigen::Matrix4f model_view_final = _model_view * _scale_mat;
     _point_cloud_renderer.render(_point_cloud, _point_size,
                                  from_cam_to_gl.matrix(),  //
-                                 model_view, _point_cloud_projection);
+                                 model_view_final, _point_cloud_projection);
   }
 
   auto get_framebuffer_sizes() const -> Eigen::Vector2i
@@ -301,6 +314,85 @@ class SingleWindowApp
       std::cout << "RESUME" << std::endl;
       return;
     }
+
+    // Use the escape key to smoothly exit the OpenGL app.
+    if ((action == GLFW_RELEASE || action == GLFW_REPEAT) &&
+        key == GLFW_KEY_ESCAPE)
+    {
+      app._quit = true;
+      return;
+    }
+
+    if (action == GLFW_RELEASE || action == GLFW_REPEAT)
+    {
+      app.move_point_cloud_camera_with_keyboard(key);
+      app.resize_point_size(key);
+      app.change_camera_step_size(key);
+      app.change_model_scale(key);
+      return;
+    }
+  }
+
+  auto move_point_cloud_camera_with_keyboard(const int key) -> void
+  {
+    if (key == GLFW_KEY_W)
+      _point_cloud_camera.move_forward(_delta);
+    if (key == GLFW_KEY_S)
+      _point_cloud_camera.move_backward(_delta);
+    if (key == GLFW_KEY_A)
+      _point_cloud_camera.move_left(_delta);
+    if (key == GLFW_KEY_D)
+      _point_cloud_camera.move_right(_delta);
+
+    if (key == GLFW_KEY_H)
+      _point_cloud_camera.no_head_movement(-_angle_delta);  // CCW
+    if (key == GLFW_KEY_K)
+      _point_cloud_camera.no_head_movement(+_angle_delta);  // CW
+
+    if (key == GLFW_KEY_U)
+      _point_cloud_camera.yes_head_movement(+_angle_delta);
+    if (key == GLFW_KEY_J)
+      _point_cloud_camera.yes_head_movement(-_angle_delta);
+
+    if (key == GLFW_KEY_R)
+      _point_cloud_camera.move_up(_delta);
+    if (key == GLFW_KEY_F)
+      _point_cloud_camera.move_down(_delta);
+
+    if (key == GLFW_KEY_Y)
+      _point_cloud_camera.maybe_head_movement(-_angle_delta);
+    if (key == GLFW_KEY_I)
+      _point_cloud_camera.maybe_head_movement(+_angle_delta);
+
+    _point_cloud_camera.update();
+    _model_view = _point_cloud_camera.view_matrix();
+  }
+
+  auto resize_point_size(const int key) -> void
+  {
+    if (key == GLFW_KEY_MINUS)
+      _point_size /= 1.1f;
+
+    if (key == GLFW_KEY_EQUAL)
+      _point_size *= 1.1f;
+  }
+
+  auto change_camera_step_size(const int key) -> void
+  {
+    if (key == GLFW_KEY_1)
+      _delta /= 1.1f;
+
+    if (key == GLFW_KEY_2)
+      _delta *= 1.1f;
+  }
+
+  auto change_model_scale(const int key) -> void
+  {
+    if (key == GLFW_KEY_Z)
+      _scale_mat.topLeftCorner<3, 3>() /= 1.1f;
+
+    if (key == GLFW_KEY_X)
+      _scale_mat.topLeftCorner<3, 3>() *= 1.1f;
   }
 
 private:
@@ -372,11 +464,17 @@ class SingleWindowApp
   kgl::ColoredPointCloudRenderer _point_cloud_renderer;
   //! @brief Point cloud rendering options.
   Eigen::Matrix4f _point_cloud_projection;
-  // kgl::Camera _point_cloud_camera;
-  float _point_size = 3.f;
+  //! @brief Camera of the point cloud scene.
+  k::Camera _point_cloud_camera;
+  Eigen::Matrix4f _model_view = Eigen::Matrix4f::Identity();
+  Eigen::Matrix4f _scale_mat = Eigen::Matrix4f::Identity();
+  float _point_size = 1.5f;
+  double _delta = (5._m).value;
+  double _angle_delta = (10._deg).value;
 
   //! @brief User interaction.
   bool _pause = false;
+  bool _quit = false;
 };
 
 bool SingleWindowApp::_glfw_initialized = false;

cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp

@@ -25,14 +25,14 @@ namespace DO::Kalpana {
     static constexpr auto YAW = -90.0f;
     static constexpr auto PITCH = 0.0f;
     static constexpr auto SPEED = 1e-1f;
-    static constexpr auto SENSITIVITY = 1e-1f;
+    static constexpr auto SENSITIVITY = 8e-1f;
     static constexpr auto ZOOM = 45.0f;
 
-    Eigen::Vector3f position{10.f * Eigen::Vector3f::UnitY()};
-    Eigen::Vector3f front{-Eigen::Vector3f::UnitZ()};
-    Eigen::Vector3f up{Eigen::Vector3f::UnitY()};
+    Eigen::Vector3f position = Eigen::Vector3f::Zero();
+    Eigen::Vector3f front = -Eigen::Vector3f::UnitZ();
+    Eigen::Vector3f up = Eigen::Vector3f::UnitY();
     Eigen::Vector3f right;
-    Eigen::Vector3f world_up{Eigen::Vector3f::UnitY()};
+    Eigen::Vector3f world_up = Eigen::Vector3f::UnitY();
 
     float yaw{YAW};
     float pitch{PITCH};

cpp/src/DO/Kalpana/EasyGL/Shader.cpp

@@ -219,7 +219,7 @@ namespace DO::Kalpana::GL {
 
     program_object = 0;
 
-    throw std::runtime_error{fmt::format("Failed to delete shader program: %d."
+    throw std::runtime_error{fmt::format("Failed to delete shader program: {}."
                                          "Delete log:\n{}",
                                          success, log)};
   }

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

@@ -172,19 +172,8 @@ auto CameraPoseEstimator::estimate_pose(
     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 0
-    if (ti < 10)
-      SARA_LOGD(logger, "Backproject point {}:\n{} -> {}", ti,
-                pixel_coords.transpose().eval(),
-                rays.col(ti).transpose().eval());
-#endif
   }
 
-#if 0
-  SARA_LOGD(logger, "Scene points:\n{}", scene_coords.leftCols(10).eval());
-  SARA_LOGD(logger, "Rays:\n{}", rays.leftCols(10).eval());
-#endif
-
   // 3. solve the PnP problem with RANSAC.
   const auto [pose, inliers, sample_best] =
       estimate_pose(point_ray_pairs, camera);

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

@@ -7,7 +7,7 @@ namespace DO::Sara {
 
   struct FeatureParams
   {
-    ImagePyramidParams image_pyr_params = ImagePyramidParams(-1);
+    ImagePyramidParams image_pyr_params = ImagePyramidParams(0);
     float sift_nn_ratio = 0.6f;
     std::size_t num_matches_max = 1000u;
     int num_inliers_min = 100;

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

@@ -93,8 +93,8 @@ auto PointCloudGenerator::filter_by_non_max_suppression(
 }
 
 auto PointCloudGenerator::find_feature_vertex_at_pose(
-    const FeatureTrack& track,
-    const PoseVertex pose_vertex) const -> std::optional<FeatureVertex>
+    const FeatureTrack& track, const PoseVertex pose_vertex) const
+    -> std::optional<FeatureVertex>
 {
   auto v = std::find_if(track.begin(), track.end(),
                         [this, pose_vertex](const auto& v) {
@@ -253,6 +253,8 @@ auto PointCloudGenerator::compress_point_cloud(
   auto point_cloud_compressed = std::vector<ScenePoint>{};
   point_cloud_compressed.reserve(tracks.size());
 
+  auto from_vertex_to_scene_point_index_new = FeatureToScenePointMap{};
+
   // Reset the scene point index for each feature track.
   for (auto t = ScenePointIndex{}; t < tracks.size(); ++t)
   {
@@ -265,16 +267,21 @@ auto PointCloudGenerator::compress_point_cloud(
     // Recalculate the scene point index as a barycenter.
     const auto scene_point = barycenter(scene_point_indices);
 
+    // Discard point at infinity.
+    if (scene_point.coords().squaredNorm() > distance_max_squared())
+      continue;
+
     // Reassign the scene point index for the given feature track.
     for (const auto& v : track)
-      _from_vertex_to_scene_point_index[v] = point_cloud_compressed.size();
+      from_vertex_to_scene_point_index_new[v] = point_cloud_compressed.size();
 
     // Only then store the new point coordinates. Otherwise the index is wrong!
     point_cloud_compressed.emplace_back(scene_point);
   }
 
   // Swap the point cloud with the set of barycenters.
   std::swap(_point_cloud, point_cloud_compressed);
+  _from_vertex_to_scene_point_index.swap(from_vertex_to_scene_point_index_new);
 
   return true;
 }
@@ -385,6 +392,9 @@ auto PointCloudGenerator::grow_point_cloud(
 
     // Calculate the scene point.
     const Eigen::Vector3d coords = X.col(j).hnormalized();
+    if (coords.squaredNorm() > distance_max_squared())  // We deem it to be a point at infinity.
+      continue;
+
     const auto color = retrieve_scene_point_color(coords, image,  //
                                                   tsfm_v, camera);
 
@@ -435,7 +445,7 @@ auto PointCloudGenerator::write_point_cloud(
     const std::vector<FeatureTrack>& ftracks,
     const std::filesystem::path& out_csv) const -> void
 {
-  std::ofstream out{out_csv.string()};
+  auto out = std::ofstream{out_csv.string()};
 
   for (const auto& ftrack : ftracks)
   {

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

@@ -11,6 +11,7 @@
 
 #pragma once
 
+#include <DO/Sara/Core/Math/UsualFunctions.hpp>
 #include <DO/Sara/MultiViewGeometry/Camera/v2/PinholeCamera.hpp>
 #include <DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp>
 #include <DO/Sara/SfM/Graph/CameraPoseGraph.hpp>
@@ -48,6 +49,16 @@ namespace DO::Sara {
     {
     }
 
+    auto distance_max() const -> double
+    {
+      return _distance_max;
+    }
+
+    auto distance_max_squared() const -> double
+    {
+      return square(_distance_max);
+    }
+
   public: /* helper feature retrieval methods */
     auto gid(const FeatureVertex u) const -> const FeatureGID&
     {
@@ -91,11 +102,12 @@ namespace DO::Sara {
                                      const PoseVertex) const
         -> std::optional<FeatureVertex>;
 
-    auto retrieve_scene_point_color(
-        const Eigen::Vector3d& scene_point,  //
-        const ImageView<Rgb8>& image,        //
-        const QuaternionBasedPose<double>& pose,
-        const v2::PinholeCamera<double>& camera) const -> Rgb64f;
+    auto
+    retrieve_scene_point_color(const Eigen::Vector3d& scene_point,  //
+                               const ImageView<Rgb8>& image,        //
+                               const QuaternionBasedPose<double>& pose,
+                               const v2::PinholeCamera<double>& camera) const
+        -> Rgb64f;
 
   public: /* data transformation methods */
     //! @brief Calculate the barycentric scene point.
@@ -121,8 +133,8 @@ namespace DO::Sara {
     //! - The scene point is recalculated as a the barycenter of the
     //!   possibly multiple scene points we have found after recalculating the
     //!   feature tracks.
-    auto
-    propagate_scene_point_indices(const std::vector<FeatureTrack>&) -> void;
+    auto propagate_scene_point_indices(const std::vector<FeatureTrack>&)
+        -> void;
 
     //! - The point cloud compression reassigns a unique scene point cloud to
     //!   each feature tracks.
@@ -154,6 +166,10 @@ namespace DO::Sara {
     PointCloud& _point_cloud;
 
     FeatureToScenePointMap _from_vertex_to_scene_point_index;
+
+    //! @brief Even if we don't have a metric reconstruction, this default value
+    //! works well in practice.
+    double _distance_max = +1e3;
   };
 
 }  // namespace DO::Sara

cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp

@@ -221,8 +221,7 @@ auto OdometryPipeline::grow_geometry() -> bool
       _feature_tracker.calculate_alive_feature_tracks(_pose_curr);
 
 #if defined(DEBUG_ABSOLUTE_POSE_RECOVERY)
-  const auto corr_csv_fp =
-      fmt::format("/Users/oddkiva/Desktop/corr_{}.csv", _pose_curr);
+  const auto corr_csv_fp = fmt::format("corr_{}.csv", _pose_curr);
   write_point_correspondences(_pose_graph, _feature_tracker._feature_graph,
                               _tracks_alive, corr_csv_fp);
 #endif
@@ -290,8 +289,7 @@ auto OdometryPipeline::grow_geometry() -> bool
                                            frame_corrected, pose_edge,
                                            _camera_corrected);
 #if defined(DEBUG_ABSOLUTE_POSE_RECOVERY)
-  const auto scene_csv_fp =
-      fmt::format("/Users/oddkiva/Desktop/scene_{}.csv", _pose_curr);
+  const auto scene_csv_fp = fmt::format("scene_{}.csv", _pose_curr);
   _point_cloud_generator->write_point_cloud(_tracks_alive, scene_csv_fp);
 #endif
 
@@ -326,8 +324,13 @@ auto OdometryPipeline::adjust_bundles() -> void
   for (const auto& ftrack : _feature_tracker._feature_tracks)
   {
     // Does a track have a 3D point? If not, discard it.
-    const auto idx = _point_cloud_generator->scene_point_index(ftrack.front());
-    if (idx == std::nullopt)
+    const auto p = _point_cloud_generator->scene_point(ftrack.front());
+    if (p == std::nullopt)
+      continue;
+
+    // Discard point at infinity.
+    if (p->coords().squaredNorm() >
+        _point_cloud_generator->distance_max_squared())
       continue;
 
     // Filter the feature track by NMS: there should be only 1 feature per