WIP: add bundle adjustment step.

cpp/examples/Sara/MultiViewGeometry/two_view_bundle_adjustment_example.cpp

@@ -247,23 +247,21 @@ GRAPHICS_MAIN()
     tracks_filtered.emplace_back(std::move(track_filtered));
   }
 
-  auto ba_problem = sara::BundleAdjuster{};
+  auto ba = sara::BundleAdjuster{};
   static constexpr auto intrinsics_dim = 4;  // fx, fy, u0, v0
   static constexpr auto extrinsics_dim = 6;
-  ba_problem.form_problem(pose_graph, feature_tracker, K, point_cloud_generator,
-                          tracks_filtered, intrinsics_dim, extrinsics_dim);
+  ba.form_problem(pose_graph, feature_tracker, K, point_cloud_generator,
+                  tracks_filtered, intrinsics_dim, extrinsics_dim);
   // Freeze all the intrinsic parameters during the optimization.
   SARA_LOGI(logger, "Freezing intrinsic camera parameters...");
   for (auto v = 0; v < 2; ++v)
-    ba_problem.problem->SetParameterBlockConstant(
-        ba_problem.data.intrinsics[v].data());
+    ba.problem->SetParameterBlockConstant(ba.data.intrinsics[v].data());
 
   // Freeze the first absolute pose parameters.
   SARA_LOGI(logger, "Freezing first absolute pose...");
-  ba_problem.problem->SetParameterBlockConstant(
-      ba_problem.data.extrinsics[0].data());
+  ba.problem->SetParameterBlockConstant(ba.data.extrinsics[0].data());
 
-  const auto& ba_data = ba_problem.data;
+  const auto& ba_data = ba.data;
   SARA_LOGI(
       logger, "[BA][BEFORE] points =\n{}",
       Eigen::MatrixXd{ba_data.point_coords.matrix().topRows<20>().eval()});
@@ -277,7 +275,7 @@ GRAPHICS_MAIN()
   }
 
   // Solve the BA.
