stabilize odom visualization

CMakeLists.txt

@@ -49,6 +49,7 @@ add_library(glim SHARED
   src/glim/util/logging.cpp
   src/glim/util/export_factors.cpp
   src/glim/util/time_keeper.cpp
+  src/glim/util/trajectory_manager.cpp
   src/glim/util/extension_module.cpp
   src/glim/util/load_module.cpp
   src/glim/util/data_validator.cpp

include/glim/mapping/sub_map.hpp

@@ -31,6 +31,36 @@ struct SubMap {
   /// @brief Get the origin odometry frame
   EstimationFrame::ConstPtr origin_odom_frame() const { return odom_frames[frames.size() / 2]; }
 
+  /**
+   * @brief Get the custom data and cast it to the specified type.
+   * @note  This method does not check the type of the custom data.
+   * @param key  Key of the custom data
+   * @return T*  Pointer to the custom data. nullptr if not found.
+   */
+  template <typename T>
+  T* get_custom_data(const std::string& key) {
+    const auto found = custom_data.find(key);
+    if (found == custom_data.end()) {
+      return nullptr;
+    }
+    return reinterpret_cast<T*>(found->second.get());
+  }
+
+  /**
+   * @brief Get the custom data and cast it to the specified type.
+   * @note  This method does not check the type of the custom data.
+   * @param key  Key of the custom data
+   * @return T*  Pointer to the custom data. nullptr if not found.
+   */
+  template <typename T>
+  const T* get_custom_data(const std::string& key) const {
+    const auto found = custom_data.find(key);
+    if (found == custom_data.end()) {
+      return nullptr;
+    }
+    return reinterpret_cast<const T*>(found->second.get());
+  }
+
   /**
    * @brief Save the submap
    * @param path  Save path
@@ -57,6 +87,8 @@ struct SubMap {
 
   std::vector<EstimationFrame::ConstPtr> frames;       ///< Optimized odometry frames
   std::vector<EstimationFrame::ConstPtr> odom_frames;  ///< Original odometry frames
+
+  std::unordered_map<std::string, std::shared_ptr<void>> custom_data;  ///< User-defined custom data
 };
 
 }  // namespace  glim

include/glim/util/trajectory_manager.hpp

@@ -10,39 +10,19 @@ namespace glim {
 
 class TrajectoryManager {
 public:
-  TrajectoryManager() {
-    odom_stamps.push_back(0.0);
-    T_odom_sensor.push_back(Eigen::Isometry3d::Identity());
+  TrajectoryManager();
+  ~TrajectoryManager();
 
-    T_world_odom.setIdentity();
-  }
-  ~TrajectoryManager() {}
+  void add_odom(double stamp, const Eigen::Isometry3d& T_odom_sensor, int priority = 1);
+  void update_anchor(double stamp, const Eigen::Isometry3d& T_world_sensor);
 
-  void add_odom(double stamp, const Eigen::Isometry3d& T_odom_sensor) {
-    this->odom_stamps.push_back(stamp);
-    this->T_odom_sensor.push_back(T_odom_sensor);
-  }
-
-  void update_anchor(double stamp, const Eigen::Isometry3d& T_world_sensor) {
-    auto found = std::lower_bound(odom_stamps.begin(), odom_stamps.end(), stamp);
-    int idx = std::distance(odom_stamps.begin(), found);
-    T_world_odom = T_world_sensor * T_odom_sensor[idx].inverse();
-
-    if (idx > 1) {
-      odom_stamps.erase(odom_stamps.begin(), odom_stamps.begin() + idx - 1);
-      T_odom_sensor.erase(T_odom_sensor.begin(), T_odom_sensor.begin() + idx - 1);
-    }
-  }
-
-  Eigen::Isometry3d current_pose() const { return T_world_odom * T_odom_sensor.back(); }
-
-  Eigen::Isometry3d odom2world(const Eigen::Isometry3d& pose) const { return T_world_odom * pose; }
-
-  Eigen::Vector3d odom2world(const Eigen::Vector3d& point) const { return T_world_odom * point; }
-
-  const Eigen::Isometry3d get_T_world_odom() const { return T_world_odom; }
+  Eigen::Isometry3d current_pose() const;
+  Eigen::Isometry3d odom2world(const Eigen::Isometry3d& pose) const;
+  Eigen::Vector3d odom2world(const Eigen::Vector3d& point) const;
+  const Eigen::Isometry3d get_T_world_odom() const;
 
 private:
+  int odom_priority;
   std::vector<double> odom_stamps;
   std::vector<Eigen::Isometry3d> T_odom_sensor;
 

src/glim/util/trajectory_manager.cpp

@@ -0,0 +1,74 @@
+#include <glim/util/trajectory_manager.hpp>
+
+namespace glim {
+
+TrajectoryManager::TrajectoryManager() {
+  odom_priority = -1;
+  odom_stamps.push_back(0.0);
+  T_odom_sensor.push_back(Eigen::Isometry3d::Identity());
+
+  T_world_odom.setIdentity();
+}
+
+TrajectoryManager::~TrajectoryManager() {}
+
+void TrajectoryManager::add_odom(double stamp, const Eigen::Isometry3d& T_odom_sensor, int priority) {
+  if (odom_priority < priority) {
+    odom_priority = priority;
+    this->odom_stamps = {0.0};
+    this->T_odom_sensor = {T_odom_sensor};
+  } else if (odom_priority != priority) {
+    return;
+  }
+
+  this->odom_stamps.push_back(stamp);
+  this->T_odom_sensor.push_back(T_odom_sensor);
+}
+
+void TrajectoryManager::update_anchor(double stamp, const Eigen::Isometry3d& T_world_sensor) {
+  const auto found = std::lower_bound(odom_stamps.begin(), odom_stamps.end(), stamp);
+  const int idx = std::distance(odom_stamps.begin(), found);
+
+  if (std::abs(stamp - odom_stamps[idx]) < 1e-6 || idx == 0) {
+    T_world_odom = T_world_sensor * T_odom_sensor[idx].inverse();
+  } else {
+    const double t0 = odom_stamps[idx - 1];
+    const double t1 = odom_stamps[idx];
+    if (t0 > stamp || t1 < stamp) {
+      T_world_odom = T_world_sensor * T_odom_sensor[idx].inverse();
+      return;
+    }
+
+    const double p = (stamp - t0) / (t1 - t0);
+
+    const Eigen::Isometry3d T0 = T_odom_sensor[idx - 1];
+    const Eigen::Isometry3d T1 = T_odom_sensor[idx];
+    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
+    T.translation() = T0.translation() * (1.0 - p) + T1.translation() * p;
+    T.linear() = Eigen::Quaterniond(T0.linear()).slerp(p, Eigen::Quaterniond(T1.linear())).toRotationMatrix();
+    T_world_odom = T_world_sensor * T.inverse();
+  }
+
+  if (idx > 1) {
+    odom_stamps.erase(odom_stamps.begin(), odom_stamps.begin() + idx - 1);
+    T_odom_sensor.erase(T_odom_sensor.begin(), T_odom_sensor.begin() + idx - 1);
+  }
+}
+
+Eigen::Isometry3d TrajectoryManager::current_pose() const {
+  return T_world_odom * T_odom_sensor.back();
+}
+
+Eigen::Isometry3d TrajectoryManager::odom2world(const Eigen::Isometry3d& pose) const {
+  return T_world_odom * pose;
+}
+
+Eigen::Vector3d TrajectoryManager::odom2world(const Eigen::Vector3d& point) const {
+  return T_world_odom * point;
+}
+
+const Eigen::Isometry3d TrajectoryManager::get_T_world_odom() const {
+  return T_world_odom;
+}
+
+}  // namespace glim

src/glim/viewer/interactive_viewer.cpp

@@ -384,7 +384,7 @@ void InteractiveViewer::odometry_on_new_frame(const EstimationFrame::ConstPtr& n
     auto viewer = guik::viewer();
     auto cloud_buffer = std::make_shared<glk::PointCloudBuffer>(new_frame->frame->points, new_frame->frame->size());
 
-    trajectory->add_odom(new_frame->stamp, new_frame->T_world_sensor());
+    trajectory->add_odom(new_frame->stamp, new_frame->T_world_sensor(), 1);
     const Eigen::Isometry3d pose = trajectory->odom2world(new_frame->T_world_sensor());
     viewer->update_drawable("current", cloud_buffer, guik::FlatOrange(pose).set_point_scale(2.0f));
   });

src/glim/viewer/standard_viewer.cpp

@@ -139,7 +139,7 @@ void StandardViewer::set_callbacks() {
       last_imu_vel = new_frame->v_world_imu;
       last_imu_bias = new_frame->imu_bias;
 
-      trajectory->add_odom(new_frame->stamp, new_frame->T_world_sensor());
+      trajectory->add_odom(new_frame->stamp, new_frame->T_world_sensor(), 1);
       const Eigen::Isometry3f pose = resolve_pose(new_frame);
 
       if (track) {
@@ -226,7 +226,13 @@ void StandardViewer::set_callbacks() {
     std::vector<int> marginalized_ids(frames.size());
     std::transform(frames.begin(), frames.end(), marginalized_ids.begin(), [](const EstimationFrame::ConstPtr& frame) { return frame->id; });
 
-    invoke([this, marginalized_ids] {
+    const EstimationFrame::ConstPtr last_frame = frames.empty() ? nullptr : frames.back();
+
+    invoke([this, marginalized_ids, last_frame] {
+      if (last_frame) {
+        trajectory->add_odom(last_frame->stamp, last_frame->T_world_sensor(), 2);
+      }
+
       auto viewer = guik::LightViewer::instance();
       for (const int id : marginalized_ids) {
         viewer->remove_drawable("frame_" + std::to_string(id));