예제 #1
0
def main():
    #solver = g2o.BlockSolverX(g2o.LinearSolverCholmodX())
    solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
    solver = g2o.OptimizationAlgorithmLevenberg(solver)

    optimizer = g2o.SparseOptimizer()
    optimizer.set_verbose(True)
    optimizer.set_algorithm(solver)

    optimizer.load(args.input)
    print('num vertices:', len(optimizer.vertices()))
    print('num edges:', len(optimizer.edges()), end='\n\n')

    optimizer.initialize_optimization()
    optimizer.optimize(args.max_iterations)

    if len(args.output) > 0:
        optimizer.save(args.output)
예제 #2
0
def main():
    optimizer = g2o.SparseOptimizer()
    solver = g2o.BlockSolverX(g2o.LinearSolverCSparseX())
    solver = g2o.OptimizationAlgorithmLevenberg(solver)
    optimizer.set_algorithm(solver)

    true_points = np.hstack([
        np.random.random((1000, 1)) * 3 - 1.5,
        np.random.random((1000, 1)) - 0.5,
        np.random.random((1000, 1)) + 10
    ])

    focal_length = (500, 500)
    principal_point = (320, 240)
    baseline = 0.075
    g2o.VertexSCam.set_cam(*focal_length, *principal_point, baseline)

    for i in range(2):
        t = np.array([0, 0, i])
        cam = g2o.Isometry3d(np.identity(3), t)

        vc = g2o.VertexSCam()
        vc.set_id(i)
        vc.set_estimate(cam)
        if i == 0:
            vc.set_fixed(True)
        vc.set_all()
        optimizer.add_vertex(vc)

    trans0 = optimizer.vertex(0).estimate().inverse()
    trans1 = optimizer.vertex(1).estimate().inverse()

    for i in range(len(true_points)):
        pt0 = trans0 * true_points[i]
        pt1 = trans1 * true_points[i]

        # add noise
        pt0 += np.random.randn(3) * args.pos_noise
        pt1 += np.random.randn(3) * args.pos_noise

        # form edge, with normals in varioius positions
        nm0 = np.array([0, i, 1])
        nm0 = nm0 / np.linalg.norm(nm0)
        nm1 = np.array([0, i, 1])
        nm1 = nm1 / np.linalg.norm(nm1)

        meas = g2o.EdgeGICP()
        meas.pos0 = pt0
        meas.pos1 = pt1
        meas.normal0 = nm0
        meas.normal1 = nm1

        edge = g2o.Edge_V_V_GICP()
        edge.set_vertex(0, optimizer.vertex(0))
        edge.set_vertex(1, optimizer.vertex(1))
        edge.set_measurement(meas)
        edge.set_information(meas.prec0(0.01))

        optimizer.add_edge(edge)

    # set up SBA projections
    if args.num_points > 0:
        true_points = np.hstack([
            np.random.random((args.num_points, 1)) * 3 - 1.5,
            np.random.random((args.num_points, 1)) - 0.5,
            np.random.random((args.num_points, 1)) + 10
        ])

        cam_num = 2
        for i, point in enumerate(true_points):
            vp = g2o.VertexSBAPointXYZ()
            vp.set_id(cam_num + i)
            vp.set_marginalized(True)
            vp.set_estimate(point + np.random.randn(3))
            optimizer.add_vertex(vp)

            for j in range(cam_num):
                z = optimizer.vertex(j).map_point(point)
                if 0 <= z[0] < 640 and 0 <= z[1] < 480:
                    z += np.random.randn(3) * args.pixel_noise * [
                        1, 1, 1 / 16.
                    ]

                    edge = g2o.Edge_XYZ_VSC()
                    edge.set_vertex(0, vp)
                    edge.set_vertex(1, optimizer.vertex(j))
                    edge.set_measurement(z)
                    edge.set_information(np.identity(3))
                    edge.set_robust_kernel(g2o.RobustKernelHuber())

                    optimizer.add_edge(edge)

    # move second cam off of its true position
    vc = optimizer.vertex(1)
    cam = g2o.Isometry3d(vc.estimate().R, np.array([-0.1, -0.1, 0.2]))
    vc.set_estimate(cam)

    optimizer.initialize_optimization()
    optimizer.compute_active_errors()
    print('Initial chi2 =', optimizer.chi2())

    optimizer.set_verbose(True)
    optimizer.optimize(20)

    print('\nSecond vertex should be near [0, 0, 1]')
    print('before optimization:', cam.t)
    print('after  optimization:', optimizer.vertex(1).estimate().t)
    print('error:', optimizer.vertex(1).estimate().t - [0, 0, 1])
    '''
예제 #3
0
파일: sba_demo2.py 프로젝트: dead/g2opy
def main():
    optimizer = g2o.SparseOptimizer()
    solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
    solver = g2o.OptimizationAlgorithmLevenberg(solver)
    optimizer.set_algorithm(solver)

    focal_length = (500, 500)
    principal_point = (320, 240)
    baseline = 0.075

    true_points = np.hstack([
        np.random.random((500, 1)) * 3 - 1.5,
        np.random.random((500, 1)) - 0.5,
        np.random.random((500, 1)) + 3
    ])

    num_pose = 15
    for i in range(num_pose):
        # pose here means transform points from camera coordinates to world coordinates
        pose = g2o.SE3Quat(np.identity(3), [i * 0.04 - 1, 0, 0])
        sbacam = g2o.SBACam(pose)
        sbacam.set_cam(*focal_length, *principal_point, baseline)

        v_cam = g2o.VertexCam()
        v_cam.set_id(i)
        v_cam.set_estimate(sbacam)
        if i < 2:
            v_cam.set_fixed(True)
        optimizer.add_vertex(v_cam)

    point_id = num_pose
    inliers = dict()
    sse = defaultdict(float)

    for i, point in enumerate(true_points):
        visible = []
        for j in range(num_pose):
            proj = optimizer.vertex(j).estimate().w2i.dot(
                np.hstack([point, [1]]))  # project to left camera
            z = proj[:2] / proj[2]
            if 0 <= z[0] < 640 and 0 <= z[1] < 480:
                visible.append((j, z))
        if len(visible) < 2:
            continue

        vp = g2o.VertexSBAPointXYZ()
        vp.set_id(point_id)
        vp.set_marginalized(True)
        vp.set_estimate(point + np.random.randn(3))
        optimizer.add_vertex(vp)

        inlier = True
        for j, z in visible:
            if np.random.random() < args.outlier_ratio:
                inlier = False
                z = np.random.random(2) * [640, 480]
            z += np.random.randn(2) * args.pixel_noise

            edge = g2o.EdgeProjectP2MC()  # if stereo, use EdgeProjectP2SC
            edge.set_vertex(0, vp)
            edge.set_vertex(1, optimizer.vertex(j))
            edge.set_measurement(z)
            edge.set_information(np.identity(2))
            if args.robust_kernel:
                edge.set_robust_kernel(g2o.RobustKernelHuber())

            #edge.set_parameter_id(0, 0)
            optimizer.add_edge(edge)

        if inlier:
            inliers[point_id] = i
            error = vp.estimate() - true_points[i]
            sse[0] += np.sum(error**2)
        point_id += 1

    print('num vertices:', len(optimizer.vertices()))
    print('num edges:', len(optimizer.edges()))

    print('Performing full BA:')
    optimizer.initialize_optimization()
    optimizer.set_verbose(True)
    optimizer.optimize(10)

    for i in inliers:
        vp = optimizer.vertex(i)
        error = vp.estimate() - true_points[inliers[i]]
        sse[1] += np.sum(error**2)

    print('\nRMSE (inliers only):')
    print('before optimization:', np.sqrt(sse[0] / len(inliers)))
    print('after  optimization:', np.sqrt(sse[1] / len(inliers)))
예제 #4
0
    def optimize_map(self, postfix = ""):
        optimizer = g2o.SparseOptimizer()
        solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
        solver = g2o.OptimizationAlgorithmLevenberg(solver)
        optimizer.set_algorithm(solver)

        # Define camera parameters
        print(self.camera_matrix)
        #focal_length = 1000
        focal_length = self.camera_matrix[0, 0]
        #principal_point = (320, 240)
        principal_point = (self.camera_matrix[0, 2], self.camera_matrix[1, 2])
        baseline = 0
        cam = g2o.CameraParameters(focal_length, principal_point, baseline)
        cam.set_id(0)
        optimizer.add_parameter(cam)

        camera_vertices = {}
        for camera in self.cameras:
            # Use the estimated pose of the second camera based on the 
            # essential matrix.
            pose = g2o.SE3Quat(camera.R, camera.t)

            # Set the poses that should be optimized.
            # Define their initial value to be the true pose
            # keep in mind that there is added noise to the observations afterwards.
            v_se3 = g2o.VertexSE3Expmap()
            v_se3.set_id(camera.camera_id)
            v_se3.set_estimate(pose)
            v_se3.set_fixed(camera.fixed)
            optimizer.add_vertex(v_se3)
            camera_vertices[camera.camera_id] = v_se3
            #print("camera id: %d" % camera.camera_id)

        point_vertices = {}
        for point in self.points:
            # Add 3d location of point to the graph
            vp = g2o.VertexPointXYZ()
            vp.set_id(point.point_id)
            vp.set_marginalized(True)
            # Use positions of 3D points from the triangulation
            point_temp = np.array(point.point, dtype=np.float64)
            vp.set_estimate(point_temp)
            optimizer.add_vertex(vp)
            point_vertices[point.point_id]= vp


        for observation in self.observations:
            # Add edge from first camera to the point
            edge = g2o.EdgeProjectXYZ2UV()

            # 3D point
            edge.set_vertex(0, point_vertices[observation.point_id]) 
            # Pose of first camera
            edge.set_vertex(1, camera_vertices[observation.camera_id]) 
            
            edge.set_measurement(observation.image_coordinates)
            edge.set_information(np.identity(2))
            edge.set_robust_kernel(g2o.RobustKernelHuber())

            edge.set_parameter_id(0, 0)
            optimizer.add_edge(edge)

        print('num vertices:', len(optimizer.vertices()))
        print('num edges:', len(optimizer.edges()))

        print('Performing full BA:')
        optimizer.initialize_optimization()
        optimizer.set_verbose(True)
        optimizer.optimize(40)
        optimizer.save("test.g2o");

        for idx, camera in enumerate(self.cameras):
            t = camera_vertices[camera.camera_id].estimate().translation()
            self.cameras[idx].t = t
            q = camera_vertices[camera.camera_id].estimate().rotation()
            self.cameras[idx].R = quarternion_to_rotation_matrix(q)

        for idx, point in enumerate(self.points):
            p = point_vertices[point.point_id].estimate()
            # It is important to copy the point estimates.
            # Otherwise I end up with some memory issues.
            # self.points[idx].point = p
            self.points[idx].point = np.copy(p)
예제 #5
0
def optimize(frames,
             points,
             local_window,
             fix_points,
             verbose=False,
             rounds=100):
    if local_window is None:
        local_frames = frames
    else:
        local_frames = frames[-local_window:]

    opt = g2opy.SparseOptimizer()
    solver = g2opy.BlockSolverSE3(g2opy.LinearSolverCSparseSE3())
    solver = g2opy.OptimizationAlgorithmLevenberg(solver)
    opt.set_algorithim(solver)

    cam = g2opy.CameraParameters(1.0, (0.0, 0.0), 0)
    cam.set_id(0)
    opt.add_paramater(cam)

    robust_kernel = g2opy.RobustKernelHuber(nq.sqrt(5.992))
    graph_frames, graph_points = {}, {}

    for f in (local_frames if fix_points else frames):
        pose = f.pose
        se3 = g2opy.SEQQuat(pose[0:3, 0:3], pose[0:3, 3])
        v_se3 = g2opy.VertexSE3Expmap()
        v_se3.set_estimate(se3)
        v_se3.set_id(f.id * 4)
        v_se3.set_fixed(f.id <= 1 or f not in local_frames)
        opt.add_vertex(v_se3)

        est = v_se3.estimate(se3)
        assert nq.allclose(pose[0:3, 0:3], est.rotation().matrix())
        assert nq.allclose(pose[0:3, 3], est.translation())

        graph_frames[f] = v_se3

    for p in points:
        if not any([f in local_frames for f in p.frames]):
            continue

        pt = g2opy.VertexSBAPointXYZ()
        pt.set_id(p.id * 2 + 1)
        pt.set_estimate(p.pt[0:3])
        pt.set_marginalized(True)
        pt.set_fixed(fix_points)
        opt.add_vertex(pt)
        graph_points[p] = pt

        for f, idx in zip(p.frames, p.idxs):
            if f not in graph_frames:
                continue
            edge = g2opy.EdgeProjectXYZ2UV()
            edge.set_parameter_id(0, 0)
            edge.set_vertex(0, pt)
            edge.set_vertex(1, graph_frames[f])
            edge.set_measurement(f.kps[idx])
            edge.set_information(nq.eye(2))
            edge.set_robust_kernel(robust_kernel)
            opt.add_edge(edge)

    if verbose:
        opt.set_verbose(True)
    opt.initialize_optimization()
    opt.optimize(rounds)

    for f in graph_frames:
        est = graph_frames[f].estimate()
        R = est.rotation().matrix().matrix()
        t = est.translation()
        f.pose = poseRt(R, t)

    if not fix_points:
        for p in graph_points:
            p.pt = nq.array(graph_points[p].estimate())

    return opt.active_chi2()
예제 #6
0
파일: sba_demo.py 프로젝트: dead/g2opy
def main():
    optimizer = g2o.SparseOptimizer()
    solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
    solver = g2o.OptimizationAlgorithmLevenberg(solver)
    optimizer.set_algorithm(solver)

    true_points = np.hstack([
        np.random.random((500, 1)) * 3 - 1.5,
        np.random.random((500, 1)) - 0.5,
        np.random.random((500, 1)) + 3
    ])

    focal_length = (500, 500)
    principal_point = (320, 240)
    baseline = 0.075
    g2o.VertexSCam.set_cam(*focal_length, *principal_point, baseline)

    true_poses = []
    num_pose = 5
    for i in range(num_pose):
        # pose here transform points from world coordinates to camera coordinates
        pose = g2o.Isometry3d(np.identity(3), [i * 0.04 - 1, 0, 0])
        true_poses.append(pose)

        v_se3 = g2o.VertexSCam()
        v_se3.set_id(i)
        v_se3.set_estimate(pose)
        if i < 2:
            v_se3.set_fixed(True)
        v_se3.set_all()
        optimizer.add_vertex(v_se3)

    point_id = num_pose
    inliers = dict()
    sse = defaultdict(float)

    for i, point in enumerate(true_points):
        visible = []
        for j in range(num_pose):
            z = optimizer.vertex(j).map_point(point)
            if 0 <= z[0] < 640 and 0 <= z[1] < 480:
                visible.append((j, z))

        if len(visible) < 2:
            continue

        vp = g2o.VertexSBAPointXYZ()
        vp.set_id(point_id)
        vp.set_marginalized(True)
        vp.set_estimate(point + np.random.randn(3))
        optimizer.add_vertex(vp)

        inlier = True
        for j, z in visible:
            if np.random.random() < args.outlier_ratio:
                inlier = False
                z = np.array([
                    np.random.uniform(64, 640),
                    np.random.uniform(0, 480),
                    np.random.uniform(0, 64)
                ])  # disparity
                z[2] = z[0] - z[2]
            z += np.random.randn(3) * args.pixel_noise * [1, 1, 1 / 16.]

            edge = g2o.Edge_XYZ_VSC()
            edge.set_vertex(0, vp)
            edge.set_vertex(1, optimizer.vertex(j))
            edge.set_measurement(z)
            edge.set_information(np.identity(3))
            if args.robust_kernel:
                edge.set_robust_kernel(g2o.RobustKernelHuber())

            edge.set_parameter_id(0, 0)
            optimizer.add_edge(edge)

        if inlier:
            inliers[point_id] = i
            error = vp.estimate() - true_points[i]
            sse[0] += np.sum(error**2)
        point_id += 1

    print('Performing full BA:')
    optimizer.initialize_optimization()
    optimizer.set_verbose(True)
    optimizer.optimize(10)

    for i in inliers:
        vp = optimizer.vertex(i)
        error = vp.estimate() - true_points[inliers[i]]
        sse[1] += np.sum(error**2)

    print('\nRMSE (inliers only):')
    print('before optimization:', np.sqrt(sse[0] / len(inliers)))
    print('after  optimization:', np.sqrt(sse[1] / len(inliers)))
예제 #7
0
def main():
    optimizer = g2o.SparseOptimizer()
    if args.schur_trick:
        solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
    else:
        solver = g2o.BlockSolverX(g2o.LinearSolverEigenX())   # slower
    solver = g2o.OptimizationAlgorithmLevenberg(solver)
    optimizer.set_algorithm(solver)

    true_points = np.hstack([
        np.random.random((500, 1)) * 3 - 1.5,
        np.random.random((500, 1)) - 0.5,
        np.random.random((500, 1)) + 3])

    
    focal_length = 1000.
    principal_point = (320, 240)
    cam = g2o.CameraParameters(focal_length, principal_point, 0)
    cam.set_id(0)

    optimizer.add_parameter(cam)

    true_poses = []
    num_pose = 15
    for i in range(num_pose):
        # pose here means transform points from world coordinates to camera coordinates
        pose = g2o.SE3Quat(np.identity(3), [i*0.04-1, 0, 0])
        true_poses.append(pose)

        v_se3 = g2o.VertexSE3Expmap()
        v_se3.set_id(i)
        v_se3.set_estimate(pose)
        if i < 2:
            v_se3.set_fixed(True)
        optimizer.add_vertex(v_se3)


    point_id = num_pose
    inliers = dict()
    sse = defaultdict(float)

    for i, point in enumerate(true_points):
        visible = []
        for j, pose in enumerate(true_poses):
            z = cam.cam_map(pose * point)
            if 0 <= z[0] < 640 and 0 <= z[1] < 480:
                visible.append((j, z))
        if len(visible) < 2:
            continue

        v_p = g2o.VertexSBAPointXYZ()
        v_p.set_id(point_id)
        v_p.set_marginalized(args.schur_trick)

        anchor = visible[0][0]
        point2 = true_poses[anchor] * (point + np.random.randn(3))
        if point2[2] == 0:
            continue
        v_p.set_estimate(invert_depth(point2))
        optimizer.add_vertex(v_p)

        inlier = True
        for j, z in visible:
            if np.random.random() < args.outlier_ratio:
                inlier = False
                z = np.random.random(2) * [640, 480]
            z += np.random.randn(2) * args.pixel_noise

            edge = g2o.EdgeProjectPSI2UV()
            edge.resize(3)
            edge.set_vertex(0, v_p)
            edge.set_vertex(1, optimizer.vertex(j))
            edge.set_vertex(2, optimizer.vertex(anchor))
            edge.set_measurement(z)
            edge.set_information(np.identity(2))
            if args.robust_kernel:
                edge.set_robust_kernel(g2o.RobustKernelHuber())

            edge.set_parameter_id(0, 0)
            optimizer.add_edge(edge)

        if inlier:
            inliers[point_id] = (i, anchor)
            error = (true_poses[anchor].inverse() * invert_depth(v_p.estimate()) - 
                true_points[i])
            sse[0] += np.sum(error**2)
        point_id += 1

    print('Performing full BA:')
    optimizer.initialize_optimization()
    optimizer.set_verbose(True)
    optimizer.optimize(10)


    for i in inliers:
        v_p = optimizer.vertex(i)
        v_anchor = optimizer.vertex(inliers[i][1])
        error = (v_anchor.estimate().inverse() * invert_depth(v_p.estimate()) - 
            true_points[inliers[i][0]])
        sse[1] += np.sum(error**2)


    print('\nRMSE (inliers only):')
    print('before optimization:', np.sqrt(sse[0] / len(inliers)))
    print('after  optimization:', np.sqrt(sse[1] / len(inliers)))
예제 #8
0
파일: gicp_demo.py 프로젝트: dead/g2opy
def main():
    optimizer = g2o.SparseOptimizer()
    solver = g2o.BlockSolverX(g2o.LinearSolverDenseX())
    algorithm = g2o.OptimizationAlgorithmLevenberg(solver)
    optimizer.set_algorithm(algorithm)

    true_points = np.hstack([
        np.random.random((1000, 1)) * 3 - 1.5,
        np.random.random((1000, 1)) - 0.5,
        np.random.random((1000, 1)) + 10])


    for i in range(2):
        t = np.array([0, 0, i])
        cam = g2o.Isometry3d(np.identity(3), t)

        vc = g2o.VertexSE3()
        vc.set_id(i)
        vc.set_estimate(cam)
        if i == 0:
            vc.set_fixed(True)
        optimizer.add_vertex(vc)

    trans0 = optimizer.vertex(0).estimate().inverse()
    trans1 = optimizer.vertex(1).estimate().inverse()

    # set up point matches
    for i in range(len(true_points)):
        # calculate the relative 3d position of the point
        pt0 = trans0 * true_points[i]
        pt1 = trans1 * true_points[i]

        # add noise
        pt0 += np.random.randn(3) * args.noise
        pt1 += np.random.randn(3) * args.noise

        # form edge, with normals in varioius positions
        nm0 = np.array([0, i, 1])
        nm0 = nm0 / np.linalg.norm(nm0)
        nm1 = np.array([0, i, 1])
        nm1 = nm1 / np.linalg.norm(nm1)

        meas = g2o.EdgeGICP()
        meas.pos0 = pt0
        meas.pos1 = pt1
        meas.normal0 = nm0
        meas.normal1 = nm1

        edge = g2o.Edge_V_V_GICP()
        edge.set_vertex(0, optimizer.vertex(0))
        edge.set_vertex(1, optimizer.vertex(1))
        edge.set_measurement(meas)
        edge.set_information(meas.prec0(0.01))

        optimizer.add_edge(edge)

    # move second cam off of its true position
    vc = optimizer.vertex(1)
    cam = g2o.Isometry3d(vc.estimate().R, np.array([0, 0, 0.2]))
    vc.set_estimate(cam)


    optimizer.initialize_optimization()
    optimizer.compute_active_errors()
    print('Initial chi2 =', optimizer.chi2())

    optimizer.save('gicp.g2o')

    optimizer.set_verbose(True)
    optimizer.optimize(5)

    print('\nSecond vertex should be near [0, 0, 1]')
    print('before optimization:', cam.t)
    print('after  optimization:', optimizer.vertex(1).estimate().t)