2D->3D detection to pointcloud new pipeline #94

IasonTheodorou · 2025-09-24T15:20:05Z

On ed_sensor_integration we integrate Yolo Sam and Bayesian model for 2D->3D pointcloud creation.

…r the RGB part

…e refitConvexHull and one on cluster algorithm to append to the point cloud only points which have close depth to their neighbooring pixels.

…eblem

…metric features for better generalization

…ilities

… we calculate posteriors not only points)

…ot fully correct)

…m) for 2D-3D inference

…sam_seg_module. Will be used as verbose features.

Copilot

Pull Request Overview

This PR integrates YOLO, SAM, and Bayesian model capabilities into the ed_sensor_integration package to enable 2D-to-3D pointcloud detection. The integration replaces traditional depth-based clustering with AI-driven segmentation using YOLO for object detection, SAM for precise mask generation, and a Bayesian mixture model for point cloud denoising.

Replaces depth-based clustering with YOLO+SAM segmentation pipeline
Adds Bayesian mixture model (BMM) for statistical point cloud filtering
Integrates ROS publishers for visualization of segmentation results

Reviewed Changes

Copilot reviewed 17 out of 18 changed files in this pull request and generated 11 comments.

Show a summary per file

File	Description
hero_sam	Submodule reference to external SAM implementation
ed_sensor_integration_py/	New Python package structure for sensor integration
ed_sensor_integration/src/kinect/updater.cpp	Modified to use new segmentation pipeline and added outlier filtering
ed_sensor_integration/src/kinect/segmenter.cpp	Replaced clustering algorithm with YOLO+SAM pipeline integration
ed_sensor_integration/src/kinect/sam_seg_module.cpp	New module implementing YOLO+SAM segmentation pipeline
ed_sensor_integration/include/	Updated headers to support new segmentation architecture
ed_sensor_integration/CMakeLists.txt	Added dependencies and build configuration for new components
ed_sensor_integration/package.xml	Added package dependencies for YOLO, SAM, and BMM

Comments suppressed due to low confidence (1)

ed_sensor_integration/src/kinect/updater.cpp:1

Commented-out code should be removed. If this is needed for testing, consider using a configuration parameter instead.

#include "ed/kinect/updater.h"

_{Tip: Customize your code reviews with copilot-instructions.md. Create the file or learn how to get started.}

Copilot · 2025-09-24T15:24:37Z

ed_sensor_integration/src/kinect/updater.cpp

+        ROS_ERROR("No segmenter configuration found, cannot initialize Updater.");
+        throw std::runtime_error("No segmenter configuration found");


The error message should be more specific about what configuration is missing. Consider changing to 'Failed to read segmenter configuration group from config file'.

Suggested change

ROS_ERROR("No segmenter configuration found, cannot initialize Updater.");

throw std::runtime_error("No segmenter configuration found");

ROS_ERROR("Failed to read segmenter configuration group from config file, cannot initialize Updater.");

throw std::runtime_error("Failed to read segmenter configuration group from config file");

Copilot · 2025-09-24T15:24:38Z

ed_sensor_integration/src/kinect/segmenter.cpp

