Pose detection working for all objects now

Author: heethesh

object_detection/launch/object_detection.launch

@@ -18,6 +18,12 @@
         name="object_pose_detection"
         output="screen">
 
+        <!-- Template object folder for ICP -->
+        <param
+            name="template_path"
+            type="string"
+            value="$(find object_detection)/templates/" />
+
         <!-- Set topics and params -->
         <rosparam>
             <!-- invert (default: true): True > Box; False > Ground Plane -->

object_detection/perception.rviz

@@ -25,7 +25,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Ground Truth
+    SyncSource: Clean Object PCL
 Visualization Manager:
   Class: ""
   Displays:
@@ -138,7 +138,7 @@ Visualization Manager:
       Color: 0; 255; 0
       Color Transformer: FlatColor
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -155,7 +155,7 @@ Visualization Manager:
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -211,7 +211,7 @@ Visualization Manager:
       Size (Pixels): 3
       Size (m): 0.00999999978
       Style: Points
-      Topic: /ground_plane_segmentation/points
+      Topic: /object_pose_detection/points
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
@@ -241,25 +241,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.57199478
+      Distance: 0.911043346
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.380761057
-        Y: 0.105236754
-        Z: 0.0502308793
+        X: -0.012347864
+        Y: -0.153358907
+        Z: -0.142543241
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.725203395
+      Pitch: 0.550203443
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.5242579
+      Yaw: 2.59425759
     Saved: ~
 Window Geometry:
   3D Bounding Box Image:

object_detection/src/object_pose_detection.cpp

@@ -66,8 +66,9 @@ string input_topic;
 string output_topic;
 string coefficients_topic;
 
+int argmin = -1;
 Eigen::Matrix4d icp_transform;
-string template_cuboid_filename;
+vector<Eigen::Matrix4d> icp_transforms;
 sensor_msgs::PointCloud2 bbox_msg;
 sensor_msgs::PointCloud2 output_msg;
 sensor_msgs::PointCloud2 template_msg;
@@ -78,6 +79,11 @@ double dimensions[3];
 double icp_fitness_score;
 pcl::PCLPointCloud2::Ptr input_pcl(new pcl::PCLPointCloud2);
 
+// Template paths
+string template_path;
+string template_filenames[] = {"", "screwdriver_ascii_tf.pcd", "eraser_ascii_tf.pcd",
+                               "clamp_ascii_tf.pcd", "marker_ascii_tf.pcd"};
+
 // Flags
 bool DEBUG = false;
 bool ICP_SUCCESS = false;
@@ -141,26 +147,23 @@ void publish_bounding_box(Eigen::Matrix4d H)
     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;
-        }
-    }
+    // 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 icp_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr input_pcl, pcl::PointCloud<pcl::PointXYZ>::Ptr template_pcl)
+double icp_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr input_pcl, pcl::PointCloud<pcl::PointXYZ>::Ptr template_pcl)
 {
+    int max_iter = 0;
     while (1)
     {
         // Point cloud containers
@@ -176,21 +179,22 @@ void icp_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr input_pcl, pcl::PointC
         icp.setEuclideanFitnessEpsilon(icp_fitness_score);
         icp.setRANSACOutlierRejectionThreshold(1.5);
         icp.align(*output_cloud);
-        icp_transform = icp.getFinalTransformation().cast<double>().inverse();
+        Eigen::Matrix4d icp_transform_local = icp.getFinalTransformation().cast<double>().inverse();
+        icp_transforms.push_back(icp_transform_local);
 
         cerr << "ICP Score Before: " << icp.getFitnessScore() << endl;
+        max_iter++;
 
-        if (icp.hasConverged() && icp.getFitnessScore() < icp_fitness_score)
+        if ((icp.hasConverged() && icp.getFitnessScore() < icp_fitness_score) || (max_iter > 10))
         {
             // Display ICP results
             cerr << "\nICP Score: " << icp.getFitnessScore() << endl;
-            cerr << "ICP Transform:\n" << icp_transform << endl;
+            cerr << "ICP Transform:\n" << icp_transform_local << endl;
 
             // Convert to ROS data type
             pcl::toROSMsg(*output_cloud, output_msg);
             pcl::toROSMsg(*template_pcl, template_msg);
-            ICP_SUCCESS = true;
-            return;
+            return icp.getFitnessScore();
         }
     }
 }
