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

made tracking more stable, added size limits for cluster + changed cluster assignment to hungarian

Browse Source
This commit is contained in:
Liam
2025-09-20 17:04:53 +02:00
parent f537027329
commit 2ed3bbf4dc
5 changed files with 203 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/target_tracking/include/cloud_clustering.hpp
View File

@@ -32,6 +32,12 @@ namespace cloud_clustering
float cluster_tolerance;
int min_cluster_size;
int max_cluster_size;
float min_width;
float min_height;
float min_length;
float max_width;
float max_height;
float max_length;
cv::KalmanFilter KF0(stateDim, measDim, ctrlDim, CV_32F);
cv::KalmanFilter KF1(stateDim, measDim, ctrlDim, CV_32F);

66
src/target_tracking/include/hungarian.hpp Normal file
View File

@@ -0,0 +1,66 @@
#include <vector>
#include <limits>
#include <algorithm>
class Hungarian {
public:
Hungarian(const std::vector<std::vector<float>>& costMatrix)
: n(costMatrix.size()), m(costMatrix[0].size()),
cost(costMatrix), u(n + 1), v(m + 1),
p(m + 1), way(m + 1) {}
// Returns assignment vector: assignment[i] = j
std::vector<int> solve() {
for (int i = 1; i <= n; i++) {
p[0] = i;
int j0 = 0;
std::vector<float> minv(m + 1, std::numeric_limits<float>::max());
std::vector<char> used(m + 1, false);
do {
used[j0] = true;
int i0 = p[j0], j1 = 0;
float delta = std::numeric_limits<float>::max();
for (int j = 1; j <= m; j++) {
if (!used[j]) {
float cur = cost[i0 - 1][j - 1] - u[i0] - v[j];
if (cur < minv[j]) {
minv[j] = cur;
way[j] = j0;
}
if (minv[j] < delta) {
delta = minv[j];
j1 = j;
}
}
}
for (int j = 0; j <= m; j++) {
if (used[j]) {
u[p[j]] += delta;
v[j] -= delta;
} else {
minv[j] -= delta;
}
}
j0 = j1;
} while (p[j0] != 0);
do {
int j1 = way[j0];
p[j0] = p[j1];
j0 = j1;
} while (j0);
}
std::vector<int> assignment(n, -1);
for (int j = 1; j <= m; j++) {
if (p[j] != 0) {
assignment[p[j] - 1] = j - 1;
}
}
return assignment;
}
private:
int n, m;
std::vector<std::vector<float>> cost;
std::vector<float> u, v;
std::vector<int> p, way;
};

22
src/target_tracking/launch/target_tracking_launch.py
View File

@@ -5,8 +5,8 @@ import math
################### user configure parameters for ros2 start ###################
# Topics/Frames
frame_id = 'velodyne'
topic_preprocessing_in = 'velodyne_points'
topic_preprocessing_out = 'filtered_points'
topic_preprocessing_in = 'filtered_points'
topic_preprocessing_out = 'new_filtered'
# Preprocessing
x_min = 0.0
@@ -17,10 +17,16 @@ tan_h_fov = math.pi / 4 # ±45°
tan_v_fov = math.pi / 6 # ±30°
# Clustering
z_dim_scale = 0.2
cluster_tolerance = 0.2
z_dim_scale = 0.1
cluster_tolerance = 0.3
min_cluster_size = 10
max_cluster_size = 1000
min_width = 0.0
min_height = 0.0
min_length = 0.0
max_width = 1.5
max_height = 2.5
max_length = 1.5
################### user configure parameters for ros2 end #####################
@@ -41,7 +47,13 @@ cloud_clustering_params = [
{"z_dim_scale": z_dim_scale},
{"cluster_tolerance": cluster_tolerance},
{"min_cluster_size": min_cluster_size},
{"max_cluster_size": max_cluster_size}
{"max_cluster_size": max_cluster_size},
{"min_width": min_width},
{"min_height": min_height},
{"min_length": min_length},
{"max_width": max_width},
{"max_height": max_height},
{"max_length": max_length}
]
def generate_launch_description():

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

