3D bbox integrated 🔲⚡

Author: heethesh

cuboid_detection/launch/iterative_closest_point.launch

@@ -4,18 +4,28 @@
     <!-- Launch Bounding Box Filter -->
     <include file="$(find cuboid_detection)/launch/bbox_filter.launch"/>
 
-    <!-- Template object path for ICP -->
-    <param
-        name="template_cuboid_path"
-        type="string"
-        value="$(find cuboid_detection)/templates/template_cuboid_L200_W100_H75_3faces.pcd" />
 
     <!-- Run ICP node -->
     <node
         pkg="cuboid_detection"
         type="iterative_closest_point"
         name="iterative_closest_point"
         output="screen">
+
+        <!-- Template object path for ICP -->
+        <param
+            name="template_cuboid_path"
+            type="string"
+            value="$(find cuboid_detection)/templates/template_cuboid_L200_W75_H100_3faces.pcd" />
+            
+        <!-- Set topics and params -->
+        <rosparam>
+            <!-- Dimensions -->
+            length: 0.2
+            width: 0.075
+            height: 0.1
+        </rosparam>
+
     </node>
 
 </launch>

cuboid_detection/src/iterative_closest_point.cpp

@@ -20,29 +20,73 @@ using namespace std;
 
 // Globals  
 ros::Publisher pcl_pub;
+ros::Publisher bbox_pub;
 ros::Publisher template_pub;
 Eigen::Matrix4d icp_transform;
 string template_cuboid_filename;
 tf::TransformListener *tf_listener; 
+sensor_msgs::PointCloud2 bbox_msg;
 sensor_msgs::PointCloud2 output_msg;
 sensor_msgs::PointCloud2 template_msg;
+double dimensions[3];
 
 // Flags
 bool DEBUG = false;
 bool ICP_SUCCESS = false;
+bool FIRST = true;
 
-void convert_icp_eigen_to_tf(Eigen::Matrix4d Tm)
+void publish_bounding_box(Eigen::Matrix4d H)
+{
+    // Extract cuboid dimensions
+    double l = dimensions[0];
+    double w = dimensions[1];
+    double h = dimensions[2];
+
+    // Create a point cloud from the vertices
+    pcl::PointCloud<pcl::PointXYZ> box_cloud;
+    box_cloud.push_back(pcl::PointXYZ(0, 0, 0));
+    box_cloud.push_back(pcl::PointXYZ(0, 0, h));
+    box_cloud.push_back(pcl::PointXYZ(0, w, 0));
+    box_cloud.push_back(pcl::PointXYZ(0, w, h));
+    box_cloud.push_back(pcl::PointXYZ(l, 0, 0));
+    box_cloud.push_back(pcl::PointXYZ(l, 0, h));
+    box_cloud.push_back(pcl::PointXYZ(l, w, 0));
+    box_cloud.push_back(pcl::PointXYZ(l, w, h));
+
+    // Transform point cloud
+    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::transformPointCloud(box_cloud, *transformed_cloud, H.cast<float>());
+
+    if (FIRST)
+    {
+        FIRST = false;
+        cout << "Bounding Box Points:" << endl;
+        for (int i = 0; i < transformed_cloud->size(); i++)
+        {
+            cout << "X: " << transformed_cloud->points[i].x << " | "
+                 << "Y: " << transformed_cloud->points[i].y << " | "
+                 << "Z: " << transformed_cloud->points[i].z << endl;
+        }
+    }
+
+    // Convert to ROS data type and publish
+    pcl::toROSMsg(*transformed_cloud, bbox_msg);
+    bbox_msg.header.frame_id = "camera_depth_optical_frame";
+    bbox_pub.publish(bbox_msg);
+}
+
+void convert_icp_eigen_to_tf(Eigen::Matrix4d H)
 {   
     // Set translation
     tf::Vector3 origin;
-    origin.setValue(Tm(0, 3), Tm(1, 3), Tm(2, 3));
+    origin.setValue(H(0, 3), H(1, 3), H(2, 3));
 
     // Set rotation
     tf::Quaternion quaternion;
     tf::Matrix3x3 rotation_matrix;
-    rotation_matrix.setValue(Tm(0, 0), Tm(0, 1), Tm(0, 2),  
-                             Tm(1, 0), Tm(1, 1), Tm(1, 2),  
-                             Tm(2, 0), Tm(2, 1), Tm(2, 2));
+    rotation_matrix.setValue(H(0, 0), H(0, 1), H(0, 2),  
+                             H(1, 0), H(1, 1), H(1, 2),  
+                             H(2, 0), H(2, 1), H(2, 2));
     rotation_matrix.getRotation(quaternion);
 
     // Make tf transform message
@@ -64,6 +108,7 @@ void icp_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
         pcl_pub.publish(output_msg);
         template_msg.header.frame_id = "camera_depth_optical_frame";
         template_pub.publish(template_msg);
+        publish_bounding_box(icp_transform);
         return;
     }
 