@@ -202,7 +206,7 @@ void pcl_callback(const sensor_msgs::PointCloud2ConstPtr& input)
     if (DEBUG) cerr << "PointCloud before filtering: " << input_pcl->width << " " << input_pcl->height << " data points." << endl;
 
     // Publish template point cloud
-    if (ICP_SUCCESS)
+    if (ICP_SUCCESS and argmin != -1)
     {
         icp_pub.publish(output_msg);
         template_msg.header.frame_id = "camera_depth_optical_frame";
@@ -274,7 +278,7 @@ bool service_callback(object_detection::ObjectDetection::Request  &req,
     pcl::PassThrough<pcl::PCLPointCloud2> pass_z2;
     pass_z2.setInputCloud(plane_cloud_ptr);
     pass_z2.setFilterFieldName("z");
-    pass_z2.setFilterLimits(0.0, 0.7);
+    pass_z2.setFilterLimits(0.0, 0.75);
     pass_z2.filter(*cloud_filtered_ptr_z2);
     if (DEBUG) cerr << "PointCloud after filtering: " << cloud_filtered_ptr_z2->width << " " << cloud_filtered_ptr->height << " data points." << endl;
 
@@ -298,10 +302,15 @@ bool service_callback(object_detection::ObjectDetection::Request  &req,
     ec.extract(object_cluster_indices);
 
     // Display number of objects detceted
+    cerr << "\nRequested Object: " << template_filenames[req.object_id] << endl;
     cerr << "Objects Detected: " << object_cluster_indices.size() << endl;
-    int obj_id = 1;
 
-    // What is this?
+    // Find the best registered object
+    int obj_id = req.object_id;
+    vector<double> icp_scores;
+    icp_transforms.clear();
+    ICP_SUCCESS = false;
+
     for (vector<pcl::PointIndices>::const_iterator it = object_cluster_indices.begin(); it != object_cluster_indices.end(); ++it)
     {
         // Create a pcl object to hold the extracted cluster
@@ -323,7 +332,7 @@ bool service_callback(object_detection::ObjectDetection::Request  &req,
         pcl_pub.publish(output);
 
         // Read template point cloud
-        string template_filename = "/home/heethesh/ROS-Workspaces/dash_ws/src/perception/object_detection/templates/eraser_ascii_tf.pcd";
+        string template_filename = template_path + template_filenames[req.object_id];
         pcl::PointCloud<pcl::PointXYZ>::Ptr template_pcl(new pcl::PointCloud<pcl::PointXYZ>);
         if (pcl::io::loadPCDFile<pcl::PointXYZ>(template_filename, *template_pcl) == -1)
         {
@@ -333,9 +342,35 @@ bool service_callback(object_detection::ObjectDetection::Request  &req,
         }
 
         // Register using ICP and broadcast TF
-        icp_registration(object_cluster_pcl, template_pcl);
+        double score = icp_registration(object_cluster_pcl, template_pcl);
+        icp_scores.push_back(score);
+    }
+
+    // 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)
+        {
+            argmin = i;
+            min_score = icp_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" << endl;
+        ICP_SUCCESS = true;
         res.success = true;
         return true;
+    } else {
+        cerr << "ICP Registration Failed to Converge within Threshold!\n" << endl;
+        ICP_SUCCESS = false;
+        res.success = false;
+        return false;
     }
 }
 
@@ -352,6 +387,7 @@ int main(int argc, char** argv)
     nh.getParam("input", input_topic);
     nh.getParam("output", output_topic);
     nh.getParam("icp_fitness_score", icp_fitness_score);
+    nh.getParam("template_path", template_path);
 
     // Params defaults
     nh.param<bool>("invert", invert, true);
@@ -366,7 +402,8 @@ int main(int argc, char** argv)
     cout << "Distance Threshold: " << distance_threshold << endl;
     cout << "Input Topic: " << input_topic << endl;
     cout << "Output Topic: " << output_topic << endl;
-    cerr << "ICP Fitness Score:" << icp_fitness_score << endl;
+    cerr << "ICP Fitness Score: " << icp_fitness_score << endl;
+    cerr << "ICP Template Folder: " << template_path << endl;
 
     // Create a ROS subscriber for the input point cloud
     ros::Subscriber sub = nh.subscribe(input_topic, 1, pcl_callback);