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]))