def apply_anchor_flip(self): if self.isInverse: self.data['a_position'] = base_process.sv3d_apply_m4( data=self.data['a_position'], m4=self.anchor_matrix) else: self.data['a_position'] = \ self.data['a_position'] = base_process.sv3d_apply_m4( data=self.data['a_position'], m4=self.anchor_matrix_inv) self.isInverse = not self.isInverse
def align_flip(self): if self.isAligned: self.data['a_position'] = \ base_process.sv3d_apply_m4(data=self.data['a_position'], m4=self.matrix_inv) else: self.data['a_position'] = \ base_process.sv3d_apply_m4(data=self.data['a_position'], m4=self.matrix) self.isAligned = not self.isAligned
def apply_anchor_flip(self): if self.isInverse: self.data['a_position'] = base_process.sv3d_apply_m4( data=self.data['a_position'],m4=self.anchor_matrix) else: self.data['a_position'] = \ self.data['a_position'] = base_process.sv3d_apply_m4( data=self.data['a_position'], m4=self.anchor_matrix_inv) self.isInverse = not self.isInverse
def align_flip(self): if self.isAligned: self.data['a_position'] = \ base_process.sv3d_apply_m4(data=self.data['a_position'], m4=self.matrix_inv) else: self.data['a_position'] = \ base_process.sv3d_apply_m4(data=self.data['a_position'], m4=self.matrix) self.isAligned = not self.isAligned
def apply_yaw_flip(self): if self.isInverse: matrix = np.eye(4, dtype=np.float32) if self.isYaw: glm.rotate(matrix, self.anchor_yaw, 0, 0, 1) self.data['a_position'] = base_process.sv3d_apply_m4( data=self.data['a_position'],m4=matrix) else: glm.rotate(matrix, self.anchor_yaw, 0, 0, -1) self.data['a_position'] = \ self.data['a_position'] = base_process.sv3d_apply_m4( data=self.data['a_position'], m4=matrix) self.isYaw = not self.isYaw
def apply_yaw_flip(self): if self.isInverse: matrix = np.eye(4, dtype=np.float32) if self.isYaw: glm.rotate(matrix, self.anchor_yaw, 0, 0, 1) self.data['a_position'] = base_process.sv3d_apply_m4( data=self.data['a_position'], m4=matrix) else: glm.rotate(matrix, self.anchor_yaw, 0, 0, -1) self.data['a_position'] = \ self.data['a_position'] = base_process.sv3d_apply_m4( data=self.data['a_position'], m4=matrix) self.isYaw = not self.isYaw
def fix_spherical_inside_out(self): """ panorama's four corner v.s center point if the spherical_ray is center point, then this process need to be done I reverse the y-axis make it inside-out :return:fixed ptCLoudData """ self.ptCLoudData['a_position'][:, 1] = -self.ptCLoudData['a_position'][:, 1] self.ptCLoudDataGnd['a_position'][:, 1] = -self.ptCLoudDataGnd['a_position'][:, 1] m4 = np.array([[ -1.00000000e+00, 0.00000000e+00, -1.22464685e-16, 0.00000000e+00], [ 0.00000000e+00, 1.00000000e+00, 0.00000000e+00, 0.00000000e+00], [ 1.22464685e-16, 0.00000000e+00, -1.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) self.ptCLoudData['a_position'] = base_process.sv3d_apply_m4(data=self.ptCLoudData['a_position'], m4=m4) self.ptCLoudDataGnd['a_position'] = base_process.sv3d_apply_m4(data=self.ptCLoudDataGnd['a_position'], m4=m4)
def create_topology_bfs(self): id_2_gps = self.fileMeta['id2GPS'] pano_list = self.fileMeta['panoList'] pano_len = self.fileMeta['cur'] + 1 topology = np.zeros((pano_len, 3), dtype=np.float32) for idx in range(0, pano_len): gps = id_2_gps[pano_list[idx]] lat, lon = float(gps[0]), float(gps[1]) ecef = np.array(base_process.geo_2_ecef(lat, lon, 22)) if idx == 0: self.anchorECEF = ecef matrix = np.eye(4, dtype=np.float32) # Change xy-plan to ecef coordinate glm.rotate(matrix, 90, 0, 1, 0) glm.rotate(matrix, 90, 1, 0, 0) glm.rotate(matrix, lat, 0, -1, 0) glm.rotate(matrix, lon, 0, 0, 1) self.anchorMatrix = matrix else: topology[idx, :] = ecef - self.anchorECEF topology = base_process.sv3d_apply_m4(topology, m4=np.linalg.inv(self.anchorMatrix)) id_2_gps = self.fileMeta['id2GPS'] pano_num = len(id_2_gps) data = np.zeros(pano_num, dtype=[('a_position', np.float32, 3), ('a_color', np.float32, 3)]) idx = 0 for panoid in id_2_gps: gps = id_2_gps[panoid] ecef = base_process.geo_2_ecef(float(gps[0]), float(gps[1]), 22) - self.anchorECEF # TODO: ecef v.s. x-y plane data['a_position'][idx] = np.asarray(ecef, dtype=np.float32) idx += 1 data['a_color'] = [0, 1, 0] data['a_position'] = base_process.sv3d_apply_m4(data=data['a_position'], m4=np.linalg.inv(self.anchorMatrix)) self.topologyData = data
def create_topology(self): id_2_gps = self.fileMeta['id2GPS'] pano_num = len(id_2_gps) data = np.zeros(pano_num, dtype=[('a_position', np.float32, 3), ('a_color', np.float32, 3)]) idx = 0 for panoid in id_2_gps: gps = id_2_gps[panoid] ecef = base_process.geo_2_ecef(float(gps[0]), float(gps[1]), 22) - self.anchorECEF # TODO: ecef v.s. x-y plane data['a_position'][idx] = np.asarray(ecef, dtype=np.float32) idx += 1 data['a_color'] = [0, 1, 0] data['a_position'] = base_process.sv3d_apply_m4(data=data['a_position'], m4=np.linalg.inv(self.anchorMatrix)) self.topologyData = data
def apply_anchor_adjustment(self, anchor_matrix): self.ptCLoudData['a_position'] = base_process.sv3d_apply_m4(data=self.ptCLoudData['a_position'], m4=np.linalg.inv(anchor_matrix))
def apply_m4(self, m4): self.data['a_position'] = \ base_process.sv3d_apply_m4(data=self.data['a_position'], m4=m4)
def apply_anchor_plus_rotate(self): matrix = self.anchor_matrix glm.rotate(matrix, 180, 0, 1, 0) self.data['a_position'] = base_process.sv3d_apply_m4(data=self.data['a_position'], m4=np.linalg.inv(matrix))
def apply_m4(self, m4): self.data['a_position'] = \ base_process.sv3d_apply_m4(data=self.data['a_position'], m4=m4)
data = np.concatenate((data, sv3D.ptCLoudData), axis=0) index += 1 #if index > 10: # break programSV3DRegion = glumpy_setting.ProgramSV3DRegion(data=data, name='ProgramSV3DRegion', point_size=1, anchor_matrix=anchor_matrix_whole) if needMatchInfo3d: programSV3DRegion.apply_anchor_flip() """ Transform the trajectory to GPS location """ trajectory = programTrajectory.data['a_position'] trajectory = base_process.sv3d_apply_m4(data=trajectory, m4=sv3DRegion.anchorMatrix) trajectory[:, 0] += sv3DRegion.anchorECEF[1] trajectory[:, 1] += sv3DRegion.anchorECEF[2] trajectory[:, 2] += sv3DRegion.anchorECEF[0] """ Output """ trajectory_gps = sfm3DRegion.trajectoryJSON cur = 0 for key, value in sorted(trajectory_gps.items()): t = trajectory[cur] lat, lon, h = base_process.ecef_2_geo(t[2], t[0], t[1]) trajectory_gps[key] = [lat, lon, h] cur += 1 #for idx, t in enumerate(trajectory):
def apply_local_adjustment(self): self.ptCLoudData['a_position'] = base_process.sv3d_apply_m4(data=self.ptCLoudData['a_position'], m4=self.matrix_local)
index += 1 #if index > 10: # break programSV3DRegion = glumpy_setting.ProgramSV3DRegion( data=data, name='ProgramSV3DRegion', point_size=1, anchor_matrix=anchor_matrix_whole) if needMatchInfo3d: programSV3DRegion.apply_anchor_flip() """ Transform the trajectory to GPS location """ trajectory = programTrajectory.data['a_position'] trajectory = base_process.sv3d_apply_m4(data=trajectory, m4=sv3DRegion.anchorMatrix) trajectory[:, 0] += sv3DRegion.anchorECEF[1] trajectory[:, 1] += sv3DRegion.anchorECEF[2] trajectory[:, 2] += sv3DRegion.anchorECEF[0] """ Output """ trajectory_gps = sfm3DRegion.trajectoryJSON cur = 0 for key, value in sorted(trajectory_gps.items()): t = trajectory[cur] lat, lon, h = base_process.ecef_2_geo(t[2], t[0], t[1]) trajectory_gps[key] = [lat, lon, h] cur += 1 #for idx, t in enumerate(trajectory):
for i in range(0, pano_length): sv3D = sv3DRegion.sv3D_Time[i] sv3D.apply_global_adjustment( ) # Absolute position on earth (lat, lon, yaw) sv3D.apply_local_adjustment( ) # Relative position according to anchor (anchor's lat,lon) if needMatchInfo3d: # This rotate the SV3D for matching x-y plane # Actually we build the point cloud on x-y plane # So we just multiply the inverse matrix of anchor sv3D.apply_anchor_adjustment( anchor_matrix=sv3DRegion.anchorMatrix) # Record all pano location(relative) pano_ori_vec[i] = sv3D.ecef - sv3DRegion.anchorECEF pano_ori_vec = base_process.sv3d_apply_m4(data=pano_ori_vec, m4=np.linalg.inv( sv3DRegion.anchorMatrix)) """ Gather the pointcloud data (may be aligned or not) """ # Why separate in two loops? # Because there once existed a process about alignment here # And if we want to know which panorama is the closet(not necessary) data, dataGnd, dataNearest = None, None, None for i in range(0, pano_length): sv3D = sv3DRegion.sv3D_Time[i] if addPlane: sv3D.auto_plane() if i == 0: data = sv3D.ptCLoudData[np.nonzero(sv3D.non_con)] dataGnd = sv3D.ptCLoudData[np.nonzero(sv3D.gnd_con)]
def apply_anchor_plus_rotate(self): matrix = self.anchor_matrix glm.rotate(matrix, 180, 0, 1, 0) self.data['a_position'] = base_process.sv3d_apply_m4( data=self.data['a_position'], m4=np.linalg.inv(matrix))
""" # Store metadata of the created region pano_ori_vec = np.zeros((pano_length, 3)) # Initialize the pano according to location(lat, lon) for i in range(0, pano_length): sv3D = sv3DRegion.sv3D_Time[i] sv3D.apply_global_adjustment() # Absolute position on earth (lat, lon, yaw) sv3D.apply_local_adjustment() # Relative position according to anchor (anchor's lat,lon) if needMatchInfo3d: # This rotate the SV3D for matching x-y plane # Actually we build the point cloud on x-y plane # So we just multiply the inverse matrix of anchor sv3D.apply_anchor_adjustment(anchor_matrix=sv3DRegion.anchorMatrix) # Record all pano location(relative) pano_ori_vec[i] = sv3D.ecef - sv3DRegion.anchorECEF pano_ori_vec = base_process.sv3d_apply_m4(data=pano_ori_vec, m4=np.linalg.inv(sv3DRegion.anchorMatrix)) """ Gather the pointcloud data (may be aligned or not) """ # Why separate in two loops? # Because there once existed a process about alignment here # And if we want to know which panorama is the closet(not necessary) data, dataGnd, dataNearest = None, None, None for i in range(0, pano_length): sv3D = sv3DRegion.sv3D_Time[i] if addPlane: sv3D.auto_plane() if i == 0: data = sv3D.ptCLoudData[np.nonzero(sv3D.non_con)] dataGnd = sv3D.ptCLoudData[np.nonzero(sv3D.gnd_con)] else: