Ejemplo n.º 1
0
def test_distortion():
    base = CameraModel.load_camera_default()
    lookat = np.array( (0.0, 0.0, 0.0) )
    up = np.array( (0.0, 0.0, 1.0) )

    cams = []
    cams.append(  base.get_view_camera(eye=np.array((1.0,0.0,1.0)),lookat=lookat,up=up) )

    distortion1 = np.array( [0.2, 0.3, 0.1, 0.1, 0.1] )
    cam_wide = CameraModel.load_camera_simple(name='cam_wide',
                                              fov_x_degrees=90,
                                              eye=np.array((-1.0,-1.0,0.7)),
                                              lookat=lookat,
                                              distortion_coefficients=distortion1,
                                              )
    cams.append(cam_wide)

    cam_ids = []
    for i in range(len(cams)):
        cams[i].name = 'cam%02d'%i
        cam_ids.append(cams[i].name)

    cam_system = MultiCameraSystem(cams)
    R = reconstruct.Reconstructor.from_pymvg(cam_system)
    for cam_id in cam_ids:
        nl_params = R.get_intrinsic_nonlinear(cam_id)
        mvg_cam = cam_system.get_camera_dict()[cam_id]
        assert np.allclose(mvg_cam.distortion, nl_params)
Ejemplo n.º 2
0
def test_equals():
    system = make_default_system()
    assert system != 1234

    system2 = MultiCameraSystem(
        [CameraModel.load_camera_simple(name='cam%d' % i) for i in range(2)])
    system3 = MultiCameraSystem(
        [CameraModel.load_camera_simple(name='cam%d' % i) for i in range(3)])
    assert system2 != system3

    system4 = make_default_system()
    d = system4.to_dict()
    d['camera_system'][0]['width'] += 1
    system5 = MultiCameraSystem.from_dict(d)
    assert system4 != system5
Ejemplo n.º 3
0
def test_pymvg_file_in_docs():
    pymvg_src_path = pymvg.__file__
    pymvg_base = os.path.split(pymvg_src_path)[0]
    pymvg_src_dir = os.path.join(pymvg_base, '..')
    fname = os.path.join(pymvg_src_dir, 'docs', 'source',
                         'pymvg_camsystem_example.json')
    system = MultiCameraSystem.from_pymvg_file(fname)
Ejemplo n.º 4
0
def transform_system(old, s, R, T):
    new_cams = []
    for name in old.get_names():
        orig_cam = old.get_camera(name)
        new_cam = orig_cam.get_aligned_camera(s, R, T)
        new_cams.append(new_cam)
    return MultiCameraSystem(new_cams)
Ejemplo n.º 5
0
def test_align():
    system1 = make_default_system()
    system2 = system1.get_aligned_copy(system1)  # This should be a no-op.
    assert system1 == system2

    system3 = MultiCameraSystem(
        [CameraModel.load_camera_simple(name='cam%d' % i) for i in range(2)])
    nose.tools.assert_raises(ValueError, system3.get_aligned_copy, system1)
Ejemplo n.º 6
0
def main():
    out_fname = sys.argv[1]
    system1 = make_default_system()
    system1.save_to_pymvg_file( out_fname )

    # just make sure we can actually read it!
    system2 = MultiCameraSystem.from_pymvg_file( out_fname )
    assert system1==system2
Ejemplo n.º 7
0
def test_roundtrip_to_pymvg_file():
    system1 = make_default_system()
    fname = tempfile.mktemp(suffix='.json')
    system1.save_to_pymvg_file( fname )
    try:
        system2 = MultiCameraSystem.from_pymvg_file( fname )
        assert system1==system2
    finally:
        os.unlink( fname )
Ejemplo n.º 8
0
def test_roundtrip_to_pymvg_file():
    system1 = make_default_system()
    fname = tempfile.mktemp(suffix='.json')
    system1.save_to_pymvg_file(fname)
    try:
        system2 = MultiCameraSystem.from_pymvg_file(fname)
        assert system1 == system2
    finally:
        os.unlink(fname)
