def add_pose(self, pose_id, pose, fixed=False):
if isinstance(pose, sp.sophuspy.SE3):
pose = g2o.SE3Quat(pose.rotationMatrix(), pose.translation())
elif isinstance(pose, np.ndarray):
pose = g2o.SE3Quat(pose[:3, :3], pose[:3, 3])
v_se3 = g2o.VertexSE3Expmap()
v_se3.set_id(pose_id * 2) # camera poses use even number
v_se3.set_estimate(pose)
v_se3.set_fixed(fixed)
super().add_vertex(v_se3)
def bundle_adjustment(ps_1, ps_2, r_mat, t_vec):
optimizer = g2o.SparseOptimizer()
solver = g2o.BlockSolverSim3(g2o.LinearSolverCSparseSim3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
optimizer.set_algorithm(solver)
pose = g2o.VertexSE3Expmap()
pose.set_estimate(g2o.SE3Quat(np.identity(3), np.zeros((3,))))
pose.set_id(0)
optimizer.add_vertex(pose)
index = 1
for p1, p2 in zip(ps_1, ps_2):
edge = g2o.EdgeStereoSE3ProjectXYZOnlyPose()
edge.cam_project(p2)
edge.set_id(index)
edge.set_vertex(0, pose)
edge.set_measurement(p1)
edge.set_information(np.identity(3))
optimizer.add_edge(edge)
index += 1
optimizer.initialize_optimization()
optimizer.set_verbose(True)
optimizer.optimize(100)
print('T = \n', pose.estimate().matrix())
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 pose_to_se3quat(pose):
"""Convert a pose vector to a :class: g2o.Isometry3d instance.
Args:
pose: A 7 element 1-d numpy array encoding x, y, z, qx, qy, qz, and qw respectively.
Returns:
A :class: g2o.Isometry3d instance encoding the same information as the input pose.
"""
return g2o.SE3Quat(g2o.Quaternion(*np.roll(pose[3:7], 1)), pose[:3])
def _compute_transform_cam0_wrt_cam1_by_bundle_adjustment(
self,
transform_cam0_wrt_cam1,
points_3d_in_camera_frame0,
iterations=10,
verbose=False):
self._bundle_adjustment.add_camera_parameters(
self._camera0.focal_length_in_pixels, self._camera0.pixel_center)
# First camera frame0 is regarded as the origin of the world
self._bundle_adjustment.add_pose(0,
g2o.SE3Quat(R=np.identity(3),
t=np.zeros(3)),
fixed=True)
# IMPORTANT NOTE: rotation and translation of pose is described as world wrt to pose
self._bundle_adjustment.add_pose(
1,
g2o.SE3Quat(R=transform_cam0_wrt_cam1.rotation,
t=transform_cam0_wrt_cam1.translation))
points_id = np.arange(len(
points_3d_in_camera_frame0.T)) + self._points_3d_initial_index
for point_id, point_3d in zip(points_id, points_3d_in_camera_frame0.T):
self._bundle_adjustment.add_point(point_id, point_3d)
pose_id_to_points_map = {
0: (points_id, self._points_in_image_frame0.T),
1: (points_id, self._points_in_image_frame1.T),
}
for pose_id, bundle in pose_id_to_points_map.items():
for point_id, measured_point_2d in zip(*bundle):
self._bundle_adjustment.add_edge(point_id,
pose_id,
measured_point_2d,
information=np.identity(2))
self._bundle_adjustment.optimize(iterations, verbose)
return HomogeneousMatrix(
self._bundle_adjustment.vertex_estimate(1).matrix()[:3, :4])
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 compute_reproj_g2o(self, srcImage, srcDepth, srcMask, dstImage,
dstDepth, dstMask):
pose_pre = g2o.SE3Quat(sp.SE3().matrix()[:3, :3],
sp.SE3().matrix()[:3, 3])
pose_cur = g2o.SE3Quat(self.initial_pose.matrix()[:3, :3],
self.initial_pose.matrix()[:3, 3])
cam = CameraParameter(self.cameraMatrix[0][0], self.cameraMatrix[1][1],
self.cameraMatrix[0][2], self.cameraMatrix[1][2])
BA = BundleAdjustment()
BA.add_pose(0, pose_pre, cam, fixed=True)
BA.add_pose(1, pose_cur, cam, fixed=False)
points2D_pre, points2D_cur, kp_pre, kp_cur = self.process_feature(
srcImage, dstImage)
points3D = []
for i, j in enumerate(points2D_pre):
row = int(kp_pre[i].pt[1])
col = int(kp_pre[i].pt[0])
z = srcDepth[row][col]
p3d_x = (j[0] - cx) * z / fx
p3d_y = (j[1] - cy) * z / fy
p3d = np.array([p3d_x, p3d_y, z])
points3D.append(p3d)
points3D = np.array(points3D)
for i in range(np.shape(points3D)[0]):
p3d = points3D[i]
if (np.isnan(p3d[2]) == False):
print(p3d)
BA.add_point(i, points3D[i], fixed=True)
BA.add_edge(i, 0, i, points2D_pre[i])
BA.add_edge(i, 1, i + 1, points2D_cur[i])
BA.optimize(max_iterations=self.max_it)
BA_pose = sp.SE3(BA.get_pose(1).to_homogeneous_matrix())
self.initial_pose = BA_pose
return BA_pose.matrix()
def optimizeWhileFiltering(self):
MaxIter = 5
it = 0
vSE3 = self.vSE3
cam = self.frame.camera
outliers = {}
while it < MaxIter:
vSE3.set_estimate(g2o.SE3Quat(cam.R0, cam.t0.reshape(3, )))
self.optimize(level=it)
# @todo : TODO
# see ORBSlam PoseOptimization
pass
pass
def add_frames(self, frames, local_frames):
# add frames to graph
for f in (local_frames if self.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)
self.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())
self.graph_frames[f] = v_se3
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 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 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 local_bundle_adjustment(keyframes, points, keyframes_ref=[], fixed_points=False, verbose=False, rounds=10, abort_flag=g2o.Flag(), map_lock=None):
# create g2o optimizer
opt = g2o.SparseOptimizer()
block_solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
#block_solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
#block_solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
solver = g2o.OptimizationAlgorithmLevenberg(block_solver)
opt.set_algorithm(solver)
opt.set_force_stop_flag(abort_flag)
#robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991)) # chi-square 2 DOFs
thHuberMono = math.sqrt(5.991); # chi-square 2 DOFS
graph_keyframes, graph_points = {}, {}
# add frame vertices to graph
for kf in keyframes:
if kf.is_bad:
continue
#print('adding vertex frame ', f.id, ' to graph')
se3 = g2o.SE3Quat(kf.Rcw, kf.tcw)
v_se3 = g2o.VertexSE3Expmap()
v_se3.set_estimate(se3)
v_se3.set_id(kf.kid * 2) # even ids (use f.kid here!)
v_se3.set_fixed(kf.kid==0) # (use f.kid here!)
opt.add_vertex(v_se3)
graph_keyframes[kf] = 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())
# add reference frame vertices to graph
for kf in keyframes_ref:
if kf.is_bad:
continue
#print('adding vertex frame ', f.id, ' to graph')
se3 = g2o.SE3Quat(kf.Rcw, kf.tcw)
v_se3 = g2o.VertexSE3Expmap()
v_se3.set_estimate(se3)
v_se3.set_id(kf.kid * 2) # even ids (use f.kid here!)
v_se3.set_fixed(True)
opt.add_vertex(v_se3)
graph_keyframes[kf] = v_se3
graph_edges = {}
num_edges = 0
num_bad_edges = 0
# add point vertices to graph
for p in points:
assert(p is not None)
if p.is_bad: # do not consider bad points
continue
if not any([f in keyframes for f in p.keyframes()]):
Printer.orange('point %d without a viewing keyframe in input keyframes!!' %(p.id))
#Printer.orange(' keyframes: ',p.observations_string())
continue
#print('adding vertex point ', p.id,' to graph')
v_p = g2o.VertexSBAPointXYZ()
v_p.set_id(p.id * 2 + 1) # odd ids
v_p.set_estimate(p.pt[0:3])
v_p.set_marginalized(True)
v_p.set_fixed(fixed_points)
opt.add_vertex(v_p)
graph_points[p] = v_p
# add edges
for kf, p_idx in p.observations():
if kf.is_bad:
continue
if kf not in graph_keyframes:
continue
if __debug__:
p_f = kf.get_point_match(p_idx)
if p_f != p:
print('frame: ', kf.id, ' missing point ', p.id, ' at index p_idx: ', p_idx)
if p_f is not None:
print('p_f:', p_f)
print('p:',p)
assert(kf.get_point_match(p_idx) is p)
#print('adding edge between point ', p.id,' and frame ', f.id)
edge = g2o.EdgeSE3ProjectXYZ()
edge.set_vertex(0, v_p)
edge.set_vertex(1, graph_keyframes[kf])
edge.set_measurement(kf.kpsu[p_idx])
invSigma2 = Frame.feature_manager.inv_level_sigmas2[kf.octaves[p_idx]]
edge.set_information(np.eye(2)*invSigma2)
edge.set_robust_kernel(g2o.RobustKernelHuber(thHuberMono))
edge.fx = kf.camera.fx
edge.fy = kf.camera.fy
edge.cx = kf.camera.cx
edge.cy = kf.camera.cy
opt.add_edge(edge)
graph_edges[edge] = (p,kf,p_idx) # one has kf.points[p_idx] == p
num_edges += 1
if verbose:
opt.set_verbose(True)
if abort_flag.value:
return -1,0
# initial optimization
opt.initialize_optimization()
opt.optimize(5)
if not abort_flag.value:
chi2Mono = 5.991 # chi-square 2 DOFs
# check inliers observation
for edge, edge_data in graph_edges.items():
p = edge_data[0]
if p.is_bad:
continue
if edge.chi2() > chi2Mono or not edge.is_depth_positive():
edge.set_level(1)
num_bad_edges += 1
edge.set_robust_kernel(None)
# optimize again without outliers
opt.initialize_optimization()
opt.optimize(rounds)
# search for final outlier observations and clean map
num_bad_observations = 0 # final bad observations
outliers_data = []
for edge, edge_data in graph_edges.items():
p, kf, p_idx = edge_data
if p.is_bad:
continue
assert(kf.get_point_match(p_idx) is p)
if edge.chi2() > chi2Mono or not edge.is_depth_positive():
num_bad_observations += 1
outliers_data.append(edge_data)
if map_lock is None:
map_lock = RLock() # put a fake lock
with map_lock:
# remove outlier observations
for d in outliers_data:
p, kf, p_idx = d
p_f = kf.get_point_match(p_idx)
if p_f is not None:
assert(p_f is p)
p.remove_observation(kf,p_idx)
# the following instruction is now included in p.remove_observation()
#f.remove_point(p) # it removes multiple point instances (if these are present)
#f.remove_point_match(p_idx) # this does not remove multiple point instances, but now there cannot be multiple instances any more
# put frames back
for kf in graph_keyframes:
est = graph_keyframes[kf].estimate()
#R = est.rotation().matrix()
#t = est.translation()
#f.update_pose(poseRt(R, t))
kf.update_pose(g2o.Isometry3d(est.orientation(), est.position()))
# put points back
if not fixed_points:
for p in graph_points:
p.update_position(np.array(graph_points[p].estimate()))
p.update_normal_and_depth(force=True)
active_edges = num_edges-num_bad_edges
mean_squared_error = opt.active_chi2()/active_edges
return mean_squared_error, num_bad_observations/max(num_edges,1)
def bundle_adjustment(keyframes, points, local_window, fixed_points=False, verbose=False, rounds=10, use_robust_kernel=False, abort_flag=g2o.Flag()):
if local_window is None:
local_frames = keyframes
else:
local_frames = keyframes[-local_window:]
# create g2o optimizer
opt = g2o.SparseOptimizer()
block_solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
#block_solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
solver = g2o.OptimizationAlgorithmLevenberg(block_solver)
opt.set_algorithm(solver)
opt.set_force_stop_flag(abort_flag)
thHuberMono = math.sqrt(5.991); # chi-square 2 DOFS
graph_keyframes, graph_points = {}, {}
# add frame vertices to graph
for kf in (local_frames if fixed_points else keyframes): # if points are fixed then consider just the local frames, otherwise we need all frames or at least two frames for each point
if kf.is_bad:
continue
#print('adding vertex frame ', f.id, ' to graph')
se3 = g2o.SE3Quat(kf.Rcw, kf.tcw)
v_se3 = g2o.VertexSE3Expmap()
v_se3.set_estimate(se3)
v_se3.set_id(kf.kid * 2) # even ids (use f.kid here!)
v_se3.set_fixed(kf.kid==0 or kf not in local_frames) #(use f.kid here!)
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_keyframes[kf] = v_se3
num_edges = 0
# add point vertices to graph
for p in points:
assert(p is not None)
if p.is_bad: # do not consider bad points
continue
if __debug__:
if not any([f in keyframes for f in p.keyframes()]):
Printer.red('point without a viewing frame!!')
continue
#print('adding vertex point ', p.id,' to graph')
v_p = g2o.VertexSBAPointXYZ()
v_p.set_id(p.id * 2 + 1) # odd ids
v_p.set_estimate(p.pt[0:3])
v_p.set_marginalized(True)
v_p.set_fixed(fixed_points)
opt.add_vertex(v_p)
graph_points[p] = v_p
# add edges
for kf, idx in p.observations():
if kf.is_bad:
continue
if kf not in graph_keyframes:
continue
#print('adding edge between point ', p.id,' and frame ', f.id)
edge = g2o.EdgeSE3ProjectXYZ()
edge.set_vertex(0, v_p)
edge.set_vertex(1, graph_keyframes[kf])
edge.set_measurement(kf.kpsu[idx])
invSigma2 = Frame.feature_manager.inv_level_sigmas2[kf.octaves[idx]]
edge.set_information(np.eye(2)*invSigma2)
if use_robust_kernel:
edge.set_robust_kernel(g2o.RobustKernelHuber(thHuberMono))
edge.fx = kf.camera.fx
edge.fy = kf.camera.fy
edge.cx = kf.camera.cx
edge.cy = kf.camera.cy
opt.add_edge(edge)
num_edges += 1
if verbose:
opt.set_verbose(True)
opt.initialize_optimization()
opt.optimize(rounds)
# put frames back
for kf in graph_keyframes:
est = graph_keyframes[kf].estimate()
#R = est.rotation().matrix()
#t = est.translation()
#f.update_pose(poseRt(R, t))
kf.update_pose(g2o.Isometry3d(est.orientation(), est.position()))
# put points back
if not fixed_points:
for p in graph_points:
p.update_position(np.array(graph_points[p].estimate()))
p.update_normal_and_depth(force=True)
mean_squared_error = opt.active_chi2()/max(num_edges,1)
return mean_squared_error
def localOptimization(frames,
points,
frames_ref=[],
fixed_points=False,
verbose=False,
rounds=10):
# create g2o optimizer
opt = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
solver = g2o.OptimizationAlgorithmLevenberg(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
graph_frames, graph_points = {}, {}
all_frames = frames + frames_ref
# add frame vertices to graph
for f in all_frames:
#print('adding vertex frame ', f.id, ' to graph')
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) # even ids
v_se3.set_fixed(f.id < 1 or f in frames_ref)
opt.add_vertex(v_se3)
graph_frames[f] = 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_edges = {}
num_point_edges = 0
# add point vertices to graph
for p in points:
assert (p is not None)
if p.is_bad and not fixed_points: # do not consider bad points unless they are fixed
continue
if not any([f in frames for f in p.frames]): # this is redundant now
continue
#print('adding vertex point ', p.id,' to graph')
v_p = g2o.VertexSBAPointXYZ()
v_p.set_id(p.id * 2 + 1) # odd ids
v_p.set_estimate(p.pt[0:3])
v_p.set_marginalized(True)
v_p.set_fixed(fixed_points)
opt.add_vertex(v_p)
graph_points[p] = v_p
# add edges
for f, p_idx in zip(p.frames, p.idxs):
assert (f.points[p_idx] == p)
if f not in graph_frames:
continue
#print('adding edge between point ', p.id,' and frame ', f.id)
edge = g2o.EdgeSE3ProjectXYZ()
edge.set_vertex(0, v_p)
edge.set_vertex(1, graph_frames[f])
edge.set_measurement(f.kpsu[p_idx])
invSigma2 = Frame.detector.inv_level_sigmas2[f.octaves[p_idx]]
edge.set_information(np.eye(2) * invSigma2)
edge.set_robust_kernel(g2o.RobustKernelHuber(thHuberMono))
edge.fx = f.fx
edge.fy = f.fy
edge.cx = f.cx
edge.cy = f.cy
opt.add_edge(edge)
graph_edges[edge] = (p, f, p_idx) # f.points[p_idx] == p
num_point_edges += 1
if verbose:
opt.set_verbose(True)
# initial optimization
opt.initialize_optimization()
opt.optimize(5)
chi2Mono = 5.991 # chi-square 2 DOFs
# check inliers observation
for edge, edge_data in graph_edges.items():
p = edge_data[0]
if p.is_bad is True:
continue
if edge.chi2() > chi2Mono or not edge.is_depth_positive():
edge.set_level(1)
edge.set_robust_kernel(None)
# optimize again without outliers
opt.initialize_optimization()
opt.optimize(rounds)
# clean map observations
num_bad_observations = 0
outliers_data = []
for edge, edge_data in graph_edges.items():
p, f, p_idx = edge_data
if p.is_bad is True:
continue
assert (f.points[p_idx] == p)
if edge.chi2() > chi2Mono or not edge.is_depth_positive():
num_bad_observations += 1
outliers_data.append((p, f, p_idx))
for d in outliers_data:
(p, f, p_idx) = d
assert (f.points[p_idx] == p)
p.remove_observation(f, p_idx)
f.remove_point(p)
# 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 fixed_points:
for p in graph_points:
p.pt = np.array(graph_points[p].estimate())
return opt.active_chi2(), num_bad_observations / num_point_edges
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 poseOptimization(frame, verbose=False, rounds=10):
is_ok = True
# create g2o optimizer
opt = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
solver = g2o.OptimizationAlgorithmLevenberg(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
se3 = g2o.SE3Quat(frame.pose[0:3, 0:3], frame.pose[0:3, 3])
v_se3 = g2o.VertexSE3Expmap()
v_se3.set_estimate(se3)
v_se3.set_id(0)
v_se3.set_fixed(False)
opt.add_vertex(v_se3)
# add point vertices to graph
for idx, p in enumerate(frame.points):
if p is None: # do not use p.is_bad here since a single point observation is ok for pose optimization
continue
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.detector.inv_level_sigmas2[frame.octaves[idx]]
edge.set_information(np.eye(2) * invSigma2)
edge.set_robust_kernel(g2o.RobustKernelHuber(thHuberMono))
edge.fx = frame.fx
edge.fy = frame.fy
edge.cx = frame.cx
edge.cy = frame.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('poseOptimization: not enough correspondences!')
is_ok = False
return 0, is_ok, 0
if verbose:
opt.set_verbose(True)
# We 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_points = 0
for it in range(4):
opt.initialize_optimization()
opt.optimize(rounds)
num_bad_points = 0
for p, edge_pair in point_edge_pairs.items():
if frame.outliers[edge_pair[1]] is True:
edge_pair[0].compute_error()
chi2 = edge_pair[0].chi2()
if chi2 > chi2Mono:
frame.outliers[edge_pair[1]] = True
edge_pair[0].set_level(1)
num_bad_points += 1
else:
frame.outliers[edge_pair[1]] = False
edge_pair[0].set_level(0)
if it == 2:
edge_pair[0].set_robust_kernel(None)
if len(opt.edges()) < 10:
Printer.red('poseOptimization: stopped - not enough edges!')
is_ok = False
break
print('pose optimization: initial ', num_point_edges, ' points, found ',
num_bad_points, ' bad points')
if num_point_edges == num_bad_points:
Printer.red(
'poseOptimization: all the initial correspondences are bad!')
is_ok = False
# update pose estimation
if is_ok is True:
est = v_se3.estimate()
R = est.rotation().matrix()
t = est.translation()
frame.pose = poseRt(R, t)
num_valid_points = num_point_edges - num_bad_points
return opt.active_chi2(), is_ok, num_valid_points
def optimization(frames,
points,
local_window,
fixed_points=False,
verbose=False,
rounds=40,
use_robust_kernel=False):
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)
thHuberMono = math.sqrt(5.991)
# chi-square 2 DOFS
graph_frames, graph_points = {}, {}
# add frame vertices to graph
for f in (
local_frames if fixed_points else frames
): # if points are fixed then consider just the local frames, otherwise we need all frames or at least two frames for each point
#print('adding vertex frame ', f.id, ' to graph')
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) # even ids
v_se3.set_fixed(f.id < 1 or f not in local_frames)
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 point vertices to graph
for p in points:
if p.is_bad and not fixed_points:
continue
if not any([f in local_frames for f in p.frames]):
continue
#print('adding vertex point ', p.id,' to graph')
v_p = g2o.VertexSBAPointXYZ()
v_p.set_id(p.id * 2 + 1) # odd ids
v_p.set_estimate(p.pt[0:3])
v_p.set_marginalized(True)
v_p.set_fixed(fixed_points)
opt.add_vertex(v_p)
graph_points[p] = v_p
# add edges
for f, idx in zip(p.frames, p.idxs):
if f not in graph_frames:
continue
#print('adding edge between point ', p.id,' and frame ', f.id)
edge = g2o.EdgeSE3ProjectXYZ()
edge.set_vertex(0, v_p)
edge.set_vertex(1, graph_frames[f])
edge.set_measurement(f.kpsu[idx])
invSigma2 = Frame.detector.inv_level_sigmas2[f.octaves[idx]]
edge.set_information(np.eye(2) * invSigma2)
if use_robust_kernel:
edge.set_robust_kernel(g2o.RobustKernelHuber(thHuberMono))
edge.fx = f.fx
edge.fy = f.fy
edge.cx = f.cx
edge.cy = f.cy
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 fixed_points:
for p in graph_points:
p.pt = np.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
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 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 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 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 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(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 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)))
markerIDset = False
unique_list = []
bag_num += 1
bag.close()
msg_point = geometry_msgs.msg.PoseStamped()
trans = geometry_msgs.msg.PoseStamped()
# Create nodes in graph for sensors
print("Generate graph for optimization...")
for snode in sensor_list:
spose = snode.pose
add_frame(t, "s" + str(snode.id), origin_frame, spose)
pose = g2o.SE3Quat(
g2o.Quaternion(spose['qw'], spose['qx'], spose['qy'], spose['qz']),
np.array([spose['x'], spose['y'], spose['z']]))
optimizer.add_vertex(snode.id, pose.Isometry3d(),
snode.id is fixed_sensor)
# Create nodes in graph for tracking points (transformed in origin frame)
assocs = []
graph_id = sensor_num + 1
for tnode in tracking_points:
if tnode.sensor_id is fixed_sensor:
tpose = tnode.pose
add_frame(t, "t" + str(tnode.tracker_id),
"s" + str(tnode.sensor_id), tpose)
pose = transform_to_SE3Quat(t, "s" + str(tnode.sensor_id),
origin_frame, tpose)
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 transform_to_SE3Quat(transformer, cur_frame, target_frame, pose):
(tpose, tquat) = transform_frame(transformer, cur_frame, target_frame,
pose)
return g2o.SE3Quat(g2o.Quaternion(tquat.w, tquat.x, tquat.y, tquat.z),
np.array([tpose.x, tpose.y, tpose.z]))
