def backf(hypH, im, found):
(code,corners,pattern) = found
persp = cv.CreateMat(3, 3, cv.CV_32FC1)
fc = [corners[i,0] for i in range(4)]
cv.GetPerspectiveTransform(fc, sizcorners, persp)
cc = cv.Reshape(cv.fromarray(numpy.array(sizcorners).astype(numpy.float32)), 2)
t1 = cv.CreateMat(4, 1, cv.CV_32FC2)
t2 = cv.CreateMat(4, 1, cv.CV_32FC2)
_persp = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Invert(persp, _persp)
_hypH = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Invert(hypH, _hypH)
cv.PerspectiveTransform(cc, t1, _hypH)
cv.PerspectiveTransform(t1, t2, _persp)
return [t2[i,0] for i in range(4)]
def backproject_sample_rect(warp_matrix, sample_dims):
warp_matrix_inv = cv2.invert(warp_matrix)[1]
#warp_matrix_inv = np.array(warp_matrix_inv)/cv.Get2D(warp_matrix_inv, 2, 2)[0]
#print warp_matrix_inv
warp_matrix_inv = cv.fromarray(warp_matrix_inv)
rectified_shape = np.array((1., 1.))
width_center = rectified_shape[1] / 2
sample_dims = [x / 2 for x in sample_dims]
#roi_bottom = (rectified_shape[0] - neighborhood_length) * np.random.random() + \
# neighborhood_length
roi_bottom = 0.5
rectified_sample_roi = [
width_center - sample_dims[0], roi_bottom + sample_dims[1],
width_center - sample_dims[0], roi_bottom - sample_dims[1],
width_center + sample_dims[0], roi_bottom - sample_dims[1],
width_center + sample_dims[0], roi_bottom + sample_dims[1]
]
rectified_sample_roi = np.array(rectified_sample_roi, dtype=np.float32)
rectified_sample_roi = rectified_sample_roi.reshape((1, 4, 2))
backprojected_sample_boundary = cv.CreateMat(1, 4, cv.CV_32FC2)
rectified_sample_roi = cv.fromarray(rectified_sample_roi)
cv.PerspectiveTransform(rectified_sample_roi,
backprojected_sample_boundary, warp_matrix_inv)
return cvutils.array2point_list(np.array(backprojected_sample_boundary))
def localize(self, tags):
global arr_ball_pos
tag_positions = Tag_Positions()
for t in tags.tags:
current_tag_position = Tag_Position()
current_tag_position.id = t.id
#This is all Open CV matrix stuff that is more complicated
#than it should be.
a = cv.fromarray(numpy.array([[[float(t.x), float(t.y)]]]))
b = cv.fromarray(numpy.empty((1, 1, 2)))
cv.PerspectiveTransform(a, b, self.transform)
current_tag_position.x = numpy.asarray(b)[0, 0, 0]
current_tag_position.y = numpy.asarray(b)[0, 0, 1]
current_tag_position.theta = t.zRot - self.zRot_offset
#Adjust tag angle based on its position
current_tag_position.theta += correct(current_tag_position.x,
current_tag_position.y,
arr_correct_theta)
if (current_tag_position.theta < -pi):
current_tag_position.theta += 2 * pi
if (current_tag_position.theta > pi):
current_tag_position.theta -= 2 * pi
tag_positions.tag_positions.append(current_tag_position)
for tcache in arr_ball_pos:
tag_positions.tag_positions.append(tcache)
self.pub.publish(tag_positions)
def transform_pts(self,pts,M):
ptMat = cv.CreateMat(len(pts),1,cv.CV_32FC2)
for i,pt in enumerate(pts):
ptMat[i,0] = (pt[0],pt[1])
returnPtMat = cv.CloneMat(ptMat)
cv.PerspectiveTransform(ptMat,returnPtMat,M)
return_pts = []
for i in range(len(pts)):
return_pts.append(returnPtMat[i,0][:2])
return return_pts
def track_ball(self, blobs):
global arr_ball_pos
arr_ball_pos = []
tag_positions = Tag_Positions()
for blob in blobs.blobs:
if (blob.red == 255 and blob.green == 0 and blob.blue == 0
and blob.area > area_crit):
current_tag_position = Tag_Position()
current_tag_position.id = ball_id
a = cv.fromarray(
numpy.array([[[float(blob.x), float(blob.y)]]]))
b = cv.fromarray(numpy.empty((1, 1, 2)))
cv.PerspectiveTransform(a, b, self.transform)
current_tag_position.x = numpy.asarray(b)[0, 0, 0]
current_tag_position.y = numpy.asarray(b)[0, 0, 1]
arr_ball_pos.append(current_tag_position)
def correct_horizontal_motion(image, prev_trackpts, curr_trackpts, boundary_pts):
image_boundaries = (1, 1, image.width-2, image.height-2)
motion_matrix=cv.CreateMat(3, 3, cv.CV_32FC1)
cv.FindHomography(prev_trackpts, curr_trackpts,
motion_matrix,
cv.CV_RANSAC, 10)
boundary_pts = boundary_pts.reshape((1, -1, 2)).astype(np.float32)
warped_boundary_pts = np.zeros_like(boundary_pts)
cv.PerspectiveTransform(boundary_pts, warped_boundary_pts, motion_matrix)
pt11, pt12 = cvutils.line_rectangle_intersection(
tuple(warped_boundary_pts[0, 0,:]), tuple(warped_boundary_pts[0, 3,:]),
image_boundaries)
pt21, pt22 = cvutils.line_rectangle_intersection(
tuple(warped_boundary_pts[0, 1,:]), tuple(warped_boundary_pts[0, 2,:]),
image_boundaries)
boundary_pts = cvutils.reorder_boundary_points(np.array((pt11, pt12, pt21, pt22)))
return boundary_pts.reshape((-1, 2)).astype(np.int), motion_matrix
def capture(self):
blank = self.get_picture_of_projection(self.blank_projection)
positive = self.get_picture_of_projection(
self.positive_chessboard_projection)
negative = self.get_picture_of_projection(
self.negative_chessboard_projection)
difference = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.Sub(positive, negative, difference)
cv.NamedWindow("blank", flags=0)
cv.ShowImage("blank", blank)
cv.WaitKey(300)
cv.NamedWindow("difference", flags=0)
cv.ShowImage("difference", difference)
cv.WaitKey(300)
camera_image_points, camera_object_points = detect_chessboard(
blank, self.printed_chessboard_corners_x,
self.printed_chessboard_corners_y, self.printed_chessboard_spacing,
self.printed_chessboard_spacing)
if camera_image_points is None:
return False
cv.UndistortPoints(camera_image_points, camera_image_points,
self.camera_model.intrinsicMatrix(),
self.camera_model.distortionCoeffs())
homography = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.FindHomography(camera_image_points, camera_object_points,
homography)
object_points, dummy = detect_chessboard(
difference, self.projected_chessboard_corners_x,
self.projected_chessboard_corners_y, None, None)
if object_points is None:
return False
cv.UndistortPoints(object_points, object_points,
self.camera_model.intrinsicMatrix(),
self.camera_model.distortionCoeffs())
cv.PerspectiveTransform(object_points, object_points, homography)
object_points_3d = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC3)
x = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
y = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
cv.Split(object_points, x, y, None, None)
z = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
cv.SetZero(z)
cv.Merge(x, y, z, None, object_points_3d)
self.object_points.append(object_points_3d)
self.image_points.append(self.get_scene_image_points())
self.number_of_scenes += 1
return True
def refinecorners(im, found):
""" For a found marker, return the refined corner positions """
t0 = time.time()
(code,corners,pattern) = found
persp = cv.CreateMat(3, 3, cv.CV_32FC1)
fc = [corners[i,0] for i in range(4)]
cv.GetPerspectiveTransform(fc, sizcorners, persp)
cim = cv.CreateMat(siz, siz, cv.CV_8UC1)
cv.WarpPerspective(im, cim, persp, flags = cv.CV_INTER_LINEAR|cv.CV_WARP_FILL_OUTLIERS, fillval = 255)
unit = siz / 14.
hunit = unit / 2
def nearest1(x, y):
ix = int((x + hunit) / unit)
iy = int((y + hunit) / unit)
if (2 <= ix < 13) and (2 <= iy < 13):
nx = int(unit * ix)
ny = int(unit * iy)
return (nx, ny)
else:
return (0,0)
def nearest(x, y):
""" Return all grid points within sqrt(2) units of (x,y), closest first """
close = []
for ix in range(2, 14):
for iy in range(2, 14):
(nx, ny) = (unit * ix, unit * iy)
d = l2(x, y, nx, ny)
close.append((d, (nx, ny)))
return [p for (d,p) in sorted(close) if d < 2*unit*unit]
corners = strongest(cim)
pool = [((x,y), nearest(x, y)) for (x, y) in corners]
ga = dict([(x+y,((x,y),P)) for ((x,y),P) in pool])
gb = dict([(x-y,((x,y),P)) for ((x,y),P) in pool])
hyp = [ga[min(ga)], ga[max(ga)],
gb[min(gb)], gb[max(gb)]]
aL = [a for (a,bs) in hyp]
oldcorners = cv.fromarray(numpy.array(corners).astype(numpy.float32))
oldcorners = cv.Reshape(oldcorners, 2)
newcorners = cv.CreateMat(len(corners), 1, cv.CV_32FC2)
best = (9999999, None)
for bL in itertools.product(*[bs for (a,bs) in hyp]):
hypH = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.GetPerspectiveTransform(aL, bL, hypH)
cv.PerspectiveTransform(oldcorners, newcorners, hypH)
error = 0
for i in range(newcorners.rows):
(x,y) = newcorners[i,0]
(nx, ny) = nearest1(x, y)
error += l2(x, y, nx, ny)
best = min(best, (error, hypH))
if error < 1000:
break
# print "took", time.time() - t0, best[0]
if best[0] < 2500:
pose = best[1]
return backf(pose, im, found)
else:
return None