Ejemplo n.º 9
0
def test_equals():
    system = make_default_system()
    assert system != 1234

    system2 = MultiCameraSystem([CameraModel.load_camera_simple(name='cam%d'%i) for i in range(2)])
    system3 = MultiCameraSystem([CameraModel.load_camera_simple(name='cam%d'%i) for i in range(3)])
    assert system2 != system3

    system4 = make_default_system()
    d = system4.to_dict()
    d['camera_system'][0]['width'] += 1
    system5 = MultiCameraSystem.from_dict( d )
    assert system4 != system5
Ejemplo n.º 10
0
def _get_cams(with_distortion):
    base = CameraModel.load_camera_default()

    lookat = np.array((0.0, 0.0, 0.0))
    up = np.array((0.0, 0.0, 1.0))

    cams = []
    cams.append(
        base.get_view_camera(eye=np.array((1.0, 0.0, 1.0)),
                             lookat=lookat,
                             up=up))
    cams.append(
        base.get_view_camera(eye=np.array((1.2, 3.4, 5.6)),
                             lookat=lookat,
                             up=up))
    cams.append(
        base.get_view_camera(eye=np.array((0, 0.3, 1.0)), lookat=lookat,
                             up=up))

    if with_distortion:
        distortion1 = np.array([0.2, 0.3, 0.1, 0.1, 0.1])
    else:
        distortion1 = np.zeros((5, ))
    cam_wide = CameraModel.load_camera_simple(
        name='cam_wide',
        fov_x_degrees=90,
        eye=np.array((-1.0, -1.0, 0.7)),
        lookat=lookat,
        distortion_coefficients=distortion1,
    )
    cams.append(cam_wide)

    for i in range(len(cams)):
        cams[i].name = 'cam%02d' % i

    cam_system = MultiCameraSystem(cams)
    reconstructor = Reconstructor.from_pymvg(cam_system)
    result = dict(
        cams=cams,
        cam_system=cam_system,
        reconstructor=reconstructor,
    )
    return result
Ejemplo n.º 11
0
def make_default_system():
    '''helper function to generate an instance of MultiCameraSystem'''
    lookat = np.array((0.0, 0.0, 0.0))

    center1 = np.array((0.0, 0.0, 5.0))
    distortion1 = np.array([0.2, 0.3, 0.1, 0.1, 0.1])
    cam1 = CameraModel.load_camera_simple(
        name='cam1',
        fov_x_degrees=90,
        eye=center1,
        lookat=lookat,
        distortion_coefficients=distortion1,
    )

    center2 = np.array((0.5, 0.0, 0.0))
    cam2 = CameraModel.load_camera_simple(
        name='cam2',
        fov_x_degrees=90,
        eye=center2,
        lookat=lookat,
    )

    center3 = np.array((0.5, 0.5, 0.0))
    cam3 = CameraModel.load_camera_simple(
        name='cam3',
        fov_x_degrees=90,
        eye=center3,
        lookat=lookat,
    )

    center4 = np.array((0.5, 0.0, 0.5))
    cam4 = CameraModel.load_camera_simple(
        name='cam4',
        fov_x_degrees=90,
        eye=center4,
        lookat=lookat,
    )

    cameras = [cam1, cam2, cam3, cam4]
    system = MultiCameraSystem(cameras)
    return system
Ejemplo n.º 12
0
def main(args):

    if not path.exists(args.data):
        raise ValueError("Data path does not exist!")

    # Load calibration:
    cam_path = path.join(args.data, "camera_system_aligned.json")
    camera_system = MultiCameraSystem.from_pymvg_file(cam_path)

    # Load data:
    data = load_data(args.data, camera_system)

    # Filter by area:
    data = data[data.area > args.area_filter]

    # Select a range:
    if args.frame_range is None:
        a, b = 0, data.index.levels[1].max()
    else:
        a, b = args.frame_range
    data = data.loc[pd.IndexSlice[:, a:b], :]

    data = data.reset_index().set_index(["camera_id",
                                         "frame_number"]).sort_index()

    # Set up reconstruction:
    rec = FastSeqH(camera_system, minimum_tracks=args.minimum_tracks)

    # Run:
    output = multi_reconstruct(data, rec, args).reset_index().set_index(
        ["frame_number", "point_id"])

    # Save:
    output_directory = path.join(args.data, "reconstruction")
    if not path.exists(output_directory):
        os.mkdir(output_directory)

    output_path = path.join(output_directory, "points.csv")
    output.to_csv(output_path)
