コード例 #1
0
def check_extrinsic_msg(cam_opts):
    """check that ROS message contains actual camera extrinsic parameters"""
    cam_opts = cam_opts.copy()
    cam_opts['get_input_data']=True
    r = _build_test_camera(**cam_opts)
    cam = r['cam']
    tfm = cam.get_extrinsics_as_bunch()
    if 'translation' in r:
        assert np.allclose(point_msg_to_tuple(tfm.translation), point_msg_to_tuple(r['translation']))
    if 'rotation' in r:
        assert np.allclose(parse_rotation_msg(tfm.rotation,force_matrix=True),
                           parse_rotation_msg(r['rotation'],force_matrix=True))
コード例 #2
0
ファイル: utils.py プロジェクト: wx-b/pymvg
def _build_test_camera(**kwargs):
    o = Bunch(**kwargs)
    if not kwargs.get('at_origin',False):
        translation = geometry_msgs.msg.Point()
        translation.x = 0.273485679077
        translation.y = 0.0707310128808
        translation.z = 0.0877802104531

        rotation = geometry_msgs.msg.Quaternion()
        rotation.x = 0.309377331102
        rotation.y = 0.600893485738
        rotation.z = 0.644637681813
        rotation.w = 0.357288321925
    else:
        translation = geometry_msgs.msg.Point()
        translation.x = 0.0
        translation.y = 0.0
        translation.z = 0.0

        rotation = geometry_msgs.msg.Quaternion()
        rotation.x = 0.0
        rotation.y = 0.0
        rotation.z = 0.0
        rotation.w = 1.0

    if 'from_file' in kwargs:
        if kwargs['type']=='ros':
            cam = CameraModel.load_camera_from_file( fname=kwargs['filename'],
                                                     extrinsics_required=False )
            i = cam.get_intrinsics_as_bunch()
            cam = CameraModel._from_parts(
                              translation=point_msg_to_tuple(translation),
                              rotation=parse_rotation_msg(rotation),
                              intrinsics=i,
                              name='cam',
                              )
        elif kwargs['type']=='pymvg':
            del translation
            del rotation
            system = MultiCameraSystem.from_pymvg_file(fname=kwargs['filename'])
            names = system.get_names()

            names.sort()
            name = names[0] # get first camera from system
            cam = system._cameras[name]

            rotation = cam.get_extrinsics_as_bunch().rotation
            translation = cam.get_extrinsics_as_bunch().translation
        else:
            raise ValueError('unknown file type')

    else:

        if o.ROS_test_data:
            i = sensor_msgs.msg.CameraInfo()
            # these are from image_geometry ROS package in the utest.cpp file
            i.height = 480
            i.width = 640
            i.distortion_model = 'plumb_bob'
            i.D = [-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0]
            i.K = [430.15433020105519,                0.0, 311.71339830549732,
                                         0.0, 430.60920415473657, 221.06824942698509,
                                         0.0,                0.0,                1.0]
            i.R = [0.99806560714807102, 0.0068562422224214027, 0.061790256276695904,
                          -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664,
                          -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516]
            i.P = [295.53402059708782, 0.0, 285.55760765075684, 0.0,
                          0.0, 295.53402059708782, 223.29617881774902, 0.0,
                          0.0, 0.0, 1.0, 0.0]
        else:
            i = sensor_msgs.msg.CameraInfo()
            i.height = 494
            i.width = 659
            i.distortion_model = 'plumb_bob'
            i.D = [-0.34146457767225, 0.196070795764995, 0.000548988393912233, 0.000657058395082583, -0.0828776806503243]
            i.K = [516.881868241566, 0.0, 333.090936517613, 0.0, 517.201263180996, 231.526036849886, 0.0, 0.0, 1.0]
            i.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
            i.P = [442.17529296875, 0.0, 334.589001099812, 0.0, 0.0, 474.757141113281, 228.646131377705, 0.0, 0.0, 0.0, 1.0, 0.0]

        cam = CameraModel._from_parts(
                          translation=point_msg_to_tuple(translation),
                          rotation=parse_rotation_msg(rotation),
                          intrinsics=i,
                          name='cam',
                          )
    if kwargs.get('flipped',False):
        cam = cam.get_flipped_camera()
    if kwargs.get('get_input_data',False):
        return dict(cam=cam,
                    translation=translation,
                    rotation=rotation,
                    )
    return cam
