Merge pull request #2 from dash-robotics/develop_avi

Bounding box filter completed

Author: heethesh

.gitignore

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

color_object_detection/src/object_detection.py

@@ -1,25 +1,26 @@
 #!/usr/bin/env python
 from __future__ import print_function
 
-import roslib
 import sys
-import rospy
+
 import cv2
+import numpy as np
+import rospy
+import roslib
 from std_msgs.msg import String
 from sensor_msgs.msg import Image
 from cv_bridge import CvBridge, CvBridgeError
-import numpy as np
 
 from color_object_detection.msg import Rectangle
 
-class image_converter:
+class ImageConverter:
 
   def __init__(self):
-    self.image_pub = rospy.Publisher("/camera/color/image_raw/segmented",Image, queue_size=10)
-    self.bbox_pub = rospy.Publisher("/bbox", Rectangle, queue_size=1000)
+    self.image_pub = rospy.Publisher("/object_detection/image_segmented",Image, queue_size=10)
+    self.bbox_pub = rospy.Publisher("/object_detection/bbox", Rectangle, queue_size=100)
 
     self.bridge = CvBridge()
-    self.image_sub = rospy.Subscriber("/camera/color/image_raw",Image,self.callback)
+    self.image_sub = rospy.Subscriber("/camera/color/image_raw", Image, self.callback)
 
   def callback(self,data):
     try:
@@ -31,39 +32,40 @@ def callback(self,data):
     hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
 
     # define range of blue color in HSV
-    lower_red1 = np.array([0,50,100])
-    upper_red1 = np.array([10,255,255])
+    lower_red1 = np.array([0, 50, 100])
+    upper_red1 = np.array([10, 255, 255])
 
-    lower_red2 = np.array([175,50,100])
-    upper_red2 = np.array([180,255,255])
+    lower_red2 = np.array([175, 50, 100])
+    upper_red2 = np.array([180, 255, 255])
 
     # Threshold the HSV image to get only blue colors
     mask1 = cv2.inRange(hsv_image, lower_red1, upper_red1)
     mask2 = cv2.inRange(hsv_image, lower_red2, upper_red2)
     mask = cv2.bitwise_or(mask1,mask2)
-    kernel = np.ones((5,5),np.uint8)
+    kernel = np.ones((5, 5), np.uint8)
 
-    mask = cv2.erode(mask,kernel,iterations = 2)
-    mask = cv2.dilate(mask,kernel,iterations = 2)
+    mask = cv2.erode(mask, kernel, iterations=2)
+    mask = cv2.dilate(mask, kernel, iterations=2)
 
-    ret,thresh = cv2.threshold(mask,200,255,0)
+    ret,thresh = cv2.threshold(mask, 200, 255, 0)
     im, contours,hierarchy= cv2.findContours(thresh, 1, 2)
 
     c = max(contours, key = cv2.contourArea)
 
     cnt = contours[0]
-    x,y,w,h = cv2.boundingRect(c)
+    x, y, w, h = cv2.boundingRect(c)
 
     # Bitwise-AND mask and original image
     # res1 = cv2.bitwise_and(cv_image,cv_image, mask= mask1)
     # res2 = cv2.bitwise_and(cv_image,cv_image, mask= mask2)
     # res = cv2.bitwise_or(res1, res2)
 
-    rect = Rectangle(x,y,x+w,y+h)
+    d = 10
+    rect = Rectangle(x - d, y - d, x + w + d, y + h + d)
     self.bbox_pub.publish(rect)
 
-    res = cv2.bitwise_and(cv_image,cv_image, mask= mask)
-    cv2.rectangle(res,(x,y),(x+w,y+h),(0,255,0),2)
+    res = cv2.bitwise_and(cv_image, cv_image, mask= mask)
+    cv2.rectangle(res, (x - d, y - d), (x + w + d, y + h + d), (0, 255, 0), 2)
 
     # cv2.imshow("Image window", res)
     # cv2.waitKey(3)
@@ -74,8 +76,8 @@ def callback(self,data):
       print(e)
 
 def main(args):
-  rospy.init_node('image_converter', anonymous=True)
-  ic = image_converter()
+  rospy.init_node('object_detector', anonymous=True)
+  ic = ImageConverter()
 
   try:
     rospy.spin()
@@ -84,4 +86,4 @@ def main(args):
   cv2.destroyAllWindows()
 
 if __name__ == '__main__':
-    main(sys.argv)
+    main(sys.argv)

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
@@ -139,15 +140,9 @@ include
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-
-## >>>>>>>>>>>>>>>>>>>>>>>
-## ADD YOUR PROGRAMS HERE
-
-# 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(bbox_filter src/bbox_filter.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -160,11 +155,6 @@ add_executable(ground_plane_segmentation src/ground_plane_segmentation.cpp)
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
-# target_link_libraries(${PROJECT_NAME}
-#   ${OpenCV_LIBS}
-# )
-# 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(bbox_filter ${catkin_LIBRARIES})

