def __init__(self): # inherits traits from g2o's Sparse Optimizer class super().__init__() # intitialize solver and optimizer solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3()) solver = g2o.OptimizationAlgorithmLevenberg(solver) super().set_algorithm(solver)
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 __init__(self): self.optimizer = g2o.SparseOptimizer() # self.solver = g2o.BlockSolverX(g2o.LinearSolverDenseX()) # self.solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3()) self.solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3()) self.algorithm = g2o.OptimizationAlgorithmLevenberg(self.solver) self.optimizer.set_algorithm(self.algorithm) self.already_initialized = False self.last_lost = 0 self.lock = ControlableLock(True)
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 oprimizer(self): ## Init optimizer optimizer = g2o.SparseOptimizer() solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3()) solver = g2o.OptimizationAlgorithmLevenberg(solver) optimizer.set_algorithm(solver) ## add frames to graph for f in self.frames: v_se3 = g2o.VertexSE3() v_se3.set_id(f.id) v_se3.set_estimate(f.pose) v_se3.set_fixed(f.id == 0) optimizer.add_vertex(v_se3)
def __init__(self): super().__init__() # g2o::BlockSlover_6_3(g2o::BlockSolver_6_3::LinearSolverType*) linear_solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3()) solver = g2o.OptimizationAlgorithmLevenberg(linear_solver) super().set_algorithm(solver) # additional parameters terminate = g2o.SparseOptimizerTerminateAction() terminate.set_gain_threshold(1e-6) super().add_post_iteration_action(terminate) # self._map = None # self.frame = None # self.points = None # self.vSE3 = None # self.measurements = None # self.vertex_seq_generator = AtomicCounter() self.edge_seq_generator = AtomicCounter() # Point | Frame | Landmark -> Vertex mapping # inverse vertex query self._ivq = {} # (Vertex, Vetex) -> Edge mapping, a sparse matrix # inverse edges query self._ieq = {} # self.USE_LANDMARKS = False # self.edges = []
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(pose) 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.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) # add points to frames(pose) 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 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 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 main(): optimizer = g2o.SparseOptimizer() solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3()) solver = g2o.OptimizationAlgorithmLevenberg(solver) optimizer.set_algorithm(solver) focal_length = 1000 principal_point = (320, 240) cam = g2o.CameraParameters(focal_length, principal_point, 0) cam.set_id(0) optimizer.add_parameter(cam) true_points = np.hstack([ np.random.random((500, 1)) * 3 - 1.5, np.random.random((500, 1)) - 0.5, np.random.random((500, 1)) + 3 ]) true_poses = [] num_pose = 15 for i in range(num_pose): # pose here means transform points from world coordinates to camera coordinates pose = g2o.SE3Quat(np.identity(3), [i * 0.04 - 1, 0, 0]) true_poses.append(pose) v_se3 = g2o.VertexSE3Expmap() v_se3.set_id(i) v_se3.set_estimate(pose) if i < 2: v_se3.set_fixed(True) optimizer.add_vertex(v_se3) point_id = num_pose inliers = dict() sse = defaultdict(float) for i, point in enumerate(true_points): visible = [] for j, pose in enumerate(true_poses): z = cam.cam_map(pose * point) if 0 <= z[0] < 640 and 0 <= z[1] < 480: visible.append((j, z)) if len(visible) < 2: continue vp = g2o.VertexSBAPointXYZ() vp.set_id(point_id) vp.set_marginalized(True) vp.set_estimate(point + np.random.randn(3)) optimizer.add_vertex(vp) inlier = True for j, z in visible: if np.random.random() < args.outlier_ratio: inlier = False z = np.random.random(2) * [640, 480] z += np.random.randn(2) * args.pixel_noise edge = g2o.EdgeProjectXYZ2UV() edge.set_vertex(0, vp) edge.set_vertex(1, optimizer.vertex(j)) edge.set_measurement(z) edge.set_information(np.identity(2)) if args.robust_kernel: edge.set_robust_kernel(g2o.RobustKernelHuber()) edge.set_parameter_id(0, 0) optimizer.add_edge(edge) if inlier: inliers[point_id] = i error = vp.estimate() - true_points[i] sse[0] += np.sum(error**2) point_id += 1 print('num vertices:', len(optimizer.vertices())) print('num edges:', len(optimizer.edges())) print('Performing full BA:') optimizer.initialize_optimization() optimizer.set_verbose(True) optimizer.optimize(10) for i in inliers: print(i) vp = optimizer.vertex(i) error = vp.estimate() - true_points[inliers[i]] sse[1] += np.sum(error**2) print('\nRMSE (inliers only):') print('before optimization:', np.sqrt(sse[0] / len(inliers))) print('after optimization:', np.sqrt(sse[1] / len(inliers)))
def optimize(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 __init__(self): self._optimizer = g2o.SparseOptimizer() block_solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3()) optimization_algorithm = g2o.OptimizationAlgorithmLevenberg( block_solver) self._optimizer.set_algorithm(optimization_algorithm)
def __init__(self): super().__init__() solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3()) solver = g2o.OptimizationAlgorithmLevenberg(solver) super().set_algorithm(solver)
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 optimize(self, local_window = LOCAL_WINDOW, fix_points = False, verbose = False): # create the g2o optimizer optimizer = g2o.SparseOptimizer() graph_solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3()) graph_solver = g2o.OptimizationAlgorithmLevenberg(graph_solver) optimizer.set_algorithm(graph_solver) robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991)) if local_window is None: local_frames = self.frames else: local_frames = self.frames[-local_window:] # add frames to the graph for f in self.frames: pose = np.linalg.inv(f.pose) sbacam = g2o.SBACam(g2o.SE3Quat(pose[0:3, 0:3], pose[0:3, 3])) sbacam.set_cam(f.k[0][0], f.k[1][1], f.k[0][2], f.k[1][2], 1.0) v_se3 = g2o.VertexCam() v_se3.set_id(f.id) v_se3.set_estimate(sbacam) v_se3.set_fixed(f.id <= 1 or f not in local_frames) optimizer.add_vertex(v_se3) # add points to the frames point_id_offset = 0x10000 for p in self.points: if not any([f in local_frames for f in p.frames]): continue pt = g2o.VertexSBAPointXYZ() pt.set_id(p.id + point_id_offset) pt.set_estimate(p.point[0:3]) pt.set_marginalized(True) pt.set_fixed(fix_points) optimizer.add_vertex(pt) for f in p.frames: edge = g2o.EdgeProjectP2MC() edge.set_vertex(0, pt) edge.set_vertex(1, optimizer.vertex(f.id)) uv = f._kps[f.pts.index(p)] edge.set_measurement(uv) edge.set_information(np.eye(2)) edge.set_robust_kernel(robust_kernel) optimizer.add_edge(edge) if verbose: optimizer.set_verbose(True) optimizer.initialize_optimization() optimizer.optimize(20) # put the frames back for f in self.frames: estimate = optimizer.vertex(f.id).estimate() r = estimate.rotation().matrix() t = estimate.translation() f.pose = np.linalg.inv(pose_rt(r, t)) # put points back if not fix_points: new_points = [] for p in self.points: vert = optimizer.vertex(p.id + point_id_offset) if vert is None: new_points.append(p) continue estimate = vert.estimate() old_point = len(p.frames) <= 2 and p.frames[-1] not in local_frames # try and computer the reprojection error of the point errors = [] for f in p.frames:g uv = f._kps[f.pts.index(p)] proj = np.dot(np.dot(f.k, f.pose[:3]), np.array([estimate[0], estimate[1], estimate[2], 1.0])) proj = proj[0:2]/proj[2] errors.append(np.linalg.norm(proj-uv)) # culling if old_point or np.mean(errors) > 5: p.delete_point() continue p.point = np.array(estimate) new_points.append(p) print("Culled points: %d" %(len(self.points) - len(new_points))) self.points = new_points return optimizer.active_chi2()
def optimize(frames, points, local_window, fix_points, verbose=False, rounds=50): # print("Entering Optimizer") if local_window is None: local_frames = frames else: local_frames = frames[-local_window:] # create g2o optimizer opt = g2o.SparseOptimizer() solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3()) solver = g2o.OptimizationAlgorithmLevenberg(solver) opt.set_algorithm(solver) # add normalized camera cam = g2o.CameraParameters(1.0, (0.0, 0.0), 0) # cam = g2o.CameraParameters(718.8560, (607.1928, 185.2157), 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]) f.SE3 = se3 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.normalized_kps[idx]) edge.set_information(np.eye(2)) edge.set_robust_kernel(robust_kernel) opt.add_edge(edge) # print(cam.cam_map(f.SE3 * p.pt), f.normalized_kps[idx]) # print('num vertices:', len(opt.vertices())) # print('num edges:', len(opt.edges())) 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()