Ejemplo n.º 13
0
def make_default_system(with_separate_distorions=False):
    '''helper function to generate an instance of MultiCameraSystem'''
    if with_separate_distorions:
        raise NotImplementedError
    camxs = np.linspace(-2, 2, 3)
    camzs = np.linspace(-2, 2, 3).tolist()
    camzs.pop(1)
    cameras = []
    lookat = np.array((0.0, 0, 0))
    up = np.array((0.0, 0, 1))

    for enum, camx in enumerate(camxs):
        center = np.array((camx, -5, 0))
        name = 'camx_%d' % (enum + 1, )
        cam = CameraModel.load_camera_simple(
            name=name,
            fov_x_degrees=45,
            eye=center,
            lookat=lookat,
            up=up,
        )
        cameras.append(cam)

    for enum, camz in enumerate(camzs):
        center = np.array((0, -5, camz))
        name = 'camz_%d' % (enum + 1, )
        cam = CameraModel.load_camera_simple(
            name=name,
            fov_x_degrees=45,
            eye=center,
            lookat=lookat,
            up=up,
        )
        cameras.append(cam)

    system = MultiCameraSystem(cameras)
    return system
Ejemplo n.º 14
0
def build_multi_camera_system(cameras, no_distortion=False):
    """
    Build a multi-camera system with pymvg package for triangulation

    Args:
        cameras: list of camera parameters
    Returns:
        cams_system: a multi-cameras system
    """
    pymvg_cameras = []
    for (name, camera) in cameras:
        R, T, f, c, k, p = unfold_camera_param(camera, avg_f=False)
        camera_matrix = np.array(
            [[f[0], 0, c[0]], [0, f[1], c[1]], [0, 0, 1]], dtype=float)
        proj_matrix = np.zeros((3, 4))
        proj_matrix[:3, :3] = camera_matrix
        distortion = np.array([k[0], k[1], p[0], p[1], k[2]])
        distortion.shape = (5,)
        T = -np.matmul(R, T)
        M = camera_matrix.dot(np.concatenate((R, T), axis=1))
        camera = CameraModel.load_camera_from_M(
            M, name=name, distortion_coefficients=None if no_distortion else distortion)
        pymvg_cameras.append(camera)
    return MultiCameraSystem(pymvg_cameras)
Ejemplo n.º 15
0
def test_pymvg_file_in_docs():
    pymvg_src_path = pymvg.__file__
    pymvg_base = os.path.split(pymvg_src_path)[0]
    pymvg_src_dir = os.path.join(pymvg_base,'..')
    fname =  os.path.join( pymvg_src_dir, 'docs', 'source', 'pymvg_camsystem_example.json')
    system = MultiCameraSystem.from_pymvg_file( fname )
Ejemplo n.º 16
0
        print "~~~~~~~~~~~~~~~~~~~~~~~~~~"
        print "Importing {0}".format(calib_id[idx])

        pmat = calib[idx][0]
        distortion = calib[idx][1]
        name = calib_id[idx]
        width = cam_settings[name]["f7"]["width"]
        height = cam_settings[name]["f7"]["height"]

        print pmat
        print distortion

        camera = CameraModel.load_camera_from_M(pmat, width=width, height=height, name=name,
                                                distortion_coefficients=distortion)

        cameras.append(camera)

    system = MultiCameraSystem(cameras)
    system.save_to_pymvg_file(join(CALIB, "camera_system.json"))

    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')

    # plot_system(ax, system)
    for name in system.get_names():
        plot_camera(ax, system.get_camera(name))

    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_zlabel('z')
    plt.show()
Ejemplo n.º 17
0
def test_roundtrip_to_dict():
    system1 = make_default_system()
    system2 = MultiCameraSystem.from_dict( system1.to_dict() )
    assert system1==system2
Ejemplo n.º 18
0
def test_roundtrip_to_str():
    system1 = make_default_system()
    buf = system1.get_pymvg_str()
    system2 = MultiCameraSystem.from_pymvg_str( buf )
    assert system1==system2
