From cb4118b01683424d16bbb30f9f700fa5f208961b Mon Sep 17 00:00:00 2001
From: David OK <david.ok8@gmail.com>
Date: Tue, 30 Apr 2024 17:39:22 +0100
Subject: [PATCH] WIP: save work.

---
 .../MinimalSolvers/P3PSolver.hpp              | 19 ++++++++++---------
 .../BuildingBlocks/CameraPoseEstimator.hpp    | 16 ++++++++--------
 .../DO/Sara/SfM/Odometry/OdometryPipeline.cpp | 15 ++++++++++++---
 3 files changed, 30 insertions(+), 20 deletions(-)

diff --git a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp
index ef86fab35..d7712200e 100644
--- a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp
+++ b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp
@@ -11,12 +11,11 @@
 
 #pragma once
 
+#include <DO/Sara/Core/EigenFormatInterop.hpp>
 #include <DO/Sara/Core/Math/UsualFunctions.hpp>
 #include <DO/Sara/MultiViewGeometry/PnP/LambdaTwist.hpp>
 #include <DO/Sara/MultiViewGeometry/PointRayCorrespondenceList.hpp>
 
-#include <fmt/format.h>
-
 
 namespace DO::Sara {
 
@@ -30,9 +29,9 @@ namespace DO::Sara {
     using data_point_type = std::array<TensorView_<T, 2>, 2>;
     using model_type = Eigen::Matrix<T, 3, 4>;
 
-    inline auto
-    operator()(const tensor_view_type& scene_points,
-               const tensor_view_type& rays) const -> std::vector<model_type>
+    inline auto operator()(const tensor_view_type& scene_points,
+                           const tensor_view_type& rays) const
+        -> std::vector<model_type>
     {
       const auto sp_mat_ = scene_points.colmajor_view().matrix();
 
@@ -108,6 +107,7 @@ namespace DO::Sara {
             "The dimension of scene points is incorrect. "
             "They must either 3D (Euclidean) or 4D (homogeneous)!"};
 
+#if defined(USE_CALIBRATION_MATRIX)
       // Project the camera coordinates to the image plane.
       //
       // The result is a list of pixel coordinates.
@@ -125,6 +125,10 @@ namespace DO::Sara {
       auto u2 = Eigen::MatrixXd{2, rays.cols()};
       for (auto i = 0; i < u2.cols(); ++i)
         u2.col(i) = C->project(rays.col(i));
+#else
+      const Eigen::MatrixXd u1 = X_camera.colwise().hnormalized();
+      const Eigen::MatrixXd u2 = rays.colwise().hnormalized();
+#endif
 
       // Check the cheirality w.r.t. the candidate pose.
       const auto cheiral = X_camera.row(2).array() > 0;
@@ -134,7 +138,7 @@ namespace DO::Sara {
       const auto ε_squared = (u2 - u1).colwise().squaredNorm().array();
       const auto ε_small = ε_squared < ε_max;
 
-#if 0
+#if defined(CHECK_PNP_RESIDUALS)
       fmt::print("Pose:\n{}\n", T);
       const auto ε_debug = Eigen::VectorXd{ε_squared.sqrt()};
       const auto col_max = std::min(Eigen::Index{10}, u2.cols());
@@ -162,7 +166,4 @@ namespace DO::Sara {
     }
   };
 
-
-  //! @}
-
 }  // namespace DO::Sara
diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp
index 1bb10f91d..e04332ead 100644
--- a/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp
+++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp
@@ -39,11 +39,11 @@ namespace DO::Sara {
 
     //! @brief Set robust estimation parameters.
     auto
-    set_estimation_params(const PixelUnit error_max = 5._px,
-                          const int ransac_iter_max = 10000,
+    set_estimation_params([[maybe_unused]] const PixelUnit error_max = 5._px,
+                          const int ransac_iter_max = 1000,
                           const double ransac_confidence_min = 0.99) -> void
     {
-      _inlier_predicate.ε = error_max.value;
+      _inlier_predicate.ε = 0.1;  // error_max.value;
       _ransac_iter_max = ransac_iter_max;
       _ransac_confidence_min = ransac_confidence_min;
     }
@@ -54,11 +54,11 @@ namespace DO::Sara {
                        const CameraIntrinsicModel&)
         -> 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&)
+        -> std::pair<PoseMatrix, bool>;
 
   private:
     P3PSolver<double> _solver;
diff --git a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp
index db5616a0a..0c3ecc5e4 100644
--- a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp
+++ b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp
@@ -315,9 +315,18 @@ auto 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   = {: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);
+  SARA_LOGI(logger, "[Rel] Global yaw   = {:0.3f} deg", angles(0) * degrees);
+  SARA_LOGI(logger, "[Rel] Global pitch = {:0.3f} deg", angles(1) * degrees);
+  SARA_LOGI(logger, "[Rel] Global roll  = {:0.3f} deg", angles(2) * degrees);
+
+
+  const auto R_abs = _pose_graph[_pose_curr].pose.q.toRotationMatrix();
+  const auto q_global_2 =
+      Eigen::Quaterniond{P * R_abs.transpose() * P.transpose()};
+  angles = calculate_yaw_pitch_roll(q_global_2);
+  SARA_LOGI(logger, "[Rel] Global yaw   = {:0.3f} deg", angles(0) * degrees);
+  SARA_LOGI(logger, "[Rel] Global pitch = {:0.3f} deg", angles(1) * degrees);
+  SARA_LOGI(logger, "[Rel] Global roll  = {:0.3f} deg", angles(2) * degrees);
 
   return true;
 }