def calculate_angle_ranges(self, cam_angle_i):
cam_angle = self.omsws.image_angles[cam_angle_i:cam_angle_i+1]
nearest_points = self.find_nearest_points(self.points, self.points_angles,
cam_angle)
cam_positions = oms_geo.pol2cart(self.cam_offset, cam_angle)
cam_positions = np.array(cam_positions).T
cam_positions = np.hstack((cam_positions, np.zeros((cam_positions.shape[0], 1))))
mid_image_vector = nearest_points - cam_positions
mid_image_vector[:,2] = np.zeros((mid_image_vector.shape[0]))
image_beg_rot = oms_geo.trans_rot_array_3d((0, 0, self.cam_angle_of_view/2),
(0, 0, 0))
image_end_rot = oms_geo.trans_rot_array_3d((0, 0, -self.cam_angle_of_view/2),
(0, 0, 0))
points_start = oms_geo.transform(mid_image_vector, image_beg_rot) + cam_positions
angles_start = np.arctan2(points_start[:, 1], points_start[:, 0])
angles_start = (angles_start + 2*math.pi) % (2*math.pi)
points_end = oms_geo.transform(mid_image_vector, image_end_rot) + cam_positions
angles_end = np.arctan2(points_end[:, 1], points_end[:, 0])
angles_end = (angles_end + 2*math.pi) % (2*math.pi)
return angles_start[0], angles_end[0]
def find_rotation_zyz(tx, ty, tz):
# find rotation matrix based on the vector x, y, z to rotate to vector
# along z axis
rot_z = math.atan(ty/tx)
rot_y = -math.pi/2 + math.atan(tz/math.sqrt(tx**2 + ty**2))
# create rotation translation matrix with z-y-z rotation and translation 0
rot_trans = oms_geo.trans_rot_array_3d((0, rot_y, rot_z),
(0,0,0), 'zyz')
rotation = rot_trans[:3, :3]
return rotation
def init_loading_images(self):
# update rotation and position of the camera based on calibration
self.update_cam_rot_matrix()
# copy camera angles
cam_angles = self.omsws.image_angles
self.im_height, self.im_width = self.omsws.images[0].shape[:2]
self.cam_head_rot = [oms_geo.trans_rot_array_3d((0, 0, -cam_angle),
(0,0,0))[:3, :3]
for cam_angle in cam_angles]
# calculate angles of all the points, make sure they are 0 - 2*pi
self.points_angles = self.calculate_points_angles(self.points)
def load_images(self):
cam_rot_matrix = oms_geo.trans_rot_array_3d(-self.cam_rot, [0,
*-self.cam_pos[1:]])
cam_angles = self.omsws.image_angles
if not isinstance(self, StitchImageLoader):
scans_points = np.asarray(self.full_copy.points)
else:
scans_points = self.points
# initialize point cloud color array
scan_colors = np.zeros_like(np.asarray(scans_points))
self.image_sources = np.zeros(scans_points.shape, dtype=np.uint16)
im_height, im_width = self.omsws.images[0].shape[:2]
# calculate distance of an image from the camera, so we can find pixels
# for point cloud assuming pixels are millimiters
im_dist = self.cam_offset + im_width /\
(2 * math.tan(self.cam_angle_of_view / 2))
# create array of rotations back to position 0, where we are loading images
real_cam_angles = np.array(cam_angles) + math.atan2(self.cam_pos[1],
self.cam_offset)
cam_head_rot = [oms_geo.trans_rot_array_3d((0, 0, -cam_angle),
(0,0,0))[:3, :3]
for cam_angle in real_cam_angles]
# calculate angles of all the points, make sure they are 0 - 2*pi
points_angles = np.arctan2(scans_points[:, 1], scans_points[:, 0])
points_angles = (points_angles + 2*math.pi) % (2*math.pi)
indexes = list(range(len(cam_angles)))
ind_cam_angles = sorted(zip(indexes, real_cam_angles), key=lambda x:x[1])
# add first and last cam angle on the beginning and end of angles list
ind_cam_angles.insert(0, (ind_cam_angles[-1][0],
ind_cam_angles[-1][1] - 2*math.pi))
ind_cam_angles.append((ind_cam_angles[1][0],
ind_cam_angles[1][1] + 2*math.pi))
(im_index, sorted_cam_angles) = zip(*ind_cam_angles)
point_im_indexes = griddata(np.array(sorted_cam_angles), np.array(im_index),
points_angles, method='nearest')
for image_i in range(len(cam_angles)):
points_i, = np.where(point_im_indexes == image_i)
points = scans_points[points_i]
if len(points != 0):
points = np.dot(points, cam_head_rot[image_i])
points[:, 0] -= self.cam_offset
points = oms_geo.transform(points, cam_rot_matrix)
# points = np.dot(points, cam_rot_matrix[:3, :3])
# points += cam_rot_matrix[:3, 3]
x = points[:, 0]; y = points[:, 1]; z = points[:, 2]
d = np.sqrt(np.power(x, 2) + np.power(y, 2) + np.power(z, 2))
# angle on hight
rxy = np.arcsin(z / d)
# angle on width
rz = np.arctan2(y, x)
# calculate the pixels that correspond to that angle
y_im = np.array(im_dist * np.tan(rxy) + im_width / 2, dtype=int)
x_im = np.array(im_dist * np.tan(rz) + im_height / 2, dtype=int)
ixy = np.column_stack((points_i, x_im, y_im))
ixy = ixy[np.logical_and(np.logical_and(0 < x_im, x_im < im_height),
np.logical_and(0 < y_im, y_im < im_width))]
scan_colors[ixy[:, 0]] =\
self.omsws.images[image_i][ixy[:, 1], ixy[:, 2], :]
return scan_colors
def update_cam_rot_matrix(self):
# update camera rotation matrix, after it was calibrated
self.cam_rot_matrix = oms_geo.trans_rot_array_3d(-self.cam_rot,
[0, *-self.cam_pos[1:]])
print('updating cam rot matrix', self.cam_rot_matrix)