From 417ae6ce2c0bda58860c31235c08c9c0987d21d1 Mon Sep 17 00:00:00 2001 From: Meshva Desai Date: Sun, 9 Jun 2024 23:01:29 +0000 Subject: [PATCH] linter --- .../tracking/dets_2d_3d/include/cluster.hpp | 35 +- .../dets_2d_3d/include/dets_2d_3d_node.hpp | 122 ++--- .../dets_2d_3d/include/projection_utils.hpp | 99 ++-- .../tracking/dets_2d_3d/src/cluster.cpp | 178 +++---- .../dets_2d_3d/src/dets_2d_3d_node.cpp | 222 +++++---- .../dets_2d_3d/src/projection_utils.cpp | 452 +++++++++--------- 6 files changed, 571 insertions(+), 537 deletions(-) diff --git a/src/perception/tracking/dets_2d_3d/include/cluster.hpp b/src/perception/tracking/dets_2d_3d/include/cluster.hpp index 13bad5d4..b19c4241 100644 --- a/src/perception/tracking/dets_2d_3d/include/cluster.hpp +++ b/src/perception/tracking/dets_2d_3d/include/cluster.hpp @@ -1,34 +1,33 @@ #pragma once -#include #include +#include #include #include -class Cluster -{ +class Cluster { public: - // makes a cloud from all points `in_cloud_ptr` that are indexed by `in_cluster_indices` - Cluster( - const pcl::PointCloud::Ptr& in_cloud_ptr, - const std::vector& in_cluster_indices); + // makes a cloud from all points `in_cloud_ptr` that are indexed by + // `in_cluster_indices` + Cluster(const pcl::PointCloud::Ptr &in_cloud_ptr, + const std::vector &in_cluster_indices); - pcl::PointXYZ getCentroid() { return centroid_; } - pcl::PointCloud::Ptr getCloud() { return cloud_; } - vision_msgs::msg::BoundingBox3D getBoundingBox(); + pcl::PointXYZ getCentroid() { return centroid_; } + pcl::PointCloud::Ptr getCloud() { return cloud_; } + vision_msgs::msg::BoundingBox3D getBoundingBox(); - bool isValid(); - int size() { return cloud_->size(); } + bool isValid(); + int size() { return cloud_->size(); } private: - pcl::PointCloud::Ptr cloud_; - pcl::PointXYZ centroid_; + pcl::PointCloud::Ptr cloud_; + pcl::PointXYZ centroid_; - pcl::PointXYZ max_point_; - pcl::PointXYZ min_point_; + pcl::PointXYZ max_point_; + pcl::PointXYZ min_point_; - static int color_index; - static std::vector> colors; + static int color_index; + static std::vector> colors; }; diff --git a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp index fa9dfe65..0f40db83 100644 --- a/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp +++ b/src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp @@ -10,11 +10,11 @@ #include #include -#include #include +#include -#include #include +#include #include "cluster.hpp" #include "projection_utils.hpp" @@ -23,65 +23,67 @@ struct ClusteringParams; -class TrackingNode : public rclcpp::Node -{ +class TrackingNode : public rclcpp::Node { public: - TrackingNode(); + TrackingNode(); private: - std::map clusteringParams; - - // lidar and camera frame names - std::string lidarFrame_; - std::string cameraFrame_; - - // synchronization utils - std::mutex lidarCloud_mutex_; - - // camera information cached - bool transformInited_; - geometry_msgs::msg::TransformStamped transform_; - sensor_msgs::msg::CameraInfo::SharedPtr camInfo_; - pcl::PointCloud::Ptr lidarCloud_; - - // subscribers to camera intrinsics, lidar pts, camera feed, 2d detection boxes - rclcpp::Subscription::SharedPtr camInfo_subscriber_; - rclcpp::Subscription::SharedPtr lidar_subscriber_; - rclcpp::Subscription::SharedPtr det_subscriber_; - - // publish the 3d detections - rclcpp::Publisher::SharedPtr det3d_publisher_; - rclcpp::Publisher::SharedPtr marker_publisher_; - rclcpp::Publisher::SharedPtr pc_publisher_; - rclcpp::Publisher::SharedPtr pc_publisher2_; - - std::unique_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - void readCameraInfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg); - void receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg); - - int highestIOUScoredBBox( - const std::vector bboxes, - const vision_msgs::msg::BoundingBox2D& detBBox, - const std::vector>& clusters); - double overlapBoundingBox( - const vision_msgs::msg::BoundingBox2D& bboxA, - const vision_msgs::msg::BoundingBox2D& bboxB); - double iouScore( - const vision_msgs::msg::BoundingBox2D& bboxA, - const vision_msgs::msg::BoundingBox2D& bboxB); - - std::map initializeClusteringParams( - const std::map& clustering_params_map); - - template - T getDefaultOrValue(std::map m, std::string key) - { - if (m.find(key) == m.end()) - return m[key]; - return m["default"]; - } - + std::map clusteringParams; + + // lidar and camera frame names + std::string lidarFrame_; + std::string cameraFrame_; + + // synchronization utils + std::mutex lidarCloud_mutex_; + + // camera information cached + bool transformInited_; + geometry_msgs::msg::TransformStamped transform_; + sensor_msgs::msg::CameraInfo::SharedPtr camInfo_; + pcl::PointCloud::Ptr lidarCloud_; + + // subscribers to camera intrinsics, lidar pts, camera feed, 2d detection + // boxes + rclcpp::Subscription::SharedPtr + camInfo_subscriber_; + rclcpp::Subscription::SharedPtr + lidar_subscriber_; + rclcpp::Subscription::SharedPtr + det_subscriber_; + + // publish the 3d detections + rclcpp::Publisher::SharedPtr + det3d_publisher_; + rclcpp::Publisher::SharedPtr + marker_publisher_; + rclcpp::Publisher::SharedPtr pc_publisher_; + rclcpp::Publisher::SharedPtr pc_publisher2_; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + void readCameraInfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg); + void receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + void + receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg); + + int highestIOUScoredBBox( + const std::vector bboxes, + const vision_msgs::msg::BoundingBox2D &detBBox, + const std::vector> &clusters); + double overlapBoundingBox(const vision_msgs::msg::BoundingBox2D &bboxA, + const vision_msgs::msg::BoundingBox2D &bboxB); + double iouScore(const vision_msgs::msg::BoundingBox2D &bboxA, + const vision_msgs::msg::BoundingBox2D &bboxB); + + std::map initializeClusteringParams( + const std::map &clustering_params_map); + + template + T getDefaultOrValue(std::map m, std::string key) { + if (m.find(key) == m.end()) + return m[key]; + return m["default"]; + } }; diff --git a/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp b/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp index 9410382c..c5e2f169 100644 --- a/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp +++ b/src/perception/tracking/dets_2d_3d/include/projection_utils.hpp @@ -2,72 +2,75 @@ #include "cluster.hpp" -#include +#include +#include #include +#include #include #include -#include -#include -#include #include +#include -struct ClusteringParams -{ - // segment by distance - std::vector clustering_distances; - // Map of the nearest neighbor distance threshold for each segment, for each detection - std::vector clustering_thresholds; +struct ClusteringParams { + // segment by distance + std::vector clustering_distances; + // Map of the nearest neighbor distance threshold for each segment, for each + // detection + std::vector clustering_thresholds; - double cluster_size_min; - double cluster_size_max; - double cluster_merge_threshold; + double cluster_size_min; + double cluster_size_max; + double cluster_merge_threshold; }; // main helper functions used by the node -- should all be static -class ProjectionUtils -{ +class ProjectionUtils { public: - static void pointsInBbox( - const pcl::PointCloud::Ptr& inlierCloud, - const pcl::PointCloud::Ptr& lidarCloud, - const std::vector& projs2d, - const vision_msgs::msg::BoundingBox2D& bbox); + static void + pointsInBbox(const pcl::PointCloud::Ptr &inlierCloud, + const pcl::PointCloud::Ptr &lidarCloud, + const std::vector &projs2d, + const vision_msgs::msg::BoundingBox2D &bbox); - // P : projection matrix, pt : 3D lidar pt, transform: transform from lidar to camera frame - static std::optional projectLidarToCamera( - const geometry_msgs::msg::TransformStamped& transform, - const std::array& P, - const pcl::PointXYZ& pt); + // P : projection matrix, pt : 3D lidar pt, transform: transform from lidar to + // camera frame + static std::optional + projectLidarToCamera(const geometry_msgs::msg::TransformStamped &transform, + const std::array &P, + const pcl::PointXYZ &pt); - static bool isPointInBbox( - const geometry_msgs::msg::Point& pt, - const vision_msgs::msg::BoundingBox2D& bbox); + static bool isPointInBbox(const geometry_msgs::msg::Point &pt, + const vision_msgs::msg::BoundingBox2D &bbox); - static void removeFloor( - const pcl::PointCloud::Ptr& lidarCloud, - const pcl::PointCloud::Ptr& cloud_filtered); + static void + removeFloor(const pcl::PointCloud::Ptr &lidarCloud, + const pcl::PointCloud::Ptr &cloud_filtered); - static std::pair>, std::vector> getClusteredBBoxes( - const pcl::PointCloud::Ptr& lidarCloud, const ClusteringParams& clusteringParams); + static std::pair>, + std::vector> + getClusteredBBoxes(const pcl::PointCloud::Ptr &lidarCloud, + const ClusteringParams &clusteringParams); private: + static std::vector> + clusterAndColor(const pcl::PointCloud::Ptr &in_cloud_ptr, + double in_max_cluster_distance, double cluster_size_min, + double cluster_size_max); - static std::vector> clusterAndColor( - const pcl::PointCloud::Ptr& in_cloud_ptr, - double in_max_cluster_distance, double cluster_size_min, double cluster_size_max); - - static void checkClusterMerge( - size_t in_cluster_id, const std::vector> &in_clusters, - std::vector &in_out_visited_clusters, - std::vector &out_merge_indices, double in_merge_threshold); + static void + checkClusterMerge(size_t in_cluster_id, + const std::vector> &in_clusters, + std::vector &in_out_visited_clusters, + std::vector &out_merge_indices, + double in_merge_threshold); - static std::shared_ptr mergeClusters( - const std::vector>& in_clusters, - const std::vector& in_merge_indices, - std::vector& in_out_merged_clusters); + static std::shared_ptr + mergeClusters(const std::vector> &in_clusters, + const std::vector &in_merge_indices, + std::vector &in_out_merged_clusters); - static std::vector> checkAllForMerge( - const std::vector>& in_clusters, - float in_merge_threshold); + static std::vector> + checkAllForMerge(const std::vector> &in_clusters, + float in_merge_threshold); }; diff --git a/src/perception/tracking/dets_2d_3d/src/cluster.cpp b/src/perception/tracking/dets_2d_3d/src/cluster.cpp index e2b16717..69d3a9f1 100644 --- a/src/perception/tracking/dets_2d_3d/src/cluster.cpp +++ b/src/perception/tracking/dets_2d_3d/src/cluster.cpp @@ -1,15 +1,15 @@ #include "cluster.hpp" -#include +#include #include #include -#include +#include #include #include #include -#include #include +#include #include #include @@ -18,103 +18,105 @@ typedef pcl::PointCloud ClusterCloud; typedef vision_msgs::msg::BoundingBox3D BBox3D; int Cluster::color_index = 0; -std::vector> Cluster::colors = {{0,0,255}, {0,255,0}, {255,0,0}, {255,255,0}, {0,255,255}, {255,0,255}}; - -Cluster::Cluster( - const pcl::PointCloud::Ptr& in_cloud_ptr, - const std::vector& in_cluster_indices) : cloud_ {new ClusterCloud()} -{ - bool indexesGiven = in_cluster_indices.size() > 0; - - size_t max_index = (indexesGiven) ? in_cluster_indices.size() : in_cloud_ptr->points.size(); - - min_point_.x = std::numeric_limits::max(); - min_point_.y = std::numeric_limits::max(); - min_point_.z = std::numeric_limits::max(); +std::vector> Cluster::colors = {{0, 0, 255}, {0, 255, 0}, + {255, 0, 0}, {255, 255, 0}, + {0, 255, 255}, {255, 0, 255}}; + +Cluster::Cluster(const pcl::PointCloud::Ptr &in_cloud_ptr, + const std::vector &in_cluster_indices) + : cloud_{new ClusterCloud()} { + bool indexesGiven = in_cluster_indices.size() > 0; + + size_t max_index = + (indexesGiven) ? in_cluster_indices.size() : in_cloud_ptr->points.size(); + + min_point_.x = std::numeric_limits::max(); + min_point_.y = std::numeric_limits::max(); + min_point_.z = std::numeric_limits::max(); + + max_point_.x = -std::numeric_limits::max(); + max_point_.y = -std::numeric_limits::max(); + max_point_.z = -std::numeric_limits::max(); + + for (size_t i = 0; i < max_index; ++i) { + size_t idx = (indexesGiven) ? in_cluster_indices[i] : i; + + pcl::PointXYZRGB pt; + pt.x = in_cloud_ptr->points[idx].x; + pt.y = in_cloud_ptr->points[idx].y; + pt.z = in_cloud_ptr->points[idx].z; + pt.r = colors[color_index % colors.size()][0]; + pt.g = colors[color_index % colors.size()][1]; + pt.b = colors[color_index % colors.size()][2]; + + if (pt.x < min_point_.x) + min_point_.x = pt.x; + if (pt.y < min_point_.y) + min_point_.y = pt.y; + if (pt.z < min_point_.z) + min_point_.z = pt.z; + + if (pt.x > max_point_.x) + max_point_.x = pt.x; + if (pt.y > max_point_.y) + max_point_.y = pt.y; + if (pt.z > max_point_.z) + max_point_.z = pt.z; + + cloud_->emplace_back(pt); + } + + ++color_index; +} - max_point_.x = -std::numeric_limits::max(); - max_point_.y = -std::numeric_limits::max(); - max_point_.z = -std::numeric_limits::max(); +BBox3D Cluster::getBoundingBox() { + BBox3D bounding_box_; - for (size_t i=0; isize() == 0) + return bounding_box_; - pcl::PointXYZRGB pt; - pt.x = in_cloud_ptr->points[idx].x; - pt.y = in_cloud_ptr->points[idx].y; - pt.z = in_cloud_ptr->points[idx].z; - pt.r = colors[color_index % colors.size()][0]; - pt.g = colors[color_index % colors.size()][1]; - pt.b = colors[color_index % colors.size()][2]; + double length_ = max_point_.x - min_point_.x; + double width_ = max_point_.y - min_point_.y; + double height_ = max_point_.z - min_point_.z; - if (pt.x < min_point_.x) min_point_.x = pt.x; - if (pt.y < min_point_.y) min_point_.y = pt.y; - if (pt.z < min_point_.z) min_point_.z = pt.z; + bounding_box_.center.position.x = min_point_.x + length_ / 2; + bounding_box_.center.position.y = min_point_.y + width_ / 2; + bounding_box_.center.position.z = min_point_.z + height_ / 2; - if (pt.x > max_point_.x) max_point_.x = pt.x; - if (pt.y > max_point_.y) max_point_.y = pt.y; - if (pt.z > max_point_.z) max_point_.z = pt.z; + bounding_box_.size.x = ((length_ < 0) ? -1 * length_ : length_); + bounding_box_.size.y = ((width_ < 0) ? -1 * width_ : width_); + bounding_box_.size.z = ((height_ < 0) ? -1 * height_ : height_); + // pose estimation + double rz = 0; - cloud_->emplace_back(pt); + { + std::vector points; + for (unsigned int i = 0; i < cloud_->points.size(); i++) { + cv::Point2f pt; + pt.x = cloud_->points[i].x; + pt.y = cloud_->points[i].y; + points.push_back(pt); } - ++color_index; -} - -BBox3D Cluster::getBoundingBox() -{ - BBox3D bounding_box_; - - if (cloud_->size() == 0) return bounding_box_; - - double length_ = max_point_.x - min_point_.x; - double width_ = max_point_.y - min_point_.y; - double height_ = max_point_.z - min_point_.z; - - bounding_box_.center.position.x = min_point_.x + length_ / 2; - bounding_box_.center.position.y = min_point_.y + width_ / 2; - bounding_box_.center.position.z = min_point_.z + height_ / 2; - - bounding_box_.size.x = ((length_ < 0) ? -1 * length_ : length_); - bounding_box_.size.y = ((width_ < 0) ? -1 * width_ : width_); - bounding_box_.size.z = ((height_ < 0) ? -1 * height_ : height_); - - // pose estimation - double rz = 0; - - { - std::vector points; - for (unsigned int i = 0; i < cloud_->points.size(); i++) - { - cv::Point2f pt; - pt.x = cloud_->points[i].x; - pt.y = cloud_->points[i].y; - points.push_back(pt); - } - - cv::RotatedRect box = cv::minAreaRect(points); - rz = box.angle * 3.14 / 180; - bounding_box_.center.position.x = box.center.x; - bounding_box_.center.position.y = box.center.y; - bounding_box_.size.x = box.size.width; - bounding_box_.size.y = box.size.height; - } + cv::RotatedRect box = cv::minAreaRect(points); + rz = box.angle * 3.14 / 180; + bounding_box_.center.position.x = box.center.x; + bounding_box_.center.position.y = box.center.y; + bounding_box_.size.x = box.size.width; + bounding_box_.size.y = box.size.height; + } - // set bounding box direction - tf2::Quaternion quat; - quat.setRPY(0.0, 0.0, rz); + // set bounding box direction + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, rz); - geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(quat); - bounding_box_.center.orientation = msg_quat; + geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(quat); + bounding_box_.center.orientation = msg_quat; - std::cout << cv::getBuildInformation() << std::endl; + std::cout << cv::getBuildInformation() << std::endl; - return bounding_box_; + return bounding_box_; } -bool Cluster::isValid() -{ - return cloud_->size() > 0; -} +bool Cluster::isValid() { return cloud_->size() > 0; } diff --git a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp index f23dcec8..d5ef416a 100644 --- a/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp +++ b/src/perception/tracking/dets_2d_3d/src/dets_2d_3d_node.cpp @@ -1,8 +1,8 @@ #include "dets_2d_3d_node.hpp" +#include #include #include -#include #include #include @@ -10,35 +10,43 @@ #include #include - -TrackingNode::TrackingNode() : Node("dets_2d_3d", rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)), - transformInited_{false} , lidarCloud_{new pcl::PointCloud()} -{ - rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true); +TrackingNode::TrackingNode() + : Node("dets_2d_3d", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)), + transformInited_{false}, lidarCloud_{ + new pcl::PointCloud()} { + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); lidarFrame_ = this->get_parameter("lidar_frame").as_string(); cameraFrame_ = this->get_parameter("camera_frame").as_string(); // setup pub subs camInfo_subscriber_ = this->create_subscription( - this->get_parameter("camera_info_topic").as_string(), 10, - std::bind(&TrackingNode::readCameraInfo, this, std::placeholders::_1)); + this->get_parameter("camera_info_topic").as_string(), 10, + std::bind(&TrackingNode::readCameraInfo, this, std::placeholders::_1)); lidar_subscriber_ = this->create_subscription( - this->get_parameter("lidar_topic").as_string(), 10, - std::bind(&TrackingNode::receiveLidar, this, std::placeholders::_1)); + this->get_parameter("lidar_topic").as_string(), 10, + std::bind(&TrackingNode::receiveLidar, this, std::placeholders::_1)); - det_subscriber_ = this->create_subscription( - this->get_parameter("detections_topic").as_string(), 10, - std::bind(&TrackingNode::receiveDetections, this, std::placeholders::_1)); + det_subscriber_ = + this->create_subscription( + this->get_parameter("detections_topic").as_string(), 10, + std::bind(&TrackingNode::receiveDetections, this, + std::placeholders::_1)); det3d_publisher_ = this->create_publisher( - this->get_parameter("publish_detections_topic").as_string(), 10); - marker_publisher_ = this->create_publisher( - this->get_parameter("publish_markers_topic").as_string(), 10); + this->get_parameter("publish_detections_topic").as_string(), 10); + marker_publisher_ = + this->create_publisher( + this->get_parameter("publish_markers_topic").as_string(), 10); pc_publisher_ = this->create_publisher( - this->get_parameter("publish_clusters_topic").as_string(), 10); - + this->get_parameter("publish_clusters_topic").as_string(), 10); + tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -47,12 +55,12 @@ TrackingNode::TrackingNode() : Node("dets_2d_3d", rclcpp::NodeOptions().allow_un clusteringParams = initializeClusteringParams(clustering_params_map); } -std::map TrackingNode::initializeClusteringParams( - const std::map& clustering_params_map) -{ +std::map +TrackingNode::initializeClusteringParams( + const std::map &clustering_params_map) { std::map clusteringParams; - for(auto iter = clustering_params_map.begin(); iter != clustering_params_map.end(); ++iter) - { + for (auto iter = clustering_params_map.begin(); + iter != clustering_params_map.end(); ++iter) { std::string key = iter->first; size_t split_loc = key.find_first_of('.'); std::string det_type = key.substr(0, split_loc); @@ -75,8 +83,8 @@ std::map TrackingNode::initializeClusteringParams return clusteringParams; } -void TrackingNode::receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloud_msg) -{ +void TrackingNode::receiveLidar( + const sensor_msgs::msg::PointCloud2::SharedPtr pointCloud_msg) { // save latest lidar info std::lock_guard guard_lidar(lidarCloud_mutex_); @@ -84,89 +92,103 @@ void TrackingNode::receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr p pcl::fromROSMsg(pointCloud, *lidarCloud_); } -void TrackingNode::readCameraInfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg) -{ +void TrackingNode::readCameraInfo( + const sensor_msgs::msg::CameraInfo::SharedPtr msg) { std::cout << msg->distortion_model << std::endl; camInfo_ = msg; camInfo_subscriber_.reset(); } -void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg) -{ - if (!transformInited_) - { +void TrackingNode::receiveDetections( + const vision_msgs::msg::Detection2DArray::SharedPtr msg) { + if (!transformInited_) { try { - transform_ = tf_buffer_ ->lookupTransform(cameraFrame_, lidarFrame_, msg->header.stamp); + transform_ = tf_buffer_->lookupTransform(cameraFrame_, lidarFrame_, + msg->header.stamp); transformInited_ = true; - } catch (const tf2::TransformException & ex) { + } catch (const tf2::TransformException &ex) { RCLCPP_INFO(this->get_logger(), "Could not transform %s", ex.what()); return; } } // remove floor from lidar cloud - pcl::PointCloud::Ptr lidarCloudNoFloor(new pcl::PointCloud()); + pcl::PointCloud::Ptr lidarCloudNoFloor( + new pcl::PointCloud()); { std::lock_guard guard_lidar(lidarCloud_mutex_); ProjectionUtils::removeFloor(lidarCloud_, lidarCloudNoFloor); - if (lidarCloudNoFloor->size() == 0) return; // do not process more, if all the points are floor points - RCLCPP_INFO(this->get_logger(), "receive detection %ld vs %ld", lidarCloud_->size(), lidarCloudNoFloor->size()); + if (lidarCloudNoFloor->size() == 0) + return; // do not process more, if all the points are floor points + RCLCPP_INFO(this->get_logger(), "receive detection %ld vs %ld", + lidarCloud_->size(), lidarCloudNoFloor->size()); } // transform all lidar pts to 2d points in the camera std::vector lidar2dProjs; - pcl::PointCloud::Ptr inCameraPoints(new pcl::PointCloud()); - for (const pcl::PointXYZ& pt : lidarCloudNoFloor->points) - { - std::optional proj = ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, pt); - if (proj) - { + pcl::PointCloud::Ptr inCameraPoints( + new pcl::PointCloud()); + for (const pcl::PointXYZ &pt : lidarCloudNoFloor->points) { + std::optional proj = + ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, pt); + if (proj) { inCameraPoints->emplace_back(pt); lidar2dProjs.emplace_back(*proj); } } - + // publish both rviz markers (for visualization) and a detection array visualization_msgs::msg::MarkerArray markerArray3d; vision_msgs::msg::Detection3DArray detArray3d; - pcl::PointCloud mergedClusters; // coloured pc to visualize points in each cluster + pcl::PointCloud + mergedClusters; // coloured pc to visualize points in each cluster // process each detection in det array int bboxId = 0; - for (const vision_msgs::msg::Detection2D& det : msg->detections) - { + for (const vision_msgs::msg::Detection2D &det : msg->detections) { vision_msgs::msg::BoundingBox2D bbox = det.bbox; // process lidarcloud with extrinsic trans from lidar to cam frame - pcl::PointCloud::Ptr inlierPoints(new pcl::PointCloud()); - ProjectionUtils::pointsInBbox(inlierPoints, inCameraPoints, lidar2dProjs, bbox); + pcl::PointCloud::Ptr inlierPoints( + new pcl::PointCloud()); + ProjectionUtils::pointsInBbox(inlierPoints, inCameraPoints, lidar2dProjs, + bbox); if (inlierPoints->size() == 0) { - RCLCPP_INFO(this->get_logger(), "no inliers found for detection %s", det.results[0].hypothesis.class_id.c_str()); + RCLCPP_INFO(this->get_logger(), "no inliers found for detection %s", + det.results[0].hypothesis.class_id.c_str()); continue; } // clustering - auto clusterAndBBoxes = ProjectionUtils::getClusteredBBoxes(inlierPoints, getDefaultOrValue(clusteringParams, det.results[0].hypothesis.class_id.c_str())); - std::vector> clusters = clusterAndBBoxes.first; // needed? for viz purposes only - std::vector allBBoxes = clusterAndBBoxes.second; - - if (clusters.size() == 0 || allBBoxes.size() == 0) continue; + auto clusterAndBBoxes = ProjectionUtils::getClusteredBBoxes( + inlierPoints, + getDefaultOrValue( + clusteringParams, det.results[0].hypothesis.class_id.c_str())); + std::vector> clusters = + clusterAndBBoxes.first; // needed? for viz purposes only + std::vector allBBoxes = + clusterAndBBoxes.second; + + if (clusters.size() == 0 || allBBoxes.size() == 0) + continue; - // score each 3d bbox based on iou with teh original 2d bbox & the density of points in the cluster + // score each 3d bbox based on iou with teh original 2d bbox & the density + // of points in the cluster int bestIndex = highestIOUScoredBBox(allBBoxes, bbox, clusters); vision_msgs::msg::BoundingBox3D bestBBox = allBBoxes[bestIndex]; - // for visauzliation : adds detection to cluster pc visualization & the marker array & the 3d detection array + // for visauzliation : adds detection to cluster pc visualization & the + // marker array & the 3d detection array mergedClusters += *(clusters[bestIndex]->getCloud()); vision_msgs::msg::Detection3D det3d; - det3d.bbox = bestBBox; + det3d.bbox = bestBBox; det3d.results = det.results; detArray3d.detections.emplace_back(det3d); visualization_msgs::msg::Marker marker; - marker.type = visualization_msgs::msg::Marker::CUBE; + marker.type = visualization_msgs::msg::Marker::CUBE; marker.scale = bestBBox.size; marker.pose = bestBBox.center; marker.header.frame_id = lidarFrame_; @@ -186,40 +208,43 @@ void TrackingNode::receiveDetections(const vision_msgs::msg::Detection2DArray::S pc_publisher_->publish(pubCloud); - RCLCPP_INFO(this->get_logger(), "published %ld 3d detections\n\n", markerArray3d.markers.size()); - + RCLCPP_INFO(this->get_logger(), "published %ld 3d detections\n\n", + markerArray3d.markers.size()); } int TrackingNode::highestIOUScoredBBox( - const std::vector bboxes, - const vision_msgs::msg::BoundingBox2D& detBBox, - const std::vector>& clusters) -{ + const std::vector bboxes, + const vision_msgs::msg::BoundingBox2D &detBBox, + const std::vector> &clusters) { int bestScore = 0; int bestBBox = 0; - for (size_t i=0; i top_left2d = ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, top_left); - std::optional bottom_right2d = ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, bottom_right); + std::optional top_left2d = + ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, + top_left); + std::optional bottom_right2d = + ProjectionUtils::projectLidarToCamera(transform_, camInfo_->p, + bottom_right); - if (!top_left2d || !bottom_right2d) return 0; + if (!top_left2d || !bottom_right2d) + return 0; vision_msgs::msg::BoundingBox2D bbox2d; - bbox2d.center.position.x = ((top_left2d->x) + (bottom_right2d->x))/2; - bbox2d.center.position.y = ((top_left2d->y) + (bottom_right2d->y))/2; + bbox2d.center.position.x = ((top_left2d->x) + (bottom_right2d->x)) / 2; + bbox2d.center.position.y = ((top_left2d->y) + (bottom_right2d->y)) / 2; bbox2d.size_x = abs(top_left2d->x - bottom_right2d->x); bbox2d.size_y = abs(top_left2d->y - bottom_right2d->y); @@ -229,8 +254,7 @@ int TrackingNode::highestIOUScoredBBox( double density = clusters[i]->size() / (b.size.x * b.size.y * b.size.z); double score = iou + density; - if (score > bestScore) - { + if (score > bestScore) { bestScore = score; bestBBox = i; } @@ -238,30 +262,34 @@ int TrackingNode::highestIOUScoredBBox( return bestBBox; } -double TrackingNode::overlapBoundingBox(const vision_msgs::msg::BoundingBox2D& boxA, const vision_msgs::msg::BoundingBox2D& boxB) -{ - double overlapHeight = - std::min(boxA.center.position.y + boxA.size_y/2, boxB.center.position.y + boxB.size_y/2) - - std::max(boxA.center.position.y - boxA.size_y/2, boxB.center.position.y - boxB.size_y/2); - if (overlapHeight <= 0) return 0; - - double overlapWidth = std::min(boxA.center.position.x + boxA.size_x/2, boxB.center.position.x + boxB.size_x/2) - - std::max(boxA.center.position.x - boxA.size_x/2, boxB.center.position.x - boxB.size_x/2); - if (overlapWidth <= 0) return 0; - +double +TrackingNode::overlapBoundingBox(const vision_msgs::msg::BoundingBox2D &boxA, + const vision_msgs::msg::BoundingBox2D &boxB) { + double overlapHeight = std::min(boxA.center.position.y + boxA.size_y / 2, + boxB.center.position.y + boxB.size_y / 2) - + std::max(boxA.center.position.y - boxA.size_y / 2, + boxB.center.position.y - boxB.size_y / 2); + if (overlapHeight <= 0) + return 0; + + double overlapWidth = std::min(boxA.center.position.x + boxA.size_x / 2, + boxB.center.position.x + boxB.size_x / 2) - + std::max(boxA.center.position.x - boxA.size_x / 2, + boxB.center.position.x - boxB.size_x / 2); + if (overlapWidth <= 0) + return 0; + return overlapHeight * overlapWidth; } -double TrackingNode::iouScore(const vision_msgs::msg::BoundingBox2D& bboxA, const vision_msgs::msg::BoundingBox2D& bboxB) -{ +double TrackingNode::iouScore(const vision_msgs::msg::BoundingBox2D &bboxA, + const vision_msgs::msg::BoundingBox2D &bboxB) { double overlap = overlapBoundingBox(bboxA, bboxB); - return (overlap / - (bboxA.size_x * bboxA.size_y + bboxB.size_x * bboxB.size_y - overlap)); - + return (overlap / (bboxA.size_x * bboxA.size_y + bboxB.size_x * bboxB.size_y - + overlap)); } -int main(int argc, char ** argv) -{ +int main(int argc, char **argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp b/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp index 1ff2768c..fa1f29d3 100644 --- a/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp +++ b/src/perception/tracking/dets_2d_3d/src/projection_utils.cpp @@ -1,14 +1,14 @@ #include "projection_utils.hpp" -#include -#include #include +#include +#include +#include #include #include #include #include -#include #include #include @@ -21,262 +21,262 @@ typedef std::shared_ptr ClusterPtr; typedef vision_msgs::msg::BoundingBox3D BBox3D; void ProjectionUtils::pointsInBbox( - const Cloud::Ptr& inlierCloud, - const Cloud::Ptr& lidarCloud, - const std::vector& projs2d, - const vision_msgs::msg::BoundingBox2D& bbox) -{ - for (size_t i=0; ipush_back(lidarCloud->points[i]); - } + const Cloud::Ptr &inlierCloud, const Cloud::Ptr &lidarCloud, + const std::vector &projs2d, + const vision_msgs::msg::BoundingBox2D &bbox) { + for (size_t i = 0; i < projs2d.size(); ++i) { + // P * [x y z 1]^T, P is row major + if (isPointInBbox(projs2d[i], bbox)) + inlierCloud->push_back(lidarCloud->points[i]); + } } - -bool ProjectionUtils::isPointInBbox(const geometry_msgs::msg::Point& pt, const vision_msgs::msg::BoundingBox2D& bbox) -{ - double padding = 0; - - if (bbox.center.position.x - bbox.size_x/2 - padding < pt.x && pt.x < bbox.center.position.x + bbox.size_x/2 + padding - && bbox.center.position.y - bbox.size_y/2 - padding < pt.y && pt.y < bbox.center.position.y + bbox.size_y/2 + padding) - { - return true; - } - return false; +bool ProjectionUtils::isPointInBbox( + const geometry_msgs::msg::Point &pt, + const vision_msgs::msg::BoundingBox2D &bbox) { + double padding = 0; + + if (bbox.center.position.x - bbox.size_x / 2 - padding < pt.x && + pt.x < bbox.center.position.x + bbox.size_x / 2 + padding && + bbox.center.position.y - bbox.size_y / 2 - padding < pt.y && + pt.y < bbox.center.position.y + bbox.size_y / 2 + padding) { + return true; + } + return false; } std::optional ProjectionUtils::projectLidarToCamera( - const geometry_msgs::msg::TransformStamped& transform, - const std::array& p, - const pcl::PointXYZ& pt) -{ - // lidar to camera frame - auto trans_pt = geometry_msgs::msg::Point(); - auto orig_pt = geometry_msgs::msg::Point(); - orig_pt.x = pt.x; - orig_pt.y = pt.y; - orig_pt.z = pt.z; - - tf2::doTransform(orig_pt, trans_pt, transform); - - if (trans_pt.z < 1) return std::nullopt; - - // camera frame to camera 2D projection - double u = p[0] * trans_pt.x + p[1] * trans_pt.y + p[2] * trans_pt.z + p[3]; - double v = p[4] * trans_pt.x + p[5] * trans_pt.y + p[6] * trans_pt.z + p[7]; - double w = p[8] * trans_pt.x + p[9] * trans_pt.y + p[10] * trans_pt.z + p[11]; - - auto proj_pt = geometry_msgs::msg::Point(); - proj_pt.x = u/w; - proj_pt.y = v/w; - - // check if inside camera frame bounds - if (proj_pt.x > 0 && proj_pt.x < 1600 && proj_pt.y > 0 && proj_pt.y < 900) return proj_pt; + const geometry_msgs::msg::TransformStamped &transform, + const std::array &p, const pcl::PointXYZ &pt) { + // lidar to camera frame + auto trans_pt = geometry_msgs::msg::Point(); + auto orig_pt = geometry_msgs::msg::Point(); + orig_pt.x = pt.x; + orig_pt.y = pt.y; + orig_pt.z = pt.z; + + tf2::doTransform(orig_pt, trans_pt, transform); + + if (trans_pt.z < 1) return std::nullopt; -} -// https://pointclouds.org/documentation/tutorials/progressive_morphological_filtering.html -void ProjectionUtils::removeFloor(const Cloud::Ptr& lidarCloud, const Cloud::Ptr& cloud_filtered) -{ - pcl::PointIndices::Ptr inliers(new pcl::PointIndices); - pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); - - // Create the filtering object - pcl::SACSegmentation seg; - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setMaxIterations(100); - seg.setAxis(Eigen::Vector3f(0, 0, 1)); - seg.setEpsAngle(0.1); - seg.setDistanceThreshold(0.5); // floor distance - seg.setOptimizeCoefficients(true); - seg.setInputCloud(lidarCloud); - seg.segment(*inliers, *coefficients); - - // Create the filtering object - pcl::ExtractIndices extract; - extract.setInputCloud(lidarCloud); - extract.setIndices(inliers); - - // Extract non-ground returns - extract.setNegative (true); - extract.filter(*cloud_filtered); -} + // camera frame to camera 2D projection + double u = p[0] * trans_pt.x + p[1] * trans_pt.y + p[2] * trans_pt.z + p[3]; + double v = p[4] * trans_pt.x + p[5] * trans_pt.y + p[6] * trans_pt.z + p[7]; + double w = p[8] * trans_pt.x + p[9] * trans_pt.y + p[10] * trans_pt.z + p[11]; -std::pair, std::vector> ProjectionUtils::getClusteredBBoxes( - const Cloud::Ptr& lidarCloud, - const ClusteringParams& clusteringParams) -{ - std::vector::Ptr> cloud_segments_array(5); - for (size_t i=0; i::Ptr temp(new pcl::PointCloud()); - cloud_segments_array[i] = temp; - } + auto proj_pt = geometry_msgs::msg::Point(); + proj_pt.x = u / w; + proj_pt.y = v / w; - // segment points into spherical shells from point cloud origin - for (const pcl::PointXYZ& pt : lidarCloud->points) - { - float origin_distance = sqrt(pow(pt.x, 2) + pow(pt.y, 2)); - if (origin_distance < clusteringParams.clustering_distances[0]) - cloud_segments_array[0]->points.push_back(pt); - else if (origin_distance < clusteringParams.clustering_distances[1]) - cloud_segments_array[1]->points.push_back(pt); - else if (origin_distance < clusteringParams.clustering_distances[2]) - cloud_segments_array[2]->points.push_back(pt); - else if (origin_distance < clusteringParams.clustering_distances[3]) - cloud_segments_array[3]->points.push_back(pt); - else - cloud_segments_array[4]->points.push_back(pt); - } + // check if inside camera frame bounds + if (proj_pt.x > 0 && proj_pt.x < 1600 && proj_pt.y > 0 && proj_pt.y < 900) + return proj_pt; + return std::nullopt; +} - // get largest cluster in each shell - std::vector all_clusters; - for (unsigned int i = 1; i < cloud_segments_array.size(); i++) - { - // add clusters from each shell - std::vector local_clusters = clusterAndColor(cloud_segments_array[i], - clusteringParams.clustering_thresholds[i], clusteringParams.cluster_size_min, - clusteringParams.cluster_size_max); - all_clusters.insert(all_clusters.end(), local_clusters.begin(), local_clusters.end()); - } +// https://pointclouds.org/documentation/tutorials/progressive_morphological_filtering.html +void ProjectionUtils::removeFloor(const Cloud::Ptr &lidarCloud, + const Cloud::Ptr &cloud_filtered) { + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + + // Create the filtering object + pcl::SACSegmentation seg; + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setMaxIterations(100); + seg.setAxis(Eigen::Vector3f(0, 0, 1)); + seg.setEpsAngle(0.1); + seg.setDistanceThreshold(0.5); // floor distance + seg.setOptimizeCoefficients(true); + seg.setInputCloud(lidarCloud); + seg.segment(*inliers, *coefficients); + + // Create the filtering object + pcl::ExtractIndices extract; + extract.setInputCloud(lidarCloud); + extract.setIndices(inliers); + + // Extract non-ground returns + extract.setNegative(true); + extract.filter(*cloud_filtered); +} - // merge clusters if possible, do this twice? - std::vector mid_clusters = (all_clusters.size() > 0) - ? ProjectionUtils::checkAllForMerge(all_clusters, clusteringParams.cluster_merge_threshold) - : all_clusters; - std::vector final_clusters = (mid_clusters.size() > 0) - ? ProjectionUtils::checkAllForMerge(mid_clusters, clusteringParams.cluster_merge_threshold) - : mid_clusters; - - // get boundingboxes for each & return all possible 3d bboxes (if valid) - std::vector bboxes; - for (const ClusterPtr& cluster : final_clusters) - { - BBox3D b = cluster->getBoundingBox(); - if (cluster->isValid()) - bboxes.emplace_back(b); - } - return std::pair, std::vector>{final_clusters, bboxes}; +std::pair, std::vector> +ProjectionUtils::getClusteredBBoxes(const Cloud::Ptr &lidarCloud, + const ClusteringParams &clusteringParams) { + std::vector::Ptr> cloud_segments_array(5); + for (size_t i = 0; i < cloud_segments_array.size(); ++i) { + pcl::PointCloud::Ptr temp( + new pcl::PointCloud()); + cloud_segments_array[i] = temp; + } + + // segment points into spherical shells from point cloud origin + for (const pcl::PointXYZ &pt : lidarCloud->points) { + float origin_distance = sqrt(pow(pt.x, 2) + pow(pt.y, 2)); + if (origin_distance < clusteringParams.clustering_distances[0]) + cloud_segments_array[0]->points.push_back(pt); + else if (origin_distance < clusteringParams.clustering_distances[1]) + cloud_segments_array[1]->points.push_back(pt); + else if (origin_distance < clusteringParams.clustering_distances[2]) + cloud_segments_array[2]->points.push_back(pt); + else if (origin_distance < clusteringParams.clustering_distances[3]) + cloud_segments_array[3]->points.push_back(pt); + else + cloud_segments_array[4]->points.push_back(pt); + } + + // get largest cluster in each shell + std::vector all_clusters; + for (unsigned int i = 1; i < cloud_segments_array.size(); i++) { + // add clusters from each shell + std::vector local_clusters = clusterAndColor( + cloud_segments_array[i], clusteringParams.clustering_thresholds[i], + clusteringParams.cluster_size_min, clusteringParams.cluster_size_max); + all_clusters.insert(all_clusters.end(), local_clusters.begin(), + local_clusters.end()); + } + + // merge clusters if possible, do this twice? + std::vector mid_clusters = + (all_clusters.size() > 0) + ? ProjectionUtils::checkAllForMerge( + all_clusters, clusteringParams.cluster_merge_threshold) + : all_clusters; + std::vector final_clusters = + (mid_clusters.size() > 0) + ? ProjectionUtils::checkAllForMerge( + mid_clusters, clusteringParams.cluster_merge_threshold) + : mid_clusters; + + // get boundingboxes for each & return all possible 3d bboxes (if valid) + std::vector bboxes; + for (const ClusterPtr &cluster : final_clusters) { + BBox3D b = cluster->getBoundingBox(); + if (cluster->isValid()) + bboxes.emplace_back(b); + } + return std::pair, std::vector>{final_clusters, + bboxes}; } std::vector ProjectionUtils::clusterAndColor( - const Cloud::Ptr& in_cloud_ptr, double in_max_cluster_distance, double cluster_size_min, double cluster_size_max) -{ - std::vector clusters; - if(in_cloud_ptr->size() == 0) return clusters; - - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); - - // create 2d pc, by copying & making it flat - Cloud::Ptr cloud_2d(new pcl::PointCloud()); - pcl::copyPointCloud(*in_cloud_ptr, *cloud_2d); - for (size_t i = 0; i < cloud_2d->points.size(); i++) - cloud_2d->points[i].z = 0; - - tree->setInputCloud(cloud_2d); - - std::vector cluster_indices; - - // perform clustering on 2d cloud : https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(in_max_cluster_distance); - ec.setMinClusterSize(cluster_size_min); - ec.setMaxClusterSize(cluster_size_max); - ec.setSearchMethod(tree); - ec.setInputCloud(cloud_2d); - ec.extract(cluster_indices); - - - // add pts at clustered indexes to cluster - for (const auto& cluster : cluster_indices) - { - ClusterPtr cloud_cluster = std::make_shared(in_cloud_ptr, cluster.indices); - clusters.emplace_back(cloud_cluster); - } - + const Cloud::Ptr &in_cloud_ptr, double in_max_cluster_distance, + double cluster_size_min, double cluster_size_max) { + std::vector clusters; + if (in_cloud_ptr->size() == 0) return clusters; -} + pcl::search::KdTree::Ptr tree( + new pcl::search::KdTree()); + + // create 2d pc, by copying & making it flat + Cloud::Ptr cloud_2d(new pcl::PointCloud()); + pcl::copyPointCloud(*in_cloud_ptr, *cloud_2d); + for (size_t i = 0; i < cloud_2d->points.size(); i++) + cloud_2d->points[i].z = 0; + + tree->setInputCloud(cloud_2d); + + std::vector cluster_indices; + + // perform clustering on 2d cloud : + // https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(in_max_cluster_distance); + ec.setMinClusterSize(cluster_size_min); + ec.setMaxClusterSize(cluster_size_max); + ec.setSearchMethod(tree); + ec.setInputCloud(cloud_2d); + ec.extract(cluster_indices); + + // add pts at clustered indexes to cluster + for (const auto &cluster : cluster_indices) { + ClusterPtr cloud_cluster = + std::make_shared(in_cloud_ptr, cluster.indices); + clusters.emplace_back(cloud_cluster); + } + + return clusters; +} void ProjectionUtils::checkClusterMerge( size_t in_cluster_id, const std::vector &in_clusters, - std::vector& in_out_visited_clusters, - std::vector& out_merge_indices, double in_merge_threshold) -{ - - pcl::PointXYZ point_a = in_clusters[in_cluster_id]->getCentroid(); - - for (size_t i = 0; i < in_clusters.size(); i++) { - if (i != in_cluster_id && !in_out_visited_clusters[i]) - { - pcl::PointXYZ point_b = in_clusters[i]->getCentroid(); - double distance = sqrt(pow(point_b.x - point_a.x, 2) + pow(point_b.y - point_a.y, 2)); - - if (distance <= in_merge_threshold) - { - in_out_visited_clusters[i] = true; - out_merge_indices.push_back(i); - // look for all other clusters that can be merged with this merge-able cluster - checkClusterMerge(i, in_clusters, in_out_visited_clusters, out_merge_indices, in_merge_threshold); - } - } + std::vector &in_out_visited_clusters, + std::vector &out_merge_indices, double in_merge_threshold) { + + pcl::PointXYZ point_a = in_clusters[in_cluster_id]->getCentroid(); + + for (size_t i = 0; i < in_clusters.size(); i++) { + if (i != in_cluster_id && !in_out_visited_clusters[i]) { + pcl::PointXYZ point_b = in_clusters[i]->getCentroid(); + double distance = + sqrt(pow(point_b.x - point_a.x, 2) + pow(point_b.y - point_a.y, 2)); + + if (distance <= in_merge_threshold) { + in_out_visited_clusters[i] = true; + out_merge_indices.push_back(i); + // look for all other clusters that can be merged with this merge-able + // cluster + checkClusterMerge(i, in_clusters, in_out_visited_clusters, + out_merge_indices, in_merge_threshold); + } } + } } +ClusterPtr +ProjectionUtils::mergeClusters(const std::vector &in_clusters, + const std::vector &in_merge_indices, + std::vector &in_out_merged_clusters) { + ColorCloud merged_cloud; -ClusterPtr ProjectionUtils::mergeClusters( - const std::vector& in_clusters, - const std::vector& in_merge_indices, - std::vector &in_out_merged_clusters) -{ - ColorCloud merged_cloud; - - // for each cluster in merge cloud indices, merge into larger cloud - for (size_t i = 0; i < in_merge_indices.size(); i++) - { - merged_cloud += *(in_clusters[in_merge_indices[i]]->getCloud()); - in_out_merged_clusters[in_merge_indices[i]] = true; - } + // for each cluster in merge cloud indices, merge into larger cloud + for (size_t i = 0; i < in_merge_indices.size(); i++) { + merged_cloud += *(in_clusters[in_merge_indices[i]]->getCloud()); + in_out_merged_clusters[in_merge_indices[i]] = true; + } - Cloud merged_cloud_uncoloured; - pcl::copyPointCloud(merged_cloud, merged_cloud_uncoloured); + Cloud merged_cloud_uncoloured; + pcl::copyPointCloud(merged_cloud, merged_cloud_uncoloured); - Cloud::Ptr merged_cloud_ptr(new Cloud(merged_cloud_uncoloured)); + Cloud::Ptr merged_cloud_ptr(new Cloud(merged_cloud_uncoloured)); - std::vector temp; - ClusterPtr merged_cluster = std::make_shared(merged_cloud_ptr, temp); - return merged_cluster; + std::vector temp; + ClusterPtr merged_cluster = std::make_shared(merged_cloud_ptr, temp); + return merged_cluster; } -std::vector ProjectionUtils::checkAllForMerge( - const std::vector& in_clusters, float in_merge_threshold) -{ - std::vector out_clusters; +std::vector +ProjectionUtils::checkAllForMerge(const std::vector &in_clusters, + float in_merge_threshold) { + std::vector out_clusters; - std::vector visited_clusters(in_clusters.size(), false); - std::vector merged_clusters(in_clusters.size(), false); + std::vector visited_clusters(in_clusters.size(), false); + std::vector merged_clusters(in_clusters.size(), false); - for (size_t i = 0; i < in_clusters.size(); i++) { - if (!visited_clusters[i]) { - visited_clusters[i] = true; + for (size_t i = 0; i < in_clusters.size(); i++) { + if (!visited_clusters[i]) { + visited_clusters[i] = true; - std::vector merge_indices; - checkClusterMerge(i, in_clusters, visited_clusters, merge_indices, in_merge_threshold); - ClusterPtr mergedCluster = mergeClusters(in_clusters, merge_indices, merged_clusters); + std::vector merge_indices; + checkClusterMerge(i, in_clusters, visited_clusters, merge_indices, + in_merge_threshold); + ClusterPtr mergedCluster = + mergeClusters(in_clusters, merge_indices, merged_clusters); - out_clusters.emplace_back(mergedCluster); - } + out_clusters.emplace_back(mergedCluster); } - for (size_t i = 0; i < in_clusters.size(); i++) { - // check for clusters not merged, add them to the output - if (!merged_clusters[i]) { - out_clusters.push_back(in_clusters[i]); - } + } + for (size_t i = 0; i < in_clusters.size(); i++) { + // check for clusters not merged, add them to the output + if (!merged_clusters[i]) { + out_clusters.push_back(in_clusters[i]); } + } - return out_clusters; + return out_clusters; }