def __init__(self):
(intrinsic_matrix,distortion_coeffs) = CameraParameters.intrinsic("undistorted")
(rvec,tvec) = CameraParameters.extrinsic("plate")
intrinsic_matrix = cvNumpy.mat_to_array(intrinsic_matrix)
rvec = cvNumpy.mat_to_array(rvec).squeeze()
tvec = cvNumpy.mat_to_array(tvec).squeeze()
# Listener/Subscribers
self.tf_listener = tf.TransformListener()
# self.Hinv = numpy.array([\
# [2.02, 7.57e-3, 3.18e2],
# [2.24e-3, -2, 2.40e2],
# [1.6e-5,2.56e-5,1]])
# alpha, beta, gamma = 0.123, -1.234, 2.345
# origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)
# I = tf.transformations.identity_matrix()
# Rx = tf.transformations.rotation_matrix(alpha, xaxis)
# rospy.logwarn("Rx = %s", str(Rx))
rvec_angle = numpy.linalg.norm(rvec)
R = tf.transformations.rotation_matrix(rvec_angle,rvec)
T = tf.transformations.translation_matrix(tvec)
# rospy.logwarn("R = \n%s", str(R))
# rospy.logwarn("T = \n%s", str(T))
Wsub = numpy.zeros((3,3))
Wsub[:,:-1] = R[:-1,:-2]
Wsub[:,-1] = T[:-1,-1]
# rospy.logwarn("Wsub = \n%s", str(Wsub))
# M = intrinsic_matrix
# rospy.logwarn("M = \n%s", str(M))
Hinv = numpy.dot(M,Wsub)
Hinv = Hinv/Hinv[-1,-1]
# rospy.logwarn("Hinv = \n%s", str(Hinv))
R = numpy.zeros((4,4))
R[-1,-1] = 1
R[-2,-2] = 1
R[:2,:2] = Hinv[:2,:2]
# rospy.logwarn("R = \n%s", str(R))
T = Hinv[:-1,-1]
# rospy.logwarn("T = \n%s", str(T))
# q = tf.transformations.quaternion_from_matrix(R)
# rospy.logwarn("q = \n%s", str(q))
H = numpy.linalg.inv(Hinv)
self.Hinv = Hinv
self.H = H
def CameraInfo_callback (self, msgCameraInfo):
if self.camerainfo is None:
self.camerainfo = msgCameraInfo
self.M = N.reshape(N.array(self.camerainfo.K),[3,3])
# Makes with respect to Camera coordinate system instead of ImageRect
self.M[:-1,-1] = 0
(self.rvec, self.tvec) = CameraParameters.extrinsic("plate")
def __init__(self):
self.initialized = False
self.images_initialized = False
self.undistorted_saved = False
self.distorted_saved = False
cv.NamedWindow("Presentation", 1)
self.tf_listener = tf.TransformListener()
self.tf_broadcaster = tf.TransformBroadcaster()
self.bridge = CvBridge()
self.image_undistorted_sub = rospy.Subscriber("UndistortedImage", Image, self.image_undistorted_callback)
self.image_undistorted_sub = rospy.Subscriber("OriginalImage", Image, self.image_distorted_callback)
self.joy_sub = rospy.Subscriber("Joystick/Commands", JoystickCommands, self.joy_callback)
self.color_max = 255
self.font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX,0.5,0.5)
self.font_color = cv.CV_RGB(self.color_max,0,0)
self.camera_plate = PointStamped()
self.camera_plate.header.frame_id = "Camera"
self.camera_origin = PointStamped()
self.camera_origin.header.frame_id = "Camera"
self.camera_origin.point.x = 0
self.camera_origin.point.y = 0
# self.image_plate_origin_found = False
# self.plate_origin = PointStamped()
# self.plate_origin.header.frame_id = "Plate"
# self.plate_origin.point.x = 0
# self.plate_origin.point.y = 0
(self.intrinsic_matrix,self.distortion_coeffs) = CameraParameters.intrinsic("undistorted")
self.KK_cx = self.intrinsic_matrix[0,2]
self.KK_cy = self.intrinsic_matrix[1,2]
self.M = cvNumpy.mat_to_array(self.intrinsic_matrix)
# Makes with respect to Camera coordinate system instead of Undistorted
self.M[:-1,-1] = 0
# Pattern info
self.pattern_size = (8,6)
self.col_corner_number = self.pattern_size[0]
self.row_corner_number = self.pattern_size[1]
self.board_corner_number = self.col_corner_number * self.row_corner_number
self.checker_size = 15
self.win = (4,4)
self.zero_zone = (2,2)
self.criteria = (cv.CV_TERMCRIT_ITER+cv.CV_TERMCRIT_EPS,100,.01)
self.image_points = cv.CreateMat(self.board_corner_number,2,cv.CV_32FC1)
self.plate_points = cv.CreateMat(self.board_corner_number,3,cv.CV_32FC1)
self.point_counts = cv.CreateMat(1,1,cv.CV_32SC1)
self.rvec = cv.CreateMat(1,3,cv.CV_32FC1)
self.tvec = cv.CreateMat(1,3,cv.CV_32FC1)
self.origin_points = cv.CreateMat(4,3,cv.CV_32FC1)
self.origin_points_projected = cv.CreateMat(4,2,cv.CV_32FC1)
self.rvec_zeros = cv.SetZero(cv.CreateMat(1,3,cv.CV_32FC1))
self.tvec_zeros = cv.SetZero(cv.CreateMat(1,3,cv.CV_32FC1))
self.initialized = True
def __init__(self):
(intrinsic_matrix,distortion_coeffs) = CameraParameters.intrinsic("undistorted")
(rvec,tvec) = CameraParameters.extrinsic("plate")
intrinsic_matrix = cvNumpy.mat_to_array(intrinsic_matrix)
rvec = cvNumpy.mat_to_array(rvec).squeeze()
tvec = cvNumpy.mat_to_array(tvec).squeeze()
rvec_angle = numpy.linalg.norm(rvec)
R = tf.transformations.rotation_matrix(rvec_angle,rvec)
T = tf.transformations.translation_matrix(tvec)
Wsub = numpy.zeros((3,3))
Wsub[:,:-1] = R[:-1,:-2]
Wsub[:,-1] = T[:-1,-1]
M = intrinsic_matrix
# Makes with respect to Camera coordinate system instead of Undistorted
M[:-1,-1] = 0
Hinv = numpy.dot(M,Wsub)
Hinv = Hinv/Hinv[-1,-1]
H = numpy.linalg.inv(Hinv)
self.Hinv = Hinv
self.H = H
def CameraInfo_callback (self, msgCameraInfo):
if self.camerainfo is None:
self.camerainfo = msgCameraInfo
M = N.reshape(N.array(self.camerainfo.K),[3,3]) #cvNumpy.mat_to_array(N.array(self.camerainfo.K))
#M = N.reshape(N.array(self.camerainfo.P),[3,4])[0:3,0:3]
M[:-1,-1] = 0 # Zero the translation entries (1,3) and (2,3).
(rvec, tvec) = CameraParameters.extrinsic("plate")
rvec = cvNumpy.mat_to_array(rvec).squeeze()
tvec = cvNumpy.mat_to_array(tvec).squeeze()
angleRvec = N.linalg.norm(rvec)
R = tf.transformations.rotation_matrix(angleRvec, rvec)
T = tf.transformations.translation_matrix(tvec)
self.Wsub = N.zeros((3,3))
self.Wsub[:,:-1] = R[:-1,:-2]
self.Wsub[:,-1] = T[:-1,-1]
self.Hinv = N.dot(M, self.Wsub)
#rospy.logwarn ('Hinv scalar %f' % self.Hinv[-1,-1])
self.Hinv = self.Hinv / self.Hinv[-1,-1]
self.H = N.linalg.inv(self.Hinv)
def __init__(self):
self.initialized = False
self.images_initialized = False
self.pose_initialized = False
self.arrays_initialized = False
self.joy_sub = rospy.Subscriber("Joystick/Commands", JoystickCommands, self.commands_callback)
self.vel_pub = rospy.Publisher("stage/command_velocity",Velocity)
self.image_sub = rospy.Subscriber("UndistortedImage", Image, self.image_callback)
self.contour_info_sub = rospy.Subscriber("ContourInfo",ContourInfo,self.contour_callback)
# self.pose_sub = rospy.Subscriber("RobotImagePose", PoseStamped, self.pose_callback)
cv.NamedWindow("Stage Plate Calibration", 1)
self.vel_stage = Velocity()
self.vel_scale_factor = 40 # mm/s
self.robot_image_pose_camera = PoseStamped()
self.robot_image_pose_undistorted = PoseStamped()
self.robot_image_pose_plate = PoseStamped()
self.bridge = CvBridge()
self.color_max = 255
self.font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX,0.5,0.5)
self.font_color = cv.CV_RGB(self.color_max,0,0)
self.sample_dist_min = 20
self.point_count_min = 10
self.dist_min = 25
self.error_text = ""
self.robot_ecc = 0
self.robot_area = 0
self.robot_ecc_array = numpy.array([])
self.robot_area_array = numpy.array([])
self.robot_min_ecc = 1000000000
self.robot_max_ecc = 0
self.robot_min_area = 1000000000
self.robot_max_area = 0
# self.plate_point_array = numpy.zeros((1,3))
# self.stage_point_array = numpy.zeros((1,3))
(self.intrinsic_matrix,self.distortion_coeffs) = CameraParameters.intrinsic("undistorted")
(self.rvec,self.tvec) = CameraParameters.extrinsic("plate")
self.origin_points = cv.CreateMat(4,3,cv.CV_32FC1)
self.origin_points_projected = cv.CreateMat(4,2,cv.CV_32FC1)
self.checker_size = 15
self.num_grid_lines = 9
self.start_points_projected = cv.CreateMat(self.num_grid_lines,2,cv.CV_32FC1)
self.end_points_projected = cv.CreateMat(self.num_grid_lines,2,cv.CV_32FC1)
self.rotate_grid = False
self.tf_listener = tf.TransformListener()
got_trans = False
while not got_trans:
try:
(self.camera_undistorted_trans,rot) = self.tf_listener.lookupTransform('/UndistortedImage', '/Camera', rospy.Time(0))
got_trans = True
except:
rospy.logdebug("tf_listener.lookupTransform threw exception \n")
rospy.wait_for_service('camera_to_plate')
try:
self.camera_to_plate = rospy.ServiceProxy('camera_to_plate', PlateCameraConversion)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
