Adding 3d to 2d & ICP cpp files

Author: ahemaesh

.gitignore

@@ -1,5 +1,7 @@
 cuboid_detection/bags/*
 
+.vscode/
+
 # Prerequisites
 *.d
 

cuboid_detection/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
   pcl_ros
   roscpp
   sensor_msgs
+  color_object_detection
 )
 
 ## System dependencies are found with CMake's conventions
@@ -146,7 +147,8 @@ include
 # add_executable(${PROJECT_NAME}_node src/cuboid_detection_node.cpp)
 # add_executable(example src/example.cpp)
 add_executable(ground_plane_segmentation src/ground_plane_segmentation.cpp)
-
+add_executable(iterative_closest_point src/iterative_closest_point.cpp)
+add_executable(three_dimension_to_two_dimension src/three_dimension_to_two_dimension.cpp)
 ## <<<<<<<<<<<<<<<<<<<<<<<
 
 ## Rename C++ executable without prefix
@@ -168,3 +170,6 @@ add_executable(ground_plane_segmentation src/ground_plane_segmentation.cpp)
 # )
 # target_link_libraries(example ${catkin_LIBRARIES})
 target_link_libraries(ground_plane_segmentation ${catkin_LIBRARIES})
+target_link_libraries(iterative_closest_point ${catkin_LIBRARIES})
+target_link_libraries(three_dimension_to_two_dimension ${catkin_LIBRARIES})
+

cuboid_detection/launch/three_dimension_to_two_dimension.launch

@@ -0,0 +1,36 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+    <!-- Launch Ground Plane Segmentation file -->
+    <include file="$(find cuboid_detection)/launch/ground_plane_segmentation.launch"/>
+    
+    <!-- Lau -->
+    <node
+        pkg="cuboid_detection"
+        type="three_dimension_to_two_dimension"
+        name="three_dimension_to_two_dimension"
+        output="screen">
+
+        <!-- Set topics and params -->
+        <!-- <rosparam> -->
+            <!-- invert (default: true): True > Box; False > Ground Plane -->
+            <!-- invert: true -->
+            <!-- voxel_size (default: 0.01): Lower the value, denser the point cloud, more CPU -->
+            <!-- voxel_size: 0.005 -->
+            <!-- distance_threshold (default: 0.01): Lower the value, more of box + table noise -->
+            <!-- distance_threshold: 0.015 -->
+            <!-- Topics -->
+            <!-- input: "/camera/depth/color/points"
+            output: "/ground_plane_segmentation/points"
+            plane_coefficients: "/ground_plane_segmentation/coefficients"
+        </rosparam> -->
+    </node>
+
+     <node
+        pkg="color_object_detection"
+        type="object_detection.py"
+        name="object_detection"
+        output="screen">
+    </node>
+
+</launch>

cuboid_detection/src/iterative_closest_point.cpp

@@ -0,0 +1,68 @@
+/*
+ *********************************************************
+ * *******************************************************
+ * ***********  Iterative Closest Point (ICP) ************
+ * *******************************************************
+ * ************ written by Avinash on 04-02-2019 *********
+ *********************************************************
+ */
+
+
+#include <iostream>
+#include <ros/ros.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 <sensor_msgs/PointCloud2.h>
+
+
+typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+static ros::Publisher icp_PCL_pub;
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr template_cuboid(new pcl::PointCloud<pcl::PointXYZ>);
+pcl::PointCloud<pcl::PointXYZ>::Ptr input_cuboid(new pcl::PointCloud<pcl::PointXYZ>);
+
+int performICP()
+{
+    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_PCL_pub.publish(icp.getFinalTransformation());
+
+    return (0);
+}
+
+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();
+}
+
+int main(int argc, char **argv)
+{
+    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);
+    }
+    
+    ros::spinOnce();
+    return 0;
+}