def triangulate_points(cameras,
                       filtered_applied,
                       stereo_triangulation,
                       min_likelihood=0.7):
    if len(cameras) < 2:
        raise Exception('Triangulation process needs at least two cameras')
    number_of_frames = len(cameras[0].frames)
    number_of_markers = len(cameras[0].frames[0].markers)
    triangulated_frames = []
    stereo_pair = None

    if stereo_triangulation:
        stereo_pair = get_best_pair(cameras)

    # set up filter values
    dt = 1.0 / 24
    transition_matrix = np.eye(9, dtype=np.float32)
    transition_matrix[0][3] = dt
    transition_matrix[0][6] = 0.5 * dt * dt
    transition_matrix[1][4] = dt
    transition_matrix[1][7] = 0.5 * dt * dt
    transition_matrix[2][5] = dt
    transition_matrix[2][8] = 0.5 * dt * dt
    measurement_matrix = np.array([(1, 0, 0, 0, 0, 0, 0, 0, 0),
                                   (0, 1, 0, 0, 0, 0, 0, 0, 0),
                                   (0, 0, 1, 0, 0, 0, 0, 0, 0)],
                                  dtype=np.float32)

    # init filters for all markers tracked
    filters = []
    for i in range(number_of_markers):
        kalman_filter = cv2.KalmanFilter(9, 3, 0)
        kalman_filter.transitionMatrix = transition_matrix
        kalman_filter.measurementMatrix = measurement_matrix
        filters.append(Filter(kalman_filter))
    for i in range(number_of_frames):
        triangulated_markers = []
        for j in range(number_of_markers):
            visible_counter = 0
            for camera in cameras:
                if camera.frames[i].markers[j].likelihood > 0 and \
                        ((stereo_triangulation and (camera in stereo_pair[0] or camera in stereo_pair[1]))
                         or not stereo_triangulation):
                    visible_counter += 1

            if visible_counter < 2:
                continue

            # check if old stereo triangulation method is used
            if stereo_triangulation:
                best_cameras = get_front_back_cameras_for_marker(
                    stereo_pair, i, j, 0.5)
                triangulated = triangulate_point(best_cameras, i, j,
                                                 best_cameras[0].image_size)
            else:
                # use n view triangulation method
                best_cameras = get_best_cameras(cameras, i, j, len(cameras),
                                                min_likelihood)
                system = MultiCameraSystem([cam.model for cam in best_cameras])
                points = [
                    (cam.model.name,
                     [cam.frames[i].markers[j].x, cam.frames[i].markers[j].y])
                    for cam in best_cameras
                ]
                triangulated = system.find3d(points)
            average_likelihood = np.mean(
                [cam.frames[i].markers[j].likelihood for cam in best_cameras])
            point_triangulated = triangulated is not None and average_likelihood > min_likelihood
            marker_key = best_cameras[0].frames[i].markers[j].marker_key

            if point_triangulated:
                # check if kalman filter is necessary
                if filtered_applied:
                    triangulated_ec_world_frame_formatted = np.array(
                        ([triangulated]), np.float32).T
                    # compensate for the initial state set to 0,0,0 in opencv kalman filter
                    if filters[j].first:
                        for l in range(100):
                            filters[j].filter.predict()
                            filters[j].filter.correct(
                                triangulated_ec_world_frame_formatted)
                        filters[j].first = False
                    filters[j].filter.predict()
                    estimated = filters[j].filter.correct(
                        triangulated_ec_world_frame_formatted)
                    # append triangulated point
                    triangulated_markers.append({
                        'point':
                        np.array([
                            estimated[0][0], estimated[1][0], estimated[2][0]
                        ]),
                        'marker':
                        marker_key,
                        'cameras':
                        "".join([str(cam.number) for cam in best_cameras]),
                        'likelihood':
                        str(average_likelihood)
                    })
                else:
                    # append triangulated point
                    triangulated_markers.append({
                        'point':
                        triangulated,
                        'marker':
                        marker_key,
                        'cameras':
                        "".join([str(cam.number) for cam in best_cameras]),
                        'likelihood':
                        str(average_likelihood)
                    })

        triangulated_frames.append(triangulated_markers)
    return triangulated_frames
