From 1f509f8d88e893923a4a197e52a0e92bc4d9409b Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Mon, 10 Feb 2025 10:31:45 +0900 Subject: [PATCH 1/2] add demo data for livox mid360 --- docs/demo.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/docs/demo.md b/docs/demo.md index 574130e7..6e6fde1b 100644 --- a/docs/demo.md +++ b/docs/demo.md @@ -33,6 +33,15 @@ ros2 run glim_ros glim_rosbag --ros-args -p config_path:=$(realpath config/kinec ## Outdoor driving test with Livox MID360 +- [Download dataset](https://zenodo.org/records/14841855) + +!!! tip + Change parameters in `config_ros.json` as follows (see also the [sensor setup guide](https://github.com/koide3/glim/wiki/Sensor-setup-guide)): + + - "acc_scale": 9.80665, + - "imu_topic": "/livox/imu", + - "points_topic": "/livox/lidar", +
From a30057a38e97707e70623bb9e78db391fb1b112d Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Mon, 10 Feb 2025 13:46:32 +0900 Subject: [PATCH 2/2] publish point intensities --- include/glim/util/ros_cloud_converter.hpp | 71 ++++++++++++++----- src/glim/odometry/odometry_estimation_imu.cpp | 6 ++ 2 files changed, 60 insertions(+), 17 deletions(-) diff --git a/include/glim/util/ros_cloud_converter.hpp b/include/glim/util/ros_cloud_converter.hpp index d0ce99f5..a6916b49 100644 --- a/include/glim/util/ros_cloud_converter.hpp +++ b/include/glim/util/ros_cloud_converter.hpp @@ -218,30 +218,67 @@ static PointCloud2ConstPtr frame_to_pointcloud2(const std::string& frame_id, con msg->width = frame.size(); msg->height = 1; - std::vector field_names = {"x", "y", "z", "t"}; - int num_fields = frame.times ? 4 : 3; - msg->fields.resize(num_fields); - - for (int i = 0; i < num_fields; i++) { - msg->fields[i].name = field_names[i]; - msg->fields[i].offset = sizeof(float) * i; - msg->fields[i].datatype = PointField::FLOAT32; - msg->fields[i].count = 1; + const auto create_field = [](const std::string& name, int offset, int datatype, int count) { + PointField field; + field.name = name; + field.offset = offset; + field.datatype = datatype; + field.count = count; + return field; + }; + + int point_step = 0; + msg->fields.reserve(6); + + msg->fields.emplace_back(create_field("x", sizeof(float) * 0, PointField::FLOAT32, 1)); + msg->fields.emplace_back(create_field("y", sizeof(float) * 1, PointField::FLOAT32, 1)); + msg->fields.emplace_back(create_field("z", sizeof(float) * 2, PointField::FLOAT32, 1)); + point_step += sizeof(float) * 3; + + if (frame.times) { + msg->fields.emplace_back(create_field("t", point_step, PointField::FLOAT32, 1)); + point_step += sizeof(float); + } + + if (frame.intensities) { + msg->fields.emplace_back(create_field("intensity", point_step, PointField::FLOAT32, 1)); + point_step += sizeof(float); + } + + const Eigen::Vector4f* colors = frame.aux_attributes.count("colors") ? frame.aux_attribute("colors") : nullptr; + if (colors) { + msg->fields.emplace_back(create_field("rgba", point_step, PointField::UINT32, 1)); + point_step += sizeof(std::uint32_t); } msg->is_bigendian = false; - msg->point_step = sizeof(float) * num_fields; - msg->row_step = sizeof(float) * num_fields * frame.size(); + msg->point_step = point_step; + msg->row_step = point_step * frame.size(); - msg->data.resize(sizeof(float) * num_fields * frame.size()); + msg->data.resize(point_step * frame.size()); for (int i = 0; i < frame.size(); i++) { - float* point = reinterpret_cast(msg->data.data() + msg->point_step * i); - for (int j = 0; j < 3; j++) { - point[j] = frame.points[i][j]; - } + unsigned char* point_bytes = msg->data.data() + msg->point_step * i; + Eigen::Map xyz(reinterpret_cast(point_bytes)); + xyz = frame.points[i].head<3>().cast(); + point_bytes += sizeof(float) * 3; if (frame.times) { - point[3] = frame.times[i]; + *reinterpret_cast(point_bytes) = frame.times[i]; + point_bytes += sizeof(float); + } + + if (frame.intensities) { + *reinterpret_cast(point_bytes) = frame.intensities[i]; + point_bytes += sizeof(float); + } + + if (colors) { + const Eigen::Matrix rgba = (colors[i].array() * 255.0f).min(255.0f).max(0.0f).cast(); + point_bytes[0] = rgba[0]; + point_bytes[1] = rgba[1]; + point_bytes[2] = rgba[2]; + point_bytes[3] = rgba[3]; + point_bytes += sizeof(std::uint32_t); } } diff --git a/src/glim/odometry/odometry_estimation_imu.cpp b/src/glim/odometry/odometry_estimation_imu.cpp index 39660eb7..0806a2b0 100644 --- a/src/glim/odometry/odometry_estimation_imu.cpp +++ b/src/glim/odometry/odometry_estimation_imu.cpp @@ -178,6 +178,9 @@ EstimationFrame::ConstPtr OdometryEstimationIMU::insert_frame(const Preprocessed covariance_estimation->estimate(points_imu, raw_frame->neighbors, normals, covs); auto frame = std::make_shared(points_imu); + if (raw_frame->intensities.size()) { + frame->add_intensities(raw_frame->intensities); + } frame->add_covs(covs); frame->add_normals(normals); new_frame->frame = frame; @@ -298,6 +301,9 @@ EstimationFrame::ConstPtr OdometryEstimationIMU::insert_frame(const Preprocessed covariance_estimation->estimate(deskewed, raw_frame->neighbors, deskewed_normals, deskewed_covs); auto frame = std::make_shared(deskewed); + if (raw_frame->intensities.size()) { + frame->add_intensities(raw_frame->intensities); + } frame->add_covs(deskewed_covs); frame->add_normals(deskewed_normals); new_frame->frame = frame;