WIP: debug code.

oddkiva · Apr 29, 2024 · b9f3a87 · b9f3a87
1 parent 157dcc2
commit b9f3a87
Show file tree

Hide file tree

Showing 5 changed files with 37 additions and 15 deletions.
diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp
@@ -382,14 +382,24 @@ class SingleWindowApp
 bool SingleWindowApp::_glfw_initialized = false;
 
 
-auto main(int const argc, char** const argv) -> int
+auto main([[maybe_unused]] int const argc,
+          [[maybe_unused]] char** const argv) -> int
 {
 #if defined(_OPENMP)
   const auto num_threads = omp_get_max_threads();
   omp_set_num_threads(num_threads);
   Eigen::setNbThreads(num_threads);
 #endif
 
+#define USE_HARDCODED_VIDEO_PATH
+#if defined(USE_HARDCODED_VIDEO_PATH)
+  const auto video_path = fs::path{"/Users/oddkiva/Desktop/datasets/sample-1.mp4"};
+  if (!fs::exists(video_path))
+  {
+    fmt::print("Video {} does not exist", video_path.string());
+    return EXIT_FAILURE;
+  }
+#else
   if (argc < 2)
   {
     std::cout << fmt::format("Usage: {} VIDEO_PATH\n",
@@ -398,6 +408,7 @@ auto main(int const argc, char** const argv) -> int
   }
 
   const auto video_path = fs::path{argv[1]};
+#endif
   auto camera = sara::v2::BrownConradyDistortionModel<double>{};
   {
     camera.fx() = 917.2878392016245;

diff --git a/cpp/src/DO/Sara/Logging/Logger.cpp b/cpp/src/DO/Sara/Logging/Logger.cpp
@@ -11,7 +11,6 @@
 
 namespace logging = boost::log;
 namespace expr = boost::log::expressions;
-namespace src = boost::log::sources;
 namespace sinks = boost::log::sinks;
 
 
@@ -20,6 +19,7 @@ namespace DO::Sara {
   static auto log_formatter(const boost::log::record_view& rec,
                             boost::log::formatting_ostream& strm) -> void
   {
+#if !defined(__APPLE__)
     auto severity = rec[logging::trivial::severity];
     if (severity)
     {
@@ -41,11 +41,11 @@ namespace DO::Sara {
         break;
       }
     }
+#endif
 
     // Get the LineID attribute value and put it into the stream
     strm << logging::extract<unsigned int>("LineID", rec) << ": ";
-    logging::value_ref<std::string> fullpath =
-        logging::extract<std::string>("File", rec);
+    const auto fullpath = logging::extract<std::string>("File", rec);
     strm << boost::filesystem::path(fullpath.get()).filename().string() << ": ";
     strm << logging::extract<std::string>("Function", rec) << ": ";
     strm << logging::extract<int>("Line", rec) << ": ";
@@ -54,9 +54,11 @@ namespace DO::Sara {
     // The simplified syntax is possible if attribute keywords are used.
     strm << "<" << rec[logging::trivial::severity] << "> ";
 
+#if !defined(__APPLE__)
     // Restore the default color
     if (severity)
       strm << "\033[0m";
+#endif
 
     // Finally, put the record message to the stream
     strm << rec[expr::smessage];

diff --git a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/QuaternionBasedPose.hpp b/cpp/src/DO/Sara/MultiViewGeometry/Geometry/QuaternionBasedPose.hpp
@@ -34,6 +34,16 @@ namespace DO::Sara {
       return {.q = Eigen::Quaternion<T>::Identity(),
               .t = Eigen::Vector3<T>::Zero()};
     }
+
+    static inline auto nan() -> QuaternionBasedPose<T>
+    {
+      static constexpr auto nan = std::numeric_limits<T>::quiet_NaN();
+
+      auto r = QuaternionBasedPose<T>{};
+      r.q = Eigen::Quaternion<T>(nan, nan, nan, nan);
+      r.t.fill(nan);
+      return r;
+    }
   };
 
 }  // namespace DO::Sara
diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.cpp
@@ -49,5 +49,5 @@ auto RelativePoseEstimator::estimate_relative_pose(
       std::make_optional(Normalizer<TwoViewGeometry>{_K, _K});
 
   return v2::ransac(X, _solver, _inlier_predicate, ransac_iterations_max,
-                    ransac_confidence, data_normalizer, true);
+                    ransac_confidence, data_normalizer);
 }
diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp
@@ -162,13 +162,12 @@ auto v2::OdometryPipeline::grow_geometry() -> bool
   SARA_LOGI(logger, "[SfM] Relative pose succeeded!");
 
   // 1. Add the absolute pose vertex.
-  auto abs_pose_curr =
-      _pose_graph.num_vertices() == 1
-          ? QuaternionBasedPose<double>{}
-          : QuaternionBasedPose<double>{
-                .q = Eigen::Quaterniond{rel_pose_data.motion.R},
-                .t = rel_pose_data.motion.t  //
-            };
+  auto abs_pose_curr = QuaternionBasedPose<double>::nan();
+  if (_pose_graph.num_vertices() == 1)
+    abs_pose_curr = {
+        .q = Eigen::Quaterniond{rel_pose_data.motion.R},  //
+        .t = rel_pose_data.motion.t                       //
+    };
   auto abs_pose_data = AbsolutePoseData{
       frame_number,             //
       std::move(keys_curr),     //
@@ -263,9 +262,9 @@ auto v2::OdometryPipeline::grow_geometry() -> bool
   const auto q_global = Eigen::Quaterniond{_current_global_rotation};
   auto angles = calculate_yaw_pitch_roll(q_global);
   static constexpr auto degrees = 180. / M_PI;
-  SARA_LOGI(logger, "Global yaw   = {} deg", angles(0) * degrees);
-  SARA_LOGI(logger, "Global pitch = {} deg", angles(1) * degrees);
-  SARA_LOGI(logger, "Global roll  = {} deg", angles(2) * degrees);
+  SARA_LOGI(logger, "Global yaw   = {:0.3f} deg", angles(0) * degrees);
+  SARA_LOGI(logger, "Global pitch = {:0.3f} deg", angles(1) * degrees);
+  SARA_LOGI(logger, "Global roll  = {:0.3f} deg", angles(2) * degrees);
 
   return true;
 }