Merge pull request #383 from oddkiva/maint-tweaks

MAINT: tweaks

cpp/examples/Sara/MultiViewGeometry/two_view_bundle_adjustment_example.cpp

@@ -215,7 +215,7 @@ GRAPHICS_MAIN()
 
   // 3. Recalculate the feature tracks that are still alive.
   const auto [tracks, track_visibility_count] =
-      feature_tracker.calculate_alive_feature_tracks(1);
+      feature_tracker.find_feature_tracks_at_pose(1);
 
   auto point_cloud = sara::PointCloudGenerator::PointCloud{};
   auto point_cloud_generator = sara::PointCloudGenerator{

cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp

@@ -104,9 +104,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);
+    // _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);
@@ -466,15 +465,20 @@ class SingleWindowApp
   Eigen::Matrix4f _point_cloud_projection;
   //! @brief Camera of the point cloud scene.
   k::Camera _point_cloud_camera;
+  //! @{
+  //! @brief Viewing parameters.
   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/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) {
@@ -392,7 +392,8 @@ 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.
+    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,  //
@@ -474,11 +475,16 @@ auto PointCloudGenerator::write_ply(const std::filesystem::path& out_ply) const
     -> void
 {
   auto coords = std::vector<Eigen::Vector3d>(_point_cloud.size());
-  auto colors = std::vector<Eigen::Vector3d>(_point_cloud.size());
+  auto colors = std::vector<Eigen::Vector3<std::uint8_t>>(_point_cloud.size());
   std::transform(_point_cloud.begin(), _point_cloud.end(), coords.begin(),
                  [](const ScenePoint& x) { return x.coords(); });
   std::transform(_point_cloud.begin(), _point_cloud.end(), colors.begin(),
-                 [](const ScenePoint& x) { return x.color(); });
+                 [](const ScenePoint& x) -> Eigen::Vector3<std::uint8_t> {
+                   Eigen::Vector3d x255 = x.color() * 255;
+                   for (auto i = 0; i < 3; ++i)
+                     x255(i) = std::clamp(x255(i), 0., 255.);
+                   return x255.cast<std::uint8_t>();
+                 });
 
   auto fb = std::filebuf{};
   fb.open(out_ply, std::ios::out);
@@ -493,7 +499,7 @@ auto PointCloudGenerator::write_ply(const std::filesystem::path& out_ply) const
                                 tinyply::Type::INVALID, 0);
 
   ply.add_properties_to_element("vertex", {"red", "green", "blue"},
-                                tinyply::Type::FLOAT64, colors.size(),
+                                tinyply::Type::UINT8, colors.size(),
                                 reinterpret_cast<std::uint8_t*>(colors.data()),
                                 tinyply::Type::INVALID, 0);
 

cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp

@@ -129,8 +129,8 @@ auto FeatureTracker::update_feature_tracks(
             _feature_tracks.size());
 }
 
-auto FeatureTracker::calculate_alive_feature_tracks(
-    const CameraPoseGraph::Vertex camera_vertex_curr) const
+auto FeatureTracker::find_feature_tracks_at_pose(
+    const CameraPoseGraph::Vertex pose_vertex) const
     -> std::tuple<TrackArray, TrackVisibilityCountArray>
 {
   auto& logger = Logger::get();
@@ -139,23 +139,23 @@ auto FeatureTracker::calculate_alive_feature_tracks(
   const FeatureGraph::Impl& fgraph = _feature_graph;
 
   const auto& ftracks = _feature_tracks;
-  auto tracks_alive = TrackArray{};
+  auto tracks_filtered = TrackArray{};
   auto track_visibility_count = TrackVisibilityCountArray{};
 
   for (const auto& ftrack : ftracks)
   {
     // Do we still see the track in the image.
     const auto is_alive =
         std::find_if(ftrack.begin(), ftrack.end(),
-                     [&fgraph, camera_vertex_curr](const auto& v) {
-                       return fgraph[v].pose_vertex == camera_vertex_curr;
+                     [&fgraph, pose_vertex](const auto& v) {
+                       return fgraph[v].pose_vertex == pose_vertex;
                      }) != ftrack.end();
 
     if (!is_alive)
       continue;
 
     // Add the newly found alive track.
-    tracks_alive.push_back(ftrack);
+    tracks_filtered.push_back(ftrack);
 
     // Carefully count the track life, it's not the number of vertices, but
     // the number of camera views in which the feature reappears.
@@ -167,7 +167,7 @@ auto FeatureTracker::calculate_alive_feature_tracks(
                    [&fgraph](const auto& v) { return fgraph[v].pose_vertex; });
     track_visibility_count.push_back(camera_vertices_where_present.size());
   }
-  SARA_LOGD(logger, "Num tracks alive: {}", tracks_alive.size());
+  SARA_LOGD(logger, "Num tracks alive: {}", tracks_filtered.size());
 
   const auto longest_track_alive = std::max_element(
       track_visibility_count.begin(), track_visibility_count.end());
@@ -186,5 +186,5 @@ auto FeatureTracker::calculate_alive_feature_tracks(
 #endif
   }
 
-  return std::make_tuple(tracks_alive, track_visibility_count);
+  return std::make_tuple(tracks_filtered, track_visibility_count);
 }

cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp

@@ -44,7 +44,7 @@ namespace DO::Sara {
     auto update_feature_tracks(const CameraPoseGraph&,
                                const CameraPoseGraph::Edge) -> void;
 
-    auto calculate_alive_feature_tracks(
+    auto find_feature_tracks_at_pose(
         const CameraPoseGraph::Vertex last_pose_vertex) const
         -> std::tuple<TrackArray, TrackVisibilityCountArray>;
   };

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

@@ -19,6 +19,7 @@
 #include <DO/Sara/SfM/Helpers/Utilities.hpp>
 
 
+// #define USE_ABSOLUTE_ROTATION
 // #define DEBUG_ABSOLUTE_POSE_RECOVERY
 
 
@@ -218,7 +219,7 @@ auto OdometryPipeline::grow_geometry() -> bool
 
   // 3. Recalculate the feature tracks that are still alive.
   std::tie(_tracks_alive, _track_visibility_count) =
-      _feature_tracker.calculate_alive_feature_tracks(_pose_curr);
+      _feature_tracker.find_feature_tracks_at_pose(_pose_curr);
 
 #if defined(DEBUG_ABSOLUTE_POSE_RECOVERY)
   const auto corr_csv_fp = fmt::format("corr_{}.csv", _pose_curr);
@@ -254,7 +255,6 @@ auto OdometryPipeline::grow_geometry() -> bool
              _tracks_alive_without_scene_point) =
         _point_cloud_generator->split_by_scene_point_knowledge(_tracks_alive);
 
-// #define USE_ABSOLUTE_ROTATION
 #if defined(USE_ABSOLUTE_ROTATION)
     const auto [abs_pose_mat, abs_pose_est_successful] =
         _abs_pose_estimator.estimate_pose(
@@ -282,16 +282,10 @@ auto OdometryPipeline::grow_geometry() -> bool
   }
 
   // 8. Grow point cloud by triangulation.
-  //
-  // TODO: don't add 3D scene points that are too far, like points in the sky
   const auto frame_corrected = _distortion_corrector->frame_rgb8();
   _point_cloud_generator->grow_point_cloud(_tracks_alive_without_scene_point,
                                            frame_corrected, pose_edge,
                                            _camera_corrected);
-#if defined(DEBUG_ABSOLUTE_POSE_RECOVERY)
-  const auto scene_csv_fp = fmt::format("scene_{}.csv", _pose_curr);
-  _point_cloud_generator->write_point_cloud(_tracks_alive, scene_csv_fp);
-#endif
 
   adjust_bundles();
 
@@ -308,7 +302,11 @@ auto OdometryPipeline::grow_geometry() -> bool
   //
   // ---------------------------------------------------------------------------
 
-  _point_cloud_generator->write_ply(fmt::format("scene_{:04d}.ply", _pose_curr));
+#if defined(DEBUG_ABSOLUTE_POSE_RECOVERY)
+  const auto scene_ply = fmt::format("scene_{:04d}.ply", _pose_curr);
+  SARA_LOGI(logger, "Saving PLY {}", scene_ply);
+  _point_cloud_generator->write_ply(scene_ply);
+#endif
 
   return true;
 }