def parse_frame_message(self, msg, frame_id):
frame = FCFrame()
frame.frame_id = frame_id
# TODO: use uncalibrated transform to approximate point positions in base frame
frame.joint_states = msg.joint_states
frame.neck_H_base = linalg.inv(ros_util.transform_to_matrix(msg.head_transform_base))
cam_H_base = np.dot(self.params.initial_cam_H_neck, frame.neck_H_base)
base_H_cam = linalg.inv(cam_H_base)
# get image points from message
for ii, marker in enumerate(msg.markers):
p = marker.center_pose.position
cf_center_point = np.array([p.x, p.y, p.z], dtype=np.float)
if not np.isfinite(cf_center_point).all():
print 'ERROR: NaN in 2D marker center point!!'
continue
frame.visible_markers.append((marker.id, cf_center_point))
return frame
stereo_cloud = ros_util.pointcloud2_msg_to_cloud(msg.points)
stereo_points = ros_util.cloud_to_points(stereo_cloud, remove_nans=False)
stereo_points_flat = ros_util.cloud_to_points(stereo_cloud, remove_nans=True)
# find plane in pointcloud
planefinder = arm_fiducial_cal.planes.PlaneFinder(
stereo_points_flat, params.ransac_numpoints, params.ransac_numiters, params.inlier_dist
)
planefinder.compute()
P = np.reshape(np.matrix(msg.camera_info.P), (3, 4))
K_inv = linalg.inv(P[:3, :3])
# get the base to neck transform
f.neck_H_base = linalg.inv(ros_util.transform_to_matrix(msg.head_transform_base))
# get image points from message
for ii, marker_i in enumerate(msg.ids):
u1, v1 = msg.u_corner_1[ii], msg.v_corner_1[ii]
u2, v2 = msg.u_corner_2[ii], msg.v_corner_2[ii]
u3, v3 = msg.u_corner_3[ii], msg.v_corner_3[ii]
u4, v4 = msg.u_corner_4[ii], msg.v_corner_4[ii]
corner_points_2d = np.array([(u1, v1), (u2, v2), (u3, v3), (u4, v4)], dtype=float)
if not np.isfinite(corner_points_2d).all():
print "ERROR: NaN in 2D corner points!!"
corner_points_3d = []
for p_2d in corner_points_2d:
p_2d = np.array((p_2d[0], p_2d[1], 1), dtype=np.float)