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 
00038 # Need to explicitly enable 'with' in python 2.5
00039 from __future__ import with_statement
00040 
00041 PKG = 'approach_table_tools'
00042 NAME = 'image_cb_detector_node'
00043 import roslib; roslib.load_manifest(PKG)
00044 
00045 import rospy
00046 import cv
00047 import math
00048 import threading
00049 import numpy
00050 import tf
00051 
00052 from sensor_msgs.msg import Image, CameraInfo
00053 from geometry_msgs.msg import PoseStamped
00054 
00055 from cv_bridge import CvBridge, CvBridgeError
00056 from approach_table_tools.srv import GetCheckerboardPose, GetCheckerboardPoseResponse
00057 from image_geometry import PinholeCameraModel
00058 
00059 class ImageCbDetector:
00060   def get_board_corners(self, corners, corners_x, corners_y):
00061     return (corners[0], corners[corners_x  - 1], 
00062         corners[(corners_y - 1) * corners_x], corners[len(corners) - 1])
00063 
00064   def detect(self, image, corners_x, corners_y, spacing_x, spacing_y, width_scaling, height_scaling):
00065     #resize the image base on the scaling parameters we've been configured with
00066     scaled_width = int(.5 + image.width * width_scaling)
00067     scaled_height = int(.5 + image.height * height_scaling)
00068     
00069     #in cvMat its row, col so height comes before width
00070     image_scaled = cv.CreateMat(scaled_height, scaled_width, cv.GetElemType(image))
00071     cv.Resize(image, image_scaled, cv.CV_INTER_LINEAR)
00072 
00073     #Here, we'll actually call the openCV detector    
00074     found, corners = cv.FindChessboardCorners(image_scaled, (corners_x, corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH)
00075 
00076     if found:
00077       board_corners = self.get_board_corners(corners, corners_x, corners_y)
00078       
00079       #find the perimeter of the checkerboard
00080       perimeter = 0.0
00081       for i in range(len(board_corners)):
00082         next = (i + 1) % 4
00083         xdiff = board_corners[i][0] - board_corners[next][0]
00084         ydiff = board_corners[i][1] - board_corners[next][1]
00085         perimeter += math.sqrt(xdiff * xdiff + ydiff * ydiff)
00086 
00087       #estimate the square size in pixels
00088       square_size = perimeter / ((corners_x - 1 + corners_y - 1) * 2)
00089       radius = int(square_size * 0.5 + 0.5)
00090 
00091       corners = cv.FindCornerSubPix(image_scaled, corners, (radius, radius), (-1, -1), (cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 30, 0.1))
00092 
00093       #uncomment to debug chessboard detection
00094       #cv.DrawChessboardCorners(image_scaled, (corners_x, corners_y), corners, 1)
00095       #cv.NamedWindow("image_scaled")
00096       #cv.ShowImage("image_scaled", image_scaled)
00097       #cv.WaitKey(600)
00098 
00099       object_points = None
00100 
00101       #we'll also generate the object points if the user has specified spacing
00102       if spacing_x != None and spacing_y != None:
00103         object_points = cv.CreateMat(3, corners_x * corners_y, cv.CV_32FC1)
00104 
00105         for y in range(corners_y):
00106           for x in range(corners_x):
00107             cv.SetReal2D(object_points, 0, y*corners_x + x, x * spacing_x)
00108             cv.SetReal2D(object_points, 1, y*corners_x + x, y * spacing_y)
00109             cv.SetReal2D(object_points, 2, y*corners_x + x, 0.0)
00110 
00111       #not sure why opencv functions return non opencv compatible datatypes... but they do so we'll convert
00112       corners_cv = cv.CreateMat(2, corners_x * corners_y, cv.CV_32FC1)
00113       for i in range(corners_x * corners_y):
00114         cv.SetReal2D(corners_cv, 0, i, corners[i][0])
00115         cv.SetReal2D(corners_cv, 1, i, corners[i][1])
00116 
00117       return (corners_cv, object_points)
00118 
00119     else:
00120       #cv.NamedWindow("image_scaled")
00121       #cv.ShowImage("image_scaled", image_scaled)
00122       #cv.WaitKey(600)
00123       rospy.logwarn("Didn't find checkerboard")
00124       return (None, None)
00125 
00126 class ImageCbDetectorNode:
00127   def __init__(self):
00128     self.mutex = threading.RLock()
00129     self.corners_x = rospy.get_param('~corners_x', 6)
00130     self.corners_y = rospy.get_param('~corners_y', 6)
00131     self.spacing_x = rospy.get_param('~spacing_x', 0.022)
00132     self.spacing_y = rospy.get_param('~spacing_y', 0.022)
00133     self.publish_period = rospy.Duration(rospy.get_param('~publish_period', 0))
00134     self.last_publish_time = rospy.Time()
00135     self.width_scaling = rospy.get_param('~width_scaling', 1)
00136     self.height_scaling = rospy.get_param('~height_scaling', 1)
00137 
00138     self.im_cb_detector = ImageCbDetector()
00139 
00140     self.pose_srv = None
00141     self.bridge = CvBridge()
00142     self.cam_info = None
00143     self.ros_image = None
00144 
00145     self.image_sub = rospy.Subscriber("image_stream", Image, self.callback)
00146     self.caminfo_sub = rospy.Subscriber("camera_info", CameraInfo, self.cam_info_cb) 
00147     self.pose_pub = rospy.Publisher("board_pose", PoseStamped)
00148 
00149   def intrinsic_matrix_from_info(self, cam_info):
00150     intrinsic_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
00151 
00152     #Because we only want the upper 3x3 (normal) portion of the rectified intrinsic matrix
00153     for i in range(0, 3):
00154       for j in range(0, 3):
00155         intrinsic_matrix[i, j] = cam_info.P[4*i+j]
00156 
00157     return intrinsic_matrix
00158 
00159   def attempt_to_advertise(self):
00160     #we'll only advertise the service after we've received a cam_info/image pair
00161     if self.ros_image != None and self.cam_info != None:
00162       self.pose_srv = rospy.Service("get_checkerboard_pose", GetCheckerboardPose, self.find_checkerboard_service)
00163 
00164   def cam_info_cb(self, cam_info):
00165     with self.mutex:
00166       self.cam_info = cam_info
00167       if self.pose_srv == None:
00168         self.attempt_to_advertise()
00169 
00170   def callback(self, ros_image):
00171     #copy over the latest image to be used when a service call is made
00172     with self.mutex:
00173       self.ros_image = ros_image
00174       if self.pose_srv == None:
00175         self.attempt_to_advertise()
00176       if rospy.Duration() != self.publish_period and rospy.rostime.get_rostime() - self.last_publish_time > self.publish_period:
00177         pose = self.find_checkerboard_pose(ros_image, self.corners_x, self.corners_y, self.spacing_x, self.spacing_y, self.width_scaling, self.height_scaling)
00178         if pose:
00179           self.pose_pub.publish(pose)    
00180           self.last_publish_time = rospy.rostime.get_rostime()
00181 
00182   def find_checkerboard_service(self, req):
00183     #copy the image over
00184     with self.mutex:
00185       image = self.ros_image
00186 
00187     pose = self.find_checkerboard_pose(image, req.corners_x, req.corners_y, req.spacing_x, req.spacing_y, self.width_scaling, self.height_scaling)
00188     return GetCheckerboardPoseResponse(pose)
00189 
00190   def find_checkerboard_pose(self, ros_image, corners_x, corners_y, spacing_x, spacing_y, width_scaling, height_scaling):
00191     #we need to convert the ros image to an opencv image
00192     try:
00193       image = self.bridge.imgmsg_to_cv(ros_image, "mono8")
00194     except CvBridgeError, e:
00195       rospy.logerror("Error importing image %s" % e)
00196       return
00197 
00198     corners, model = self.im_cb_detector.detect(image, corners_x, corners_y, spacing_x, spacing_y, width_scaling, height_scaling)
00199 
00200     if corners != None and model != None and self.cam_info != None:
00201       #find the pose of the checkerboard
00202       rot = cv.CreateMat(3, 1, cv.CV_32FC1)
00203       trans = cv.CreateMat(3, 1, cv.CV_32FC1)
00204       with self.mutex:
00205         kc = cv.CreateMat(1, 4, cv.CV_32FC1)
00206         cv.Set(kc, 0.0)
00207         intrinsic_matrix = self.intrinsic_matrix_from_info(self.cam_info)
00208 
00209         cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc, rot, trans)
00210 
00211       #We want to build a transform now, but first we have to convert the 
00212       #rotation vector we get back from OpenCV into a rotation matrix
00213       rot_mat = cv.CreateMat(3, 3, cv.CV_32FC1)
00214       cv.Rodrigues2(rot, rot_mat)
00215 
00216       #Now we need to convert this rotation matrix into a quaternion
00217       #This can be done better in never versions of opencv, but we need to be boxturtle compatible
00218       numpy_mat = numpy.fromstring(rot_mat.tostring(), dtype = numpy.float32).reshape((3,3))
00219 
00220       #of course, tf expects all matricies passed to it to be 4x4... so we'll build a full matrix here
00221       full_pose = numpy.zeros((4, 4))
00222 
00223       #first, copy in the rotation matrix
00224       full_pose[0:3, 0:3] = numpy_mat[0:3, 0:3]
00225 
00226       #next, we'll copy in the translation
00227       full_pose[0:3, 3] = [trans[i, 0] for i in range(3)]
00228 
00229       #and make sure to add a 1 in the lower right corner
00230       full_pose[3][3] = 1.0
00231 
00232       rospy.logdebug("%s" % numpy_mat)
00233       rospy.logdebug("%s" % full_pose)
00234 
00235       tf_trans = tf.transformations.translation_from_matrix(full_pose)
00236       tf_rot = tf.transformations.quaternion_from_matrix(full_pose)
00237 
00238       #and now we'll actually build our pose stamped
00239       board_pose = PoseStamped()
00240       board_pose.header = ros_image.header
00241       board_pose.pose.position.x = tf_trans[0]
00242       board_pose.pose.position.y = tf_trans[1]
00243       board_pose.pose.position.z = tf_trans[2]
00244       board_pose.pose.orientation.x = tf_rot[0]
00245       board_pose.pose.orientation.y = tf_rot[1]
00246       board_pose.pose.orientation.z = tf_rot[2]
00247       board_pose.pose.orientation.w = tf_rot[3]
00248       rospy.logdebug("%s" % board_pose)
00249 
00250       #we'll publish the pose so we can display it in rviz
00251       self.pose_pub.publish(board_pose)
00252       return board_pose
00253 
00254 def cb_detector_main(argv=None):
00255   rospy.init_node(NAME, anonymous=False)
00256   cb_detector = ImageCbDetectorNode()
00257   rospy.spin()
00258 
00259 if __name__ == '__main__':
00260   cb_detector_main()
00261 


approach_table_tools
Author(s):
autogenerated on Mon Oct 6 2014 08:50:22