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