Read camera matrix from camera_info topic

This makes DOPE work with any ROS camera driver, without having to specify the camera intrinsics in the config file.
kini5gowda · Jun 13, 2019 · 39a16c3 · 39a16c3
1 parent a481e0a
commit 39a16c3
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 38 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -10,6 +10,7 @@ project(dope)
 find_package(catkin REQUIRED COMPONENTS
   cv_bridge
   geometry_msgs
+  message_filters
   rospy
   std_msgs
   tf2

diff --git a/config/config_pose.yaml b/config/config_pose.yaml
@@ -1,4 +1,5 @@
 topic_camera: "/dope/webcam_rgb_raw"
+topic_camera_info: "/dope/camera_info"
 topic_publishing: "dope"
 
 # Comment any of these lines to prevent detection / pose estimation of that object
@@ -34,17 +35,6 @@ draw_colors: {
 
 }
 
-# Camera intrinsics (Logitech C920)
-camera_settings: {
-    "name": "logitech_c920",
-    "width": 640,
-    "height": 480,
-    "fx": 641.5,
-    "fy": 641.5,
-    "cx": 320.0,
-    "cy": 240.0
-}
-
 # Config params for DOPE
 thresh_angle: 0.5
 thresh_map: 0.01

diff --git a/nodes/dope b/nodes/dope
@@ -11,6 +11,7 @@ listening to an image topic and publishing poses.
 
 from __future__ import print_function
 
+import message_filters
 import numpy as np
 import rospkg
 import rospy
@@ -22,7 +23,7 @@ from dope.inference.cuboid import Cuboid3d
 from dope.inference.cuboid_pnp_solver import CuboidPNPSolver
 from dope.inference.detector import ModelData, ObjectDetector
 from geometry_msgs.msg import PoseStamped
-from sensor_msgs.msg import Image as ImageSensor_msg
+from sensor_msgs.msg import CameraInfo, Image as ImageSensor_msg
 from std_msgs.msg import String
 
 
@@ -98,17 +99,6 @@ class DopeNode(object):
         self.dimensions = {}
         self.cv_bridge = CvBridge()
 
-        # Initialize parameters
-        matrix_camera = np.zeros((3, 3))
-        matrix_camera[0, 0] = params["camera_settings"]['fx']
-        matrix_camera[1, 1] = params["camera_settings"]['fy']
-        matrix_camera[0, 2] = params["camera_settings"]['cx']
-        matrix_camera[1, 2] = params["camera_settings"]['cy']
-        matrix_camera[2, 2] = 1
-        dist_coeffs = np.zeros((4, 1))
-
-        if "dist_coeffs" in params["camera_settings"]:
-            dist_coeffs = np.array(params["camera_settings"]['dist_coeffs'])
         self.config_detect = lambda: None
         self.config_detect.mask_edges = 1
         self.config_detect.mask_faces = 1
@@ -137,9 +127,7 @@ class DopeNode(object):
             self.pnp_solvers[model] = \
                 CuboidPNPSolver(
                     model,
-                    matrix_camera,
-                    Cuboid3d(params['dimensions'][model]),
-                    dist_coeffs=dist_coeffs
+                    cuboid3d=Cuboid3d(params['dimensions'][model])
                 )
             self.pubs[model] = \
                 rospy.Publisher(
@@ -162,21 +150,31 @@ class DopeNode(object):
                 queue_size=10
             )
 
-        topic_cam = params['topic_camera']
-
-        # Starts ROS listener
-        rospy.Subscriber(
-            topic_cam,
-            ImageSensor_msg,
-            self.image_callback,
-            queue_size=1
+        # Start ROS subscriber
+        image_sub = message_filters.Subscriber(
+            params['topic_camera'],
+            ImageSensor_msg
+        )
+        info_sub = message_filters.Subscriber(
+            params['topic_camera_info'],
+            CameraInfo
         )
+        ts = message_filters.TimeSynchronizer([image_sub, info_sub], 1)
+        ts.registerCallback(self.image_callback)
 
-        print("Running DOPE...  (Listening to camera topic: '{}')".format(topic_cam))
+        print("Running DOPE...  (Listening to camera topic: '{}')".format(params['topic_camera']))
         print("Ctrl-C to stop")
 
-    def image_callback(self, image_msg):
+    def image_callback(self, image_msg, camera_info):
         """Image callback"""
+
+        # Update camera matrix
+        P = np.matrix(camera_info.P, dtype='float64')
+        P.resize((3, 4))
+        camera_matrix = P[:, :3]
+        for m in self.models:
+            self.pnp_solvers[m].set_camera_intrinsic_matrix(camera_matrix)
+
         img = self.cv_bridge.imgmsg_to_cv2(image_msg, "rgb8")
         # cv2.imwrite('img.png', cv2.cvtColor(img, cv2.COLOR_BGR2RGB))  # for debugging
 

diff --git a/package.xml b/package.xml
@@ -51,6 +51,7 @@
   <buildtool_depend>catkin</buildtool_depend>
   <depend>cv_bridge</depend>
   <depend>geometry_msgs</depend>
+  <depend>message_filters</depend>
   <depend>rospy</depend>
   <depend>std_msgs</depend>
   <depend>tf2</depend>

diff --git a/readme.md b/readme.md
@@ -65,12 +65,12 @@ This is the official DOPE ROS package for detection and 6-DoF pose estimation of
   
 
 3. **Edit config info** (if desired) in `~/catkin_ws/src/dope/config/config_pose.yaml`
-    * `topic_camera`: RGB topic to listen to
+    * `topic_camera`: RGB topic to listen to (must be rectified)
+    * `topic_camera_info`: camera info topic to listen to
     * `topic_publishing`: topic name for publishing
     * `weights`: dictionary of object names and there weights path name, **comment out any line to disable detection/estimation of that object**
     * `dimension`: dictionary of dimensions for the objects  (key values must match the `weights` names)
     * `draw_colors`: dictionary of object colors  (key values must match the `weights` names)
-    * `camera_settings`: dictionary for the camera intrinsics; edit these values to match your camera
     * `thresh_points`: Thresholding the confidence for object detection; increase this value if you see too many false positives, reduce it if  objects are not detected. 
     
 4. **Start DOPE node**