Robust object detection 🔨

Author: heethesh

object_detection/perception.rviz

@@ -3,8 +3,7 @@ Panels:
     Help Height: 75
     Name: Displays
     Property Tree Widget:
-      Expanded:
-        - /Axes1
+      Expanded: ~
       Splitter Ratio: 0.5
     Tree Height: 371
   - Class: rviz/Selection
@@ -25,7 +24,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: Camera View
 Visualization Manager:
   Class: ""
   Displays:
@@ -56,11 +55,11 @@ Visualization Manager:
       Value: true
     - Class: rviz/Image
       Enabled: true
-      Image Topic: /draw_bbox/image
+      Image Topic: /camera/color/image_raw
       Max Value: 1
       Median window: 5
       Min Value: 0
-      Name: 3D Bounding Box Image
+      Name: Camera View
       Normalize Range: true
       Queue Size: 2
       Transport Hint: raw
@@ -114,7 +113,7 @@ Visualization Manager:
       Max Intensity: 4096
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: ICP Registered PCL
+      Name: Detected ICP
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
@@ -221,7 +220,7 @@ Visualization Manager:
       Marker Topic: /object_pose_detection/object_pose_detection/grasp_pose
       Name: Marker
       Namespaces:
-        {}
+        grasp_pose: true
       Queue Size: 100
       Value: true
   Enabled: true
@@ -249,35 +248,35 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.911043346
+      Distance: 0.904538155
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.012347864
-        Y: -0.153358907
-        Z: -0.142543241
+        X: 0.232943743
+        Y: -0.0362719707
+        Z: -0.0569765195
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.550203443
+      Pitch: 0.834797025
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.59425759
+      Yaw: 2.36926389
     Saved: ~
 Window Geometry:
-  3D Bounding Box Image:
+  Camera View:
     collapsed: false
   Displays:
     collapsed: false
   Height: 1056
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000022200000396fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001ff000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002a0033004400200042006f0075006e00640069006e006700200042006f007800200049006d006100670065010000022d000001910000001600fffffffb0000000a0049006d006100670065000000028d000001310000000000000000000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007410000003efc0100000002fb0000000800540069006d00650100000000000007410000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005190000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000022200000396fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001ff000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001600430061006d00650072006100200056006900650077010000022d000001910000001600fffffffb0000000a0049006d006100670065000000028d000001310000000000000000000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007410000003efc0100000002fb0000000800540069006d00650100000000000007410000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005190000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

object_detection/src/object_pose_detection.cpp

@@ -12,6 +12,7 @@ add_executable(ground_plane_segmentation src/ground_plane_segmentation.cpp)
 target_link_libraries(ground_plane_segmentation ${catkin_LIBRARIES})
 */
 
+#include <cmath>
 #include "object_detection/boost.h"
 #include <visualization_msgs/Marker.h>
 
@@ -76,9 +77,10 @@ sensor_msgs::PointCloud2 template_msg;
 tf::TransformListener* listener;
 geometry_msgs::Pose pose_msg;
 Eigen::Affine3d pose_transform;
-double dimensions[] = { 0.02, 0.15, 0.02 };
+double dimensions[] = { 0.02, 0.02, 0.15 };
 double icp_fitness_score;
 pcl::PCLPointCloud2::Ptr input_pcl(new pcl::PCLPointCloud2);
+vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> output_pcls;
 
 // Template paths
 string template_path;
@@ -207,7 +209,7 @@ void publish_bounding_box(Eigen::Matrix4d H)
     bbox_pub.publish(bbox_msg);
 }
 
