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

.gitmodules

@@ -14,3 +14,7 @@
 	path = src/FAST_LIO
 	url = https://github.com/Ericsii/FAST_LIO.git
 	branch = ros2
+[submodule "src/livox_ros2_driver"]
+	path = src/livox_ros2_driver
+	url = https://github.com/Livox-SDK/livox_ros2_driver
+	branch = fix_build_error

scripts/calibrate_lidar.py

@@ -0,0 +1,231 @@
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+from sensor_msgs.msg import PointCloud2
+from geometry_msgs.msg import TransformStamped
+import sensor_msgs_py.point_cloud2 as pc2
+from tf2_ros import StaticTransformBroadcaster
+import numpy as np
+import matplotlib.pyplot as plt
+import struct
+from io import BytesIO
+import threading
+import time
+import open3d as o3d
+from scipy.spatial.transform import Rotation as R
+
+class PointCloudSaver(Node):
+    def __init__(self, node_name: str, pointcloud_topic: str, buffer, timeout_ms: int):
+        super().__init__(node_name)
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            pointcloud_topic,
+            self.callback,
+            10
+        )
+        self.buffer = buffer
+        self.finished = False
+        self.points = []
+        self.end_time = self.get_clock().now().nanoseconds + (timeout_ms * 1_000_000)
+        self.cmap = plt.get_cmap('jet') 
+
+    def callback(self, msg):
+        now = self.get_clock().now().nanoseconds
+        for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
+            self.points.append([p[0], p[1], p[2], p[3]])
+
+        if now > self.end_time:
+            if not self.points:
+                self.get_logger().warn("No points received!")
+                self.destroy_node()
+                self.finished = True
+                return
+
+            np_points = np.array(self.points, dtype=np.float32)
+            intensities = np_points[:, 3]
+            norm_int = (intensities - intensities.min()) / (intensities.ptp() + 1e-8)
+
+            # Map normalized intensity to RGB colormap
+            colors = self.cmap(norm_int)[:, :3]  # RGB 0-1
+            colors = (colors * 255).astype(np.uint8)
+            rgb_int = np.left_shift(colors[:,0].astype(np.uint32), 16) | \
+                      np.left_shift(colors[:,1].astype(np.uint32), 8)  | \
+                      colors[:,2].astype(np.uint32)
+
+            filename = "pointcloud.pcd"
+            self.write_pcd_with_intensity_rgb(filename, np_points, rgb_int)
+            self.get_logger().info(f"Saved {filename}")
+            self.destroy_node()
+            self.finished = True
+
+    def write_pcd_with_intensity_rgb(self, filename, points, rgb_int):
+        header = f"""# .PCD v0.7 - Point Cloud Data file format
+VERSION 0.7
+FIELDS x y z intensity rgb
+SIZE 4 4 4 4 4
+TYPE F F F F U
+COUNT 1 1 1 1 1
+WIDTH {points.shape[0]}
+HEIGHT 1
+VIEWPOINT 0 0 0 1 0 0 0
+POINTS {points.shape[0]}
+DATA binary
+"""
+        self.buffer.write(header.encode('ascii'))
+        for i in range(points.shape[0]):
+            self.buffer.write(struct.pack('ffffI', points[i,0], points[i,1], points[i,2], points[i,3], rgb_int[i]))
+
+
+class LidarTransformPublisher(Node):
+    def __init__(self, lidar1_buffer, lidar1_frame, lidar2_buffer, lidar2_frame):
+        super().__init__('static_transform_lidar_offsets')
+        self.br = StaticTransformBroadcaster(self)
+        self.lidar1_buffer = lidar1_buffer
+        self.lidar2_buffer = lidar2_buffer
+        self.lidar1_frame = lidar1_frame
+        self.lidar2_frame = lidar2_frame
+
+    def publish(self):
+        self.pcd_1 = self.pcd_buffer_to_o3d(self.lidar1_buffer)
+        self.pcd_2 = self.pcd_buffer_to_o3d(self.lidar2_buffer)
+
+        self.T = self.compute_transform()
+        self.get_logger().info(f"Computed initial transform:\n{self.T}")
+
+        T_copy = np.array(self.T, copy=True)
+        trans = T_copy[:3, 3]
+        rot_quat = R.from_matrix(T_copy[:3, :3]).as_quat()
+
+        t = TransformStamped()
+        t.header.stamp.sec = 0
+        t.header.stamp.nanosec = 0
+        t.header.frame_id = self.lidar1_frame
+        t.child_frame_id = self.lidar2_frame
+        t.transform.translation.x = trans[0]
+        t.transform.translation.y = trans[1]
+        t.transform.translation.z = trans[2]
+        t.transform.rotation.x = rot_quat[0]
+        t.transform.rotation.y = rot_quat[1]
+        t.transform.rotation.z = rot_quat[2]
+        t.transform.rotation.w = rot_quat[3]
+
+        self.br.sendTransform(t)
+        self.get_logger().info("Published static transform.")
+
+    def pcd_buffer_to_o3d(self, buffer: BytesIO):
+        buffer.seek(0)
+        header_lines = []
+        while True:
+            line = buffer.readline().decode('ascii').strip()
+            if not line:
+                continue
+            header_lines.append(line)
+            if line.startswith("DATA"):
+                data_line = line
+                break
+
+        if not data_line.lower().startswith("data binary"):
+            raise NotImplementedError("Only binary PCD supported")
+
+        num_points = 0
+        for line in header_lines:
+            if line.startswith("POINTS"):
+                num_points = int(line.split()[1])
+        if num_points == 0:
+            raise ValueError("PCD header missing point count")
+
+        dtype = np.dtype([
+            ('x', 'f4'), ('y', 'f4'), ('z', 'f4'),
+            ('intensity', 'f4'), ('rgb', 'u4')
+        ])
+
+        data = buffer.read(num_points * dtype.itemsize)
+        points_array = np.frombuffer(data, dtype=dtype, count=num_points)
+
+        pcd = o3d.geometry.PointCloud()
+        xyz = np.stack([points_array['x'], points_array['y'], points_array['z']], axis=-1)
+        pcd.points = o3d.utility.Vector3dVector(xyz)
+        return pcd
+
+    def compute_transform(self):
+        voxel_size = 0.2  # coarse-to-fine pyramid base
+
+        # --- Feature extraction ---
+        def preprocess(pcd, voxel):
+            pcd_down = pcd.voxel_down_sample(voxel)
+            pcd_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=voxel*2, max_nn=30))
+            fpfh = o3d.pipelines.registration.compute_fpfh_feature(
+                pcd_down, o3d.geometry.KDTreeSearchParamHybrid(radius=voxel*5, max_nn=100))
+            return pcd_down, fpfh
+
+        src_down, src_fpfh = preprocess(self.pcd_2, voxel_size)
+        tgt_down, tgt_fpfh = preprocess(self.pcd_1, voxel_size)
+
+        # --- Global alignment with RANSAC ---
+        result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
+            src_down, tgt_down, src_fpfh, tgt_fpfh, True,
+            1.5,  # distance threshold
+            o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
+            4,
+            [
+                o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
+                o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(1.5)
+            ],
+            o3d.pipelines.registration.RANSACConvergenceCriteria(4000000, 500)
+        )
+
+        trans_init = result_ransac.transformation
+
+        # --- Multi-scale ICP refinement ---
+        voxel_radii = [0.2, 0.1, 0.05]
+        max_iters = [50, 30, 14]
+        transformation = trans_init
+        for radius, iters in zip(voxel_radii, max_iters):
+            src_down = self.pcd_2.voxel_down_sample(radius)
+            tgt_down = self.pcd_1.voxel_down_sample(radius)
+            src_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius*2, max_nn=30))
+            tgt_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius*2, max_nn=30))
+
+            result_icp = o3d.pipelines.registration.registration_icp(
+                src_down, tgt_down, radius*2, transformation,
+                o3d.pipelines.registration.TransformationEstimationPointToPlane()
+            )
+            transformation = result_icp.transformation
+
+        return transformation
+
+
+def monitor_nodes(nodes, publisher):
+    while rclpy.ok():
+        if all(node.finished for node in nodes):
+            print("All pointclouds captured")
+            publisher.publish()
+            return
+        time.sleep(0.1)
+
+def main():
+    rclpy.init()
+    buffer_velodyne = BytesIO()
+    buffer_livox = BytesIO()
+
+    executor = MultiThreadedExecutor()
+    nodes = [
+        PointCloudSaver('velodyne_pcd_saver', '/velodyne_points', buffer_velodyne, 350),
+        PointCloudSaver('livox_pcd_saver', '/livox/lidar', buffer_livox, 1000),
+    ]
+    publisher = LidarTransformPublisher(buffer_velodyne, 'velodyne', buffer_livox, 'frame_default')
+
+    monitor_thread = threading.Thread(target=monitor_nodes, args=(nodes,publisher), daemon=True)
+    monitor_thread.start()
+
+    for node in nodes:
+        executor.add_node(node)
+    try:
+        executor.spin()
+    finally:
+        monitor_thread.join()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

