Added option to remove points in sensor origin (some sensors place points in sensor origin instead of using NaNs).

Author: carlosmccosta

include/dynamic_robot_localization/common/impl/pointcloud_conversions.hpp

@@ -28,7 +28,7 @@ bool fromROSMsg(const nav_msgs::OccupancyGrid& occupancy_grid, pcl::PointCloud<P
 				Eigen::Transform<float, 3, Eigen::Affine>(Eigen::Quaternionf(occupancy_grid.info.origin.orientation.w, occupancy_grid.info.origin.orientation.x, occupancy_grid.info.origin.orientation.y, occupancy_grid.info.origin.orientation.z));
 
 		pointcloud.height = 1;
-		pointcloud.is_dense = false;
+		pointcloud.is_dense = true;
 		pointcloud.header.frame_id = occupancy_grid.header.frame_id;
 		pointcloud.header.stamp = occupancy_grid.header.stamp.toNSec() / 1e3;
 		pointcloud.clear();

include/dynamic_robot_localization/common/impl/pointcloud_utils.hpp

@@ -54,6 +54,29 @@ void colorizePointCloudWithCurvature(pcl::PointCloud<PointT>& pointcloud) {
 }
 
 
+template <typename PointT>
+void removePointsOnSensorOrigin(pcl::PointCloud<PointT>& pointcloud) {
+	size_t number_of_original_points = pointcloud.size();
+	size_t number_of_valid_points = 0;
+	size_t number_of_invalid_points = 0;
+	for (size_t i = 0; i < pointcloud.size(); ++i) {
+		if (pointcloud[i].x != pointcloud.sensor_origin_.x() || pointcloud[i].y != pointcloud.sensor_origin_.y() || pointcloud[i].z != pointcloud.sensor_origin_.z()) {
+			if (number_of_invalid_points > 0) {
+				pointcloud[number_of_valid_points] = pointcloud[i];
+			}
+			++number_of_valid_points;
+		} else {
+			++number_of_invalid_points;
+		}
+	}
+	if (number_of_valid_points < number_of_original_points) {
+		pointcloud.resize(number_of_valid_points);
+		pointcloud.height = 1;
+		pointcloud.width = static_cast<std::uint32_t>(number_of_valid_points);
+	}
+}
+
+
 template <typename PointT>
 void colorizePointCloudClusters(const pcl::PointCloud<PointT>& pointcloud, const std::vector<pcl::PointIndices>& cluster_indices, pcl::PointCloud<PointT>& pointcloud_colored_out) {
 	for (size_t cluster_index = 0; cluster_index < cluster_indices.size(); ++cluster_index) {

include/dynamic_robot_localization/common/pointcloud_utils.h

@@ -31,6 +31,9 @@ void concatenatePointClouds(const std::vector< typename pcl::PointCloud<PointT>:
 template <typename PointT>
 void colorizePointCloudWithCurvature(pcl::PointCloud<PointT>& pointcloud);
 
+template <typename PointT>
+void removePointsOnSensorOrigin(pcl::PointCloud<PointT>& pointcloud);
+
 template <typename PointT>
 void colorizePointCloudClusters(const pcl::PointCloud<PointT>& pointcloud, const std::vector<pcl::PointIndices>& cluster_indices, pcl::PointCloud<PointT>& pointcloud_colored_out);
 

include/dynamic_robot_localization/localization/impl/localization.hpp

@@ -455,6 +455,8 @@ void Localization<PointT>::setupMessageManagement(const std::string& configurati
 	private_node_handle_->param(configuration_namespace + "message_management/min_seconds_between_reference_pointcloud_update", min_seconds_between_reference_pointcloud_update, 5.0);
 	min_seconds_between_reference_pointcloud_update_.fromSec(min_seconds_between_reference_pointcloud_update);
 
+	private_node_handle_->param(configuration_namespace + "message_management/remove_points_in_sensor_origin", remove_points_in_sensor_origin_, false);
+
 	private_node_handle_->param(configuration_namespace + "message_management/minimum_number_of_points_in_ambient_pointcloud", minimum_number_of_points_in_ambient_pointcloud_, 10);
 
 	private_node_handle_->param(configuration_namespace + "message_management/circular_buffer_require_reception_of_pointcloud_msgs_from_all_topics_before_doing_registration", circular_buffer_require_reception_of_pointcloud_msgs_from_all_topics_before_doing_registration_, false);
@@ -1735,14 +1737,22 @@ bool Localization<PointT>::processAmbientPointCloud(typename pcl::PointCloud<Poi
 		}
 
 		tf2::Transform pose_tf_initial_guess = last_accepted_pose_odom_to_map_ * transform_base_link_to_odom;
+
 		size_t ambient_pointcloud_size = ambient_pointcloud->size();
 		std::vector<int> indexes;
 		ambient_pointcloud->is_dense = false;
+		ROS_DEBUG_STREAM("Removing NaNs from ambient cloud with " << ambient_pointcloud_size << " points");
 		pcl::removeNaNFromPointCloud(*ambient_pointcloud, *ambient_pointcloud, indexes);
 		indexes.clear();
 		size_t number_of_nans_in_ambient_pointcloud = ambient_pointcloud_size - ambient_pointcloud->size();
-		if (number_of_nans_in_ambient_pointcloud > 0) {
-			ROS_DEBUG_STREAM("Removed " << number_of_nans_in_ambient_pointcloud << " NaNs from ambient cloud with " << ambient_pointcloud_size << " points");
+		ROS_DEBUG_STREAM("Removed " << number_of_nans_in_ambient_pointcloud << " NaNs from ambient cloud with " << ambient_pointcloud_size << " points");
+
+		if (remove_points_in_sensor_origin_) {
+			size_t number_of_points_in_ambient_pointcloud_before_sensor_origin_removal = ambient_pointcloud->size();
+			ROS_DEBUG_STREAM("Removing points in sensor origin from a ambient cloud with " << number_of_points_in_ambient_pointcloud_before_sensor_origin_removal << " points");
+			pointcloud_utils::removePointsOnSensorOrigin(*ambient_pointcloud);
+			size_t number_of_points_in_sensor_origin_in_ambient_pointcloud = number_of_points_in_ambient_pointcloud_before_sensor_origin_removal - ambient_pointcloud->size();
+			ROS_DEBUG_STREAM("Removed " << number_of_points_in_sensor_origin_in_ambient_pointcloud << " points in sensor origin from ambient cloud with " << number_of_points_in_ambient_pointcloud_before_sensor_origin_removal << " points");
 		}
 
 		tf2::Quaternion pose_tf_initial_guess_q = pose_tf_initial_guess.getRotation().normalize();

include/dynamic_robot_localization/localization/localization.h

@@ -404,6 +404,7 @@ class Localization : public ConfigurableObject {
 		ros::Duration pose_tracking_timeout_;
 		ros::Duration pose_tracking_recovery_timeout_;
 		ros::Duration initial_pose_estimation_timeout_;
+		bool remove_points_in_sensor_origin_;
 		int minimum_number_of_points_in_ambient_pointcloud_;
 		int minimum_number_of_points_in_reference_pointcloud_;
 		bool localization_detailed_use_millimeters_in_root_mean_square_error_inliers_;

yaml/schema/drl_configs.yaml

@@ -140,6 +140,7 @@ message_management:
     max_seconds_ambient_pointcloud_offset_to_last_estimated_pose: 0.0   # Point clouds that older than this offset in relation to the last [estimated pose / sensor data received] are discarded (useful when there are several sources of sensor data and one has higher update rate -> ex: kinect+lasers) -> for disabling this check, set to <= 0
     min_seconds_between_scan_registration: 0.0                          # Ambient point clouds received before this duration is reached (after a successful pose estimation) will be discarded -> for disabling this check, set to <= 0
     min_seconds_between_reference_pointcloud_update: 5.0                # Clouds coming from topics reference_costmap_topic | reference_pointcloud_topic will be discarded if the last reference cloud was updated less than [this value] seconds ago
+    remove_points_in_sensor_origin: false
     minimum_number_of_points_in_ambient_pointcloud: 10
     circular_buffer_require_reception_of_pointcloud_msgs_from_all_topics_before_doing_registration: false
     circular_buffer_clear_inserted_points_if_registration_fails: false