tuning

Author: juhe9842

elevation_mapping_cupy/config/setups/g1/g1_parameters.yaml

@@ -2,7 +2,7 @@
 resolution: 0.04                                # resolution in m.
 map_length: 8                                   # map's size in m.
 sensor_noise_factor: 0.05                       # point's noise is sensor_noise_factor*z^2 (z is distance from sensor).
-mahalanobis_thresh: 2.0                         # points outside this distance is outlier.
+mahalanobis_thresh: 1.5                        # points outside this distance is outlier.
 outlier_variance: 0.01                          # if point is outlier, add this value to the cell.
 drift_compensation_variance_inler: 0.05         # cells under this value is used for drift compensation.
 max_drift: 0.1                                  # drift compensation happens only the drift is smaller than this value (for safety)
@@ -12,7 +12,7 @@ max_variance: 100.0                             # maximum variance for each cell
 initial_variance: 1000.0                        # initial variance for each cell.
 traversability_inlier: 0.9                      # cells with higher traversability are used for drift compensation.
 dilation_size: 3                                # dilation filter size before traversability filter.
-wall_num_thresh: 8                             # if there are more points than this value, only higher points than the current height are used to make the wall more sharp.
+wall_num_thresh: 20                             # if there are more points than this value, only higher points than the current height are used to make the wall more sharp.
 min_height_drift_cnt: 100                       # drift compensation only happens if the valid cells are more than this number.
 position_noise_thresh: 0.01                     # if the position change is bigger than this value, the drift compensation happens.
 orientation_noise_thresh: 0.01                  # if the orientation change is bigger than this value, the drift compensation happens.
@@ -23,15 +23,15 @@ max_height_range: -0.1                           # points higher than this value
 ramped_height_range_a: 0.3                      # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
 ramped_height_range_b: 1.0                      # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
 ramped_height_range_c: 0.2                      # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
-update_variance_fps: 5.0                        # fps for updating variance.
-update_pose_fps: 10.0                           # fps for updating pose and shift the center of map.
+update_variance_fps: 10.0                       # fps for updating variance.
+update_pose_fps: 20.0                           # fps for updating pose and shift the center of map.
 time_interval: 0.1                              # Time layer is updated with this interval.
-map_acquire_fps: 5.0                            # Raw map is fetched from GPU memory in this fps.
+map_acquire_fps: 10.0                           # Raw map is fetched from GPU memory in this fps.
 publish_statistics_fps: 1.0                     # Publish statistics topic in this fps.
 
 max_ray_length: 10.0                            # maximum length for ray tracing.
-cleanup_step: 0.001                               # subtitute this value from validity layer at visibiltiy cleanup.
-cleanup_cos_thresh: 0.8                         # subtitute this value from validity layer at visibiltiy cleanup.
+cleanup_step: 0.005                               # subtitute this value from validity layer at visibiltiy cleanup.
+cleanup_cos_thresh: 0.7                         # subtitute this value from validity layer at visibiltiy cleanup.
 
 safe_thresh: 0.7                                # if traversability is smaller, it is counted as unsafe cell.
 safe_min_thresh: 0.4                            # polygon is unsafe if there exists lower traversability than this.
@@ -47,7 +47,7 @@ corrected_map_frame: 'odom_corrected'
 #### Feature toggles ########
 enable_edge_sharpen: true
 enable_visibility_cleanup: true
-enable_drift_compensation: true
+enable_drift_compensation: false
 enable_overlap_clearance: true
 enable_pointcloud_publishing: false
 enable_drift_corrected_TF_publishing: false

elevation_mapping_cupy/config/setups/g1/g1_sensor_parameter.yaml

@@ -37,6 +37,13 @@ subscribers:
   Livox:
     topic_name: /livox/lidar_filtered
     data_type: pointcloud
-  D435:
-    topic_name: /camera/depth_registered/points
-    data_type: pointcloud
+    # Frame whose origin is the true sensor optical center, used as the visibility-cleanup
+    # ray source (and point-validity sensor origin). Required because the cloud is deskewed
+    # into 'odom', so its header frame is no longer the sensor. If omitted, the cloud's
+    # header frame is used (original behavior).
+    ray_source_frame: livox_frame
+
+  # Odin:
+  #   topic_name: /odin1/cloud_slam
+  #   data_type: pointcloud
+  #   ray_source_frame: odin1_base_link

elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp

@@ -91,7 +91,7 @@ class ElevationMappingNode {
   void readParameters();
   void setupMapPublishers();
   void pointcloudCallback(const sensor_msgs::PointCloud2& cloud, const std::string& key);
-  void inputPointCloud(const sensor_msgs::PointCloud2& cloud, const std::vector<std::string>& channels);
+  void inputPointCloud(const sensor_msgs::PointCloud2& cloud, const std::vector<std::string>& channels, const std::string& key);
   void inputImage(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::vector<std::string>& channels);
   void imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::string& key);
   void imageChannelCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg);
