SHERPA/sherpa_ros2_ws
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added headerfile for clustering and cleaned up code

Browse Source
This commit is contained in:
Liam
2025-08-30 14:50:43 +02:00
parent 902de6a12b
commit f537027329
4 changed files with 719 additions and 625 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
src/target_tracking/CMakeLists.txt
View File

@@ -38,11 +38,17 @@ ament_target_dependencies(cloud_preprocessing_node
# Node for clustering # Node for clustering
add_executable(cloud_clustering_node add_executable(cloud_clustering_node
src/cloud_clustering.cpp src/cloud_clustering_node.cpp
) )
add_library(cloud_clustering SHARED
src/cloud_clustering.cpp)
target_compile_definitions(cloud_clustering
PRIVATE "COMPOSITION_BUILDING_DLL")
# Dependencies for clustering # Dependencies for clustering
ament_target_dependencies(cloud_clustering_node ament_target_dependencies(cloud_clustering
rclcpp rclcpp
sensor_msgs sensor_msgs
visualization_msgs visualization_msgs
@@ -51,7 +57,10 @@ ament_target_dependencies(cloud_clustering_node
OpenCV OpenCV
) )
#target_link_libraries(target_tracking_node ${PCL_LIBRARIES}) rclcpp_components_register_nodes(cloud_clustering "cloud_clustering::CloudClustering")
set(node_plugins "${node_plugins}cloud_clustering::CloudClustering;$<TARGET_FILE:cloud_clustering>\n")
target_link_libraries(cloud_clustering_node cloud_clustering)
# Install target # Install target
install(TARGETS install(TARGETS
@@ -60,6 +69,13 @@ install(TARGETS
DESTINATION lib/${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}
) )
install(TARGETS
cloud_clustering
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PACKAGE_NAME}
)
# Install launchfile # Install launchfile
install( install(
DIRECTORY launch/ DIRECTORY launch/

100
src/target_tracking/include/cloud_clustering.hpp Normal file
View File

@@ -0,0 +1,100 @@
#ifndef CLOUD_CLUSTERING_HPP
#define CLOUD_CLUSTERING_HPP
#include "rclcpp/rclcpp.hpp"
#include <geometry_msgs/msg/point.hpp>
#include "visualization_msgs/msg/marker_array.hpp"
#include "opencv2/video/tracking.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include "std_msgs/msg/int32_multi_array.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/segmentation/extract_clusters.h>
namespace cloud_clustering
{
// KF init
int stateDim = 4; // [x,y,v_x,v_y]//,w,h]
int measDim = 2; // [z_x,z_y,z_w,z_h]
int ctrlDim = 0;
std::string frame_id;
std::string topic_in;
float z_dim_scale;
float cluster_tolerance;
int min_cluster_size;
int max_cluster_size;
cv::KalmanFilter KF0(stateDim, measDim, ctrlDim, CV_32F);
cv::KalmanFilter KF1(stateDim, measDim, ctrlDim, CV_32F);
cv::KalmanFilter KF2(stateDim, measDim, ctrlDim, CV_32F);
cv::KalmanFilter KF3(stateDim, measDim, ctrlDim, CV_32F);
cv::KalmanFilter KF4(stateDim, measDim, ctrlDim, CV_32F);
cv::KalmanFilter KF5(stateDim, measDim, ctrlDim, CV_32F);
std::vector<geometry_msgs::msg::Point> prevClusterCenters;
cv::Mat state(stateDim, 1, CV_32F);
cv::Mat_<float> measurement(2, 1);
std::vector<int> objID; // Output of the data association using KF
// measurement.setTo(Scalar(0));
bool firstFrame = true;
/*
cv::Mat state(stateDim, 1, CV_32F);
cv::Mat_<float> measurement(2, 1);
*/
class CloudClustering : public rclcpp::Node
{
public:
CloudClustering(
const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
CloudClustering(
const std::string &name_space,
const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
private:
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr markerPub;
rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr objID_pub;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster0;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster1;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster2;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster3;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster4;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster5;
std::vector<geometry_msgs::msg::Point> prevClusterCenters;
std::vector<int> objID; // Output of the data association using KF
// measurement.setTo(Scalar(0));
bool firstFrame = true;
rclcpp::Clock::SharedPtr clock_;
std::string frame_id;
std::string filtered_cloud;
double euclidean_distance(geometry_msgs::msg::Point &p1, geometry_msgs::msg::Point &p2);
std::pair<int, int> findIndexOfMin(std::vector<std::vector<float>> distMat);
void KFT(const std_msgs::msg::Float32MultiArray ccs);
void publish_cloud(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub,
pcl::PointCloud<pcl::PointXYZI>::Ptr cluster);
void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
std::vector<std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, float>> &cluster_vec,
std::vector<pcl::PointXYZI> &clusterCentroids);
void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input);
};
}
#endif // CLOUD_CLUSTERING_HPP

433
src/target_tracking/src/cloud_clustering.cpp
View File

@@ -1,126 +1,152 @@
#include <pcl_conversions/pcl_conversions.h> // Default C++ imports
#include <sensor_msgs/msg/point_cloud2.hpp> #include <limits>
#include <pcl/point_cloud.h> #include <utility>
#include <pcl/point_types.h> #include <string.h>
#include <algorithm>
#include <fstream>
#include <geometry_msgs/msg/point.hpp>
#include <iostream> #include <iostream>
#include <iterator> #include <iterator>
#include <opencv2/video/video.hpp> #include <algorithm>
#include <pcl/io/pcd_io.h> #include <fstream>
#include <pcl/point_types.h>
// Headerfile
#include "cloud_clustering.hpp"
// Ros2 import
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <opencv2/video/video.hpp>
#include <pcl_conversions/pcl_conversions.h>
// PCL Types
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
// Ros2 messages
#include <std_msgs/msg/float32_multi_array.hpp> #include <std_msgs/msg/float32_multi_array.hpp>
#include <std_msgs/msg/int32_multi_array.hpp> #include <std_msgs/msg/int32_multi_array.hpp>
#include <string.h> #include <geometry_msgs/msg/point.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/common/centroid.h> #include <pcl/common/centroid.h>
#include <pcl/common/geometry.h> #include <pcl/common/geometry.h>
#include <pcl/features/normal_3d.h> #include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h> #include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h> #include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree.h> #include <pcl/kdtree/kdtree.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h> #include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h> #include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/extract_clusters.h> #include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h> #include <pcl/segmentation/sac_segmentation.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <limits> namespace cloud_clustering
#include <utility> {
#include <visualization_msgs/msg/marker.hpp> static const rclcpp::Logger LOGGER = rclcpp::get_logger("cloud_clustering");
#include <visualization_msgs/msg/marker_array.hpp>
using namespace std; CloudClustering::CloudClustering(
using namespace cv; const rclcpp::NodeOptions &options) : CloudClustering("", options)
{
}
rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr objID_pub; CloudClustering::CloudClustering(
const std::string &name_space,
const rclcpp::NodeOptions &options) : Node("CloudClustering", name_space, options)
{
// Topic parameters
this->declare_parameter<std::string>("topic_in", "filtered_points");
this->declare_parameter<std::string>("frame_id", "velodyne");
// KF init // Cluster parameters
int stateDim = 4; // [x,y,v_x,v_y]//,w,h] this->declare_parameter<float>("z_dim_scale", 0);
int measDim = 2; // [z_x,z_y,z_w,z_h] this->declare_parameter<float>("cluster_tolerance", 0.0f);
int ctrlDim = 0; this->declare_parameter<int>("min_cluster_size", 0);
this->declare_parameter<int>("max_cluster_size", 0);
std::string frame_id; this->get_parameter("topic_in", topic_in);
std::string topic_in; this->get_parameter("frame_id", frame_id);
this->get_parameter("z_dim_scale", z_dim_scale);
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);
float z_dim_scale; // Initialize publishers
float cluster_tolerance; objID_pub = this->create_publisher<std_msgs::msg::Int32MultiArray>("object_ids", 10);
int min_cluster_size; pub_cluster0 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster0", 10);
int max_cluster_size; pub_cluster1 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster1", 10);
pub_cluster2 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster2", 10);
pub_cluster3 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster3", 10);
pub_cluster4 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster4", 10);
pub_cluster5 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster5", 10);
markerPub = this->create_publisher<visualization_msgs::msg::MarkerArray>("markers", 10);
cv::KalmanFilter KF0(stateDim, measDim, ctrlDim, CV_32F); RCLCPP_INFO(this->get_logger(), "About to setup callback");
cv::KalmanFilter KF1(stateDim, measDim, ctrlDim, CV_32F); clock_ = this->get_clock();
cv::KalmanFilter KF2(stateDim, measDim, ctrlDim, CV_32F);
cv::KalmanFilter KF3(stateDim, measDim, ctrlDim, CV_32F);
cv::KalmanFilter KF4(stateDim, measDim, ctrlDim, CV_32F);
cv::KalmanFilter KF5(stateDim, measDim, ctrlDim, CV_32F);
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster0; // Create a ROS subscriber for the input point cloud
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster1; sub = this->create_subscription<sensor_msgs::msg::PointCloud2>(
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster2; topic_in,
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster3; 10, // queue size
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster4; std::bind(&CloudClustering::cloud_cb, this, std::placeholders::_1));
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster5;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr markerPub; std::cout << "Started clustering node with parameters:\n"
<< "topic_in: " << topic_in << "\n"
<< "frame_id: " << frame_id << "\n"
<< "z_dim_scale: " << z_dim_scale << "\n"
<< "cluster_tolerance: " << cluster_tolerance << "\n"
<< "min_cluster_size: " << min_cluster_size << "\n"
<< "max_cluster_size: " << max_cluster_size << "\n";
}
std::vector<geometry_msgs::msg::Point> prevClusterCenters; // calculate euclidean distance of two points
double CloudClustering::euclidean_distance(geometry_msgs::msg::Point &p1, geometry_msgs::msg::Point &p2)
cv::Mat state(stateDim, 1, CV_32F); {
cv::Mat_<float> measurement(2, 1);
std::vector<int> objID; // Output of the data association using KF
// measurement.setTo(Scalar(0));
bool firstFrame = true;
// calculate euclidean distance of two points
double euclidean_distance(geometry_msgs::msg::Point &p1, geometry_msgs::msg::Point &p2) {
return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) +
(p1.z - p2.z) * (p1.z - p2.z) * z_dim_scale); (p1.z - p2.z) * (p1.z - p2.z) * z_dim_scale);
} }
/* /*
//Count unique object IDs. just to make sure same ID has not been assigned to //Count unique object IDs. just to make sure same ID has not been assigned to
two KF_Trackers. int countIDs(vector<int> v) two KF_Trackers. int countIDs(vector<int> v)
{ {
transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n = transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n =
distance(v.end(), v.begin()) sort(v.begin(), v.end()); // Average case O(n log distance(v.end(), v.begin()) sort(v.begin(), v.end()); // Average case O(n log
n), worst case O(n^2) (usually implemented as quicksort. n), worst case O(n^2) (usually implemented as quicksort.
// To guarantee worst case O(n log n) replace with make_heap, then // To guarantee worst case O(n log n) replace with make_heap, then
sort_heap. sort_heap.
// Unique will take a sorted range, and move things around to get duplicated // Unique will take a sorted range, and move things around to get duplicated
// items to the back and returns an iterator to the end of the unique // items to the back and returns an iterator to the end of the unique
section of the range auto unique_end = unique(v.begin(), v.end()); // Again n section of the range auto unique_end = unique(v.begin(), v.end()); // Again n
comparisons return distance(unique_end, v.begin()); // Constant time for random comparisons return distance(unique_end, v.begin()); // Constant time for random
access iterators (like vector's) access iterators (like vector's)
} }
*/ */
/* /*
objID: vector containing the IDs of the clusters that should be associated with objID: vector containing the IDs of the clusters that should be associated with
each KF_Tracker objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc. each KF_Tracker objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc.
*/ */
std::pair<int, int> findIndexOfMin(std::vector<std::vector<float>> distMat) { std::pair<int, int> CloudClustering::findIndexOfMin(std::vector<std::vector<float>> distMat)
{
std::pair<int, int> minIndex; std::pair<int, int> minIndex;
float minEl = std::numeric_limits<float>::max(); float minEl = std::numeric_limits<float>::max();
for (int i = 0; i < distMat.size(); i++) for (int i = 0; i < distMat.size(); i++)
for (int j = 0; j < distMat.at(0).size(); j++) { for (int j = 0; j < distMat.at(0).size(); j++)
if (distMat[i][j] < minEl) { {
if (distMat[i][j] < minEl)
{
minEl = distMat[i][j]; minEl = distMat[i][j];
minIndex = std::make_pair(i, j); minIndex = std::make_pair(i, j);
} }
} }
return minIndex; return minIndex;
} }
void KFT(const std_msgs::msg::Float32MultiArray ccs) {
void CloudClustering::KFT(const std_msgs::msg::Float32MultiArray ccs)
{
// First predict, to update the internal statePre variable // First predict, to update the internal statePre variable
@@ -138,7 +164,8 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
int i = 0; int i = 0;
for (std::vector<float>::const_iterator it = ccs.data.begin(); for (std::vector<float>::const_iterator it = ccs.data.begin();
it != ccs.data.end(); it += 3) { it != ccs.data.end(); it += 3)
{
geometry_msgs::msg::Point pt; geometry_msgs::msg::Point pt;
pt.x = *it; pt.x = *it;
pt.y = *(it + 1); pt.y = *(it + 1);
@@ -149,7 +176,8 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
std::vector<geometry_msgs::msg::Point> KFpredictions; std::vector<geometry_msgs::msg::Point> KFpredictions;
i = 0; i = 0;
for (auto it = pred.begin(); it != pred.end(); it++) { for (auto it = pred.begin(); it != pred.end(); it++)
{
geometry_msgs::msg::Point pt; geometry_msgs::msg::Point pt;
pt.x = (*it).at<float>(0); pt.x = (*it).at<float>(0);
pt.y = (*it).at<float>(1); pt.y = (*it).at<float>(1);
@@ -167,9 +195,11 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
std::vector<geometry_msgs::msg::Point> copyOfClusterCenters(clusterCenters); std::vector<geometry_msgs::msg::Point> copyOfClusterCenters(clusterCenters);
std::vector<std::vector<float>> distMat; std::vector<std::vector<float>> distMat;
for (int filterN = 0; filterN < 6; filterN++) { for (int filterN = 0; filterN < 6; filterN++)
{
std::vector<float> distVec; std::vector<float> distVec;
for (int n = 0; n < 6; n++) { for (int n = 0; n < 6; n++)
{
distVec.push_back( distVec.push_back(
euclidean_distance(KFpredictions[filterN], copyOfClusterCenters[n])); euclidean_distance(KFpredictions[filterN], copyOfClusterCenters[n]));
} }
@@ -185,10 +215,11 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
not assigned to another cluster copyOfClusterCenters[ID].y=10000; not assigned to another cluster copyOfClusterCenters[ID].y=10000;
copyOfClusterCenters[ID].z=10000; copyOfClusterCenters[ID].z=10000;
*/ */
//cout << "filterN=" << filterN << "\n"; // cout << "filterN=" << filterN << "\n";
} }
for (int clusterCount = 0; clusterCount < 6; clusterCount++) { for (int clusterCount = 0; clusterCount < 6; clusterCount++)
{
// 1. Find min(distMax)==> (i,j); // 1. Find min(distMax)==> (i,j);
std::pair<int, int> minIndex(findIndexOfMin(distMat)); std::pair<int, int> minIndex(findIndexOfMin(distMat));
// 2. objID[i]=clusterCenters[j]; counter++ // 2. objID[i]=clusterCenters[j]; counter++
@@ -203,7 +234,7 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
distMat[row][minIndex.second] = 10000.0; distMat[row][minIndex.second] = 10000.0;
} }
// 4. if(counter<6) got to 1. // 4. if(counter<6) got to 1.
//cout << "clusterCount=" << clusterCount << "\n"; // cout << "clusterCount=" << clusterCount << "\n";
} }
// cout<<"Got object IDs"<<"\n"; // cout<<"Got object IDs"<<"\n";
@@ -219,7 +250,8 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
visualization_msgs::msg::MarkerArray clusterMarkers; visualization_msgs::msg::MarkerArray clusterMarkers;
for (int i = 0; i < 6; i++) { for (int i = 0; i < 6; i++)
{
visualization_msgs::msg::Marker m; visualization_msgs::msg::Marker m;
m.id = i; m.id = i;
@@ -254,8 +286,9 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
objID_pub->publish(obj_id); objID_pub->publish(obj_id);
// convert clusterCenters from geometry_msgs::msg::Point to floats // convert clusterCenters from geometry_msgs::msg::Point to floats
std::vector<std::vector<float>> cc; std::vector<std::vector<float>> cc;
for (int i = 0; i < 6; i++) { for (int i = 0; i < 6; i++)
vector<float> pt; {
std::vector<float> pt;
pt.push_back(clusterCenters[objID[i]].x); pt.push_back(clusterCenters[objID[i]].x);
pt.push_back(clusterCenters[objID[i]].y); pt.push_back(clusterCenters[objID[i]].y);
pt.push_back(clusterCenters[objID[i]].z); pt.push_back(clusterCenters[objID[i]].z);
@@ -280,29 +313,28 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
// cout<<"meas0Mat"<<meas0Mat<<"\n"; // cout<<"meas0Mat"<<meas0Mat<<"\n";
if (!(meas0Mat.at<float>(0, 0) == 0.0f || meas0Mat.at<float>(1, 0) == 0.0f)) if (!(meas0Mat.at<float>(0, 0) == 0.0f || meas0Mat.at<float>(1, 0) == 0.0f))
Mat estimated0 = KF0.correct(meas0Mat); cv::Mat estimated0 = KF0.correct(meas0Mat);
if (!(meas1[0] == 0.0f || meas1[1] == 0.0f)) if (!(meas1[0] == 0.0f || meas1[1] == 0.0f))
Mat estimated1 = KF1.correct(meas1Mat); cv::Mat estimated1 = KF1.correct(meas1Mat);
if (!(meas2[0] == 0.0f || meas2[1] == 0.0f)) if (!(meas2[0] == 0.0f || meas2[1] == 0.0f))
Mat estimated2 = KF2.correct(meas2Mat); cv::Mat estimated2 = KF2.correct(meas2Mat);
if (!(meas3[0] == 0.0f || meas3[1] == 0.0f)) if (!(meas3[0] == 0.0f || meas3[1] == 0.0f))
Mat estimated3 = KF3.correct(meas3Mat); cv::Mat estimated3 = KF3.correct(meas3Mat);
if (!(meas4[0] == 0.0f || meas4[1] == 0.0f)) if (!(meas4[0] == 0.0f || meas4[1] == 0.0f))
Mat estimated4 = KF4.correct(meas4Mat); cv::Mat estimated4 = KF4.correct(meas4Mat);
if (!(meas5[0] == 0.0f || meas5[1] == 0.0f)) if (!(meas5[0] == 0.0f || meas5[1] == 0.0f))
Mat estimated5 = KF5.correct(meas5Mat); cv::Mat estimated5 = KF5.correct(meas5Mat);
// Publish the point clouds belonging to each clusters // Publish the point clouds belonging to each clusters
// cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n"; // cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n";
// Point statePt(estimated.at<float>(0),estimated.at<float>(1)); // Point statePt(estimated.at<float>(0),estimated.at<float>(1));
// cout<<"DONE KF_TRACKER\n"; // cout<<"DONE KF_TRACKER\n";
} }
void publish_cloud( void CloudClustering::publish_cloud(
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub, rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub,
pcl::PointCloud<pcl::PointXYZI>::Ptr cluster, pcl::PointCloud<pcl::PointXYZI>::Ptr cluster)
rclcpp::Node::SharedPtr node) {
{
// Create a PointCloud2 message // Create a PointCloud2 message
auto clustermsg = std::make_shared<sensor_msgs::msg::PointCloud2>(); auto clustermsg = std::make_shared<sensor_msgs::msg::PointCloud2>();
@@ -311,37 +343,19 @@ void publish_cloud(
// Set header info // Set header info
clustermsg->header.frame_id = frame_id; clustermsg->header.frame_id = frame_id;
clustermsg->header.stamp = node->now(); clustermsg->header.stamp = clock_->now();
// Publish the message // Publish the message
pub->publish(*clustermsg); pub->publish(*clustermsg);
}
void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
std::vector<std::pair<
pcl::PointCloud<
pcl::PointXYZI
>::Ptr
,float>>& cluster_vec,
std::vector<pcl::PointXYZI>& clusterCentroids
) {
pcl::PointCloud<pcl::PointXYZI>::Ptr input_a(new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(*input, *input_a);
// Apply filters
pcl::PointCloud<pcl::PointXYZI>::Ptr fov_filtered(new pcl::PointCloud<pcl::PointXYZI>);
fov_filtered->reserve(input_a->size());
for (const auto& point : input_a->points) {
if (point.x < 0 || point.x > 40) continue;
if (isnan(point.x) || isnan(point.y) || isnan(point.z)) continue;
if (point.z < -0.5 || point.z > 1.5) continue;
fov_filtered->points.push_back(point); // emplace_back also works
} }
if (fov_filtered->empty()) return; // nothing to cluster void CloudClustering::parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
std::vector<std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, float>> &cluster_vec,
std::vector<pcl::PointXYZI> &clusterCentroids)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud = fov_filtered; pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(*input, *input_cloud);
// Setup KdTree // Setup KdTree
pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>); pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
@@ -360,7 +374,8 @@ void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
std::vector<pcl::PointIndices>::const_iterator it; std::vector<pcl::PointIndices>::const_iterator it;
std::vector<int>::const_iterator pit; std::vector<int>::const_iterator pit;
for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_cluster( pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_cluster(
new pcl::PointCloud<pcl::PointXYZI>); new pcl::PointCloud<pcl::PointXYZI>);
@@ -369,7 +384,8 @@ void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
int numPts = 0; int numPts = 0;
float intensity_sum = 0.0; float intensity_sum = 0.0;
for (pit = it->indices.begin(); pit != it->indices.end(); pit++) { for (pit = it->indices.begin(); pit != it->indices.end(); pit++)
{
cloud_cluster->points.push_back(input_cloud->points[*pit]); cloud_cluster->points.push_back(input_cloud->points[*pit]);
x += input_cloud->points[*pit].x; x += input_cloud->points[*pit].x;
@@ -383,7 +399,8 @@ void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
// mindist_this_cluster); // mindist_this_cluster);
} }
if (numPts == 0) continue; if (numPts == 0)
continue;
pcl::PointXYZI centroid; pcl::PointXYZI centroid;
centroid.x = x / numPts; centroid.x = x / numPts;
@@ -397,7 +414,8 @@ void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
} }
// Ensure at least 6 clusters exist to publish (later clusters may be empty) // Ensure at least 6 clusters exist to publish (later clusters may be empty)
while (cluster_vec.size() < 6) { while (cluster_vec.size() < 6)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr empty_cluster( pcl::PointCloud<pcl::PointXYZI>::Ptr empty_cluster(
new pcl::PointCloud<pcl::PointXYZI>); new pcl::PointCloud<pcl::PointXYZI>);
@@ -413,10 +431,10 @@ void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
centroid.intensity = 0.0; centroid.intensity = 0.0;
cluster_vec.push_back(std::make_pair(empty_cluster, 0.0f)); cluster_vec.push_back(std::make_pair(empty_cluster, 0.0f));
} }
while (clusterCentroids.size() < 6) { while (clusterCentroids.size() < 6)
{
pcl::PointXYZI centroid; pcl::PointXYZI centroid;
centroid.x = 0.0; centroid.x = 0.0;
centroid.y = 0.0; centroid.y = 0.0;
@@ -425,17 +443,18 @@ void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
clusterCentroids.push_back(centroid); clusterCentroids.push_back(centroid);
} }
} }
void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input, rclcpp::Node::SharedPtr node) void CloudClustering::cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input)
{ {
// Vector of cluster pointclouds // Vector of cluster pointclouds
std::vector<std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr,float>> cluster_vec; std::vector<std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, float>> cluster_vec;
// Cluster centroids // Cluster centroids
std::vector<pcl::PointXYZI> clusterCentroids; std::vector<pcl::PointXYZI> clusterCentroids;
if (firstFrame) { if (firstFrame)
{
// If this is the first frame, initialize kalman filters for the clustered objects // If this is the first frame, initialize kalman filters for the clustered objects
// Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset. // Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset.
// Could be made generic by creating a Kalman Filter only when a new object // Could be made generic by creating a Kalman Filter only when a new object
@@ -445,17 +464,17 @@ void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input, rclcpp::Node
float dvy = 0.01f; // 1.0 float dvy = 0.01f; // 1.0
float dx = 1.0f; float dx = 1.0f;
float dy = 1.0f; float dy = 1.0f;
KF0.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, KF0.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
dvx, 0, 0, 0, 0, dvy); dvx, 0, 0, 0, 0, dvy);
KF1.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, KF1.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
dvx, 0, 0, 0, 0, dvy); dvx, 0, 0, 0, 0, dvy);
KF2.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, KF2.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
dvx, 0, 0, 0, 0, dvy); dvx, 0, 0, 0, 0, dvy);
KF3.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, KF3.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
dvx, 0, 0, 0, 0, dvy); dvx, 0, 0, 0, 0, dvy);
KF4.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, KF4.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
dvx, 0, 0, 0, 0, dvy); dvx, 0, 0, 0, 0, dvy);
KF5.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, KF5.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
dvx, 0, 0, 0, 0, dvy); dvx, 0, 0, 0, 0, dvy);
cv::setIdentity(KF0.measurementMatrix); cv::setIdentity(KF0.measurementMatrix);
@@ -473,12 +492,12 @@ void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input, rclcpp::Node
//// [ 0 0 0 0 0 Eh ] //// [ 0 0 0 0 0 Eh ]
float sigmaP = 0.01; float sigmaP = 0.01;
float sigmaQ = 0.1; float sigmaQ = 0.1;
setIdentity(KF0.processNoiseCov, Scalar::all(sigmaP)); setIdentity(KF0.processNoiseCov, cv::Scalar::all(sigmaP));
setIdentity(KF1.processNoiseCov, Scalar::all(sigmaP)); setIdentity(KF1.processNoiseCov, cv::Scalar::all(sigmaP));
setIdentity(KF2.processNoiseCov, Scalar::all(sigmaP)); setIdentity(KF2.processNoiseCov, cv::Scalar::all(sigmaP));
setIdentity(KF3.processNoiseCov, Scalar::all(sigmaP)); setIdentity(KF3.processNoiseCov, cv::Scalar::all(sigmaP));
setIdentity(KF4.processNoiseCov, Scalar::all(sigmaP)); setIdentity(KF4.processNoiseCov, cv::Scalar::all(sigmaP));
setIdentity(KF5.processNoiseCov, Scalar::all(sigmaP)); setIdentity(KF5.processNoiseCov, cv::Scalar::all(sigmaP));
// Meas noise cov matrix R // Meas noise cov matrix R
cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(sigmaQ)); // 1e-1 cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(sigmaQ)); // 1e-1
cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(sigmaQ)); cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(sigmaQ));
@@ -528,7 +547,8 @@ void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input, rclcpp::Node
firstFrame = false; firstFrame = false;
for (int i = 0; i < 6; i++) { for (int i = 0; i < 6; i++)
{
geometry_msgs::msg::Point pt; geometry_msgs::msg::Point pt;
pt.x = clusterCentroids.at(i).x; pt.x = clusterCentroids.at(i).x;
pt.y = clusterCentroids.at(i).y; pt.y = clusterCentroids.at(i).y;
@@ -536,11 +556,13 @@ void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input, rclcpp::Node
} }
} }
else { else
{
parse_cloud(input, cluster_vec, clusterCentroids); parse_cloud(input, cluster_vec, clusterCentroids);
std_msgs::msg::Float32MultiArray cc; std_msgs::msg::Float32MultiArray cc;
for (int i = 0; i < 6; i++) { for (int i = 0; i < 6; i++)
{
cc.data.push_back(clusterCentroids.at(i).x); cc.data.push_back(clusterCentroids.at(i).x);
cc.data.push_back(clusterCentroids.at(i).y); cc.data.push_back(clusterCentroids.at(i).y);
cc.data.push_back(clusterCentroids.at(i).z); cc.data.push_back(clusterCentroids.at(i).z);
@@ -550,41 +572,47 @@ void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input, rclcpp::Node
KFT(cc); KFT(cc);
int i = 0; int i = 0;
std::sort(objID.begin(), objID.end(), [cluster_vec](const int &a, const int &b) { std::sort(objID.begin(), objID.end(), [cluster_vec](const int &a, const int &b)
return cluster_vec[a].second < cluster_vec[b].second; { return cluster_vec[a].second < cluster_vec[b].second; });
});
for (auto it = objID.begin(); it != objID.end(); it++) { for (auto it = objID.begin(); it != objID.end(); it++)
switch (i) { {
case 0: { switch (i)
{
publish_cloud(pub_cluster0, cluster_vec[*it].first, node); case 0:
{
publish_cloud(pub_cluster0, cluster_vec[*it].first);
i++; i++;
break; break;
} }
case 1: { case 1:
publish_cloud(pub_cluster1, cluster_vec[*it].first, node); {
publish_cloud(pub_cluster1, cluster_vec[*it].first);
i++; i++;
break; break;
} }
case 2: { case 2:
publish_cloud(pub_cluster2, cluster_vec[*it].first, node); {
publish_cloud(pub_cluster2, cluster_vec[*it].first);
i++; i++;
break; break;
} }
case 3: { case 3:
publish_cloud(pub_cluster3, cluster_vec[*it].first, node); {
publish_cloud(pub_cluster3, cluster_vec[*it].first);
i++; i++;
break; break;
} }
case 4: { case 4:
publish_cloud(pub_cluster4, cluster_vec[*it].first, node); {
publish_cloud(pub_cluster4, cluster_vec[*it].first);
i++; i++;
break; break;
} }
case 5: { case 5:
publish_cloud(pub_cluster5, cluster_vec[*it].first, node); {
publish_cloud(pub_cluster5, cluster_vec[*it].first);
i++; i++;
break; break;
} }
@@ -593,72 +621,5 @@ void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input, rclcpp::Node
} }
} }
} }
} }
int main(int argc, char **argv) {
// ROS init
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("cloud_clustering");
//Topic parameters
node->declare_parameter<std::string>("topic_in", "!defaultTopic!");
node->declare_parameter<std::string>("frame_id", "!defaultFrameId!");
//Cluster parameters
node->declare_parameter<float>("z_dim_scale", 0);
node->declare_parameter<float>("cluster_tolerance", 0.0f);
node->declare_parameter<int>("min_cluster_size", 0);
node->declare_parameter<int>("max_cluster_size", 0);
node->get_parameter("topic_in", topic_in);
node->get_parameter("frame_id", frame_id);
node->get_parameter("z_dim_scale", z_dim_scale);
node->get_parameter("cluster_tolerance", cluster_tolerance);
node->get_parameter("min_cluster_size", min_cluster_size);
node->get_parameter("max_cluster_size", max_cluster_size);
// Initialize publishers
objID_pub = node->create_publisher<std_msgs::msg::Int32MultiArray>("object_ids", 10);
pub_cluster0 = node->create_publisher<sensor_msgs::msg::PointCloud2>("cluster0", 10);
pub_cluster1 = node->create_publisher<sensor_msgs::msg::PointCloud2>("cluster1", 10);
pub_cluster2 = node->create_publisher<sensor_msgs::msg::PointCloud2>("cluster2", 10);
pub_cluster3 = node->create_publisher<sensor_msgs::msg::PointCloud2>("cluster3", 10);
pub_cluster4 = node->create_publisher<sensor_msgs::msg::PointCloud2>("cluster4", 10);
pub_cluster5 = node->create_publisher<sensor_msgs::msg::PointCloud2>("cluster5", 10);
markerPub = node->create_publisher<visualization_msgs::msg::MarkerArray>("markers", 10);
// Publishers to publish the state of the objects (pos and vel)
// objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1);
cout << "About to setup callback\n";
// Create a ROS subscriber for the input point cloud
auto sub = node->create_subscription<sensor_msgs::msg::PointCloud2>(
topic_in,
10, // queue size
[node](sensor_msgs::msg::PointCloud2::SharedPtr msg) {
cloud_cb(msg, node);
});
// Publishers
objID_pub = node->create_publisher<std_msgs::msg::Int32MultiArray>("obj_id", 10);
markerPub = node->create_publisher<visualization_msgs::msg::MarkerArray>("viz", 10);
cout << "Started clustering node with parameters:\n"
<< "topic_in: " << topic_in << "\n"
<< "frame_id: " << frame_id << "\n"
<< "z_dim_scale: " << z_dim_scale << "\n"
<< "cluster_tolerance: " << cluster_tolerance << "\n"
<< "min_cluster_size: " << min_cluster_size << "\n"
<< "max_cluster_size: " << max_cluster_size << "\n";
// Example if you still need cluster centers publisher:
// cc_pos = node->create_publisher<std_msgs::msg::Float32MultiArray>("ccs", 10);
/* Point cloud clustering
*/
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
} }

17
src/target_tracking/src/cloud_clustering_node.cpp Normal file
View File

@@ -0,0 +1,17 @@
#include "cloud_clustering.hpp"
int main(int argc, char * argv[])
{
// Force flush of the stdout buffer.
// This ensures a correct sync of all prints
// even when executed simultaneously within a launch file.
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
const rclcpp::NodeOptions options;
rclcpp::spin(std::make_shared<cloud_clustering::CloudClustering>(options));
rclcpp::shutdown();
return 0;
}