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 = 'image_cb_detector'
00038 NAME = 'image_cb_detector_node'
00039 import roslib; roslib.load_manifest(PKG)
00040 
00041 import rospy
00042 import cv
00043 import math
00044 
00045 from sensor_msgs.msg import Image
00046 from image_cb_detector.msg import ObjectInImage
00047 
00048 from cv_bridge import CvBridge, CvBridgeError
00049 
00050 class ImageCbDetector:
00051   def __init__(self, corners_x, corners_y, spacing_x = None, spacing_y = None, width_scaling = 1, height_scaling = 1):
00052     self.corners_x = corners_x
00053     self.corners_y = corners_y
00054     self.spacing_x = spacing_x
00055     self.spacing_y = spacing_y
00056     self.width_scaling = width_scaling
00057     self.height_scaling = height_scaling
00058 
00059   def get_board_corners(self, corners):
00060     return (corners[0], corners[self.corners_x  - 1], 
00061         corners[(self.corners_y - 1) * self.corners_x], corners[len(corners) - 1])
00062 
00063   def detect(self, image):
00064     #resize the image base on the scaling parameters we've been configured with
00065     scaled_width = int(.5 + image.width * self.width_scaling)
00066     scaled_height = int(.5 + image.height * self.height_scaling)
00067     
00068     #in cvMat its row, col so height comes before width
00069     image_scaled = cv.CreateMat(scaled_height, scaled_width, cv.GetElemType(image))
00070     cv.Resize(image, image_scaled, cv.CV_INTER_LINEAR)
00071 
00072     
00073     found, corners = cv.FindChessboardCorners(image_scaled, (self.corners_x, self.corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH)
00074 
00075     if found:
00076       rospy.logdebug("Found cb")
00077       board_corners = self.get_board_corners(corners)
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 / ((self.corners_x - 1 + self.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       #cv.DrawChessboardCorners(image_scaled, (self.corners_x, self.corners_y), corners, 1)
00093       #cv.NamedWindow("image_scaled")
00094       #cv.ShowImage("image_scaled", image_scaled)
00095       #cv.WaitKey()
00096 
00097       object_points = None
00098 
00099       #we'll also generate the object points if they've been requested
00100       if self.spacing_x != None and self.spacing_y != None:
00101         object_points = [None] * (self.corners_x * self.corners_y)
00102 
00103         for i in range(self.corners_y):
00104           for j in range(self.corners_x):
00105             object_points[i * self.corners_x + j]  = (j * self.spacing_x, i * self.spacing_y)
00106 
00107       return (corners, object_points)
00108 
00109     else:
00110       rospy.logdebug("Didn't find checkerboard")
00111       return (None, None)
00112 
00113 class ImageCbDetectorNode:
00114   def __init__(self):
00115     corners_x = rospy.get_param('~corners_x', 6)
00116     corners_y = rospy.get_param('~corners_y', 6)
00117     spacing_x = rospy.get_param('~spacing_x', None)
00118     spacing_y = rospy.get_param('~spacing_y', None)
00119     width_scaling = rospy.get_param('~width_scaling', 1)
00120     height_scaling = rospy.get_param('~height_scaling', 1)
00121 
00122     self.im_cb_detector = ImageCbDetector(corners_x, corners_y, spacing_x, 
00123         spacing_y, width_scaling, height_scaling)
00124 
00125     rospy.Subscriber("image_stream", Image, self.callback)
00126     self.corner_pub = rospy.Publisher("corners_in_image", ObjectInImage)
00127     self.bridge = CvBridge()
00128 
00129   def callback(self, ros_image):
00130     #we need to convert the ros image to an opencv image
00131     try:
00132       image = self.bridge.imgmsg_to_cv(ros_image, "mono8")
00133     except CvBridgeError, e:
00134       rospy.logerror("Error importing image %s" % e)
00135       return
00136 
00137     corners, model = self.im_cb_detector.detect(image)
00138 
00139     #now we need to publish them out over ros
00140     obj_msg = ObjectInImage()
00141     obj_msg.header = ros_image.header
00142 
00143     if corners != None:
00144       obj_msg.set_image_points_size(len(corners))
00145       for i in range(len(corners)):
00146         obj_msg.image_points[i].x = corners[i][0]
00147         obj_msg.image_points[i].y = corners[i][1]
00148 
00149     if model != None:
00150       obj_msg.set_model_points_size(len(model))
00151       for i in range(len(corners)):
00152         obj_msg.model_points[i].x = model[i][0]
00153         obj_msg.model_points[i].y = model[i][1]
00154 
00155     self.corner_pub.publish(obj_msg)
00156 
00157 def cb_detector_main(argv=None):
00158   rospy.init_node(NAME, anonymous=False)
00159   cb_detector = ImageCbDetectorNode()
00160   rospy.spin()
00161 
00162 if __name__ == '__main__':
00163   cb_detector_main()
00164 


image_cb_detector
Author(s): Vijay Pradeep, Eitan Marder-Eppstein
autogenerated on Tue Sep 27 2016 04:06:37