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:
