def main():
    optimizer = g2o.SparseOptimizer()
    if args.schur_trick:
        solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
    else:
        solver = g2o.BlockSolverX(g2o.LinearSolverEigenX())
    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.0
    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 = 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))))
def main():
    optimizer = g2o.SparseOptimizer()
    if args.schur_trick:
        solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
    else:
        solver = g2o.BlockSolverX(g2o.LinearSolverEigenX())
    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.0
    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 = 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))))