Fixed build

Author: heethesh

object_detection/src/ground_plane_segmentation.cpp

@@ -44,6 +44,8 @@ target_link_libraries(ground_plane_segmentation ${catkin_LIBRARIES})
 #include <pcl_ros/transforms.h>
 #include <pcl_conversions/pcl_conversions.h>
 
+using namespace std;
+
 // Publishers
 ros::Publisher pcl_pub;
 ros::Publisher coef_pub;
@@ -55,9 +57,9 @@ ros::Publisher pose_pub;
 bool invert;
 double voxel_size;
 double distance_threshold;
-std::string input_topic;
-std::string output_topic;
-std::string coefficients_topic;
+string input_topic;
+string output_topic;
+string coefficients_topic;
 
 Eigen::Matrix4d icp_transform;
 string template_cuboid_filename;
@@ -76,6 +78,81 @@ bool ICP_SUCCESS = false;
 bool FIRST = true;
 bool POSE_FLAG = false;
 
+void publish_pose(Eigen::Matrix4d H)
+{
+    // Set translation
+    tf::Vector3 origin;
+    origin.setValue(H(0, 3), H(1, 3), H(2, 3));
+    
+    // Set rotation
+    tf::Quaternion quaternion;
+    tf::Matrix3x3 rotation_matrix;
+    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
+    tf::Transform transform;
+    transform.setOrigin(origin);
+    transform.setRotation(quaternion);
+
+    // Make pose message
+    geometry_msgs::Pose p;
+    p.position.x = H(0, 3);
+    p.position.y = H(1, 3);
+    p.position.z = H(2, 3);
+    p.orientation.x = quaternion.x();
+    p.orientation.y = quaternion.y();
+    p.orientation.z = quaternion.z();
+    p.orientation.w = quaternion.w();
+
+    // Broadcast the transforms
+    static tf::TransformBroadcaster br;
+    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "camera_depth_optical_frame", "icp_cuboid_frame"));
+    pose_pub.publish(p);
+}
+
+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(-l/2, -w/2, -h/2));
+    box_cloud.push_back(pcl::PointXYZ(-l/2, -w/2, h/2));
+    box_cloud.push_back(pcl::PointXYZ(-l/2, w/2, -h/2));
+    box_cloud.push_back(pcl::PointXYZ(-l/2, w/2, h/2));
+    box_cloud.push_back(pcl::PointXYZ(l/2, -w/2, -h/2));
+    box_cloud.push_back(pcl::PointXYZ(l/2, -w/2, h/2));
+    box_cloud.push_back(pcl::PointXYZ(l/2, w/2, -h/2));
+    box_cloud.push_back(pcl::PointXYZ(l/2, w/2, h/2));
+
+    // 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 icp_registration(const sensor_msgs::PointCloud2::ConstPtr& msg)
 {
     // Compute ICP only once
@@ -152,23 +229,23 @@ void callback(const sensor_msgs::PointCloud2ConstPtr& input)
     pcl::PCLPointCloud2::Ptr cloud_filtered_ptr_z(new pcl::PCLPointCloud2);
     pcl::PCLPointCloud2::Ptr cloud_filtered_ptr(new pcl::PCLPointCloud2);
     pcl_conversions::toPCL(*input, *cloud_ptr);
-    if (DEBUG) std::cerr << "PointCloud before filtering: " << cloud_ptr->width << " " << cloud_ptr->height << " data points." << std::endl;
+    if (DEBUG) cerr << "PointCloud before filtering: " << cloud_ptr->width << " " << cloud_ptr->height << " data points." << endl;
 
     // Filter the points in z-axis
     pcl::PassThrough<pcl::PCLPointCloud2> pass_z;
     pass_z.setInputCloud(cloud_ptr);
     pass_z.setFilterFieldName("z");
     pass_z.setFilterLimits(0.0, 0.9);
     pass_z.filter(*cloud_filtered_ptr_z);
-    if (DEBUG) std::cerr << "PointCloud after filtering: " << cloud_filtered_ptr_z->width << " " << cloud_filtered_ptr->height << " data points." << std::endl;
+    if (DEBUG) cerr << "PointCloud after filtering: " << cloud_filtered_ptr_z->width << " " << cloud_filtered_ptr->height << " data points." << endl;
 
     // Filter the points in y-axis
     pcl::PassThrough<pcl::PCLPointCloud2> pass;
     pass.setInputCloud(cloud_filtered_ptr_z);
     pass.setFilterFieldName("x");
     pass.setFilterLimits(-0.2, 0.2);
     pass.filter(*cloud_filtered_ptr);
-    if (DEBUG) std::cerr << "PointCloud after filtering in x-axis: " << cloud_filtered_ptr->width << " " << cloud_filtered_ptr->height << " data points." << std::endl;
+    if (DEBUG) cerr << "PointCloud after filtering in x-axis: " << cloud_filtered_ptr->width << " " << cloud_filtered_ptr->height << " data points." << endl;
 
     // Downsample the points
     pcl::PCLPointCloud2::Ptr voxel_ptr(new pcl::PCLPointCloud2);
@@ -204,7 +281,7 @@ void callback(const sensor_msgs::PointCloud2ConstPtr& input)
     extract.setIndices(inliers);
     extract.setNegative(invert);
     extract.filter(*plane_cloud_ptr);
-    if (DEBUG) std::cerr << "PointCloud representing the planar component: " << plane_cloud_ptr->width << " " << plane_cloud_ptr->height << " data points." << std::endl;
+    if (DEBUG) cerr << "PointCloud representing the planar component: " << plane_cloud_ptr->width << " " << plane_cloud_ptr->height << " data points." << endl;
 
     // Additional filtering
     pcl::PCLPointCloud2::Ptr cloud_filtered_ptr_z2(new pcl::PCLPointCloud2);
@@ -213,7 +290,7 @@ void callback(const sensor_msgs::PointCloud2ConstPtr& input)
     pass_z2.setFilterFieldName("z");
     pass_z2.setFilterLimits(0.0, 0.7);
     pass_z2.filter(*cloud_filtered_ptr_z2);
-    if (DEBUG) std::cerr << "PointCloud after filtering: " << cloud_filtered_ptr_z2->width << " " << cloud_filtered_ptr->height << " data points." << std::endl;
+    if (DEBUG) cerr << "PointCloud after filtering: " << cloud_filtered_ptr_z2->width << " " << cloud_filtered_ptr->height << " data points." << endl;
 
     // Create the KdTree object for the search method of the extraction
     pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
@@ -222,7 +299,7 @@ void callback(const sensor_msgs::PointCloud2ConstPtr& input)
     tree->setInputCloud(pcl_cloud_cleaned);
 
     // Create the extraction object for the clusters
-    std::vector<pcl::PointIndices> object_cluster_indices;
+    vector<pcl::PointIndices> object_cluster_indices;
     // pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
     pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
     // specify euclidean cluster parameters
@@ -235,10 +312,10 @@ void callback(const sensor_msgs::PointCloud2ConstPtr& input)
     ec.extract(object_cluster_indices);
 
     // Display number of objects detceted
-    std::cerr << "Objects Detected: " << object_cluster_indices.size() << std::endl;
+    cerr << "Objects Detected: " << object_cluster_indices.size() << endl;
 
     // What is this?
-    for (std::vector<pcl::PointIndices>::const_iterator it = object_cluster_indices.begin(); it != object_cluster_indices.end(); ++it)
+    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);
@@ -257,7 +334,7 @@ void callback(const sensor_msgs::PointCloud2ConstPtr& input)
 
         // now we are in a vector of indices pertaining to a single cluster.
         // Assign each point corresponding to this cluster in xyzCloudPtrPassthroughFiltered a specific color for identification purposes
