def add_points(self, points, local_frames):
# add points to frames
for p in 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.pt[0:3])
pt.set_marginalized(True)
pt.set_fixed(self.fix_points)
self.opt.add_vertex(pt)
self.graph_points[p] = pt
# add edges
for f, idx in zip(p.frames, p.idxs):
if f not in self.graph_frames:
continue
edge = g2o.EdgeProjectXYZ2UV()
edge.set_parameter_id(0, 0)
edge.set_vertex(0, pt)
edge.set_vertex(1, self.graph_frames[f])
edge.set_measurement(f.kps[idx])
edge.set_information(np.eye(2))
edge.set_robust_kernel(self.robust_kernel)
self.opt.add_edge(edge)
def add_edge(self, edge_id, point_id, pose_id,
measurement,
information=np.identity(2),
robust_kernel=g2o.RobustKernelHuber(np.sqrt(5.991))):
# edge = g2o.EdgeSE3ProjectXYZ()
edge = g2o.EdgeProjectXYZ2UV()
edge.set_id(edge_id)
edge.set_vertex(0, self.vertex(point_id) )
edge.set_vertex(1, self.vertex(pose_id) )
edge.set_measurement(measurement)
edge.set_information(information)
if robust_kernel is not None:
edge.set_robust_kernel(robust_kernel)
edge.set_parameter_id(0,0)
super().add_edge(edge)
return edge
def add_edge(
self,
point_id,
pose_id,
measurement,
information=np.identity(2),
robust_kernel=g2o.RobustKernelHuber(np.sqrt(5.991)),
):
edge = g2o.EdgeProjectXYZ2UV()
edge.set_parameter_id(0, 0)
edge.set_vertex(0, self._optimizer.vertex(point_id))
edge.set_vertex(1, self._optimizer.vertex(pose_id))
edge.set_measurement(measurement) # pixel point (u,v)
edge.set_information(information)
edge.set_robust_kernel(robust_kernel)
self._optimizer.add_edge(edge)
def bundle_adjustment(points_3d, points_2d, r_mat, t_vec):
optimizer = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
optimizer.set_algorithm(solver)
pose = g2o.VertexSE3Expmap()
pose.set_estimate(g2o.SE3Quat(r_mat, t_vec.reshape((3, ))))
pose.set_id(0)
optimizer.add_vertex(pose)
index = 1
for p_3d in points_3d:
point = g2o.VertexSBAPointXYZ()
point.set_id(index)
point.set_estimate(p_3d)
point.set_marginalized(True)
optimizer.add_vertex(point)
index += 1
camera = g2o.CameraParameters(K[0, 0], np.array([K[0, 2], K[1, 2]]), 0)
camera.set_id(0)
optimizer.add_parameter(camera)
index = 1
for p_2d in points_2d:
edge = g2o.EdgeProjectXYZ2UV()
edge.set_id(index)
edge.set_vertex(0, optimizer.vertex(index))
edge.set_vertex(1, pose)
edge.set_measurement(p_2d)
edge.set_parameter_id(0, 0)
edge.set_information(np.identity(2))
optimizer.add_edge(edge)
index += 1
optimizer.initialize_optimization()
optimizer.set_verbose(True)
optimizer.optimize(100)
print('T = \n', pose.estimate().matrix())
def add_edge(self,
id,
vertices,
measurement,
parm_id,
information=np.identity(2),
robust_kernel=None):
# edge = g2o.EdgeSE3()
edge = g2o.EdgeProjectXYZ2UV()
edge.set_id(id)
for i, v in enumerate(vertices):
if isinstance(v, int):
v = self.vertex(v)
edge.set_vertex(i, v)
edge.set_measurement(measurement) # relative pose
edge.set_parameter_id(0, parm_id)
edge.set_information(information)
if robust_kernel is not None:
edge.set_robust_kernel(robust_kernel)
super().add_edge(edge)
def main():
optimizer = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
optimizer.set_algorithm(solver)
focal_length = 1000
principal_point = (320, 240)
cam = g2o.CameraParameters(focal_length, principal_point, 0)
cam.set_id(0)
optimizer.add_parameter(cam)
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
])
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
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.EdgeProjectXYZ2UV()
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:
print(i)
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(frames,
points,
local_window,
fix_points,
verbose=False,
rounds=50):
if local_window is None:
local_frames = frames
else:
local_frames = frames[-local_window:]
# create g2o optimizer
opt = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
opt.set_algorithm(solver)
# add normalized camera
cam = g2o.CameraParameters(1.0, (0.0, 0.0), 0)
cam.set_id(0)
opt.add_parameter(cam)
robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991))
graph_frames, graph_points = {}, {}
# add frames to graph
for f in (local_frames if fix_points else frames):
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 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.pt[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
edge = g2o.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(np.eye(2))
edge.set_robust_kernel(robust_kernel)
opt.add_edge(edge)
if verbose:
opt.set_verbose(True)
opt.initialize_optimization()
opt.optimize(rounds)
# put frames back
for f in graph_frames:
est = graph_frames[f].estimate()
R = est.rotation().matrix()
t = est.translation()
f.pose = poseRt(R, t)
# put points back
if not fix_points:
for p in graph_points:
p.pt = np.array(graph_points[p].estimate())
return opt.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()