-double icp_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr input_pcl, pcl::PointCloud<pcl::PointXYZ>::Ptr template_pcl)
+double icp_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr object_pcl, pcl::PointCloud<pcl::PointXYZ>::Ptr template_pcl)
 {
     int max_iter = 0;
     while (1) {
@@ -216,7 +218,7 @@ double icp_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr input_pcl, pcl::Poin
 
         // Run ICP
         pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
-        icp.setInputSource(input_pcl);
+        icp.setInputSource(object_pcl);
         icp.setInputTarget(template_pcl);
         icp.setMaximumIterations(5000);
         icp.setTransformationEpsilon(1e-9);
@@ -227,7 +229,7 @@ double icp_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr input_pcl, pcl::Poin
         Eigen::Matrix4d icp_transform_local = icp.getFinalTransformation().cast<double>().inverse();
         icp_transforms.push_back(icp_transform_local);
 
-        cerr << "ICP Score Before: " << icp.getFitnessScore() << endl;
+        // cerr << "ICP Score Before: " << icp.getFitnessScore() << endl;
         max_iter++;
 
         if ((icp.hasConverged() && icp.getFitnessScore() < icp_fitness_score) || (max_iter > 10)) {
@@ -237,8 +239,8 @@ double icp_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr input_pcl, pcl::Poin
                  << icp_transform_local << endl;
 
             // Convert to ROS data type
-            pcl::toROSMsg(*output_cloud, output_msg);
             pcl::toROSMsg(*template_pcl, template_msg);
+            output_pcls.push_back(output_cloud);
             return icp.getFitnessScore();
         }
     }
@@ -253,6 +255,10 @@ void pcl_callback(const sensor_msgs::PointCloud2ConstPtr& input)
 
     // Publish template point cloud
     if (ICP_SUCCESS and argmin != -1) {
+        // Convert to ROS data type
+        pcl::toROSMsg(*output_pcls[argmin], output_msg);
+
+        // Publish topics
         icp_pub.publish(output_msg);
         template_msg.header.frame_id = "camera_depth_optical_frame";
         template_pub.publish(template_msg);
@@ -362,6 +368,8 @@ bool service_callback(object_detection::ObjectDetection::Request& req,
     // Find the best registered object
     int obj_id = req.object_id;
     vector<double> icp_scores;
+    vector<double> diff_scores;
+    output_pcls.clear();
     icp_transforms.clear();
     ICP_SUCCESS = false;
 
@@ -396,29 +404,36 @@ bool service_callback(object_detection::ObjectDetection::Request& req,
         // Register using ICP and broadcast TF
         double score = icp_registration(object_cluster_pcl, template_pcl);
         icp_scores.push_back(score);
+
+        // Check number of points
+        // cerr << "Object Points: " << (int)object_cluster_pcl->size() << endl;
+        // cerr << "Template Points: " << (int)template_pcl->size() << endl;
+        cerr << "Difference Score: " << abs((int)object_cluster_pcl->size() - (int)template_pcl->size()) << endl;
+        diff_scores.push_back(abs((int)object_cluster_pcl->size() - (int)template_pcl->size()));
     }
 
     // Find the object with best ICP score
     double min_score = 1000;
     argmin = -1;
-    for (int i = 0; i < icp_scores.size(); i++) {
-        if (icp_scores[i] < min_score) {
+    for (int i = 0; i < diff_scores.size(); i++) {
+        if (diff_scores[i] < min_score) {
             argmin = i;
-            min_score = icp_scores[i];
+            min_score = diff_scores[i];
         }
     }
 
     cerr << "\nLowest Object Index: " << argmin << " with Score: " << min_score << endl;
     icp_transform = icp_transforms[argmin];
 
-    if (min_score < icp_fitness_score) {
-        cerr << "ICP Registration Success!\n"
+    // if (min_score < icp_fitness_score) {
+    if (min_score < 250) {
+        cerr << "Object Registration Success!\n"
              << endl;
         ICP_SUCCESS = true;
         res.success = true;
         return true;
     } else {
-        cerr << "ICP Registration Failed to Converge within Threshold!\n"
+        cerr << "Object Registration Failed to Converge within Threshold!\n"
              << endl;
         ICP_SUCCESS = false;
         res.success = false;