feat: ROS2 point reordering

ros/README.md

@@ -21,53 +21,46 @@ 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
 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:=<path_to_rosbag> topic:=<topic_name> num_columns:=<num_columns> num_rows:=<num_rows>
+ros2 launch form odometry.launch.py bagfile:=<path_to_rosbag> topic:=<topic_name>
 ```
 
 ### 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
 

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,

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"

ros/src/format.hpp

@@ -0,0 +1,54 @@
+#pragma once
+
+#include <map>
+#include <optional>
+#include <ostream>
+#include <string>
+#include <vector>
+
+struct LidarFormat {
+  int num_rows = 0;
+  int num_columns = 0;
+  bool row_major = false;
+  bool all_points_present = false;
+  std::optional<std::vector<long>> 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<long> invert_map(const std::vector<long> &map_fire_to_row) {
+  std::vector<long> 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<long> default_row_to_fire(size_t num_rows) {
+  std::vector<long> 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<std::string, LidarFormat> 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
+};

ros/src/node.cpp

@@ -6,12 +6,12 @@
 // (Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
 // Stachniss)
 #include <memory>
+#include <rclcpp/logging.hpp>
 #include <utility>
 
 // 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<double>("position_covariance", 0.1);
   orientation_covariance_ = declare_parameter<double>("orientation_covariance", 0.1);
 
+  // LiDAR format
+  std::string model_name = declare_parameter<std::string>("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<int>("num_rows", params.extraction.num_rows);
-  params.extraction.num_columns = declare_parameter<int>("num_columns", params.extraction.num_columns);
-  auto min_range = declare_parameter<double>("min_range", 1.0);
-  auto max_range = declare_parameter<double>("max_range", 100.0);
+  // Feature extraction
+  auto min_range = declare_parameter<double>("min_range", 0.0);
+  auto max_range = declare_parameter<double>("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<int>("num_rows", 0);
+  params.extraction.num_columns = declare_parameter<int>("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<int>("neighbor_points", params.extraction.neighbor_points);
   params.extraction.num_sectors             = declare_parameter<int>("num_sectors", params.extraction.num_sectors);
   params.extraction.planar_threshold        = declare_parameter<double>("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<PointXYZf> 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);

ros/src/node.hpp

@@ -9,9 +9,11 @@
 
 // FORM
 #include "form/form.hpp"
+#include "format.hpp"
 #include "utils.hpp"
 
 // ROS 2
+#include <optional>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
@@ -56,6 +58,9 @@ class EstimatorNode : public rclcpp::Node {
   bool publish_odom_tf_;
   bool publish_debug_clouds_;
 
+  /// For extracting geometry
+  std::optional<LidarFormat> lidar_format_;
+
   /// Data subscribers.
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;