feat!(pl): Create pose estimate buffer

.github/workflows/on_main.yml

@@ -79,4 +79,4 @@ jobs:
       - name: Github Release
         env:
           GITHUB_TOKEN: ${{ secrets.RELEASE_PLEASE_GITHUB_TOKEN }}
-        run: gh release upload -R ${{ github.repository }} ${{ needs.release.outputs.tag_name }} ./dist/*
+        run: gh release upload -R ${{ github.repository }} ${{ needs.release.outputs.tag_name }} ./dist/*

cpp/bindings/pipeline.h

@@ -21,10 +21,6 @@ class PyPipeline: public evalio::Pipeline {
   NB_TRAMPOLINE(Pipeline, 9);
 
   // Getters
-  const evalio::SE3 pose() override {
-    NB_OVERRIDE_PURE(pose);
-  }
-
   const std::map<std::string, std::vector<evalio::Point>> map() override {
     NB_OVERRIDE_PURE(map);
   }
@@ -56,8 +52,7 @@ class PyPipeline: public evalio::Pipeline {
     NB_OVERRIDE_PURE(add_imu, mm);
   }
 
-  std::map<std::string, std::vector<Point>>
-  add_lidar(evalio::LidarMeasurement mm) override {
+  void add_lidar(evalio::LidarMeasurement mm) override {
     NB_OVERRIDE_PURE(add_lidar, mm);
   }
 }; // namespace evalio
@@ -81,7 +76,31 @@ inline void makeBasePipeline(nb::module_& m) {
       &evalio::Pipeline::version,
       "Version of the pipeline."
     )
-    .def("pose", &evalio::Pipeline::pose, "Most recent pose estimate.")
+    .def(
+      "saved_estimates",
+      &evalio::Pipeline::saved_estimates,
+      "Most recent stamped pose estimates. Will be removed after this method is called."
+    )
+    .def(
+      "saved_features",
+      &evalio::Pipeline::saved_features,
+      "Most recent stamped feature scans. Will be removed after this method is called."
+    )
+    .def(
+      "saved_maps",
+      &evalio::Pipeline::saved_maps,
+      "Most recent stamped maps. Will be removed after this method is called."
+    )
+    .def(
+      "saved_features_matrix",
+      &evalio::Pipeline::saved_features_matrix,
+      "Most recent stamped feature scans as matrices. Will be removed after this method is called."
+    )
+    .def(
+      "saved_maps_matrix",
+      &evalio::Pipeline::saved_maps_matrix,
+      "Most recent stamped maps as matrices. Will be removed after this method is called."
+    )
     .def("map", &evalio::Pipeline::map, "Map of the environment.")
     .def(
       "initialize",
@@ -126,6 +145,31 @@ inline void makeBasePipeline(nb::module_& m) {
       "T"_a,
       "Set the transformation from IMU to LiDAR frame."
     )
+    .def(
+      "set_visualizing",
+      &evalio::Pipeline::set_visualizing,
+      "visualize"_a.none(),
+      "Enable or disable visualization."
+    )
+    .def(
+      "save",
+      nb::overload_cast<const evalio::Stamp&, const evalio::SE3&>(
+        &evalio::Pipeline::save<const evalio::SE3&>
+      ),
+      "stamp"_a,
+      "pose"_a,
+      "Save the current pose estimate at the given timestamp."
+    )
+    // Use a lambda here, as nb::overload_cast has issues finding the non-template methods
+    .def(
+      "save",
+      [](Pipeline& self, const Stamp& stamp, const Map<>& features) {
+        self.save(stamp, features);
+      },
+      "stamp"_a,
+      "features"_a,
+      "Save the current feature map at the given timestamp."
+    )
     .doc() =
     "Base class for all pipelines. This class defines the interface "
     "for interacting with pipelines, and is intended to be "

cpp/bindings/pipelines/ct_icp.h

@@ -204,14 +204,8 @@ class CTICP: public ev::Pipeline {
   // clang-format on
 
   // Getters
-  const ev::SE3 pose() override {
-    const auto pose = ct_icp_->Trajectory().back();
-    return ev::convert<ev::SE3>(pose) * lidar_T_imu_;
-  }
-
   const std::map<std::string, std::vector<ev::Point>> map() override {
-    auto map = ct_icp_->GetLocalMap();
-    return ev::convert_map<decltype(map)>({{"planar", map}});
+    return ev::make_map("planar", ct_icp_->GetLocalMap());
   }
 
   // Setters
@@ -233,8 +227,7 @@ class CTICP: public ev::Pipeline {
 
   void add_imu(ev::ImuMeasurement mm) override {}
 
-  std::map<std::string, std::vector<ev::Point>>
-  add_lidar(ev::LidarMeasurement mm) override {
+  void add_lidar(ev::LidarMeasurement mm) override {
     // Convert
     auto pc = ev::convert_iter<std::vector<ct_icp::Point3D>>(mm.points);
 
@@ -260,11 +253,14 @@ class CTICP: public ev::Pipeline {
 
     // Run through pipeline
     const auto summary = ct_icp_->RegisterFrame(pc);
-    scan_idx_++;
 
-    // Return the used points
-    return ev::convert_map<std::vector<ct_icp::Point3D>>(
-      {{"planar", summary.keypoints}}
-    );
+    // Save the estimate
+    const auto pose = ct_icp_->Trajectory().back();
+    this->save(mm.stamp, ev::convert<ev::SE3>(pose) * lidar_T_imu_);
+
+    // Save the used points
+    this->save(mm.stamp, "planar", summary.keypoints);
+
+    scan_idx_++;
   }
 };

cpp/bindings/pipelines/genz_icp.h

@@ -52,16 +52,8 @@ class GenZICP: public ev::Pipeline {
   // clang-format on
 
   // Getters
-  const ev::SE3 pose() override {
-    const auto pose =
-      !genz_icp_->poses().empty() ? genz_icp_->poses().back() : Sophus::SE3d();
-    return ev::convert<ev::SE3>(pose * lidar_T_imu_);
-  }
-
   const std::map<std::string, std::vector<ev::Point>> map() override {
-    return ev::convert_map<std::vector<Eigen::Vector3d>>(
-      {{"map", genz_icp_->LocalMap()}}
-    );
+    return ev::make_map("map", genz_icp_->LocalMap());
   }
 
   // Setters
@@ -84,34 +76,35 @@ class GenZICP: public ev::Pipeline {
 
   void add_imu(ev::ImuMeasurement mm) override {}
 
-  std::map<std::string, std::vector<ev::Point>>
-  add_lidar(ev::LidarMeasurement mm) override {
+  void add_lidar(ev::LidarMeasurement mm) override {
     // Set everything up
     auto points = ev::convert_iter<std::vector<Eigen::Vector3d>>(mm.points);
     auto timestamps = ev::convert_iter<std::vector<double>>(mm.points);
 
     // Run through pipeline
     auto [planar, nonplanar] = genz_icp_->RegisterFrame(points, timestamps);
-    auto lidar_T_world = genz_icp_->poses().back().inverse();
+    auto world_T_lidar = genz_icp_->poses().back();
+    auto lidar_T_world = world_T_lidar.inverse();
+
+    // Save the estimate
+    this->save(mm.stamp, world_T_lidar * lidar_T_imu_);
 
     // These are all in the global frame, so we need to convert them
     std::transform(
       planar.begin(),
       planar.end(),
       planar.begin(),
-      [&](auto point) { return lidar_T_imu_ * point; }
+      [&](auto point) { return lidar_T_world * point; }
     );
     std::transform(
       nonplanar.begin(),
       nonplanar.end(),
       nonplanar.begin(),
-      [&](auto point) { return lidar_T_imu_ * point; }
+      [&](auto point) { return lidar_T_world * point; }
     );
 
-    // Return the used points
-    return ev::convert_map<std::vector<Eigen::Vector3d>>(
-      {{"planar", planar}, {"nonplanar", nonplanar}}
-    );
+    // Save the used points
+    this->save(mm.stamp, "planar", planar, "nonplanar", nonplanar);
   }
 
 private:

cpp/bindings/pipelines/kiss_icp.h

@@ -39,15 +39,8 @@ class KissICP: public ev::Pipeline {
   );
 
   // Getters
-  const ev::SE3 pose() override {
-    const Sophus::SE3d pose = kiss_icp_->pose() * lidar_T_imu_;
-    return ev::convert<ev::SE3>(pose);
-  }
-
   const std::map<std::string, std::vector<ev::Point>> map() override {
-    return ev::convert_map<std::vector<Eigen::Vector3d>>(
-      {{"point", kiss_icp_->LocalMap()}}
-    );
+    return ev::make_map("point", kiss_icp_->LocalMap());
   }
 
   // Setters
@@ -69,19 +62,19 @@ class KissICP: public ev::Pipeline {
 
   void add_imu(ev::ImuMeasurement mm) override {}
 
-  std::map<std::string, std::vector<ev::Point>>
-  add_lidar(ev::LidarMeasurement mm) override {
+  void add_lidar(ev::LidarMeasurement mm) override {
     // Convert inputs
     auto points = ev::convert_iter<std::vector<Eigen::Vector3d>>(mm.points);
     auto timestamps = ev::convert_iter<std::vector<double>>(mm.points);
 
     // Run through pipeline
     const auto& [_, used_points] = kiss_icp_->RegisterFrame(points, timestamps);
 
-    // Convert outputs
-    return ev::convert_map<std::vector<Eigen::Vector3d>>(
-      {{"point", used_points}}
-    );
+    // Save the estimate
+    this->save(mm.stamp, kiss_icp_->pose() * lidar_T_imu_);
+
+    // Save features
+    this->save(mm.stamp, "point", used_points);
   }
 
 private:

cpp/bindings/pipelines/lio_sam.h

@@ -35,14 +35,14 @@ inline Point convert(const lio_sam::PointType& in) {
 
 template<>
 inline lio_sam::PointXYZIRT convert(const ev::Point& in) {
-  return lio_sam::PointXYZIRT {
-    .x = static_cast<float>(in.x),
-    .y = static_cast<float>(in.y),
-    .z = static_cast<float>(in.z),
-    .intensity = static_cast<float>(in.intensity),
-    .ring = static_cast<uint16_t>(in.row),
-    .time = static_cast<float>(in.t.to_sec()),
-  };
+  lio_sam::PointXYZIRT out;
+  out.x = static_cast<float>(in.x);
+  out.y = static_cast<float>(in.y);
+  out.z = static_cast<float>(in.z);
+  out.intensity = static_cast<float>(in.intensity);
+  out.ring = static_cast<uint16_t>(in.row);
+  out.time = static_cast<float>(in.t.to_sec());
+  return out;
 }
 
 // IMU conversions
@@ -117,14 +117,8 @@ class LioSam: public ev::Pipeline {
   // clang-format on
 
   // Getters
-  const ev::SE3 pose() override {
-    return ev::convert<ev::SE3>(lio_sam_->getPose()) * lidar_T_imu_;
-  }
-
   const std::map<std::string, std::vector<ev::Point>> map() override {
-    return ev::convert_map<pcl::PointCloud<lio_sam::PointType>>(
-      {{"point", *lio_sam_->getMap()}}
-    );
+    return ev::make_map("map", *lio_sam_->getMap());
   }
 
   // Setters
@@ -158,8 +152,7 @@ class LioSam: public ev::Pipeline {
     lio_sam_->addImuMeasurement(ev::convert<lio_sam::Imu>(mm));
   }
 
-  std::map<std::string, std::vector<ev::Point>>
-  add_lidar(ev::LidarMeasurement mm) override {
+  void add_lidar(ev::LidarMeasurement mm) override {
     // Set everything up
     auto cloud =
       ev::convert_iter<pcl::PointCloud<lio_sam::PointXYZIRT>>(mm.points)
@@ -169,10 +162,12 @@ class LioSam: public ev::Pipeline {
     // Run through pipeline
     lio_sam_->addLidarMeasurement(mm.stamp.to_sec(), cloud);
 
-    // Return features
-    return ev::convert_map<pcl::PointCloud<lio_sam::PointType>>(
-      {{"point", *lio_sam_->getMostRecentFrame()}}
-    );
+    // Save pose
+    const auto pose = ev::convert<ev::SE3>(lio_sam_->getPose()) * lidar_T_imu_;
+    this->save(mm.stamp, pose);
+
+    // Save features
+    this->save(mm.stamp, "features", *lio_sam_->getMostRecentFrame());
   }
 
 private:

cpp/bindings/pipelines/loam.h

@@ -63,10 +63,6 @@ class LOAM: public ev::Pipeline {
   // clang-format on
 
   // Getters
-  const ev::SE3 pose() override {
-    return ev::convert<ev::SE3>(current_estimated_pose) * lidar_T_imu_;
-  }
-
   const std::map<std::string, std::vector<ev::Point>> map() override {
     return features_to_points(
       transform_features(map_features(), current_estimated_pose)
@@ -94,8 +90,7 @@ class LOAM: public ev::Pipeline {
 
   void add_imu(ev::ImuMeasurement mm) override {}
 
-  std::map<std::string, std::vector<ev::Point>>
-  add_lidar(ev::LidarMeasurement mm) override {
+  void add_lidar(ev::LidarMeasurement mm) override {
     // Handle Edge case of the first scan
     if (past_k_scans_.size() == 0) {
       // Extract Features from the first scan
@@ -106,9 +101,9 @@ class LOAM: public ev::Pipeline {
         std::make_pair(loam::Pose3d::Identity(), scan_features)
       );
       // Initialize the odometry frame pose
-      current_estimated_pose = loam::Pose3d::Identity();
+      this->save(mm.stamp, evalio::SE3::identity() * lidar_T_imu_);
       // Return the initial scan features
-      return features_to_points(scan_features);
+      this->save(mm.stamp, features_to_points(scan_features));
     }
     // Default case for all iterations except the first
     else {
@@ -134,8 +129,11 @@ class LOAM: public ev::Pipeline {
       }
       // Update the Odometry frame pose
       current_estimated_pose = current_estimated_pose.compose(map_T_scan);
+      const auto pose_ev =
+        ev::convert<ev::SE3>(current_estimated_pose) * lidar_T_imu_;
+      this->save(mm.stamp, pose_ev);
       // Return the points associated with the used features
-      return features_to_points(scan_features);
+      this->save(mm.stamp, features_to_points(scan_features));
     }
   }