def optimize(self):
# create g2o optimizer
opt = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
opt.set_algorithm(solver)
robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991))
# add frames to graph
for f in self.frames:
pose = f.pose
#pose = np.linalg.inv(pose)
sbacam = g2o.SBACam(g2o.SE3Quat(pose[0:3, 0:3], pose[0:3, 3]))
sbacam.set_cam(f.K[0][0], f.K[1][1], f.K[0][2], f.K[1][2], 1.0)
v_se3 = g2o.VertexCam()
v_se3.set_id(f.id)
v_se3.set_estimate(sbacam)
v_se3.set_fixed(f.id <= 1)
opt.add_vertex(v_se3)
# add points to frames
PT_ID_OFFSET = 0x10000
for p in self.points:
pt = g2o.VertexSBAPointXYZ()
pt.set_id(p.id + PT_ID_OFFSET)
pt.set_estimate(p.pt[0:3])
pt.set_marginalized(True)
pt.set_fixed(False)
opt.add_vertex(pt)
for f in p.frames:
edge = g2o.EdgeProjectP2MC()
edge.set_vertex(0, pt)
edge.set_vertex(1, opt.vertex(f.id))
uv = f.kpus[f.pts.index(p)]
edge.set_measurement(uv)
edge.set_information(np.eye(2))
edge.set_robust_kernel(robust_kernel)
opt.add_edge(edge)
opt.set_verbose(True)
opt.initialize_optimization()
opt.optimize(50)
# put frames back
for f in self.frames:
est = opt.vertex(f.id).estimate()
R = est.rotation().matrix()
t = est.translation()
f.pose = poseRt(R, t)
# put points back
for p in self.points:
est = opt.vertex(p.id + PT_ID_OFFSET).estimate()
p.pt = np.array(est)
def add_pose(self, pose_id, pose, cam, fixed=False):
sbacam = g2o.SBACam(pose.orientation(), pose.position())
sbacam.set_cam(cam.fx, cam.fy, cam.cx, cam.cy, cam.baseline)
v_se3 = g2o.VertexCam()
v_se3.set_id(pose_id * 2) # internal id
v_se3.set_estimate(sbacam)
v_se3.set_fixed(fixed)
super().add_vertex(v_se3)
def optimize(self):
# init g2o solver
opt = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991))
opt.set_algorithm(solver)
robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991))
# add frames to graph
for frame in self.frames:
pose = frame.pose
sbacam = g2o.SBACam(
g2o.SE3Quat(frame.pose[0:3, 0:3], frame.pose[0:3, 3]))
# sbacam.set_cam(frame.K[0][0], frame.K[1][1], frame.K[2][0], frame.K[2][1], 1.0)
sbacam.set_cam(1.0, 1.0, 0.0, 0.0, 1.0)
v_se3 = g2o.VertexCam()
v_se3.set_id(frame.id)
v_se3.set_estimate(sbacam)
v_se3.set_fixed(frame.id == 0)
opt.add_vertex(v_se3)
# add points to frames
PT_ID_OFFSET = 0x10000
for point in self.points:
pt = g2o.VertexSBAPointXYZ()
pt.set_id(point.id + PT_ID_OFFSET)
pt.set_estimate(point.pt[0:3])
pt.set_marginalized(True)
pt.set_fixed(False)
opt.add_vertex(pt)
for frame in point.frames:
edge = g2o.EdgeProjectP2MC()
edge.set_vertex(0, pt)
edge.set_vertex(1, opt.vertex(frame.id))
uv = frame.kps[frame.pts.index(point)]
edge.set_measurement(uv)
edge.set_information(np.eye(2))
edge.set_robust_kernel(robust_kernel)
opt.add_edge(edge)
# opt.set_verbose(True)
opt.initialize_optimization()
opt.optimize(20)
# add pose back to frame
for frame in self.frames:
est = opt.vertex(frame.id).estimate()
R = est.rotation().matrix()
t = est.translation()
frame.pose = poseRt(R, t)
def optimize(self, max_iters):
opt = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
opt.set_algorithm(solver)
robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991))
# adding frames to graph
for f in self.frames:
sbacam = g2o.SBACam(g2o.SE3Quat(f.pose[0:3, 0:3], f.pose[0:3, 3]))
sbacam.set_cam(f.T[0][0], f.T[1][1], f.T[2][0], f.T[2][1], 1.0)
v_se3 = g2o.VertexCam()
v_se3.set_id(f.id)
v_se3.set_estimate(sbacam)
v_se3.set_fixed(f.id == 0)
opt.add_vertex(v_se3)
# add points to graph
for p in self.points:
pt = g2o.VertexSBAPointXYZ()
pt.set_id(p.id + 0x10000)
pt.set_estimate(p.location[0:3])
pt.set_marginalized(True)
pt.set_fixed(False)
opt.add_vertex(pt)
# add connections between frames that contain a point in the graph
for f in p.frames:
edge = g2o.EdgeProjectP2MC()
edge.set_vertex(0, pt)
edge.set_vertex(1, opt.vertex(f.id))
uv = f.kpts[f.pts.index(p)]
edge.set_measurement(uv)
edge.set_information(np.eye(2))
edge.set_robust_kernel(robust_kernel)
opt.add_edge(edge)
opt.set_verbose(True)
opt.initialize_optimization()
opt.optimize(max_iters)
def optimize(self):
# optimizer
opt = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
opt.set_algorithm(solver)
robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991))
for f in self.frames:
sbacam = g2o.SBACam(g2o.SE3Quat(f.pose[:3, :3], f.pose[:3, 3]))
sbacam.set_cam(f.K[0][0], f.K[1][1], f.K[2][0], f.K[2][1], 1.0)
v_se3 = g2o.VertexCam()
v_se3.set_id(f.id)
v_se3.set_estimate(sbacam)
v_se3.set_fixed(f.id == 0)
opt.add_vertex(v_se3)
for p in self.points:
pt = g2o.VertexSBAPointXYZ()
pt.set_id(p.id + 0x10000)
pt.set_estimate(p.pt[0:3])
pt.set_marginalized(True)
pt.set_fixed(False)
opt.add_vertex(pt)
for f in p.frames:
edge = g2o.EdgeProjectP2MC()
edge.set_vertex(0, pt)
edge.set_vertex(1, opt.vertex(f.id))
uv = f.kps[f.pts.index(p)]
edge.set_measurement(uv)
edge.set_information(np.eye(2))
edge.set_robust_kernel(robust_kernel)
opt.add_edge(edge)
opt.set_verbose(True)
opt.initialize_optimization()
opt.optimize(20)
def optimize(self):
# create g2o optimizer
opt = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
opt.set_algorithm(solver)
robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991))
if LOCAL_WINDOW is None:
local_frames = self.frames
else:
local_frames = self.frames[-LOCAL_WINDOW:]
# add frames to graph
for f in self.frames:
pose = f.pose
sbacam = g2o.SBACam(g2o.SE3Quat(pose[0:3, 0:3], pose[0:3, 3]))
sbacam.set_cam(f.K[0][0], f.K[1][1], f.K[0][2], f.K[1][2], 1.0)
v_se3 = g2o.VertexCam()
v_se3.set_id(f.id)
v_se3.set_estimate(sbacam)
v_se3.set_fixed(f.id <= 1 or f not in local_frames)
opt.add_vertex(v_se3)
# add points to frames
PT_ID_OFFSET = 0x10000
for p in self.points:
if not any([f in local_frames for f in p.frames]):
continue
pt = g2o.VertexSBAPointXYZ()
pt.set_id(p.id + PT_ID_OFFSET)
pt.set_estimate(p.pt[0:3])
pt.set_marginalized(True)
pt.set_fixed(False)
opt.add_vertex(pt)
for f in p.frames:
edge = g2o.EdgeProjectP2MC()
edge.set_vertex(0, pt)
edge.set_vertex(1, opt.vertex(f.id))
uv = f.kpus[f.pts.index(p)]
edge.set_measurement(uv)
edge.set_information(np.eye(2))
edge.set_robust_kernel(robust_kernel)
opt.add_edge(edge)
#opt.set_verbose(True)
opt.initialize_optimization()
opt.optimize(50)
# put frames back
for f in self.frames:
est = opt.vertex(f.id).estimate()
R = est.rotation().matrix()
t = est.translation()
f.pose = poseRt(R, t)
# put points back (and cull)
new_points = []
for p in self.points:
vert = opt.vertex(p.id + PT_ID_OFFSET)
if vert is None:
new_points.append(p)
continue
est = vert.estimate()
# 2 match point that's old
old_point = len(p.frames) == 2 and p.frames[-1] not in local_frames
# compute reprojection error
errs = []
for f in p.frames:
uv = f.kpus[f.pts.index(p)]
proj = np.dot(np.dot(f.K, np.linalg.inv(f.pose)[:3]),
np.array([est[0], est[1], est[2], 1.0]))
proj = proj[0:2] / proj[2]
errs.append(np.linalg.norm(proj-uv))
# cull
if (old_point and np.mean(errs) > 30) or np.mean(errs) > 100:
p.delete()
continue
p.pt = np.array(est)
new_points.append(p)
self.points = new_points
return opt.chi2()
def optimize(self, K, frames, fix_points=False):
PT_OFFSET = 10000
opt = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
opt.set_algorithm(solver)
rk = g2o.RobustKernelHuber(np.sqrt(5.991))
# only optimize these poses and their Points
local_frames = self.frames[-frames:]
# a vertex for each camera pose
for f in self.frames:
assert (f.id < PT_OFFSET)
quat = g2o.SE3Quat(f.pose[:3, :3], f.pose[:3, 3])
sbacam = g2o.SBACam(quat)
sbacam.set_cam(K[0, 0], K[1, 1], K[0, 2], K[1, 2],
1.0) # for pixel input
v = g2o.VertexCam()
v.set_id(f.id)
v.set_estimate(sbacam)
v.set_fixed(f.id < 2 or f not in local_frames)
opt.add_vertex(v)
# a vertex for each point in cloud
for p in self.points:
# if p.frames[-1] not in local_frames:
if not any([f in local_frames for f in p.frames]):
continue
vp = g2o.VertexSBAPointXYZ()
vp.set_id(PT_OFFSET + p.id)
vp.set_estimate(p.pt3d)
vp.set_marginalized(True)
vp.set_fixed(fix_points)
opt.add_vertex(vp)
# edge connects every camera with every point
for f in p.frames:
edge = g2o.EdgeProjectP2MC()
# edge = g2o.EdgeSE3ProjectXYZ()
edge.set_vertex(0, vp)
edge.set_vertex(1, opt.vertex(f.id))
idx = f.pts.index(p)
edge.set_measurement(f.kpus[idx])
edge.set_information(np.eye(2))
edge.set_robust_kernel(rk)
opt.add_edge(edge)
# opt.set_verbose(True)
opt.initialize_optimization()
opt.optimize(20)
# print("chi2", opt.active_chi2())
# get back optimized poses
self.opath = []
for f in self.frames:
f_est = opt.vertex(f.id).estimate()
R = f_est.rotation().matrix()
t = f_est.translation()
Rt = np.eye(4)
Rt[:3, :3] = R
Rt[:3, 3] = t.T
# print(Rt)
self.opath.append(t)
f.pose = Rt
if fix_points:
return
new_points = []
# add back or cull
for p in self.points:
vp = opt.vertex(PT_OFFSET + p.id)
if vp is None: # not updated
new_points.append(p)
continue
vp = opt.vertex(PT_OFFSET + p.id)
p_est = vp.estimate()
# if (len(p.frames) <= 5) and (p.frames[-1] not in local_frames):
# p.delete()
# continue
errors = []
for f in p.frames:
f_est = opt.vertex(f.id).estimate()
measured = f.kpus[f.pts.index(p)]
projected = f_est.w2i.dot(np.hstack(
[p_est, [1]])) # only works because is VertexCam
projected = projected[:2] / projected[
2] # don't forget to h**o
errors.append(np.linalg.norm(measured - projected))
error = np.mean(errors) # mean of squares - over all frames
if error > 1.0: # px
# print(f"error {error}, dropping")
p.delete()
else:
p.pt3d = np.array(p_est)
new_points.append(p)
print("Dropping:",
len(self.points) - len(new_points), ", Remaining:",
len(new_points))
self.points = new_points
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(self, local_window = LOCAL_WINDOW, fix_points = False, verbose = False):
# create the g2o optimizer
optimizer = g2o.SparseOptimizer()
graph_solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
graph_solver = g2o.OptimizationAlgorithmLevenberg(graph_solver)
optimizer.set_algorithm(graph_solver)
robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991))
if local_window is None:
local_frames = self.frames
else:
local_frames = self.frames[-local_window:]
# add frames to the graph
for f in self.frames:
pose = np.linalg.inv(f.pose)
sbacam = g2o.SBACam(g2o.SE3Quat(pose[0:3, 0:3], pose[0:3, 3]))
sbacam.set_cam(f.k[0][0], f.k[1][1], f.k[0][2], f.k[1][2], 1.0)
v_se3 = g2o.VertexCam()
v_se3.set_id(f.id)
v_se3.set_estimate(sbacam)
v_se3.set_fixed(f.id <= 1 or f not in local_frames)
optimizer.add_vertex(v_se3)
# add points to the frames
point_id_offset = 0x10000
for p in self.points:
if not any([f in local_frames for f in p.frames]):
continue
pt = g2o.VertexSBAPointXYZ()
pt.set_id(p.id + point_id_offset)
pt.set_estimate(p.point[0:3])
pt.set_marginalized(True)
pt.set_fixed(fix_points)
optimizer.add_vertex(pt)
for f in p.frames:
edge = g2o.EdgeProjectP2MC()
edge.set_vertex(0, pt)
edge.set_vertex(1, optimizer.vertex(f.id))
uv = f._kps[f.pts.index(p)]
edge.set_measurement(uv)
edge.set_information(np.eye(2))
edge.set_robust_kernel(robust_kernel)
optimizer.add_edge(edge)
if verbose:
optimizer.set_verbose(True)
optimizer.initialize_optimization()
optimizer.optimize(20)
# put the frames back
for f in self.frames:
estimate = optimizer.vertex(f.id).estimate()
r = estimate.rotation().matrix()
t = estimate.translation()
f.pose = np.linalg.inv(pose_rt(r, t))
# put points back
if not fix_points:
new_points = []
for p in self.points:
vert = optimizer.vertex(p.id + point_id_offset)
if vert is None:
new_points.append(p)
continue
estimate = vert.estimate()
old_point = len(p.frames) <= 2 and p.frames[-1] not in local_frames
# try and computer the reprojection error of the point
errors = []
for f in p.frames:g
uv = f._kps[f.pts.index(p)]
proj = np.dot(np.dot(f.k, f.pose[:3]),
np.array([estimate[0], estimate[1], estimate[2], 1.0]))
proj = proj[0:2]/proj[2]
errors.append(np.linalg.norm(proj-uv))
# culling
if old_point or np.mean(errors) > 5:
p.delete_point()
continue
p.point = np.array(estimate)
new_points.append(p)
print("Culled points: %d" %(len(self.points) - len(new_points)))
self.points = new_points
return optimizer.active_chi2()
def optimize(self, local_window=20, fix_points=False, verbose=False):
# create g2o optimizer
opt = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
opt.set_algorithm(solver)
if self.alt:
principal_point = (self.frames[0].K[0][2], self.frames[0].K[1][2])
cam = g2o.CameraParameters(self.frames[0].K[0][0], principal_point,
0)
cam.set_id(0)
opt.add_parameter(cam)
robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991))
if local_window is None:
local_frames = self.frames
else:
local_frames = self.frames[-local_window:]
graph_frames, graph_points = {}, {}
# add frames to graph
for f in (local_frames if fix_points else self.frames):
if not self.alt:
pose = np.linalg.inv(f.pose)
se3 = g2o.SE3Quat(pose[0:3, 0:3], pose[0:3, 3])
sbacam = g2o.SBACam(se3)
sbacam.set_cam(f.K[0][0], f.K[1][1], f.K[0][2], f.K[1][2], 0.0)
v_se3 = g2o.VertexCam()
v_se3.set_estimate(sbacam)
else:
pose = f.pose
se3 = g2o.SE3Quat(pose[0:3, 0:3], pose[0:3, 3])
v_se3 = g2o.VertexSE3Expmap()
v_se3.set_estimate(se3)
v_se3.set_id(f.id * 2)
v_se3.set_fixed(f.id <= 1 or f not in local_frames)
# v_se3.set_fixed(f.id != 0)
opt.add_vertex(v_se3)
# confirm pose correctness
est = v_se3.estimate()
assert np.allclose(pose[0:3, 0:3], est.rotation().matrix())
assert np.allclose(pose[0:3, 3], est.translation())
graph_frames[f] = v_se3
# add points to frames
for p in self.points:
if not any([f in local_frames for f in p.frames]):
continue
pt = g2o.VertexSBAPointXYZ()
pt.set_id(p.id * 2 + 1)
pt.set_estimate(p.loc[0:3])
pt.set_marginalized(True)
pt.set_fixed(fix_points)
opt.add_vertex(pt)
graph_points[p] = pt
# add edges
for f, idx in zip(p.frames, p.idxs):
if f not in graph_frames:
continue
if not self.alt:
edge = g2o.EdgeProjectP2MC()
else:
edge = g2o.EdgeProjectXYZ2UV()
edge.set_parameter_id(0, 0)
edge.set_vertex(0, pt)
edge.set_vertex(1, graph_frames[f])
uv = f.raw_pts[idx]
edge.set_measurement(uv)
edge.set_information(np.eye(2))
edge.set_robust_kernel(robust_kernel)
opt.add_edge(edge)
if verbose:
opt.set_verbose(True)
opt.initialize_optimization()
opt.optimize(50)
# put frames back
for f in graph_frames:
est = graph_frames[f].estimate()
R = est.rotation().matrix()
t = est.translation()
if not self.alt:
f.pose = np.linalg.inv(pose_homogeneous(R, t))
else:
f.pose = pose_homogeneous(R, t)
# put points back (and cull)
if not fix_points:
culled_pt_count = 0
for p in graph_points:
est = graph_points[p].estimate()
p.pt = np.array(est)
# remove points if the number of observations <= (n-1)
old_point = len(
p.frames) <= 9 and p.frames[-1].id + 17 < self.max_frame
# compute reprojection error
errs = []
for f, idx in zip(p.frames, p.idxs):
uv = f.raw_pts[idx]
proj = np.dot(np.dot(f.K, f.pose[:3]),
np.array([est[0], est[1], est[2], 1.0]))
proj = proj[0:2] / proj[2]
errs.append(np.linalg.norm(proj - uv))
# cull
if old_point or np.mean(errs) > 2:
culled_pt_count += 1
self.points.remove(p)
p.delete()
print("Culled: %d points" % culled_pt_count)
return opt.active_chi2()
