00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 import roslib
00041 roslib.load_manifest('pr2_interactive_segmentation')
00042 import rospy
00043
00044 import numpy
00045
00046 import sys
00047 import urllib
00048
00049
00050 import cv
00051 import cv_bridge
00052 import geometry_msgs.msg
00053 import pr2_interactive_segmentation.srv
00054
00055
00056 class CornerFinder():
00057
00058 def __init__(self):
00059 self.smooth_scale = 29
00060 self.eigval_threshold = 0.05
00061 self.max_ratio = 2.0
00062 self.convexity_thresh = 0.5
00063 self.check_step_radius = 15
00064 pass
00065
00066
00067
00068 def findCorners(self,src):
00069 dest = cv.CreateMat(src.height, 6 * src.width, cv.CV_32F)
00070 col = cv.CreateMat(src.height, src.width, cv.CV_8UC3)
00071 canny = cv.CreateMat(src.height, src.width, cv.CV_8U)
00072 canny_dil = cv.CreateMat(src.height, src.width, cv.CV_8U)
00073 cv.Canny(src, canny, 20, 80)
00074
00075 cv.Dilate(canny,canny_dil)
00076
00077
00078 accumulator = cv.CreateMat(src.height, src.width, cv.CV_32F)
00079 cv.Zero(accumulator)
00080 cv.CvtColor(src, col, cv.CV_GRAY2BGR)
00081 cv.CornerEigenValsAndVecs(src, dest, self.smooth_scale, 5)
00082
00083 a = numpy.asarray(dest)
00084 a = a.reshape((src.height, src.width, 6))
00085 for j in range(src.height):
00086 for i in range(src.width):
00087 if canny_dil[j,i] == 0:
00088 continue
00089
00090 lambda1 = abs(a[j,i,0])
00091 lambda2 = abs(a[j,i,1])
00092
00093
00094
00095 if lambda1 > self.eigval_threshold and lambda2 > self.eigval_threshold:
00096
00097
00098
00099
00100
00101
00102
00103
00104 if max(lambda1,lambda2)/min(lambda1,lambda2) < self.max_ratio and \
00105 min(lambda1,lambda2)/max(lambda1,lambda2) > 1.0/self.max_ratio:
00106
00107 accumulator[j,i] = 1.0
00108
00109
00110
00111
00112
00113
00114 cv.Smooth(accumulator, accumulator, cv.CV_GAUSSIAN, self.smooth_scale, self.smooth_scale)
00115
00116 accumulator_dil = cv.CreateMat(src.height, src.width, cv.CV_32F)
00117 cv.AddS(accumulator, 0.0, accumulator, canny)
00118 corners = []
00119 cv.Dilate(accumulator,accumulator_dil)
00120 for j in range(src.height):
00121 for i in range(src.width):
00122 if accumulator[j,i] > 0:
00123 if accumulator[j,i] == accumulator_dil[j,i]:
00124 corners.append((j,i))
00125
00126
00127
00128 check_step_radius_img = cv.CreateMat(2*self.check_step_radius+2, 2*self.check_step_radius+2, cv.CV_8U)
00129 cv.Circle(check_step_radius_img,(self.check_step_radius+1, self.check_step_radius +1), self.check_step_radius, 255, -1)
00130 check_area = cv.Sum(check_step_radius_img)[0]
00131
00132 cv.NamedWindow("check",1)
00133 convex_corners = []
00134 convex_corner_directions = []
00135 concave_corners = []
00136 concave_corner_directions = []
00137
00138 col2 = cv.CloneMat(col)
00139
00140
00141
00142
00143 for (j,i) in corners:
00144
00145 vec1 = a[j,i,2:4]
00146 vec2 = a[j,i,4:6]
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156 check_circle_img = cv.CreateMat(src.height, src.width, cv.CV_8U)
00157 cv.Zero(check_circle_img)
00158 cv.Circle(check_circle_img,(i, j), self.check_step_radius, 255, -1)
00159 check_circle_img_temp = cv.CreateMat(src.height, src.width, cv.CV_8U)
00160 cv.And(src, check_circle_img, check_circle_img_temp )
00161 check_area_this = cv.Sum(check_circle_img_temp)[0]
00162
00163
00164
00165
00166
00167 if check_area_this/check_area > self.convexity_thresh:
00168
00169 concave_corners.append((j,i))
00170
00171 cv.Circle(col, (i,j), 12, (0,255,0,0), 3)
00172 cv.Circle(col2, (i,j), 12, (0,255,0,0), 3)
00173
00174
00175
00176 check_line1_img = cv.CreateMat(src.height, src.width, cv.CV_8U)
00177 cv.Zero(check_line1_img)
00178 cv.Line(check_line1_img, (i,j),\
00179 tuple(numpy.array(self.check_step_radius * vec1 + (i,j), dtype = 'i')), 255, 1)
00180 cv.And(check_circle_img_temp, check_line1_img, check_line1_img )
00181
00182 check_line1_len_pos = cv.Sum(check_line1_img)[0]
00183 cv.Zero(check_line1_img)
00184 cv.Line(check_line1_img, (i,j),\
00185 tuple(numpy.array(self.check_step_radius * vec2 + (i,j), dtype = 'i')), 255, 1)
00186 cv.And(check_circle_img_temp, check_line1_img, check_line1_img )
00187 check_line2_len_pos = cv.Sum(check_line1_img)[0]
00188
00189 cv.Zero(check_line1_img)
00190 cv.Line(check_line1_img, (i,j),\
00191 tuple(numpy.array(-self.check_step_radius * vec1 + (i,j), dtype = 'i')), 255, 1)
00192 cv.And(check_circle_img_temp, check_line1_img, check_line1_img )
00193 check_line1_len_neg = cv.Sum(check_line1_img)[0]
00194 cv.Zero(check_line1_img)
00195 cv.Line(check_line1_img, (i,j),\
00196 tuple(numpy.array(-self.check_step_radius * vec2 + (i,j), dtype = 'i')), 255, 1)
00197 cv.And(check_circle_img_temp, check_line1_img, check_line1_img )
00198 check_line2_len_neg = cv.Sum(check_line1_img)[0]
00199
00200
00201
00202 if check_line1_len_neg + check_line1_len_pos < check_line2_len_neg + check_line2_len_pos:
00203 if check_line1_len_neg > check_line1_len_pos:
00204 vec1 = -1.0 * vec1
00205
00206 cv.Line(col, (i,j), tuple(numpy.array(20 * vec1 + (i,j), dtype = 'i')), (0,255,0), 3)
00207 concave_corner_directions.append(vec1)
00208
00209 else:
00210 if check_line2_len_neg > check_line2_len_pos:
00211 vec2 = -1.0 * vec2
00212
00213 cv.Line(col, (i,j), tuple(numpy.array(20 * vec2 + (i,j), dtype = 'i')), (0,255,0), 3)
00214 concave_corner_directions.append(vec2)
00215
00216
00217
00218
00219
00220
00221 else:
00222 convex_corners.append((j,i))
00223
00224
00225
00226 cv.Circle(col, (i,j), 12, (0,0,255,0), 3)
00227 cv.Circle(col2, (i,j), 12, (0,0,255,0), 3)
00228
00229
00230 check_line1_img = cv.CreateMat(src.height, src.width, cv.CV_8U)
00231 cv.Zero(check_line1_img)
00232 cv.Line(check_line1_img, (i,j),\
00233 tuple(numpy.array(self.check_step_radius * vec1 + (i,j), dtype = 'i')), 255, 1)
00234 cv.And(check_circle_img_temp, check_line1_img, check_line1_img )
00235 check_line1_len_pos = cv.Sum(check_line1_img)[0]
00236 cv.Zero(check_line1_img)
00237 cv.Line(check_line1_img, (i,j),\
00238 tuple(numpy.array(self.check_step_radius * vec2 + (i,j), dtype = 'i')), 255, 1)
00239 cv.And(check_circle_img_temp, check_line1_img, check_line1_img )
00240 check_line2_len_pos = cv.Sum(check_line1_img)[0]
00241
00242 cv.Zero(check_line1_img)
00243 cv.Line(check_line1_img, (i,j),\
00244 tuple(numpy.array(-self.check_step_radius * vec1 + (i,j), dtype = 'i')), 255, 1)
00245 cv.And(check_circle_img_temp, check_line1_img, check_line1_img )
00246 check_line1_len_neg = cv.Sum(check_line1_img)[0]
00247 cv.Zero(check_line1_img)
00248 cv.Line(check_line1_img, (i,j),\
00249 tuple(numpy.array(-self.check_step_radius * vec2 + (i,j), dtype = 'i')), 255, 1)
00250 cv.And(check_circle_img_temp, check_line1_img, check_line1_img )
00251 check_line2_len_neg = cv.Sum(check_line1_img)[0]
00252
00253
00254 if check_line1_len_neg + check_line1_len_pos < check_line2_len_neg + check_line2_len_pos:
00255 if check_line2_len_neg > check_line2_len_pos:
00256 vec2 = -1.0 * vec2
00257 cv.Line(col, (i,j), tuple(numpy.array(20 * vec2 + (i,j), dtype = 'i')), (0,0,255), 3)
00258 convex_corner_directions.append(vec2)
00259
00260 else:
00261 if check_line1_len_neg > check_line1_len_pos:
00262 vec1 = -1.0 * vec1
00263 cv.Line(col, (i,j), tuple(numpy.array(20 * vec1 + (i,j), dtype = 'i')), (0,0,255), 3)
00264 convex_corner_directions.append(vec1)
00265
00266
00267
00268
00269
00270
00271
00272
00273 cv.NamedWindow("corner_finder",1)
00274 cv.ShowImage("corner_finder", col)
00275 cv.WaitKey()
00276 cv.ShowImage("corner_finder", col2)
00277 cv.WaitKey()
00278
00279
00280
00281
00282
00283 return (col,accumulator,convex_corners, convex_corner_directions, concave_corners, concave_corner_directions)
00284
00285
00286
00287
00288
00289 def findCornersService(self,req):
00290 br = cv_bridge.CvBridge()
00291 iplImage = br.imgmsg_to_cv(req.image)
00292 (col,accumulator,convex_corners, convex_corner_directions, concave_corners, concave_corner_directions) = self.findCorners(iplImage)
00293 res = pr2_interactive_segmentation.srv.cornerFindResponse()
00294 for i in range(len (convex_corners)):
00295 pt =geometry_msgs.msg.Point()
00296 pt.x = convex_corners[i][1]
00297 pt.y = convex_corners[i][0]
00298 res.corner_convex.append(pt)
00299 pt = geometry_msgs.msg.Point32()
00300 pt.x = convex_corner_directions[i][0]
00301 pt.y = convex_corner_directions[i][1]
00302 res.push_direction_convex.append(pt)
00303
00304 for i in range(len (concave_corners)):
00305 pt =geometry_msgs.msg.Point()
00306 pt.x = concave_corners[i][1]
00307 pt.y = concave_corners[i][0]
00308 res.corner.append(pt)
00309 pt = geometry_msgs.msg.Point32()
00310 pt.x = concave_corner_directions[i][0]
00311 pt.y = concave_corner_directions[i][1]
00312 res.push_direction.append(pt)
00313 return res
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323 if __name__ == '__main__':
00324 rospy.init_node('coner_finder_server')
00325 rospy.sleep(1.0)
00326 cf = CornerFinder()
00327 s = rospy.Service('find_corners', pr2_interactive_segmentation.srv.cornerFind, lambda a: cf.findCornersService(a) )
00328 rospy.spin()
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344