Ejemplo n.º 20
0
def check_mcsc_roundtrip(with_rad_files=False,align_existing=False):
    np.random.seed(3)
    mcscdir = os.path.join( os.path.dirname(mcsce.__file__),
                            '..', '..', 'MultiCamSelfCal' )
    mcscdir = os.path.abspath(mcscdir)
    out_dirname = tempfile.mkdtemp()
    try:
        mcsc = mcsce.MultiCamSelfCal(out_dirname=out_dirname,mcscdir=mcscdir)
        if with_rad_files:
            orig_cams = make_default_system(with_separate_distorions=True)
        else:
            orig_cams = make_default_system()

        cam_resolutions = dict(  (n['name'],  (n['width'],n['height']))
                               for n in orig_cams.to_dict()['camera_system'])

        pts_3d = get_default_points()

        if 0:
            import matplotlib.pyplot as plt
            from mpl_toolkits.mplot3d import Axes3D

            from pymvg.plot_utils import plot_camera
            np.set_printoptions(precision=3, suppress=True)
            fig = plt.figure()
            ax = fig.add_subplot(111, projection='3d')
            for name,cam in orig_cams.get_camera_dict().iteritems():
                print()
                print(name,'-'*80)
                print('  center:',cam.get_camcenter())
                print('  lookat:',cam.get_lookat())
                print('  up:',cam.get_up())
                print('  P')
                print(cam.P)
                print()
                plot_camera( ax, cam)#, scale = dim/5.0 )
            ax.plot( pts_3d[:,0], pts_3d[:,1], pts_3d[:,2], 'k.')
            plt.show()

        cam_points=defaultdict(list)

        nan_tup = (np.nan, np.nan)
        for pt_3d in pts_3d:
            valid = True
            for name in orig_cams.get_names():
                w,h = cam_resolutions[name]

                x,y = orig_cams.find2d( name, pt_3d )

                if 1:
                    # add noise to the 2D images
                    xn, yn = 0.01*np.random.randn( 2 )

                    x += xn
                    y += yn

                if (0 <= x) and (x < w) and (0 <= y) and (y < h):
                    cam_points[name].append((x,y))
                else:
                    cam_points[name].append(nan_tup)
                    valid = False
            if not valid:
                for name in orig_cams.get_names():
                    cam_points[name].pop()

        cam_ids=orig_cams.get_names()
        if with_rad_files:
            cam_calibrations = {} # dictionary of .yaml filenames with ROS yaml format
            raise NotImplementedError
        else:
            cam_calibrations = {}

        if align_existing:
            cam_centers = np.array([ orig_cams.get_cam(n).get_camcenter() for n in orig_cams.get_names() ]).T
        else:
            cam_centers = []

        mcsc.create_from_cams(cam_ids=cam_ids,
                              cam_resolutions=cam_resolutions,
                              cam_points=cam_points,
                              cam_calibrations=cam_calibrations,
                              cam_centers=cam_centers,
                              )

        result = mcsc.execute(silent=True)
        raw_cams = MultiCameraSystem.from_mcsc( result )
        if align_existing:
            aligned_cams = raw_cams
        else:
            aligned_cams = raw_cams.get_aligned_copy( orig_cams )

        assert is_close(orig_cams,aligned_cams,pts_3d)
    finally:
        shutil.rmtree(out_dirname)
