Solved Merge Conflicts

Author: ykarmesh

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: ""
 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,11 +211,19 @@ 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
       Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /object_pose_detection/object_pose_detection/grasp_pose
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -241,25 +249,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

@@ -13,7 +13,7 @@ target_link_libraries(ground_plane_segmentation ${catkin_LIBRARIES})
 */
 
 #include "object_detection/boost.h"
-#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
 
 // TF includes
 #include <tf/tf.h>
@@ -57,6 +57,7 @@ ros::Publisher coef_pub;
 ros::Publisher bbox_pub;
 ros::Publisher template_pub;
 ros::Publisher pose_pub;
+ros::Publisher marker_pub;
 
 // Globals
 bool invert;
@@ -66,24 +67,72 @@ 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;
 tf::TransformListener* listener;
 geometry_msgs::Pose pose_msg;
 Eigen::Affine3d pose_transform;
-double dimensions[3];
+double dimensions[] = { 0.1, 0.02, 0.02 };
 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;
 bool FIRST = true;
 bool POSE_FLAG = false;
 
+void publish_grasp_marker(geometry_msgs::Pose& p)
+{
+    visualization_msgs::Marker marker;
+    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
+    marker.header.frame_id = "camera_depth_optical_frame";
+    marker.header.stamp = ros::Time::now();
+
+    // Set the namespace and id for this marker
+    marker.ns = "grasp_pose";
+    marker.id = 0;
+
+    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
+    marker.type = visualization_msgs::Marker::CUBE;
+
+    // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
+    marker.action = visualization_msgs::Marker::ADD;
+
+    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
+    marker.pose.position.x = p.position.x;
+    marker.pose.position.y = p.position.y;
+    marker.pose.position.z = p.position.z;
+    marker.pose.orientation.x = p.orientation.x;
+    marker.pose.orientation.y = p.orientation.y;
+    marker.pose.orientation.z = p.orientation.z;
+    marker.pose.orientation.w = p.orientation.w;
+
+    // Set the scale of the marker -- 1x1x1 here means 1m on a side
+    marker.scale.x = dimensions[0];
+    marker.scale.y = dimensions[1];
+    marker.scale.z = dimensions[2];
+
+    // Set the color -- be sure to set alpha to something non-zero!
+    marker.color.r = 1.0f;
+    marker.color.g = 0.0f;
+    marker.color.b = 0.0f;
+    marker.color.a = 0.5f;
+
+    // Publish the marker
+    marker.lifetime = ros::Duration();
+    marker_pub.publish(marker);
+}
+
 void publish_pose(Eigen::Matrix4d H)
 {
     // Set translation
@@ -113,6 +162,9 @@ void publish_pose(Eigen::Matrix4d H)
     p.orientation.z = quaternion.z();
     p.orientation.w = quaternion.w();
 
+    // Publish visualization marker
+    publish_grasp_marker(p);
+
     // Broadcast the transforms
     static tf::TransformBroadcaster br;
     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "camera_depth_optical_frame", "object_frame"));
@@ -141,24 +193,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
         pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -173,21 +224,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;
+                 << 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();
         }
     }
 }
@@ -200,11 +252,11 @@ void pcl_callback(const sensor_msgs::PointCloud2ConstPtr& input)
         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";
         template_pub.publish(template_msg);
-        publish_bounding_box(icp_transform);
+        // publish_bounding_box(icp_transform);
         publish_pose(icp_transform);
     }
 }
@@ -274,7 +326,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;
@@ -299,10 +351,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
         pcl::PCLPointCloud2::Ptr object_cluster(new pcl::PCLPointCloud2);
@@ -323,7 +380,7 @@ bool service_callback(object_detection::ObjectDetection::Request& req,
         pcl_pub.publish(output);
 
         // Read template point cloud
-        string template_filename = "/home/karmesh/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) {
             PCL_ERROR("Couldn't read the template PCL file");
@@ -332,9 +389,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;
     }
 }
 
@@ -351,6 +434,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);
@@ -365,7 +449,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);
@@ -377,6 +462,7 @@ int main(int argc, char** argv)
     bbox_pub = nh.advertise<sensor_msgs::PointCloud2>("/icp/bbox_points", 1);
     template_pub = nh.advertise<sensor_msgs::PointCloud2>("/icp/template", 1);
     pose_pub = nh.advertise<geometry_msgs::Pose>("/icp/pose", 1);
+    marker_pub = nh.advertise<visualization_msgs::Marker>("object_pose_detection/grasp_pose", 1);
 
     // Spin
     ros::spin();