def main(): #solver = g2o.BlockSolverX(g2o.LinearSolverCholmodX()) solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3()) solver = g2o.OptimizationAlgorithmLevenberg(solver) optimizer = g2o.SparseOptimizer() optimizer.set_verbose(True) optimizer.set_algorithm(solver) optimizer.load(args.input) print('num vertices:', len(optimizer.vertices())) print('num edges:', len(optimizer.edges()), end='\n\n') optimizer.initialize_optimization() optimizer.optimize(args.max_iterations) if len(args.output) > 0: optimizer.save(args.output)
def main(): optimizer = g2o.SparseOptimizer() solver = g2o.BlockSolverX(g2o.LinearSolverCSparseX()) solver = g2o.OptimizationAlgorithmLevenberg(solver) optimizer.set_algorithm(solver) true_points = np.hstack([ np.random.random((1000, 1)) * 3 - 1.5, np.random.random((1000, 1)) - 0.5, np.random.random((1000, 1)) + 10 ]) focal_length = (500, 500) principal_point = (320, 240) baseline = 0.075 g2o.VertexSCam.set_cam(*focal_length, *principal_point, baseline) for i in range(2): t = np.array([0, 0, i]) cam = g2o.Isometry3d(np.identity(3), t) vc = g2o.VertexSCam() vc.set_id(i) vc.set_estimate(cam) if i == 0: vc.set_fixed(True) vc.set_all() optimizer.add_vertex(vc) trans0 = optimizer.vertex(0).estimate().inverse() trans1 = optimizer.vertex(1).estimate().inverse() for i in range(len(true_points)): pt0 = trans0 * true_points[i] pt1 = trans1 * true_points[i] # add noise pt0 += np.random.randn(3) * args.pos_noise pt1 += np.random.randn(3) * args.pos_noise # form edge, with normals in varioius positions nm0 = np.array([0, i, 1]) nm0 = nm0 / np.linalg.norm(nm0) nm1 = np.array([0, i, 1]) nm1 = nm1 / np.linalg.norm(nm1) meas = g2o.EdgeGICP() meas.pos0 = pt0 meas.pos1 = pt1 meas.normal0 = nm0 meas.normal1 = nm1 edge = g2o.Edge_V_V_GICP() edge.set_vertex(0, optimizer.vertex(0)) edge.set_vertex(1, optimizer.vertex(1)) edge.set_measurement(meas) edge.set_information(meas.prec0(0.01)) optimizer.add_edge(edge) # set up SBA projections if args.num_points > 0: true_points = np.hstack([ np.random.random((args.num_points, 1)) * 3 - 1.5, np.random.random((args.num_points, 1)) - 0.5, np.random.random((args.num_points, 1)) + 10 ]) cam_num = 2 for i, point in enumerate(true_points): vp = g2o.VertexSBAPointXYZ() vp.set_id(cam_num + i) vp.set_marginalized(True) vp.set_estimate(point + np.random.randn(3)) optimizer.add_vertex(vp) for j in range(cam_num): z = optimizer.vertex(j).map_point(point) if 0 <= z[0] < 640 and 0 <= z[1] < 480: z += np.random.randn(3) * args.pixel_noise * [ 1, 1, 1 / 16. ] 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(3)) edge.set_robust_kernel(g2o.RobustKernelHuber()) optimizer.add_edge(edge) # move second cam off of its true position vc = optimizer.vertex(1) cam = g2o.Isometry3d(vc.estimate().R, np.array([-0.1, -0.1, 0.2])) vc.set_estimate(cam) optimizer.initialize_optimization() optimizer.compute_active_errors() print('Initial chi2 =', optimizer.chi2()) optimizer.set_verbose(True) optimizer.optimize(20) print('\nSecond vertex should be near [0, 0, 1]') print('before optimization:', cam.t) print('after optimization:', optimizer.vertex(1).estimate().t) print('error:', optimizer.vertex(1).estimate().t - [0, 0, 1]) '''
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_map(self, postfix = ""): optimizer = g2o.SparseOptimizer() solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3()) solver = g2o.OptimizationAlgorithmLevenberg(solver) optimizer.set_algorithm(solver) # Define camera parameters print(self.camera_matrix) #focal_length = 1000 focal_length = self.camera_matrix[0, 0] #principal_point = (320, 240) principal_point = (self.camera_matrix[0, 2], self.camera_matrix[1, 2]) baseline = 0 cam = g2o.CameraParameters(focal_length, principal_point, baseline) cam.set_id(0) optimizer.add_parameter(cam) camera_vertices = {} for camera in self.cameras: # Use the estimated pose of the second camera based on the # essential matrix. pose = g2o.SE3Quat(camera.R, camera.t) # Set the poses that should be optimized. # Define their initial value to be the true pose # keep in mind that there is added noise to the observations afterwards. v_se3 = g2o.VertexSE3Expmap() v_se3.set_id(camera.camera_id) v_se3.set_estimate(pose) v_se3.set_fixed(camera.fixed) optimizer.add_vertex(v_se3) camera_vertices[camera.camera_id] = v_se3 #print("camera id: %d" % camera.camera_id) point_vertices = {} for point in self.points: # Add 3d location of point to the graph vp = g2o.VertexPointXYZ() vp.set_id(point.point_id) vp.set_marginalized(True) # Use positions of 3D points from the triangulation point_temp = np.array(point.point, dtype=np.float64) vp.set_estimate(point_temp) optimizer.add_vertex(vp) point_vertices[point.point_id]= vp for observation in self.observations: # Add edge from first camera to the point edge = g2o.EdgeProjectXYZ2UV() # 3D point edge.set_vertex(0, point_vertices[observation.point_id]) # Pose of first camera edge.set_vertex(1, camera_vertices[observation.camera_id]) edge.set_measurement(observation.image_coordinates) edge.set_information(np.identity(2)) edge.set_robust_kernel(g2o.RobustKernelHuber()) edge.set_parameter_id(0, 0) optimizer.add_edge(edge) 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(40) optimizer.save("test.g2o"); for idx, camera in enumerate(self.cameras): t = camera_vertices[camera.camera_id].estimate().translation() self.cameras[idx].t = t q = camera_vertices[camera.camera_id].estimate().rotation() self.cameras[idx].R = quarternion_to_rotation_matrix(q) for idx, point in enumerate(self.points): p = point_vertices[point.point_id].estimate() # It is important to copy the point estimates. # Otherwise I end up with some memory issues. # self.points[idx].point = p self.points[idx].point = np.copy(p)
def optimize(frames, points, local_window, fix_points, verbose=False, rounds=100): if local_window is None: local_frames = frames else: local_frames = frames[-local_window:] opt = g2opy.SparseOptimizer() solver = g2opy.BlockSolverSE3(g2opy.LinearSolverCSparseSE3()) solver = g2opy.OptimizationAlgorithmLevenberg(solver) opt.set_algorithim(solver) cam = g2opy.CameraParameters(1.0, (0.0, 0.0), 0) cam.set_id(0) opt.add_paramater(cam) robust_kernel = g2opy.RobustKernelHuber(nq.sqrt(5.992)) graph_frames, graph_points = {}, {} for f in (local_frames if fix_points else frames): pose = f.pose se3 = g2opy.SEQQuat(pose[0:3, 0:3], pose[0:3, 3]) v_se3 = g2opy.VertexSE3Expmap() v_se3.set_estimate(se3) v_se3.set_id(f.id * 4) v_se3.set_fixed(f.id <= 1 or f not in local_frames) opt.add_vertex(v_se3) est = v_se3.estimate(se3) assert nq.allclose(pose[0:3, 0:3], est.rotation().matrix()) assert nq.allclose(pose[0:3, 3], est.translation()) graph_frames[f] = v_se3 for p in points: if not any([f in local_frames for f in p.frames]): continue pt = g2opy.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 for f, idx in zip(p.frames, p.idxs): if f not in graph_frames: continue edge = g2opy.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(nq.eye(2)) edge.set_robust_kernel(robust_kernel) opt.add_edge(edge) if verbose: opt.set_verbose(True) opt.initialize_optimization() opt.optimize(rounds) for f in graph_frames: est = graph_frames[f].estimate() R = est.rotation().matrix().matrix() t = est.translation() f.pose = poseRt(R, t) if not fix_points: for p in graph_points: p.pt = nq.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 g2o.VertexSCam.set_cam(*focal_length, *principal_point, baseline) true_poses = [] num_pose = 5 for i in range(num_pose): # pose here transform points from world coordinates to camera coordinates pose = g2o.Isometry3d(np.identity(3), [i * 0.04 - 1, 0, 0]) true_poses.append(pose) v_se3 = g2o.VertexSCam() v_se3.set_id(i) v_se3.set_estimate(pose) if i < 2: v_se3.set_fixed(True) v_se3.set_all() optimizer.add_vertex(v_se3) point_id = num_pose inliers = dict() sse = defaultdict(float) for i, point in enumerate(true_points): visible = [] for j in range(num_pose): z = optimizer.vertex(j).map_point(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(3) * args.pixel_noise * [1, 1, 1 / 16.] 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(3)) 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('\nRMSE (inliers only):') print('before optimization:', np.sqrt(sse[0] / len(inliers))) print('after optimization:', np.sqrt(sse[1] / len(inliers)))
def main(): optimizer = g2o.SparseOptimizer() if args.schur_trick: solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3()) else: solver = g2o.BlockSolverX(g2o.LinearSolverEigenX()) # slower solver = g2o.OptimizationAlgorithmLevenberg(solver) optimizer.set_algorithm(solver) true_points = np.hstack([ np.random.random((500, 1)) * 3 - 1.5, np.random.random((500, 1)) - 0.5, np.random.random((500, 1)) + 3]) focal_length = 1000. principal_point = (320, 240) cam = g2o.CameraParameters(focal_length, principal_point, 0) cam.set_id(0) optimizer.add_parameter(cam) true_poses = [] num_pose = 15 for i in range(num_pose): # pose here means transform points from world coordinates to camera coordinates pose = g2o.SE3Quat(np.identity(3), [i*0.04-1, 0, 0]) true_poses.append(pose) v_se3 = g2o.VertexSE3Expmap() v_se3.set_id(i) v_se3.set_estimate(pose) if i < 2: v_se3.set_fixed(True) optimizer.add_vertex(v_se3) point_id = num_pose inliers = dict() sse = defaultdict(float) for i, point in enumerate(true_points): visible = [] for j, pose in enumerate(true_poses): z = cam.cam_map(pose * point) if 0 <= z[0] < 640 and 0 <= z[1] < 480: visible.append((j, z)) if len(visible) < 2: continue v_p = g2o.VertexSBAPointXYZ() v_p.set_id(point_id) v_p.set_marginalized(args.schur_trick) anchor = visible[0][0] point2 = true_poses[anchor] * (point + np.random.randn(3)) if point2[2] == 0: continue v_p.set_estimate(invert_depth(point2)) optimizer.add_vertex(v_p) inlier = True for j, z in visible: if np.random.random() < args.outlier_ratio: inlier = False z = np.random.random(2) * [640, 480] z += np.random.randn(2) * args.pixel_noise edge = g2o.EdgeProjectPSI2UV() edge.resize(3) edge.set_vertex(0, v_p) edge.set_vertex(1, optimizer.vertex(j)) edge.set_vertex(2, optimizer.vertex(anchor)) edge.set_measurement(z) edge.set_information(np.identity(2)) if args.robust_kernel: edge.set_robust_kernel(g2o.RobustKernelHuber()) edge.set_parameter_id(0, 0) optimizer.add_edge(edge) if inlier: inliers[point_id] = (i, anchor) error = (true_poses[anchor].inverse() * invert_depth(v_p.estimate()) - true_points[i]) sse[0] += np.sum(error**2) point_id += 1 print('Performing full BA:') optimizer.initialize_optimization() optimizer.set_verbose(True) optimizer.optimize(10) for i in inliers: v_p = optimizer.vertex(i) v_anchor = optimizer.vertex(inliers[i][1]) error = (v_anchor.estimate().inverse() * invert_depth(v_p.estimate()) - true_points[inliers[i][0]]) sse[1] += np.sum(error**2) print('\nRMSE (inliers only):') print('before optimization:', np.sqrt(sse[0] / len(inliers))) print('after optimization:', np.sqrt(sse[1] / len(inliers)))
def main(): optimizer = g2o.SparseOptimizer() solver = g2o.BlockSolverX(g2o.LinearSolverDenseX()) algorithm = g2o.OptimizationAlgorithmLevenberg(solver) optimizer.set_algorithm(algorithm) true_points = np.hstack([ np.random.random((1000, 1)) * 3 - 1.5, np.random.random((1000, 1)) - 0.5, np.random.random((1000, 1)) + 10]) for i in range(2): t = np.array([0, 0, i]) cam = g2o.Isometry3d(np.identity(3), t) vc = g2o.VertexSE3() vc.set_id(i) vc.set_estimate(cam) if i == 0: vc.set_fixed(True) optimizer.add_vertex(vc) trans0 = optimizer.vertex(0).estimate().inverse() trans1 = optimizer.vertex(1).estimate().inverse() # set up point matches for i in range(len(true_points)): # calculate the relative 3d position of the point pt0 = trans0 * true_points[i] pt1 = trans1 * true_points[i] # add noise pt0 += np.random.randn(3) * args.noise pt1 += np.random.randn(3) * args.noise # form edge, with normals in varioius positions nm0 = np.array([0, i, 1]) nm0 = nm0 / np.linalg.norm(nm0) nm1 = np.array([0, i, 1]) nm1 = nm1 / np.linalg.norm(nm1) meas = g2o.EdgeGICP() meas.pos0 = pt0 meas.pos1 = pt1 meas.normal0 = nm0 meas.normal1 = nm1 edge = g2o.Edge_V_V_GICP() edge.set_vertex(0, optimizer.vertex(0)) edge.set_vertex(1, optimizer.vertex(1)) edge.set_measurement(meas) edge.set_information(meas.prec0(0.01)) optimizer.add_edge(edge) # move second cam off of its true position vc = optimizer.vertex(1) cam = g2o.Isometry3d(vc.estimate().R, np.array([0, 0, 0.2])) vc.set_estimate(cam) optimizer.initialize_optimization() optimizer.compute_active_errors() print('Initial chi2 =', optimizer.chi2()) optimizer.save('gicp.g2o') optimizer.set_verbose(True) optimizer.optimize(5) print('\nSecond vertex should be near [0, 0, 1]') print('before optimization:', cam.t) print('after optimization:', optimizer.vertex(1).estimate().t)