[feature] add MMS cropbox filter

config/config_preprocess.json

@@ -9,6 +9,10 @@
   // enable_outlier_removal      : If true, enable statistical outlier removal
   // outlier_removal_k           : Number of neighbors for outlier removal
   // outlier_std_mul_factor      : Standard deviation multiplier for outlier removal
+  // enable_cropbox_filter       : If true, filter points out points within box
+  // crop_bbox_frame             : Bounding box reference frame
+  // crop_bbox_min               : Bounding box min point
+  // crop_bbox_max               : Bounding box max point
   // k_correspondences           : Number of neighbors for covariance estimation
   // num_threads                 : Number of threads for preprocessing
   */
@@ -22,6 +26,10 @@
     "enable_outlier_removal": false,
     "outlier_removal_k": 10,
     "outlier_std_mul_factor": 1.0,
+    "enable_cropbox_filter": false,
+    "crop_bbox_frame": "lidar",
+    "crop_bbox_min": [-1.0, -1.0, -1.0],
+    "crop_bbox_max": [1.0, 1.0, 1.0],
     "k_correspondences": 10,
     "num_threads": 2
   }

include/glim/preprocess/cloud_preprocessor.hpp

@@ -28,9 +28,14 @@ struct CloudPreprocessorParams {
   bool enable_outlier_removal;        ///< If true, apply statistical outlier removal
   int outlier_removal_k;              ///< Number of neighbors used for outlier removal
   double outlier_std_mul_factor;      ///< Statistical outlier removal std dev threshold multiplication factor
+  bool enable_cropbox_filter;         ///< If true, filter points out points within box
+  std::string crop_bbox_frame;        ///< Bounding box reference frame
+  Eigen::Vector3d crop_bbox_min;      ///< Bounding box min point
+  Eigen::Vector3d crop_bbox_max;      ///< Bounding box max point
+  Eigen::Isometry3d T_imu_lidar;      ///< LiDAR-IMU transformation when cropbox is defined in IMU frame
   int k_correspondences;              ///< Number of neighboring points
 
-  int num_threads;  ///< Number of threads
+  int num_threads;                    ///< Number of threads
 };
 
 /**

src/glim/preprocess/cloud_preprocessor.cpp

@@ -9,6 +9,7 @@
 #include <gtsam_points/util/parallelism.hpp>
 
 #include <glim/util/config.hpp>
+#include <glim/util/convert_to_string.hpp>
 
 #ifdef GTSAM_POINTS_USE_TBB
 #include <tbb/task_arena.h>
@@ -33,6 +34,21 @@ CloudPreprocessorParams::CloudPreprocessorParams() {
   outlier_removal_k = config.param<int>("preprocess", "outlier_removal_k", 10);
   outlier_std_mul_factor = config.param<double>("preprocess", "outlier_std_mul_factor", 2.0);
 
+  enable_cropbox_filter = config.param<bool>("preprocess", "enable_cropbox_filter", false);
+  crop_bbox_frame = config.param<std::string>("preprocess", "crop_bbox_frame", "lidar");
+  crop_bbox_min = config.param<Eigen::Vector3d>("preprocess", "crop_bbox_min", {});
+  crop_bbox_max = config.param<Eigen::Vector3d>("preprocess", "crop_bbox_max", {});
+  Eigen::Isometry3d T_lidar_imu = sensor_config.param<Eigen::Isometry3d>("sensors", "T_lidar_imu", Eigen::Isometry3d::Identity());
+  T_imu_lidar = T_lidar_imu.inverse();
+
+  if (enable_cropbox_filter) {
+    if (crop_bbox_frame != "lidar" && crop_bbox_frame != "imu") {
+      throw std::runtime_error(fmt::format("Unsupported crop bbox frame: {}", crop_bbox_frame));
+    } else if ((crop_bbox_min.array() > crop_bbox_max.array()).any()) {
+      throw std::runtime_error(fmt::format("Misconfigured bbox: min={}, max={}", convert_to_string(crop_bbox_min), convert_to_string(crop_bbox_max)));
+    }
+  }
+
   k_correspondences = config.param<int>("preprocess", "k_correspondences", 8);
 
   num_threads = config.param<int>("preprocess", "num_threads", 2);
@@ -94,7 +110,7 @@ PreprocessedFrame::Ptr CloudPreprocessor::preprocess_impl(const RawPoints::Const
   indices.reserve(frame->size());
   double squared_distance_near_thresh = params.distance_near_thresh * params.distance_near_thresh;
   double squared_distance_far_thresh  = params.distance_far_thresh  * params.distance_far_thresh;
-  
+
   for (int i = 0; i < frame->size(); i++) {
     const bool is_finite = frame->points[i].allFinite();
     const double squared_dist = (Eigen::Vector4d() << frame->points[i].head<3>(), 0.0).finished().squaredNorm();
@@ -115,6 +131,34 @@ PreprocessedFrame::Ptr CloudPreprocessor::preprocess_impl(const RawPoints::Const
     std::fill(frame->times, frame->times + frame->size(), 0.0);
   }
 
+  // Cropbox filter
+  if (params.enable_cropbox_filter) {
+    if (params.crop_bbox_frame == "lidar") {
+      auto is_inside_bbox = [&](const Eigen::Vector3d& p_lidar) {
+        return (p_lidar.array() >= params.crop_bbox_min.array()).all()
+            && (p_lidar.array() <= params.crop_bbox_max.array()).all();
+      };
+
+      frame = gtsam_points::filter(frame, [&](const auto& pt) {
+        return !is_inside_bbox(pt.template head<3>());
+      });
+
+    } else if (params.crop_bbox_frame == "imu") {
+      auto is_inside_bbox = [&](const Eigen::Vector3d& p_lidar) {
+        const auto p_imu = params.T_imu_lidar * p_lidar;
+        return (p_imu.array() >= params.crop_bbox_min.array()).all()
+            && (p_imu.array() <= params.crop_bbox_max.array()).all();
+      };
+
+      frame = gtsam_points::filter(frame, [&](const auto& pt) {
+        return !is_inside_bbox(pt.template head<3>());
+      });
+
+    } else {
+      throw std::runtime_error(fmt::format("Unsupported crop bbox frame: {}", params.crop_bbox_frame));
+    }
+  }
+
   // Outlier removal
   if (params.enable_outlier_removal) {
     frame = gtsam_points::remove_outliers(frame, params.outlier_removal_k, params.outlier_std_mul_factor, params.num_threads);