def prepare_optimiser(self, verbose):
# create g2o optimizer
self.opt = g2o.SparseOptimizer()
self.solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
self.solver = g2o.OptimizationAlgorithmLevenberg(self.solver)
self.opt.set_algorithm(self.solver)
# add normalized camera
cam = g2o.CameraParameters(1.0, (0.0, 0.0), 0)
cam.set_id(0)
self.opt.add_parameter(cam)
self.robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991))
self.graph_frames, self.graph_points = {}, {}
if verbose:
self.opt.set_verbose(True)
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 main():
optimizer = g2o.SparseOptimizer()
# solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
# solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
optimizer.set_algorithm(solver)
# Convergence Criterion
terminate = g2o.SparseOptimizerTerminateAction()
terminate.set_gain_threshold(1e-6)
optimizer.add_post_iteration_action(terminate)
# Robust cost Function (Huber function) delta
delta = np.sqrt(5.991)
true_points = np.hstack([
np.random.random((25, 1)) * 3 - 1.5,
np.random.random((25, 1)) - 0.5,
np.random.random((25, 1)) + 3])
fx = 600.
fy = 600.
cx = 320.
cy = 240.
principal_point = (cx, cy)
cam = g2o.CameraParameters(fx, principal_point, 0)
true_poses = []
num_pose = 10
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)
print(optimizer.vertices())
point_id = num_pose
inliers = dict()
sse = defaultdict(float)
vp_edge = []
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_estimate(point + np.random.randn(3))
vp.set_id(point_id)
vp.set_marginalized(True)
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
e = g2o.EdgeSE3ProjectXYZ()
e.set_vertex(0,vp)
e.set_vertex(1,optimizer.vertex(j))
e.set_measurement(z)
e.set_information(np.identity(2))
rk = g2o.RobustKernelHuber()
e.set_robust_kernel(rk)
rk.set_delta(delta)
e.fx = fx
e.fy = fy
e.cx = cx
e.cy = cy
optimizer.add_edge(e)
vp_edge.append(e)
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(optimizer.vertices())
print()
print('Performing full BA:')
optimizer.initialize_optimization()
optimizer.set_verbose(True)
optimizer.optimize(5)
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)))
# print(optimizer.vertices())
print()
for i in xrange(num_pose):
print(optimizer.vertex(i).estimate().inverse().matrix())
j = num_pose
for i in xrange(len(inliers)):
print(optimizer.vertex(j).estimate().shape)
j += 1
i = 0
"""
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 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
camera = g2o.CameraParameters(500, [320, 240], 0.075)
camera.set_id(10000)
optimizer.add_parameter(camera)
true_poses = []
num_pose = 5
for i in range(num_pose):
# pose here 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 = camera.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.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(2) * args.pixel_noise
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(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('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('\nCamera focal: \n{}\n'.format(camera.focal_length))
print('\nCamera principal point: \n{}\n'.format(camera.principal_point))
print('\nRMSE (inliers only):')
print('before optimization:', np.sqrt(sse[0] / len(inliers)))
print('after optimization:', np.sqrt(sse[1] / len(inliers)))
def add_camera_parameters(self, focal_length, principle_point):
camera_parameter = g2o.CameraParameters(focal_length, principle_point,
0)
camera_parameter.set_id(0)
self._optimizer.add_parameter(camera_parameter)
return camera_parameter
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()
def graph_to_optimizer(self):
"""Convert a :class: graph to a :class: g2o.SparseOptimizer. Only the edges and vertices fields need to be
filled out.
Returns:
A :class: g2o.SparseOptimizer that can be optimized via its optimize class method.
"""
optimizer = g2o.SparseOptimizer()
optimizer.set_algorithm(g2o.OptimizationAlgorithmLevenberg(
g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())))
if self.is_sparse_bundle_adjustment:
for i in self.vertices:
if self.vertices[i].mode == VertexType.TAGPOINT:
vertex = g2o.VertexSBAPointXYZ()
vertex.set_estimate(self.vertices[i].estimate[:3])
else:
vertex = g2o.VertexSE3Expmap()
vertex.set_estimate(pose_to_se3quat(self.vertices[i].estimate))
vertex.set_id(i)
vertex.set_fixed(self.vertices[i].fixed)
optimizer.add_vertex(vertex)
cam_idx = 0
for i in self.edges:
if self.edges[i].corner_ids is None:
edge = g2o.EdgeSE3Expmap()
for j, k in enumerate([self.edges[i].startuid,
self.edges[i].enduid]):
edge.set_vertex(j, optimizer.vertex(k))
edge.set_measurement(pose_to_se3quat(self.edges[i].measurement))
edge.set_information(self.edges[i].information)
optimizer.add_edge(edge)
else:
# Note: we only use the focal length in the x direction since: (a) that's all that g2o supports and
# (b) it is always the same in ARKit (at least currently)
cam = g2o.CameraParameters(self.edges[i].camera_intrinsics[0],
self.edges[i].camera_intrinsics[2:], 0)
cam.set_id(cam_idx)
optimizer.add_parameter(cam)
for corner_idx, corner_id in enumerate(self.edges[i].corner_ids):
edge = g2o.EdgeProjectPSI2UV()
edge.resize(3)
edge.set_vertex(0, optimizer.vertex(corner_id))
edge.set_vertex(1, optimizer.vertex(self.edges[i].startuid))
edge.set_vertex(2, optimizer.vertex(self.edges[i].enduid))
edge.set_information(self.edges[i].information)
edge.set_measurement(self.edges[i].measurement[corner_idx * 2:corner_idx * 2 + 2])
edge.set_parameter_id(0, cam_idx)
if self.use_huber:
edge.set_robust_kernel(g2o.RobustKernelHuber(self.huber_delta))
optimizer.add_edge(edge)
cam_idx += 1
else:
for i in self.vertices:
vertex = g2o.VertexSE3()
vertex.set_id(i)
vertex.set_estimate(pose_to_isometry(self.vertices[i].estimate))
vertex.set_fixed(self.vertices[i].fixed)
optimizer.add_vertex(vertex)
for i in self.edges:
edge = g2o.EdgeSE3()
for j, k in enumerate([self.edges[i].startuid,
self.edges[i].enduid]):
edge.set_vertex(j, optimizer.vertex(k))
edge.set_measurement(pose_to_isometry(self.edges[i].measurement))
edge.set_information(self.edges[i].information)
edge.set_id(i)
optimizer.add_edge(edge)
return optimizer
def Init(self):
pointCloud = self.points
measurements = self.measurements
once = False
# set current frame as vertex to be optimized
cam = self.frame.camera
if not once:
K = cam.K
focal_length = (K[0, 0] + K[1, 1]) / 2
pp = (K[0, 2], K[1, 2])
cam_p = g2o.CameraParameters(focal_length, pp, 0)
cam_p.set_id(0)
self.add_parameter(cam_p)
once = True
pose = g2o.SE3Quat(cam.R0, cam.t0.reshape(3, ))
v_idx = self.vertex_seq_generate("Frame", self.frame.seq)
self.vSE3 = self.add_pose(v_idx, pose, False)
# add point
# set array of MapPoint as vertices
for _, point in pointCloud.items():
v = point.data
v_idx = self.vertex_seq_generate("MapPoint", point.seq)
# We only optimize pose, it is also possible to use g2o::EdgeSE3ProjectXYZOnlyPose
self.add_point(v_idx, v, marginalized=True, fixed=True)
# viewed by the frame
key = (v_idx, self.indexOfVertex("Frame", self.frame.seq))
e_idx = self.edge_seq_generate(key)
# measurement
measurement = measurements[point.seq]
px = measurement.px2d2
# @todo: TODO compute invSigma for : see ORBSlam implementation for details
# I have little idea how this should be set
invSigma = 1.
edge = self.add_edge(e_idx, key[0], key[1], px.data,
information=np.identity(2) * invSigma)
#
device = self.frame.camera.device
# modify python/types/sba/type_six_dof_expmap.h#L81
#
# Projection using focal_length in x and y directions
# py::class_<EdgeSE3ProjectXYZ, BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>>(m, "EdgeSE3ProjectXYZ")
# .def(py::init<>())
# .def("compute_error", &EdgeSE3ProjectXYZ::computeError)
# .def("is_depth_positive", &EdgeSE3ProjectXYZ::isDepthPositive)
# .def("cam_project", &EdgeSE3ProjectXYZ::cam_project)
# + .def_readwrite("fx", &EdgeSE3ProjectXYZ::fx)
# + .def_readwrite("fy", &EdgeSE3ProjectXYZ::fy)
# + .def_readwrite("cx", &EdgeSE3ProjectXYZ::cx)
# + .def_readwrite("cy", &EdgeSE3ProjectXYZ::cy)
# ;
#
# edge.fx = device.fx
# edge.fy = device.fy
# edge.cx = device.cx
# edge.cy = device.cy
pass
def Init(self):
adjusted, fixed = self.get_local_keyframes()
print("[LocalBA] %d adjusted frames" % len(adjusted))
print("[LocalBA] %d fixed frames" % len(fixed))
self.adjusted_frames = adjusted
self.fixed_frames = fixed
if len(adjusted) == 0:
print("Something wrong here ...")
pass
once = False
# construct graph
# set key frame as vertices
for i, frame in enumerate(adjusted):
cam = frame.camera
pose = None
if cam is None:
continue
pose = g2o.SE3Quat(cam.R0, cam.t0.reshape(3, ))
v_idx = self.vertex_seq_generate("Frame", frame.seq)
# only set the first frame as stational piont
# self.add_pose(v_idx, pose, False)#fixed=frame.seq == 1)
# when use ground truth
self.add_pose(v_idx, pose, fixed=False)
if not once:
K = cam.K
focal_length = (K[0, 0] + K[1, 1]) / 2
pp = (K[0, 2], K[1, 2])
cam_p = g2o.CameraParameters(focal_length, pp, 0)
cam_p.set_id(0)
self.add_parameter(cam_p)
once = True
pointCloud, Measurement = frame.get_measurements()
N = len(pointCloud)
for i in range(N):
point = pointCloud[i]
v = point.data # + np.random.randn(3)
v_idx = self.vertex_seq_generate("MapPoint", point.seq)
self.add_point(v_idx, v, marginalized=True)
# set edge
cam = frame.camera
if cam is None:
continue
key = (v_idx, self.indexOfVertex("Frame", frame.seq))
e_idx = self.edge_seq_generate(key)
# measurement
px = Measurement[i]
# @todo: TODO compute invSigma for : see ORBSlam implementation for details
invSigma = 1.
if not isinstance(key[1], int):
print("key[1]", key[1])
raise Exception("Wrong value!")
edge = self.add_edge(e_idx, key[0], key[1], px.data, # + np.random.randn(2),
information=np.identity(2) * invSigma)
self.mappoints.append((point, frame.seq, px))
# set camera parameters to compute reprojection error with measurements
cam = frame.camera
device = cam.device
# modify python/types/sba/type_six_dof_expmap.h#L81
#
# Projection using focal_length in x and y directions
# py::class_<EdgeSE3ProjectXYZ, BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>>(m, "EdgeSE3ProjectXYZ")
# .def(py::init<>())
# .def("compute_error", &EdgeSE3ProjectXYZ::computeError)
# .def("is_depth_positive", &EdgeSE3ProjectXYZ::isDepthPositive)
# .def("cam_project", &EdgeSE3ProjectXYZ::cam_project)
# + .def_readwrite("fx", &EdgeSE3ProjectXYZ::fx)
# + .def_readwrite("fy", &EdgeSE3ProjectXYZ::fy)
# + .def_readwrite("cx", &EdgeSE3ProjectXYZ::cx)
# + .def_readwrite("cy", &EdgeSE3ProjectXYZ::cy)
# ;
#
# edge.fx = device.fx
# edge.fy = device.fy
# edge.cx = device.cx
# edge.cy = device.cy
# ===== FIXED =====
for i, frame in enumerate(fixed):
cam = frame.camera
pose = None
if cam is None:
continue
pose = g2o.SE3Quat(cam.R0, cam.t0.reshape(3, ))
v_idx = self.vertex_seq_generate("Frame", frame.seq)
# only set the first frame as stational piont
# self.add_pose(v_idx, pose, False)#fixed=frame.seq == 1)
# when use ground truth
self.add_pose(v_idx, pose, fixed=True)
if not once:
K = cam.K
focal_length = (K[0, 0] + K[1, 1]) / 2
pp = (K[0, 2], K[1, 2])
cam_p = g2o.CameraParameters(focal_length, pp, 0)
cam_p.set_id(0)
self.add_parameter(cam_p)
once = True
pointCloud, Measurement = frame.get_measurements()
N = len(pointCloud)
for i in range(N):
point = pointCloud[i]
v = point.data # + np.random.randn(3)
v_idx = self.vertex_seq_generate("MapPoint", point.seq)
self.add_point(v_idx, v, marginalized=True)
# set edge
cam = frame.camera
if cam is None:
continue
key = (v_idx, self.indexOfVertex("Frame", frame.seq))
e_idx = self.edge_seq_generate(key)
# measurement
px = Measurement[i]
# @todo: TODO compute invSigma for : see ORBSlam implementation for details
invSigma = 1.
if not isinstance(key[1], int):
print("key[1]", key[1])
raise Exception("Wrong value!")
edge = self.add_edge(e_idx, key[0], key[1], px.data, # + np.random.randn(2),
information=np.identity(2) * invSigma)
self.mappoints.append((point, frame.seq, px))
# set camera parameters to compute reprojection error with measurements
cam = frame.camera
device = cam.device
# modify python/types/sba/type_six_dof_expmap.h#L81
#
# Projection using focal_length in x and y directions
# py::class_<EdgeSE3ProjectXYZ, BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>>(m, "EdgeSE3ProjectXYZ")
# .def(py::init<>())
# .def("compute_error", &EdgeSE3ProjectXYZ::computeError)
# .def("is_depth_positive", &EdgeSE3ProjectXYZ::isDepthPositive)
# .def("cam_project", &EdgeSE3ProjectXYZ::cam_project)
# + .def_readwrite("fx", &EdgeSE3ProjectXYZ::fx)
# + .def_readwrite("fy", &EdgeSE3ProjectXYZ::fy)
# + .def_readwrite("cx", &EdgeSE3ProjectXYZ::cx)
# + .def_readwrite("cy", &EdgeSE3ProjectXYZ::cy)
# ;
#
# edge.fx = device.fx
# edge.fy = device.fy
# edge.cx = device.cx
# edge.cy = device.cy
pass
def Init(self):
frames = self._map.get_active_frames()
landmarks = self._map.trackList()
pointCloud = self._map.trackPointsList()
once = False
# construct graph
# set key frame as vertices
for i, frame in enumerate(frames):
cam = frame.camera
pose = None
if cam is None:
continue
pose = g2o.SE3Quat(cam.R0, cam.t0.reshape(3, ))
v_idx = self.vertex_seq_generate("Frame", frame.seq)
# only set the first frame as stational piont
# self.add_pose(v_idx, pose, False)#fixed=frame.seq == 1)
# when use ground truth
self.add_pose(v_idx, pose, fixed=frame.seq == 1)
if not once:
K = cam.K
focal_length = (K[0, 0] + K[1, 1]) / 2
pp = (K[0, 2], K[1, 2])
cam_p = g2o.CameraParameters(focal_length, pp, 0)
cam_p.set_id(0)
self.add_parameter(cam_p)
once = True
# set array of MapPoint as vertices
for _, point in pointCloud.items():
v = point.data # + np.random.randn(3)
v_idx = self.vertex_seq_generate("MapPoint", point.seq)
self.add_point(v_idx, v, marginalized=True)
# set edges
try:
observations = point.frames
except AttributeError:
observations = point.observations
for frame_key, pixel_pos in observations.items():
frame = self._map.findFrame(frame_key)
cam = frame.camera
if cam is None:
continue
key = (v_idx, self.indexOfVertex("Frame", frame_key))
e_idx = self.edge_seq_generate(key)
# measurement
px = frame.pixels[pixel_pos]
# @todo: TODO compute invSigma for : see ORBSlam implementation for details
invSigma = 1.
if not isinstance(key[1], int):
print("key[1]", key[1])
raise Exception("Wrong value!")
edge = self.add_edge(e_idx, key[0], key[1], px.data, # + np.random.randn(2),
information=np.identity(2) * invSigma)
# set camera parameters to compute reprojection error with measurements
cam = frame.camera
device = cam.device
# modify python/types/sba/type_six_dof_expmap.h#L81
#
# Projection using focal_length in x and y directions
# py::class_<EdgeSE3ProjectXYZ, BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>>(m, "EdgeSE3ProjectXYZ")
# .def(py::init<>())
# .def("compute_error", &EdgeSE3ProjectXYZ::computeError)
# .def("is_depth_positive", &EdgeSE3ProjectXYZ::isDepthPositive)
# .def("cam_project", &EdgeSE3ProjectXYZ::cam_project)
# + .def_readwrite("fx", &EdgeSE3ProjectXYZ::fx)
# + .def_readwrite("fy", &EdgeSE3ProjectXYZ::fy)
# + .def_readwrite("cx", &EdgeSE3ProjectXYZ::cx)
# + .def_readwrite("cy", &EdgeSE3ProjectXYZ::cy)
# ;
#
# edge.fx = device.fx
# edge.fy = device.fy
# edge.cx = device.cx
# edge.cy = device.cy
# check our modification is correct: I am not sure whether g2opy runs as expected so we check the result manually.
# used for EdgeSE3ProjectXYZ
# measurement = edge.cam_project( edge.vertex(1).estimate().map( edge.vertex(0).estimate() ) )
# print("Measurement: %s" % measurement)
# print("px: %s" % px.data)
# assert int(measurement[0]) == int(px.x) and int(measurement[1]) == int(px.y)
pass
if self.USE_LANDMARKS:
# treat landmark as stationary points group and compute key points from it
logging.info("Using landmarks for bundle adjustment ...")
# set landmarks as vertices
for _, landmark in landmarks.items():
# representing landmark as top centre point
logging.info("%s points size %d" % (landmark, len(landmark.points)))
# compute bbox
# Note: I just use AABB instead of OBB, because to estimate OBB we need dense points
# bbox = landmark.computeAABB()
# topCentre = bbox.topCentre()
# v = topCentre.data
centroid = landmark.Centroid()
v = centroid.data
v_idx = self.vertex_seq_generate("MapPoint", centroid.seq)
self.add_point(v_idx, v, marginalized=True)
# estimate measurement
# we suppose the positions (centers) of landmarks are stable, which will be used in PnP later
# @todo : TODO
# set edges
observations = landmark.observations
# choose an estimate pose
visited_pxes = {}
for (point_key, frame_seq), pixel_pos in observations.items():
point = pointCloud[point_key]
# frame = frames[frame_seq - 1]
frame = self._map.findFrame(frame_key)
cam = frame.camera
if cam is None:
continue
px_pose = point[frame_seq]
px = frame.pixels[pixel_pos]
if visited_pxes.get(frame_seq, None) is None:
visited_pxes = []
visited_pxes[frame_seq].append(px.data)
pass
for frame_seq, projected_pixels in visited_pxes:
reduced = reduce(lambda pre, cur: pre + cur, projected_pixels)
reduced /= len(projected_pixels)
key = (v_idx, self.indexOfVertex("Frame", frame_seq))
e_idx = self.edge_seq_generate(key)
# @todo: TODO compute invSigma for : see ORBSlam implementation for details
# I have little idea how this should be set
invSigma = 1.
edge = self.add_edge(self.vertexPair_edges[key], key[0], key[1], reduced,
information=np.identity(2) * invSigma)
# set camera parameters to compute reprojection error with measurements
cam = frame.camera
device = cam.device
# add camera parameters to compute reprojection errors
# edge.fx = device.fx
# edge.fy = device.fy
# edge.cx = device.cx
# edge.cy = device.cy
pass
pass
logging.info("Number of vertices:", len(self.vertices()))
logging.info("Number of edges:", len(self.edges()))
return self
def add_camera_parameter(self, focal_length, principal_point):
cam = g2o.CameraParameters(focal_length, principal_point, 0)
cam.set_id(0)
super().add_parameter(cam)
triangulationPairsPoints_RB)
F2_err = camera_F2.computeReprojectionError(triangulationPairsPoints_LF,
triangulationPairsPoints_FR)
print(F_err)
print(L_err)
print(B_err)
print(R_err)
print(F2_err)
# Optimiaztion
poseOptimizer = PoseGraphOptimization()
# Add pose vertices & Set camera parameter
pose = g2o.SE3Quat(camera_F.R, camera_F.t.flatten())
vertexF = poseOptimizer.add_vertex_pose(0, pose, fixed=True)
parm = g2o.CameraParameters(camera_F.K[0, 0], camera_F.K[0:2, 2], 0)
parm.set_id(0)
poseOptimizer.add_parameter(parm)
vertexes = []
vertexes.append(vertexF)
for cam, id in zip([camera_L, camera_B, camera_R, camera_F2],
list(range(1, 5))):
pose = g2o.SE3Quat(cam.R, cam.t.flatten())
vertex = poseOptimizer.add_vertex_pose(id, pose, fixed=False)
vertexes.append(vertex)
parm = g2o.CameraParameters(cam.K[0, 0], cam.K[0:2, 2], 0)
parm.set_id(id)
poseOptimizer.add_parameter(parm)
# Add 3d points and 2 edge from 2d points
