From c436042f09703d474aaa7e9ba9b4d4373bbe98e8 Mon Sep 17 00:00:00 2001
From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com>
Date: Sun, 26 May 2024 20:51:57 +0100
Subject: [PATCH] MAINT: debug PLY file and clean up code.

---
 .../SfM/BuildingBlocks/PointCloudGenerator.cpp | 18 ++++++++++++------
 .../DO/Sara/SfM/Odometry/OdometryPipeline.cpp  | 14 ++++++--------
 2 files changed, 18 insertions(+), 14 deletions(-)

diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp
index 5fd2ec4f3..dfd9e66a4 100644
--- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp
+++ b/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);
 
diff --git a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp
index fe1a05df8..4398f6f3e 100644
--- a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp
+++ b/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
 
 
@@ -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;
 }