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
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
ament_target_dependencies(cloud_clustering_node
ament_target_dependencies(cloud_clustering
rclcpp
sensor_msgs
visualization_msgs
@@ -51,7 +57,10 @@ ament_target_dependencies(cloud_clustering_node
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(TARGETS
@@ -60,6 +69,13 @@ install(TARGETS
DESTINATION lib/${PROJECT_NAME}
)
install(TARGETS
cloud_clustering
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PACKAGE_NAME}
)
# Install launchfile
install(
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

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

@@ -1,86 +1,107 @@
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <algorithm>
#include <fstream>
#include <geometry_msgs/msg/point.hpp>
// Default C++ imports
#include <limits>
#include <utility>
#include <string.h>
#include <iostream>
#include <iterator>
#include <opencv2/video/video.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <algorithm>
#include <fstream>
// Headerfile
#include "cloud_clustering.hpp"
// Ros2 import
#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/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/geometry.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.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/model_types.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <limits>
#include <utility>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
namespace cloud_clustering
{
static const rclcpp::Logger LOGGER = rclcpp::get_logger("cloud_clustering");
using namespace std;
using namespace cv;
CloudClustering::CloudClustering(
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
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;
// Cluster parameters
this->declare_parameter<float>("z_dim_scale", 0);
this->declare_parameter<float>("cluster_tolerance", 0.0f);
this->declare_parameter<int>("min_cluster_size", 0);
this->declare_parameter<int>("max_cluster_size", 0);
std::string frame_id;
std::string topic_in;
this->get_parameter("topic_in", 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;
float cluster_tolerance;
int min_cluster_size;
int max_cluster_size;
// Initialize publishers
objID_pub = this->create_publisher<std_msgs::msg::Int32MultiArray>("object_ids", 10);
pub_cluster0 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster0", 10);
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);
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);
RCLCPP_INFO(this->get_logger(), "About to setup callback");
clock_ = this->get_clock();
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;
// Create a ROS subscriber for the input point cloud
sub = this->create_subscription<sensor_msgs::msg::PointCloud2>(
topic_in,
10, // queue size
std::bind(&CloudClustering::cloud_cb, this, std::placeholders::_1));
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr markerPub;
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;
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";
}
// calculate euclidean distance of two points
double euclidean_distance(geometry_msgs::msg::Point &p1, geometry_msgs::msg::Point &p2) {
double CloudClustering::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) +
(p1.z - p2.z) * (p1.z - p2.z) * z_dim_scale);
}
@@ -108,19 +129,24 @@ 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.
*/
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;
float minEl = std::numeric_limits<float>::max();
for (int i = 0; i < distMat.size(); i++)
for (int j = 0; j < distMat.at(0).size(); j++) {
if (distMat[i][j] < minEl) {
for (int j = 0; j < distMat.at(0).size(); j++)
{
if (distMat[i][j] < minEl)
{
minEl = distMat[i][j];
minIndex = std::make_pair(i, j);
}
}
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
@@ -138,7 +164,8 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
int i = 0;
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;
pt.x = *it;
pt.y = *(it + 1);
@@ -149,7 +176,8 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
std::vector<geometry_msgs::msg::Point> KFpredictions;
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;
pt.x = (*it).at<float>(0);
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<std::vector<float>> distMat;
for (int filterN = 0; filterN < 6; filterN++) {
for (int filterN = 0; filterN < 6; filterN++)
{
std::vector<float> distVec;
for (int n = 0; n < 6; n++) {
for (int n = 0; n < 6; n++)
{
distVec.push_back(
euclidean_distance(KFpredictions[filterN], copyOfClusterCenters[n]));
}
@@ -188,7 +218,8 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
// cout << "filterN=" << filterN << "\n";
}
for (int clusterCount = 0; clusterCount < 6; clusterCount++) {
for (int clusterCount = 0; clusterCount < 6; clusterCount++)
{
// 1. Find min(distMax)==> (i,j);
std::pair<int, int> minIndex(findIndexOfMin(distMat));
// 2. objID[i]=clusterCenters[j]; counter++
@@ -219,7 +250,8 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
visualization_msgs::msg::MarkerArray clusterMarkers;
for (int i = 0; i < 6; i++) {
for (int i = 0; i < 6; i++)
{
visualization_msgs::msg::Marker m;
m.id = i;
@@ -254,8 +286,9 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
objID_pub->publish(obj_id);
// convert clusterCenters from geometry_msgs::msg::Point to floats
std::vector<std::vector<float>> cc;
for (int i = 0; i < 6; i++) {
vector<float> pt;
for (int i = 0; i < 6; i++)
{
std::vector<float> pt;
pt.push_back(clusterCenters[objID[i]].x);
pt.push_back(clusterCenters[objID[i]].y);
pt.push_back(clusterCenters[objID[i]].z);
@@ -280,17 +313,17 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
// cout<<"meas0Mat"<<meas0Mat<<"\n";
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))
Mat estimated1 = KF1.correct(meas1Mat);
cv::Mat estimated1 = KF1.correct(meas1Mat);
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))
Mat estimated3 = KF3.correct(meas3Mat);
cv::Mat estimated3 = KF3.correct(meas3Mat);
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))
Mat estimated5 = KF5.correct(meas5Mat);
cv::Mat estimated5 = KF5.correct(meas5Mat);
// Publish the point clouds belonging to each clusters
@@ -298,10 +331,9 @@ void KFT(const std_msgs::msg::Float32MultiArray ccs) {
// Point statePt(estimated.at<float>(0),estimated.at<float>(1));
// cout<<"DONE KF_TRACKER\n";
}
void publish_cloud(
void CloudClustering::publish_cloud(
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub,
pcl::PointCloud<pcl::PointXYZI>::Ptr cluster,
rclcpp::Node::SharedPtr node)
pcl::PointCloud<pcl::PointXYZI>::Ptr cluster)
{
// Create a PointCloud2 message
auto clustermsg = std::make_shared<sensor_msgs::msg::PointCloud2>();
@@ -311,37 +343,19 @@ void publish_cloud(
// Set header info
clustermsg->header.frame_id = frame_id;
clustermsg->header.stamp = node->now();
clustermsg->header.stamp = clock_->now();
// Publish the message
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);
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)
{
// 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
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
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<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(
new pcl::PointCloud<pcl::PointXYZI>);
@@ -369,7 +384,8 @@ void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
int numPts = 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]);
x += input_cloud->points[*pit].x;
@@ -383,7 +399,8 @@ void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
// mindist_this_cluster);
}
if (numPts == 0) continue;
if (numPts == 0)
continue;
pcl::PointXYZI centroid;
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)
while (cluster_vec.size() < 6) {
while (cluster_vec.size() < 6)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr empty_cluster(
new pcl::PointCloud<pcl::PointXYZI>);
@@ -413,10 +431,10 @@ void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
centroid.intensity = 0.0;
cluster_vec.push_back(std::make_pair(empty_cluster, 0.0f));
}
while (clusterCentroids.size() < 6) {
while (clusterCentroids.size() < 6)
{
pcl::PointXYZI centroid;
centroid.x = 0.0;
centroid.y = 0.0;
@@ -427,7 +445,7 @@ void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
}
}
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
@@ -435,7 +453,8 @@ void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input, rclcpp::Node
// Cluster centroids
std::vector<pcl::PointXYZI> clusterCentroids;
if (firstFrame) {
if (firstFrame)
{
// If this is the first frame, initialize kalman filters for the clustered objects
// 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
@@ -445,17 +464,17 @@ void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input, rclcpp::Node
float dvy = 0.01f; // 1.0
float dx = 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);
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);
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);
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);
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);
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);
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 ]
float sigmaP = 0.01;
float sigmaQ = 0.1;
setIdentity(KF0.processNoiseCov, Scalar::all(sigmaP));
setIdentity(KF1.processNoiseCov, Scalar::all(sigmaP));
setIdentity(KF2.processNoiseCov, Scalar::all(sigmaP));
setIdentity(KF3.processNoiseCov, Scalar::all(sigmaP));
setIdentity(KF4.processNoiseCov, Scalar::all(sigmaP));
setIdentity(KF5.processNoiseCov, Scalar::all(sigmaP));
setIdentity(KF0.processNoiseCov, cv::Scalar::all(sigmaP));
setIdentity(KF1.processNoiseCov, cv::Scalar::all(sigmaP));
setIdentity(KF2.processNoiseCov, cv::Scalar::all(sigmaP));
setIdentity(KF3.processNoiseCov, cv::Scalar::all(sigmaP));
setIdentity(KF4.processNoiseCov, cv::Scalar::all(sigmaP));
setIdentity(KF5.processNoiseCov, cv::Scalar::all(sigmaP));
// Meas noise cov matrix R
cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(sigmaQ)); // 1e-1
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;
for (int i = 0; i < 6; i++) {
for (int i = 0; i < 6; i++)
{
geometry_msgs::msg::Point pt;
pt.x = clusterCentroids.at(i).x;
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);
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).y);
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);
int i = 0;
std::sort(objID.begin(), objID.end(), [cluster_vec](const int &a, const int &b) {
return cluster_vec[a].second < cluster_vec[b].second;
});
std::sort(objID.begin(), objID.end(), [cluster_vec](const int &a, const int &b)
{ return cluster_vec[a].second < cluster_vec[b].second; });
for (auto it = objID.begin(); it != objID.end(); it++) {
switch (i) {
case 0: {
publish_cloud(pub_cluster0, cluster_vec[*it].first, node);
for (auto it = objID.begin(); it != objID.end(); it++)
{
switch (i)
{
case 0:
{
publish_cloud(pub_cluster0, cluster_vec[*it].first);
i++;
break;
}
case 1: {
publish_cloud(pub_cluster1, cluster_vec[*it].first, node);
case 1:
{
publish_cloud(pub_cluster1, cluster_vec[*it].first);
i++;
break;
}
case 2: {
publish_cloud(pub_cluster2, cluster_vec[*it].first, node);
case 2:
{
publish_cloud(pub_cluster2, cluster_vec[*it].first);
i++;
break;
}
case 3: {
publish_cloud(pub_cluster3, cluster_vec[*it].first, node);
case 3:
{
publish_cloud(pub_cluster3, cluster_vec[*it].first);
i++;
break;
}
case 4: {
publish_cloud(pub_cluster4, cluster_vec[*it].first, node);
case 4:
{
publish_cloud(pub_cluster4, cluster_vec[*it].first);
i++;
break;
}
case 5: {
publish_cloud(pub_cluster5, cluster_vec[*it].first, node);
case 5:
{
publish_cloud(pub_cluster5, cluster_vec[*it].first);
i++;
break;
}
@@ -594,71 +622,4 @@ 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;
}