Skip to content

feat(pl): Type conversions #62

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
contagon merged 6 commits into from
Oct 30, 2025
Merged

feat(pl): Type conversions #62

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
2b5d2f1
Add in conversion module
contagon Oct 29, 2025
5bc4663
Got vector conversions working as well!
contagon Oct 30, 2025
99b0a67
Do it to genz as well
contagon Oct 30, 2025
2eadb0b
Clean up a few more pipelines with new conversions
contagon Oct 30, 2025
2979d15
Move all of binding conversion over to new convert function
contagon Oct 30, 2025
3869e83
Move conversions of iters and maps to separate methods
contagon Oct 30, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
140 changes: 67 additions & 73 deletions cpp/bindings/pipelines/ct_icp.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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();
}

Expand All @@ -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}}
);
}
};
115 changes: 41 additions & 74 deletions cpp/bindings/pipelines/genz_icp.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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_() {}

Expand Down Expand Up @@ -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);
}
};
Loading