scripts/publish_lidar_offset.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from tf2_ros import StaticTransformBroadcaster
+from geometry_msgs.msg import TransformStamped
+import numpy as np
+import open3d as o3d
+from scipy.spatial.transform import Rotation as R
+
+class StaticTransformPublisher(Node):
+    def __init__(self):
+        super().__init__('static_transform_from_pointclouds')
+
+        # Static TF broadcaster
+        self.br = StaticTransformBroadcaster(self)
+
+        # Pointcloud files
+        self.velodyne_file = "/root/velodyne.pcd"
+        self.livox_file = "/root/livox.pcd"
+
+        # Frames
+        self.livox_frame = "frame_default"
+        self.velodyne_frame = "velodyne"
+
+        # Compute transform once
+        self.T = self.compute_transform()
+        self.get_logger().info(f"Computed initial transform:\n{self.T}")
+
+        # Prepare translation and rotation
+        T_copy = np.array(self.T, copy=True)
+        trans = T_copy[:3, 3]
+        rot_quat = R.from_matrix(T_copy[:3, :3]).as_quat()  # [x, y, z, w]
+
+        # Create static TransformStamped
+        t = TransformStamped()
+        t.header.stamp.sec = 0
+        t.header.stamp.nanosec = 0
+        t.header.frame_id = self.velodyne_frame
+        t.child_frame_id = self.livox_frame
+        t.transform.translation.x = trans[0]
+        t.transform.translation.y = trans[1]
+        t.transform.translation.z = trans[2]
+        t.transform.rotation.x = rot_quat[0]
+        t.transform.rotation.y = rot_quat[1]
+        t.transform.rotation.z = rot_quat[2]
+        t.transform.rotation.w = rot_quat[3]
+
+        # Publish once
+        self.br.sendTransform(t)
+        self.get_logger().info("Published static transform.")
+
+    def compute_transform(self):
+        # Load point clouds
+        pcd_vel = o3d.io.read_point_cloud(self.velodyne_file)
+        pcd_liv = o3d.io.read_point_cloud(self.livox_file)
+
+        # Downsample
+        voxel_size = 0.05
+        pcd_vel_ds = pcd_vel.voxel_down_sample(voxel_size)
+        pcd_liv_ds = pcd_liv.voxel_down_sample(voxel_size)
+
+        # Estimate normals
+        pcd_vel_ds.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamKNN(knn=20))
+        pcd_liv_ds.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamKNN(knn=20))
+
+        # ICP registration
+        threshold = 0.5
+        reg_result = o3d.pipelines.registration.registration_icp(
+            pcd_liv_ds, pcd_vel_ds, threshold,
+            np.eye(4),
+            o3d.pipelines.registration.TransformationEstimationPointToPoint()
+        )
+        return reg_result.transformation
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = StaticTransformPublisher()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

