Merge pull request #3 from dash-robotics/icp_debug

ICP works now

Author: heethesh

cuboid_detection/launch/iterative_closest_point.launch

@@ -0,0 +1,21 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+    <!-- 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">
+    </node>
+
+</launch>

cuboid_detection/src/bbox_filter.cpp

@@ -56,10 +56,10 @@ void info_cb(const sensor_msgs::CameraInfoConstPtr& camInfo_msg)
 {
     if (READ_INFO)
     {
-        cerr << "\nCamera Info:" << endl;
+        if (DEBUG) cerr << "\nCamera Info:" << endl;
         for (int i = 0; i < 12; i++)
         {
-            cerr << camInfo_msg->P[i] << " ";
+            if (DEBUG) cerr << camInfo_msg->P[i] << " ";
             proj_matrix.push_back(camInfo_msg->P[i]);
         }
         READ_INFO = false;

cuboid_detection/src/ground_plane_segmentation.cpp

@@ -52,7 +52,7 @@ void callback(const sensor_msgs::PointCloud2ConstPtr& input)
     pcl::PassThrough<pcl::PCLPointCloud2> pass;
     pass.setInputCloud(cloud_ptr);
     pass.setFilterFieldName("z");
-    pass.setFilterLimits(0.0, 1.4);
+    pass.setFilterLimits(0.0, 0.9);
     pass.filter(*cloud_filtered_ptr);
     if (debug) std::cerr << "PointCloud after filtering: " << cloud_filtered_ptr->width << " " << cloud_filtered_ptr->height << " data points." << std::endl;
 

cuboid_detection/src/iterative_closest_point.cpp

@@ -1,68 +1,135 @@
 /*
- *********************************************************
- * *******************************************************
- * ***********  Iterative Closest Point (ICP) ************
- * *******************************************************
- * ************ written by Avinash on 04-02-2019 *********
- *********************************************************
- */
-
+Author: Avinash
+Version: 1.0.1
+Date: Apr 03, 2019
+*/
 
 #include <iostream>
 #include <ros/ros.h>
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
 #include <pcl_ros/point_cloud.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
 #include <pcl/registration/icp.h>
-#include <sensor_msgs/point_cloud_conversion.h>
-#include <sensor_msgs/PointCloud.h>
+#include <pcl_ros/transforms.h>
 #include <sensor_msgs/PointCloud2.h>
 
+using namespace std;
+
+// Globals  
+ros::Publisher pcl_pub;
+ros::Publisher template_pub;
+Eigen::Matrix4d icp_transform;
+string template_cuboid_filename;
+tf::TransformListener *tf_listener; 
+sensor_msgs::PointCloud2 output_msg;
+sensor_msgs::PointCloud2 template_msg;
+
+// Flags
+bool DEBUG = false;
+bool ICP_SUCCESS = false;
 
-typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
-static ros::Publisher icp_PCL_pub;
+void convert_icp_eigen_to_tf(Eigen::Matrix4d Tm)
+{   
+    // Set translation
+    tf::Vector3 origin;
+    origin.setValue(Tm(0, 3), Tm(1, 3), Tm(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.getRotation(quaternion);
+
+    // Make tf transform message
+    tf::Transform transform;
+    transform.setOrigin(origin);
+    transform.setRotation(quaternion);
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr template_cuboid(new pcl::PointCloud<pcl::PointXYZ>);
-pcl::PointCloud<pcl::PointXYZ>::Ptr input_cuboid(new pcl::PointCloud<pcl::PointXYZ>);
+    // Broadcast the transforms
+    static tf::TransformBroadcaster br;
+    // br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "cuboid_frame", "camera_depth_frame"));
+}
 
-int performICP()
+void icp_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
 {
-    pcl::IterativeClosestPoint <pcl::PointXYZ, pcl::PointXYZ> icp;
+    // Compute ICP only once
+    if (ICP_SUCCESS)
+    {
+        convert_icp_eigen_to_tf(icp_transform);
+        pcl_pub.publish(output_msg);
+        template_msg.header.frame_id = "camera_depth_optical_frame";
+        template_pub.publish(template_msg);
+        return;
+    }
+
+    // Point cloud containers
+    pcl::PointCloud<pcl::PointXYZ>::Ptr template_cuboid(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cuboid(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+    // Read input point cloud
+    pcl::fromROSMsg(*msg, *input_cuboid);
+    
+    // Read template point cloud
+    if (pcl::io::loadPCDFile<pcl::PointXYZ>(template_cuboid_filename, *template_cuboid) == -1)
+    {
+        PCL_ERROR("Couldn't read the template PCL file");
+        return;
+    }
+    
+    // Run ICP
+    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
     icp.setInputSource(input_cuboid);
     icp.setInputTarget(template_cuboid);
-    pcl::PointCloud <pcl::PointXYZ> Final;
-    icp.align(Final);
-    std::cout << "has converged:" << icp.hasConverged() << " score: " <<
-              icp.getFitnessScore() << std::endl;
-    std::cout << icp.getFinalTransformation() << std::endl;
+    icp.setMaximumIterations(2000);
+    icp.setTransformationEpsilon(1e-9);
+    // icp.setMaxCorrespondenceDistance(0.05);
+    // icp.setEuclideanFitnessEpsilon(1);
+    // icp.setRANSACOutlierRejectionThreshold(1.5);
+    icp.align(*output_cloud);
+    icp_transform = icp.getFinalTransformation().cast<double>().inverse();
+
+    // Convert ICP results and broadcast TF
+    if (icp.hasConverged())
+    {
+        cerr << "\nICP Score: " << icp.getFitnessScore() << endl;
+        cerr << "ICP Transform:\n" << icp_transform << endl;
 
-    //icp_PCL_pub.publish(icp.getFinalTransformation());
+        ICP_SUCCESS = true;
+        convert_icp_eigen_to_tf(icp_transform);
 
-    return (0);
-}
+        // Publish template point cloud
+        template_msg.header.frame_id = "camera_depth_optical_frame";
+        pcl_pub.publish(output_msg);
+        template_pub.publish(template_msg);
 
-void rawPCL_cb(const sensor_msgs::PointCloud2::ConstPtr& msg)
-{
-    pcl::PCLPointCloud2 pcl_pc2;
-    pcl_conversions::toPCL(*msg,pcl_pc2);
-    pcl::fromPCLPointCloud2(pcl_pc2,*input_cuboid);
-    performICP();
+        // Convert to ROS data type
+        pcl::toROSMsg(*output_cloud, output_msg);
+        pcl::toROSMsg(*template_cuboid, template_msg);
+    }
 }
 
 int main(int argc, char **argv)
 {
-    ros::init(argc,argv,"iterative_closest_point");
+    // Init node
+    ros::init(argc, argv, "iterative_closest_point");
     ros::NodeHandle nh;
-    //icp_PCL_pub = nh.advertise<geometry_msgs::>("iterative_closest_point", 1000);
-    ros::Subscriber raw_PCL_sub = nh.subscribe<sensor_msgs::PointCloud2>("/ground_plane_segmentation/points", 1000, rawPCL_cb);
 
-    if (pcl::io::loadPCDFile<pcl::PointXYZ> ("$/{workspaceFolder}/src/perception/cuboid_detection/templates/template_cuboid_L200_W100_H75.pcd", *template_cuboid) == -1) //* load the file
-    {
-        PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
-        return (-1);
-    }
+    // Handle params
+    nh.getParam("template_cuboid_path", template_cuboid_filename);
+    cerr << "\nTemplate filename: " << template_cuboid_filename;
 
-    ros::spinOnce();
-    return 0;
+    // 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);
+    template_pub = nh.advertise<sensor_msgs::PointCloud2>("/icp/template", 1);
+
+    ros::spin();
 }