@@ -90,7 +135,7 @@ void icp_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
     icp.setTransformationEpsilon(1e-9);
     // icp.setMaxCorrespondenceDistance(0.05);
     // icp.setEuclideanFitnessEpsilon(1);
-    // icp.setRANSACOutlierRejectionThreshold(1.5);
+    icp.setRANSACOutlierRejectionThreshold(1.5);
     icp.align(*output_cloud);
     icp_transform = icp.getFinalTransformation().cast<double>().inverse();
 
@@ -118,17 +163,26 @@ int main(int argc, char **argv)
 {
     // Init node
     ros::init(argc, argv, "iterative_closest_point");
-    ros::NodeHandle nh;
+    ros::NodeHandle nh("~");
 
     // Handle params
     nh.getParam("template_cuboid_path", template_cuboid_filename);
-    cerr << "\nTemplate filename: " << template_cuboid_filename;
+    nh.getParam("length", dimensions[0]);
+    nh.getParam("width", dimensions[1]);
+    nh.getParam("height", dimensions[2]);
+
+    // Display params
+    cerr << "\nTemplate filename: " << template_cuboid_filename << endl;
+    cerr << "Length (m): " << dimensions[0] << endl;
+    cerr << "Width (m): " << dimensions[1] << endl;
+    cerr << "Height (m): " << dimensions[2] << endl;
 
     // Subscribers
     ros::Subscriber pcl_sub = nh.subscribe<sensor_msgs::PointCloud2>("/ground_plane_segmentation/points", 1, icp_callback);
 
     // Publishers
     pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/icp/aligned_points", 1);
+    bbox_pub = nh.advertise<sensor_msgs::PointCloud2>("/icp/bbox_points", 1);
     template_pub = nh.advertise<sensor_msgs::PointCloud2>("/icp/template", 1);
 
     ros::spin();

cuboid_detection/templates/make_cuboid.py

@@ -16,7 +16,7 @@
 
 # Handle filename
 if not args.filename:
-    args.filename = 'template_cuboid_L%d_W%d_H%d.pcd' % (L * 1000, W * 1000, H * 1000)
+    args.filename = 'template_cuboid_L%d_W%d_H%d_3faces.pcd' % (L * 1000, W * 1000, H * 1000)
 elif not args.filename.endswith('.pcd'):
     args.filename += '.pcd'
 
@@ -49,13 +49,14 @@ def face(xin, yin):
 column = lambda x: np.full((x[1], 1), x[0])
 
 # Make all 6 faces
-faces = [None] * 6
+faces = [None] * 3
 faces[0] = np.hstack((face(X, Y), column([0, N[2]])))
 faces[1] = np.hstack((face(X, Z)[:, 0], column([0, N[1]]), face(X, Z)[:, 1]))
 faces[2] = np.hstack((column([0, N[0]]), face(Y, Z)[:, 0], face(Y, Z)[:, 1]))
-faces[3] = np.hstack((face(X, Y), column([H, N[2]])))
-faces[4] = np.hstack((face(X, Z)[:, 0], column([W, N[1]]), face(X, Z)[:, 1]))
-faces[5] = np.hstack((column([L, N[0]]), face(Y, Z)[:, 0], face(Y, Z)[:, 1]))
+# faces[3] = np.hstack((face(X, Y), column([H, N[2]])))
+# faces[4] = np.hstack((face(X, Z)[:, 0], column([W, N[1]]), face(X, Z)[:, 1]))
+# faces[5] = np.hstack((column([L, N[0]]), face(Y, Z)[:, 0], face(Y, Z)[:, 1]))
+
 faces = np.vstack(faces)
 
 # Save PCD file