cb_detector.py
Go to the documentation of this file.
1 #! /usr/bin/python
2 #***********************************************************
3 #* Software License Agreement (BSD License)
4 #*
5 #* Copyright (c) 2009, Willow Garage, Inc.
6 #* All rights reserved.
7 #*
8 #* Redistribution and use in source and binary forms, with or without
9 #* modification, are permitted provided that the following conditions
10 #* are met:
11 #*
12 #* * Redistributions of source code must retain the above copyright
13 #* notice, this list of conditions and the following disclaimer.
14 #* * Redistributions in binary form must reproduce the above
15 #* copyright notice, this list of conditions and the following
16 #* disclaimer in the documentation and/or other materials provided
17 #* with the distribution.
18 #* * Neither the name of Willow Garage, Inc. nor the names of its
19 #* contributors may be used to endorse or promote products derived
20 #* from this software without specific prior written permission.
21 #*
22 #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 #* POSSIBILITY OF SUCH DAMAGE.
34 #*
35 #* Author: Eitan Marder-Eppstein
36 #***********************************************************
37 PKG = 'image_cb_detector'
38 NAME = 'image_cb_detector_node'
39 import roslib; roslib.load_manifest(PKG)
40 
41 import rospy
42 import cv
43 import math
44 
45 from sensor_msgs.msg import Image
46 from image_cb_detector.msg import ObjectInImage
47 
48 from cv_bridge import CvBridge, CvBridgeError
49 
51  def __init__(self, corners_x, corners_y, spacing_x = None, spacing_y = None, width_scaling = 1, height_scaling = 1):
52  self.corners_x = corners_x
53  self.corners_y = corners_y
54  self.spacing_x = spacing_x
55  self.spacing_y = spacing_y
56  self.width_scaling = width_scaling
57  self.height_scaling = height_scaling
58 
59  def get_board_corners(self, corners):
60  return (corners[0], corners[self.corners_x - 1],
61  corners[(self.corners_y - 1) * self.corners_x], corners[len(corners) - 1])
62 
63  def detect(self, image):
64  #resize the image base on the scaling parameters we've been configured with
65  scaled_width = int(.5 + image.width * self.width_scaling)
66  scaled_height = int(.5 + image.height * self.height_scaling)
67 
68  #in cvMat its row, col so height comes before width
69  image_scaled = cv.CreateMat(scaled_height, scaled_width, cv.GetElemType(image))
70  cv.Resize(image, image_scaled, cv.CV_INTER_LINEAR)
71 
72 
73  found, corners = cv.FindChessboardCorners(image_scaled, (self.corners_x, self.corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH)
74 
75  if found:
76  rospy.logdebug("Found cb")
77  board_corners = self.get_board_corners(corners)
78 
79  #find the perimeter of the checkerboard
80  perimeter = 0.0
81  for i in range(len(board_corners)):
82  next = (i + 1) % 4
83  xdiff = board_corners[i][0] - board_corners[next][0]
84  ydiff = board_corners[i][1] - board_corners[next][1]
85  perimeter += math.sqrt(xdiff * xdiff + ydiff * ydiff)
86 
87  #estimate the square size in pixels
88  square_size = perimeter / ((self.corners_x - 1 + self.corners_y - 1) * 2)
89  radius = int(square_size * 0.5 + 0.5)
90 
91  corners = cv.FindCornerSubPix(image_scaled, corners, (radius, radius), (-1, -1), (cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 30, 0.1))
92  #cv.DrawChessboardCorners(image_scaled, (self.corners_x, self.corners_y), corners, 1)
93  #cv.NamedWindow("image_scaled")
94  #cv.ShowImage("image_scaled", image_scaled)
95  #cv.WaitKey()
96 
97  object_points = None
98 
99  #we'll also generate the object points if they've been requested
100  if self.spacing_x != None and self.spacing_y != None:
101  object_points = [None] * (self.corners_x * self.corners_y)
102 
103  for i in range(self.corners_y):
104  for j in range(self.corners_x):
105  object_points[i * self.corners_x + j] = (j * self.spacing_x, i * self.spacing_y)
106 
107  return (corners, object_points)
108 
109  else:
110  rospy.logdebug("Didn't find checkerboard")
111  return (None, None)
112 
114  def __init__(self):
115  corners_x = rospy.get_param('~corners_x', 6)
116  corners_y = rospy.get_param('~corners_y', 6)
117  spacing_x = rospy.get_param('~spacing_x', None)
118  spacing_y = rospy.get_param('~spacing_y', None)
119  width_scaling = rospy.get_param('~width_scaling', 1)
120  height_scaling = rospy.get_param('~height_scaling', 1)
121 
122  self.im_cb_detector = ImageCbDetector(corners_x, corners_y, spacing_x,
123  spacing_y, width_scaling, height_scaling)
124 
125  rospy.Subscriber("image_stream", Image, self.callback)
126  self.corner_pub = rospy.Publisher("corners_in_image", ObjectInImage)
127  self.bridge = CvBridge()
128 
129  def callback(self, ros_image):
130  #we need to convert the ros image to an opencv image
131  try:
132  image = self.bridge.imgmsg_to_cv(ros_image, "mono8")
133  except CvBridgeError, e:
134  rospy.logerror("Error importing image %s" % e)
135  return
136 
137  corners, model = self.im_cb_detector.detect(image)
138 
139  #now we need to publish them out over ros
140  obj_msg = ObjectInImage()
141  obj_msg.header = ros_image.header
142 
143  if corners != None:
144  obj_msg.set_image_points_size(len(corners))
145  for i in range(len(corners)):
146  obj_msg.image_points[i].x = corners[i][0]
147  obj_msg.image_points[i].y = corners[i][1]
148 
149  if model != None:
150  obj_msg.set_model_points_size(len(model))
151  for i in range(len(corners)):
152  obj_msg.model_points[i].x = model[i][0]
153  obj_msg.model_points[i].y = model[i][1]
154 
155  self.corner_pub.publish(obj_msg)
156 
157 def cb_detector_main(argv=None):
158  rospy.init_node(NAME, anonymous=False)
159  cb_detector = ImageCbDetectorNode()
160  rospy.spin()
161 
162 if __name__ == '__main__':
164 
def cb_detector_main(argv=None)
Definition: cb_detector.py:157
def __init__(self, corners_x, corners_y, spacing_x=None, spacing_y=None, width_scaling=1, height_scaling=1)
Definition: cb_detector.py:51


image_cb_detector
Author(s): Vijay Pradeep, Eitan Marder-Eppstein
autogenerated on Mon Jun 10 2019 12:57:16