scripts/save_pointcloud.py

@@ -1,11 +1,14 @@
 import rclpy
 from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
 from sensor_msgs.msg import PointCloud2
 import sensor_msgs_py.point_cloud2 as pc2
 import numpy as np
 import matplotlib.pyplot as plt
 import struct
 from io import BytesIO
+import threading
+import time
 
 class PointCloudSaver(Node):
     def __init__(self, node_name: str, pointcloud_topic: str, buffer, timeout_ms: int):
@@ -17,6 +20,7 @@ class PointCloudSaver(Node):
             10
         )
         self.buffer = buffer
+        self.finished = False
         self.points = []
         self.end_time = self.get_clock().now().nanoseconds + (timeout_ms * 1_000_000)
         self.cmap = plt.get_cmap('jet') 
@@ -29,7 +33,8 @@ class PointCloudSaver(Node):
         if now > self.end_time:
             if not self.points:
                 self.get_logger().warn("No points received!")
-                rclpy.shutdown()
+                self.destroy_node()
+                self.finished = True
                 return
 
             np_points = np.array(self.points, dtype=np.float32)
@@ -45,8 +50,9 @@ class PointCloudSaver(Node):
 
             filename = "pointcloud.pcd"
             self.write_pcd_with_intensity_rgb(filename, np_points, rgb_int)
