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

src/target_tracking/include/cloud_clustering.hpp

@@ -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);

src/target_tracking/include/hungarian.hpp

@@ -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;
+};

src/target_tracking/launch/target_tracking_launch.py

@@ -5,8 +5,8 @@ import math
 ################### user configure parameters for ros2 start ###################
 # Topics/Frames
 frame_id        = 'velodyne'
-topic_preprocessing_in  = 'velodyne_points'
+topic_preprocessing_in  = 'filtered_points'
-topic_preprocessing_out = '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
+z_dim_scale     = 0.1
-cluster_tolerance   = 0.2
+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():

src/target_tracking/src/cloud_clustering.cpp

@@ -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++)
       {
-
+        auto &pt = input_cloud->points[*pit];
-        cloud_cluster->points.push_back(input_cloud->points[*pit]);
+        cloud_cluster->points.push_back(pt);
-        x += input_cloud->points[*pit].x;
+        x += pt.x;
-        y += input_cloud->points[*pit].y;
+        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)
+      //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++)
+      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;
         }

src/target_tracking/src/cloud_preprocessing.cpp

@@ -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 (fabsf(point.y * inv_x) > tan_h_fov) continue;
-      if (std::abs(point.z * inv_x) > tan_v_fov) continue;
+      if (fabsf(point.z * inv_x) > tan_v_fov) continue;
-      if (point.z < z_min || point.z > z_max) continue;
+
-      fov_filtered->emplace_back(point);
+      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_;