Ejemplo n.º 1
0
    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
Ejemplo n.º 2
0
    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)