Final submission

Author: heethesh

cuboid_detection/CMakeLists.txt

@@ -8,13 +8,17 @@ add_compile_options(-std=c++11)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
+  rospy
+  roscpp
+  pcl_ros
+  pcl_conversions
   cv_bridge
   image_transport
-  pcl_conversions
-  pcl_ros
-  roscpp
+  std_msgs
   sensor_msgs
-  color_object_detection
+  geometry_msgs
+  message_generation
+  eigen_conversions
 )
 
 ## System dependencies are found with CMake's conventions
@@ -54,11 +58,10 @@ include_directories(${OpenCV_INCLUDE_DIRS})
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+  FILES
+  Rectangle.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -75,10 +78,11 @@ include_directories(${OpenCV_INCLUDE_DIRS})
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   sensor_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  sensor_msgs
+  std_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -112,7 +116,7 @@ include_directories(${OpenCV_INCLUDE_DIRS})
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES cuboid_detection
- CATKIN_DEPENDS cv_bridge image_transport pcl_conversions pcl_ros roscpp sensor_msgs
+ CATKIN_DEPENDS cv_bridge image_transport pcl_conversions pcl_ros roscpp sensor_msgs rospy std_msgs geometry_msgs message_generation eigen_conversions
  # DEPENDS system_lib
 )
 

cuboid_detection/launch/bbox_filter.launch

@@ -3,11 +3,10 @@
 
     <!-- Launch ground plane segmentation -->
     <include file="$(find cuboid_detection)/launch/ground_plane_segmentation.launch"/>
-    <!-- <include file="$(find cuboid_detection)/launch/surface_normal_estimation.launch"/> -->
 
     <!-- Detect 2D bbox -->
     <node
-        pkg="color_object_detection"
+        pkg="cuboid_detection"
         type="object_detection.py"
         name="object_detection"
         output="screen">

cuboid_detection/launch/iterative_closest_point.launch

@@ -3,9 +3,10 @@
 
     <!-- Launch Bounding Box Filter -->
     <include file="$(find cuboid_detection)/launch/bbox_filter.launch"/>
-     <include file="$(find cuboid_detection)/launch/surface_normal_estimation.launch"/>
-
 
+    <!-- Launch surface normal estimation -->
+    <include file="$(find cuboid_detection)/launch/surface_normal_estimation.launch"/>
+
     <!-- Run ICP node -->
     <node
         pkg="cuboid_detection"
@@ -26,7 +27,23 @@
             width: 0.075
             height: 0.1
         </rosparam>
+    </node>
+
+    <!-- Display the 3D bounding box -->
+    <node
+        pkg="cuboid_detection"
+        type="draw_bbox.py"
+        name="draw_bbox"
+        output="screen">
 
+        <!-- Set topics and params -->
+        <rosparam>
+            camera_info_topic: "/camera/color/camera_info"
+            image_color_topic: "/camera/color/image_raw"
+            bbox_points_topic: "/icp/bbox_points"
+            extrinsics: "/camera/extrinsics/depth_to_color"
+            output_topic: "/draw_bbox/image"
+        </rosparam>
     </node>
 
 </launch>

cuboid_detection/msg/Rectangle.msg

@@ -0,0 +1,4 @@
+int64 x1
+int64 y1
+int64 x2
+int64 y2

cuboid_detection/package.xml

@@ -55,19 +55,40 @@
   <build_depend>roscpp</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>libpcl-all-dev</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>vision_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>eigen_conversions</build_depend>
+  
   <build_export_depend>cv_bridge</build_export_depend>
   <build_export_depend>image_transport</build_export_depend>
   <build_export_depend>pcl_conversions</build_export_depend>
   <build_export_depend>pcl_ros</build_export_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>vision_msgs</build_export_depend>
+  <build_export_depend>message_generation</build_export_depend>
+  <build_export_depend>eigen_conversions</build_export_depend>
+  
   <exec_depend>libpcl-all</exec_depend>
   <exec_depend>cv_bridge</exec_depend>
   <exec_depend>image_transport</exec_depend>
   <exec_depend>pcl_conversions</exec_depend>
   <exec_depend>pcl_ros</exec_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>vision_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>message_generation</exec_depend>
+  <exec_depend>eigen_conversions</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

cuboid_detection/scripts/draw_bbox.py

