diff --git a/ros/README.md b/ros/README.md index 2980e53..c615ee2 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** 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 @@ -30,44 +29,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` | `"" (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 | +| `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, - -| 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) +FORM requires point clouds to be in row-major order with no dropped points for its feature extraction method. It does its 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, -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 is difficult (but not impossible) to tell where along the scanline 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 quite as accurately. +2. **Row Major, Dropped Invalid Points**: This is very rare, but when it does occur it *is* impossible to tell where along the scanline points belong. In this case, FORM places them at the end of the scanline. +FORM will output details about its point cloud format inference for debugging purposes if you ever hit any edge cases that don't seem to be working. Generally fixes *should* be as easy as setting the parameters that FORM incorrectly inferred. ### Development diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index 8c2500a..797e755 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,10 +70,14 @@ 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)") - min_range = make_config(la, "min_range", "Minimum LiDAR range", 1.0) - max_range = make_config(la, "max_range", "Maximum LiDAR range", 100.0) + # either specify a model + lidar_model = make_config(la, "lidar_model", "Predefined LiDAR model (e.g. 'VLP-16')", "") + # 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) + min_range = make_config(la, "min_range", "Minimum LiDAR range", 0.0) + max_range = make_config(la, "max_range", "Maximum LiDAR range", 0.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", "") @@ -101,11 +106,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/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 new file mode 100644 index 0000000..5cd8c6b --- /dev/null +++ b/ros/src/format.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include +#include +#include +#include +#include + +struct LidarFormat { + int num_rows = 0; + int num_columns = 0; + 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 + << ", min_range=" << format.min_range << ", max_range=" << format.max_range + << " )"; + return os; +} + +/// 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 +/// 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 = { + {"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 902af0a..5645157 100644 --- a/ros/src/node.cpp +++ b/ros/src/node.cpp @@ -6,12 +6,12 @@ // (Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill // Stachniss) #include +#include #include // FORM-ROS #include "node.hpp" #include "ros_pc2.h" -#include "utils.hpp" // FORM #include "form/form.hpp" @@ -62,18 +62,52 @@ 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; + RCLCPP_INFO(this->get_logger(), "Using LiDAR format for model '%s'", model_name.c_str()); + } + } + // 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); - auto min_range = declare_parameter("min_range", 1.0); - auto max_range = declare_parameter("max_range", 100.0); + // Feature extraction + 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; - // Feature extraction + // 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()) { + // 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() ) { + 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); params.extraction.planar_threshold = declare_parameter("planar_threshold", params.extraction.planar_threshold); @@ -132,10 +166,53 @@ EstimatorNode::EstimatorNode(const rclcpp::NodeOptions &options) void EstimatorNode::register_frame( const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { - // Convert PointCloud2 -> organized vector using pc2_conversions + // 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); + + // 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); + 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; + } + 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); + 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 = - 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..0729ce4 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 @@ -13,14 +14,26 @@ #include #include #include +#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 +95,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 +152,174 @@ 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); + + // 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; +} + +// ------------------------- 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 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.value()[curr_row] < + model.map_row_to_fire.value()[prev_row]) { + col = prev_col + 1; + } else { + col = prev_col; + } + }; + + _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, + std::optional logger = std::nullopt) { + const int n_points = static_cast(raw.size()); - // 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); + // 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) { + // 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); + 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