Add on_update_new_frame callback (#145)

koide3 · Feb 10, 2025 · d1c23be · d1c23be
1 parent 359d005
commit d1c23be
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 1 deletion.
diff --git a/include/glim/odometry/callbacks.hpp b/include/glim/odometry/callbacks.hpp
@@ -66,11 +66,18 @@ struct OdometryEstimationCallbacks {
   static CallbackSlot<void(const PreprocessedFrame::Ptr& frame)> on_insert_frame;
 
   /**
-   * @brief New odometry estimation frame creation callback
+   * @brief New odometry estimation frame creation callback (Sensor state predicted with IMU forward integration)
    * @param frame  Odometry estimation frame (deskewed points and initial transformation estimate)
    */
   static CallbackSlot<void(const EstimationFrame::ConstPtr& frame)> on_new_frame;
 
+  /**
+   * @brief New odometry estimation frame update callback (Sensor state corrected with point cloud observation)
+   * @param frame  Updated odometry estimation frame
+   * @note  This is equivalent to `on_update_frames` with only the latest frame
+   */
+  static CallbackSlot<void(const EstimationFrame::ConstPtr& frame)> on_update_new_frame;
+
   /**
    * @brief Odometry estimation frames update callback
    * @param frames  Updated frames

diff --git a/src/glim/odometry/callbacks.cpp b/src/glim/odometry/callbacks.cpp
@@ -12,6 +12,7 @@ CallbackSlot<void(const double, const Eigen::Vector3d&, const Eigen::Vector3d&)>
 CallbackSlot<void(const PreprocessedFrame::Ptr& frame)> OdometryEstimationCallbacks::on_insert_frame;
 
 CallbackSlot<void(const EstimationFrame::ConstPtr&)> OdometryEstimationCallbacks::on_new_frame;
+CallbackSlot<void(const EstimationFrame::ConstPtr&)> OdometryEstimationCallbacks::on_update_new_frame;
 
 CallbackSlot<void(const std::vector<EstimationFrame::ConstPtr>&)> OdometryEstimationCallbacks::on_marginalized_frames;
 CallbackSlot<void(const std::vector<EstimationFrame::ConstPtr>&)> OdometryEstimationCallbacks::on_marginalized_keyframes;

diff --git a/src/glim/odometry/odometry_estimation_ct.cpp b/src/glim/odometry/odometry_estimation_ct.cpp
@@ -269,6 +269,7 @@ EstimationFrame::ConstPtr OdometryEstimationCT::insert_frame(const PreprocessedF
   }
 
   std::vector<EstimationFrame::ConstPtr> active_frames(frames.begin() + marginalized_cursor, frames.end());
+  Callbacks::on_update_new_frame(active_frames.back());
   Callbacks::on_update_frames(active_frames);
 
   // Update target point cloud (just for visualization)

diff --git a/src/glim/odometry/odometry_estimation_imu.cpp b/src/glim/odometry/odometry_estimation_imu.cpp
@@ -337,6 +337,7 @@ EstimationFrame::ConstPtr OdometryEstimationIMU::insert_frame(const Preprocessed
   update_frames(current, new_factors);
 
   std::vector<EstimationFrame::ConstPtr> active_frames(frames.begin() + marginalized_cursor, frames.end());
+  Callbacks::on_update_new_frame(active_frames.back());
   Callbacks::on_update_frames(active_frames);
   logger->trace("frames updated");