def predict_pose(self, timestamp, prev_position=None, prev_orientation=None): ''' Predict the next camera pose. ''' if prev_position is not None: self.position = prev_position if prev_orientation is not None: self.orientation = prev_orientation if not self.initialized: return (g2o.Isometry3d(self.orientation, self.position), self.covariance) dt = timestamp - self.timestamp delta_angle = g2o.AngleAxis(self.v_angular_angle * dt * self.damp, self.v_angular_axis) delta_orientation = g2o.Quaternion(delta_angle) orientation = delta_orientation * self.orientation position = self.position + delta_orientation * (self.v_linear * dt * self.damp) return (g2o.Isometry3d(orientation, position), self.covariance)
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 __init__(self, timestamp=None, initial_position=None, initial_orientation=None, initial_covariance=None): super().__init__(timestamp, initial_position, initial_orientation, initial_covariance) self.delta_position = np.zeros(3) # delta translation self.delta_orientation = g2o.Quaternion()
def __init__(self, params): self.params = params self.timestamp = None self.position = np.zeros(3) self.orientation = g2o.Quaternion() self.covariance = None # pose covariance self.v_linear = np.zeros(3) # linear velocity self.v_angular_angle = 0 self.v_angular_axis = np.array([1, 0, 0]) self.initialized = False self.damp = 0.95 # damping factor
def process(self, imu_msg): while self.is_measuring: time.sleep(1e-4) self.is_processing = True timestamp, data = imu_msg state_new = ESKFState(self.config) state_new.wm = data[0] state_new.am = data[1] state_new.timestamp = timestamp if len(self.state_buffer) == 0: self.initialize(state_new) return # ======================== # === ESKF Propagation === # ======================== state_old = self.state_buffer[-1] dt = state_new.timestamp - state_old.timestamp if dt < 0: return if dt >= self.config.imu_max_reading_time: print('[Warning] The interval between readings is too big') raise RuntimeError # self.initialize(state_new, True) # return if np.linalg.norm(state_new.am) >= self.config.imu_max_reading_accel: print('[Warning] The accelerometer readings is too big') state_new.am = state_old.am if np.linalg.norm(state_new.wm) >= self.config.imu_max_reading_gyro: print('[Warning] The gyroscope readings is too big') state_new.wm = state_old.wm self.propagate(state_old, state_new) self.state_buffer.append(state_new) self.shrink_buffer() print(state_new.timestamp, '\n p ', state_new.p, state_new.p-self.state_buffer[0].p, '\n q ', state_new.q, '\n ba ', state_new.ba, '\n bw ', state_new.bw) print(' v ', state_new.v) self.current_pose = g2o.Isometry3d( g2o.Quaternion(state_new.q), state_new.p) self.is_processing = False
def add_edge(self, v0,v1, measurement=None, information=np.identity(6), robust_kernel = None): #95% CI edge = g2o.EdgeSE3() # edge = g2o.EdgeProjectXYZ2UV() edge.set_vertex(0, v0) edge.set_vertex(1, v1) if measurement is None: measurement = g2o.Isometry3d(g2o.Quaternion(0,0,0,1),np.array([0,1,0])) edge.set_measurement(measurement) # relative pose edge.set_information(information) # edge.set_parameter_id(0, 0) robust_kernel=g2o.RobustKernelHuber(np.sqrt(5.991))#5.991 if robust_kernel is not None: edge.set_robust_kernel(robust_kernel) super().add_edge(edge)
def __init__(self, timestamp=None, initial_position=np.zeros(3), initial_orientation=g2o.Quaternion(), initial_covariance=None): self.timestamp = timestamp self.position = initial_position self.orientation = initial_orientation self.covariance = initial_covariance # pose covariance self.v_linear = np.zeros(3) # linear velocity self.v_angular_angle = 0 self.v_angular_axis = np.array([1, 0, 0]) self.initialized = False # damping factor self.damp = 0.95
def predict_pose(self, timestamp): ''' Predict the next camera pose. ''' if not self.initialized: return (g2o.Isometry3d(self.orientation, self.position), self.covariance) dt = timestamp - self.timestamp # Use quaternion instead of rotation matrix due to performance delta_angle = g2o.AngleAxis(self.v_angular_angle * dt * self.damp, self.v_angular_axis) delta_orientation = g2o.Quaternion(delta_angle) position = self.position + self.v_linear * dt * self.damp orientation = self.orientation * delta_orientation return (g2o.Isometry3d(orientation, position), self.covariance)
def optimize(self, max_iterations=20): optimizer = G2OPoseGraphOptimizer() id_to_name = {} name_to_id = {} # populate optimizer with self._lock: # add vertices for nname, ndata in self.nodes.items(): # compute node ID node_id = len(id_to_name) id_to_name[node_id] = nname name_to_id[nname] = node_id # get node pose and other attributes pose = g2o.Isometry3d(g2o.Quaternion(ndata["pose"].Q('wxyz')), ndata["pose"].t) \ if "pose" in ndata else None fixed = ndata["fixed"] if "fixed" in ndata else False # add vertex optimizer.add_vertex(node_id, pose=pose, fixed=fixed) # add edges for u, v, edata in self.edges.data(): # get nodes IDs iu = name_to_id[u] iv = name_to_id[v] # get edge measurement and other attributes measurement = edata["measurement"] T = tr.compose_matrix( translate=measurement.t, angles=tr.euler_from_quaternion(measurement.q) ) # add edge optimizer.add_edge([iu, iv], g2o.Isometry3d(T)) # optimize optimizer.optimize(max_iterations=max_iterations) # update graph with self._lock: for nid, nname in id_to_name.items(): npose = optimizer.get_pose(nid) T = tr.compose_matrix( translate=npose.t, angles=tr.euler_from_matrix(npose.R) ) q = tr.quaternion_from_matrix(T) self.nodes[nname]["pose"] = TF(t=npose.t, q=q)
def __init__(self, timestamp=None, initial_position=None, initial_orientation=None, initial_covariance=None): self.timestamp = timestamp if initial_position is not None: self.position = initial_position else: self.position = np.zeros(3) if initial_orientation is not None: self.orientation = initial_orientation else: self.orientation = g2o.Quaternion() self.orientation = initial_orientation self.covariance = initial_covariance # pose covariance self.is_ok = False self.initialized = False
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]))
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 measure(self, vo_msg): timestamp_from, timestamp_to, vo_T = vo_msg assert timestamp_from < timestamp_to # if timestamp_from < self.state_buffer[0].timestamp + 3: # test # return vo_q = vo_T.rotation() vo_t = vo_T.translation() tic = time.time() while (self.is_processing or len(self.state_buffer) < 2 or self.state_buffer[-1].timestamp < timestamp_to): time.sleep(1e-4) if time.time() - tic > 1: print('[Update] Something went wrong with IMU !!') self.is_measuring = False return self.is_measuring = True if self.state_buffer[0].timestamp > timestamp_from: print('[Update] Keyframe time is older than the first state,' 'drop this update') self.is_measuring = False return if self.state_buffer[-1].timestamp - timestamp_to > self.config.max_vo_imu_delay: print('[Update] Delay between imu and vo is too large, drop this update') self.is_measuring = False return state_from = self.get_closest_state(timestamp_from) state_to = self.get_closest_state(timestamp_to) if state_from is None or state_to is None: print('[Update] Nearest state not found') self.is_measuring = False return from_q = quaternion(state_from.q) to_q = quaternion(state_to.q) odom_q = self.cam2imu_q * vo_q * self.cam2imu_q.conjugate() odom_q.normalize() odom_t = self.cam2imu_q * vo_t + self.cam2imu_t meas_q = from_q * odom_q meas_q.normalize() meas_p = from_q * odom_t + state_from.p R = np.identity(6) R[:3, :3] = self.config.vo_fixedcov_p * np.identity(3) R[3:, 3:] = self.config.vo_fixedcov_q * np.identity(3) # ======================== # === Update procedure === # ======================== # ## Step1: Calculate the residual residual_p = meas_p - state_to.p residual_q = meas_q * to_q.conjugate() residual_th = 2 * residual_q.vec() / residual_q.w() # q ~= [1, 0.5*theta] residual = np.concatenate([residual_p, residual_th]) # ## Step2: Compute the Innovation, Kalman gain and Correction state S = self.H @ state_to.P @ self.H.T + R # (6, 6) S_inv = np.linalg.inv(S) K = state_to.P @ self.H.T @ S_inv # (15, 6) update = K.dot(residual) # ##Step3: Update the state state_to.p += update[0:3] state_to.v += update[3:6] state_to.ba += update[9:12] state_to.bw += update[12:15] q_update = np.array([1, *(0.5 * update[6:9])]) state_to.q = quatmat(state_to.q).dot(q_update) state_to.q /= np.linalg.norm(state_to.q) # covariance: P k+1|k+1 = (I d −KH)P k+1|k (I d −KH) T +KRK T KH = np.identity(15) - K @ self.H # (15, 15) state_to.P = KH @ state_to.P @ KH.T + K @ R @ K.T # Make sure P stays symmetric state_to.P = (state_to.P + state_to.P.T) / 2. # ##Step4: Repropagate State & Covariance assert state_to in self.state_buffer idx = self.state_buffer.index(state_to) if idx < len(self.state_buffer) - 1: for i in range(idx, len(self.state_buffer) - 1): self.propagate(self.state_buffer[i], self.state_buffer[i+1]) self.current_pose = g2o.Isometry3d( g2o.Quaternion(self.state_buffer[-1].q), self.state_buffer[-1].p) self.is_measuring = False
pose_graph = PoseGraphOptimization() pose_file = open('sphere.g2o') line = pose_file.readline() while line: data = line.split(' ') if (data[0] == 'VERTEX_SE3:QUAT'): pose_id = int(data[1]) pose_info = np.array([float(i) for i in data[2:9]]) # print(pose_id,pose_info) q = g2o.Quaternion(pose_info[6], pose_info[3], pose_info[4], pose_info[5]) t = g2o.Isometry3d(q, pose_info[0:3]) pose_graph.add_vertex(pose_id, t) if (data[0] == 'EDGE_SE3:QUAT'): pose_id_left = int(data[1]) pose_id_righ = int(data[2]) pose_info = np.array([float(i) for i in data[3:10]]) q = g2o.Quaternion(pose_info[6], pose_info[3], pose_info[4], pose_info[5]) t = g2o.Isometry3d(q, pose_info[0:3]) pose_graph.add_edge([pose_id_left, pose_id_righ], t)
def main(): # camera matrix fx = 3551.342810 fy = 3522.689669 cx = 2033.513326 cy = 1455.489194 # fx = 718.8560 # fy = 718.8560 # cx = 607.1928 # cy = 185.2157 K = np.float64([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) D = np.float64([-0.276796, 0.113400, -0.000349, -0.000469]) #load images # dataset1_dir = '/home/linjian/datasets/Data_trajectory/2018-08-21/22_47_20_load/' # dataset2_dir = '/home/linjian/datasets/Data_trajectory/2018-08-22/' # dataset1_dir ='/home/linjian/dataset/docking_dataset/image/Data_trajectory/2018-08-21/22_47_20_load/' # dataset1_dir = '/home/linjian/dataset/docking_dataset/image/Data_trajectory/2018-08-22/16h-26m-42s load/' # dataset1_dir = '/home/linjian/dataset/docking_dataset/image/raw_data/2018-08-22/17h-21m-57s load/' # dataset1_dir = '/home/linjian/dataset/docking_dataset/image/raw_data/2018-08-22/17h-24m-21s unload/' # dataset1_dir = '/home/linjian/dataset/tum_image/sequence_30/resized_images/' dataset1_dir = '/home/linjian/dataset/docking_dataset/image/Data_trajectory/load_unload/' filelist1 = glob.glob(dataset1_dir + '*.jpg') # filelist1 = sorted(filelist1) filelist1.sort(key=lambda f: float(re.findall(r'(\d+\.\d)', f)[0])) print(filelist1) img_num = len(filelist1) #load scale as speed of the wheel # loaded_data = load_data(dataset1_dir) # scale = loaded_data.get_speed() #initialization #poses rotation_array = [] transformation_array = [] pose_array = [] R = np.eye(3) t = np.zeros((1, 3)) rotation_array.append(R) transformation_array.append(t) pose_array.append(t) #optimization initialization optimization_class = PoseGraphOptimization() #let first vertex fixed pose = g2o.Isometry3d(g2o.Quaternion(0, 0, 0, 1), np.transpose(pose_array[0])) optimization_class.add_vertex(0, pose, True) #bag of virtual words init detector = cv2.ORB_create() bovw_class = bovw(detector) #init loop closure class # loopclosure_class = loopclosure() loopclosure_pairs = [] max_num_lc = 50 #init keypoints and descriptors keypoints_list = [] descriptors_list = [] #keyframe init keyframe_file_list = [] keyframe_file_list.append(filelist1[0]) keyframe_number = 1 frame_skipped = 0 frame_skipped_floor = 5 frame_skipped_ceil = 20 #initialize input images img1 = cv2.imread(filelist1[0]) img2 = cv2.imread(filelist1[1]) keyframe_index = 1 #init the relative scale list relative_scale_list = [] relative_scale_list.append(1) #initialize matching class with camera parameters matching_class = matching(K, D) #create a detector detector = cv2.ORB_create(nfeatures=800) for i in range(0, 170): # for i in range(1,img_num): # for i in range(0,img_num): #insert images matching_class.load_image(img1, img2) #scan matching goodkf, matches = matching_class.match_images(detector) #keyframe choose conditions are #a. not skip more than skipped threshold frames #b. real scale are always not 0 #c. have enough matches #d. the adjacent frames # if (enough_match == False or scale[i-1] < 0.001) and (frame_skipped < frame_skipped_threshold): if (goodkf == False or frame_skipped < frame_skipped_floor) and ( frame_skipped < frame_skipped_ceil): # print("for the ",i,"image absolute scale is ",scale[i-1]) # print('not a good keyframe') frame_skipped = frame_skipped + 1 keyframe_index = keyframe_index + 1 print("keyframe index is ", keyframe_index, "skipped ", frame_skipped) if keyframe_index > img_num - 1: break img2 = cv2.imread(filelist1[keyframe_index]) continue #get keypoints kp1_match, kp2_match = matches keypoints_list.append(matching_class.kp1) descriptors_list.append(matching_class.des1) #calculate the relative scale relative_scale = comput_relative_scale(kp1_match, kp2_match) #remove absolute wrong scale if relative_scale > 3: keyframe_index = keyframe_index + 1 print("keyframe index is ", keyframe_index) if keyframe_index > img_num - 1: break img2 = cv2.imread(filelist1[keyframe_index]) continue # cumulate_relative_scale = relative_scale_list[-1] * relative_scale relative_scale_list.append(cumulate_relative_scale) # print("for the ",i,"image relative scale between two keyframe is ",relative_scale) print( "for the ", i, "image calculated relative scale relate to the first keyframe is ", cumulate_relative_scale) #reinit frame_skipped frame_skipped = 0 #append keyframe files keyframe_file_list.append(filelist1[keyframe_index]) #add into bovw bovw_class.add_histogram(matching_class.des1) #calculate rotation and transformation dR = matching_class.getRotation() rotation_array.append(dR) dt = np.transpose(matching_class.getTransformation()) dt = dt.dot(R) * relative_scale_list[-1] transformation_array.append(dt) R = dR.dot(R) t = t + dt pose_array.append(t) #plot # plot_pose(np.asarray(pose_array)) # plt.show() # cv2.waitKey(1) # plt.close() ###############optimizatoin############### #####add vertex vertex_id = len(pose_array) - 1 pose = g2o.Isometry3d(g2o.Quaternion(0, 0, 0, 1), np.transpose(pose_array[vertex_id])) optimization_class.add_vertex(vertex_id, pose) #####add edge between adjacent frames constraint = dt + np.append( np.random.normal(0, 0, 2), np.array([0]), axis=0) constraint = np.transpose(constraint) optimization_class.add_edge(optimization_class.vertex(vertex_id), optimization_class.vertex(vertex_id - 1), measurement=g2o.Isometry3d( g2o.Quaternion(0, 0, 0, 1), constraint)) #find loop closure lc_index, lc_cost = bovw_class.find_lc(matching_class.des2) lc_indices = bovw_class.get_lowest_costs_index( max_num_lc) # number of lowest cost indices print(lc_cost) # img_lc = cv2.imread(keyframe_file_list[lc_index]) # cv2.imshow('Loop closure matched',img_lc) for lc_i in lc_indices: if (lc_cost < 0.15) and (lc_i < keyframe_number - 5): img_lc = cv2.imread(keyframe_file_list[lc_i]) cv2.imshow('Loop closure matched', img_lc) #add the loopclosure kf to list loopclosure_pairs.append([lc_i, keyframe_number]) #scale calculate, 1st calutate the good matches, then relative scale # lc_scale = comput_relative_scale(,) # print('lc scale is ', scale[i-2]/relative_scale_list[i-1]*lc_scale) cv2.waitKey(1) #assign new keyframe image keyframe_number = keyframe_number + 1 img1 = img2 keyframe_index = keyframe_index + 1 if keyframe_index > img_num - 1: break #plot lc matched image img2 = cv2.imread(filelist1[keyframe_index]) print("loopclosure pairs are:", loopclosure_pairs) ##############optimization #add vertices # opt = PoseGraphOptimization() # for vertex_id in range(len(keyframe_file_list)): # pose = g2o.Isometry3d(g2o.Quaternion(0,0,0,1),np.transpose(pose_array[vertex_id])) # if vertex_id == 0: # opt.add_vertex(vertex_id,pose,True) # else: # opt.add_vertex(vertex_id,pose) #get measurement print(keyframe_file_list) print(len(keyframe_file_list)) save_to_pickle(keyframe_file_list, "keyframe_file_list.pkl") save_to_pickle(loopclosure_pairs, "loopclosure_pairs.pkl") # def compute_scale(file_list, id1, id2): # img1 = cv2.imread(file_list[id1]) # img2 = cv2.imread(file_list[id2]) # matching_class.load_image(img1,img2) # #scan matching # enough_match,matches = matching_class.match_images(detector) # cv2.waitKey(1) # kp1_match,kp2_match = matches # #calculate the relative scale # relative_scale = comput_relative_scale(kp1_match,kp2_match) # #remove absolute wrong scale # if relative_scale >2: # print('bad scale') # # continue # measurement_scale = relative_scale_list[id1]*relative_scale # dt = np.transpose(pose_array[id2]- pose_array[id1]) # # dt = matching_class.getTransformation() # # print("scale, ",measurement_scale) # # print("dt, ", dt) # # print("dt, ", dt.shape) # measurement= measurement_scale*dt for pairs in loopclosure_pairs: # load loopclosed keyframe pairs keyframe_id = pairs[0] keyframe_id2 = pairs[1] img1 = cv2.imread(keyframe_file_list[keyframe_id]) img2 = cv2.imread(keyframe_file_list[keyframe_id2]) matching_class.load_image(img1, img2) #scan matching enough_match, matches = matching_class.match_images(detector) cv2.waitKey(1) kp1_match, kp2_match = matches #calculate the relative scale relative_scale = comput_relative_scale(kp1_match, kp2_match) #remove absolute wrong scale if relative_scale > 2: print('bad scale') # continue measurement_scale = relative_scale_list[keyframe_id] * relative_scale dt = np.transpose(pose_array[keyframe_id2] - pose_array[keyframe_id]) # dt = matching_class.getTransformation() # print("scale, ",measurement_scale) # print("dt, ", dt) # print("dt, ", dt.shape) measurement = measurement_scale * dt #add to edge optimization_class.add_edge(optimization_class.vertex(keyframe_id2), optimization_class.vertex(keyframe_id), measurement=g2o.Isometry3d( g2o.Quaternion(0, 0, 0, 1), measurement)) # pair_id = 0 # keyframe_id = loopclosure_pairs[pair_id][0] # keyframe_id2 = loopclosure_pairs[pair_id][1] # img1 = cv2.imread(keyframe_file_list[keyframe_id]) # img2 = cv2.imread(keyframe_file_list[keyframe_id2]) # matching_class.load_image(img1,img2) # #scan matching # enough_match,matches = matching_class.match_images(detector) # kp1_match,kp2_match = matches # #calculate the relative scale # relative_scale = comput_relative_scale(kp1_match,kp2_match) # #remove absolute wrong scale # if relative_scale >5: # print('bad scale') # # continue # measurement_scale = relative_scale_list[keyframe_id]*relative_scale # dt = np.transpose(pose_array[keyframe_id2]- pose_array[keyframe_id]) # # dt = matching_class.getTransformation() # print("scale, ",measurement_scale) # print("dt, ", dt) # print("dt, ", dt.shape) # measurement= measurement_scale*dt # #add to edge # optimization_class.add_edge(optimization_class.vertex(keyframe_id2),optimization_class.vertex(keyframe_id),measurement=g2o.Isometry3d(g2o.Quaternion(0,0,0,1),measurement)) # print("original pose0 is, ",pose_array[keyframe_id]) # print("original pose1 is, ",pose_array[keyframe_id2]) # print("measurement is ,", measurement, "between vertex ",keyframe_id," and ",keyframe_id2) print("error", optimization_class.compute_active_errors()) # optimization_class.optimize(0) # print("optimized fisrt pose is ", optimization_class.get_pose(0).translation()) # print("optimized fisrt pose is ", optimization_class.get_pose(1).translation()) # print("optimized fisrt pose is ", optimization_class.get_pose(2).translation()) # optimization_class.optimize(2) # print("optimized fisrt pose is ", optimization_class.get_pose(0).translation()) # print("optimized fisrt pose is ", optimization_class.get_pose(1).translation()) # print("optimized fisrt pose is ", optimization_class.get_pose(2).translation()) ##############end optimization # bovw_class.save_bovw_lib() save_to_pickle(keyframe_file_list, "image_file_list.pkl") #convert lists to array #plot origin rotation_array = np.asarray(rotation_array) transformation_array = np.asarray(transformation_array) pose_array = np.asarray(pose_array) plot_pose(pose_array) print(pose_array.shape) plt.show() optimization_class.optimize(10) #plot opt opt_pose = [] for i in range(len(pose_array)): arr = optimization_class.get_pose(i).translation() opt_pose.append(np.expand_dims(arr, axis=0)) opt_pose = np.asarray(opt_pose) plot_pose(opt_pose) plt.show()
def rotmat(q): q = g2o.Quaternion(q) q.normalize() return q.matrix()
super().add_edge(edge) def test_add_edge(self,edge): super().add_edge(edge) if __name__ == "__main__": pose0 = np.array([0,0,0]) #+ np.random.normal(0,0.1,3) pose1 = np.array([0,1,0]) + np.random.normal(0,0.1,3) pose2 = np.array([0,2,0]) + np.random.normal(0,0.1,3) print("poses:",'\n',pose0,'\n',pose1,'\n',pose2,'\n') opt = PoseGraphOptimization() # pose0 = g2o.SE3Quat(np.identity(6), pose0) # pose1 = g2o.SE3Quat(np.identity(6), pose1) # pose2 = g2o.SE3Quat(np.identity(6), pose2) pose0 = g2o.Isometry3d(g2o.Quaternion(0,0,0,1),pose0) pose1 = g2o.Isometry3d(g2o.Quaternion(0,0,0,1),pose1) pose2 = g2o.Isometry3d(g2o.Quaternion(0,0,0,1),pose2) opt.add_vertex(0,pose0,True) opt.add_vertex(1,pose1) opt.add_vertex(2,pose2) vertices = opt.get_vertices() opt.add_edge(vertices[0],vertices[1],measurement=g2o.Isometry3d(g2o.Quaternion(0,0,0,1),np.array([0,-1,0]))) opt.add_edge(vertices[1],vertices[2],measurement=g2o.Isometry3d(g2o.Quaternion(0,0,0,1),np.array([0,-1,0]))) opt.add_edge(vertices[0],vertices[2],measurement=g2o.Isometry3d(g2o.Quaternion(0,0,0,1),np.array([0,-2,0]))) opt.initialize_optimization() print("error", opt.compute_active_errors())
def quaternion(q): q = g2o.Quaternion(q) q.normalize() return q
def set_from_rotation_and_translation(self, Rcw, tcw): self.set(g2o.Isometry3d(g2o.Quaternion(Rcw), tcw))
def set_rotation_matrix(self, Rcw): self.set(g2o.Isometry3d(g2o.Quaternion(Rcw), self._pose.position()))
def create_pose(self, orientation: np.ndarray, translation: np.ndarray) -> g2o.Isometry3d: pose = g2o.Isometry3d() pose.set_translation(translation) q = g2o.Quaternion(orientation) pose.set_rotation(q) return pose
import sys import numpy as np import math sys.path.append("../../") from config import Config import g2o position = np.zeros(3) # delta translation orientation = g2o.Quaternion() pose = g2o.Isometry3d(orientation, position) print('pose: ', pose.matrix()) position[0] = 1 print('pose: ', pose.matrix()) orientation2 = g2o.Quaternion(g2o.AngleAxis(math.pi, [0, 0, 1])) print('position', position) print('orientation2*position', orientation2 * position)