cb_detector.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 #***********************************************************
00003 #* Software License Agreement (BSD License)
00004 #*
00005 #*  Copyright (c) 2009, Willow Garage, Inc.
00006 #*  All rights reserved.
00007 #*
00008 #*  Redistribution and use in source and binary forms, with or without
00009 #*  modification, are permitted provided that the following conditions
00010 #*  are met:
00011 #*
00012 #*   * Redistributions of source code must retain the above copyright
00013 #*     notice, this list of conditions and the following disclaimer.
00014 #*   * Redistributions in binary form must reproduce the above
00015 #*     copyright notice, this list of conditions and the following
00016 #*     disclaimer in the documentation and/or other materials provided
00017 #*     with the distribution.
00018 #*   * Neither the name of Willow Garage, Inc. nor the names of its
00019 #*     contributors may be used to endorse or promote products derived
00020 #*     from this software without specific prior written permission.
00021 #*
00022 #*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 #*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 #*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 #*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 #*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 #*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 #*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 #*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 #*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 #*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 #*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 #*  POSSIBILITY OF SUCH DAMAGE.
00034 #* 
00035 #* Author: Eitan Marder-Eppstein
00036 #***********************************************************
00037 PKG = 'kinect_depth_calibration'
00038 NAME = 'image_cb_detector_node'
00039 import roslib; roslib.load_manifest(PKG)
00040 
00041 import rospy
00042 import cv
00043 import math
00044 import threading
00045 import numpy
00046 import tf
00047 import PyKDL
00048 
00049 from sensor_msgs.msg import Image, CameraInfo
00050 from geometry_msgs.msg import PoseStamped
00051 from tf_conversions import posemath
00052 
00053 from cv_bridge import CvBridge, CvBridgeError
00054 from kinect_depth_calibration.srv import GetCheckerboardPose, GetCheckerboardPoseResponse
00055 
00056 class ImageCbDetector:
00057   def get_board_corners(self, corners, corners_x, corners_y):
00058     return (corners[0], corners[corners_x  - 1], 
00059         corners[(corners_y - 1) * corners_x], corners[len(corners) - 1])
00060 
00061   def detect(self, image, corners_x, corners_y, spacing_x, spacing_y, width_scaling, height_scaling):
00062     #resize the image base on the scaling parameters we've been configured with
00063     scaled_width = int(.5 + image.width * width_scaling)
00064     scaled_height = int(.5 + image.height * height_scaling)
00065     
00066     #in cvMat its row, col so height comes before width
00067     image_scaled = cv.CreateMat(scaled_height, scaled_width, cv.GetElemType(image))
00068     cv.Resize(image, image_scaled, cv.CV_INTER_LINEAR)
00069 
00070     #Here, we'll actually call the openCV detector    
00071     found, corners = cv.FindChessboardCorners(image_scaled, (corners_x, corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH)
00072 
00073     if found:
00074       board_corners = self.get_board_corners(corners, corners_x, corners_y)
00075       
00076       #find the perimeter of the checkerboard
00077       perimeter = 0.0
00078       for i in range(len(board_corners)):
00079         next = (i + 1) % 4
00080         xdiff = board_corners[i][0] - board_corners[next][0]
00081         ydiff = board_corners[i][1] - board_corners[next][1]
00082         perimeter += math.sqrt(xdiff * xdiff + ydiff * ydiff)
00083 
00084       #estimate the square size in pixels
00085       square_size = perimeter / ((corners_x - 1 + corners_y - 1) * 2)
00086       radius = int(square_size * 0.5 + 0.5)
00087 
00088       corners = cv.FindCornerSubPix(image_scaled, corners, (radius, radius), (-1, -1), (cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 30, 0.1))
00089 
00090       #uncomment to debug chessboard detection
00091       print 'Chessboard found'
00092       #cv.DrawChessboardCorners(image_scaled, (corners_x, corners_y), corners, 1)
00093       #cv.NamedWindow("image_scaled")
00094       #cv.ShowImage("image_scaled", image_scaled)
00095       #cv.WaitKey(600)
00096 
00097       object_points = None
00098 
00099       #we'll also generate the object points if the user has specified spacing
00100       if spacing_x != None and spacing_y != None:
00101         object_points = cv.CreateMat(3, corners_x * corners_y, cv.CV_32FC1)
00102 
00103         for y in range(corners_y):
00104           for x in range(corners_x):
00105             cv.SetReal2D(object_points, 0, y*corners_x + x, x * spacing_x)
00106             cv.SetReal2D(object_points, 1, y*corners_x + x, y * spacing_y)
00107             cv.SetReal2D(object_points, 2, y*corners_x + x, 0.0)
00108 
00109       #not sure why opencv functions return non opencv compatible datatypes... but they do so we'll convert
00110       corners_cv = cv.CreateMat(2, corners_x * corners_y, cv.CV_32FC1)
00111       for i in range(corners_x * corners_y):
00112         cv.SetReal2D(corners_cv, 0, i, corners[i][0])
00113         cv.SetReal2D(corners_cv, 1, i, corners[i][1])
00114 
00115       return (corners_cv, object_points)
00116 
00117     else:
00118       #cv.NamedWindow("image_scaled")
00119       #cv.ShowImage("image_scaled", image_scaled)
00120       #cv.WaitKey(600)
00121       rospy.logwarn("Didn't find checkerboard")
00122       return (None, None)
00123 
00124 class ImageCbDetectorNode:
00125   def __init__(self):
00126     self.corners_x = rospy.get_param('~corners_x', 6)
00127     self.corners_y = rospy.get_param('~corners_y', 6)
00128     self.spacing_x = rospy.get_param('~spacing_x', None)
00129     self.spacing_y = rospy.get_param('~spacing_y', None)
00130     self.width_scaling = rospy.get_param('~width_scaling', 1)
00131     self.height_scaling = rospy.get_param('~height_scaling', 1)
00132 
00133     self.im_cb_detector = ImageCbDetector()
00134 
00135     self.image_sub = rospy.Subscriber("image_stream", Image, self.callback)
00136     self.caminfo_sub = rospy.Subscriber("camera_info", CameraInfo, self.cam_info_cb) 
00137     self.pose_pub = rospy.Publisher("board_pose", PoseStamped, latch=True)
00138     self.pose_srv = None
00139     self.bridge = CvBridge()
00140     self.mutex = threading.RLock()
00141     self.cam_info = None
00142     self.ros_image = None
00143 
00144   def intrinsic_matrix_from_info(self, cam_info):
00145     intrinsic_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
00146 
00147     #Because we only want the upper 3x3 (normal) portion of the rectified intrinsic matrix
00148     for i in range(0, 3):
00149       for j in range(0, 3):
00150         intrinsic_matrix[i, j] = cam_info.P[4*i+j]
00151 
00152     return intrinsic_matrix
00153 
00154   def attempt_to_advertise(self):
00155     #we'll only advertise the service after we've received a cam_info/image pair
00156     if self.ros_image != None and self.cam_info != None:
00157       self.pose_srv = rospy.Service("get_checkerboard_pose", GetCheckerboardPose, self.find_checkerboard_service)
00158 
00159   def cam_info_cb(self, cam_info):
00160     with self.mutex:
00161       self.cam_info = cam_info
00162       if self.pose_srv == None:
00163         self.attempt_to_advertise()
00164 
00165   def callback(self, ros_image):
00166     #copy over the latest image to be used when a service call is made
00167     with self.mutex:
00168       self.ros_image = ros_image
00169       if self.pose_srv == None:
00170         self.attempt_to_advertise()
00171 
00172   def find_checkerboard_service(self, req):
00173     poses = []
00174     min_x = self.ros_image.width
00175     min_y = self.ros_image.height
00176     max_x = 0
00177     max_y = 0
00178     for i in range(10):
00179         rospy.sleep(0.5)
00180 
00181         #copy the image over
00182         with self.mutex:
00183           image = self.ros_image
00184 
00185         result = self.find_checkerboard_pose(image, req.corners_x, 
00186                                                  req.corners_y, req.spacing_x, 
00187                                                  req.spacing_y, self.width_scaling, 
00188                                                  self.height_scaling)
00189         if result is None:
00190             rospy.logerr("Couldn't find a checkerboard")
00191             continue
00192 
00193         p, corners = result
00194         poses.append(p)
00195         for j in range(corners.cols):
00196             x = cv.GetReal2D(corners, 0, j)
00197             y = cv.GetReal2D(corners, 1, j)
00198             min_x = min(min_x, x)
00199             max_x = max(max_x, x)
00200             min_y = min(min_y, y)
00201             max_y = max(max_y, y)
00202 
00203     # convert all poses to twists
00204     twists = []
00205     for p in poses:
00206       twists.append(PyKDL.diff(PyKDL.Frame.Identity(), posemath.fromMsg(p.pose)))
00207 
00208     # get average twist 
00209     twist_res = PyKDL.Twist()
00210     PyKDL.SetToZero(twist_res)
00211     for t in twists:
00212       for i in range(3):
00213         twist_res.vel[i] += t.vel[i]/len(poses)
00214         twist_res.rot[i] += t.rot[i]/len(poses)
00215 
00216     # get noise
00217     noise_rot = 0
00218     noise_vel = 0
00219     for t in twists:
00220       n_vel = (t.vel - twist_res.vel).Norm()
00221       n_rot = (t.rot - twist_res.rot).Norm()
00222       if n_vel > noise_vel:
00223         noise_vel = n_vel
00224       if n_rot > noise_rot:
00225         noise_rot = n_rot
00226 
00227     # set service resul
00228     pose_res = PyKDL.addDelta(PyKDL.Frame.Identity(), twist_res, 1.0)
00229     pose_msg = PoseStamped()
00230     pose_msg.header = poses[0].header
00231     pose_msg.pose = posemath.toMsg(pose_res)
00232     return GetCheckerboardPoseResponse(pose_msg, min_x, max_x, min_y, max_y, noise_vel, noise_rot)
00233 
00234 
00235   def find_checkerboard_pose(self, ros_image, corners_x, corners_y, spacing_x, spacing_y, width_scaling, height_scaling):
00236     #we need to convert the ros image to an opencv image
00237     try:
00238       image = self.bridge.imgmsg_to_cv(ros_image, "mono8")
00239     except CvBridgeError, e:
00240       rospy.logerror("Error importing image %s" % e)
00241       return
00242 
00243     corners, model = self.im_cb_detector.detect(image, corners_x, corners_y, spacing_x, spacing_y, width_scaling, height_scaling)
00244 
00245     if corners != None and model != None and self.cam_info != None:
00246       #find the pose of the checkerboard
00247       rot = cv.CreateMat(3, 1, cv.CV_32FC1)
00248       trans = cv.CreateMat(3, 1, cv.CV_32FC1)
00249       with self.mutex:
00250         intrinsic_matrix = self.intrinsic_matrix_from_info(self.cam_info)
00251         kc = cv.CreateMat(1, 4, cv.CV_32FC1)
00252         cv.Set(kc, 0.0)
00253         cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc, rot, trans)
00254 
00255       #We want to build a transform now, but first we have to convert the 
00256       #rotation vector we get back from OpenCV into a rotation matrix
00257       rot_mat = cv.CreateMat(3, 3, cv.CV_32FC1)
00258       cv.Rodrigues2(rot, rot_mat)
00259 
00260       #Now we need to convert this rotation matrix into a quaternion
00261       #This can be done better in never versions of opencv, but we need to be boxturtle compatible
00262       numpy_mat = numpy.fromstring(rot_mat.tostring(), dtype = numpy.float32).reshape((3,3))
00263 
00264       #of course, tf expects all matricies passed to it to be 4x4... so we'll build a full matrix here
00265       full_pose = numpy.zeros((4, 4))
00266 
00267       #first, copy in the rotation matrix
00268       full_pose[0:3, 0:3] = numpy_mat[0:3, 0:3]
00269 
00270       #next, we'll copy in the translation
00271       full_pose[0:3, 3] = [trans[i, 0] for i in range(3)]
00272 
00273       #and make sure to add a 1 in the lower right corner
00274       full_pose[3][3] = 1.0
00275 
00276       rospy.logdebug("%s" % numpy_mat)
00277       rospy.logdebug("%s" % full_pose)
00278 
00279       tf_trans = tf.transformations.translation_from_matrix(full_pose)
00280       tf_rot = tf.transformations.quaternion_from_matrix(full_pose)
00281 
00282       #and now we'll actually build our pose stamped
00283       board_pose = PoseStamped()
00284       board_pose.header = ros_image.header
00285       board_pose.pose.position.x = tf_trans[0]
00286       board_pose.pose.position.y = tf_trans[1]
00287       board_pose.pose.position.z = tf_trans[2]
00288       board_pose.pose.orientation.x = tf_rot[0]
00289       board_pose.pose.orientation.y = tf_rot[1]
00290       board_pose.pose.orientation.z = tf_rot[2]
00291       board_pose.pose.orientation.w = tf_rot[3]
00292       rospy.logdebug("%s" % board_pose)
00293 
00294       #we'll publish the pose so we can display it in rviz
00295       self.pose_pub.publish(board_pose)
00296       return (board_pose, corners)
00297 
00298 def cb_detector_main(argv=None):
00299   rospy.init_node(NAME, anonymous=False)
00300   cb_detector = ImageCbDetectorNode()
00301   rospy.spin()
00302 
00303 if __name__ == '__main__':
00304   cb_detector_main()
00305 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


kinect_depth_calibration
Author(s): Eitan Marder-Eppstein
autogenerated on Thu Aug 15 2013 10:18:38