def remap_extrinsics(self):
X = [self.camera_plate.point.x, self.camera_plate.point.x + self.checker_size]
Y = [self.camera_plate.point.y, self.camera_plate.point.y]
Z = [1,1]
camera_points = numpy.array([X,Y,Z])
plate_points = numpy.dot(self.H,camera_points)
x0 = plate_points[0,0]
x1 = plate_points[0,1]
y0 = plate_points[1,0]
y1 = plate_points[1,1]
rot_angle = numpy.arctan2((y1-y0),(x1-x0))
# rospy.logwarn("plate_points = %s", str(plate_points))
checkerboard_plate_vector = plate_points[:,0]
checkerboard_plate_vector[2] = 0
self.tf_broadcaster.sendTransform(checkerboard_plate_vector,
tf.transformations.quaternion_from_euler(0, 0, rot_angle),
rospy.Time.now(),
"Plate",
"Checkerboard")
try:
# self.image_plate_origin = self.tf_listener.transformPoint("Image",self.plate_origin)
(trans,rot_quat) = self.tf_listener.lookupTransform('Image', 'Plate', rospy.Time(0))
# rospy.logwarn("trans = %s", str(trans))
# rospy.logwarn("rot = %s", str(rot))
rot_array = tf.transformations.quaternion_matrix(rot_quat)
rot_array = rot_array[0:3,0:3]
rot_array = rot_array.astype('float32')
rot_mat = cvNumpy.array_to_mat(rot_array)
cv.Rodrigues2(rot_mat,self.rvec)
rvec_array_new = cvNumpy.mat_to_array(self.rvec).squeeze()
# tvec_array_new = numpy.array([self.image_plate_origin.point.x,
# self.image_plate_origin.point.y,
# self.image_plate_origin.point.z])
tvec_array_new = numpy.array(trans)
self.rvec_array = rvec_array_new
self.tvec_array = tvec_array_new
self.rvec = cvNumpy.array_to_mat(self.rvec_array)
self.tvec = cvNumpy.array_to_mat(self.tvec_array)
display_text = "rvec = [%0.3f, %0.3f, %0.3f]" % (self.rvec_array[0],self.rvec_array[1],self.rvec_array[2])
cv.PutText(self.im_display,display_text,(25,420),self.font,self.font_color)
display_text = "tvec = [%0.3f, %0.3f, %0.3f]" % (self.tvec_array[0],self.tvec_array[1],self.tvec_array[2])
cv.PutText(self.im_display,display_text,(25,440),self.font,self.font_color)
# self.image_plate_origin_found = True
self.draw_origin(self.im_display)
except (tf.LookupException, tf.ConnectivityException):
pass
def remap_extrinsics(self):
# self.find_H_image_to_plate()
# display_text = "undistorted_plate.point = " + str(int(self.undistorted_plate.point.x)) + "," + str(int(self.undistorted_plate.point.y))
# cv.PutText(self.im_display,display_text,(25,300),self.font,self.font_color)
# Ximage = [self.undistorted_plate.point.x.tolist()]
# Yimage = [self.undistorted_plate.point.y.tolist()]
# Xplate,Yplate = self.image_to_plate(Ximage,Yimage)
# display_text = "new origin point = " + str(int(Xplate)) + "," + str(int(Yplate))
# cv.PutText(self.im_display,display_text,(25,320),self.font,self.font_color)
# cv.SetZero(self.distortion_coeffs)
# rotation_matrix = cv.CreateMat(3,3,cv.CV_32FC1)
# cv.Rodrigues2(self.rvec, rotation_matrix)
# R = cvNumpy.mat_to_array(rotation_matrix)
# T = cvNumpy.mat_to_array(self.tvec).transpose()
# W = numpy.concatenate((R,numpy.zeros((1,3))),0)
# T = numpy.append(T,1)
# T = numpy.array([T]).transpose()
# rospy.logwarn("R = %s", str(R))
# rospy.logwarn("T = %s", str(T))
# W = numpy.concatenate((W,T),1)
# rospy.logwarn("W = %s", str(W))
# T2 = numpy.array([36.1,55.3,0,1])
# T2 = numpy.array([10,0,0,1])
# T2 = numpy.array([T2]).transpose()
# ang = 1.086
# ang = -1.086
# R2 = numpy.array([[math.cos(ang), math.sin(ang), 0],
# [-math.sin(ang), math.cos(ang), 0],
# [0,0,1]])
# rospy.logwarn("R2 = %s", str(R2))
# rospy.logwarn("T2 = %s", str(T2))
# W2 = numpy.concatenate((R2,numpy.zeros((1,3))),0)
# W2 = numpy.concatenate((W2,T2),1)
# rospy.logwarn("W2 = %s", str(W2))
# W3 = numpy.dot(W,W2)
# W3 = numpy.dot(W2,W)
# rospy.logwarn("W3 = %s", str(W3))
# tvec_new = W3[:-1,-1]
# R_new = W3[:-1,:-1]
# R_new = R_new.astype('float32')
# rvec_new = cv.CreateMat(1,3,cv.CV_32FC1)
# R_new = cvNumpy.array_to_mat(R_new)
# cv.Rodrigues2(R_new,rvec_new)
X = [self.camera_plate.point.x, self.camera_plate.point.x + self.checker_size]
Y = [self.camera_plate.point.y, self.camera_plate.point.y]
Z = [1,1]
camera_points = numpy.array([X,Y,Z])
plate_points = numpy.dot(self.H,camera_points)
x0 = plate_points[0,0]
x1 = plate_points[0,1]
y0 = plate_points[1,0]
y1 = plate_points[1,1]
rot_angle = numpy.arctan2((y1-y0),(x1-x0))
# rospy.logwarn("plate_points = %s", str(plate_points))
checkerboard_plate_vector = plate_points[:,0]
checkerboard_plate_vector[2] = 0
self.tf_broadcaster.sendTransform(checkerboard_plate_vector,
tf.transformations.quaternion_from_euler(0, 0, rot_angle),
rospy.Time.now(),
"Plate",
"Checkerboard")
try:
# self.image_plate_origin = self.tf_listener.transformPoint("Image",self.plate_origin)
(trans,rot_quat) = self.tf_listener.lookupTransform('Image', 'Plate', rospy.Time(0))
# rospy.logwarn("trans = %s", str(trans))
# rospy.logwarn("rot = %s", str(rot))
rot_array = tf.transformations.quaternion_matrix(rot_quat)
rot_array = rot_array[0:3,0:3]
rot_array = rot_array.astype('float32')
rot_mat = cvNumpy.array_to_mat(rot_array)
cv.Rodrigues2(rot_mat,self.rvec)
rvec_array_new = cvNumpy.mat_to_array(self.rvec).squeeze()
# tvec_array_new = numpy.array([self.image_plate_origin.point.x,
# self.image_plate_origin.point.y,
# self.image_plate_origin.point.z])
tvec_array_new = numpy.array(trans)
self.rvec_array = rvec_array_new
self.tvec_array = tvec_array_new
self.rvec = cvNumpy.array_to_mat(self.rvec_array)
self.tvec = cvNumpy.array_to_mat(self.tvec_array)
display_text = "rvec = [%0.3f, %0.3f, %0.3f]" % (self.rvec_array[0],self.rvec_array[1],self.rvec_array[2])
cv.PutText(self.im_display,display_text,(25,420),self.font,self.font_color)
display_text = "tvec = [%0.3f, %0.3f, %0.3f]" % (self.tvec_array[0],self.tvec_array[1],self.tvec_array[2])
cv.PutText(self.im_display,display_text,(25,440),self.font,self.font_color)
# self.image_plate_origin_found = True
self.draw_origin(self.im_display)
except (tf.LookupException, tf.ConnectivityException):
pass
def draw_grid(self,im_color):
points_range = (numpy.array(range(self.num_grid_lines)) - (self.num_grid_lines-1)/2) * self.checker_size
points_zeros = numpy.zeros(points_range.shape)
points_ones = numpy.ones(points_range.shape) * self.checker_size * (self.num_grid_lines - 1)/2
x_start_points_array = numpy.array([-points_ones,points_range,points_zeros]).astype('float32')
x_end_points_array = numpy.array([points_ones,points_range,points_zeros]).astype('float32')
y_start_points_array = numpy.array([points_range,-points_ones,points_zeros]).astype('float32')
y_end_points_array = numpy.array([points_range,points_ones,points_zeros]).astype('float32')
if self.rotate_grid:
x_start_points_array = self.from_homo(numpy.dot(self.T_stage_plate,self.to_homo(x_start_points_array)))
x_end_points_array = self.from_homo(numpy.dot(self.T_stage_plate,self.to_homo(x_end_points_array)))
y_start_points_array = self.from_homo(numpy.dot(self.T_stage_plate,self.to_homo(y_start_points_array)))
y_end_points_array = self.from_homo(numpy.dot(self.T_stage_plate,self.to_homo(y_end_points_array)))
self.rotate_grid = False
x_start_points = cvNumpy.array_to_mat(x_start_points_array.transpose())
x_end_points = cvNumpy.array_to_mat(x_end_points_array.transpose())
y_start_points = cvNumpy.array_to_mat(y_start_points_array.transpose())
y_end_points = cvNumpy.array_to_mat(y_end_points_array.transpose())
axis_line_width = 1
cv.ProjectPoints2(x_start_points,
self.rvec,
self.tvec,
self.intrinsic_matrix,
self.distortion_coeffs,
self.start_points_projected)
cv.ProjectPoints2(x_end_points,
self.rvec,
self.tvec,
self.intrinsic_matrix,
self.distortion_coeffs,
self.end_points_projected)
start_points = cvNumpy.mat_to_array(self.start_points_projected)
end_points = cvNumpy.mat_to_array(self.end_points_projected)
for line_n in range(self.num_grid_lines):
cv.Line(im_color,tuple(start_points[line_n,:]),tuple(end_points[line_n,:]),cv.CV_RGB(self.color_max,0,0),axis_line_width)
cv.ProjectPoints2(y_start_points,
self.rvec,
self.tvec,
self.intrinsic_matrix,
self.distortion_coeffs,
self.start_points_projected)
cv.ProjectPoints2(y_end_points,
self.rvec,
self.tvec,
self.intrinsic_matrix,
self.distortion_coeffs,
self.end_points_projected)
start_points = cvNumpy.mat_to_array(self.start_points_projected)
end_points = cvNumpy.mat_to_array(self.end_points_projected)
for line_n in range(self.num_grid_lines):
cv.Line(im_color,tuple(start_points[line_n,:]),tuple(end_points[line_n,:]),cv.CV_RGB(0,self.color_max,0),axis_line_width)
def draw_grid(self, im_color):
if self.camerainfo is not None:
points_range = (N.array(range(self.num_grid_lines)) - (self.num_grid_lines-1)/2) * self.checker_size
points_zeros = N.zeros(points_range.shape)
points_ones = N.ones(points_range.shape) * self.checker_size * (self.num_grid_lines - 1)/2
x_start_points_array = N.array([-points_ones, points_range, points_zeros]).astype('float32')
x_end_points_array = N.array([points_ones, points_range, points_zeros]).astype('float32')
y_start_points_array = N.array([points_range, -points_ones, points_zeros]).astype('float32')
y_end_points_array = N.array([points_range, points_ones, points_zeros]).astype('float32')
if self.rotate_grid:
x_start_points_array = self.from_homo(N.dot(self.T_stage_plate,self.to_homo(x_start_points_array)))
x_end_points_array = self.from_homo(N.dot(self.T_stage_plate,self.to_homo(x_end_points_array)))
y_start_points_array = self.from_homo(N.dot(self.T_stage_plate,self.to_homo(y_start_points_array)))
y_end_points_array = self.from_homo(N.dot(self.T_stage_plate,self.to_homo(y_end_points_array)))
self.rotate_grid = False
x_start_points = cvNumpy.array_to_mat(x_start_points_array.transpose())
x_end_points = cvNumpy.array_to_mat(x_end_points_array.transpose())
y_start_points = cvNumpy.array_to_mat(y_start_points_array.transpose())
y_end_points = cvNumpy.array_to_mat(y_end_points_array.transpose())
axis_line_width = 1
cvmatK = cv.fromarray(N.reshape(self.camerainfo.K,[3,3]))
cvmatD = cv.fromarray(N.reshape(self.camerainfo.D,[5,1]))
cv.ProjectPoints2(x_start_points,
self.rvec,
self.tvec,
cvmatK,
cvmatD,
self.start_points_projected)
cv.ProjectPoints2(x_end_points,
self.rvec,
self.tvec,
cvmatK,
cvmatD,
self.end_points_projected)
start_points = cvNumpy.mat_to_array(self.start_points_projected)
end_points = cvNumpy.mat_to_array(self.end_points_projected)
for line_n in range(self.num_grid_lines):
cv.Line(im_color, tuple(start_points[line_n,:]), tuple(end_points[line_n,:]), cv.CV_RGB(self.color_max,0,0), axis_line_width)
cv.ProjectPoints2(y_start_points,
self.rvec,
self.tvec,
cvmatK,
cvmatD,
self.start_points_projected)
cv.ProjectPoints2(y_end_points,
self.rvec,
self.tvec,
cvmatK,
cvmatD,
self.end_points_projected)
start_points = cvNumpy.mat_to_array(self.start_points_projected)
end_points = cvNumpy.mat_to_array(self.end_points_projected)
for line_n in range(self.num_grid_lines):
cv.Line(im_color, tuple(start_points[line_n,:]), tuple(end_points[line_n,:]), cv.CV_RGB(0,self.color_max,0), axis_line_width)
def remap_extrinsics(self, now):
X = [self.originPlate.point.x, self.originPlate.point.x + self.checker_size]
Y = [self.originPlate.point.y, self.originPlate.point.y]
Z = [1, 1]
camera_points = N.array([X,Y,Z])
plate_points = N.dot(self.H, camera_points)
x0 = plate_points[0,0]
x1 = plate_points[0,1]
y0 = plate_points[1,0]
y1 = plate_points[1,1]
angleRot = N.arctan2((y1-y0),(x1-x0))
# rospy.logwarn("plate_points = %s", str(plate_points))
checkerboard_plate_vector = plate_points[:,0]
checkerboard_plate_vector[2] = 0
self.tfbx.sendTransform(checkerboard_plate_vector,
tf.transformations.quaternion_from_euler(0, 0, angleRot),
now,
FRAME_PLATE,
FRAME_CHECKERBOARD)
try:
# self.image_plate_origin = self.tfrx.transformPoint("Image",self.plate_origin)
self.tfrx.waitForTransform(FRAME_IMAGERECT, FRAME_PLATE, now, rospy.Duration(1.0))
(trans, quat) = self.tfrx.lookupTransform(FRAME_IMAGERECT, FRAME_PLATE, now)
# rospy.logwarn("trans = %s", str(trans))
# rospy.logwarn("rot = %s", str(rot))
rot_array = tf.transformations.quaternion_matrix(quat)
rot_array = rot_array[0:3,0:3]
rot_array = rot_array.astype('float32')
rot_mat = cvNumpy.array_to_mat(rot_array)
cv.Rodrigues2(rot_mat, self.rvec)
self.rvec_array = cvNumpy.mat_to_array(self.rvec).squeeze()
self.tvec_array = N.array(trans)
self.rvec = cvNumpy.array_to_mat(self.rvec_array)
self.tvec = cvNumpy.array_to_mat(self.tvec_array)
display_text = "rvec = [%0.3f, %0.3f, %0.3f]" % (self.rvec_array[0],
self.rvec_array[1],
self.rvec_array[2])
cv.PutText(self.im_display,
display_text,
(25,420),
self.font,
self.font_color)
display_text = "tvec = [%0.3f, %0.3f, %0.3f]" % (self.tvec_array[0],
self.tvec_array[1],
self.tvec_array[2])
cv.PutText(self.im_display,
display_text,
(25,440),
self.font,
self.font_color)
# self.image_plate_origin_found = True
self.draw_origin(self.im_display)
except (tf.Exception):
pass