コード例 #3
0
ファイル: utils.py プロジェクト: thinkerahead/pymvg
def _build_test_camera(**kwargs):
    o = Bunch(**kwargs)
    if not kwargs.get('at_origin',False):
        translation = geometry_msgs.msg.Point()
        translation.x = 0.273485679077
        translation.y = 0.0707310128808
        translation.z = 0.0877802104531

        rotation = geometry_msgs.msg.Quaternion()
        rotation.x = 0.309377331102
        rotation.y = 0.600893485738
        rotation.z = 0.644637681813
        rotation.w = 0.357288321925
    else:
        translation = geometry_msgs.msg.Point()
        translation.x = 0.0
        translation.y = 0.0
        translation.z = 0.0

        rotation = geometry_msgs.msg.Quaternion()
        rotation.x = 0.0
        rotation.y = 0.0
        rotation.z = 0.0
        rotation.w = 1.0

    if 'from_file' in kwargs:
        if kwargs['type']=='ros':
            cam = CameraModel.load_camera_from_file( fname=kwargs['filename'],
                                                     extrinsics_required=False )
            i = cam.get_intrinsics_as_bunch()
            cam = CameraModel._from_parts(
                              translation=point_msg_to_tuple(translation),
                              rotation=parse_rotation_msg(rotation),
                              intrinsics=i,
                              name='cam',
                              )
        elif kwargs['type']=='pymvg':
            del translation
            del rotation
            system = MultiCameraSystem.from_pymvg_file(fname=kwargs['filename'])
            names = system.get_names()

            names.sort()
            name = names[0] # get first camera from system
            cam = system._cameras[name]

            rotation = cam.get_extrinsics_as_bunch().rotation
            translation = cam.get_extrinsics_as_bunch().translation
        else:
            raise ValueError('unknown file type')

    else:

        if o.ROS_test_data:
            i = sensor_msgs.msg.CameraInfo()
            # these are from image_geometry ROS package in the utest.cpp file
            i.height = 480
            i.width = 640
            i.distortion_model = 'plumb_bob'
            i.D = [-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0]
            i.K = [430.15433020105519,                0.0, 311.71339830549732,
                                         0.0, 430.60920415473657, 221.06824942698509,
                                         0.0,                0.0,                1.0]
            i.R = [0.99806560714807102, 0.0068562422224214027, 0.061790256276695904,
                          -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664,
                          -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516]
            i.P = [295.53402059708782, 0.0, 285.55760765075684, 0.0,
                          0.0, 295.53402059708782, 223.29617881774902, 0.0,
                          0.0, 0.0, 1.0, 0.0]
        else:
            i = sensor_msgs.msg.CameraInfo()
            i.height = 494
            i.width = 659
            i.distortion_model = 'plumb_bob'
            i.D = [-0.34146457767225, 0.196070795764995, 0.000548988393912233, 0.000657058395082583, -0.0828776806503243]
            i.K = [516.881868241566, 0.0, 333.090936517613, 0.0, 517.201263180996, 231.526036849886, 0.0, 0.0, 1.0]
            i.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
            i.P = [442.17529296875, 0.0, 334.589001099812, 0.0, 0.0, 474.757141113281, 228.646131377705, 0.0, 0.0, 0.0, 1.0, 0.0]

        cam = CameraModel._from_parts(
                          translation=point_msg_to_tuple(translation),
                          rotation=parse_rotation_msg(rotation),
                          intrinsics=i,
                          name='cam',
                          )
    if kwargs.get('flipped',False):
        cam = cam.get_flipped_camera()
    if kwargs.get('get_input_data',False):
        return dict(cam=cam,
                    translation=translation,
                    rotation=rotation,
                    )
    return cam
コード例 #4
0
    def load_camera_from_opened_bagfile( cls, bag, extrinsics_required=True ): # pragma: no cover
        """factory function for class CameraModel
        arguments
        ---------
        bag - an opened rosbag.Bag instance
        extrinsics_required - are extrinsic parameters required
        """
        camera_name = None
        translation = None
        rotation = None
        intrinsics = None

        for topic, msg, t in bag.read_messages():
            if 1:
                parts = topic.split('/')
                if parts[0]=='':
                    parts = parts[1:]
                topic = parts[-1]
                parts = parts[:-1]
                if len(parts)>1:
                    this_camera_name = '/'.join(parts)
                else:
                    this_camera_name = parts[0]
                # empty, this_camera_name, topic = parts
                # assert empty==''
            if camera_name is None:
                camera_name = this_camera_name
            else:
                assert this_camera_name == camera_name

            if topic == 'tf':
                translation = msg.translation
                rotation = msg.rotation # quaternion
            elif topic == 'matrix_tf':
                translation = msg.translation
                rotation = msg.rotation # matrix
            elif topic == 'camera_info':
                intrinsics = msg
            else:
                warnings.warn('skipping message topic %r'%topic)
                continue

        bag.close()

        if translation is None or rotation is None:
            if extrinsics_required:
                raise ValueError('no extrinsic parameters in bag file')
            else:
                translation = (np.nan, np.nan, np.nan)
                rotation = (np.nan, np.nan, np.nan, np.nan)
        else:
            translation = point_msg_to_tuple(translation)
            rotation = parse_rotation_msg(rotation)

        if intrinsics is None:
            raise ValueError('no intrinsic parameters in bag file')

        result = cls._from_parts(translation=translation,
                                   rotation=rotation,
                                   intrinsics=intrinsics,
                                   name=camera_name,
                                   )
        return result