From d01d41186cc52103e7a0f4de189b4c2527cf0989 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Tue, 24 Mar 2026 11:26:03 -0400 Subject: [PATCH 1/7] Begin upgrading automatic point reordering --- ros/launch/odometry.launch.py | 13 +- ros/src/format.hpp | 51 +++++++ ros/src/node.cpp | 60 +++++++-- ros/src/node.hpp | 5 + ros/src/ros_pc2.h | 241 +++++++++++++++++++++------------- 5 files changed, 267 insertions(+), 103 deletions(-) create mode 100644 ros/src/format.hpp diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index 8c2500a..0f1b061 100644 --- a/ros/launch/odometry.launch.py +++ b/ros/launch/odometry.launch.py @@ -44,6 +44,7 @@ class config: num_threads: int = 0 # Covariance diagonal values + # TODO: Extract covariance from our graph position_covariance: float = 0.1 orientation_covariance: float = 0.1 @@ -69,8 +70,13 @@ def generate_launch_description(): # fmt: off topic = make_config(la, "topic", "Input point cloud topic") # lidar geometry config - num_columns = make_config(la, "num_columns", "LiDAR image width (columns)") - num_rows = make_config(la, "num_rows", "LiDAR image height (rows)") + # either specify a model + lidar_model = make_config(la, "lidar_model", "Predefined LiDAR model (e.g. 'VLP-16')", "") + # specify geometry directly (overrides model if provided) + num_columns = make_config(la, "num_columns", "LiDAR image width (columns)", 0) + num_rows = make_config(la, "num_rows", "LiDAR image height (rows)", 0) + # Or let them be inferred from the first point cloud message (suboptimal, but will usually still work fine + min_range = make_config(la, "min_range", "Minimum LiDAR range", 1.0) max_range = make_config(la, "max_range", "Maximum LiDAR range", 100.0) # optional visualization and rosbag play @@ -101,11 +107,12 @@ def generate_launch_description(): "publish_odom_tf": publish_odom_tf, "invert_odom_tf": invert_odom_tf, # LiDAR geometry + "lidar_model": lidar_model, "num_columns": num_columns, "num_rows": num_rows, + # Feature extraction "min_range": min_range, "max_range": max_range, - # Feature extraction "neighbor_points": config.neighbor_points, "num_sectors": config.num_sectors, "planar_threshold": config.planar_threshold, diff --git a/ros/src/format.hpp b/ros/src/format.hpp new file mode 100644 index 0000000..45187e2 --- /dev/null +++ b/ros/src/format.hpp @@ -0,0 +1,51 @@ +#pragma once + +#include +#include +#include +#include + +struct LidarFormat { + int num_rows = 0; + int num_columns = 0; + bool row_major = false; + bool all_points_present = false; + std::vector map_row_to_fire = {}; +}; + +inline std::ostream &operator<<(std::ostream &os, const LidarFormat &format) { + os << "LidarFormat(rows=" << format.num_rows << ", cols=" << format.num_columns + << ", row_major=" << format.row_major + << ", all_points=" << format.all_points_present << ")"; + return os; +} + +/// Invert a map from firing order to row index into a map from row index to firing +/// order +/// +/// By default printing out the row index in the order they are fired gives +/// map_fire_to_row, but for some reordering logic it's more convenient to have the +/// inverse +inline std::vector invert_map(const std::vector &map_fire_to_row) { + std::vector map_row_to_fire(map_fire_to_row.size()); + for (size_t i = 0; i < map_fire_to_row.size(); ++i) { + map_row_to_fire[map_fire_to_row[i]] = i; + } + return map_row_to_fire; +} + +inline std::vector default_row_to_fire(size_t num_rows) { + std::vector map_row_to_fire(num_rows); + for (size_t i = 0; i < num_rows; ++i) { + map_row_to_fire[i] = i; + } + return map_row_to_fire; +} + +inline const std::map LIDAR_FORMATS = { + {"VLP-16", + {64, 1024, false, false, + invert_map({0, 8, 1, 9, 2, 10, 3, 11, 4, 12, 5, 13, 6, 14, 7, 15})}}, + + // Add more models as needed +}; \ No newline at end of file diff --git a/ros/src/node.cpp b/ros/src/node.cpp index 902af0a..3750121 100644 --- a/ros/src/node.cpp +++ b/ros/src/node.cpp @@ -11,7 +11,6 @@ // FORM-ROS #include "node.hpp" #include "ros_pc2.h" -#include "utils.hpp" // FORM #include "form/form.hpp" @@ -62,18 +61,33 @@ EstimatorNode::EstimatorNode(const rclcpp::NodeOptions &options) position_covariance_ = declare_parameter("position_covariance", 0.1); orientation_covariance_ = declare_parameter("orientation_covariance", 0.1); + // LiDAR format + std::string model_name = declare_parameter("lidar_model", ""); + // If the name is given specifically, use it + if (!model_name.empty()) { + auto it = LIDAR_FORMATS.find(model_name); + if (it == LIDAR_FORMATS.end()) { + RCLCPP_WARN(this->get_logger(), "Unknown LiDAR model '%s', defaulting to inferring model", model_name.c_str()); + } else { + lidar_format_ = it->second; + } + } + // FORM parameters form::Estimator::Params params; - // LiDAR geometry (required) - params.extraction.num_rows = declare_parameter("num_rows", params.extraction.num_rows); - params.extraction.num_columns = declare_parameter("num_columns", params.extraction.num_columns); + // Feature extraction auto min_range = declare_parameter("min_range", 1.0); auto max_range = declare_parameter("max_range", 100.0); params.extraction.min_norm_squared = min_range * min_range; params.extraction.max_norm_squared = max_range * max_range; - - // Feature extraction + if(lidar_format_.has_value()) { + params.extraction.num_rows = lidar_format_->num_rows; + params.extraction.num_columns = lidar_format_->num_columns; + } else { + params.extraction.num_rows = declare_parameter("num_rows", 0); + params.extraction.num_columns = declare_parameter("num_columns", 0); + } params.extraction.neighbor_points = declare_parameter("neighbor_points", params.extraction.neighbor_points); params.extraction.num_sectors = declare_parameter("num_sectors", params.extraction.num_sectors); params.extraction.planar_threshold = declare_parameter("planar_threshold", params.extraction.planar_threshold); @@ -132,10 +146,38 @@ EstimatorNode::EstimatorNode(const rclcpp::NodeOptions &options) void EstimatorNode::register_frame( const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { - // Convert PointCloud2 -> organized vector using pc2_conversions + // Convert PointCloud2 -> RawPoints + auto raw_points = form_ros::load_pc2(msg); + + // Infer LiDAR sizes if not set by user + if (estimator_.m_extractor.params.num_rows == 0 || + estimator_.m_extractor.params.num_columns == 0) { + const auto [num_rows, num_columns] = form_ros::infer_lidar_size(raw_points); + if (estimator_.m_extractor.params.num_rows == 0) { + estimator_.m_extractor.params.num_rows = num_rows; + } + if (estimator_.m_extractor.params.num_columns == 0) { + estimator_.m_extractor.params.num_columns = num_columns; + } + RCLCPP_INFO(this->get_logger(), + "Inferred LiDAR sizes: num_rows=%lu, num_columns=%lu", num_rows, + num_columns); + } + // Infer lidar ordering properties if not set by user + if (!lidar_format_.has_value()) { + lidar_format_ = form_ros::infer_lidar_order( + raw_points, estimator_.m_extractor.params.num_rows, + estimator_.m_extractor.params.num_columns); + RCLCPP_INFO(this->get_logger(), + "Inferred LiDAR ordering: %s, %s, sequential firing order", + lidar_format_->row_major ? "row major" : "column major", + lidar_format_->all_points_present ? "all points present" + : "not all points present"); + } + + // RawPoints -> Structured form::PointXYZf const auto points = - form_ros::PointCloud2ToForm(msg, estimator_.m_params.extraction.num_rows, - estimator_.m_params.extraction.num_columns); + form_ros::reorder(raw_points, *lidar_format_, this->get_logger()); // Register frame with FORM const auto &[planar_kp, point_kp] = estimator_.register_scan(points); diff --git a/ros/src/node.hpp b/ros/src/node.hpp index 6b0a5cd..4915ed0 100644 --- a/ros/src/node.hpp +++ b/ros/src/node.hpp @@ -9,9 +9,11 @@ // FORM #include "form/form.hpp" +#include "format.hpp" #include "utils.hpp" // ROS 2 +#include #include #include #include @@ -56,6 +58,9 @@ class EstimatorNode : public rclcpp::Node { bool publish_odom_tf_; bool publish_debug_clouds_; + /// For extracting geometry + std::optional lidar_format_; + /// Data subscribers. rclcpp::Subscription::SharedPtr pointcloud_sub_; diff --git a/ros/src/ros_pc2.h b/ros/src/ros_pc2.h index 0656c69..44a6d36 100644 --- a/ros/src/ros_pc2.h +++ b/ros/src/ros_pc2.h @@ -5,6 +5,7 @@ // (row-major, num_rows * num_columns) suitable for FORM's feature extraction. #pragma once +#include #include #include #include @@ -15,12 +16,23 @@ #include #include +#include "format.hpp" #include
+#include #include namespace form_ros { -// ----------------------------- Data getters ----------------------------- // +// ----------------------------- PC2 -> RawPoints ----------------------------- // +// Holds parsed per-point data before reordering into the organized grid. +struct RawPoint { + float x = 0.0f; + float y = 0.0f; + float z = 0.0f; + uint8_t row = 0; + uint16_t col = 0; +}; + // Build a lambda that reads a field of type T from raw PointCloud2 byte data // at the given offset, handling the various PointField datatypes. template @@ -82,83 +94,8 @@ template std::function blank() { return [](T &, const uint8_t *) noexcept {}; } -// ------------------------- Reordering Helpers ------------------------- // -// Holds parsed per-point data before reordering into the organized grid. -struct RawPoint { - float x = 0.0f; - float y = 0.0f; - float z = 0.0f; - uint8_t row = 0; - uint16_t col = 0; -}; - -// Iterates through points to fill in columns -inline void -_fill_col(std::vector &mm, - std::function - func_col) { - // fill out the first one to kickstart - uint16_t prev_col = 0; - uint8_t prev_row = mm[0].row; - for (auto p = mm.begin() + 1; p != mm.end(); ++p) { - func_col(p->col, prev_col, prev_row, p->row); - prev_col = p->col; - prev_row = p->row; - } -} - -// Fills in column index for row major order -inline void fill_col_row_major(std::vector &mm) { - auto func_col = [](uint16_t &col, const uint16_t &prev_col, - const uint8_t &prev_row, const uint8_t &curr_row) { - if (prev_row != curr_row) { - col = 0; - } else { - col = prev_col + 1; - } - }; - - _fill_col(mm, func_col); -} - -// Fills in column index for column major order -inline void fill_col_col_major(std::vector &mm) { - auto func_col = [](uint16_t &col, const uint16_t &prev_col, - const uint8_t &prev_row, const uint8_t &curr_row) { - if (curr_row < prev_row) { - col = prev_col + 1; - } else { - col = prev_col; - } - }; - - _fill_col(mm, func_col); -} - -inline std::vector -reorder_points(std::vector &mm, size_t num_rows, size_t num_cols) { - std::vector output(num_rows * num_cols, - form::PointXYZf(0.0f, 0.0f, 0.0f)); - - for (auto p : mm) { - if (p.row >= num_rows || p.col >= num_cols) { - std::cerr << "Warning: Point with row " << static_cast(p.row) - << " and col " << p.col - << " is out of bounds for num_rows=" << num_rows - << " and num_cols=" << num_cols << ". Skipping this point." - << std::endl; - continue; - } - output[p.row * num_cols + p.col] = form::PointXYZf(p.x, p.y, p.z); - } - return output; -} - -// ------------------------- Main Conversion ------------------------- // -inline std::vector -PointCloud2ToForm(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, - int num_rows, int num_columns) { +inline std::vector +load_pc2(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { if (msg->is_bigendian) { throw std::runtime_error("Big-endian PointCloud2 is not supported"); } @@ -214,28 +151,150 @@ PointCloud2ToForm(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, func_row(raw[i].row, pt); } - // Catch some potential edge cases with bad values - if (n_points > num_columns * num_rows) { - std::cerr << "Warning: PointCloud2 has more points than expected from num_rows " - "and num_columns. Please check your parameters." - << std::endl; + return raw; +} + +// ------------------------- Infer LiDAR Properties ------------------------- // +inline std::tuple +infer_lidar_size(const std::vector &raw) { + size_t num_rows = 0; + size_t num_columns = 0; + + // Infer the number of rings/rows by finding the max row index in the data + // And then rounding up to the nearest power of 2 for safety + // (Occasionally entire rows will be missing) + num_rows = std::max_element( + raw.begin(), raw.end(), + [](const RawPoint &a, const RawPoint &b) { return a.row < b.row; }) + ->row + + 1; // rows are 0-indexed + num_rows = static_cast(std::pow(2, std::ceil(std::log2(num_rows)))); + + // Count the number of each row to infer the number of columns + std::vector row_counts(num_rows, 0); + for (const auto &p : raw) { + row_counts[p.row]++; + } + + // Find the maximum number of points in any row + num_columns = *std::max_element(row_counts.begin(), row_counts.end()); + + return std::make_tuple(num_rows, num_columns); +} + +inline LidarFormat infer_lidar_order(const std::vector &raw, + size_t num_rows, size_t num_columns) { + LidarFormat model; + model.num_rows = num_rows; + model.num_columns = num_columns; + + model.row_major = raw[0].row == raw[1].row; + model.all_points_present = (raw.size() == num_rows * num_columns); + model.map_row_to_fire = default_row_to_fire(num_rows); + + return model; +} + +// ------------------------- Reordering Helpers ------------------------- // + +// Iterates through points to fill in columns +inline void +_fill_col(std::vector &mm, + std::function + func_col) { + // fill out the first one to kickstart + uint16_t prev_col = 0; + uint8_t prev_row = mm[0].row; + for (auto p = mm.begin() + 1; p != mm.end(); ++p) { + func_col(p->col, prev_col, prev_row, p->row); + prev_col = p->col; + prev_row = p->row; } +} + +// Fills in column index for row major order +inline void fill_col_row_major(std::vector &mm) { + auto func_col = [](uint16_t &col, const uint16_t &prev_col, + const uint8_t &prev_row, const uint8_t &curr_row) { + if (prev_row != curr_row) { + col = 0; + } else { + col = prev_col + 1; + } + }; + + _fill_col(mm, func_col); +} + +// Fills in column index for column major order +inline void fill_col_col_major(std::vector &mm, const LidarFormat &model) { + auto func_col = [&model](uint16_t &col, const uint16_t &prev_col, + const uint8_t &prev_row, const uint8_t &curr_row) { + if (model.map_row_to_fire[curr_row] < model.map_row_to_fire[prev_row]) { + col = prev_col + 1; + } else { + col = prev_col; + } + }; - // Infer properties of the cloud - bool all_points_present = (n_points == num_rows * num_columns); - bool row_major = true; - if (n_points >= 2) { - row_major = (raw[0].row == raw[1].row); + _fill_col(mm, func_col); +} + +// Otherwise, we won't be able to reorder things properly +inline std::vector +reorder(std::vector &raw, const LidarFormat &model, + std::optional logger = std::nullopt) { + const int n_points = static_cast(raw.size()); + + // Catch some potential edge cases with bad values + if (n_points > model.num_columns * model.num_rows) { + if (logger) { + RCLCPP_WARN(logger.value(), + "Warning: PointCloud2 has more points (%d) than expected from " + "num_rows and num_columns (%d). Parameters were inferred or " + "provided incorrectly.", + n_points, model.num_rows * model.num_columns); + } else { + std::cerr + << "Warning: PointCloud2 has more points than expected from num_rows " + << "and num_columns. Parameters were inferred or provided incorrectly." + << std::endl; + } } // Fill out column indices based on inferred ordering - if (row_major) { + if (model.row_major) { fill_col_row_major(raw); } else { - fill_col_col_major(raw); + fill_col_col_major(raw, model); } - return reorder_points(raw, num_rows, num_columns); + std::vector output(model.num_rows * model.num_columns, + form::PointXYZf(0.0f, 0.0f, 0.0f)); + + for (auto p : raw) { + if (p.row >= model.num_rows || p.col >= model.num_columns) { + if (logger) { + RCLCPP_WARN(logger.value(), + "Warning: Point with row %d and col %d is out of " + "bounds for num_rows=%d and num_cols=%d, skipping. Parameters " + "were inferred or provided incorrectly.", + static_cast(p.row), p.col, model.num_rows, + model.num_columns); + } else { + std::cerr << "Warning: Point with row " << static_cast(p.row) + << " and col " << p.col + << " is out of bounds for num_rows=" << model.num_rows + << " and num_cols=" << model.num_columns + << ", skipping. Parameters were inferred or provided incorrectly." + << std::endl; + } + continue; + } + output[p.row * model.num_columns + p.col] = form::PointXYZf(p.x, p.y, p.z); + } + return output; } } // namespace form_ros From 9b378dc68c51cbf16364ea8df667b4f3b5cb4f7b Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Tue, 24 Mar 2026 11:34:50 -0400 Subject: [PATCH 2/7] Setup unknown firing order option --- ros/src/format.hpp | 3 ++- ros/src/node.cpp | 5 +++-- ros/src/ros_pc2.h | 22 ++++++++++++++++++---- 3 files changed, 23 insertions(+), 7 deletions(-) diff --git a/ros/src/format.hpp b/ros/src/format.hpp index 45187e2..43f9b70 100644 --- a/ros/src/format.hpp +++ b/ros/src/format.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -10,7 +11,7 @@ struct LidarFormat { int num_columns = 0; bool row_major = false; bool all_points_present = false; - std::vector map_row_to_fire = {}; + std::optional> map_row_to_fire = std::nullopt; }; inline std::ostream &operator<<(std::ostream &os, const LidarFormat &format) { diff --git a/ros/src/node.cpp b/ros/src/node.cpp index 3750121..f39a41b 100644 --- a/ros/src/node.cpp +++ b/ros/src/node.cpp @@ -169,10 +169,11 @@ void EstimatorNode::register_frame( raw_points, estimator_.m_extractor.params.num_rows, estimator_.m_extractor.params.num_columns); RCLCPP_INFO(this->get_logger(), - "Inferred LiDAR ordering: %s, %s, sequential firing order", + "Inferred LiDAR ordering: %s, %s, %s firing order", lidar_format_->row_major ? "row major" : "column major", lidar_format_->all_points_present ? "all points present" - : "not all points present"); + : "not all points present", + lidar_format_->map_row_to_fire.has_value() ? "known" : "unknown"); } // RawPoints -> Structured form::PointXYZf diff --git a/ros/src/ros_pc2.h b/ros/src/ros_pc2.h index 44a6d36..8828ad4 100644 --- a/ros/src/ros_pc2.h +++ b/ros/src/ros_pc2.h @@ -190,7 +190,6 @@ inline LidarFormat infer_lidar_order(const std::vector &raw, model.row_major = raw[0].row == raw[1].row; model.all_points_present = (raw.size() == num_rows * num_columns); - model.map_row_to_fire = default_row_to_fire(num_rows); return model; } @@ -227,11 +226,12 @@ inline void fill_col_row_major(std::vector &mm) { _fill_col(mm, func_col); } -// Fills in column index for column major order +// Fills in column index for column major order with known firing order inline void fill_col_col_major(std::vector &mm, const LidarFormat &model) { auto func_col = [&model](uint16_t &col, const uint16_t &prev_col, const uint8_t &prev_row, const uint8_t &curr_row) { - if (model.map_row_to_fire[curr_row] < model.map_row_to_fire[prev_row]) { + if (model.map_row_to_fire.value()[curr_row] < + model.map_row_to_fire.value()[prev_row]) { col = prev_col + 1; } else { col = prev_col; @@ -241,6 +241,18 @@ inline void fill_col_col_major(std::vector &mm, const LidarFormat &mod _fill_col(mm, func_col); } +// Fills in column index for column major order when we don't know firing order, +// by counting how many times each row has been seen so far +// This will put all invalid points at the end of each row +inline void fill_col_col_major_unknown(std::vector &mm, + const LidarFormat &model) { + std::vector col_index(model.num_rows, 0); + for (auto &p : mm) { + p.col = col_index[p.row]; + col_index[p.row]++; + } +} + // Otherwise, we won't be able to reorder things properly inline std::vector reorder(std::vector &raw, const LidarFormat &model, @@ -263,9 +275,11 @@ reorder(std::vector &raw, const LidarFormat &model, } } - // Fill out column indices based on inferred ordering + // Fill out column indices based on current setup if (model.row_major) { fill_col_row_major(raw); + } else if (!model.map_row_to_fire.has_value()) { + fill_col_col_major_unknown(raw, model); } else { fill_col_col_major(raw, model); } From e92c3b01b8c49d873b3f8eb6868d83613c964142 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Tue, 24 Mar 2026 12:08:15 -0400 Subject: [PATCH 3/7] Add some more robustness to inferring lidar formats --- ros/launch/odometry.launch.py | 7 +++--- ros/pixi.toml | 2 +- ros/src/format.hpp | 12 +++++---- ros/src/node.cpp | 47 ++++++++++++++++++++++++----------- ros/src/ros_pc2.h | 10 ++++++++ 5 files changed, 53 insertions(+), 25 deletions(-) diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index 0f1b061..797e755 100644 --- a/ros/launch/odometry.launch.py +++ b/ros/launch/odometry.launch.py @@ -72,13 +72,12 @@ def generate_launch_description(): # lidar geometry config # either specify a model lidar_model = make_config(la, "lidar_model", "Predefined LiDAR model (e.g. 'VLP-16')", "") - # specify geometry directly (overrides model if provided) + # specify some geometry directly (will override model if set) num_columns = make_config(la, "num_columns", "LiDAR image width (columns)", 0) num_rows = make_config(la, "num_rows", "LiDAR image height (rows)", 0) - # Or let them be inferred from the first point cloud message (suboptimal, but will usually still work fine + min_range = make_config(la, "min_range", "Minimum LiDAR range", 0.0) + max_range = make_config(la, "max_range", "Maximum LiDAR range", 0.0) - min_range = make_config(la, "min_range", "Minimum LiDAR range", 1.0) - max_range = make_config(la, "max_range", "Maximum LiDAR range", 100.0) # optional visualization and rosbag play visualize = make_config(la, "visualize", "Launch RViz and debug visualization", True) bagfile = make_config(la, "bagfile", "Optional rosbag file/folder to play", "") diff --git a/ros/pixi.toml b/ros/pixi.toml index 2437c2b..0fd19cc 100644 --- a/ros/pixi.toml +++ b/ros/pixi.toml @@ -10,7 +10,7 @@ scripts = ["install/setup.sh"] [tasks] build = "colcon build --event-handlers console_direct+" -oxford = "ros2 launch form odometry.launch.py bagfile:=$EVALIO_DATA/oxford_spires/blenheim_palace_01 topic:=/hesai/pandar num_rows:=64 num_columns:=1200 max_range:=60.0" +oxford = "ros2 launch form odometry.launch.py bagfile:=$EVALIO_DATA/oxford_spires/blenheim_palace_01 topic:=/hesai/pandar lidar_model:=Hesai-QT64" [dependencies] ros-jazzy-ros-base = ">=0.11.0,<0.12" diff --git a/ros/src/format.hpp b/ros/src/format.hpp index 43f9b70..b3ef772 100644 --- a/ros/src/format.hpp +++ b/ros/src/format.hpp @@ -12,12 +12,16 @@ struct LidarFormat { bool row_major = false; bool all_points_present = false; std::optional> map_row_to_fire = std::nullopt; + double min_range = 0.0; + double max_range = 0.0; }; inline std::ostream &operator<<(std::ostream &os, const LidarFormat &format) { os << "LidarFormat(rows=" << format.num_rows << ", cols=" << format.num_columns << ", row_major=" << format.row_major - << ", all_points=" << format.all_points_present << ")"; + << ", all_points=" << format.all_points_present + << ", min_range=" << format.min_range << ", max_range=" << format.max_range + << " )"; return os; } @@ -44,9 +48,7 @@ inline std::vector default_row_to_fire(size_t num_rows) { } inline const std::map LIDAR_FORMATS = { - {"VLP-16", - {64, 1024, false, false, - invert_map({0, 8, 1, 9, 2, 10, 3, 11, 4, 12, 5, 13, 6, 14, 7, 15})}}, - + {"Hesai-QT64", {64, 1200, false, false, default_row_to_fire(64), 1.0, 60.0}}, + {"VLP-16", {16, 3624, false, true, default_row_to_fire(16), 0.1, 100.0}}, // Add more models as needed }; \ No newline at end of file diff --git a/ros/src/node.cpp b/ros/src/node.cpp index f39a41b..c06066f 100644 --- a/ros/src/node.cpp +++ b/ros/src/node.cpp @@ -6,6 +6,7 @@ // (Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill // Stachniss) #include +#include #include // FORM-ROS @@ -70,6 +71,7 @@ EstimatorNode::EstimatorNode(const rclcpp::NodeOptions &options) RCLCPP_WARN(this->get_logger(), "Unknown LiDAR model '%s', defaulting to inferring model", model_name.c_str()); } else { lidar_format_ = it->second; + RCLCPP_INFO(this->get_logger(), "Using LiDAR format for model '%s'", model_name.c_str()); } } @@ -77,16 +79,25 @@ EstimatorNode::EstimatorNode(const rclcpp::NodeOptions &options) form::Estimator::Params params; // Feature extraction - auto min_range = declare_parameter("min_range", 1.0); - auto max_range = declare_parameter("max_range", 100.0); + auto min_range = declare_parameter("min_range", 0.0); + auto max_range = declare_parameter("max_range", 0.0); + if (min_range == 0.0) { + min_range = lidar_format_.has_value() ? lidar_format_->min_range : 0.1; + } + if (max_range == 0.0) { + max_range = lidar_format_.has_value() ? lidar_format_->max_range : 100.0; + } params.extraction.min_norm_squared = min_range * min_range; params.extraction.max_norm_squared = max_range * max_range; - if(lidar_format_.has_value()) { + + // If neither lidar_format or these parameters are set directly, they'll be inferred later + params.extraction.num_rows = declare_parameter("num_rows", 0); + params.extraction.num_columns = declare_parameter("num_columns", 0); + if(lidar_format_.has_value() && params.extraction.num_rows == 0) { params.extraction.num_rows = lidar_format_->num_rows; + } + if(lidar_format_.has_value() && params.extraction.num_columns == 0) { params.extraction.num_columns = lidar_format_->num_columns; - } else { - params.extraction.num_rows = declare_parameter("num_rows", 0); - params.extraction.num_columns = declare_parameter("num_columns", 0); } params.extraction.neighbor_points = declare_parameter("neighbor_points", params.extraction.neighbor_points); params.extraction.num_sectors = declare_parameter("num_sectors", params.extraction.num_sectors); @@ -150,6 +161,7 @@ void EstimatorNode::register_frame( auto raw_points = form_ros::load_pc2(msg); // Infer LiDAR sizes if not set by user + bool inferred_sizes = false; if (estimator_.m_extractor.params.num_rows == 0 || estimator_.m_extractor.params.num_columns == 0) { const auto [num_rows, num_columns] = form_ros::infer_lidar_size(raw_points); @@ -159,22 +171,27 @@ void EstimatorNode::register_frame( if (estimator_.m_extractor.params.num_columns == 0) { estimator_.m_extractor.params.num_columns = num_columns; } - RCLCPP_INFO(this->get_logger(), - "Inferred LiDAR sizes: num_rows=%lu, num_columns=%lu", num_rows, - num_columns); + inferred_sizes = true; } + RCLCPP_INFO_ONCE(this->get_logger(), "%s LiDAR sizes: num_rows=%d, num_columns=%d", + inferred_sizes ? "Inferred" : "User-specified", + estimator_.m_extractor.params.num_rows, + estimator_.m_extractor.params.num_columns); + // Infer lidar ordering properties if not set by user + bool inferred_format = false; if (!lidar_format_.has_value()) { lidar_format_ = form_ros::infer_lidar_order( raw_points, estimator_.m_extractor.params.num_rows, estimator_.m_extractor.params.num_columns); - RCLCPP_INFO(this->get_logger(), - "Inferred LiDAR ordering: %s, %s, %s firing order", - lidar_format_->row_major ? "row major" : "column major", - lidar_format_->all_points_present ? "all points present" - : "not all points present", - lidar_format_->map_row_to_fire.has_value() ? "known" : "unknown"); + inferred_format = true; } + RCLCPP_INFO_ONCE(this->get_logger(), "%s LiDAR ordering: %s, %s, %s firing order", + inferred_format ? "Inferred" : "User-specified", + lidar_format_->row_major ? "row major" : "column major", + lidar_format_->all_points_present ? "all points present" + : "not all points present", + lidar_format_->map_row_to_fire.has_value() ? "known" : "unknown"); // RawPoints -> Structured form::PointXYZf const auto points = diff --git a/ros/src/ros_pc2.h b/ros/src/ros_pc2.h index 8828ad4..4365093 100644 --- a/ros/src/ros_pc2.h +++ b/ros/src/ros_pc2.h @@ -191,6 +191,16 @@ inline LidarFormat infer_lidar_order(const std::vector &raw, model.row_major = raw[0].row == raw[1].row; model.all_points_present = (raw.size() == num_rows * num_columns); + // If all points are present, we can infer the firing order + // by looking at the first num_rows points + if (model.all_points_present && !model.row_major) { + std::vector map_fire_to_row(num_rows, -1); + for (size_t i = 0; i < num_rows; ++i) { + map_fire_to_row[i] = raw[i].row; + } + model.map_row_to_fire = invert_map(map_fire_to_row); + } + return model; } From 99e58c52ee78a4d1a75c5864fe920f5007bc35df Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Tue, 24 Mar 2026 12:20:32 -0400 Subject: [PATCH 4/7] Fill out README --- ros/README.md | 46 ++++++++++++++++++++-------------------------- ros/src/format.hpp | 2 +- 2 files changed, 21 insertions(+), 27 deletions(-) diff --git a/ros/README.md b/ros/README.md index 2980e53..a9c01db 100644 --- a/ros/README.md +++ b/ros/README.md @@ -30,44 +30,38 @@ ros2 launch form odometry.launch.py --show-args ``` which will output the following: -| Parameter | Default | Description | -|--------------------|--------------|--------------------------------------| -| `topic` | `None` | Input point cloud topic | -| `num_columns` | `None` | LiDAR image width (columns) | -| `num_rows` | `None` | LiDAR image height (rows) | -| `min_range` | `1.0` | Minimum LiDAR range | -| `max_range` | `100.0` | Maximum LiDAR range | -| `visualize` | `true` | Launch RViz and publish point clouds | -| `bagfile` | `''` | Optional rosbag file/folder to play | -| `base_frame` | `''` | Base frame id | -| `lidar_odom_frame` | `odom_lidar` | Odometry frame id | -| `publish_odom_tf` | `True` | Publish odom->base TF | -| `invert_odom_tf` | `True` | Invert published odom TF | -| `use_sim_time` | `True` | Use simulation time | +| Parameter | Default | Description | +|--------------------|--------------|------------------------------------------------------------------------------------------------------------------------------------------| +| `topic` | `None` | Input point cloud topic (**Only required parameter!**) | +| `lidar_model` | `None` | LiDAR model parameters to use. See `format.hpp` for options. Sets num_columns, num_rows, min_range, max_range, and other format options. | +| `num_columns` | `Inferred` | LiDAR image width (columns). Overrides value from lidar_model. | +| `num_rows` | `Inferred` | LiDAR image height (rows). Overrides value from lidar_model. | +| `min_range` | `1.0` | Minimum LiDAR range. Overrides value from lidar_model. | +| `max_range` | `100.0` | Maximum LiDAR range. Overrides value from lidar_model. | +| `visualize` | `true` | Launch RViz and publish point clouds | +| `bagfile` | `''` | Optional rosbag file/folder to play | +| `base_frame` | `''` | Base frame id | +| `lidar_odom_frame` | `odom_lidar` | Odometry frame id | +| `publish_odom_tf` | `True` | Publish odom->base TF | +| `invert_odom_tf` | `True` | Invert published odom TF | +| `use_sim_time` | `True` | Use simulation time | If a bagfile is provided, the node will play the bagfile and process the point clouds. If not, it will just subscribe to the topic and process incoming point clouds in real-time. Thus as an example ```sh -ros2 launch form odometry.launch.py bagfile:= topic:= num_columns:= num_rows:= +ros2 launch form odometry.launch.py bagfile:= topic:= ``` ### Pointcloud Format -FORM requires point clouds to be in row-major order with no dropped points for its feature extraction method. It does it's best to infer the ordering and density of the point cloud using the following heuristics, +FORM requires point clouds to be in row-major order with no dropped points for its feature extraction method. It does it's best to infer the size, ordering, and density of the point cloud to reorder things properly. There are a handful of cases that can prove suboptimal if not manually set however, -| Format | Heuristic to Infer | -|----------------|----------------------------------------------------------| -| all points | Point cloud size equaling `num_columns` * `num_rows` | -| dropped points | Point cloud size not equaling `num_columns` * `num_rows` | -| row-major | Stationary ring number for first few points | -| column-major | Increasing ring number for first few points | - -IMPORTANT: If you're point cloud *does not* is not in row or column major format, FORM will crash! Please open an issue and we can add a flag to handle you're appropriate cloud formatting. (For example, I've seen some velodyne point clouds have ring order returned as 0, 8, 1, 9, 2, 10, ... This will break things) - -NOTE: If your point cloud is row major and has dropped invalid points, there is usually no way to figure out where along the scan line the dropped points belong. FORM places all of them at the end. This may have an impact on feature extraction, but things generally *should* still work. +1. **Column Major, Dropped Invalid Points**: In this case it's difficult (but not impossible) to tell which "column" the dropped/invalid points come from. To be able to do so, we need the returned "firing order" of a column. This is often sequential, but I've seen ring orders such as 0, 8, 1, 9, ... so forth. To set this, add your LiDAR format to format.hpp and pass it in with `lidar_model`. If it is not set, all dropped points will be added at the end of the scanline. FORM will still run fine, but likely not as accurately. +2. **Row Major, Dropped Invalid Points**: This is very rare, but when it does occur it *is* impossible to tell which column the dropped points are from. In this case, FORM places them at the end of the scanline. +FORM will output details about it's point cloud format inference for debugging purposes if you ever hit any edge cases that don't seem to be working. ### Development diff --git a/ros/src/format.hpp b/ros/src/format.hpp index b3ef772..5cd8c6b 100644 --- a/ros/src/format.hpp +++ b/ros/src/format.hpp @@ -25,7 +25,7 @@ inline std::ostream &operator<<(std::ostream &os, const LidarFormat &format) { return os; } -/// Invert a map from firing order to row index into a map from row index to firing +/// Invert a map from firing order -> row index into a map from row index -> firing /// order /// /// By default printing out the row index in the order they are fired gives From b22cc463d809fec6c879de2c5938b5b0ec4e9d2b Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Tue, 24 Mar 2026 12:24:08 -0400 Subject: [PATCH 5/7] One more README tweak --- ros/README.md | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/ros/README.md b/ros/README.md index a9c01db..68f1ba1 100644 --- a/ros/README.md +++ b/ros/README.md @@ -21,8 +21,7 @@ source ./install/setup.bash ### Running -The main launch file is `odometry.launch.py` which will launch the odometry node. FORM has three required inputs, **topic name**, **number of scanlines/rings/columns**, and **number of rows/circular count**. This last two are required for FORM's feature extraction. - +The main launch file is `odometry.launch.py` which will launch the odometry node. FORM only requires the point cloud **topic** to operate. To view all the available arguments, you can run: ```sh @@ -58,10 +57,10 @@ ros2 launch form odometry.launch.py bagfile:= topic:= Date: Tue, 24 Mar 2026 15:05:50 -0400 Subject: [PATCH 6/7] Apply suggestions from code review Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- ros/README.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/ros/README.md b/ros/README.md index 68f1ba1..eebe864 100644 --- a/ros/README.md +++ b/ros/README.md @@ -29,15 +29,15 @@ ros2 launch form odometry.launch.py --show-args ``` which will output the following: -| Parameter | Default | Description | -|--------------------|--------------|------------------------------------------------------------------------------------------------------------------------------------------| -| `topic` | `None` | Input point cloud topic (**Only required parameter!**) | -| `lidar_model` | `None` | LiDAR model parameters to use. See `format.hpp` for options. Sets num_columns, num_rows, min_range, max_range, and other format options. | -| `num_columns` | `Inferred` | LiDAR image width (columns). Overrides value from lidar_model. | -| `num_rows` | `Inferred` | LiDAR image height (rows). Overrides value from lidar_model. | -| `min_range` | `1.0` | Minimum LiDAR range. Overrides value from lidar_model. | -| `max_range` | `100.0` | Maximum LiDAR range. Overrides value from lidar_model. | -| `visualize` | `true` | Launch RViz and publish point clouds | +| Parameter | Default | Description | +|--------------------|---------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| `topic` | `None` | Input point cloud topic (**Only required parameter!**) | +| `lidar_model` | `"" (auto)` | LiDAR model parameters to use. Empty string lets the node infer/use model defaults from the data. See `format.hpp` for options. Sets num_columns, num_rows, ranges, etc. | +| `num_columns` | `Inferred` | LiDAR image width (columns). Overrides value from lidar_model. | +| `num_rows` | `Inferred` | LiDAR image height (rows). Overrides value from lidar_model. | +| `min_range` | `0.0 (auto)` | Minimum LiDAR range. `0.0` means infer/use defaults from lidar_model (falling back to the sensor’s nominal minimum range, currently 0.1 m). | +| `max_range` | `0.0 (auto)` | Maximum LiDAR range. `0.0` means infer/use defaults from lidar_model (falling back to the sensor’s nominal maximum range, currently 100.0 m). | +| `visualize` | `true` | Launch RViz and publish point clouds | | `bagfile` | `''` | Optional rosbag file/folder to play | | `base_frame` | `''` | Base frame id | | `lidar_odom_frame` | `odom_lidar` | Odometry frame id | From e1d56c57eb9a8cf300992d33f6c0987f588250b0 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Tue, 24 Mar 2026 15:07:21 -0400 Subject: [PATCH 7/7] Small typo fixes, some edge case guards --- ros/README.md | 8 ++++---- ros/src/node.cpp | 25 +++++++++++++++++++++---- ros/src/ros_pc2.h | 1 + 3 files changed, 26 insertions(+), 8 deletions(-) diff --git a/ros/README.md b/ros/README.md index eebe864..c615ee2 100644 --- a/ros/README.md +++ b/ros/README.md @@ -21,7 +21,7 @@ source ./install/setup.bash ### Running -The main launch file is `odometry.launch.py` which will launch the odometry node. FORM only requires the point cloud **topic** to operate. +The main launch file is `odometry.launch.py` which will launch the odometry node. FORM only requires the point cloud **topic** parameter and requires the topic to have type `PointCloud2` with fields `x`, `y`, `z`, and `ring`/`row`/`channel`. To view all the available arguments, you can run: ```sh @@ -55,12 +55,12 @@ ros2 launch form odometry.launch.py bagfile:= topic:=("num_rows", 0); params.extraction.num_columns = declare_parameter("num_columns", 0); - if(lidar_format_.has_value() && params.extraction.num_rows == 0) { - params.extraction.num_rows = lidar_format_->num_rows; + if(lidar_format_.has_value()) { + // Use lidar_model if no user input, but override with user input if provided, otherwise rely on inference + if(params.extraction.num_rows == 0) { + params.extraction.num_rows = lidar_format_->num_rows; + } else { + lidar_format_->num_rows = params.extraction.num_rows; + } } - if(lidar_format_.has_value() && params.extraction.num_columns == 0) { - params.extraction.num_columns = lidar_format_->num_columns; + if(lidar_format_.has_value() ) { + if(params.extraction.num_columns == 0) { + params.extraction.num_columns = lidar_format_->num_columns; + } else { + lidar_format_->num_columns = params.extraction.num_columns; + } } params.extraction.neighbor_points = declare_parameter("neighbor_points", params.extraction.neighbor_points); params.extraction.num_sectors = declare_parameter("num_sectors", params.extraction.num_sectors); @@ -157,6 +166,14 @@ EstimatorNode::EstimatorNode(const rclcpp::NodeOptions &options) void EstimatorNode::register_frame( const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { + // A lot of work later requires at least 2 points + if (msg->width * msg->height <= 2) { + RCLCPP_WARN(this->get_logger(), + "Received PointCloud2 message with too few points: %d", + msg->width * msg->height); + return; + } + // Convert PointCloud2 -> RawPoints auto raw_points = form_ros::load_pc2(msg); diff --git a/ros/src/ros_pc2.h b/ros/src/ros_pc2.h index 4365093..0729ce4 100644 --- a/ros/src/ros_pc2.h +++ b/ros/src/ros_pc2.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include "format.hpp"