@@ -0,0 +1,128 @@
+#!/usr/bin/env python
+
+# Python 2/3 compatibility
+from __future__ import print_function
+
+# Built-in modules
+import os
+import sys
+
+# External modules
+import cv2
+import numpy as np
+
+# ROS modules
+import rospy
+import ros_numpy
+import message_filters
+from cv_bridge import CvBridge, CvBridgeError
+from sensor_msgs.msg import Image, CameraInfo, PointCloud2
+from realsense2_camera.msg import Extrinsics
+
+# Globals
+CV_BRIDGE = CvBridge()
+BBOX_POINTS = None
+EXTRINSICS = None
+
+
+def ext_callback(ext_msg):
+    global EXTRINSICS
+    if EXTRINSICS is None:
+        extrinsic_matrix = np.eye(4)
+        extrinsic_matrix[:3, :3] = np.asarray(ext_msg.rotation).reshape(3, 3)
+        extrinsic_matrix[:3, -1] = np.asarray(ext_msg.translation)
+        EXTRINSICS = extrinsic_matrix
+        print('\nExtrinsics:\n', EXTRINSICS)
+
+
+def bbox_callback(bbox_msg):
+    global BBOX_POINTS
+    BBOX_POINTS = ros_numpy.point_cloud2.pointcloud2_to_array(bbox_msg)
+    BBOX_POINTS = np.asarray(BBOX_POINTS.tolist())
+
+
+def callback(img_msg, info_msg, image_pub):
+    # Read image using CV bridge
+    try:
+        img = CV_BRIDGE.imgmsg_to_cv2(img_msg, 'bgr8')
+    except CvBridgeError as e: 
+        rospy.logerr(e)
+        return
+
+    # Extract points from message
+    if (BBOX_POINTS is None) or (EXTRINSICS is None): return
+    bbox3D = BBOX_POINTS
+    bbox3D = np.hstack((bbox3D, np.full((bbox3D.shape[0], 1), 1)))
+    
+    # Project 3D to 2D and filter bbox within image boundaries
+    P = np.matrix(info_msg.P).reshape(3, 4)
+    M = np.matmul(P, EXTRINSICS)
+    bbox2D = np.matmul(M, bbox3D.T).T
+    bbox2D = bbox2D / bbox2D[:, -1]
+    bbox2D = bbox2D[:, :2].astype('int').tolist()
+
+    # Draw the projected 3D bbox
+    color = (0, 255, 0)
+    cv2.line(img, tuple(bbox2D[0]), tuple(bbox2D[1]), color, 2)
+    cv2.line(img, tuple(bbox2D[0]), tuple(bbox2D[2]), color, 2)
+    cv2.line(img, tuple(bbox2D[0]), tuple(bbox2D[4]), color, 2)
+    cv2.line(img, tuple(bbox2D[1]), tuple(bbox2D[3]), color, 2)
+    cv2.line(img, tuple(bbox2D[1]), tuple(bbox2D[5]), color, 2)
+    cv2.line(img, tuple(bbox2D[2]), tuple(bbox2D[3]), color, 2)
+    cv2.line(img, tuple(bbox2D[2]), tuple(bbox2D[6]), color, 2)
+    cv2.line(img, tuple(bbox2D[3]), tuple(bbox2D[7]), color, 2)
+    cv2.line(img, tuple(bbox2D[4]), tuple(bbox2D[5]), color, 2)
+    cv2.line(img, tuple(bbox2D[4]), tuple(bbox2D[6]), color, 2)
+    cv2.line(img, tuple(bbox2D[5]), tuple(bbox2D[7]), color, 2)
+    cv2.line(img, tuple(bbox2D[6]), tuple(bbox2D[7]), color, 2)
+
+    # Publish the projected bbox image
+    try:
+        image_pub.publish(CV_BRIDGE.cv2_to_imgmsg(img, "bgr8"))
+    except CvBridgeError as e: 
+        rospy.logerr(e)
+
+
+def listener():
+    # Start node
+    rospy.init_node('draw_bbox', anonymous=True)
+    
+    # Handle params
+    camera_info = rospy.get_param('~camera_info_topic')
+    image_color = rospy.get_param('~image_color_topic')
+    bbox_points = rospy.get_param('~bbox_points_topic')
+    extrinsics = rospy.get_param('~extrinsics')
+    output_topic = rospy.get_param('~output_topic')
+    
+    # Log info
+    rospy.loginfo('Current PID: [%d]' % os.getpid())
+    rospy.loginfo('CameraInfo topic: %s' % camera_info)
+    rospy.loginfo('Image topic: %s' % image_color)
+    rospy.loginfo('Bbox topic: %s' % bbox_points)
+    rospy.loginfo('Extrinsics topic: %s' % extrinsics)
+    rospy.loginfo('Output topic: %s' % output_topic)
+
+    # Subscribe to topics
+    info_sub = message_filters.Subscriber(camera_info, CameraInfo)
+    image_sub = message_filters.Subscriber(image_color, Image)
+    bbox_sub = rospy.Subscriber(bbox_points, PointCloud2, bbox_callback)
+    ext_sub = rospy.Subscriber(extrinsics, Extrinsics, ext_callback)
+
+    # Publish output topic
+    image_pub = rospy.Publisher(output_topic, Image, queue_size=5)
+
+    # Synchronize the topics by time
+    ats = message_filters.ApproximateTimeSynchronizer(
+        [image_sub, info_sub], queue_size=10, slop=0.1)
+    ats.registerCallback(callback, image_pub)
+
+    # Keep python from exiting until this node is stopped
+    try:
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        rospy.loginfo('Shutting down')
+
+
+if __name__ == '__main__':
+    # Start node
+    listener()

cuboid_detection/scripts/object_detection.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python
+from __future__ import print_function
+
+import sys
+
+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
+
+from color_object_detection.msg import Rectangle
+
+class ImageConverter:
+
+  def __init__(self):
+    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)
+
+  def callback(self,data):
+    try:
+      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
+    except CvBridgeError as e:
+      print(e)
+
+    (rows,cols,channels) = cv_image.shape
+    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_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)
+
+    mask = cv2.erode(mask, kernel, iterations=2)
+    mask = cv2.dilate(mask, kernel, iterations=2)
+
+    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)
+
+    # 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)
+
+    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 - d, y - d), (x + w + d, y + h + d), (0, 255, 0), 2)
+
+    # cv2.imshow("Image window", res)
+    # cv2.waitKey(3)
+
+    try:
+      self.image_pub.publish(self.bridge.cv2_to_imgmsg(res, "bgr8"))
+    except CvBridgeError as e:
+      print(e)
+
+def main(args):
+  rospy.init_node('object_detector', anonymous=True)
+  ic = ImageConverter()
+  
+  try:
+    rospy.spin()
+  except KeyboardInterrupt:
+    print("Shutting down")
+  cv2.destroyAllWindows()
+
+if __name__ == '__main__':
+    main(sys.argv)