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)
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])
'''
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)))
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)
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()
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)))
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)))
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)