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)