Ejemplo n.º 21
0
Archivo: utils.py Proyecto: 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
Ejemplo n.º 22
0
def test_pymvg_roundtrip():
    from pymvg.camera_model import CameraModel
    from pymvg.multi_camera_system import MultiCameraSystem
    from flydra_core.reconstruct import Reconstructor

    # ----------- with no distortion ------------------------
    center1 = np.array( (0, 0.0, 5) )
    center2 = np.array( (1, 0.0, 5) )

    lookat = np.array( (0,1,0))

    cam1 = CameraModel.load_camera_simple(fov_x_degrees=90,
                                          name='cam1',
                                          eye=center1,
                                          lookat=lookat)
    cam2 = CameraModel.load_camera_simple(fov_x_degrees=90,
                                          name='cam2',
                                          eye=center2,
                                          lookat=lookat)
    mvg = MultiCameraSystem( cameras=[cam1, cam2] )
    R = Reconstructor.from_pymvg(mvg)
    mvg2 = R.convert_to_pymvg()

    cam_ids = ['cam1','cam2']
    for distorted in [True,False]:
        for cam_id in cam_ids:
            v1 = mvg.find2d(  cam_id, lookat, distorted=distorted )
            v2 = R.find2d(    cam_id, lookat, distorted=distorted )
            v3 = mvg2.find2d( cam_id, lookat, distorted=distorted )
            assert np.allclose(v1,v2)
            assert np.allclose(v1,v3)

    # ----------- with distortion ------------------------
    cam1dd = cam1.to_dict()
    cam1dd['D'] = (0.1, 0.2, 0.3, 0.4, 0.0)
    cam1d = CameraModel.from_dict(cam1dd)

    cam2dd = cam2.to_dict()
    cam2dd['D'] = (0.11, 0.21, 0.31, 0.41, 0.0)
    cam2d = CameraModel.from_dict(cam2dd)

    mvgd = MultiCameraSystem( cameras=[cam1d, cam2d] )
    Rd = Reconstructor.from_pymvg(mvgd)
    mvg2d = Rd.convert_to_pymvg()
    cam_ids = ['cam1','cam2']
    for distorted in [True,False]:
        for cam_id in cam_ids:
            v1 = mvgd.find2d(  cam_id, lookat, distorted=distorted )
            v2 = Rd.find2d(    cam_id, lookat, distorted=distorted )
            v3 = mvg2d.find2d( cam_id, lookat, distorted=distorted )
            assert np.allclose(v1,v2)
            assert np.allclose(v1,v3)

    # ------------ with distortion at different focal length ------
    mydir = os.path.dirname(__file__)
    caldir = os.path.join(mydir,'sample_calibration')
    print mydir
    print caldir
    R3 = Reconstructor(caldir)
    mvg3 = R3.convert_to_pymvg()
    #R4 = Reconstructor.from_pymvg(mvg3)
    mvg3b = MultiCameraSystem.from_mcsc( caldir )

    for distorted in [True,False]:
        for cam_id in R3.cam_ids:
            v1 = R3.find2d(   cam_id, lookat, distorted=distorted )
            v2 = mvg3.find2d( cam_id, lookat, distorted=distorted )
            #v3 = R4.find2d(   cam_id, lookat, distorted=distorted )
            v4 = mvg3b.find2d( cam_id, lookat, distorted=distorted )
            assert np.allclose(v1,v2)
            #assert np.allclose(v1,v3)
            assert np.allclose(v1,v4)
Ejemplo n.º 23
0
def check_mcsc_roundtrip(with_rad_files=False, align_existing=False):
    np.random.seed(3)
    mcscdir = os.path.join(os.path.dirname(mcsce.__file__), '..', '..',
                           'MultiCamSelfCal')
    mcscdir = os.path.abspath(mcscdir)
    out_dirname = tempfile.mkdtemp()
    try:
        mcsc = mcsce.MultiCamSelfCal(out_dirname=out_dirname, mcscdir=mcscdir)
        if with_rad_files:
            orig_cams = make_default_system(with_separate_distorions=True)
        else:
            orig_cams = make_default_system()

        cam_resolutions = dict((n['name'], (n['width'], n['height']))
                               for n in orig_cams.to_dict()['camera_system'])

        pts_3d = get_default_points()

        if 0:
            import matplotlib.pyplot as plt
            from mpl_toolkits.mplot3d import Axes3D

            from pymvg.plot_utils import plot_camera
            np.set_printoptions(precision=3, suppress=True)
            fig = plt.figure()
            ax = fig.add_subplot(111, projection='3d')
            for name, cam in orig_cams.get_camera_dict().iteritems():
                print()
                print(name, '-' * 80)
                print('  center:', cam.get_camcenter())
                print('  lookat:', cam.get_lookat())
                print('  up:', cam.get_up())
                print('  P')
                print(cam.P)
                print()
                plot_camera(ax, cam)  #, scale = dim/5.0 )
            ax.plot(pts_3d[:, 0], pts_3d[:, 1], pts_3d[:, 2], 'k.')
            plt.show()

        cam_points = defaultdict(list)

        nan_tup = (np.nan, np.nan)
        for pt_3d in pts_3d:
            valid = True
            for name in orig_cams.get_names():
                w, h = cam_resolutions[name]

                x, y = orig_cams.find2d(name, pt_3d)

                if 1:
                    # add noise to the 2D images
                    xn, yn = 0.01 * np.random.randn(2)

                    x += xn
                    y += yn

                if (0 <= x) and (x < w) and (0 <= y) and (y < h):
                    cam_points[name].append((x, y))
                else:
                    cam_points[name].append(nan_tup)
                    valid = False
            if not valid:
                for name in orig_cams.get_names():
                    cam_points[name].pop()

        cam_ids = orig_cams.get_names()
        if with_rad_files:
            cam_calibrations = {
            }  # dictionary of .yaml filenames with ROS yaml format
            raise NotImplementedError
        else:
            cam_calibrations = {}

        if align_existing:
            cam_centers = np.array([
                orig_cams.get_cam(n).get_camcenter()
                for n in orig_cams.get_names()
            ]).T
        else:
            cam_centers = []

        mcsc.create_from_cams(
            cam_ids=cam_ids,
            cam_resolutions=cam_resolutions,
            cam_points=cam_points,
            cam_calibrations=cam_calibrations,
            cam_centers=cam_centers,
        )

        result = mcsc.execute(silent=True)
        raw_cams = MultiCameraSystem.from_mcsc(result)
        if align_existing:
            aligned_cams = raw_cams
        else:
            aligned_cams = raw_cams.get_aligned_copy(orig_cams)

        assert is_close(orig_cams, aligned_cams, pts_3d)
    finally:
        shutil.rmtree(out_dirname)