-        // for (std::vector<int>::const_iterator idx = it->indices.begin(); idx != it->indices.end(); ++idx)
+        // for (vector<int>::const_iterator idx = it->indices.begin(); idx != it->indices.end(); ++idx)
         // {
         //     object_cluster->points.push_back(cloud_filtered_ptr_z2->points[*idx]);
         // }
@@ -298,17 +375,17 @@ void callback(const sensor_msgs::PointCloud2ConstPtr& input)
 //     nh.param<bool>("invert", invert, true);
 //     nh.param<double>("voxel_size", voxel_size, 0.01);
 //     nh.param<double>("distance_threshold", distance_threshold, 0.01);
-//     nh.param<std::string>("input", input_topic, "/camera/depth/color/points");
-//     nh.param<std::string>("output", output_topic, "/ground_plane_segmentation/points");
-//     nh.param<std::string>("plane_coefficients", coefficients_topic, "/ground_plane_segmentation/coefficients");
+//     nh.param<string>("input", input_topic, "/camera/depth/color/points");
+//     nh.param<string>("output", output_topic, "/ground_plane_segmentation/points");
+//     nh.param<string>("plane_coefficients", coefficients_topic, "/ground_plane_segmentation/coefficients");
 
 //     // Display params
-//     std::cout << "\nInvert Segmentation: " << invert << std::endl;
-//     std::cout << "Voxel Size: " << voxel_size << std::endl;
-//     std::cout << "Distance Threshold: " << distance_threshold << std::endl;
-//     std::cout << "Input Topic: " << input_topic << std::endl;
-//     std::cout << "Output Topic: " << output_topic << std::endl;
-//     std::cout << "Co-efficients Topic: " << coefficients_topic << std::endl;
+//     cout << "\nInvert Segmentation: " << invert << endl;
+//     cout << "Voxel Size: " << voxel_size << endl;
+//     cout << "Distance Threshold: " << distance_threshold << endl;
+//     cout << "Input Topic: " << input_topic << endl;
+//     cout << "Output Topic: " << output_topic << endl;
+//     cout << "Co-efficients Topic: " << coefficients_topic << endl;
 
 //     // Create a ROS subscriber for the input point cloud
 //     ros::Subscriber sub = nh.subscribe(input_topic, 1, callback);
@@ -328,7 +405,7 @@ int main(int argc, char **argv)
     ros::NodeHandle nh("~");
 
     template_pub = nh.advertise<sensor_msgs::PointCloud2>("/icp/template", 1);
-    std::string template_cuboid_filename = "";
+    string template_cuboid_filename = "";
 
     while(1)
     {
@@ -340,7 +417,7 @@ int main(int argc, char **argv)
         if (pcl::io::loadPCDFile<pcl::PointXYZ>(template_cuboid_filename, *template_cuboid) == -1)
         {
             PCL_ERROR("Couldn't read the template PCL file");
-            return;
+            return 0;
         }
 
         // Convert to ROS data type