@@ -7,8 +7,9 @@
#include <algorithm>
#include <fstream>
// Headerfile
// Headerfiles of project
#include "cloud_clustering.hpp"
#include <hungarian.hpp>
// Ros2 import
#include <rclcpp/rclcpp.hpp>
@@ -63,6 +64,12 @@ namespace cloud_clustering
this->declare_parameter<float>("cluster_tolerance", 0.0f);
this->declare_parameter<int>("min_cluster_size", 0);
this->declare_parameter<int>("max_cluster_size", 0);
this->declare_parameter<float>("min_width", 0.0f);
this->declare_parameter<float>("min_height", 0.0f);
this->declare_parameter<float>("min_length", 0.0f);
this->declare_parameter<float>("max_width", 0.0f);
this->declare_parameter<float>("max_height", 0.0f);
this->declare_parameter<float>("max_length", 0.0f);
this->get_parameter("topic_in", topic_in);
this->get_parameter("frame_id", frame_id);
@@ -70,6 +77,12 @@ namespace cloud_clustering
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);
this->get_parameter("min_width", min_width);
this->get_parameter("min_height", min_height);
this->get_parameter("min_length", min_length);
this->get_parameter("max_width", max_width);
this->get_parameter("max_height", max_height);
this->get_parameter("max_length", max_length);
// Initialize publishers
objID_pub = this->create_publisher<std_msgs::msg::Int32MultiArray>("object_ids", 10);
@@ -218,6 +231,14 @@ namespace cloud_clustering
// cout << "filterN=" << filterN << "\n";
}
Hungarian hungarian(distMat);
std::vector<int> assignment = hungarian.solve();
for (int i = 0; i < assignment.size(); i++) {
objID[i] = assignment[i];
}
/*
for (int clusterCount = 0; clusterCount < 6; clusterCount++)
{
// 1. Find min(distMax)==> (i,j);
@@ -236,6 +257,8 @@ namespace cloud_clustering
// 4. if(counter<6) got to 1.
// cout << "clusterCount=" << clusterCount << "\n";
}
*/
// cout<<"Got object IDs"<<"\n";
// countIDs(objID);// for verif/corner cases
@@ -384,12 +407,19 @@ namespace cloud_clustering
int numPts = 0;
float intensity_sum = 0.0;
float min_x = std::numeric_limits<float>::max();
float min_y = std::numeric_limits<float>::max();
float min_z = std::numeric_limits<float>::max();
float max_x = -std::numeric_limits<float>::max();
float max_y = -std::numeric_limits<float>::max();
float max_z = -std::numeric_limits<float>::max();
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;
y += input_cloud->points[*pit].y;
auto &pt = input_cloud->points[*pit];
cloud_cluster->points.push_back(pt);
x += pt.x;
y += pt.y;
numPts++;
intensity_sum += input_cloud->points[*pit].intensity;
@@ -397,11 +427,33 @@ namespace cloud_clustering
// origin);
// mindist_this_cluster = std::min(dist_this_point,
// mindist_this_cluster);
min_x = std::min(min_x, pt.x);
min_y = std::min(min_y, pt.y);
min_z = std::min(min_z, pt.z);
max_x = std::max(max_x, pt.x);
max_y = std::max(max_y, pt.y);
max_z = std::max(max_z, pt.z);
}
if (numPts == 0)
continue;
float width = max_x - min_x;
float length = max_y - min_y;
float height = max_z - min_z;
// Reject clusters outside size limits
if (width < min_width || width > max_width ||
height < min_height || height > max_height ||
length < min_length || length > max_length)
{
continue;
}
pcl::PointXYZI centroid;
centroid.x = x / numPts;
centroid.y = y / numPts;
@@ -572,47 +624,61 @@ namespace cloud_clustering
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++)
{
std::vector<std::pair<int, float>> kf_intensity;
for (int i = 0; i < objID.size(); i++) {
int cluster_index = objID[i];
float mean_intensity = cluster_vec[cluster_index].second;
kf_intensity.push_back(std::make_pair(i, mean_intensity));
}
std::sort(kf_intensity.begin(), kf_intensity.end(),
[](const auto &a, const auto &b) {
return a.second > b.second;
});
for (auto &[kf_idx, intensity] : kf_intensity) {
int cluster_index = objID[kf_idx];
auto &cluster = cluster_vec[cluster_index].first;
switch (i)
{
case 0:
{
publish_cloud(pub_cluster0, cluster_vec[*it].first);
publish_cloud(pub_cluster0, cluster_vec[cluster_index].first);
i++;
break;
}
case 1:
{
publish_cloud(pub_cluster1, cluster_vec[*it].first);
publish_cloud(pub_cluster1, cluster_vec[cluster_index].first);
i++;
break;
}
case 2:
{
publish_cloud(pub_cluster2, cluster_vec[*it].first);
publish_cloud(pub_cluster2, cluster_vec[cluster_index].first);
i++;
break;
}
case 3:
{
publish_cloud(pub_cluster3, cluster_vec[*it].first);
publish_cloud(pub_cluster3, cluster_vec[cluster_index].first);
i++;
break;
}
case 4:
{
publish_cloud(pub_cluster4, cluster_vec[*it].first);
publish_cloud(pub_cluster4, cluster_vec[cluster_index].first);
i++;
break;
}
case 5:
{
publish_cloud(pub_cluster5, cluster_vec[*it].first);
publish_cloud(pub_cluster5, cluster_vec[cluster_index].first);
i++;
break;
}

46
src/target_tracking/src/cloud_preprocessing.cpp
View File

@@ -7,6 +7,7 @@
#include <cmath>
#include <random>
#include <chrono>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
@@ -33,6 +34,14 @@ class CloudFilterNode : public rclcpp::Node
float tan_h_fov;
float tan_v_fov;
//Preallocate pointclouds
pcl::PointCloud<pcl::PointXYZI>::Ptr input {new pcl::PointCloud<pcl::PointXYZI>};
pcl::PointCloud<pcl::PointXYZI>::Ptr fov_filtered {new pcl::PointCloud<pcl::PointXYZI>};
pcl::PointCloud<pcl::PointXYZI>::Ptr voxel_filtered {new pcl::PointCloud<pcl::PointXYZI>};
//Preallocate filter
pcl::VoxelGrid<pcl::PointXYZI> voxel_filter;
public:
CloudFilterNode()
: Node("cloud_preprocessing"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_)
@@ -60,6 +69,8 @@ public:
this->get_parameter("tan_h_fov", tan_h_fov);
this->get_parameter("tan_v_fov", tan_v_fov);
voxel_filter.setLeafSize(0.03f, 0.03f, 0.03f); // Adjust as needed
RCLCPP_INFO(this->get_logger(), "Starting filter node with parameters:\n"
"topic_in: %s\n"
"topic_out: %s\n"
@@ -82,31 +93,32 @@ public:
private:
void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
//auto start = std::chrono::high_resolution_clock::now();
//Clear all pointclouds
input->clear();
fov_filtered->clear();
voxel_filtered->clear();
// Convert to PCL
pcl::PointCloud<pcl::PointXYZI>::Ptr input(new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(*msg, *input);
pcl::PointCloud<pcl::PointXYZI>::Ptr fov_filtered(new pcl::PointCloud<pcl::PointXYZI>);
fov_filtered->reserve(input->size());
for (const auto& point : input->points)
{
if (point.x < x_min || point.x > x_max) continue;
if (isnan(point.x) || isnan(point.y) || isnan(point.z)) continue;
if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) continue;
if (point.z < z_min || point.z > z_max) continue;
float inv_x = 1.0f / point.x;
if (std::abs(point.y * inv_x) > tan_h_fov) continue;
if (std::abs(point.z * inv_x) > tan_v_fov) continue;
if (point.z < z_min || point.z > z_max) continue;
fov_filtered->emplace_back(point);
if (fabsf(point.y * inv_x) > tan_h_fov) continue;
if (fabsf(point.z * inv_x) > tan_v_fov) continue;
fov_filtered->push_back(point);
}
// Apply voxel grid filter
pcl::VoxelGrid<pcl::PointXYZI> voxel_filter;
voxel_filter.setInputCloud(fov_filtered);
voxel_filter.setLeafSize(0.03f, 0.03f, 0.03f); // Adjust as needed
pcl::PointCloud<pcl::PointXYZI>::Ptr voxel_filtered(new pcl::PointCloud<pcl::PointXYZI>);
voxel_filter.filter(*voxel_filtered);
//Apply ransac
@@ -119,7 +131,8 @@ private:
// Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);
seg.setDistanceThreshold(0.02);
seg.setMaxIterations(200);
seg.setInputCloud(voxel_filtered);
seg.segment (*inliers, *coefficients);
@@ -144,7 +157,14 @@ private:
out_msg.header.frame_id = msg->header.frame_id;
filtered_publisher_->publish(out_msg);
RCLCPP_INFO(this->get_logger(), "Filtered %zu -> %zu points", input->points.size(), voxel_filtered->points.size());
//RCLCPP_INFO(this->get_logger(), "Filtered %zu -> %zu points", input->points.size(), voxel_filtered->points.size());
//auto stop = std::chrono::high_resolution_clock::now();
//auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
// To get the value of duration use the count()
// member function on the duration object
//std::cout << duration.count() << std::endl;
}
tf2_ros::Buffer tf_buffer_;