-            self.get_logger().info(f"Saved {filename} with intensity (grayscale) and colored RGB")
+            self.get_logger().info(f"Saved {filename}")
-            rclpy.shutdown()
+            self.destroy_node()
+            self.finished = True
 
     def write_pcd_with_intensity_rgb(self, filename, points, rgb_int):
         header = f"""# .PCD v0.7 - Point Cloud Data file format
@@ -64,13 +70,43 @@ DATA binary
         self.buffer.write(header.encode('ascii'))
         for i in range(points.shape[0]):
             # x, y, z, intensity as float32, rgb as uint32
-            f.write(struct.pack('ffffI', points[i,0], points[i,1], points[i,2], points[i,3], rgb_int[i]))
+            self.buffer.write(struct.pack('ffffI', points[i,0], points[i,1], points[i,2], points[i,3], rgb_int[i]))
+
+def monitor_nodes(nodes):
+    """Separate thread that monitors node status and shuts down ROS when done."""
+    while rclpy.ok():
+        if all(node.finished for node in nodes):
+            print("All nodes finished. Shutting down ROS.")
+            rclpy.shutdown()
+            break
+        time.sleep(0.1)  # check periodically
 
 def main():
     rclpy.init()
-    with open('/root/velodyne.pcd', "w+") as f:
+    file_velodyne = open('/root/velodyne.pcd', "wb+")
-        node = PointCloudSaver('velodyne_pcd_saver', '/velodyne_points', f, 5000)
+    file_livox = open('/root/livox.pcd', "wb+")
-    rclpy.spin(node)
+
+    executor = MultiThreadedExecutor()
+
+    nodes = [
+        PointCloudSaver('velodyne_pcd_saver', '/velodyne_points', file_velodyne, 5000),
+        PointCloudSaver('livox_pcd_saver', '/livox/lidar', file_livox, 5000),
+    ]
+
+    monitor_thread = threading.Thread(target=monitor_nodes, args=(nodes,), daemon=True)
+    monitor_thread.start()
+
+    for node in nodes:
+        executor.add_node(node)
+    try:
+        executor.spin()
+    finally:
+        monitor_thread.join()
+        print("Executor and monitor thread exited cleanly.")
+
+        file_velodyne.close()
+        file_livox.close()
+
 
 if __name__ == "__main__":
     main()

src/target_tracking/CMakeLists.txt

@@ -0,0 +1,85 @@
+cmake_minimum_required(VERSION 3.8)
+project(target_tracking)
+
+# Required packages
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(PCL REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_sensor_msgs REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(visualization_msgs REQUIRED)
+
+
+# Include paths
+include_directories(
+  include
+  ${PCL_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+# Node for preprocessing
+add_executable(cloud_preprocessing_node 
+      src/cloud_preprocessing.cpp
+)
+
+# Dependencies for preprocessing
+ament_target_dependencies(cloud_preprocessing_node
+  rclcpp
+  sensor_msgs
+  pcl_conversions
+  tf2
+  tf2_ros
+  tf2_sensor_msgs
+)
+
+# Node for clustering
+add_executable(cloud_clustering_node
+      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
+  rclcpp
+  sensor_msgs
+  visualization_msgs 
+  PCL
+  pcl_conversions
+  OpenCV
+)
+
+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
+  cloud_preprocessing_node  
+  cloud_clustering_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(TARGETS
+    cloud_clustering
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION lib/${PACKAGE_NAME}
+)
+
+# Install launchfile
+install(
+  DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
+ament_package()

src/target_tracking/README.md

@@ -0,0 +1,3 @@
+# Target tracking
+
+Ros2 implementation of a filtering node in combination with a clustering node to find a target with lidar data

src/target_tracking/include/cloud_clustering.hpp

@@ -0,0 +1,106 @@
+#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;
+    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);
+    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

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

@@ -0,0 +1,81 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import math
+
+################### user configure parameters for ros2 start ###################
+# Topics/Frames
+frame_id        = 'velodyne'
+topic_preprocessing_in  = 'filtered_points'
+topic_preprocessing_out = 'new_filtered'
+
+# Preprocessing
+x_min           = 0.0
+x_max           = 20.0
+z_min           = -0.5
+z_max           = 1.5
+tan_h_fov       = math.pi / 4 # ±45°
+tan_v_fov       = math.pi / 6 # ±30°
+
+# Clustering
+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 ##################### 
+
+cloud_preprocessing_params = [
+    {"topic_in": topic_preprocessing_in},
+    {"topic_out": topic_preprocessing_out},
+    {"x_min": x_min},
+    {"x_max": x_max},
+    {"z_min": z_min},
+    {"z_max": z_max},
+    {"tan_h_fov": tan_h_fov},
+    {"tan_v_fov": tan_v_fov}  
+]
+
+cloud_clustering_params = [
+    {"topic_in": topic_preprocessing_out},
+    {"frame_id": frame_id},
+    {"z_dim_scale": z_dim_scale},
+    {"cluster_tolerance": cluster_tolerance},
+    {"min_cluster_size": min_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():
+    return LaunchDescription([
+        Node(
+            package='target_tracking',
+            executable='cloud_preprocessing_node',
+            name='cloud_preprocessing',
+            parameters=cloud_preprocessing_params,
+            output={
+                'stdout': 'screen',
+                'stderr': 'screen',
+            }
+        ),
+         Node(
+            package='target_tracking',
+            executable='cloud_clustering_node',
+            name='cloud_clustering',
+            parameters=cloud_clustering_params,
+            output={
+                'stdout': 'screen',
+                'stderr': 'screen',
+            }
+        )
+    ])

src/target_tracking/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>target_tracking</name>
+  <version>1.0.1</version>
+  <description>Studienprojekt</description>
+  <maintainer email="liam20030625@gmail.com">Liam</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <!-- Dependencies for voxel -->
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>PCL</depend>
+  <depend>cv_bridge</depend>
+  <depend>OpenCV</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/target_tracking/src/cloud_clustering.cpp

@@ -0,0 +1,691 @@
+// Default C++ imports
+#include <limits>
+#include <utility>
+#include <string.h>
+#include <iostream>
+#include <iterator>
+#include <algorithm>
+#include <fstream>
+
+// Headerfiles of project
+#include "cloud_clustering.hpp"
+#include <hungarian.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 <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/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>
+
+namespace cloud_clustering
+{
+  static const rclcpp::Logger LOGGER = rclcpp::get_logger("cloud_clustering");
+
+  CloudClustering::CloudClustering(
+      const rclcpp::NodeOptions &options) : CloudClustering("", options)
+  {
+  }
+
+  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");
+
+    // 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);
+    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);
+    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);
+    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);
+    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);
+
+    RCLCPP_INFO(this->get_logger(), "About to setup callback");
+    clock_ = this->get_clock();
+
+    // 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));
+
+    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 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);
+  }
+  /*
+  //Count unique object IDs. just to make sure same ID has not been assigned to
+  two KF_Trackers. int countIDs(vector<int> v)
+  {
+      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
+  n), worst case O(n^2) (usually implemented as quicksort.
+      // To guarantee worst case O(n log n) replace with make_heap, then
+  sort_heap.
+
+      // 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
+  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
+  access iterators (like vector's)
+  }
+  */
+
+  /*
+
+  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> 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)
+        {
+          minEl = distMat[i][j];
+          minIndex = std::make_pair(i, j);
+        }
+      }
+    return minIndex;
+  }
+
+  void CloudClustering::KFT(const std_msgs::msg::Float32MultiArray ccs)
+  {
+
+    // First predict, to update the internal statePre variable
+
+    std::vector<cv::Mat> pred{KF0.predict(), KF1.predict(), KF2.predict(),
+                              KF3.predict(), KF4.predict(), KF5.predict()};
+
+    // cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
+    // cout<<"Prediction 1
+    // ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n";
+
+    // Get measurements
+    // Extract the position of the clusters forom the multiArray. To check if the
+    // data coming in, check the .z (every third) coordinate and that will be 0.0
+    std::vector<geometry_msgs::msg::Point> clusterCenters; // clusterCenters
+
+    int i = 0;
+    for (std::vector<float>::const_iterator it = ccs.data.begin();
+         it != ccs.data.end(); it += 3)
+    {
+      geometry_msgs::msg::Point pt;
+      pt.x = *it;
+      pt.y = *(it + 1);
+      pt.z = *(it + 2);
+
+      clusterCenters.push_back(pt);
+    }
+
+    std::vector<geometry_msgs::msg::Point> KFpredictions;
+    i = 0;
+    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);
+      pt.z = (*it).at<float>(2);
+
+      KFpredictions.push_back(pt);
+    }
+
+    // Find the cluster that is more probable to be belonging to a given KF.
+    objID.clear();   // Clear the objID vector
+    objID.resize(6); // Allocate default elements so that [i] doesnt segfault.
+                     // Should be done better
+    // Copy clusterCentres for modifying it and preventing multiple assignments of
+    // the same ID
+    std::vector<geometry_msgs::msg::Point> copyOfClusterCenters(clusterCenters);
+    std::vector<std::vector<float>> distMat;
+
+    for (int filterN = 0; filterN < 6; filterN++)
+    {
+      std::vector<float> distVec;
+      for (int n = 0; n < 6; n++)
+      {
+        distVec.push_back(
+            euclidean_distance(KFpredictions[filterN], copyOfClusterCenters[n]));
+      }
+
+      distMat.push_back(distVec);
+      /*// Based on distVec instead of distMat (global min). Has problems with the
+      person's leg going out of scope int
+      ID=std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end()));
+       //cout<<"finterlN="<<filterN<<"   minID="<<ID
+       objID.push_back(ID);
+      // Prevent assignment of the same object ID to multiple clusters
+       copyOfClusterCenters[ID].x=100000;// A large value so that this center is
+      not assigned to another cluster copyOfClusterCenters[ID].y=10000;
+       copyOfClusterCenters[ID].z=10000;
+      */
+      // 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);
+      std::pair<int, int> minIndex(findIndexOfMin(distMat));
+      // 2. objID[i]=clusterCenters[j]; counter++
+      objID[minIndex.first] = minIndex.second;
+
+      // 3. distMat[i,:]=10000; distMat[:,j]=10000
+      distMat[minIndex.first] =
+          std::vector<float>(6, 10000.0); // Set the row to a high number.
+      for (int row = 0; row < distMat.size();
+           row++) // set the column to a high number
+      {
+        distMat[row][minIndex.second] = 10000.0;
+      }
+      // 4. if(counter<6) got to 1.
+      // cout << "clusterCount=" << clusterCount << "\n";
+    }
+    */
+    
+
+    // cout<<"Got object IDs"<<"\n";
+    // countIDs(objID);// for verif/corner cases
+
+    // display objIDs
+    /* DEBUG
+      cout<<"objID= ";
+      for(auto it=objID.begin();it!=objID.end();it++)
+          cout<<*it<<" ,";
+      cout<<"\n";
+      */
+
+    visualization_msgs::msg::MarkerArray clusterMarkers;
+
+    for (int i = 0; i < 6; i++)
+    {
+      visualization_msgs::msg::Marker m;
+
+      m.id = i;
+      m.type = visualization_msgs::msg::Marker::CUBE;
+      m.header.frame_id = frame_id;
+      m.scale.x = 0.3;
+      m.scale.y = 0.3;
+      m.scale.z = 0.3;
+      m.action = visualization_msgs::msg::Marker::ADD;
+      m.color.a = 1.0;
+      m.color.r = i % 2 ? 1 : 0;
+      m.color.g = i % 3 ? 1 : 0;
+      m.color.b = i % 4 ? 1 : 0;
+
+      // geometry_msgs::msg::Point clusterC(clusterCenters.at(objID[i]));
+      geometry_msgs::msg::Point clusterC(KFpredictions[i]);
+      m.pose.position.x = clusterC.x;
+      m.pose.position.y = clusterC.y;
+      m.pose.position.z = clusterC.z;
+
+      clusterMarkers.markers.push_back(m);
+    }
+
+    prevClusterCenters = clusterCenters;
+
+    markerPub->publish(clusterMarkers);
+
+    std_msgs::msg::Int32MultiArray obj_id;
+    for (auto it = objID.begin(); it != objID.end(); it++)
+      obj_id.data.push_back(*it);
+    // Publish the object IDs
+    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++)
+    {
+      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);
+
+      cc.push_back(pt);
+    }
+    // cout<<"cc[5][0]="<<cc[5].at(0)<<"cc[5][1]="<<cc[5].at(1)<<"cc[5][2]="<<cc[5].at(2)<<"\n";
+    float meas0[2] = {cc[0].at(0), cc[0].at(1)};
+    float meas1[2] = {cc[1].at(0), cc[1].at(1)};
+    float meas2[2] = {cc[2].at(0), cc[2].at(1)};
+    float meas3[2] = {cc[3].at(0), cc[3].at(1)};
+    float meas4[2] = {cc[4].at(0), cc[4].at(1)};
+    float meas5[2] = {cc[5].at(0), cc[5].at(1)};
+
+    // The update phase
+    cv::Mat meas0Mat = cv::Mat(2, 1, CV_32F, meas0);
+    cv::Mat meas1Mat = cv::Mat(2, 1, CV_32F, meas1);
+    cv::Mat meas2Mat = cv::Mat(2, 1, CV_32F, meas2);
+    cv::Mat meas3Mat = cv::Mat(2, 1, CV_32F, meas3);
+    cv::Mat meas4Mat = cv::Mat(2, 1, CV_32F, meas4);
+    cv::Mat meas5Mat = cv::Mat(2, 1, CV_32F, meas5);
+
+    // cout<<"meas0Mat"<<meas0Mat<<"\n";
+    if (!(meas0Mat.at<float>(0, 0) == 0.0f || meas0Mat.at<float>(1, 0) == 0.0f))
+      cv::Mat estimated0 = KF0.correct(meas0Mat);
+    if (!(meas1[0] == 0.0f || meas1[1] == 0.0f))
+      cv::Mat estimated1 = KF1.correct(meas1Mat);
+    if (!(meas2[0] == 0.0f || meas2[1] == 0.0f))
+      cv::Mat estimated2 = KF2.correct(meas2Mat);
+    if (!(meas3[0] == 0.0f || meas3[1] == 0.0f))
+      cv::Mat estimated3 = KF3.correct(meas3Mat);
+    if (!(meas4[0] == 0.0f || meas4[1] == 0.0f))
+      cv::Mat estimated4 = KF4.correct(meas4Mat);
+    if (!(meas5[0] == 0.0f || meas5[1] == 0.0f))
+      cv::Mat estimated5 = KF5.correct(meas5Mat);
+
+    // Publish the point clouds belonging to each clusters
+
+    // cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n";
+    // Point statePt(estimated.at<float>(0),estimated.at<float>(1));
+    // cout<<"DONE KF_TRACKER\n";
+  }
+  void CloudClustering::publish_cloud(
+      rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub,
+      pcl::PointCloud<pcl::PointXYZI>::Ptr cluster)
+  {
+    // Create a PointCloud2 message
+    auto clustermsg = std::make_shared<sensor_msgs::msg::PointCloud2>();
+
+    // Convert PCL cloud to ROS 2 message
+    pcl::toROSMsg(*cluster, *clustermsg);
+
+    // Set header info
+    clustermsg->header.frame_id = frame_id;
+    clustermsg->header.stamp = clock_->now();
+
+    // Publish the message
+    pub->publish(*clustermsg);
+  }
+
+  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(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>);
+    tree->setInputCloud(input_cloud);
+
+    std::vector<pcl::PointIndices> cluster_indices;
+    pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec;
+    ec.setClusterTolerance(cluster_tolerance);
+    ec.setMinClusterSize(min_cluster_size);
+    ec.setMaxClusterSize(max_cluster_size);
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(input_cloud);
+    /* Extract the clusters out of pc and save indices in cluster_indices.*/
+    ec.extract(cluster_indices);
+
+    std::vector<pcl::PointIndices>::const_iterator it;
+    std::vector<int>::const_iterator pit;
+
+    for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
+    {
+
+      pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_cluster(
+          new pcl::PointCloud<pcl::PointXYZI>);
+      float x = 0.0;
+      float y = 0.0;
+      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(pt);
+        x += pt.x;
+        y += pt.y;
+        numPts++;
+
+        intensity_sum += input_cloud->points[*pit].intensity;
+        // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
+        //                                          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;
+      centroid.z = 0.0;
+
+      cluster_vec.push_back(std::make_pair(cloud_cluster, intensity_sum / numPts));
+
+      // Get the centroid of the cluster
+      clusterCentroids.push_back(centroid);
+    }
+
+    // Ensure at least 6 clusters exist to publish (later clusters may be empty)
+    while (cluster_vec.size() < 6)
+    {
+      pcl::PointCloud<pcl::PointXYZI>::Ptr empty_cluster(
+          new pcl::PointCloud<pcl::PointXYZI>);
+
+      pcl::PointXYZI pt;
+      pt.x = pt.y = pt.z = pt.intensity = 0.0f;
+
+      empty_cluster->points.push_back(pt);
+
+      pcl::PointXYZI centroid;
+      centroid.x = 0.0;
+      centroid.y = 0.0;
+      centroid.z = 0.0;
+      centroid.intensity = 0.0;
+
+      cluster_vec.push_back(std::make_pair(empty_cluster, 0.0f));
+    }
+
+    while (clusterCentroids.size() < 6)
+    {
+      pcl::PointXYZI centroid;
+      centroid.x = 0.0;
+      centroid.y = 0.0;
+      centroid.z = 0.0;
+      centroid.intensity = 0.0;
+
+      clusterCentroids.push_back(centroid);
+    }
+  }
+
+  void CloudClustering::cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input)
+
+  {
+    // Vector of cluster pointclouds
+    std::vector<std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, float>> cluster_vec;
+    // Cluster centroids
+    std::vector<pcl::PointXYZI> clusterCentroids;
+
+    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
+      // is detected
+
+      float dvx = 0.01f; // 1.0
+      float dvy = 0.01f; // 1.0
+      float dx = 1.0f;
+      float dy = 1.0f;
+      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 = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
+                              dvx, 0, 0, 0, 0, dvy);
+      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 = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
+                              dvx, 0, 0, 0, 0, dvy);
+      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 = (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);
+      cv::setIdentity(KF1.measurementMatrix);
+      cv::setIdentity(KF2.measurementMatrix);
+      cv::setIdentity(KF3.measurementMatrix);
+      cv::setIdentity(KF4.measurementMatrix);
+      cv::setIdentity(KF5.measurementMatrix);
+      // Process Noise Covariance Matrix Q
+      // [ Ex 0  0    0 0    0 ]
+      // [ 0  Ey 0    0 0    0 ]
+      // [ 0  0  Ev_x 0 0    0 ]
+      // [ 0  0  0    1 Ev_y 0 ]
+      //// [ 0  0  0    0 1    Ew ]
+      //// [ 0  0  0    0 0    Eh ]
+      float sigmaP = 0.01;
+      float sigmaQ = 0.1;
+      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));
+      cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(sigmaQ));
+      cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(sigmaQ));
+      cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(sigmaQ));
+      cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ));
+
+      // Process the point cloud
+      parse_cloud(input, cluster_vec, clusterCentroids);
+
+      // Set initial state
+      KF0.statePre.at<float>(0) = clusterCentroids.at(0).x;
+      KF0.statePre.at<float>(1) = clusterCentroids.at(0).y;
+      KF0.statePre.at<float>(2) = 0; // initial v_x
+      KF0.statePre.at<float>(3) = 0; // initial v_y
+
+      // Set initial state
+      KF1.statePre.at<float>(0) = clusterCentroids.at(1).x;
+      KF1.statePre.at<float>(1) = clusterCentroids.at(1).y;
+      KF1.statePre.at<float>(2) = 0; // initial v_x
+      KF1.statePre.at<float>(3) = 0; // initial v_y
+
+      // Set initial state
+      KF2.statePre.at<float>(0) = clusterCentroids.at(2).x;
+      KF2.statePre.at<float>(1) = clusterCentroids.at(2).y;
+      KF2.statePre.at<float>(2) = 0; // initial v_x
+      KF2.statePre.at<float>(3) = 0; // initial v_y
+
+      // Set initial state
+      KF3.statePre.at<float>(0) = clusterCentroids.at(3).x;
+      KF3.statePre.at<float>(1) = clusterCentroids.at(3).y;
+      KF3.statePre.at<float>(2) = 0; // initial v_x
+      KF3.statePre.at<float>(3) = 0; // initial v_y
+
+      // Set initial state
+      KF4.statePre.at<float>(0) = clusterCentroids.at(4).x;
+      KF4.statePre.at<float>(1) = clusterCentroids.at(4).y;
+      KF4.statePre.at<float>(2) = 0; // initial v_x
+      KF4.statePre.at<float>(3) = 0; // initial v_y
+
+      // Set initial state
+      KF5.statePre.at<float>(0) = clusterCentroids.at(5).x;
+      KF5.statePre.at<float>(1) = clusterCentroids.at(5).y;
+      KF5.statePre.at<float>(2) = 0; // initial v_x
+      KF5.statePre.at<float>(3) = 0; // initial v_y
+
+      firstFrame = false;
+
+      for (int i = 0; i < 6; i++)
+      {
+        geometry_msgs::msg::Point pt;
+        pt.x = clusterCentroids.at(i).x;
+        pt.y = clusterCentroids.at(i).y;
+        prevClusterCenters.push_back(pt);
+      }
+    }
+
+    else
+    {
+      parse_cloud(input, cluster_vec, clusterCentroids);
+
+      std_msgs::msg::Float32MultiArray cc;
+      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);
+      }
+
+      // cc_pos->publish(cc);// Publish cluster mid-points.
+      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::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[cluster_index].first);
+          i++;
+          break;
+        }
+        case 1:
+        {
+          publish_cloud(pub_cluster1, cluster_vec[cluster_index].first);
+          i++;
+          break;
+        }
+        case 2:
+        {
+          publish_cloud(pub_cluster2, cluster_vec[cluster_index].first);
+          i++;
+          break;
+        }
+        case 3:
+        {
+          publish_cloud(pub_cluster3, cluster_vec[cluster_index].first);
+          i++;
+          break;
+        }
+        case 4:
+        {
+          publish_cloud(pub_cluster4, cluster_vec[cluster_index].first);
+          i++;
+          break;
+        }
+
+        case 5:
+        {
+          publish_cloud(pub_cluster5, cluster_vec[cluster_index].first);
+          i++;
+          break;
+        }
+        default:
+          break;
+        }
+      }
+    }
+  }
+}

src/target_tracking/src/cloud_clustering_node.cpp

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

src/target_tracking/src/cloud_preprocessing.cpp

@@ -0,0 +1,186 @@
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
+#include <cmath>
+
+#include <random>
+#include <chrono>
+
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/io/pcd_io.h>
+
+class CloudFilterNode : public rclcpp::Node
+{
+  //Topic parameters
+  std::string topic_in;
+  std::string topic_out;
+
+  // Filter parameters
+  float x_min;
+  float x_max;
+  float z_min;
+  float z_max;
+  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_)
+  {
+
+    //Topic parameters
+    this->declare_parameter<std::string>("topic_in", "test");
+    this->declare_parameter<std::string>("topic_out", "test2");
+
+    //Filter parameters
+    this->declare_parameter<float>("x_min", 0.0f);
+    this->declare_parameter<float>("x_max", 0.0f);
+    this->declare_parameter<float>("z_min", 0.0f);
+    this->declare_parameter<float>("z_max", 0.0f);
+    this->declare_parameter<float>("tan_h_fov", 0.0f);  // ±45°
+    this->declare_parameter<float>("tan_v_fov", 0.0f);  // ±30°
+
+    this->get_parameter("topic_in", topic_in);
+    this->get_parameter("topic_out", topic_out);
+
+    this->get_parameter("x_min", x_min);
+    this->get_parameter("x_max", x_max);
+    this->get_parameter("z_min", z_min);
+    this->get_parameter("z_max", z_max);
+    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"
+      "x_min: %f\n"
+      "x_max: %f\n"
+      "z_min: %f\n"
+      "z_max: %f\n"
+      "tan_h_fov: %f\n"
+      "tan_v_fov: %f"
+    , topic_in.c_str(), topic_out.c_str(), x_min, x_max, z_min, z_max, tan_h_fov, tan_v_fov
+  );
+
+    subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
+      topic_in, rclcpp::SensorDataQoS(),
+      std::bind(&CloudFilterNode::cloud_callback, this, std::placeholders::_1));
+
+    filtered_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(topic_out, 10);
+  }
+
+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::fromROSMsg(*msg, *input);
+
+    fov_filtered->reserve(input->size());
+
+    for (const auto& point : input->points)
+    {
+      if (point.x < x_min || point.x > x_max) 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 (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
+    voxel_filter.setInputCloud(fov_filtered);
+    voxel_filter.filter(*voxel_filtered);
+
+    //Apply ransac
+    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
+    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+    // Create the segmentation object
+    pcl::SACSegmentation<pcl::PointXYZI> seg;
+    // Optional
+    seg.setOptimizeCoefficients(true);
+    // Mandatory
+    seg.setModelType(pcl::SACMODEL_PLANE);
+    seg.setMethodType(pcl::SAC_RANSAC);
+    seg.setDistanceThreshold(0.02);
+    seg.setMaxIterations(200);
+  
+    seg.setInputCloud(voxel_filtered);
+    seg.segment (*inliers, *coefficients);
+  
+    if (inliers->indices.size () == 0)
+    {
+      RCLCPP_INFO(this->get_logger(), "Oh oh. No inliers");
+      return;
+    }
+
+    //Select remaining points
+    pcl::ExtractIndices<pcl::PointXYZI> extract;
+    extract.setInputCloud(voxel_filtered);
+    extract.setIndices(inliers);
+    extract.setNegative(true);
+    extract.filter(*voxel_filtered);
+
+    // Convert and publish
+    sensor_msgs::msg::PointCloud2 out_msg;
+    pcl::toROSMsg(*voxel_filtered, out_msg);
+    out_msg.header.stamp = this->get_clock()->now();
+    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());
+    //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_;
+  tf2_ros::TransformListener tf_listener_;
+
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
+
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr filtered_publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr original_publisher_;
+};
+
+int main(int argc, char * argv[])
+{
+  srand((unsigned int) time (NULL));
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<CloudFilterNode>());
+  rclcpp::shutdown();
+  return 0;
+}