-  ba_problem.solve();
+  ba.solve();
 
   SARA_LOGI(logger, "Checking the BA...");
   SARA_LOGI(logger, "[BA][AFTER ] camera_parameters =\n{}",

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

@@ -66,6 +66,15 @@ namespace DO::Sara {
       return feature(u).center();
     }
 
+    auto scene_point_index(const FeatureVertex u) const
+        -> std::optional<ScenePointIndex>
+    {
+      const auto it = _from_vertex_to_scene_point_index.find(u);
+      if (it == _from_vertex_to_scene_point_index.end())
+        return std::nullopt;
+      return it->second;
+    }
+
     auto scene_point(const FeatureVertex u) const -> std::optional<ScenePoint>
     {
       const auto it = _from_vertex_to_scene_point_index.find(u);
@@ -82,12 +91,11 @@ 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.
@@ -113,8 +121,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.

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

@@ -296,6 +296,8 @@ auto OdometryPipeline::grow_geometry() -> bool
   _point_cloud_generator->write_point_cloud(_tracks_alive, scene_csv_fp);
 #endif
 
+  this->adjust_bundles();
+
   // ---------------------------------------------------------------------------
   // FOR DEBUGGING PURPOSES.
   //
@@ -311,3 +313,92 @@ auto OdometryPipeline::grow_geometry() -> bool
 
   return true;
 }
+
+
+auto OdometryPipeline::adjust_bundles() -> void
+{
+  auto& logger = Logger::get();
+
+  // Filter the feature tracks by NMS.
+  SARA_LOGI(logger, "Collect all tracks that have a scene point...");
+  auto ftracks_filtered = std::vector<FeatureTracker::Track>{};
+  for (const auto& ftrack : _feature_tracker._feature_tracks)
+  {
+    // Does a track have a 3D point? If not, discard it.
+    const auto p = _point_cloud_generator->scene_point(ftrack.front());
+    if (p == std::nullopt)
+      continue;
+
+    // Filter the feature track by NMS: there should be only 1 feature per
+    // image.
+    auto ftrack_filtered = _point_cloud_generator  //
+                               ->filter_by_non_max_suppression(ftrack);
+
+    ftracks_filtered.emplace_back(std::move(ftrack_filtered));
+  }
+
+
+  auto K = Eigen::Matrix3d{};
+  const auto fx = _camera_corrected.fx();
+  const auto fy = _camera_corrected.fy();
+  const auto s = _camera_corrected.shear();
+  const auto u0 = _camera_corrected.u0();
+  const auto v0 = _camera_corrected.v0();
+  if (s != 0)
+    throw std::runtime_error{
+        "Unimplemented: we require the shear value to be 0!"};
+  // clang-format off
+  K <<
+    fx,  0, u0,
+     0, fy, v0,
+     0,  0,  1;
+  // clang-format on
+  const auto Ks = std::vector<Eigen::Matrix3d>(_pose_graph.num_vertices(), K);
+  static constexpr auto intrinsics_dim = 4;  // fx, fy, u0, v0
+  static constexpr auto extrinsics_dim = 6;
+  _bundle_adjuster.form_problem(_pose_graph, _feature_tracker, Ks,
+                                *_point_cloud_generator, ftracks_filtered,
+                                intrinsics_dim, extrinsics_dim);
+
+  // Freeze all the intrinsic parameters during the optimization.
+  SARA_LOGI(logger, "Freezing intrinsic camera parameters...");
+  const auto num_vertices = static_cast<int>(_pose_graph.num_vertices());
+  for (auto v = 0; v < num_vertices; ++v)
+    _bundle_adjuster.problem->SetParameterBlockConstant(
+        _bundle_adjuster.data.intrinsics[v].data());
+
+  // Freeze the first absolute pose parameters.
+  SARA_LOGI(logger, "Freezing first absolute pose...");
+  _bundle_adjuster.problem->SetParameterBlockConstant(
+      _bundle_adjuster.data.extrinsics[0].data());
+
+  // Solve the BA.
+  _bundle_adjuster.solve();
+
+  // Update the absolute poses.
+  const auto& ba_data = _bundle_adjuster.data;
+  for (auto v = 0; v < num_vertices; ++v)
+  {
+    const auto aaxis_3d =
+        ba_data.extrinsics.matrix().row(v).head(3).transpose();
+    const auto angle = aaxis_3d.norm();
+    const auto axis = aaxis_3d.normalized();
+    const auto aaxis = Eigen::AngleAxisd{angle, axis};
+    const auto t = ba_data.extrinsics.matrix().row(v).tail(3).transpose();
+    _pose_graph[v].pose.q = Eigen::Quaterniond{aaxis};
+    _pose_graph[v].pose.t = t;
+  }
+
+  // Update the point cloud.
+  for (const auto& ftrack : ftracks_filtered)
+  {
+    const auto idx = _point_cloud_generator->scene_point_index(ftrack.front());
+    if (idx == std::nullopt)
+      throw std::runtime_error{fmt::format(
+          "Error: the feature vertex {} must have a scene point index!",
+          ftrack.front())};
+
+    const auto i = static_cast<int>(*idx);
+    _point_cloud[i].coords() = ba_data.point_coords[i].vector();
+  }
+}

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

@@ -13,6 +13,7 @@
 
 #include <DO/Sara/Features/KeypointList.hpp>
 #include <DO/Sara/MultiViewGeometry/Camera/v2/PinholeCamera.hpp>
+#include <DO/Sara/SfM/BuildingBlocks/BundleAdjuster.hpp>
 #include <DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp>
 #include <DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp>
 #include <DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.hpp>
@@ -26,9 +27,9 @@ namespace DO::Sara {
   class OdometryPipeline
   {
   public:
-    auto set_config(const std::filesystem::path& video_path,
-                    const v2::BrownConradyDistortionModel<double>& camera)
-        -> void;
+    auto
+    set_config(const std::filesystem::path& video_path,
+               const v2::BrownConradyDistortionModel<double>& camera) -> void;
 
     auto read() -> bool;
 
@@ -45,13 +46,13 @@ namespace DO::Sara {
     auto detect_keypoints(const ImageView<float>&) const
         -> KeypointList<OERegion, float>;
 
-    auto
-    estimate_relative_pose(const KeypointList<OERegion, float>& keys_src,
-                           const KeypointList<OERegion, float>& keys_dst) const
-        -> std::pair<RelativePoseData, TwoViewGeometry>;
+    auto estimate_relative_pose(const KeypointList<OERegion, float>& keys_src,
+                                const KeypointList<OERegion, float>& keys_dst)
+        const -> std::pair<RelativePoseData, TwoViewGeometry>;
 
   private: /* graph update tasks */
     auto grow_geometry() -> bool;
+    auto adjust_bundles() -> void;
 
   public: /* data members */
     VideoStreamer _video_streamer;
@@ -64,6 +65,7 @@ namespace DO::Sara {
     RelativePoseEstimator _rel_pose_estimator;
     CameraPoseEstimator _abs_pose_estimator;
     std::unique_ptr<PointCloudGenerator> _point_cloud_generator;
+    BundleAdjuster _bundle_adjuster;
     //! @}
 
     //! @brief SfM data.