+    for (int y = 0; y < rgb_image.rows; y++) {
+        for (int x = 0; x < rgb_image.cols; x++) {
+            int idx = y * rgb_image.cols + x;
+            if (filtered_depth_image.at<float>(idx) > 0) {


The index calculation idx = y * rgb_image.cols + x assumes the depth image is stored as a 1D array, but filtered_depth_image.at<float>(idx) accesses it as such. However, OpenCV Mat objects are typically accessed as 2D with at<float>(y, x). This could cause out-of-bounds access or incorrect pixel mapping.

Suggested change

if (filtered_depth_image.at<float>(idx) > 0) {

if (filtered_depth_image.at<float>(y, x) > 0) {

Copilot · 2025-09-24T15:24:38Z

ed_sensor_integration/src/kinect/sam_seg_module.cpp

+        if (result.classId == 60) {
+            std::cout << "Class ID is: " << yoloDetector->classes[result.classId] << " So we dont append"<< std::endl;


The magic number 60 should be replaced with a named constant or configuration parameter. Consider defining constexpr int IGNORED_CLASS_ID = 60; or making this configurable.

Copilot · 2025-09-24T15:24:38Z

ed_sensor_integration/src/kinect/sam_seg_module.cpp

+{
+// // Overlay masks on the RGB image
+    cv::Mat visualization = rgb.clone();
+    cv::imwrite("/tmp/visualization.png", visualization);


Hard-coded file paths in /tmp/ should be configurable or use proper temporary file handling. These debug outputs could fill up disk space and should be controlled by a debug flag or configuration parameter.

Copilot · 2025-09-24T15:24:38Z

ed_sensor_integration/src/kinect/sam_seg_module.cpp

+        // Apply a colormap for better visibility
+        cv::Mat depth_color;
+        cv::applyColorMap(depth_vis, depth_color, cv::COLORMAP_JET);
+        cv::imwrite("/tmp/visualization_depth_color.png", depth_color);


Hard-coded file paths in /tmp/ should be configurable or use proper temporary file handling. These debug outputs could fill up disk space and should be controlled by a debug flag or configuration parameter.

Copilot · 2025-09-24T15:24:39Z

ed_sensor_integration/src/kinect/sam_seg_module.cpp

+    cv::imwrite("/tmp/visualization_depth.png", depth_vis);
+    overlayMasksOnImage_(visualization, clustered_images);
+    // save after overlaying masks
+    cv::imwrite("/tmp/visualization_with_masks.png", visualization);


Hard-coded file paths in /tmp/ should be configurable or use proper temporary file handling. These debug outputs could fill up disk space and should be controlled by a debug flag or configuration parameter.

Copilot · 2025-09-24T15:24:39Z

ed_sensor_integration/src/kinect/segmenter.cpp

+            }
+        }
+        // Safety check
+        if (filtered_points.size() > 10 && filtered_points.size() > 0.1 * cluster.points.size()) {


The magic numbers 10 and 0.1 should be replaced with named constants. Consider defining constexpr size_t MIN_FILTERED_POINTS = 10; and constexpr double MIN_RETENTION_RATIO = 0.1;.

Copilot · 2025-09-24T15:24:39Z

ed_sensor_integration/CMakeLists.txt

+
+# -------------- ONNXRuntime Setup (define this early) ------------------#
+set(ONNXRUNTIME_VERSION 1.21.1)
+set(ONNXRUNTIME_ROOT "/home/amigo/Documents/repos/hero_sam.bak/onnxruntime-linux-x64-gpu-${ONNXRUNTIME_VERSION}")


Hard-coded absolute paths make the build non-portable. These paths should be configurable through environment variables or CMake cache variables, e.g., set(ONNXRUNTIME_ROOT $ENV{ONNXRUNTIME_ROOT} CACHE PATH \"Path to ONNX Runtime\").

Copilot · 2025-09-24T15:24:40Z

ed_sensor_integration/CMakeLists.txt

+set(YOLO_MODELS_PATH "/home/amigo/Documents/repos/hero_sam.bak/yolo_inference/model")
+set(SAM_MODELS_PATH "/home/amigo/Documents/repos/hero_sam.bak/sam_inference/model")


Hard-coded absolute paths make the build non-portable. These paths should be configurable through environment variables or CMake cache variables, e.g., set(ONNXRUNTIME_ROOT $ENV{ONNXRUNTIME_ROOT} CACHE PATH \"Path to ONNX Runtime\").

Copilot · 2025-09-24T15:24:40Z

ed_sensor_integration/src/kinect/segmenter.cpp

+            // Filter points that are too far from centroid
+            std::vector<geo::Vec3> filtered_points;
+            for (const auto& p : cluster.points) {
+                if ((p - centroid).length() < 2.5 * stddev)


The magic number 2.5 for the standard deviation threshold should be a named constant. Consider defining constexpr double OUTLIER_STDDEV_THRESHOLD = 2.5;.

…unused lines

…ronger

…s 1D arrray so it could break

Copilot

Pull Request Overview

Copilot reviewed 17 out of 18 changed files in this pull request and generated 7 comments.

_{Tip: Customize your code reviews with copilot-instructions.md. Create the file or learn how to get started.}

Copilot · 2025-09-26T09:51:22Z

ed_sensor_integration/src/kinect/updater.cpp

    float z_max = -1e9;

    int i_pixel = 0;
+    std::vector<float> z_values;


The z_values vector is allocated inside a loop that processes every pixel. Consider reserving capacity based on the filtered_depth_image size to avoid repeated reallocations.

Suggested change

std::vector<float> z_values;

std::vector<float> z_values;

z_values.reserve(filtered_depth_image.rows * filtered_depth_image.cols);

Copilot · 2025-09-26T09:51:23Z

ed_sensor_integration/src/kinect/segmenter.cpp

+// Internal constants (tuning thresholds)
+constexpr std::size_t MIN_FILTERED_POINTS = 10;
+constexpr double      MIN_RETENTION_RATIO = 0.10;  // 10%


These magic numbers should be made configurable parameters read from the configuration file instead of being hardcoded constants.

Copilot · 2025-09-26T09:51:23Z

ed_sensor_integration/src/kinect/segmenter.cpp

-    int dirs[] = { -1, 1, -width, width,
-                   -2, 2, -width * 2, width * 2};  // Also try one pixel skipped (filtering may cause some 1-pixel gaps)
-
+    std::vector<cv::Mat> masks = SegmentationPipeline(rgb_image.clone());


Cloning the entire RGB image before passing to SegmentationPipeline is unnecessary and wasteful. Pass by const reference instead and only clone if modifications are needed within the pipeline.

Suggested change

std::vector<cv::Mat> masks = SegmentationPipeline(rgb_image.clone());

std::vector<cv::Mat> masks = SegmentationPipeline(rgb_image);

Copilot · 2025-09-26T09:51:23Z

ed_sensor_integration/src/kinect/sam_seg_module.cpp

+    for (const auto& result : resYolo) {
+        // This should be deleted as we would upload everything
+        // (here we are skipping the table object but it should happen only on the rosservice scenario: on_top_of dinner_table )
+        int table_classification = 60;


The hardcoded table classification ID (60) should be made configurable or defined as a named constant to improve maintainability.

Copilot · 2025-09-26T09:51:24Z

ed_sensor_integration/CMakeLists.txt

+
+# -------------- ONNXRuntime Setup (define this early) ------------------#
+set(ONNXRUNTIME_VERSION 1.21.1)
+set(ONNXRUNTIME_ROOT "/home/amigo/Documents/repos/hero_sam.bak/onnxruntime-linux-x64-gpu-${ONNXRUNTIME_VERSION}")


Hardcoded absolute paths make the build non-portable. Use environment variables, find_package, or relative paths to make the build system more flexible and portable across different systems.

Suggested change

set(ONNXRUNTIME_ROOT "/home/amigo/Documents/repos/hero_sam.bak/onnxruntime-linux-x64-gpu-${ONNXRUNTIME_VERSION}")

# Set ONNXRUNTIME_ROOT via environment variable or CMake cache variable for portability.

if(NOT DEFINED ONNXRUNTIME_ROOT AND DEFINED ENV{ONNXRUNTIME_ROOT})

set(ONNXRUNTIME_ROOT $ENV{ONNXRUNTIME_ROOT})

endif()

set(ONNXRUNTIME_ROOT "${ONNXRUNTIME_ROOT}" CACHE PATH "Path to ONNX Runtime installation (set via -DONNXRUNTIME_ROOT=... or environment variable ONNXRUNTIME_ROOT)")

if(NOT ONNXRUNTIME_ROOT)

message(FATAL_ERROR "ONNXRUNTIME_ROOT is not set. Please set it via -DONNXRUNTIME_ROOT=... or export ONNXRUNTIME_ROOT=...")

endif()

Copilot · 2025-09-26T09:51:24Z

ed_sensor_integration/CMakeLists.txt

+#OR
+# Define model paths
+set(YOLO_MODELS_PATH "/home/amigo/Documents/repos/hero_sam.bak/yolo_inference/model")
+set(SAM_MODELS_PATH "/home/amigo/Documents/repos/hero_sam.bak/sam_inference/model")


Hardcoded absolute paths make the build non-portable. Use environment variables, find_package, or relative paths to make the build system more flexible and portable across different systems.

Suggested change

set(SAM_MODELS_PATH "/home/amigo/Documents/repos/hero_sam.bak/sam_inference/model")

# Set the path to the SAM model files. Override with -DSAM_MODELS_PATH=/your/path when running cmake.

set(SAM_MODELS_PATH "" CACHE PATH "Path to the SAM model files")

Copilot · 2025-09-26T09:51:24Z

ed_sensor_integration/src/kinect/sam_seg_module.cpp

+    // Overlay masks on the RGB image
+    cv::Mat visualization = rgb.clone();
+    // Create a path to save the image
+    std::string path = "/tmp";


The hardcoded /tmp path for saving visualization images should be made configurable or use a more appropriate directory structure to avoid conflicts and improve maintainability.

MatthijsBurgh and others added 30 commits February 11, 2025 21:14

Add python playground pkg

6e7d5f4

added onnx yolo c++ implementation for object classification

96a8ce2

Updated Cmake to find the onnx file

2ccc003

Disable CUDA and include absolute path to the model

4782ec0

calling ClassificaitionINference functio

7a72f40

Added the new YOLO and SAM implementation

9844303

Added visualization on a rostopic for debugging

970078f

Add new segmented implementation on the pipeline

4252c39

Segment and display on rqt the results

d117ba6

Added rgb preprocessing by appliying the depth preprocessed filter fo…

6f62a5a

…r the RGB part

Added new visualization

bf03804

Added some more filtering (a statistical one to remove outliers in th…

913d02a

…e refitConvexHull and one on cluster algorithm to append to the point cloud only points which have close depth to their neighbooring pixels.

Added debug statements and publish pointcloud (still not fully working)

d1c8fab

Publish the pointcloud as a ROS topic for visual inspection of the pr…

37eda3d

…eblem

Added new filtering for point cloud estimation

aae3dab

Better visualization on rqt

ef905cd

Applied GMM and DBSCAN to clean the pointcloud noise

f84f153

Implemented Bayesian GMM for having also priors on the model

5264017

Kept GMM with no-prior enabled

9fe979d

Added ros parames to tune all the priors of the bayesian GMM

0af0aaf

Added ros parames small fix to compile

046e257

Implemented GMM with parameters for fast turning (without compile)

cd350be

Select the inlier based on argmax responsibility and not based on geo…

0342e9e

…metric features for better generalization

Update non-bayesian GMM to perform hard-clustering based on responsib…

5730d57

…ilities

Added also a fully variational inference engine for bayesian GMM (now…

23d5d85

… we calculate posteriors not only points)

Add rosparameters for dbscan as well

6d52901

Removed custom simple statistical filter (kept only GMM)

540ee56

Model the outliers as uniform distribution

8c50423

Used SAM and BMM models from ros packages instead of bad way (still n…

6aadaab

…ot fully correct)

Made ed_sensor_integration use only the new ros packages (yolo-sam-bm…

df14783

…m) for 2D-3D inference

IasonTheodorou added 4 commits September 12, 2025 19:33

Removed all visualization function from updater and included it into …

b15d832

…sam_seg_module. Will be used as verbose features.

Included the old behavior now with segmenter_ object as a pointer

8f82ba0

Updating CMakeLists and small refactoring/fixes

f77fc9c

Changed function naming to proper match as part of the refactoring

61fe7e4

IasonTheodorou requested review from MatthijsBurgh and Copilot September 24, 2025 15:20

Copilot AI reviewed Sep 24, 2025

View reviewed changes

IasonTheodorou added 5 commits September 26, 2025 11:07

added doxygen description for the new functions

08658a0

removed hardcoded values (added variables for them) and removed some …

d103982

…unused lines

commented out simple statistical outlier removal since GMM is much st…

58f4df7

…ronger

made indexing of the CV mat more robust since before it was treated a…

70aa2ad

…s 1D arrray so it could break

impoved ROS error logging

7a2532c

IasonTheodorou requested a review from Copilot September 26, 2025 09:49

Copilot AI reviewed Sep 26, 2025

View reviewed changes

added sam correct header paths

7a5dff6

		ROS_ERROR("No segmenter configuration found, cannot initialize Updater.");
		throw std::runtime_error("No segmenter configuration found");

	if (filtered_depth_image.at<float>(idx) > 0) {
	if (filtered_depth_image.at<float>(y, x) > 0) {

		if (result.classId == 60) {
		std::cout << "Class ID is: " << yoloDetector->classes[result.classId] << " So we dont append"<< std::endl;

		set(YOLO_MODELS_PATH "/home/amigo/Documents/repos/hero_sam.bak/yolo_inference/model")
		set(SAM_MODELS_PATH "/home/amigo/Documents/repos/hero_sam.bak/sam_inference/model")

	std::vector<float> z_values;
	std::vector<float> z_values;
	z_values.reserve(filtered_depth_image.rows * filtered_depth_image.cols);

	std::vector<cv::Mat> masks = SegmentationPipeline(rgb_image.clone());
	std::vector<cv::Mat> masks = SegmentationPipeline(rgb_image);

-set(ONNXRUNTIME_ROOT "/home/amigo/Documents/repos/hero_sam.bak/onnxruntime-linux-x64-gpu-${ONNXRUNTIME_VERSION}")
+# Set ONNXRUNTIME_ROOT via environment variable or CMake cache variable for portability.
+if(NOT DEFINED ONNXRUNTIME_ROOT AND DEFINED ENV{ONNXRUNTIME_ROOT})
+  set(ONNXRUNTIME_ROOT $ENV{ONNXRUNTIME_ROOT})
+endif()
+set(ONNXRUNTIME_ROOT "${ONNXRUNTIME_ROOT}" CACHE PATH "Path to ONNX Runtime installation (set via -DONNXRUNTIME_ROOT=... or environment variable ONNXRUNTIME_ROOT)")
+if(NOT ONNXRUNTIME_ROOT)
+  message(FATAL_ERROR "ONNXRUNTIME_ROOT is not set. Please set it via -DONNXRUNTIME_ROOT=... or export ONNXRUNTIME_ROOT=...")
+endif()

	set(SAM_MODELS_PATH "/home/amigo/Documents/repos/hero_sam.bak/sam_inference/model")
	# Set the path to the SAM model files. Override with -DSAM_MODELS_PATH=/your/path when running cmake.
	set(SAM_MODELS_PATH "" CACHE PATH "Path to the SAM model files")

2D->3D detection to pointcloud new pipeline #94

Are you sure you want to change the base?

2D->3D detection to pointcloud new pipeline #94

Uh oh!

Conversation

IasonTheodorou commented Sep 24, 2025

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull Request Overview

Reviewed Changes

Uh oh!

Copilot AI Sep 24, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 24, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 24, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 24, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 24, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 24, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 24, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 24, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 24, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 24, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull Request Overview

Uh oh!

Copilot AI Sep 26, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 26, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 26, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 26, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 26, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 26, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Sep 26, 2025

Choose a reason for hiding this comment

Uh oh!

Uh oh!