text: #!/usr/bin/env python3
  # This file is covered by the LICENSE file in the root of this project.
  
  import os
  from collections import namedtuple
  
  import numpy as np
  import pykitti.utils as utils
  
  
  def project_velo_to_cam2(calib):
      P_velo2cam_ref = np.vstack((calib['Tr_velo_to_cam'].reshape(3, 4), np.array([0., 0., 0., 1.])))  # velo2ref_cam
      R_ref2rect = np.eye(4)
      R0_rect = calib['R0_rect'].reshape(3, 3)  # ref_cam2rect
      R_ref2rect[:3, :3] = R0_rect
      P_rect2cam2 = calib['P2'].reshape((3, 4))
      proj_mat = P_rect2cam2 @ R_ref2rect @ P_velo2cam_ref
      return proj_mat
  
  
  def project_to_image(points, proj_mat):
      """
      Apply the perspective projection
      Args:
          pts_3d:     3D points in camera coordinate [3, npoints]
          proj_mat:   Projection matrix [3, 4]
      """
      num_pts = points.shape[1]
  
      # Change to homogenous coordinate
      points = np.vstack((points, np.ones((1, num_pts))))
      points = proj_mat @ points
      points[:2, :] /= points[2, :]
      return points[:2, :]
  
  
  class LaserScan:
      """Class that contains LaserScan with x,y,z,r"""
      EXTENSIONS_SCAN = ['.bin']
  
      def __init__(self, project=False, H=64, W=1024, fov_up=3.0, fov_down=-25.0):
          self.project = project
          self.proj_H = H
          self.proj_W = W
          self.proj_fov_up = fov_up
          self.proj_fov_down = fov_down
          self.reset()
  
      def _load_calib(self, path):
          """Load and compute intrinsic and extrinsic calibration parameters."""
          # We'll build the calibration parameters as a dictionary, then
          # convert it to a namedtuple to prevent it from being modified later
          data = {}
  
          # Load the calibration file
          calib_filepath = os.path.join(path, 'calib.txt')
          filedata = utils.read_calib_file(calib_filepath)
  
          # Create 3x4 projection matrices
          P_rect_00 = np.reshape(filedata['P0'], (3, 4))
          P_rect_10 = np.reshape(filedata['P1'], (3, 4))
          P_rect_20 = np.reshape(filedata['P2'], (3, 4))
          P_rect_30 = np.reshape(filedata['P3'], (3, 4))
  
          data['P_rect_00'] = P_rect_00
          data['P_rect_10'] = P_rect_10
          data['P_rect_20'] = P_rect_20
          data['P_rect_30'] = P_rect_30
  
          # Compute the rectified extrinsics from cam0 to camN
          T1 = np.eye(4)
          T1[0, 3] = P_rect_10[0, 3] / P_rect_10[0, 0]
          T2 = np.eye(4)
          T2[0, 3] = P_rect_20[0, 3] / P_rect_20[0, 0]
          T3 = np.eye(4)
          T3[0, 3] = P_rect_30[0, 3] / P_rect_30[0, 0]
  
          # Compute the velodyne to rectified camera coordinate transforms
          data['T_cam0_velo'] = np.reshape(filedata['Tr'], (3, 4))
          data['T_cam0_velo'] = np.vstack([data['T_cam0_velo'], [0, 0, 0, 1]])
          data['T_cam1_velo'] = T1.dot(data['T_cam0_velo'])
          data['T_cam2_velo'] = T2.dot(data['T_cam0_velo'])
          data['T_cam3_velo'] = T3.dot(data['T_cam0_velo'])
  
          # Compute the camera intrinsics
          data['K_cam0'] = P_rect_00[0:3, 0:3]
          data['K_cam1'] = P_rect_10[0:3, 0:3]
          data['K_cam2'] = P_rect_20[0:3, 0:3]
          data['K_cam3'] = P_rect_30[0:3, 0:3]
  
          # Compute the stereo baselines in meters by projecting the origin of
          # each camera frame into the velodyne frame and computing the distances
          # between them
          p_cam = np.array([0, 0, 0, 1])
          p_velo0 = np.linalg.inv(data['T_cam0_velo']).dot(p_cam)
          p_velo1 = np.linalg.inv(data['T_cam1_velo']).dot(p_cam)
          p_velo2 = np.linalg.inv(data['T_cam2_velo']).dot(p_cam)
          p_velo3 = np.linalg.inv(data['T_cam3_velo']).dot(p_cam)
  
          data['b_gray'] = np.linalg.norm(p_velo1 - p_velo0)  # gray baseline
          data['b_rgb'] = np.linalg.norm(p_velo3 - p_velo2)  # rgb baseline
  
          self.calib = namedtuple('CalibData', data.keys())(*data.values())
  
      def reset(self):
          """ Reset scan members. """
          self.points = np.zeros((0, 3), dtype=np.float32)  # [m, 3]: x, y, z
          self.remissions = np.zeros((0, 1), dtype=np.float32)  # [m ,1]: remission
  
          # projected range image - [H,W] range (-1 is no data)
          self.proj_range = np.full((self.proj_H, self.proj_W), -1,
                                    dtype=np.float32)
  
          # unprojected range (list of depths for each point)
          self.unproj_range = np.zeros((0, 1), dtype=np.float32)
  
          # projected point cloud xyz - [H,W,3] xyz coord (-1 is no data)
          self.proj_xyz = np.full((self.proj_H, self.proj_W, 3), -1,
                                  dtype=np.float32)
  
          # projected remission - [H,W] intensity (-1 is no data)
          self.proj_remission = np.full((self.proj_H, self.proj_W), -1,
                                        dtype=np.float32)
  
          # projected index (for each pixel, what I am in the pointcloud)
          # [H,W] index (-1 is no data)
          self.proj_idx = np.full((self.proj_H, self.proj_W), -1,
                                  dtype=np.int32)
  
          # for each point, where it is in the range image
          self.proj_x = np.zeros((0, 1), dtype=np.int32)  # [m, 1]: x
          self.proj_y = np.zeros((0, 1), dtype=np.int32)  # [m, 1]: y
  
          # mask containing for each pixel, if it contains a point or not
          self.proj_mask = np.zeros((self.proj_H, self.proj_W),
                                    dtype=np.int32)  # [H,W] mask
  
      def size(self):
          """ Return the size of the point cloud. """
          return self.points.shape[0]
  
      def __len__(self):
          return self.size()
  
      def open_scan(self, filename):
          """ Open raw scan and fill in attributes
          """
          # print(filename)
          # print('/'.join(filename.split('/')[:-2]))
          # exit()
          self._load_calib('/'.join(filename.split('/')[:-2]))
          # reset just in case there was an open structure
          self.reset()
  
          # check filename is string
          if not isinstance(filename, str):
              raise TypeError("Filename should be string type, "
                              "but was {type}".format(type=str(type(filename))))
  
          # check extension is a laserscan
          if not any(filename.endswith(ext) for ext in self.EXTENSIONS_SCAN):
              raise RuntimeError("Filename extension is not valid scan file.")
  
          # if all goes well, open pointcloud
          scan = np.fromfile(filename, dtype=np.float32)
          scan = scan.reshape((-1, 4))
          # cam2_points = self.calib.T_cam3_velo.dot(scan.T).T
          # self.idx = cam2_points[:, 2] > 0
          # cam2_points = cam2_points[idx]
          # scan = scan[idx]
  
          # cam2_points = cam2_points[:, :3] / cam2_points[:, 2:3]
          # intrinsic_cam2 = self.calib.K_cam2
          # cam2_points = intrinsic_cam2.dot(cam2_points.T).T[:, [1, 0]]
          # cam2_points = cam2_points.astype(int)
          # img_width = 1241
          # img_height = 376
          # self.idx = np.where((cam2_points[:,0] < img_height) & (cam2_points[:,1] < img_width)
          #                    & (cam2_points[:,0]>0) & (cam2_points[:,1]>0))[0]
          # cam2_points = cam2_points[:, :3] / cam2_points[:, 2:3]
          # intrinsic_cam2 = self.calib.K_cam3
          # cam2_points = intrinsic_cam2.dot(cam2_points.T).T[:, [1, 0]]
          # self.idx2 = cam2_points[:]
          # print(cam2_points.shape)
          # exit()
          # scan = scan[self.idx]
          # put in attribute
          points = scan[:, 0:3]  # get xyz
          remissions = scan[:, 3]  # get remission
          self.set_points(points, remissions)
  
      def set_points(self, points, remissions=None):
          """ Set scan attributes (instead of opening from file)
          """
          # reset just in case there was an open structure
          self.reset()
  
          # check scan makes sense
          if not isinstance(points, np.ndarray):
              raise TypeError("Scan should be numpy array")
  
          # check remission makes sense
          if remissions is not None and not isinstance(remissions, np.ndarray):
              raise TypeError("Remissions should be numpy array")
  
          # put in attribute
          self.points = points  # get xyz
          if remissions is not None:
              self.remissions = remissions  # get remission
          else:
              self.remissions = np.zeros((points.shape[0]), dtype=np.float32)
  
          # if projection is wanted, then do it and fill in the structure
          if self.project:
              self.do_range_projection()
  
      def do_range_projection(self):
          """ Project a pointcloud into a spherical projection image.projection.
              Function takes no arguments because it can be also called externally
              if the value of the constructor was not set (in case you change your
              mind about wanting the projection)
          """
          # laser parameters
          fov_up = self.proj_fov_up / 180.0 * np.pi  # field of view up in rad
          fov_down = self.proj_fov_down / 180.0 * np.pi  # field of view down in rad
          fov = abs(fov_down) + abs(fov_up)  # get field of view total in rad
  
          # get depth of all points
          depth = np.linalg.norm(self.points, 2, axis=1)
  
          # get scan components
          scan_x = self.points[:, 0]
          scan_y = self.points[:, 1]
          scan_z = self.points[:, 2]
  
          # get angles of all points
          yaw = -np.arctan2(scan_y, scan_x)
          pitch = np.arcsin(scan_z / depth)
  
          # get projections in image coords
          proj_x = 0.5 * (yaw / (np.pi / 4) + 1.0)  # in [0.0, 1.0]
  
          proj_y = 1.0 - (pitch + abs(fov_down)) / fov  # in [0.0, 1.0]
  
          # scale to image size using angular resolution
          proj_x *= self.proj_W  # in [0.0, W]
          proj_y *= self.proj_H  # in [0.0, H]
  
          # round and clamp for use as index
          proj_x = np.floor(proj_x)
          proj_x = np.minimum(self.proj_W - 1, proj_x)
          proj_x = np.maximum(0, proj_x).astype(np.int32)  # in [0,W-1]
          self.proj_x = np.copy(proj_x)  # store a copy in orig order
  
          proj_y = np.floor(proj_y)
          proj_y = np.minimum(self.proj_H - 1, proj_y)
          proj_y = np.maximum(0, proj_y).astype(np.int32)  # in [0,H-1]
          self.proj_y = np.copy(proj_y)  # stope a copy in original order
  
          # copy of depth in original order
          self.unproj_range = np.copy(depth)
  
          # order in decreasing depth
          indices = np.arange(depth.shape[0])
          order = np.argsort(depth)[::-1]
          depth = depth[order]
          indices = indices[order]
          points = self.points[order]
          remission = self.remissions[order]
          proj_y = proj_y[order]
          proj_x = proj_x[order]
  
          # assing to images
          self.proj_range[proj_y, proj_x] = depth
          self.proj_xyz[proj_y, proj_x] = points
          self.proj_remission[proj_y, proj_x] = remission
          self.proj_idx[proj_y, proj_x] = indices
          self.proj_mask = (self.proj_idx > 0).astype(np.int32)
  
  
  class SemLaserScan(LaserScan):
      """Class that contains LaserScan with x,y,z,r,sem_label,sem_color_label,inst_label,inst_color_label"""
      EXTENSIONS_LABEL = ['.label']
  
      def __init__(self, sem_color_dict=None, project=False, H=64, W=1024, fov_up=3.0, fov_down=-25.0, max_classes=300):
          super(SemLaserScan, self).__init__(project, H, W, fov_up, fov_down)
          self.reset()
  
          # make semantic colors
          if sem_color_dict:
              # if I have a dict, make it
              max_sem_key = 0
              for key, data in sem_color_dict.items():
                  if key + 1 > max_sem_key:
                      max_sem_key = key + 1
              self.sem_color_lut = np.zeros((max_sem_key + 100, 3), dtype=np.float32)
              for key, value in sem_color_dict.items():
                  self.sem_color_lut[key] = np.array(value, np.float32) / 255.0
          else:
              # otherwise make random
              max_sem_key = max_classes
              self.sem_color_lut = np.random.uniform(low=0.0,
                                                     high=1.0,
                                                     size=(max_sem_key, 3))
              # force zero to a gray-ish color
              self.sem_color_lut[0] = np.full((3), 0.1)
  
          # make instance colors
          max_inst_id = 100000
          self.inst_color_lut = np.random.uniform(low=0.0,
                                                  high=1.0,
                                                  size=(max_inst_id, 3))
          # force zero to a gray-ish color
          self.inst_color_lut[0] = np.full((3), 0.1)
  
      def reset(self):
          """ Reset scan members. """
          super(SemLaserScan, self).reset()
  
          # semantic labels
          self.sem_label = np.zeros((0, 1), dtype=np.int32)  # [m, 1]: label
          self.sem_label_color = np.zeros((0, 3), dtype=np.float32)  # [m ,3]: color
  
          # instance labels
          self.inst_label = np.zeros((0, 1), dtype=np.int32)  # [m, 1]: label
          self.inst_label_color = np.zeros((0, 3), dtype=np.float32)  # [m ,3]: color
  
          # projection color with semantic labels
          self.proj_sem_label = np.zeros((self.proj_H, self.proj_W),
                                         dtype=np.int32)  # [H,W]  label
          self.proj_sem_color = np.zeros((self.proj_H, self.proj_W, 3),
                                         dtype=np.float)  # [H,W,3] color
  
          # projection color with instance labels
          self.proj_inst_label = np.zeros((self.proj_H, self.proj_W),
                                          dtype=np.int32)  # [H,W]  label
          self.proj_inst_color = np.zeros((self.proj_H, self.proj_W, 3),
                                          dtype=np.float)  # [H,W,3] color
  
      def open_label(self, filename):
          """ Open raw scan and fill in attributes
          """
          # check filename is string
          if not isinstance(filename, str):
              raise TypeError("Filename should be string type, "
                              "but was {type}".format(type=str(type(filename))))
  
          # check extension is a laserscan
          if not any(filename.endswith(ext) for ext in self.EXTENSIONS_LABEL):
              raise RuntimeError("Filename extension is not valid label file.")
  
          # if all goes well, open label
          label = np.fromfile(filename, dtype=np.int32)
          label = label.reshape((-1))
          # label = label[self.idx]
          # set it
          self.set_label(label)
  
      def set_label(self, label):
          """ Set points for label not from file but from np
          """
          # check label makes sense
          if not isinstance(label, np.ndarray):
              raise TypeError("Label should be numpy array")
  
          # only fill in attribute if the right size
          if label.shape[0] == self.points.shape[0]:
              self.sem_label = label & 0xFFFF  # semantic label in lower half
              self.inst_label = label >> 16  # instance id in upper half
          else:
              print("Points shape: ", self.points.shape)
              print("Label shape: ", label.shape)
              raise ValueError("Scan and Label don't contain same number of points")
  
          # sanity check
          assert ((self.sem_label + (self.inst_label << 16) == label).all())
  
          if self.project:
              self.do_label_projection()
  
      def colorize(self):
          """ Colorize pointcloud with the color of each semantic label
          """
          self.sem_label_color = self.sem_color_lut[self.sem_label]
          self.sem_label_color = self.sem_label_color.reshape((-1, 3))
  
          self.inst_label_color = self.inst_color_lut[self.inst_label]
          self.inst_label_color = self.inst_label_color.reshape((-1, 3))
  
      def do_label_projection(self):
          # only map colors to labels that exist
          mask = self.proj_idx >= 0
  
          # semantics
          self.proj_sem_label[mask] = self.sem_label[self.proj_idx[mask]]
          self.proj_sem_color[mask] = self.sem_color_lut[self.sem_label[self.proj_idx[mask]]]
  
          # instances
          self.proj_inst_label[mask] = self.inst_label[self.proj_idx[mask]]
          self.proj_inst_color[mask] = self.inst_color_lut[self.inst_label[self.proj_idx[mask]]]
  