Adding excluded points publisher in track_with_cloud_node.

[h-wata]

Branch

noetic-devel

Description

Thanks to sharing great project.

I've noticed that the tracker_with_cloud node currently only publishes /detection_points. However, it would be highly beneficial to also have it publish excluded_points, which would be the original Point Cloud excluding the /detection_points.

For example, when doing Mapping or Localization, we may need a point cloud excluding people and cars. I think excluded_points would be useful in such cases.

Additional

I've implemented the publisher in my fork of the project. If you're interested, I'd be happy to send over a PR. Just let me know!

Are you willing to submit a PR?

• Yes I'd like to help by submitting a PR!

[Alpaca-zip]

Thank you @h-wata for your report! 👍🏻

Your idea is quite interesting and valuable, especially for SLAM where filtering out dynamic objects such as people and cars is essential. Since the KITTI datasets come with IMU data, it could be easy to evaluate this concept with SLAM like LIO-SAM.

I would really appreciate it if you could submit a Pull Request. Your contribution could significantly enhance the project's utility.

On another note, I am considering updating the noetic-devel branch to align with humble-devel. Specifically, I will move the VoxelGrid filter from euclideanClusterExtraction() to syncCallback().

ultralytics_ros/src/tracker_with_cloud_node.cpp

Lines 53 to 80 in a8a6a7e

	void TrackerWithCloudNode::syncCallback(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info_msg,
	const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud_msg,
	const ultralytics_ros::msg::YoloResult::ConstSharedPtr& yolo_result_msg)
	{
	rclcpp::Time current_call_time = this->now();
	rclcpp::Duration callback_interval = current_call_time - last_call_time_;
	last_call_time_ = current_call_time;
	pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_cloud(new pcl::PointCloud<pcl::PointXYZ>());
	downsampled_cloud = downsampleCloudMsg(cloud_msg);
	cam_model_.fromCameraInfo(camera_info_msg);
	pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
	transformed_cloud = cloud2TransformedCloud(downsampled_cloud, cloud_msg->header.frame_id, cam_model_.tfFrame(),
	cloud_msg->header.stamp);
	vision_msgs::msg::Detection3DArray detection3d_array_msg;
	sensor_msgs::msg::PointCloud2 detection_cloud_msg;
	projectCloud(transformed_cloud, yolo_result_msg, cloud_msg->header, detection3d_array_msg, detection_cloud_msg);
	visualization_msgs::msg::MarkerArray marker_array_msg =
	createMarkerArray(detection3d_array_msg, callback_interval.seconds());
	detection3d_pub_->publish(detection3d_array_msg);
	detection_cloud_pub_->publish(detection_cloud_msg);
	marker_pub_->publish(marker_array_msg);
	}

ultralytics_ros/src/tracker_with_cloud_node.cpp

Lines 266 to 304 in a8a6a7e

	TrackerWithCloudNode::euclideanClusterExtraction(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
	{
	this->get_parameter("cluster_tolerance", cluster_tolerance_);
	this->get_parameter("min_cluster_size", min_cluster_size_);
	this->get_parameter("max_cluster_size", max_cluster_size_);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	std::vector<pcl::PointIndices> cluster_indices;
	pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
	ec.setClusterTolerance(cluster_tolerance_);
	ec.setMinClusterSize(min_cluster_size_);
	ec.setMaxClusterSize(max_cluster_size_);
	ec.setSearchMethod(tree);
	ec.setInputCloud(cloud);
	ec.extract(cluster_indices);
	float min_distance = std::numeric_limits<float>::max();
	pcl::PointCloud<pcl::PointXYZ>::Ptr closest_cluster(new pcl::PointCloud<pcl::PointXYZ>());
	for (const auto& cluster : cluster_indices)
	{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>());
	for (const auto& indice : cluster.indices)
	{
	cloud_cluster->push_back((*cloud)[indice]);
	}
	Eigen::Vector4f centroid;
	pcl::compute3DCentroid(*cloud_cluster, centroid);
	float distance = centroid.norm();
	if (distance < min_distance)
	{
	min_distance = distance;
	*closest_cluster = *cloud_cluster;
	}
	}
	return closest_cluster;
	}

In that case, Instead of creating a new function like processPointsWithMask(), it may be more efficient to refine euclideanClusterExtraction() to directly extract the removed_cloud from pcl::PointIndices.

Best regards,

[h-wata]

Thank you for your replay and sharing the road map of this project.

In that case, Instead of creating a new function like processPointsWithMask(), it may be more efficient to refine euclideanClusterExtraction() to directly extract the removed_cloud from pcl::PointIndices.

I think your suggestion is a good idea, but how do you deal with the case of multiple targets?
I am concerned that combining removed_cloud with another removed_cloud, as we do with detection_cloud, might recreate the original point cloud. Wouldn't it be simpler to exclude combine_detection_cloud from the original point cloud for clarity?

[Alpaca-zip]

I think your suggestion is a good idea, but how do you deal with the case of multiple targets?
I am concerned that combining removed_cloud with another removed_cloud, as we do with detection_cloud, might recreate the original point cloud. Wouldn't it be simpler to exclude combine_detection_cloud from the original point cloud for clarity?

I see that your point is valid after looking at the code again. It looks like significant changes to the projectCloud() function would be necessary to split the original point cloud into combine_detection_cloud and excluded_cloud using pcl::PointIndices.

Considering a short-term implementation of this feature, I think your approach should be fine. Could you please go ahead and submit a pull request? I'll do a functionality check and a detailed code review.

However, due to my current academic schedule, I may be a bit slow to respond. Thank you for your understanding.

Best regards,