diff --git a/cpp/bindings/types.h b/cpp/bindings/types.h index 414bb20..65c0d01 100644 --- a/cpp/bindings/types.h +++ b/cpp/bindings/types.h @@ -267,6 +267,13 @@ inline void make_types(nb::module_& m) { "List of points in the " "point cloud. Note, this is always in row major format." ) + .def_static( + "from_vec_positions", + &LidarMeasurement::from_vec_positions, + "stamp"_a, + "positions"_a, + "Construct a LidarMeasurement from a stamp and a (n,3) numpy array." + ) .def( "to_vec_positions", &LidarMeasurement::to_vec_positions, diff --git a/cpp/evalio/types.h b/cpp/evalio/types.h index 55d4e63..4f3524f 100644 --- a/cpp/evalio/types.h +++ b/cpp/evalio/types.h @@ -218,6 +218,16 @@ struct LidarMeasurement { return oss.str(); } + static LidarMeasurement + from_vec_positions(Stamp stamp, const Eigen::MatrixX3d& positions) { + std::vector pts; + pts.reserve(positions.rows()); + for (const auto& p : positions.rowwise()) { + pts.push_back(Point {.x = p.x(), .y = p.y(), .z = p.z()}); + } + return LidarMeasurement(stamp, pts); + } + std::vector to_vec_positions() const { std::vector eigen_points; eigen_points.reserve(points.size());