cuboid_detection/launch/bbox_filter.launch

@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+    <!-- Launch ground plane segmentation -->
+    <include file="$(find cuboid_detection)/launch/ground_plane_segmentation.launch"/>
+
+    <!-- Detect 2D bbox -->
+    <node
+        pkg="color_object_detection"
+        type="object_detection.py"
+        name="object_detection"
+        output="screen">
+    </node>
+    
+    <!-- Filter points within the bbox -->
+    <node
+        pkg="cuboid_detection"
+        type="bbox_filter"
+        name="bbox_filter"
+        output="screen">
+    </node>
+
+</launch>

cuboid_detection/src/bbox_filter.cpp

@@ -0,0 +1,127 @@
+/*
+Author: Avinash
+Version: 1.0.2
+Date: Apr 03, 2019
+*/
+
+#include <iostream>
+#include <vector>
+#include <ros/ros.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl_ros/filters/extract_indices.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <color_object_detection/Rectangle.h>
+
+using namespace std;
+
+// Flags
+bool DEBUG = false;
+bool READ_INFO = true;
+
+int bbox[4];
+int min_inlier[2];
+int max_inlier[2];
+
+ros::Publisher pcl_pub;
+vector<double> proj_matrix;
+
+bool within_bbox(float x, float y, float z)
+{
+    // Check if projection matrix is read
+    if (READ_INFO) return false;
+
+    // 3D to 2D projection
+    float u = (proj_matrix[0] * x) + (proj_matrix[1] * y) + (proj_matrix[2] * z) + proj_matrix[3];
+    float v = (proj_matrix[4] * x) + (proj_matrix[5] * y) + (proj_matrix[6] * z) + proj_matrix[7];
+    float w = (proj_matrix[8] * x) + (proj_matrix[9] * y) + (proj_matrix[10] * z) + proj_matrix[11];
+    
+    // Normalize the uv points
+    u /= w;
+    v /= w;
+
+    // Check if these points are within the 2D bbox
+    if (bbox[0] < u && u < bbox[2]) {
+        if (bbox[1] < v && v < bbox[3]) {
+            return true;
+        }
+    }
+    return false;
+}
+
+/****************** Callbacks *********************/
+
+void info_cb(const sensor_msgs::CameraInfoConstPtr& camInfo_msg)
+{
+    if (READ_INFO)
+    {
+        cerr << "\nCamera Info:" << endl;
+        for (int i = 0; i < 12; i++)
+        {
+            cerr << camInfo_msg->P[i] << " ";
+            proj_matrix.push_back(camInfo_msg->P[i]);
+        }
+        READ_INFO = false;
+    }
+}
+
+void bbox_cb(const color_object_detection::Rectangle msg)
+{
+    bbox[0] = msg.x1;
+    bbox[1] = msg.y1;
+    bbox[2] = msg.x2;
+    bbox[3] = msg.y2;
+}
+
+void pcl_cb(const sensor_msgs::PointCloud2ConstPtr& input)
+{
+    // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
+    pcl::PCLPointCloud2::Ptr input_cloud_ptr(new pcl::PCLPointCloud2);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl_conversions::toPCL(*input, *input_cloud_ptr);
+    pcl::fromPCLPointCloud2(*input_cloud_ptr, *pcl_cloud_ptr);
+
+    // Store the inliers
+    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
+
+    // Check inliers for all points `in the PCL
+    for (int i = 0; i < pcl_cloud_ptr->size(); i++)
+    {
+        if (within_bbox(pcl_cloud_ptr->points[i].x, pcl_cloud_ptr->points[i].y, pcl_cloud_ptr->points[i].z))
+        {
+            inliers->indices.push_back(i);
+        }
+    }
+
+    // Extract the inliers
+    pcl::PCLPointCloud2::Ptr output_ptr(new pcl::PCLPointCloud2);
+    pcl::ExtractIndices<pcl::PCLPointCloud2> extract;
+    extract.setInputCloud(input_cloud_ptr);
+    extract.setIndices(inliers);
+    extract.setNegative(false);
+    extract.filter(*output_ptr);
+
+    // Convert to ROS data type
+    sensor_msgs::PointCloud2 output;
+    pcl_conversions::fromPCL(*output_ptr, output);
+    pcl_pub.publish(output);
+}
+
+int main(int argc, char** argv)
+{
+    // Setup node
+    ros::init(argc, argv, "bbox_filter");
+    ros::NodeHandle nh;
+
+    // Subscribers
+    ros::Subscriber bbox_sub = nh.subscribe("/object_detection/bbox", 1, bbox_cb);
+    ros::Subscriber info_sub = nh.subscribe("/camera/color/camera_info", 1, info_cb);
+    ros::Subscriber pcl_sub = nh.subscribe("/ground_plane_segmentation/points", 1, pcl_cb);
+
+    // Publisher
+    pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/bbox_filter/points", 1);
+
+    // Spin
+    ros::spin();
+}

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;
+}