Ejemplo n.º 24
0
def test_roundtrip_to_str():
    system1 = make_default_system()
    buf = system1.get_pymvg_str()
    system2 = MultiCameraSystem.from_pymvg_str(buf)
    assert system1 == system2
Ejemplo n.º 25
0
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
Ejemplo n.º 26
0
    print cam.get_camera_info()

    cam.set_property_values(cam_settings[sn])
    cam.set_format_configuration(**cam_settings[sn]["f7"])
    cam.set_trigger(True)

    cam.start_capture()
    time.sleep(0.5)

    return cam


if __name__ == "__main__":

    # Load Camera System:
    camsys = MultiCameraSystem.from_pymvg_file(BASE_CALIB)

    # Set up sync:
    sync = FrameSync()
    sync.set_framerate(30, duty_cycle=0.5)
    sync.start()

    time.sleep(1)

    cams = [set_up_camera(sn) for sn in cam_settings.keys()]
    camid = [cam.get_camera_info()["serial_number"] for cam in cams]

    cv2.namedWindow("viewer", cv2.WINDOW_AUTOSIZE)
    starttime = time.time()

    update_rate = 1.0
Ejemplo n.º 27
0
def test_load_mcsc():
    mydir = os.path.dirname(__file__)
    mcsc_dirname = os.path.join(mydir,'mcsc_output_20130726')
    cam_system = MultiCameraSystem.from_mcsc( mcsc_dirname )
Ejemplo n.º 28
0
    new_cams = []
    for name in old.get_names():
        orig_cam = old.get_camera(name)
        new_cam = orig_cam.get_aligned_camera(s, R, T)
        new_cams.append(new_cam)
    return MultiCameraSystem(new_cams)


if __name__ == "__main__":

    images, camids = take_images()
    points = run_point_select(camids, images)

    # np.save("points", points)

    camera_system = MultiCameraSystem.from_pymvg_file(path.join(BASE_CALIBRATION, "camera_system.json"))

    # points = np.load("points.npy")
    final_points = triangulate_points(camera_system, points)
    sorted_points = identify_points(final_points)
    plot_reprojection(camera_system, images, sorted_points)

    s, R, T = align.estsimt(sorted_points.T, CALIBRATED_COORDINATES.T)
    new_system = transform_system(camera_system, s, R, T)

    new_system.save_to_pymvg_file(path.join(BASE_CALIBRATION, "camera_system_aligned.json"))

    # DEBUGGING:

    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
Ejemplo n.º 29
0
def test_load_mcsc():
    mydir = os.path.dirname(__file__)
    mcsc_dir = os.path.join(mydir, 'external', 'mcsc')
    mcsc_dirname = os.path.join(mcsc_dir, 'mcsc_output_20130726')
    cam_system = MultiCameraSystem.from_mcsc(mcsc_dirname)
Ejemplo n.º 30
0
def test_roundtrip_to_dict():
    system1 = make_default_system()
    system2 = MultiCameraSystem.from_dict(system1.to_dict())
    assert system1 == system2