def __init__(self, verbose=False):
self.solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
self.solver = g2o.OptimizationAlgorithmLevenberg(self.solver)
self.optimizer = g2o.SparseOptimizer()
self.optimizer.set_verbose(verbose)
self.optimizer.set_algorithm(self.solver)
def __init__(self, max_iterations=100, verbose=False, online=False):
solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
self.optimizer = g2o.SparseOptimizer()
self.optimizer.set_verbose(True)
self.optimizer.set_algorithm(solver)
self.current_pose = None
self.max_iterations = max_iterations
self.verbose = verbose
self.online = online
self.clear()
def main():
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:{}'.format(len(optimizer.vertices())))
print('num edges:{}'.format(len(optimizer.edges())))
for i in range(len(optimizer.vertices())):
vp_se3 = optimizer.vertex(i)
print(dir(vp_se3))
print(vp_se3.id)
break
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)
import sys
import numpy as np
import math
sys.path.append("../../")
from config import Config
import g2o
opt = g2o.SparseOptimizer()
block_solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
solver = g2o.OptimizationAlgorithmLevenberg(block_solver)
opt.set_algorithm(solver)
flag = g2o.Flag()
print('flag: ', flag.value)
opt.set_force_stop_flag(flag)
flag.value = False
print('opt flag: ', opt.force_stop_flag())
flag.value = True
print('opt flag: ', opt.force_stop_flag())
def pose_optimization(frame, verbose=False, rounds=10):
is_ok = True
# create g2o optimizer
opt = g2o.SparseOptimizer()
#block_solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
#block_solver = g2o.BlockSolverSE3(g2o.LinearSolverDenseSE3())
block_solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
solver = g2o.OptimizationAlgorithmLevenberg(block_solver)
opt.set_algorithm(solver)
#robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991)) # chi-square 2 DOFs
thHuberMono = math.sqrt(5.991); # chi-square 2 DOFS
point_edge_pairs = {}
num_point_edges = 0
v_se3 = g2o.VertexSE3Expmap()
v_se3.set_estimate(g2o.SE3Quat(frame.Rcw, frame.tcw))
v_se3.set_id(0)
v_se3.set_fixed(False)
opt.add_vertex(v_se3)
with MapPoint.global_lock:
# add point vertices to graph
for idx, p in enumerate(frame.points):
if p is None:
continue
# reset outlier flag
frame.outliers[idx] = False
# add edge
#print('adding edge between point ', p.id,' and frame ', frame.id)
edge = g2o.EdgeSE3ProjectXYZOnlyPose()
edge.set_vertex(0, opt.vertex(0))
edge.set_measurement(frame.kpsu[idx])
invSigma2 = Frame.feature_manager.inv_level_sigmas2[frame.octaves[idx]]
edge.set_information(np.eye(2)*invSigma2)
edge.set_robust_kernel(g2o.RobustKernelHuber(thHuberMono))
edge.fx = frame.camera.fx
edge.fy = frame.camera.fy
edge.cx = frame.camera.cx
edge.cy = frame.camera.cy
edge.Xw = p.pt[0:3]
opt.add_edge(edge)
point_edge_pairs[p] = (edge, idx) # one edge per point
num_point_edges += 1
if num_point_edges < 3:
Printer.red('pose_optimization: not enough correspondences!')
is_ok = False
return 0, is_ok, 0
if verbose:
opt.set_verbose(True)
# perform 4 optimizations:
# after each optimization we classify observation as inlier/outlier;
# at the next optimization, outliers are not included, but at the end they can be classified as inliers again
chi2Mono = 5.991 # chi-square 2 DOFs
num_bad_point_edges = 0
for it in range(4):
v_se3.set_estimate(g2o.SE3Quat(frame.Rcw, frame.tcw))
opt.initialize_optimization()
opt.optimize(rounds)
num_bad_point_edges = 0
for p, edge_pair in point_edge_pairs.items():
edge, idx = edge_pair
if frame.outliers[idx]:
edge.compute_error()
chi2 = edge.chi2()
if chi2 > chi2Mono:
frame.outliers[idx] = True
edge.set_level(1)
num_bad_point_edges +=1
else:
frame.outliers[idx] = False
edge.set_level(0)
if it == 2:
edge.set_robust_kernel(None)
if len(opt.edges()) < 10:
Printer.red('pose_optimization: stopped - not enough edges!')
is_ok = False
break
print('pose optimization: available ', num_point_edges, ' points, found ', num_bad_point_edges, ' bad points')
if num_point_edges == num_bad_point_edges:
Printer.red('pose_optimization: all the available correspondences are bad!')
is_ok = False
# update pose estimation
if is_ok:
est = v_se3.estimate()
# R = est.rotation().matrix()
# t = est.translation()
# frame.update_pose(poseRt(R, t))
frame.update_pose(g2o.Isometry3d(est.orientation(), est.position()))
# since we have only one frame here, each edge corresponds to a single distinct point
num_valid_points = num_point_edges - num_bad_point_edges
mean_squared_error = opt.active_chi2()/max(num_valid_points,1)
return mean_squared_error, is_ok, num_valid_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 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)))