@@ -159,6 +159,10 @@ class ElevationMappingNode {
   std::set<double> map_fps_unique_;
   std::vector<ros::Timer> mapTimers_;
   std::map<std::string, std::vector<std::string>> channels_;
+  // Optional per-subscriber frame whose origin is used as the visibility-cleanup ray source
+  // (and point validity sensor origin), instead of the point cloud's header frame. Needed when
+  // clouds are published in a non-sensor frame (e.g. deskewed into odom).
+  std::map<std::string, std::string> raySourceFrames_;
 
   std::vector<std::string> initialize_frame_id_;
   std::vector<double> initialize_tf_offset_;

elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp

@@ -47,7 +47,7 @@ class ElevationMappingWrapper {
   void initialize(ros::NodeHandle& nh);
 
   void input(const RowMatrixXd& points, const std::vector<std::string>& channels, const RowMatrixXd& R, const Eigen::VectorXd& t,
-             const double positionNoise, const double orientationNoise);
+             const Eigen::VectorXd& sensorOrigin, const double positionNoise, const double orientationNoise);
   void input_image(const std::vector<ColMatrixXf>& multichannel_image, const std::vector<std::string>& channels, const RowMatrixXd& R,
                    const Eigen::VectorXd& t, const RowMatrixXd& cameraMatrix, const Eigen::VectorXd& D, const std::string distortion_model, int height, int width);
   void move_to(const Eigen::VectorXd& p, const RowMatrixXd& R);

elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py

@@ -313,16 +313,19 @@ def shift_translation_to_map_center(self, t):
         """
         t -= self.center
 
-    def update_map_with_kernel(self, points_all, channels, R, t, position_noise, orientation_noise):
+    def update_map_with_kernel(self, points_all, channels, R, t, position_noise, orientation_noise, sensor_t=None):
         """Update map with new measurement.
 
         Args:
             points_all (cupy._core.core.ndarray):
             channels (List[str]):
             R (cupy._core.core.ndarray):
-            t (cupy._core.core.ndarray):
+            t (cupy._core.core.ndarray): Translation of the point-cloud frame (used to place points).
             position_noise (float):
             orientation_noise (float):
+            sensor_t (cupy._core.core.ndarray): Origin of the sensor (ray_source_frame) used for
+                visibility-cleanup ray tracing and point validity checks. Defaults to ``t`` when None,
+                which reproduces the original behavior (cloud header frame == sensor frame).
         """
         self.new_map *= 0.0
         error = cp.array([0.0], dtype=cp.float32)
@@ -331,6 +334,13 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori
         # additional_fusion = self.get_fusion_of_pcl(channels)
         with self.map_lock:
             self.shift_translation_to_map_center(t)
+            # Resolve the sensor origin used for ray tracing / validity. It must be expressed
+            # in the same map-center-relative frame as t, so apply the same shift.
+            if sensor_t is None:
+                sensor_t = t
+            else:
+                sensor_t = cp.asarray(sensor_t, dtype=self.data_type).copy()
+                self.shift_translation_to_map_center(sensor_t)
             self.error_counting_kernel(
                 self.elevation_map,
                 points,
@@ -360,6 +370,7 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori
                 cp.array([0.0], dtype=self.data_type),
                 R,
                 t,
+                sensor_t,
                 self.normal_map,
                 points,
                 self.elevation_map,
@@ -439,16 +450,19 @@ def input_pointcloud(
         t: cp._core.core.ndarray,
         position_noise: float,
         orientation_noise: float,
+        sensor_t: cp._core.core.ndarray = None,
     ):
         """Input the point cloud and fuse the new measurements to update the elevation map.
 
         Args:
             raw_points (cupy._core.core.ndarray):
             channels (List[str]):
             R  (cupy._core.core.ndarray):
-            t (cupy._core.core.ndarray):
+            t (cupy._core.core.ndarray): Translation of the point-cloud frame.
             position_noise (float):
             orientation_noise (float):
+            sensor_t (cupy._core.core.ndarray): Origin of the sensor (ray_source_frame) used for
+                visibility cleanup and validity checks. Defaults to ``t`` when None.
 
         Returns:
             None:
@@ -463,6 +477,7 @@ def input_pointcloud(
             cp.asarray(t, dtype=self.data_type),
             position_noise,
             orientation_noise,
+            None if sensor_t is None else cp.asarray(sensor_t, dtype=self.data_type),
         )
 
     def input_image(

elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_kernels.py

@@ -142,7 +142,7 @@ def add_points_kernel(
     enable_visibility_cleanup=True,
 ):
     add_points_kernel = cp.ElementwiseKernel(
-        in_params="raw U center_x, raw U center_y, raw U R, raw U t, raw U norm_map",
+        in_params="raw U center_x, raw U center_y, raw U R, raw U t, raw U sensor_t, raw U norm_map",
         out_params="raw U p, raw U map, raw T newmap",
         preamble=map_utils(
             resolution,
@@ -165,7 +165,7 @@ def add_points_kernel(
             U z = transform_p(rx, ry, rz, R[6], R[7], R[8], t[2]);
             U v = z_noise(rz);
             int idx = get_idx(x, y, center_x[0], center_y[0]);
-            if (is_valid(x, y, z, t[0], t[1], t[2])) {
+            if (is_valid(x, y, z, sensor_t[0], sensor_t[1], sensor_t[2])) {
                 if (is_inside(idx)) {
                     U map_h = map[get_map_idx(idx, 0)];
                     U map_v = map[get_map_idx(idx, 1)];
@@ -197,7 +197,11 @@ def add_points_kernel(
             }
             if (${enable_visibility_cleanup}) {
                 float16 ray_x, ray_y, ray_z;
-                float16 ray_length = ray_vector(t[0], t[1], t[2], x, y, z, ray_x, ray_y, ray_z);
+                // Visibility cleanup traces rays from the sensor origin. Use sensor_t (the
+                // configured ray_source_frame origin) instead of t (the point-cloud frame
+                // origin), so deskewed clouds whose header frame is not the sensor still get
+                // the true sensor optical center as the ray start. Defaults to t upstream.
+                float16 ray_length = ray_vector(sensor_t[0], sensor_t[1], sensor_t[2], x, y, z, ray_x, ray_y, ray_z);
                 ray_length = min(ray_length, (float16)${max_ray_length});
                 int last_nidx = -1;
                 for (float16 s=${ray_step}; s < ray_length; s+=${ray_step}) {
@@ -223,7 +227,7 @@ def add_points_kernel(
 
                     // If point is close or is farther away than ray length, skip.
                     float16 d = (x - nx) * (x - nx) + (y - ny) * (y - ny) + (z - nz) * (z - nz);
-                    if (d < 0.1 || !is_valid(x, y, z, t[0], t[1], t[2])) {continue;}
+                    if (d < 0.1 || !is_valid(x, y, z, sensor_t[0], sensor_t[1], sensor_t[2])) {continue;}
 
                     // If invalid, do upper bound check, then skip
                     if (nmap_valid < 0.5) {
@@ -258,7 +262,7 @@ def add_points_kernel(
                 }
             }
             p[i * 3]= idx;
-            p[i * 3 + 1] = is_valid(x, y, z, t[0], t[1], t[2]);
+            p[i * 3 + 1] = is_valid(x, y, z, sensor_t[0], sensor_t[1], sensor_t[2]);
             p[i * 3 + 2] = is_inside(idx);
             """
         ).substitute(

elevation_mapping_cupy/src/elevation_mapping_ros.cpp

@@ -84,6 +84,12 @@ ElevationMappingNode::ElevationMappingNode(ros::NodeHandle& nh)
       channels_[key].push_back("x");
       channels_[key].push_back("y");
       channels_[key].push_back("z");
+      // Optional override of the frame used as the visibility-cleanup ray source. If the cloud is
+      // published in a non-sensor frame (e.g. deskewed into odom), set this to the true sensor frame.
+      if (subscriber.second.hasMember("ray_source_frame")) {
+        raySourceFrames_[key] = static_cast<std::string>(subscriber.second["ray_source_frame"]);
+        ROS_INFO_STREAM("[" << key << "] ray_source_frame override: " << raySourceFrames_[key]);
+      }
       boost::function<void(const sensor_msgs::PointCloud2&)> f = boost::bind(&ElevationMappingNode::pointcloudCallback, this, _1, key);
       ros::Subscriber sub = nh_.subscribe<sensor_msgs::PointCloud2>(pointcloud_topic, 1, f);
       pointcloudSubs_.push_back(sub);
@@ -310,14 +316,14 @@ void ElevationMappingNode::pointcloudCallback(const sensor_msgs::PointCloud2& cl
     auto& field = fields[it];
     channels.push_back(field.name);
   }
-  inputPointCloud(cloud, channels);
+  inputPointCloud(cloud, channels, key);
 
   // This is used for publishing as statistics.
   pointCloudProcessCounter_++;
 }
 
 void ElevationMappingNode::inputPointCloud(const sensor_msgs::PointCloud2& cloud,
-                                          const std::vector<std::string>& channels) {
+                                          const std::vector<std::string>& channels, const std::string& key) {
   auto start = ros::Time::now();
   auto* pcl_pc = new pcl::PCLPointCloud2;
   pcl::PCLPointCloud2ConstPtr cloudPtr(pcl_pc);
@@ -351,15 +357,53 @@ void ElevationMappingNode::inputPointCloud(const sensor_msgs::PointCloud2& cloud
     return;
   }
 
+  // Resolve the sensor origin used for visibility-cleanup ray tracing and point validity.
+  // By default this is the origin of the cloud's header frame. If a ray_source_frame is
+  // configured for this subscriber (e.g. because the cloud is deskewed into odom and its
+  // header frame no longer coincides with the sensor), look up that frame's origin instead.
+  Eigen::Vector3d rayOrigin = transformationSensorToMap.translation();
+  std::string rayFrameUsed = sensorFrameId;
+  auto rayFrameIt = raySourceFrames_.find(key);
+  if (rayFrameIt != raySourceFrames_.end() && !rayFrameIt->second.empty()) {
+    tf::StampedTransform raySourceTf;
+    Eigen::Affine3d raySourceToMap;
+    try {
+      transformListener_.waitForTransform(mapFrameId_, rayFrameIt->second, timeStamp, ros::Duration(1.0));
+      transformListener_.lookupTransform(mapFrameId_, rayFrameIt->second, timeStamp, raySourceTf);
+      poseTFToEigen(raySourceTf, raySourceToMap);
+      rayOrigin = raySourceToMap.translation();
+      rayFrameUsed = rayFrameIt->second;
+    } catch (tf::TransformException& ex) {
+      // A configured ray_source_frame that cannot be resolved must NOT silently fall back to the
+      // cloud header frame: that origin (e.g. odom) is exactly what causes the supporting surface
+      // to be erased by visibility cleanup. Drop this cloud instead and raise a loud alert.
+      ROS_ERROR_STREAM_THROTTLE(
+          2.0, "\n"
+                   << "========================================================================\n"
+                   << "  [ELEVATION MAPPING] ray_source_frame NOT AVAILABLE -- SKIPPING CLOUD\n"
+                   << "  subscriber key   : " << key << "\n"
+                   << "  ray_source_frame : " << rayFrameIt->second << "\n"
+                   << "  map frame        : " << mapFrameId_ << "\n"
+                   << "  TF error         : " << ex.what() << "\n"
+                   << "  This point cloud is NOT used for mapping. Fix the TF tree or the\n"
+                   << "  ray_source_frame setting in the sensor parameter YAML.\n"
+                   << "========================================================================");
+      return;
+    }
+  }
+  ROS_INFO_STREAM_THROTTLE(5.0, "[" << key << "] point frame: " << sensorFrameId << ", cleanup ray source frame: "
+                                    << rayFrameUsed << ", ray origin in " << mapFrameId_ << " = ["
+                                    << rayOrigin.x() << ", " << rayOrigin.y() << ", " << rayOrigin.z() << "]");
+
   double positionError{0.0};
   double orientationError{0.0};
   {
     std::lock_guard<std::mutex> lock(errorMutex_);
     positionError = positionError_;
     orientationError = orientationError_;
   }
-  map_.input(points, channels, transformationSensorToMap.rotation(), transformationSensorToMap.translation(), positionError,
-             orientationError);
+  map_.input(points, channels, transformationSensorToMap.rotation(), transformationSensorToMap.translation(), rayOrigin,
+             positionError, orientationError);
 
   if (enableDriftCorrectedTFPublishing_) {
     publishMapToOdom(map_.get_additive_mean_error());

elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp

@@ -171,10 +171,12 @@ void ElevationMappingWrapper::setParameters(ros::NodeHandle& nh) {
 }
 
 void ElevationMappingWrapper::input(const RowMatrixXd& points, const std::vector<std::string>& channels, const RowMatrixXd& R,
-                                    const Eigen::VectorXd& t, const double positionNoise, const double orientationNoise) {
+                                    const Eigen::VectorXd& t, const Eigen::VectorXd& sensorOrigin, const double positionNoise,
+                                    const double orientationNoise) {
   py::gil_scoped_acquire acquire;
   map_.attr("input_pointcloud")(Eigen::Ref<const RowMatrixXd>(points), channels, Eigen::Ref<const RowMatrixXd>(R),
-                     Eigen::Ref<const Eigen::VectorXd>(t), positionNoise, orientationNoise);
+                     Eigen::Ref<const Eigen::VectorXd>(t), positionNoise, orientationNoise,
+                     Eigen::Ref<const Eigen::VectorXd>(sensorOrigin));
 }
 
 void ElevationMappingWrapper::input_image(const std::vector<ColMatrixXf>& multichannel_image, const std::vector<std::string>& channels, const RowMatrixXd& R,