feat(pl): Type conversions module (#62)

* Add in conversion module
* Got vector conversions working as well!
* Do it to genz as well
* Clean up a few more pipelines with new conversions
* Move all of binding conversion over to new convert function
* Move conversions of iters and maps to separate methods

Author: contagon

cpp/bindings/pipelines/ct_icp.h

@@ -5,9 +5,45 @@
 
 #include "ct_icp/odometry.hpp"
 #include "ct_icp/types.hpp"
+#include "evalio/convert/base.h"
+#include "evalio/convert/eigen.h"
 #include "evalio/pipeline.h"
 #include "evalio/types.h"
 
+namespace ev = evalio;
+
+// ------------------------- Handle all conversions ------------------------- //
+namespace evalio {
+template<>
+inline evalio::Point convert(const ct_icp::Point3D& from) {
+  return ev::Point {
+    .x = from.raw_pt.x(),
+    .y = from.raw_pt.y(),
+    .z = from.raw_pt.z(),
+    .intensity = 0.0,
+    .t = ev::Duration::from_nsec(0),
+    .row = 0,
+    .col = 0,
+  };
+}
+
+template<>
+inline ct_icp::Point3D convert(const evalio::Point& from) {
+  ct_icp::Point3D to;
+  to.raw_pt = Eigen::Vector3d(from.x, from.y, from.z);
+  to.pt = to.raw_pt;
+  to.alpha_timestamp = from.t.to_sec();
+  to.index_frame = 0;
+  return to;
+}
+
+template<>
+inline evalio::SE3 convert(const ct_icp::TrajectoryFrame& in) {
+  return ev::SE3(ev::SO3::fromMat(in.begin_R), in.begin_t);
+}
+} // namespace evalio
+
+// ------------------------- Actual Wrapper ------------------------- //
 // Simple enum wrapper to parse all strings -> enum options
 struct CTICPEnumParams {
   std::string motion_compensation;
@@ -91,44 +127,16 @@ struct CTICPEnumParams {
   }
 };
 
-class CTICP: public evalio::Pipeline {
+class CTICP: public ev::Pipeline {
 private:
   std::unique_ptr<ct_icp::Odometry> ct_icp_;
   ct_icp::OdometryOptions config_ =
     ct_icp::OdometryOptions::DefaultDrivingProfile();
   CTICPEnumParams enum_params_;
 
-  evalio::SE3 lidar_T_imu_ = evalio::SE3::identity();
+  ev::SE3 lidar_T_imu_ = ev::SE3::identity();
   size_t scan_idx_ = 0;
 
-  inline evalio::SE3 to_evalio_pose(const ct_icp::TrajectoryFrame& pose) const {
-    return evalio::SE3(evalio::SO3::fromMat(pose.begin_R), pose.begin_t);
-  }
-
-  inline evalio::Point to_evalio_point(const ct_icp::Point3D& point) const {
-    return evalio::Point {
-      .x = point.raw_pt.x(),
-      .y = point.raw_pt.y(),
-      .z = point.raw_pt.z(),
-      .intensity = 0.0,
-      .t = evalio::Duration::from_nsec(0),
-      .row = 0,
-      .col = 0,
-    };
-  }
-
-  inline evalio::Point to_evalio_point(const Eigen::Vector3d& point) const {
-    return evalio::Point {
-      .x = point.x(),
-      .y = point.y(),
-      .z = point.z(),
-      .intensity = 0.0,
-      .t = evalio::Duration::from_nsec(0),
-      .row = 0,
-      .col = 0,
-    };
-  }
-
 public:
   CTICP() {
     config_.debug_print = false;
@@ -196,29 +204,24 @@ class CTICP: public evalio::Pipeline {
   // clang-format on
 
   // Getters
-  const evalio::SE3 pose() override {
+  const ev::SE3 pose() override {
     const auto pose = ct_icp_->Trajectory().back();
-    return to_evalio_pose(pose) * lidar_T_imu_;
+    return ev::convert<ev::SE3>(pose) * lidar_T_imu_;
   }
 
-  const std::map<std::string, std::vector<evalio::Point>> map() override {
-    const auto map = ct_icp_->GetLocalMap();
-    std::vector<evalio::Point> ev_points;
-    ev_points.reserve(map.size());
-    for (const auto& point : map) {
-      ev_points.push_back(to_evalio_point(point));
-    }
-    return {{"planar", std::move(ev_points)}};
+  const std::map<std::string, std::vector<ev::Point>> map() override {
+    auto map = ct_icp_->GetLocalMap();
+    return ev::convert_map<decltype(map)>({{"planar", map}});
   }
 
   // Setters
-  void set_imu_params(evalio::ImuParams params) override {}
+  void set_imu_params(ev::ImuParams params) override {}
 
-  void set_lidar_params(evalio::LidarParams params) override {
+  void set_lidar_params(ev::LidarParams params) override {
     config_.max_distance = params.max_range;
   }
 
-  void set_imu_T_lidar(evalio::SE3 T) override {
+  void set_imu_T_lidar(ev::SE3 T) override {
     lidar_T_imu_ = T.inverse();
   }
 
@@ -228,49 +231,40 @@ class CTICP: public evalio::Pipeline {
     ct_icp_ = std::make_unique<ct_icp::Odometry>(config_);
   }
 
-  void add_imu(evalio::ImuMeasurement mm) override {}
+  void add_imu(ev::ImuMeasurement mm) override {}
 
-  std::map<std::string, std::vector<evalio::Point>>
-  add_lidar(evalio::LidarMeasurement mm) override {
-    // Set everything up
-    std::vector<ct_icp::Point3D> pc;
-    pc.reserve(mm.points.size());
+  std::map<std::string, std::vector<ev::Point>>
+  add_lidar(ev::LidarMeasurement mm) override {
+    // Convert
+    auto pc = ev::convert_iter<std::vector<ct_icp::Point3D>>(mm.points);
 
-    // Figure out min/max timesteps
+    // Normalize timestamps to [0, 1]
     const auto& [min, max] = std::minmax_element(
-      mm.points.cbegin(),
-      mm.points.cend(),
-      [](const evalio::Point& a, const evalio::Point& b) { return a.t < b.t; }
+      pc.cbegin(),
+      pc.cend(),
+      [](const ct_icp::Point3D& a, const ct_icp::Point3D& b) {
+        return a.alpha_timestamp < b.alpha_timestamp;
+      }
     );
 
-    const auto min_t = min->t.to_sec();
-    const auto max_t = max->t.to_sec();
-    const auto normalize = [min_t, max_t](evalio::Duration t) {
-      return (t.to_sec() - min_t) / (max_t - min_t);
+    const auto min_t = min->alpha_timestamp;
+    const auto max_t = max->alpha_timestamp;
+    const auto normalize = [min_t, max_t](double t) {
+      return (t - min_t) / (max_t - min_t);
     };
 
     // Copy
-    for (const auto& point : mm.points) {
-      ct_icp::Point3D p;
-      p.raw_pt = Eigen::Vector3d(point.x, point.y, point.z);
-      p.pt = p.raw_pt;
-      p.alpha_timestamp = normalize(point.t);
-      p.index_frame = scan_idx_;
-      pc.push_back(p);
+    for (auto& p : pc) {
+      p.alpha_timestamp = normalize(p.alpha_timestamp);
     }
 
     // Run through pipeline
     const auto summary = ct_icp_->RegisterFrame(pc);
-
-    // Return the used points
-    std::vector<evalio::Point> ev_planar_points;
-    ev_planar_points.reserve(summary.keypoints.size());
-    for (const auto& point : summary.keypoints) {
-      ev_planar_points.push_back(to_evalio_point(point));
-    }
-
     scan_idx_++;
 
-    return {{"planar", ev_planar_points}};
+    // Return the used points
+    return ev::convert_map<std::vector<ct_icp::Point3D>>(
+      {{"planar", summary.keypoints}}
+    );
   }
 };

cpp/bindings/pipelines/genz_icp.h

@@ -1,12 +1,18 @@
 #pragma once
 
+#include <algorithm>
 #include <memory>
 
+#include "evalio/convert/base.h"
+#include "evalio/convert/eigen.h"
+#include "evalio/convert/sophus.h"
 #include "evalio/pipeline.h"
 #include "evalio/types.h"
 #include "genz_icp/pipeline/GenZICP.hpp"
 
-class GenZICP: public evalio::Pipeline {
+namespace ev = evalio;
+
+class GenZICP: public ev::Pipeline {
 public:
   GenZICP() : config_() {}
 
@@ -46,109 +52,70 @@ class GenZICP: public evalio::Pipeline {
   // clang-format on
 
   // Getters
-  const evalio::SE3 pose() override {
+  const ev::SE3 pose() override {
     const auto pose =
       !genz_icp_->poses().empty() ? genz_icp_->poses().back() : Sophus::SE3d();
-    return to_evalio_se3(pose * lidar_T_imu_);
+    return ev::convert<ev::SE3>(pose * lidar_T_imu_);
   }
 
-  const std::map<std::string, std::vector<evalio::Point>> map() override {
-    std::vector<Eigen::Vector3d> map = genz_icp_->LocalMap();
-    std::vector<evalio::Point> evalio_map;
-    evalio_map.reserve(map.size());
-    for (auto point : map) {
-      evalio_map.push_back(to_evalio_point(point));
-    }
-    return {{"point", evalio_map}};
+  const std::map<std::string, std::vector<ev::Point>> map() override {
+    return ev::convert_map<std::vector<Eigen::Vector3d>>(
+      {{"map", genz_icp_->LocalMap()}}
+    );
   }
 
   // Setters
-  void set_imu_params(evalio::ImuParams params) override {}
+  void set_imu_params(ev::ImuParams params) override {}
 
-  void set_lidar_params(evalio::LidarParams params) override {
+  void set_lidar_params(ev::LidarParams params) override {
     config_.min_range = params.min_range;
     config_.max_range = params.max_range;
     config_.map_cleanup_radius = params.max_range;
   }
 
-  void set_imu_T_lidar(evalio::SE3 T) override {
-    lidar_T_imu_ = to_sophus_se3(T).inverse();
+  void set_imu_T_lidar(ev::SE3 T) override {
+    lidar_T_imu_ = ev::convert<Sophus::SE3d>(T).inverse();
   }
 
   // Doers
   void initialize() override {
     genz_icp_ = std::make_unique<genz_icp::pipeline::GenZICP>(config_);
   }
 
-  void add_imu(evalio::ImuMeasurement mm) override {}
+  void add_imu(ev::ImuMeasurement mm) override {}
 
-  std::map<std::string, std::vector<evalio::Point>>
-  add_lidar(evalio::LidarMeasurement mm) override {
+  std::map<std::string, std::vector<ev::Point>>
+  add_lidar(ev::LidarMeasurement mm) override {
     // Set everything up
-    std::vector<Eigen::Vector3d> points;
-    points.reserve(mm.points.size());
-    std::vector<double> timestamps;
-    timestamps.reserve(mm.points.size());
-
-    // Copy
-    for (auto point : mm.points) {
-      points.push_back(to_eigen_point(point));
-      timestamps.push_back(point.t.to_sec());
-    }
+    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& [planar_points, nonplanar_points] =
-      genz_icp_->RegisterFrame(points, timestamps);
-    const auto lidar_T_world = genz_icp_->poses().back().inverse();
+    auto [planar, nonplanar] = genz_icp_->RegisterFrame(points, timestamps);
+    auto lidar_T_world = genz_icp_->poses().back().inverse();
 
-    // Return the used points
     // These are all in the global frame, so we need to convert them
-    std::vector<evalio::Point> ev_planar_points;
-    ev_planar_points.reserve(planar_points.size());
-    for (auto point : planar_points) {
-      ev_planar_points.push_back(to_evalio_point(lidar_T_world * point));
-    }
-
-    std::vector<evalio::Point> ev_nonplanar_points;
-    ev_nonplanar_points.reserve(nonplanar_points.size());
-    for (auto point : nonplanar_points) {
-      ev_nonplanar_points.push_back(to_evalio_point(lidar_T_world * point));
-    }
-
-    return {{"nonplanar", ev_nonplanar_points}, {"planar", ev_planar_points}};
+    std::transform(
+      planar.begin(),
+      planar.end(),
+      planar.begin(),
+      [&](auto point) { return lidar_T_imu_ * point; }
+    );
+    std::transform(
+      nonplanar.begin(),
+      nonplanar.end(),
+      nonplanar.begin(),
+      [&](auto point) { return lidar_T_imu_ * point; }
+    );
+
+    // Return the used points
+    return ev::convert_map<std::vector<Eigen::Vector3d>>(
+      {{"planar", planar}, {"nonplanar", nonplanar}}
+    );
   }
 
 private:
   std::unique_ptr<genz_icp::pipeline::GenZICP> genz_icp_;
   genz_icp::pipeline::GenZConfig config_;
   Sophus::SE3d lidar_T_imu_;
-
-  // Misc helpers
-  inline evalio::Point to_evalio_point(Eigen::Vector3d point) {
-    return {
-      .x = point[0],
-      .y = point[1],
-      .z = point[2],
-      .intensity = 0.0,
-      .t = evalio::Duration::from_sec(0),
-      .row = 0,
-      .col = 0
-    };
-  }
-
-  inline Eigen::Vector3d to_eigen_point(evalio::Point point) {
-    return {point.x, point.y, point.z};
-  }
-
-  inline evalio::SE3 to_evalio_se3(Sophus::SE3d pose) {
-    const auto t = pose.translation();
-    const auto q = pose.unit_quaternion();
-    const auto rot =
-      evalio::SO3 {.qx = q.x(), .qy = q.y(), .qz = q.z(), .qw = q.w()};
-    return evalio::SE3(rot, t);
-  }
-
-  inline Sophus::SE3d to_sophus_se3(evalio::SE3 pose) {
-    return Sophus::SE3d(Sophus::SO3d(pose.rot.toEigen()), pose.trans);
-  }
 };