├── .gitignore ├── Calibration ├── README.md ├── app_calib.py ├── extract_frame_from_bag.py └── src │ ├── calib_lidar_camera.py │ ├── camera.py │ ├── colorize_all.bat │ ├── colorize_pc.py │ ├── extract_chessboard.py │ └── lidar.py ├── Preprocessing ├── README.md ├── camera_trajectory.py ├── extract_images_from_bag.py └── src │ ├── pose.py │ ├── trajectory.py │ ├── transform.py │ └── utils.py ├── README.md ├── Rendering ├── README.md ├── render.py ├── shaders │ ├── fragment.c │ ├── fragment_circle.c │ ├── fragment_ellipsis.c │ ├── fragment_ellipsis_turbo.c │ ├── fragment_viridis_intensity.c │ ├── vertex.c │ └── vertex_intensity.c └── src │ ├── __init__.py │ ├── frameManager.py │ ├── pose.py │ ├── renderApp.py │ ├── trajectory.py │ ├── transform.py │ └── utils.py └── assets └── paper_concept.png /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/latest/usage/project/#working-with-version-control 110 | .pdm.toml 111 | .pdm-python 112 | .pdm-build/ 113 | 114 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 115 | __pypackages__/ 116 | 117 | # Celery stuff 118 | celerybeat-schedule 119 | celerybeat.pid 120 | 121 | # SageMath parsed files 122 | *.sage.py 123 | 124 | # Environments 125 | .env 126 | .venv 127 | env/ 128 | venv/ 129 | ENV/ 130 | env.bak/ 131 | venv.bak/ 132 | 133 | # Spyder project settings 134 | .spyderproject 135 | .spyproject 136 | 137 | # Rope project settings 138 | .ropeproject 139 | 140 | # mkdocs documentation 141 | /site 142 | 143 | # mypy 144 | .mypy_cache/ 145 | .dmypy.json 146 | dmypy.json 147 | 148 | # Pyre type checker 149 | .pyre/ 150 | 151 | # pytype static type analyzer 152 | .pytype/ 153 | 154 | # Cython debug symbols 155 | cython_debug/ 156 | 157 | # PyCharm 158 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 159 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 160 | # and can be added to the global gitignore or merged into this file. For a more nuclear 161 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 162 | #.idea/ 163 | -------------------------------------------------------------------------------- /Calibration/README.md: -------------------------------------------------------------------------------- 1 | # Calibration 2 | The purpose of the calibration is to obtain simultaneously the intrisic matrix of the camera and the extrinsic calibration between the LiDAR and the camera. 3 | 4 | ## Hardware 5 | To perform this calibration, you need a camera, a LiDAR and a [large checkerboard target](https://github.com/opencv/opencv/blob/4.x/doc/pattern.png). 6 | You'll need to perform a calibration process by recording multiple images of the checkerboard in front of the sensors with various locations and orientations. 7 | 8 | ## Using Ouster LiDAR 9 | 10 | 11 | If you have a Ouster LiDAR and a USB camera you can use our app_calib.py to perform a real-time recording of synchronized calibration frame of the two sensors. Start the tool using: 12 | 13 | ```bash 14 | python app_calib.py -d [video_device] -l [lidar_hostname] -p [lidar_port] -m [checkerboard_square_size] -c [checkerboard_size] 15 | ``` 16 | 17 | Then, you can click or use "Space" to trigger a recording, it will take a picture when it detect the checkerboard. 18 | 19 | ## From Bag 20 | 21 | If you don't have a Ouster, prefer recording a bag with both your LiDAR and camera and run our tool to choose and extract relevant frames. 22 | 23 | ```bash 24 | python extract_frame_from_bag.py [bag_file] [topic_im] [topic_lidar] [output_dir] 25 | ``` 26 | 27 | You can start/stop using "Space", use the arrows to navigate, press "s" to extract the current frame. 28 | 29 | ## Pick LiDAR planes 30 | 31 | After your frames has been extracted, use the following tool to extract LiDAR frames. 32 | 33 | ```bash 34 | python app_calib.py --pick-planes 35 | ``` 36 | 37 | You must click on the target plane (one point) using "Shift+Click" and then type "Echap" for each extracted frames. 38 | 39 | ## Compute calibration 40 | 41 | Finally, you can use the following commands to compute both calibration: 42 | 43 | ```bash 44 | python app_calib.py --compute-intrinsic 45 | ``` 46 | 47 | 48 | ```bash 49 | python app_calib.py --compute-extrinsic 50 | ``` 51 | 52 | Congrats ! -------------------------------------------------------------------------------- /Calibration/app_calib.py: -------------------------------------------------------------------------------- 1 | import cv2 as cv 2 | import argparse 3 | from src.camera import CameraOpenCV 4 | from src.lidar import LidarOuster 5 | from src.colorize_pc import colorize,depth 6 | 7 | class AppCalib: 8 | 9 | def __init__(self,camera,lidar,calibration_filename): 10 | self.camera = camera 11 | self.lidar = lidar 12 | self.calibration_filename = calibration_filename 13 | 14 | self.record = False 15 | self.display_help = True 16 | 17 | self.use_lidar = True 18 | if(type(lidar)==type(None)): 19 | print("Starting without lidar") 20 | self.use_lidar = False 21 | 22 | self.print_help() 23 | 24 | #Switch between display mode and record mode 25 | def toggle_record(self, event=None, x=None, y=None, flags=None, param=None): 26 | if event == cv.EVENT_LBUTTONDOWN or event == None: 27 | self.record = not self.record 28 | 29 | def print_help(self): 30 | print("Click or Space : Capture Frame") 31 | print("c : Calibrate") 32 | print("u : Undistort first captured frame") 33 | print("r : Undistort current frame") 34 | print("p : Pick planes in lidar scans") 35 | print("l : Compute extrinsic lidar and camera matrix") 36 | print("t : Try a point cloud colorization from calibration") 37 | print("d : Try a depth esimation from calibration") 38 | print("h : Show this help") 39 | print("Escape : Exit App") 40 | 41 | def show_help(self,frame): 42 | font = cv.FONT_HERSHEY_SIMPLEX 43 | fontScale = 0.5 44 | color = (0, 0, 0) 45 | thickness = 1 46 | frame = cv.putText(frame, 'Click or Space : Capture Frame', (5,15), font, fontScale, color, thickness, cv.LINE_AA) 47 | frame = cv.putText(frame, 'c : Calibrate Camera', (5,30), font, fontScale, color, thickness, cv.LINE_AA) 48 | frame = cv.putText(frame, 'u : Undistort first captured frame', (5,45), font, fontScale, color, thickness, cv.LINE_AA) 49 | frame = cv.putText(frame, 'r : Undistort current frame', (5,60), font, fontScale, color, thickness, cv.LINE_AA) 50 | frame = cv.putText(frame, 'p : Pick planes in lidar scans', (5,75), font, fontScale, color, thickness, cv.LINE_AA) 51 | frame = cv.putText(frame, 'l : Compute extrinsic lidar and camera matrix', (5,92), font, fontScale, color, thickness, cv.LINE_AA) 52 | frame = cv.putText(frame, 't : Try a point cloud colorization from calibration', (5,107), font, fontScale, color, thickness, cv.LINE_AA) 53 | frame = cv.putText(frame, 'd : Try a depth esimation from calibration', (5,122), font, fontScale, color, thickness, cv.LINE_AA) 54 | frame = cv.putText(frame, 'h : Show/Hide this help', (5,137), font, fontScale, color, thickness, cv.LINE_AA) 55 | frame = cv.putText(frame, 'Escape : Exit app', (5,152), font, fontScale, color, thickness, cv.LINE_AA) 56 | return frame 57 | 58 | 59 | def show_frame(self): 60 | showframe = cv.resize(self.camera.get_frame(), None, fx=0.8, fy=0.8, interpolation=cv.INTER_AREA) 61 | if self.display_help: 62 | showframe = self.show_help(showframe) 63 | cv.imshow('Record Calibration', showframe) 64 | cv.setMouseCallback('Record Calibration', self.toggle_record, [0]) 65 | 66 | def compare_frame(self,dst,ori): 67 | cv.imshow("Undistorded Image", dst) 68 | cv.imshow("Original Image", ori) 69 | 70 | def record_frame(self): 71 | retchess, corners = self.camera.find_chessboard(fast=True) 72 | # If found, add object points, image points (after refining them) 73 | if retchess == True: 74 | nb = self.camera.get_next_file_nb() 75 | self.camera.save(nb) 76 | print("Saving at nb = ", nb) 77 | if self.use_lidar: 78 | self.lidar.save(nb) 79 | self.camera.draw_chessboard() 80 | self.show_frame() 81 | cv.waitKey(1000) 82 | self.record = False 83 | 84 | def user_action(self): 85 | c = cv.waitKey(1) 86 | if c == ord(' '): 87 | self.toggle_record() 88 | return True 89 | if c == ord('c'): 90 | self.camera.calibrate(self.calibration_filename) 91 | self.show_frame() 92 | return True 93 | if c == ord('u'): 94 | dst,ori = self.camera.undistort() 95 | self.compare_frame(dst,ori) 96 | self.show_frame() 97 | if c == ord('r'): 98 | dst,ori = self.camera.undistort(True) 99 | self.compare_frame(dst,ori) 100 | self.show_frame() 101 | return True 102 | if c == ord('p'): 103 | self.lidar.pick_planes() 104 | self.show_frame() 105 | return True 106 | if c == ord('l'): 107 | self.lidar.calib_lidar_camera(self.calibration_filename) 108 | self.show_frame() 109 | return True 110 | if c == ord('t'): 111 | self.camera.sample() 112 | self.lidar.sample() 113 | colorize(self.lidar.get_pcd(),self.camera.get_frame(),self.calibration_filename,"calib_lidar_ref_cam.txt",None,True) 114 | self.show_frame() 115 | return True 116 | if c == ord('d'): 117 | self.camera.sample() 118 | self.lidar.sample() 119 | depth(self.lidar.get_pcd(),self.camera.get_frame(),self.calibration_filename,"calib_lidar_ref_cam.txt") 120 | self.show_frame() 121 | return True 122 | if c == ord('h'): 123 | self.display_help = not self.display_help 124 | self.print_help() 125 | return True 126 | if c == 27: #Escape 127 | return False 128 | return True 129 | 130 | def run(self): 131 | self.camera.sample() 132 | self.show_frame() 133 | 134 | while True: 135 | self.camera.sample() 136 | self.show_frame() 137 | if self.record: 138 | if self.use_lidar: 139 | self.lidar.sample() 140 | self.record_frame() 141 | 142 | if not self.user_action(): 143 | break 144 | 145 | if cv.getWindowProperty('Record Calibration', cv.WND_PROP_VISIBLE) < 1: # window closed 146 | break 147 | 148 | 149 | self.camera.release_cam() 150 | cv.destroyAllWindows() 151 | 152 | def compute_intrinsic(self): 153 | self.camera.calibrate(self.calibration_filename) 154 | 155 | def pick_planes(self): 156 | self.lidar.pick_planes() 157 | 158 | def compute_extrinsic(self): 159 | self.lidar.calib_lidar_camera(self.calibration_filename) 160 | 161 | 162 | def parseProgramArgs(): 163 | parser = argparse.ArgumentParser(description='LiDAR - Camera extrinsic calibration.') 164 | parser.add_argument('-d', '--video-device', help='OpenCV camera device number', default=4) 165 | parser.add_argument('-W', '--width', help='Camera image width', default=1920) 166 | parser.add_argument('-H', '--height', help='Camera image height', default=1080) 167 | parser.add_argument('-i', '--img_dir', help='Camera image output directory', default='images') 168 | parser.add_argument('-s', '--scan_dir', help='Lidar scan output directory', default='lidar_scans') 169 | parser.add_argument('-l', '--lidar-hostname', help='Lidar hostname', default='os-122320000354.local') 170 | parser.add_argument('-p', '--lidar-port', help='Lidar port', default=7502) 171 | parser.add_argument('-m', '--square-size', help='Size of square in chessboard (mm)', default=117.2) 172 | parser.add_argument('-n', '--no-lidar', help="Don't use lidar", action='store_true') 173 | parser.add_argument('-c', '--chessboard-size', help="Number of chessboard corners w,h. Example : -c 9 6", nargs='+', type=int, default=[9,6]) 174 | parser.add_argument('-o', '--calibration-filename', help="Output camera calibration file", default='calib.json') 175 | parser.add_argument('--compute-intrinsic', help="Only compute camera intrinsic from files", action="store_true") 176 | parser.add_argument('--pick-planes', help="Only pick lidar planes from files", action="store_true") 177 | parser.add_argument('--compute-extrinsic', help="Only compute lidar camera extrinsic from files", action="store_true") 178 | 179 | return parser.parse_args() 180 | 181 | 182 | if __name__ == "__main__": 183 | args = parseProgramArgs() 184 | 185 | no_lidar = args.no_lidar 186 | if(args.compute_intrinsic or args.pick_planes or args.compute_extrinsic): 187 | camera = CameraOpenCV(None,args.width,args.height,args.img_dir,args.chessboard_size, args.square_size) 188 | no_lidar = True 189 | else: 190 | camera = CameraOpenCV(args.video_device,args.width,args.height,args.img_dir,args.chessboard_size,args.square_size) 191 | 192 | lidar = None 193 | if(not no_lidar): 194 | print("Lidar port", args.lidar_port) 195 | lidar = LidarOuster(args.scan_dir,args.lidar_hostname,args.lidar_port) 196 | else: 197 | lidar = LidarOuster(args.scan_dir,None,None) 198 | 199 | app = AppCalib(camera,lidar,args.calibration_filename) 200 | 201 | 202 | if(args.compute_intrinsic): 203 | app.compute_intrinsic() 204 | elif(args.compute_extrinsic): 205 | app.compute_extrinsic() 206 | elif(args.pick_planes): 207 | app.pick_planes() 208 | else: 209 | app.run() 210 | -------------------------------------------------------------------------------- /Calibration/extract_frame_from_bag.py: -------------------------------------------------------------------------------- 1 | import os 2 | import rosbag 3 | import cv2 4 | from cv_bridge import CvBridge 5 | import numpy as np 6 | import argparse 7 | import glob 8 | from plyfile import PlyData, PlyElement 9 | 10 | 11 | def writePly(filename, pc): 12 | has_i = False 13 | if pc.shape[1] == 4: 14 | has_i = True 15 | 16 | if has_i: 17 | vertex = np.zeros(len(pc), dtype=[('x', 'f4'), ('y', 'f4'), ('z', 'f4'), ('intensity','f4')]) 18 | vertex['intensity'] = pc[:,3] 19 | else: 20 | vertex = np.zeros(len(pc), dtype=[('x', 'f4'), ('y', 'f4'), ('z', 'f4')]) 21 | vertex['x'] = pc[:,0] 22 | vertex['y'] = pc[:,1] 23 | vertex['z'] = pc[:,2] 24 | el = PlyElement.describe(vertex, 'vertex') 25 | PlyData([el]).write(filename) 26 | 27 | def extract_one_image(bag_file, topic, output_dir, t): 28 | theora = topic.endswith("compressed") 29 | output_dir = os.path.join(output_dir, "images") 30 | bag = rosbag.Bag(bag_file, 'r') 31 | bridge = CvBridge() 32 | count = len(glob.glob1(output_dir, "*.jpg")) 33 | try: 34 | topic, msg, t = next(bag.read_messages(topics=[topic], start_time=t)) 35 | except StopIteration: 36 | print("No messages in topic %s" % topic) 37 | 38 | cv_image = get_im(msg, bridge, theora) 39 | filename = os.path.join(output_dir, "%i.jpg" % count) 40 | cv2.imwrite(filename, cv_image) 41 | 42 | bag.close() 43 | return cv_image 44 | 45 | def extract_one_lidar(bag_file, topic, output_dir, t): 46 | output_dir = os.path.join(output_dir, "lidar_scans") 47 | bag = rosbag.Bag(bag_file, 'r') 48 | count = len(glob.glob1(output_dir, "*.ply")) 49 | try: 50 | topic, msg, t = next(bag.read_messages(topics=[topic], start_time=t)) 51 | except StopIteration: 52 | print("No messages in topic %s" % topic) 53 | 54 | pc = read_pc_msg(msg) 55 | 56 | filename = os.path.join(output_dir, "%i.ply" % count) 57 | writePly(filename, pc) 58 | 59 | bag.close() 60 | return pc 61 | 62 | def read_pc_msg(msg): 63 | # convert to numpy array msg.data 64 | point_step = msg.point_step 65 | offset_xyz = [0, 4, 8] 66 | offset_intensity = -1 67 | #print(msg.fields) 68 | for f in msg.fields: 69 | if f.name == "x": 70 | offset_xyz[0] = f.offset 71 | elif f.name == "y": 72 | offset_xyz[1] = f.offset 73 | elif f.name == "z": 74 | offset_xyz[2] = f.offset 75 | elif f.name == "intensity": 76 | offset_intensity = f.offset 77 | pc = [] 78 | nb_points = int(len(msg.data) / point_step) 79 | for i in range(nb_points): 80 | offset = i * point_step 81 | x = np.frombuffer(msg.data, dtype=np.float32, count=1, offset=offset + offset_xyz[0]) 82 | y = np.frombuffer(msg.data, dtype=np.float32, count=1, offset=offset + offset_xyz[1]) 83 | z = np.frombuffer(msg.data, dtype=np.float32, count=1, offset=offset + offset_xyz[2]) 84 | if offset_intensity > 0: 85 | i = np.frombuffer(msg.data, dtype=np.float32, count=1, offset=offset + offset_intensity) 86 | pc.append([x, y, z, i]) 87 | else: 88 | pc.append([x,y,z]) 89 | 90 | pc = np.array(pc).reshape(-1, len(pc[0])) 91 | return pc 92 | 93 | 94 | def get_im(msg, bridge, theora=False): 95 | if theora: 96 | np_arr = np.fromstring(msg.data, np.uint8) 97 | cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) 98 | else: 99 | cv_image = bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") 100 | return cv_image 101 | 102 | def get_lidar_im(msg): 103 | pc = read_pc_msg(msg) 104 | #Fake intrinsics TODO: find a general way to handle projection 105 | K = np.array([[1614.5606189613611, 0.0, 940.140768866003], 106 | [0.0, 1614.303435111037, 523.0433416805166], 107 | [0.0, 0.0, 1.0]]) 108 | T = np.array([[0.0, -1.0, 0.0], 109 | [0.0, 0.0, -1.0], 110 | [1.0, 0.0, 0.0]]) 111 | intensity = None 112 | if pc.shape[1] == 4: # intensity 113 | intensity = pc[:,3] 114 | pc = pc[:,:3] 115 | 116 | # Project to image plane 117 | px = (T @ pc.T).T 118 | px = (K @ px.T).T 119 | 120 | # Get normalized coordinates 121 | z = px[:,2] 122 | px = px / z.reshape(-1, 1) 123 | mask = z > 0 124 | px = px[mask] 125 | z = z[mask] 126 | if intensity is not None: 127 | intensity = intensity[mask] 128 | 129 | # Filter out points outside of image 130 | px = px[:, :2].astype(np.int32) 131 | mask = (px[:,0] >= 0) & (px[:,0] < 1920) & (px[:,1] >= 0) & (px[:,1] < 1080) 132 | px = px[mask] 133 | z = z[mask] 134 | if intensity is not None: 135 | intensity = intensity[mask] 136 | 137 | v_max = z.max() if intensity is None else intensity.max() 138 | im = np.ones((1080, 1920), dtype=np.uint8) * 255 139 | 140 | for i in range(-1,2): # 3x3 kernel for densification 141 | for j in range(-1,2): 142 | coor = px + np.array([i, j]) 143 | mask = (coor[:,0] >= 0) & (coor[:,0] < 1920) & (coor[:,1] >= 0) & (coor[:,1] < 1080) 144 | coor = coor[mask] 145 | z_d = z[mask] 146 | if intensity is not None: 147 | intensity_d = intensity[mask] 148 | v = (intensity_d / v_max * 255).astype(np.uint8) 149 | else: 150 | v = (z_d / v_max * 255).astype(np.uint8) 151 | 152 | im[coor[:,1], coor[:,0]] = np.minimum(im[coor[:,1], coor[:,0]], v) 153 | 154 | im[im == 255] = 0 155 | im = im.astype(np.uint8) 156 | im = cv2.applyColorMap(im, cv2.COLORMAP_VIRIDIS) 157 | 158 | return im 159 | 160 | 161 | 162 | 163 | def show_frames(bag_file, topic_im,topic_lidar, output_dir, show_lidar): 164 | bag = rosbag.Bag(bag_file, 'r') 165 | count = 0 166 | if not show_lidar: 167 | bridge = CvBridge() 168 | theora = topic_im.endswith("compressed") 169 | topic = topic_im 170 | iter = bag.read_messages(topics=[topic]) 171 | else: 172 | topic = topic_lidar 173 | iter = bag.read_messages(topics=[topic]) 174 | 175 | ts = [] 176 | pause = True 177 | 178 | while True: 179 | try: 180 | topic, msg, t = next(iter) 181 | if len(ts)==0 or t>ts[-1]: 182 | ts.append(t) 183 | except StopIteration: 184 | break 185 | 186 | if not show_lidar: 187 | cv_image = get_im(msg, bridge, theora) 188 | else: 189 | cv_image = get_lidar_im(msg) 190 | cv2.imshow('ExtractFrames', cv_image) 191 | if not pause: 192 | key = cv2.waitKey(1) 193 | else: 194 | key = cv2.waitKey(100) 195 | while key==-1: 196 | key = cv2.waitKey(100) 197 | if cv2.getWindowProperty('ExtractFrames', cv2.WND_PROP_VISIBLE) < 1: # window closed 198 | break 199 | if key == 27: # escape key 200 | break 201 | elif key == 32: # space bar 202 | pause = not pause 203 | elif key == 81: # left arrow key 204 | iter = bag.read_messages(topics=[topic], start_time=ts[count-1]) 205 | count -= 1 206 | continue 207 | elif key == 115: # 's' key 208 | print("Save frame %i" % count) 209 | extract_one_image(bag_file, topic_im, output_dir, ts[count]) 210 | extract_one_lidar(bag_file, topic_lidar, output_dir, ts[count]) 211 | count += 1 212 | 213 | if cv2.getWindowProperty('ExtractFrames', cv2.WND_PROP_VISIBLE) < 1: # window closed 214 | break 215 | bag.close() 216 | 217 | 218 | if __name__ == "__main__": 219 | parser = argparse.ArgumentParser(description='Extract frames from a ROS bag.') 220 | parser.add_argument('bag_file', help='input ROS bag') 221 | parser.add_argument('topic_im', help='topic image to extract') 222 | parser.add_argument('topic_lidar', help='topic lidar to extract') 223 | parser.add_argument('output_dir', help='output directory') 224 | parser.add_argument('--show-lidar', action='store_true', help='Show images from lidar scans instead of camera') 225 | args = parser.parse_args() 226 | 227 | os.makedirs(args.output_dir, exist_ok=True) 228 | os.makedirs(args.output_dir + "/images", exist_ok=True) 229 | os.makedirs(args.output_dir + "/lidar_scans", exist_ok=True) 230 | 231 | show_frames(args.bag_file, args.topic_im,args.topic_lidar ,args.output_dir, args.show_lidar) 232 | -------------------------------------------------------------------------------- /Calibration/src/calib_lidar_camera.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import glob 3 | import json 4 | import natsort 5 | 6 | import cv2 as cv 7 | import numpy as np 8 | from scipy.spatial.transform import Rotation 9 | 10 | import math 11 | 12 | # from pdm.transform import * 13 | 14 | 15 | def translation(T): 16 | """ 17 | Returns translation vector block from SE3 (R|t) 4x4 matrix. 18 | """ 19 | return T[0:3, 3] 20 | 21 | def rotation(T): 22 | """ 23 | Returns rotation matrix block vector from SE3 (R|t) 4x4 matrix. 24 | """ 25 | return T[0:3, 0:3] 26 | 27 | 28 | # x.n = d plane 29 | class Plane: 30 | def __init__(self, n=np.zeros(3), d=0.0, p=None): 31 | # Normal vector 32 | self.n_ = n 33 | # Distance to origin 34 | self.d_ = d 35 | # Point that belongs to the plane 36 | self.p_ = p 37 | 38 | @staticmethod 39 | def fromRigidTransform(T, orient_normal_toward_sensor=True): 40 | """ 41 | Builds a plane from an input rigid transform. Normal vector is assumed 42 | to be Z axis from the rotation matrix. 43 | """ 44 | n = rotation(T)[:, 2] # normal is the z axis of the rotation matrix. 45 | p = translation(T) 46 | d = np.dot(n, p) 47 | 48 | if orient_normal_toward_sensor and (d > 0.0): 49 | n = -n 50 | d = -d 51 | 52 | return Plane(n, d, p) 53 | 54 | 55 | @staticmethod 56 | def fromNormalPoint(n,p, orient_normal_toward_sensor=True): 57 | """ 58 | Builds a plane from an input rigid transform. Normal vector is assumed 59 | to be Z axis from the rotation matrix. 60 | """ 61 | d = np.dot(n, p) 62 | 63 | if orient_normal_toward_sensor and (d > 0.0): 64 | n = -n 65 | d = -d 66 | 67 | return Plane(n, d, p) 68 | 69 | 70 | class CalibLiDARCamera: 71 | def __init__(self): 72 | self.opencv_calib_ = None 73 | self.cam_mtx_ = None 74 | self.cam_dist_ = None 75 | 76 | self.planes_cam_ = [] 77 | self.planes_lidar_ = [] 78 | 79 | self.max_iter_ = 100 80 | self.residuals_n_ = [] 81 | self.residuals_p_ = [] 82 | self.C_ = np.identity(4) 83 | 84 | 85 | def readOpenCVIntrinsics(self, src_fpath): 86 | with open(src_fpath, 'r') as f: 87 | self.opencv_calib_ = json.load(f) 88 | 89 | self.cam_mtx_ = self.opencv_calib_['mtx'] 90 | self.cam_dist_ = self.opencv_calib_['dist'] 91 | rvecs = self.opencv_calib_['rvecs'] 92 | tvecs = self.opencv_calib_['tvecs'] 93 | 94 | N = len(rvecs) 95 | 96 | for i in range(N): 97 | r = np.array(rvecs[i])[:,0] 98 | t_vec = 1e-3 * np.array(tvecs[i])[:,0] # default unit is mm 99 | r_mat = cv.Rodrigues(r)[0] 100 | T = np.identity(4) 101 | T[0:3, 0:3] = r_mat 102 | T[0:3, 3] = t_vec 103 | self.planes_cam_.append(Plane.fromRigidTransform(T, True)) 104 | 105 | print('Read OpenCV intrinsics') 106 | print('num samples: {}\n'.format(N)) 107 | 108 | 109 | def readLidarPlanePoses(self, src_dir): 110 | fpaths = natsort.natsorted(glob.glob(src_dir + '/*.plane.txt')) 111 | 112 | 113 | for fpath in fpaths: 114 | n,p = np.loadtxt(fpath) 115 | self.planes_lidar_.append(Plane.fromNormalPoint(n,p, True)) 116 | 117 | print('Read extracted LiDAR planes') 118 | print('num planes: {}\n'.format(len(self.planes_lidar_))) 119 | 120 | 121 | def computeExtrinsicCalibration(self): 122 | N = len(self.planes_lidar_) 123 | 124 | if len(self.planes_cam_) != N: 125 | raise RuntimeError('Mismatch between camera and lidar number input files') 126 | 127 | # 1. Compute rotation 128 | #----------------------------------------------------------------------# 129 | self.optimizeRotationSVD() 130 | 131 | # Compute optimization residuals 132 | R_opt = rotation(self.C_) 133 | 134 | for i in range(N): 135 | n_cam = self.planes_cam_[i].n_ 136 | n_lidar = self.planes_lidar_[i].n_ 137 | sin_angle = np.linalg.norm(np.cross(n_lidar, R_opt @ n_cam)) 138 | angle = 180.0 / math.pi * math.asin(sin_angle) 139 | self.residuals_n_.append(angle) 140 | 141 | self.residuals_n_ = np.array(self.residuals_n_) 142 | 143 | print('Computed extrinsic rotation') 144 | print('roll pitch yaw (deg): {}'.format(Rotation.from_matrix(rotation(self.C_)).as_euler('xyz', degrees=True))) 145 | print('residuals mean: {} deg'.format(np.mean(self.residuals_n_))) 146 | print('residuals sigma: {} deg\n'.format(np.std(self.residuals_n_))) 147 | 148 | # 2. Compute translation 149 | #----------------------------------------------------------------------# 150 | self.optimizeTransaltion() 151 | 152 | # Compute optimization residuals 153 | u_opt = self.C_[0:3, 3] 154 | 155 | for i in range(N): 156 | p_cam = self.planes_cam_[i].p_ 157 | n_lidar = self.planes_lidar_[i].n_ 158 | d_lidar = self.planes_lidar_[i].d_ 159 | 160 | dist_point_to_plane = np.dot(n_lidar, R_opt @ p_cam + u_opt) - d_lidar 161 | self.residuals_p_.append(dist_point_to_plane) 162 | 163 | self.residuals_p_ = np.array(self.residuals_p_) 164 | 165 | print('Computed extrinsic translation') 166 | print('u (cm): {}'.format(100.0 * u_opt)) 167 | print('residuals mean: {} cm'.format(100.0 * np.mean(self.residuals_p_))) 168 | print('residuals sigma: {} cm\n'.format(100.0 * np.std(self.residuals_p_))) 169 | 170 | 171 | def optimizeRotationSVD(self): 172 | N = len(self.planes_lidar_) 173 | 174 | normals_cam = np.zeros((N, 3)) 175 | normals_lidar = np.zeros((N, 3)) 176 | 177 | for i in range(N): 178 | normals_cam[i,:] = self.planes_cam_[i].n_ 179 | normals_lidar[i,:] = self.planes_lidar_[i].n_ 180 | 181 | Cov = normals_cam.T @ normals_lidar 182 | 183 | U, s, Vh = np.linalg.svd(Cov) 184 | 185 | D_unit = np.identity(3) 186 | 187 | if np.linalg.det(Vh.T @ U.T) < 0.0: 188 | D_unit[2,2] = -1 189 | 190 | R_opt = Vh.T @ D_unit @ U.T 191 | 192 | self.C_[0:3, 0:3] = R_opt 193 | 194 | 195 | def optimizeTransaltion(self): 196 | N = len(self.planes_lidar_) 197 | 198 | DIM_ERR = N 199 | DIM_VAR = 3 200 | 201 | e = np.zeros(DIM_ERR) # error vec 202 | J = np.zeros((DIM_ERR, DIM_VAR)) # jacobian vec 203 | Q = np.zeros((DIM_VAR, DIM_VAR)) # Canonical quadratic system matrix 204 | b = np.zeros(DIM_VAR) # Canonical quadratic system vec 205 | x = np.zeros(3) # optimization vector 206 | u = np.zeros(3) 207 | R = rotation(self.C_) 208 | 209 | for iter in range(self.max_iter_): 210 | e.fill(0) 211 | J.fill(0) 212 | Q.fill(0) 213 | b.fill(0) 214 | x.fill(0) 215 | 216 | # 1. build linear system 217 | #------------------------------------------------------------------# 218 | err_idx = 0 219 | 220 | for i in range(N): 221 | n_hat = self.planes_lidar_[i].n_ 222 | d_hat = self.planes_lidar_[i].d_ 223 | n = self.planes_cam_[i].n_ 224 | p = self.planes_cam_[i].p_ 225 | 226 | e_i = np.dot(n_hat, R @ p + u) - d_hat 227 | J_i = n_hat.T 228 | 229 | e[err_idx] = e_i 230 | J[err_idx, :] = J_i 231 | 232 | err_idx += 1 233 | 234 | Q = J.transpose() @ J 235 | b = J.transpose() @ e 236 | 237 | # 2. solve system 238 | #------------------------------------------------------------------# 239 | x = np.linalg.solve(Q, -b) 240 | 241 | u += x 242 | 243 | if np.linalg.norm(x) < 1e-9: 244 | print('Optimize translation') 245 | print('Convergence reached after {} iter\n'.format(iter)) 246 | self.C_[0:3, 3] = u 247 | return 248 | 249 | print('Optimize translation') 250 | print('Convergence not reached after {} iter\n'.format(self.max_iter_)) 251 | self.C_[0:3, 3] = u 252 | 253 | 254 | def saveNormalsDBG(self): 255 | N = len(self.planes_lidar_) 256 | normals = np.zeros((N, 3)) 257 | 258 | for i in range(N): normals[i,:] = self.planes_cam_[i].n_ 259 | np.savetxt("dbg_normals_cam.txt", normals, fmt='%1.10f') 260 | 261 | for i in range(N): normals[i,:] = self.planes_lidar_[i].n_ 262 | np.savetxt("dbg_normals_lidar.txt", normals, fmt='%1.10f') 263 | 264 | tvecs = np.zeros((N, 3)) 265 | 266 | for i in range(N): tvecs[i,:] = self.planes_cam_[i].p_ 267 | np.savetxt("dbg_tvecs_cam.txt", tvecs, fmt='%1.10f') 268 | 269 | 270 | def saveCalibExtrinsic(self): 271 | np.savetxt("calib_cam_ref_lidar.txt", self.C_, fmt='%1.10f') 272 | np.savetxt("calib_lidar_ref_cam.txt", np.linalg.inv(self.C_), fmt='%1.10f') 273 | 274 | print('Written calib to calib_cam_ref_lidar.txt and calib_lidar_ref_cam.txt\n') 275 | 276 | 277 | def parseProgramArgs(): 278 | parser = argparse.ArgumentParser(description='LiDAR - Camera extrinsic calibration.') 279 | parser.add_argument('-c', '--camera-calib', help='Camera instirinsics output by OpenCV', default='calib.json') 280 | parser.add_argument('-l', '--lidar-planes', help='Input directory that contains lidar planes pose matrices in .plane.txt Cloud Compare format', default='lidar_planes') 281 | return parser.parse_args() 282 | 283 | 284 | def main(): 285 | args = parseProgramArgs() 286 | 287 | calib = CalibLiDARCamera() 288 | calib.readOpenCVIntrinsics(args.camera_calib) 289 | calib.readLidarPlanePoses(args.lidar_planes) 290 | calib.computeExtrinsicCalibration() 291 | 292 | # calib.saveNormalsDBG() 293 | calib.saveCalibExtrinsic() 294 | 295 | 296 | if __name__== "__main__": 297 | main() 298 | -------------------------------------------------------------------------------- /Calibration/src/camera.py: -------------------------------------------------------------------------------- 1 | import cv2 as cv 2 | import os 3 | import natsort 4 | import glob 5 | import numpy as np 6 | import json 7 | 8 | class CameraOpenCV: 9 | def __init__(self,video_device,width,height,output_dir,chessboard_size,square_size): 10 | 11 | self.video_device = video_device 12 | self.width = width 13 | self.height = height 14 | if(type(chessboard_size)==int): 15 | chessboard_size = [chessboard_size,chessboard_size] 16 | if(len(chessboard_size)!=2): 17 | raise IOError("Please input number of corners in the chessboard") 18 | self.chessboard_size = tuple(chessboard_size) 19 | self.square_size = float(square_size) 20 | 21 | #Create output directory 22 | self.output_dir = output_dir 23 | if(not os.path.exists(output_dir)): 24 | os.makedirs(output_dir) 25 | 26 | #Init Camera Device 27 | if(self.video_device is not None): 28 | self.cap = cv.VideoCapture(self.video_device) 29 | self.cap.set(cv.CAP_PROP_FRAME_WIDTH, self.width) 30 | self.cap.set(cv.CAP_PROP_FRAME_HEIGHT, self.height) 31 | 32 | if not self.cap.isOpened(): 33 | raise IOError("Cannot open webcam") 34 | 35 | #Init attribute 36 | self.frame = None 37 | self.ret = False 38 | self.mtx = self.dist = self.rvecs = self.tvecs = None 39 | 40 | #Set criteria for subpix corner search 41 | self.criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001) 42 | 43 | 44 | 45 | #Retrieve current frame 46 | def sample(self): 47 | self.ret, self.frame = self.cap.read() 48 | 49 | #Get current frame 50 | def get_frame(self): 51 | return self.frame 52 | 53 | #Save current frame 54 | def save(self,filename): 55 | if(type(self.frame)==type(None)): 56 | print("There is no current frame") 57 | return 58 | 59 | if(type(filename)==int): 60 | filename = str(filename) 61 | 62 | if(filename==""): 63 | print("Please enter a filename") 64 | return 65 | 66 | if(not filename.endswith(".jpg")): 67 | filename += ".jpg" 68 | img_name = os.path.join(self.output_dir , filename) 69 | cv.imwrite(img_name, self.frame) 70 | 71 | #Save current frame if chessboard is 72 | def find_chessboard(self,fast=False): 73 | gray = cv.cvtColor(self.frame, cv.COLOR_BGR2GRAY) 74 | if(fast): 75 | return cv.findChessboardCorners(gray, self.chessboard_size,cv.CALIB_CB_FAST_CHECK)#, None) 76 | 77 | retchess, corners = cv.findChessboardCorners(gray, self.chessboard_size, None) 78 | corners2 = cv.cornerSubPix(gray,corners, (11,11), (-1,-1), self.criteria) 79 | return retchess,corners2 80 | 81 | def draw_chessboard(self): 82 | retchess,corners = self.find_chessboard() 83 | cv.drawChessboardCorners(self.frame, (9,6), corners, retchess) 84 | 85 | def calibrate(self,output_file): 86 | 87 | if(output_file==""): 88 | print("Please choose an output filename") 89 | return 90 | 91 | # termination criteria with high accuracy for calibration 92 | criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 100, 0.0001) 93 | 94 | print("Calibrating using square_size = ", self.square_size) 95 | #Creating the grid 96 | objp = np.zeros((self.chessboard_size[0]*self.chessboard_size[1],3), np.float32) 97 | objp[:,:2] = np.mgrid[0:self.chessboard_size[0],0:self.chessboard_size[1]].T.reshape(-1,2) * self.square_size#mm 98 | 99 | # Arrays to store object points and image points from all the images. 100 | objpoints = [] # 3d point in real world space 101 | imgpoints = [] # 2d points in image plane. 102 | 103 | images = natsort.natsorted(glob.glob(os.path.join(self.output_dir,"*.jpg"))) 104 | 105 | if(len(images)==0): 106 | print("You have no image for the calibration") 107 | return 108 | 109 | #Compute corners in each frames 110 | for fname in images: 111 | img = cv.imread(fname) 112 | gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) 113 | # Find the chess board corners 114 | ret, corners = cv.findChessboardCorners(gray, self.chessboard_size, None) 115 | # If found, add object points, image points (after refining them) 116 | if ret == True: 117 | objpoints.append(objp) 118 | corners2 = cv.cornerSubPix(gray,corners, (11,11), (-1,-1), criteria) 119 | imgpoints.append(corners2) 120 | # Draw and display the corners 121 | cv.drawChessboardCorners(img, self.chessboard_size, corners2, ret) 122 | cv.imshow('Calibration', img) 123 | cv.waitKey(500) 124 | else: 125 | print("Chessboard not found in frame : ", fname) 126 | cv.destroyAllWindows() 127 | 128 | #Compute and show calibration values 129 | ret, self.mtx, self.dist, self.rvecs, self.tvecs = cv.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None) 130 | print("Calibration Done: ") 131 | print("="*20) 132 | print("ret :", self.ret) 133 | 134 | print("mtx :", self.mtx) 135 | print("dist :", self.dist) 136 | 137 | #Save Calibration 138 | calib = {"ret":self.ret,"mtx":self.mtx.tolist(),"dist":self.dist.tolist(),"rvecs":np.array(self.rvecs).tolist(),"tvecs":np.array(self.tvecs).tolist()} 139 | with open(output_file, 'w') as f: 140 | json.dump(calib, f) 141 | 142 | #Compute error 143 | mean_error = 0 144 | for i in range(len(objpoints)): 145 | imgpoints2, _ = cv.projectPoints(objpoints[i], self.rvecs[i], self.tvecs[i], self.mtx, self.dist) 146 | error = cv.norm(imgpoints[i], imgpoints2, cv.NORM_L2)/len(imgpoints2) 147 | mean_error += error 148 | print( "Total error: {}".format(mean_error/len(objpoints)) ) 149 | print("="*20) 150 | 151 | def undistort(self,capture=False): 152 | #Verify a calibration has been made 153 | if type(self.mtx) == type(None): 154 | print("You need to calibrate first") 155 | return 156 | #Make a new frame capture or retrieve one from directory 157 | if capture: 158 | self.sample() 159 | img = self.frame 160 | else: 161 | images = natsort.natsorted(glob.glob(os.path.join(self.output_dir,"*.jpg"))) 162 | img = cv.imread(images[0]) 163 | h, w = img.shape[:2] 164 | 165 | #Get new camera matrix 166 | newcameramtx, roi = cv.getOptimalNewCameraMatrix(self.mtx, self.dist, (w,h), 1, (w,h)) 167 | 168 | # # undistort 169 | dst = cv.undistort(img, self.mtx, self.dist, None, newcameramtx) 170 | # # crop the image 171 | x, y, w, h = roi 172 | dst = dst[y:y+h, x:x+w] 173 | 174 | return dst,img 175 | 176 | 177 | def release_cam(self): 178 | self.cap.release() 179 | 180 | def get_next_file_nb(self): 181 | 182 | last_files = natsort.natsorted(glob.glob(os.path.join(self.output_dir,"*.jpg"))) 183 | if len(last_files)==0: 184 | return 1 185 | return int(os.path.basename(last_files[-1]).split(".")[0])+1 186 | -------------------------------------------------------------------------------- /Calibration/src/colorize_all.bat: -------------------------------------------------------------------------------- 1 | for /l %%i in (1, 1, 20) do ( 2 | python colorize_pc.py ..\lidar_scans\%%i.ply ..\images\%%i.jpg -o ..\lidar_scans_colorized\%%i.ply 3 | ) -------------------------------------------------------------------------------- /Calibration/src/colorize_pc.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import glob 3 | import json 4 | import natsort 5 | import open3d as o3d 6 | 7 | import cv2 as cv 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | 11 | class CameraModel: 12 | def __init__(self): 13 | self.opencv_calib_ = None 14 | self.mtx_ = None 15 | self.dist_ = None 16 | 17 | self.new_mtx_ = None 18 | self.roi_ = None 19 | 20 | @staticmethod 21 | def fromOpenCVIntrinsics(src_fpath): 22 | model = CameraModel() 23 | 24 | with open(src_fpath, 'r') as f: 25 | model.opencv_calib_ = json.load(f) 26 | 27 | model.mtx_ = np.array(model.opencv_calib_['mtx']) 28 | model.dist_ = np.array(model.opencv_calib_['dist']) 29 | 30 | print('Read camera intrinsics from {}'.format(src_fpath)) 31 | print('mtx:\n{}\n'.format(model.mtx_)) 32 | 33 | return model 34 | 35 | def computeOptimalMTXAndUndistort(self, src_img): 36 | h, w = src_img.shape[:2] 37 | self.new_mtx_, self.roi_ = cv.getOptimalNewCameraMatrix(self.mtx_, self.dist_, (w,h), 1, (w,h)) 38 | 39 | dst_img = cv.undistort(src_img, self.mtx_, self.dist_, None, self.new_mtx_) 40 | x, y, w, h = self.roi_ 41 | dst_img = dst_img[y:y+h, x:x+w] 42 | 43 | print('Computed optimal MTX and rectified image') 44 | print('roi: {} {} {} {}'.format(x,y,w,h)) 45 | print('new mtx:\n{}\n'.format(self.new_mtx_)) 46 | 47 | return dst_img 48 | 49 | 50 | def parseProgramArgs(): 51 | parser = argparse.ArgumentParser(description='LiDAR - Camera extrinsic calibration.') 52 | parser.add_argument('pc', help='input PC') 53 | parser.add_argument('img', help='Input image') 54 | parser.add_argument('-i', '--intrinsic', help='Camera instirinsics output by OpenCV in JSON format', default='calib.json') 55 | parser.add_argument('-e', '--extrinsic', help='Extrinsic calibration: LiDAR pose in camera frame', default='calib_lidar_ref_cam.txt') 56 | parser.add_argument('-v', '--vis', help='Vizualisation', action="store_true") 57 | parser.add_argument('-o', '--dst', help='Output file') 58 | return parser.parse_args() 59 | 60 | def colorize_from_file(pc,img,intrinsic,extrinsic,dst,visu=False): 61 | # 1. Read PC 62 | #--------------------------------------------------------------------------# 63 | pcd = o3d.io.read_point_cloud(pc) 64 | # 2. Read image 65 | #--------------------------------------------------------------------------# 66 | img = cv.imread(img) 67 | colorize(pcd,img,intrinsic,extrinsic,dst,visu) 68 | 69 | 70 | def colorize(pcd,img,intrinsic,extrinsic,dst,visu=False): 71 | 72 | # 1. Read PC 73 | #--------------------------------------------------------------------------# 74 | pcd.paint_uniform_color(np.ones(3)) 75 | 76 | print('Read point cloud.') 77 | print('num points: {}\n'.format(len(pcd.points))) 78 | 79 | points = np.asarray(pcd.points) 80 | colors = np.asarray(pcd.colors) 81 | 82 | 83 | # 2. Read image 84 | #--------------------------------------------------------------------------# 85 | print('Read image') 86 | print('shape: {}\n'.format(img.shape)) 87 | 88 | 89 | # 3. Read camera model and rectify iamge 90 | #--------------------------------------------------------------------------# 91 | cam_model = CameraModel.fromOpenCVIntrinsics(intrinsic) 92 | 93 | img = cam_model.computeOptimalMTXAndUndistort(img) 94 | 95 | 96 | # 4. Read camera - lidar calibration 97 | #--------------------------------------------------------------------------# 98 | C = np.loadtxt(extrinsic) 99 | print('Read camera lidar extrinsic calibration') 100 | print('{}\n'.format(C)) 101 | 102 | 103 | # 5. Colorize PC 104 | #--------------------------------------------------------------------------# 105 | N = points.shape[0] 106 | W = img.shape[1] 107 | H = img.shape[0] 108 | p = np.ones(4) 109 | 110 | new_points = [] 111 | new_colors = [] 112 | 113 | for i in range(N): 114 | # point in lidar coords in homogeonous coordinates 115 | p[0:3] = points[i,:] 116 | # point in camera coords 117 | p_cam = C @ p 118 | z_cam = p_cam[2] 119 | 120 | if z_cam > 0.05: 121 | # point in normalized device coords 122 | p_ndc = np.array([p_cam[0] / z_cam, p_cam[1] / z_cam, 1.0]) 123 | # projected point 124 | p_proj = cam_model.new_mtx_ @ p_ndc 125 | 126 | x = int(p_proj[0]) 127 | y = int(p_proj[1]) 128 | 129 | if x >= 0 and x < W and y >= 0 and y < H: 130 | col = img[y,x] 131 | 132 | colors[i] = 1.0 / 255.0 * col 133 | colors[i][0], colors[i][2] = colors[i][2], colors[i][0] 134 | 135 | new_points.append(points[i,:]) 136 | new_colors.append(colors[i]) 137 | 138 | 139 | # 5. Colorize PC 140 | #--------------------------------------------------------------------------# 141 | if not type(dst)==type(None): 142 | 143 | o3d.io.write_point_cloud(dst, pcd) 144 | print('Written colorized PC to {}\n'.format(dst)) 145 | 146 | if visu: 147 | pcd_color = o3d.geometry.PointCloud() 148 | pcd_color.points = o3d.utility.Vector3dVector(new_points) 149 | pcd_color.colors = o3d.utility.Vector3dVector(new_colors) 150 | o3d.visualization.draw_geometries([pcd_color],zoom=0.001, 151 | front=[-1,0,0], 152 | lookat=[0,0,0], 153 | up=[0,0,1]) 154 | 155 | 156 | def set_depth(depth,y,x,v): 157 | W = depth.shape[1] 158 | H = depth.shape[0] 159 | if x >= 0 and x < W and y >= 0 and y < H: 160 | depth[y,x] = v 161 | return depth 162 | 163 | def depth(pcd,img,intrinsic,extrinsic): 164 | 165 | # 1. Read PC 166 | #--------------------------------------------------------------------------# 167 | pcd.paint_uniform_color(np.ones(3)) 168 | 169 | print('Read point cloud.') 170 | print('num points: {}\n'.format(len(pcd.points))) 171 | 172 | points = np.asarray(pcd.points) 173 | colors = np.asarray(pcd.colors) 174 | 175 | 176 | # 2. Read image 177 | #--------------------------------------------------------------------------# 178 | print('Read image') 179 | print('shape: {}\n'.format(img.shape)) 180 | 181 | 182 | # 3. Read camera model and rectify iamge 183 | #--------------------------------------------------------------------------# 184 | cam_model = CameraModel.fromOpenCVIntrinsics(intrinsic) 185 | 186 | img = cam_model.computeOptimalMTXAndUndistort(img) 187 | 188 | 189 | # 4. Read camera - lidar calibration 190 | #--------------------------------------------------------------------------# 191 | C = np.loadtxt(extrinsic) 192 | print('Read camera lidar extrinsic calibration') 193 | print('{}\n'.format(C)) 194 | 195 | 196 | # 5. Colorize PC 197 | #--------------------------------------------------------------------------# 198 | N = points.shape[0] 199 | W = img.shape[1] 200 | H = img.shape[0] 201 | p = np.ones(4) 202 | 203 | depth = -np.ones((H,W)) 204 | 205 | for i in range(N): 206 | # point in lidar coords in homogeonous coordinates 207 | p[0:3] = points[i,:] 208 | # point in camera coords 209 | p_cam = C @ p 210 | z_cam = p_cam[2] 211 | 212 | if z_cam > 0.05: 213 | # point in normalized device coords 214 | p_ndc = np.array([p_cam[0] / z_cam, p_cam[1] / z_cam, 1.0]) 215 | # projected point 216 | p_proj = cam_model.new_mtx_ @ p_ndc 217 | 218 | x = int(p_proj[0]) 219 | y = int(p_proj[1]) 220 | 221 | if x >= 0 and x < W and y >= 0 and y < H: 222 | #Compute depth 223 | dist = np.linalg.norm(p_cam) 224 | 225 | #Taking nearest point 226 | if(depth[y,x]!=-1): 227 | dist = min(depth[y,x], dist) 228 | 229 | #Interpolate around pixel 230 | # for u in range(-5,6): 231 | # for v in range(-5,6): 232 | # depth = set_depth(depth,y+u,x+v,dist) 233 | depth = set_depth(depth,y,x,dist) 234 | #Showing result 235 | plt.figure() 236 | plt.imshow(depth,cmap='turbo') 237 | plt.colorbar() 238 | plt.title("Depth Estimation") 239 | plt.show() 240 | 241 | 242 | 243 | if __name__== "__main__": 244 | 245 | args = parseProgramArgs() 246 | colorize_from_file(args.pc,args.img,args.intrinsic,args.extrinsic,args.dst,args.vis) 247 | -------------------------------------------------------------------------------- /Calibration/src/extract_chessboard.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import glob 3 | import json 4 | import natsort 5 | import open3d as o3d 6 | import os 7 | import cv2 as cv 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | 11 | def parseProgramArgs(): 12 | parser = argparse.ArgumentParser(description='LiDAR - Camera extrinsic calibration.') 13 | parser.add_argument('-i', '--intrinsic', help='Camera instirinsics output by OpenCV in JSON format', default='../calib.json') 14 | parser.add_argument('-f', '--folder', help='Folder with images', default='../images') 15 | parser.add_argument('-o', '--dst', help='Output file') 16 | return parser.parse_args() 17 | 18 | def draw(img, corners, imgpts): 19 | corner = tuple(np.array(corners[0].ravel(),int)) 20 | print(corner,tuple(imgpts[0].ravel()),) 21 | img = cv.line(img, corner, tuple(np.array(imgpts[0].ravel(),int)), (255,0,0), 5) 22 | img = cv.line(img, corner, tuple(np.array(imgpts[1].ravel(),int)), (0,255,0), 5) 23 | img = cv.line(img, corner, tuple(np.array(imgpts[2].ravel(),int)), (0,0,255), 5) 24 | return img 25 | 26 | def extract_chessboard(calib,folder,output,vis=False): 27 | 28 | with open(calib, 'r') as f: 29 | opencv_calib = json.load(f) 30 | 31 | mtx = np.array(opencv_calib['mtx']) 32 | dist = np.array(opencv_calib['dist']) 33 | 34 | criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 100, 0.0001) 35 | objp = np.zeros((9*6,3), np.float32) 36 | objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2) * 37.4 37 | 38 | rvecs_array = [] 39 | tvecs_array = [] 40 | 41 | axis = np.float32([[30,0,0], [0,30,0], [0,0,-30]]).reshape(-1,3) 42 | 43 | for fname in natsort.natsorted(glob.glob(os.path.join(folder,'*.jpg'))): 44 | img = cv.imread(fname) 45 | gray = cv.cvtColor(img,cv.COLOR_BGR2GRAY) 46 | ret, corners = cv.findChessboardCorners(gray, (9,6),None) 47 | 48 | 49 | if ret == True: 50 | corners2 = cv.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria) 51 | 52 | # Find the rotation and translation vectors. 53 | ret,rvecs, tvecs = cv.solvePnP(objp, corners2, mtx, dist) 54 | 55 | tvecs_array.append(tvecs) 56 | rvecs_array.append(rvecs) 57 | 58 | if(vis): 59 | # project 3D points to image plane 60 | imgpts, jac = cv.projectPoints(axis, rvecs, tvecs, mtx, dist) 61 | 62 | img = draw(img,corners2,imgpts) 63 | cv.imshow('img',img) 64 | k = cv.waitKey(0) 65 | 66 | print("Rvecs len :", len(rvecs_array)) 67 | print("Tvecs len :", len(tvecs_array)) 68 | calib = {"ret":ret,"mtx":mtx.tolist(),"dist":dist.tolist(),"rvecs":np.array(rvecs_array).tolist(),"tvecs":np.array(tvecs_array).tolist()} 69 | with open(output, 'w') as f: 70 | json.dump(calib, f) 71 | cv.destroyAllWindows() 72 | 73 | 74 | if __name__== "__main__": 75 | 76 | args = parseProgramArgs() 77 | extract_chessboard(args.intrinsic,args.folder,args.dst) 78 | -------------------------------------------------------------------------------- /Calibration/src/lidar.py: -------------------------------------------------------------------------------- 1 | import os 2 | import open3d as o3d 3 | import glob 4 | import natsort 5 | import numpy as np 6 | from .calib_lidar_camera import CalibLiDARCamera 7 | 8 | class LidarOuster(): 9 | def __init__(self,output_dir,hostname,port): 10 | 11 | #Configure lidar connexion 12 | self.lidar_hostname = hostname 13 | self.lidar_port = port 14 | self.use_sensor = True 15 | 16 | if(type(self.lidar_hostname)==type(None)): 17 | self.use_sensor = False 18 | 19 | if(self.use_sensor): 20 | from ouster import client 21 | #Retrieve Scan iterator 22 | self.metadata,self.scans_it = client.Scans.sample(self.lidar_hostname,1,self.lidar_port) 23 | self.xyzlut = client.XYZLut(self.metadata) 24 | 25 | #Current scan sample 26 | self.scan = None 27 | 28 | #Create output directory 29 | self.output_dir = output_dir 30 | if(not os.path.exists(output_dir)): 31 | os.makedirs(output_dir) 32 | 33 | 34 | #Get a scan from the scan iterator 35 | def sample(self): 36 | if self.use_sensor: 37 | self.scan = next(self.scans_it)[0] 38 | 39 | 40 | def get_pcd(self): 41 | if self.use_sensor: 42 | xyz = self.xyzlut(self.scan.field(client.ChanField.RANGE)) 43 | #Use Open3D to save to PLY ====> Should change for plyfile 44 | pcd = o3d.geometry.PointCloud() 45 | pcd.points = o3d.utility.Vector3dVector(xyz.reshape(-1,3)) 46 | return pcd 47 | 48 | #Save current scan to ply 49 | def save(self,filename): 50 | if not self.use_sensor: 51 | return 52 | 53 | if(type(self.scan)==type(None)): 54 | print("There is no current scan") 55 | return 56 | 57 | if(type(filename)==int): 58 | filename = str(filename) 59 | 60 | 61 | if(filename==""): 62 | print("Please enter a filename") 63 | return 64 | 65 | if(not filename.endswith(".ply")): 66 | filename += ".ply" 67 | 68 | #Get coordinates 69 | xyz = self.xyzlut(self.scan.field(client.ChanField.RANGE)) 70 | 71 | #Use Open3D to save to PLY ====> Should change for plyfile 72 | pcd = o3d.geometry.PointCloud() 73 | pcd.points = o3d.utility.Vector3dVector(xyz.reshape(-1,3)) 74 | scan_path = os.path.join(self.output_dir , filename) 75 | o3d.io.write_point_cloud(scan_path, pcd) 76 | 77 | def get_next_file_nb(self): 78 | 79 | last_files = natsort.natsorted(glob.glob(os.path.join(self.output_dir,"*.ply"))) 80 | if len(last_files)==0: 81 | return 1 82 | return int(os.path.basename(last_files[-1]).split(".")[0])+1 83 | 84 | def pick_points(self,pcd): 85 | print("") 86 | print( 87 | "1) Please pick one points one the plane using [shift + left click]" 88 | ) 89 | print(" Press [shift + right click] to undo point picking") 90 | print("2) Afther picking points, press esc for close the window") 91 | vis = o3d.visualization.VisualizerWithEditing() 92 | vis.create_window() 93 | vis.add_geometry(pcd) 94 | view_ctl = vis.get_view_control() 95 | #TODO : Find a more generic way to set the view 96 | view_ctl.set_up((0, 0, 1)) # set the positive direction of the x-axis as the up direction 97 | #view_ctl.set_up((0, -1, 0)) # set the negative direction of the y-axis as the up direction 98 | #view_ctl.set_front((1, 0, 0)) # set the positive direction of the x-axis toward you 99 | view_ctl.set_front((-1, 0, 0)) # set the positive direction of the x-axis toward you 100 | view_ctl.set_lookat((0, 0, 0)) # set the original point as the center point of the window 101 | view_ctl.set_zoom(0.001) # set the original point as the center point of the window 102 | vis.run() # user picks points 103 | vis.destroy_window() 104 | print("") 105 | return vis.get_picked_points() 106 | 107 | def pick_planes(self): 108 | paths = natsort.natsorted(glob.glob(os.path.join(self.output_dir,"*.ply"))) 109 | for path in paths: 110 | print("Working on file : ", path) 111 | name = os.path.basename(path).split(".")[0] 112 | 113 | #Retrieving file 114 | pcd = o3d.io.read_point_cloud(path) 115 | points = np.asarray(pcd.points) 116 | 117 | #Filtrering point in front of the sensor 118 | # points_front = points[(points[:,0]>0) & (points[:,1]>-3) & (points[:,1]<3)] 119 | points_front = points 120 | pcd_front = o3d.geometry.PointCloud() 121 | pcd_front.points = o3d.utility.Vector3dVector(points_front) 122 | 123 | 124 | #Get point from user 125 | picked_points = self.pick_points(pcd_front) 126 | while len(picked_points)>1: 127 | print("Please pick 1 points") 128 | picked_points = self.pick_points(pcd_front) 129 | 130 | if(len(picked_points)==0): 131 | exit() 132 | 133 | picked_coord = np.asarray(pcd_front.select_by_index(picked_points).points) 134 | 135 | dist_to_plane = 1000000 #Distance picked point to estimated plane 136 | dist_to_point = 0.9 #Threshold for first filtering (m) 137 | while(dist_to_plane>0.05): #Threshold for plane fit acceptance 138 | dist_to_point-=0.1 139 | plane_points = [] 140 | 141 | #Keeping only point in the dist_to_point threshold 142 | for point in points_front: 143 | if np.linalg.norm(picked_coord[0]-point)=min_dist): 15 | output.append(trajectory[i]) 16 | 17 | return output 18 | 19 | @staticmethod 20 | def matchTraj(ref_traj,tomatch_traj):#,method="dist"): 21 | output = [] 22 | i=0 23 | N = len(tomatch_traj) 24 | for p in ref_traj: 25 | u_ref = p.transform.translation() 26 | # if(method=="dist"): 27 | # while i np.linalg.norm(u_ref-tomatch_traj[i+1].T.translation()): 28 | # i+=1 29 | # else: 30 | while i abs(p.timestamp-tomatch_traj[i+1].timestamp): 31 | i+=1 32 | output.append(tomatch_traj[i]) 33 | 34 | return output 35 | 36 | @staticmethod 37 | def readTrajectory(filename): 38 | traj = [] 39 | ply = PlyData.read(filename) 40 | ply_props = [p.name for p in ply['vertex'].properties] 41 | i=0 42 | for vertex in ply['vertex']: 43 | #Retrive position 44 | u = np.array([vertex['x'], vertex['y'], vertex['z']]) 45 | q = quaternion.from_float_array([vertex['q_w'],vertex['q_x'],vertex['q_y'],vertex['q_z']]) 46 | timestamp = vertex['timestamp'] 47 | index = vertex['indices'] if 'indices' in ply_props else None 48 | 49 | if timestamp == 0: 50 | timestamp = i 51 | 52 | #Compute pose 53 | P = Pose.fromTranslationQuaternion(u,q,timestamp,index) 54 | 55 | #Store 56 | traj.append(P) 57 | i+=1 58 | 59 | return traj 60 | 61 | @staticmethod 62 | def writeTrajectory(traj,output_file): 63 | #formating values 64 | values = [] 65 | for p in traj: 66 | u = p.transform.translation() 67 | q = p.transform.quaternion() 68 | values.append((u[0],u[1],u[2],q.w,q.x,q.y,q.z,p.timestamp,p.index)) 69 | #print(p.timestamp) 70 | 71 | #writing vertex 72 | vertex = np.array(values,dtype=[('x','f8'),('y','f8'),('z','f8'),('q_w','f8'),('q_x','f8'),('q_y','f8'),('q_z','f8'),('timestamp','f8'),('indices','i4')]) 73 | el = PlyElement.describe(vertex,"vertex") 74 | 75 | #saving to file 76 | if not output_file.endswith(".ply"): 77 | output_file += ".ply" 78 | PlyData([el]).write(output_file) 79 | 80 | @staticmethod 81 | def interpolateFromTimestamp(ref_trajectory,timestamps): 82 | #lidar poses 83 | i=1 84 | pl1 = ref_trajectory[i-1] 85 | pl2 = ref_trajectory[i] 86 | 87 | N = len(ref_trajectory) 88 | traj = [] 89 | 90 | for j,t in enumerate(timestamps): 91 | #Finding matching timestamp between lidar and camera 92 | while(pl2.timestamp < t) and i0 and dist0: 125 | return traj,idx 126 | return traj -------------------------------------------------------------------------------- /Preprocessing/src/transform.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import quaternion 3 | 4 | class Transform: 5 | def __init__(self,T): 6 | self.T = T 7 | 8 | 9 | def translation(self): 10 | """ 11 | Returns translation vector block from SE3 (R|t) 4x4 matrix. 12 | """ 13 | return self.T[0:3, 3] 14 | 15 | def rotation(self): 16 | """ 17 | Returns rotation matrix block vector from SE3 (R|t) 4x4 matrix. 18 | """ 19 | return self.T[0:3, 0:3] 20 | 21 | 22 | def quaternion(self): 23 | """ 24 | Returns rotation matrix block vector from SE3 (R|t) 4x4 matrix. 25 | """ 26 | return quaternion.from_rotation_matrix(self.rotation()) 27 | 28 | @staticmethod 29 | def fromTranslationQuaternion(u,q): 30 | R = quaternion.as_rotation_matrix(q) 31 | #Ridig Transform 32 | T = np.eye(4) 33 | T[0:3,0:3] = R 34 | T[0:3, 3] = u 35 | return Transform(T) 36 | 37 | @staticmethod 38 | def fromTranslationRotation(u,R): 39 | #Ridig Transform 40 | T = np.eye(4) 41 | T[0:3,0:3] = R 42 | T[0:3, 3] = u 43 | return Transform(T) 44 | 45 | def matrix(self): 46 | """ 47 | Returns the SE3 matrix. 48 | """ 49 | return self.T -------------------------------------------------------------------------------- /Preprocessing/src/utils.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | from plyfile import PlyData,PlyElement 4 | 5 | def read_shader(file): 6 | with open(file) as f: 7 | return f.read() 8 | 9 | 10 | def getPlyName(indice): 11 | return "frame_" + "%06d"%indice + ".ply" 12 | 13 | def readPly(filename): 14 | ply = PlyData.read(filename) 15 | 16 | dt = ply['vertex'].dtype().descr 17 | dt_others = [d for d in dt if d[0] not in ["x","y","z"]] 18 | 19 | xyz = [ply["vertex"][p] for p in ["x","y","z"]] 20 | xyz = np.array(xyz).T 21 | 22 | others = [ply["vertex"][d[0]] for d in dt_others] 23 | others = np.array(others).T 24 | return xyz,others,dt_others 25 | 26 | def OpenCVMtxToOpenGLMTx(mtx,width,height,zfar,znear): 27 | fx = mtx[0,0] 28 | fy = mtx[1,1] 29 | cx = mtx[0,2] 30 | cy = mtx[1,2] 31 | 32 | new_mtx = np.zeros((4,4),np.float32) 33 | 34 | new_mtx[0,0] = 2.0 * fx / width 35 | new_mtx[1,0] = 0.0 36 | new_mtx[2,0] = 0.0 37 | new_mtx[3,0] = 0.0 38 | 39 | new_mtx[0,1] = 0.0 40 | new_mtx[1,1] = -2.0 * fy / height 41 | new_mtx[2,1] = 0.0 42 | new_mtx[3,1] = 0.0 43 | 44 | new_mtx[0,2] = 2.0 * cx / width - 1.0 45 | new_mtx[1,2] = 1.0 - 2.0 * cy / height 46 | new_mtx[2,2] = -(zfar + znear) / (znear - zfar) 47 | new_mtx[3,2] = 1.0 48 | 49 | 50 | new_mtx[0,3] = 0.0 51 | new_mtx[1,3] = 0.0 52 | new_mtx[2,3] = 2.0 * zfar * znear / (znear - zfar) 53 | new_mtx[3,3] = 0.0 54 | 55 | return new_mtx 56 | 57 | 58 | def findPoseIdx(pose,traj): 59 | for j,p in enumerate(traj): 60 | if p.index == pose.index: 61 | return j 62 | return 0 63 | 64 | def getPropertieIndex(dt_others,propertie): 65 | for i,d in enumerate(dt_others): 66 | if d[0] == propertie: 67 | return i 68 | return None 69 | 70 | def getImagesTimestamp(folder): 71 | return np.loadtxt(os.path.join(folder,"timestamps.txt")) 72 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DOC-Depth: A novel approach for dense depth ground truth generation 2 | Official implementation of the DOC-Depth method. 3 | 4 | [Project Page](https://simondemoreau.github.io/DOC-Depth) 5 | 6 | ![Paper concept](assets/paper_concept.png) 7 | 8 | If you use our method in your research, please cite : 9 | ```bibtex 10 | TBA 11 | ``` 12 | 13 | ## Dense Depth KITTI annotations 14 | Please visit our [project page](https://simondemoreau.github.io/DOC-Depth) to download the dense annotations of KITTI. 15 | 16 | ## Calibration 17 | The first step of the pipeline is to calibrate together LiDAR and Camera. See the [Calibration](Calibration) folder to use our tool. 18 | 19 | ## Recording 20 | The easiest way to record your dataset is to use [ROS](https://ros.org/) to record all your sensors into ".bag" files. 21 | 22 | ## Preprocessing 23 | After recording, you must use our pre-processing pipeline with SLAM and DOC to obtain a dense and classified reconstruction of your record. See the [Preprocessing](Preprocessing) folder for more informations. 24 | 25 | ## Rendering 26 | Finally, you can use our tool to apply our composite rendering to the classified LiDAR frames and obtain your dense depth. See the [Rendering](Rendering) folder to access our tool. 27 | 28 | ## License 29 | TBA -------------------------------------------------------------------------------- /Rendering/README.md: -------------------------------------------------------------------------------- 1 | ## Rendering 2 | 3 | To apply DOC-Depth composite rendering to the pre-processed dataset, use the following command: 4 | 5 | 6 | ```bash 7 | python render.py [traj_odometry.ply file] [traj_camera.ply file] [lidar_frame_directory] --intrinsic [calib.json file] --extrinsic [calib_lidar_ref_cam.txt file] -o [output_directory] 8 | ``` 9 | 10 | 11 | For more options see : 12 | ```bash 13 | python render.py -h 14 | ``` 15 | 16 | You should now have your dataset with fully dense depth annotations! -------------------------------------------------------------------------------- /Rendering/render.py: -------------------------------------------------------------------------------- 1 | from src.renderApp import RenderApp 2 | import argparse 3 | 4 | def parseProgramArgs(): 5 | parser = argparse.ArgumentParser(description='Depth Map Estimation in Camera point of view using Lidar data') 6 | parser.add_argument('lidar_trajectory', help='Lidar trajectory odometry') 7 | parser.add_argument('camera_trajectory', help='Camera trajectory') 8 | parser.add_argument('frames', help='Folder with lidar frames') 9 | parser.add_argument('-o', '--output', help='Output directory') 10 | parser.add_argument('--output-mode', help='Output mode [depth,image,kitti]', default="depth") 11 | 12 | parser.add_argument('--width', help='Width of the image', default=1920,type=int) 13 | parser.add_argument('--height', help='Height of the image', default=1080,type=int) 14 | parser.add_argument('--shader-vertex', help='Vertex shader', default="shaders/vertex.c") 15 | parser.add_argument('--shader-fragment', help='Fragment shader', default="shaders/fragment_ellipsis.c") 16 | parser.add_argument('--pts-per-frame', help='Number of points per frame', default=150000,type=int) 17 | parser.add_argument('--cam-step', help='Distance step between camera poses', default=1.0,type=float) 18 | parser.add_argument('--dist-merge', help='Distance between aggregated frames in the depth', default=0.01,type=float) 19 | parser.add_argument('--max-depth', help='Maximum distance considered in the depth (z_far)' , default=100,type=float) 20 | parser.add_argument('--min-depth', help='Minimum distance considered in the depth (z_near)', default=0.1,type=float) 21 | parser.add_argument('--dist-past', help='Maximum distance before the current pose considered in the depth for the merge' , default=0.001,type=float) 22 | parser.add_argument('--classif-property-name', help='Classif static property', default="classid",type=str) 23 | parser.add_argument('--intrinsic', help='Intrinsic camera calibration file', default="calib.json") 24 | parser.add_argument('--extrinsic', help='Extrinsic camera and lidar calibration file', default="calib_lidar_ref_cam.txt") 25 | parser.add_argument('--max-pts-size', help='Maximum size of the static points', default=50.0,type=float) 26 | parser.add_argument('--min-pts-size', help='Minimum size of the static points', default=3.0,type=float) 27 | parser.add_argument('--max-pts-size-dyn', help='Maximum size of the dynamic points', default=80,type=float) 28 | parser.add_argument('--min-pts-size-dyn', help='Minimum size of the dynamic points', default=35,type=float) 29 | parser.add_argument('--start-index', help='Start frame', default=0,type=int) 30 | parser.add_argument('--crop-norm', help='Max point norm in merged frames', default=300.0,type=float) 31 | parser.add_argument('--scan-only', help='Project only current LiDAR frame', action='store_true') 32 | 33 | return parser.parse_args() 34 | 35 | if __name__ == '__main__': 36 | args = parseProgramArgs() 37 | 38 | app = RenderApp(args.width,args.height,args.shader_vertex,args.shader_fragment, 39 | args.pts_per_frame,args.camera_trajectory,args.lidar_trajectory, args.frames, 40 | args.cam_step,args.dist_merge,args.max_depth,args.min_depth,args.dist_past,args.classif_property_name, 41 | args.intrinsic,args.extrinsic,args.max_pts_size,args.min_pts_size,args.max_pts_size_dyn,args.min_pts_size_dyn, 42 | args.start_index, args.output_mode, args.output, args.crop_norm, args.scan_only 43 | ) 44 | 45 | app.run() -------------------------------------------------------------------------------- /Rendering/shaders/fragment.c: -------------------------------------------------------------------------------- 1 | varying vec3 v_depth; 2 | 3 | vec4 turbo(float x) { 4 | float r = 0.1357 + x * ( 4.5974 - x * ( 42.3277 - x * ( 130.5887 - x * ( 150.5666 - x * 58.1375 )))); 5 | float g = 0.0914 + x * ( 2.1856 + x * ( 4.8052 - x * ( 14.0195 - x * ( 4.2109 + x * 2.7747 )))); 6 | float b = 0.1067 + x * ( 12.5925 - x * ( 60.1097 - x * ( 109.0745 - x * ( 88.5066 - x * 26.8183 )))); 7 | return vec4(r,g,b,1); 8 | } 9 | 10 | float colormap_red(float x) { 11 | if (x < 0.7) { 12 | return 4.0 * x - 1.5; 13 | } else { 14 | return -4.0 * x + 4.5; 15 | } 16 | } 17 | 18 | float colormap_green(float x) { 19 | if (x < 0.5) { 20 | return 4.0 * x - 0.5; 21 | } else { 22 | return -4.0 * x + 3.5; 23 | } 24 | } 25 | 26 | float colormap_blue(float x) { 27 | if (x < 0.3) { 28 | return 4.0 * x + 0.5; 29 | } else { 30 | return -4.0 * x + 2.5; 31 | } 32 | } 33 | 34 | vec4 colormap(float x) { 35 | float r = clamp(colormap_red(x), 0.0, 1.0); 36 | float g = clamp(colormap_green(x), 0.0, 1.0); 37 | float b = clamp(colormap_blue(x), 0.0, 1.0); 38 | return vec4(r, g, b, 1.0); 39 | } 40 | 41 | void main() { 42 | float u = 2.0 * gl_PointCoord.x - 1.0; 43 | float v = 2.0 * gl_PointCoord.y - 1.0; 44 | float sq_ruv = u*u + v*v; 45 | 46 | // Gen round points 47 | if (sq_ruv > 1.0) { discard; } 48 | 49 | gl_FragColor = turbo(v_depth.x); 50 | gl_FragDepth = v_depth.x; 51 | } -------------------------------------------------------------------------------- /Rendering/shaders/fragment_circle.c: -------------------------------------------------------------------------------- 1 | varying vec3 v_depth; 2 | 3 | vec4 turbo(float x) { 4 | float r = 0.1357 + x * ( 4.5974 - x * ( 42.3277 - x * ( 130.5887 - x * ( 150.5666 - x * 58.1375 )))); 5 | float g = 0.0914 + x * ( 2.1856 + x * ( 4.8052 - x * ( 14.0195 - x * ( 4.2109 + x * 2.7747 )))); 6 | float b = 0.1067 + x * ( 12.5925 - x * ( 60.1097 - x * ( 109.0745 - x * ( 88.5066 - x * 26.8183 )))); 7 | return vec4(r,g,b,1); 8 | } 9 | 10 | float colormap_red(float x) { 11 | if (x < 0.7) { 12 | return 4.0 * x - 1.5; 13 | } else { 14 | return -4.0 * x + 4.5; 15 | } 16 | } 17 | 18 | float colormap_green(float x) { 19 | if (x < 0.5) { 20 | return 4.0 * x - 0.5; 21 | } else { 22 | return -4.0 * x + 3.5; 23 | } 24 | } 25 | 26 | float colormap_blue(float x) { 27 | if (x < 0.3) { 28 | return 4.0 * x + 0.5; 29 | } else { 30 | return -4.0 * x + 2.5; 31 | } 32 | } 33 | 34 | vec4 colormap(float x) { 35 | float r = clamp(colormap_red(x), 0.0, 1.0); 36 | float g = clamp(colormap_green(x), 0.0, 1.0); 37 | float b = clamp(colormap_blue(x), 0.0, 1.0); 38 | return vec4(r, g, b, 1.0); 39 | } 40 | 41 | float saturate( float x ) { return clamp( x, 0.0, 1.0 ); } 42 | 43 | vec3 viridis_quintic( float x ) 44 | { 45 | x = saturate( x ); 46 | vec4 x1 = vec4( 1.0, x, x * x, x * x * x ); // 1 x x2 x3 47 | vec4 x2 = x1 * x1.w * x; // x4 x5 x6 x7 48 | return vec3( 49 | dot( x1.xyzw, vec4( +0.280268003, -0.143510503, +2.225793877, -14.815088879 ) ) + dot( x2.xy, vec2( +25.212752309, -11.772589584 ) ), 50 | dot( x1.xyzw, vec4( -0.002117546, +1.617109353, -1.909305070, +2.701152864 ) ) + dot( x2.xy, vec2( -1.685288385, +0.178738871 ) ), 51 | dot( x1.xyzw, vec4( +0.300805501, +2.614650302, -12.019139090, +28.933559110 ) ) + dot( x2.xy, vec2( -33.491294770, +13.762053843 ) ) ); 52 | } 53 | 54 | 55 | void main() { 56 | float u = 2.0 * gl_PointCoord.x - 1.0; 57 | float v = 2.0 * gl_PointCoord.y - 1.0; 58 | // u *= 1.5; // ellipse 59 | float sq_ruv = u*u + v*v; 60 | 61 | // Gen round points 62 | if (sq_ruv > 1.0) { discard; } 63 | 64 | 65 | float dist = log(v_depth.x * 100.0); 66 | dist = 1 - (dist-log(4.0))/(log(100.0 - 4.0)); // For enhanced contrast TODO: change for more general case 67 | 68 | 69 | // gl_FragColor = turbo(v_depth.x); 70 | gl_FragColor = vec4(viridis_quintic(dist), 1.0); 71 | gl_FragDepth = v_depth.x; 72 | } -------------------------------------------------------------------------------- /Rendering/shaders/fragment_ellipsis.c: -------------------------------------------------------------------------------- 1 | varying vec3 v_depth; 2 | 3 | vec4 turbo(float x) { 4 | float r = 0.1357 + x * ( 4.5974 - x * ( 42.3277 - x * ( 130.5887 - x * ( 150.5666 - x * 58.1375 )))); 5 | float g = 0.0914 + x * ( 2.1856 + x * ( 4.8052 - x * ( 14.0195 - x * ( 4.2109 + x * 2.7747 )))); 6 | float b = 0.1067 + x * ( 12.5925 - x * ( 60.1097 - x * ( 109.0745 - x * ( 88.5066 - x * 26.8183 )))); 7 | return vec4(r,g,b,1); 8 | } 9 | 10 | float colormap_red(float x) { 11 | if (x < 0.7) { 12 | return 4.0 * x - 1.5; 13 | } else { 14 | return -4.0 * x + 4.5; 15 | } 16 | } 17 | 18 | float colormap_green(float x) { 19 | if (x < 0.5) { 20 | return 4.0 * x - 0.5; 21 | } else { 22 | return -4.0 * x + 3.5; 23 | } 24 | } 25 | 26 | float colormap_blue(float x) { 27 | if (x < 0.3) { 28 | return 4.0 * x + 0.5; 29 | } else { 30 | return -4.0 * x + 2.5; 31 | } 32 | } 33 | 34 | vec4 colormap(float x) { 35 | float r = clamp(colormap_red(x), 0.0, 1.0); 36 | float g = clamp(colormap_green(x), 0.0, 1.0); 37 | float b = clamp(colormap_blue(x), 0.0, 1.0); 38 | return vec4(r, g, b, 1.0); 39 | } 40 | 41 | float saturate( float x ) { return clamp( x, 0.0, 1.0 ); } 42 | 43 | vec3 viridis_quintic( float x ) 44 | { 45 | x = saturate( x ); 46 | vec4 x1 = vec4( 1.0, x, x * x, x * x * x ); // 1 x x2 x3 47 | vec4 x2 = x1 * x1.w * x; // x4 x5 x6 x7 48 | return vec3( 49 | dot( x1.xyzw, vec4( +0.280268003, -0.143510503, +2.225793877, -14.815088879 ) ) + dot( x2.xy, vec2( +25.212752309, -11.772589584 ) ), 50 | dot( x1.xyzw, vec4( -0.002117546, +1.617109353, -1.909305070, +2.701152864 ) ) + dot( x2.xy, vec2( -1.685288385, +0.178738871 ) ), 51 | dot( x1.xyzw, vec4( +0.300805501, +2.614650302, -12.019139090, +28.933559110 ) ) + dot( x2.xy, vec2( -33.491294770, +13.762053843 ) ) ); 52 | } 53 | 54 | 55 | void main() { 56 | float u = 2.0 * gl_PointCoord.x - 1.0; 57 | float v = 2.0 * gl_PointCoord.y - 1.0; 58 | u *= 1.5; // ellipse 59 | float sq_ruv = u*u + v*v; 60 | 61 | // Gen round points 62 | if (sq_ruv > 1.0) { discard; } 63 | 64 | 65 | float dist = log(v_depth.x * 250.0); 66 | dist = 1 - (dist-log(4.0))/(log(250.0 - 4.0)); // For enhanced contrast TODO: change for more general case 67 | 68 | 69 | // gl_FragColor = turbo(v_depth.x); 70 | gl_FragColor = vec4(viridis_quintic(dist), 1.0); 71 | gl_FragDepth = v_depth.x; 72 | } -------------------------------------------------------------------------------- /Rendering/shaders/fragment_ellipsis_turbo.c: -------------------------------------------------------------------------------- 1 | varying vec3 v_depth; 2 | 3 | vec4 turbo(float x) { 4 | float r = 0.1357 + x * ( 4.5974 - x * ( 42.3277 - x * ( 130.5887 - x * ( 150.5666 - x * 58.1375 )))); 5 | float g = 0.0914 + x * ( 2.1856 + x * ( 4.8052 - x * ( 14.0195 - x * ( 4.2109 + x * 2.7747 )))); 6 | float b = 0.1067 + x * ( 12.5925 - x * ( 60.1097 - x * ( 109.0745 - x * ( 88.5066 - x * 26.8183 )))); 7 | return vec4(r,g,b,1); 8 | } 9 | 10 | float colormap_red(float x) { 11 | if (x < 0.7) { 12 | return 4.0 * x - 1.5; 13 | } else { 14 | return -4.0 * x + 4.5; 15 | } 16 | } 17 | 18 | float colormap_green(float x) { 19 | if (x < 0.5) { 20 | return 4.0 * x - 0.5; 21 | } else { 22 | return -4.0 * x + 3.5; 23 | } 24 | } 25 | 26 | float colormap_blue(float x) { 27 | if (x < 0.3) { 28 | return 4.0 * x + 0.5; 29 | } else { 30 | return -4.0 * x + 2.5; 31 | } 32 | } 33 | 34 | vec4 colormap(float x) { 35 | float r = clamp(colormap_red(x), 0.0, 1.0); 36 | float g = clamp(colormap_green(x), 0.0, 1.0); 37 | float b = clamp(colormap_blue(x), 0.0, 1.0); 38 | return vec4(r, g, b, 1.0); 39 | } 40 | 41 | void main() { 42 | float u = 2.0 * gl_PointCoord.x - 1.0; 43 | float v = 2.0 * gl_PointCoord.y - 1.0; 44 | u *= 1.5; // ellipse 45 | float sq_ruv = u*u + v*v; 46 | 47 | // Gen round points 48 | if (sq_ruv > 1.0) { discard; } 49 | 50 | gl_FragColor = turbo(v_depth.x); 51 | gl_FragDepth = v_depth.x; 52 | } -------------------------------------------------------------------------------- /Rendering/shaders/fragment_viridis_intensity.c: -------------------------------------------------------------------------------- 1 | varying vec3 v_depth; 2 | varying float v_intensity; 3 | 4 | vec4 turbo(float x) { 5 | float r = 0.1357 + x * ( 4.5974 - x * ( 42.3277 - x * ( 130.5887 - x * ( 150.5666 - x * 58.1375 )))); 6 | float g = 0.0914 + x * ( 2.1856 + x * ( 4.8052 - x * ( 14.0195 - x * ( 4.2109 + x * 2.7747 )))); 7 | float b = 0.1067 + x * ( 12.5925 - x * ( 60.1097 - x * ( 109.0745 - x * ( 88.5066 - x * 26.8183 )))); 8 | return vec4(r,g,b,1); 9 | } 10 | 11 | float colormap_red(float x) { 12 | if (x < 0.7) { 13 | return 4.0 * x - 1.5; 14 | } else { 15 | return -4.0 * x + 4.5; 16 | } 17 | } 18 | 19 | float colormap_green(float x) { 20 | if (x < 0.5) { 21 | return 4.0 * x - 0.5; 22 | } else { 23 | return -4.0 * x + 3.5; 24 | } 25 | } 26 | 27 | float colormap_blue(float x) { 28 | if (x < 0.3) { 29 | return 4.0 * x + 0.5; 30 | } else { 31 | return -4.0 * x + 2.5; 32 | } 33 | } 34 | 35 | vec4 colormap(float x) { 36 | float r = clamp(colormap_red(x), 0.0, 1.0); 37 | float g = clamp(colormap_green(x), 0.0, 1.0); 38 | float b = clamp(colormap_blue(x), 0.0, 1.0); 39 | return vec4(r, g, b, 1.0); 40 | } 41 | 42 | float saturate( float x ) { return clamp( x, 0.0, 1.0 ); } 43 | 44 | vec3 viridis_quintic( float x ) 45 | { 46 | x = saturate( x ); 47 | vec4 x1 = vec4( 1.0, x, x * x, x * x * x ); // 1 x x2 x3 48 | vec4 x2 = x1 * x1.w * x; // x4 x5 x6 x7 49 | return vec3( 50 | dot( x1.xyzw, vec4( +0.280268003, -0.143510503, +2.225793877, -14.815088879 ) ) + dot( x2.xy, vec2( +25.212752309, -11.772589584 ) ), 51 | dot( x1.xyzw, vec4( -0.002117546, +1.617109353, -1.909305070, +2.701152864 ) ) + dot( x2.xy, vec2( -1.685288385, +0.178738871 ) ), 52 | dot( x1.xyzw, vec4( +0.300805501, +2.614650302, -12.019139090, +28.933559110 ) ) + dot( x2.xy, vec2( -33.491294770, +13.762053843 ) ) ); 53 | } 54 | 55 | 56 | void main() { 57 | float u = 2.0 * gl_PointCoord.x - 1.0; 58 | float v = 2.0 * gl_PointCoord.y - 1.0; 59 | float sq_ruv = u*u + v*v; 60 | 61 | // Gen round points 62 | if (sq_ruv > 1.0) { discard; } 63 | 64 | gl_FragColor = vec4(viridis_quintic(v_intensity/80.0), 1.0); // Saturated intensity 65 | gl_FragDepth = v_depth.x; 66 | } -------------------------------------------------------------------------------- /Rendering/shaders/vertex.c: -------------------------------------------------------------------------------- 1 | attribute vec3 points; 2 | 3 | uniform mat4 T; // Current LiDAR frame to Camera pose transformation 4 | uniform mat4 C; // Extrinsic calibration 5 | uniform mat4 K; // Intrisinc calibration 6 | varying vec3 v_depth; 7 | 8 | uniform float max_pts_size; 9 | uniform float min_pts_size; 10 | 11 | 12 | uniform float max_dist; 13 | void main() { 14 | 15 | if(abs(points.z) < 1 && abs(points.y)<1 && abs(points.x)<1){ 16 | gl_Position = vec4(2,2,2,1); 17 | return; 18 | } 19 | vec4 pos = C * T * vec4(points,1); 20 | float z = abs(pos.z); 21 | v_depth = vec3(z/max_dist,z/max_dist,z/max_dist); 22 | 23 | vec4 pos_proj = K * pos; 24 | 25 | gl_Position = vec4(pos_proj.x,pos_proj.y,pos_proj.z,pos_proj.w); 26 | 27 | gl_PointSize = clamp(max_pts_size / log(pos.x*pos.x + pos.y*pos.y + pos.z*pos.z), min_pts_size, max_pts_size); 28 | 29 | 30 | } -------------------------------------------------------------------------------- /Rendering/shaders/vertex_intensity.c: -------------------------------------------------------------------------------- 1 | attribute vec3 points; 2 | attribute float intensities; 3 | 4 | uniform mat4 T; // Current LiDAR frame to Camera pose transformation 5 | uniform mat4 C; // Extrinsic calibration 6 | uniform mat4 K; // Intrisinc calibration 7 | varying vec3 v_depth; 8 | varying float v_intensity; 9 | 10 | uniform float max_pts_size; 11 | uniform float min_pts_size; 12 | 13 | 14 | uniform float max_dist; 15 | void main() { 16 | 17 | if(abs(points.z) < 1 && abs(points.y)<1 && abs(points.x)<1){ 18 | gl_Position = vec4(2,2,2,1); 19 | return; 20 | } 21 | vec4 pos = C * T * vec4(points,1); 22 | float z = abs(pos.z); 23 | v_depth = vec3(z/max_dist,z/max_dist,z/max_dist); 24 | v_intensity = intensities; 25 | 26 | vec4 pos_proj = K * pos; 27 | 28 | gl_Position = vec4(pos_proj.x,pos_proj.y,pos_proj.z,pos_proj.w); 29 | 30 | gl_PointSize = clamp(max_pts_size / log(pos.x*pos.x + pos.y*pos.y + pos.z*pos.z), min_pts_size, max_pts_size); 31 | 32 | } -------------------------------------------------------------------------------- /Rendering/src/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SimondeMoreau/DOC-Depth/07646512ac96a673ec85fbc4544506de4ec1dddb/Rendering/src/__init__.py -------------------------------------------------------------------------------- /Rendering/src/frameManager.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | import quaternion 4 | 5 | from .trajectory import Trajectory 6 | from .utils import * 7 | 8 | class FrameManager: 9 | 10 | def __init__(self,traj_cam_f,traj_lidar_f,frame_folder,pts_per_frame,cam_step,dist_merge,max_depth,dist_past,classif_property_name,crop_norm): 11 | self.traj_cam = Trajectory.readTrajectory(traj_cam_f) 12 | self.traj_lidar = Trajectory.readTrajectory(traj_lidar_f) 13 | self.frame_folder = frame_folder 14 | self.pts_per_frame = pts_per_frame 15 | self.cam_step = cam_step 16 | self.dist_merge = dist_merge 17 | self.max_depth = max_depth 18 | self.dist_past = dist_past 19 | self.classif_property_name = classif_property_name 20 | self.crop_norm = crop_norm 21 | 22 | # Verify timestamps of the trajectories are chronological 23 | self.traj_cam = self.clean_traj_timestamp(self.traj_cam) 24 | self.traj_lidar = self.clean_traj_timestamp(self.traj_lidar) 25 | 26 | # Subsample trajectories 27 | self.traj_cam = Trajectory.subSample(self.traj_cam,cam_step) 28 | self.traj_lidar = Trajectory.subSample(self.traj_lidar, dist_merge) 29 | 30 | # Match trajectories 31 | self.traj_match = Trajectory.matchTraj(self.traj_cam,self.traj_lidar) 32 | 33 | 34 | self.lidar_frames = {} 35 | self.dynamic_frame = None 36 | 37 | 38 | def loadFrames(self,i): 39 | # Find lidar frames for the current camera pose 40 | matched_idx = findPoseIdx(self.traj_match[i],self.traj_lidar) 41 | traj_depth,_ = Trajectory.cutDistance(self.traj_lidar,self.max_depth,matched_idx,self.dist_past) # Cut the trajectory to keep only the points in the depth 42 | #traj_depth = Trajectory.subSample(traj_depth,args.dist_merge) 43 | 44 | 45 | # Load frames 46 | needed_frames = [] 47 | for pose in traj_depth: 48 | needed_frames.append(pose.index) 49 | if pose.index not in self.lidar_frames.keys(): 50 | self.lidar_frames[pose.index] = readPly(os.path.join(self.frame_folder,getPlyName(pose.index))) 51 | 52 | # Read the point cloud 53 | ply_basename = getPlyName(pose.index) 54 | ply_filename = os.path.join(self.frame_folder,ply_basename) 55 | 56 | trame,meta,dt_meta = readPly(ply_filename) 57 | 58 | # Get static points 59 | # GENERAL 60 | # trame = self.getStatic(trame,meta,dt_meta) 61 | 62 | #KITTI 63 | if abs(pose.index - self.traj_lidar[-1].index) < 5: # Last frames we want to keep the full point cloud to keep maximum density 64 | trame = self.getStatic(trame,meta,dt_meta,no_mask=True) 65 | else: 66 | trame = self.getStatic(trame,meta,dt_meta) 67 | 68 | # Get the point cloud in the right size 69 | trame = self.getSizedPointcloud(trame) 70 | 71 | # Save the point cloud 72 | self.lidar_frames[pose.index] = { 73 | "trame": trame, 74 | "pose": pose 75 | } 76 | 77 | 78 | 79 | # Delete frames not needed anymore 80 | to_delete = [] 81 | for frame in self.lidar_frames.keys(): 82 | if frame not in needed_frames: 83 | to_delete.append(frame) 84 | for frame in to_delete: 85 | del self.lidar_frames[frame] 86 | 87 | 88 | # Load dynamic frame 89 | ply_basename = getPlyName(self.traj_lidar[matched_idx].index) 90 | ply_filename = os.path.join(self.frame_folder,ply_basename) 91 | 92 | trame,others,dt_others = readPly(ply_filename) 93 | 94 | # Get dynamic points 95 | trame = self.getDynamic(trame,others,dt_others) 96 | 97 | # Get the point cloud in the right size 98 | trame = self.getSizedPointcloud(trame) 99 | 100 | self.dynamic_frame = { 101 | "trame": trame, 102 | "pose": self.traj_lidar[matched_idx] 103 | } 104 | 105 | 106 | def getCameraPose(self,i): 107 | return self.traj_cam[i] 108 | 109 | def getStaticFrames(self): 110 | return self.lidar_frames.values() 111 | 112 | def getDynamicFrame(self): 113 | return self.dynamic_frame 114 | 115 | def getStatic(self,trame,others,dt_others,no_mask=False): 116 | # Retrieve the classification 117 | idx_classid = getPropertieIndex(dt_others,self.classif_property_name) 118 | mask_static = others[:,idx_classid]<100 119 | mask_floor = others[:,idx_classid]<50 120 | 121 | trame = trame[mask_static] 122 | mask_floor = mask_floor[mask_static] 123 | 124 | # TODO: Find a better way to manage the crop 125 | # GENERAL 126 | # crop_mask = (np.abs(trame[:,0])<30) | (mask_floor & (np.abs(trame[:,0])<30)) 127 | # trame = trame[crop_mask] 128 | 129 | #FOR KITTI 130 | if no_mask: 131 | crop_mask = np.ones(trame.shape[0],dtype=bool) 132 | else: 133 | crop_mask = (~mask_floor) | (mask_floor & (np.abs(trame[:,0])<20)) 134 | trame = trame[crop_mask] 135 | 136 | norm = np.linalg.norm(trame,axis=1) 137 | mask = norm=100 145 | 146 | pts_moving = trame[mask_moving] 147 | 148 | norm = np.linalg.norm(pts_moving,axis=1) 149 | mask = ~((norm<2.5) & (pts_moving[:,2]>=-1)) 150 | pts_moving = pts_moving[mask] 151 | 152 | return pts_moving 153 | 154 | def getCurrentTrame(self,i): 155 | # Load current frame 156 | matched_idx = findPoseIdx(self.traj_match[i],self.traj_lidar) 157 | ply_basename = getPlyName(self.traj_lidar[matched_idx].index) 158 | ply_filename = os.path.join(self.frame_folder,ply_basename) 159 | 160 | trame,others,dt_others = readPly(ply_filename) 161 | 162 | # Get the point cloud in the right size 163 | trame = self.getSizedPointcloud(trame) 164 | 165 | return { 166 | "trame": trame, 167 | "pose": self.traj_lidar[matched_idx] 168 | } 169 | 170 | 171 | def getSizedPointcloud(self,trame): 172 | pts_full = np.zeros((self.pts_per_frame,3)) 173 | pts_full[:trame.shape[0]] = trame 174 | return pts_full 175 | 176 | 177 | def clean_traj_timestamp(self,traj): 178 | deleted = [] 179 | # Verify timestamps of the trajectory are chronological 180 | j=1 181 | while j < len(traj): 182 | if(traj[j].timestamp0: 188 | print("Error in trajectory, timestamps are not chronological") 189 | print("Deleting frames", deleted) 190 | return traj 191 | 192 | 193 | def len(self): 194 | return len(self.traj_cam) 195 | 196 | -------------------------------------------------------------------------------- /Rendering/src/pose.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import quaternion 3 | 4 | from .transform import Transform 5 | 6 | class Pose: 7 | def __init__(self,T,timestamp=None,index=None): 8 | self.transform = T 9 | self.timestamp = timestamp 10 | self.index = index 11 | 12 | @staticmethod 13 | def fromTranslationQuaternion(u,q,timestamp=None,index=None): 14 | T = Transform.fromTranslationQuaternion(u,q) 15 | return Pose(T,timestamp,index) 16 | 17 | 18 | def interpolate(p1,p2,t): 19 | #Get position 20 | u1 = p1.T.translation() 21 | u2 = p2.T.translation() 22 | q1 = p1.T.quaternion() 23 | q2 = p2.T.quaternion() 24 | t1 = p1.timestamp 25 | t2 = p2.timestamp 26 | 27 | #Interpolate XYZ 28 | x = np.interp(t,[t1,t2],[u1[0],u2[0]]) 29 | y = np.interp(t,[t1,t2],[u1[1],u2[1]]) 30 | z = np.interp(t,[t1,t2],[u1[2],u2[2]]) 31 | u = [x,y,z] 32 | 33 | #Interpolate Quaternion 34 | q = quaternion.slerp(q1,q2,t1,t2,t) 35 | 36 | return Pose.fromTranslationQuaternion(u,q,t) -------------------------------------------------------------------------------- /Rendering/src/renderApp.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from glumpy import app, gloo, gl,glm 4 | from glumpy.transforms import PanZoom, Position,Trackball 5 | from glumpy.ext import png 6 | from glumpy.app import clock 7 | 8 | import json 9 | 10 | from .frameManager import FrameManager 11 | 12 | from .utils import * 13 | 14 | import os 15 | os.environ["OPENCV_IO_ENABLE_OPENEXR"]="1" 16 | import cv2 17 | 18 | 19 | 20 | class RenderApp: 21 | 22 | def __init__(self, width, height, shader_file_vertex, shader_file_fragment, pts_per_frame, 23 | traj_cam, traj_lidar, frame_folder, cam_step, dist_merge, max_depth, min_depth, dist_past, 24 | classif_property_name, intrinsic_file, extrinsic_file, max_pts_size, min_pts_size, max_pts_size_dyn, min_pts_size_dyn, 25 | start_index, output_mode, output, crop_norm, scan_mode): 26 | self.index = start_index 27 | self.width = width 28 | self.height = height 29 | self.shader_file_vertex = shader_file_vertex 30 | self.shader_file_fragment = shader_file_fragment 31 | self.pts_per_frame = pts_per_frame 32 | self.max_depth = max_depth 33 | self.min_depth = min_depth #TODO: add as argument 34 | self.max_pts_size = max_pts_size 35 | self.min_pts_size = min_pts_size 36 | self.max_pts_size_dyn = max_pts_size_dyn 37 | self.min_pts_size_dyn = min_pts_size_dyn 38 | self.output_mode = output_mode # depth or image or kitti 39 | self.output = output 40 | self.scan_mode = scan_mode 41 | 42 | if self.output is not None: 43 | os.makedirs(self.output,exist_ok=True) 44 | 45 | # read the shaders 46 | vertex = self.read_shader(shader_file_vertex) 47 | fragment = self.read_shader(shader_file_fragment) 48 | 49 | # Create the window 50 | self.window = app.Window(width, height, color=(1,1,1,1)) 51 | self.window_var = gloo.Program(vertex, fragment, count=pts_per_frame) 52 | 53 | self.frameManager = FrameManager(traj_cam, traj_lidar, frame_folder, pts_per_frame, cam_step, dist_merge, max_depth, dist_past, classif_property_name, crop_norm) 54 | 55 | 56 | # Read camera intrinsic 57 | self.K = np.array(json.load(open(intrinsic_file))["mtx"]) 58 | 59 | # Read camera and lidar extrinsic 60 | self.C = np.loadtxt(extrinsic_file) 61 | 62 | 63 | self.setWindowVar() 64 | 65 | @self.window.event 66 | def on_init(): 67 | gl.glEnable(gl.GL_DEPTH_TEST) 68 | 69 | @self.window.event 70 | def on_draw(dt): 71 | self.update(dt) 72 | 73 | def setWindowVar(self): 74 | self.window_var["points"] = np.zeros((self.pts_per_frame, 3)) 75 | self.window_var["max_dist"] = self.max_depth 76 | self.window_var["T"] = np.eye(4) 77 | self.window_var["C"] = self.C.T 78 | self.window_var["K"] = OpenCVMtxToOpenGLMTx(self.K,self.width,self.height,self.max_depth,self.min_depth).T 79 | self.window_var["max_pts_size"] = self.max_pts_size 80 | self.window_var["min_pts_size"] = self.min_pts_size 81 | 82 | 83 | def read_shader(self,file): 84 | with open(file) as f: 85 | return f.read() 86 | 87 | def update(self,dt): 88 | self.window.clear() 89 | 90 | 91 | # Render the frames 92 | if self.scan_mode: 93 | self.renderScanDepth() 94 | else: 95 | self.renderDenseDepth() 96 | 97 | # Save the output 98 | if self.output is not None: 99 | pose_cam = self.frameManager.getCameraPose(self.index) 100 | filename = os.path.join(self.output,str(str(pose_cam.index).zfill(6))) 101 | self.saveOutput(filename) 102 | 103 | self.printStatus() 104 | self.index += 1 105 | #TODO: add a stop condition 106 | 107 | def renderDenseDepth(self): 108 | # Load the frames 109 | self.frameManager.loadFrames(self.index) 110 | 111 | # Render static frames 112 | self.window_var["max_pts_size"] = self.max_pts_size 113 | self.window_var["min_pts_size"] = self.min_pts_size 114 | frames = self.frameManager.getStaticFrames() 115 | pose_cam = self.frameManager.getCameraPose(self.index) 116 | pcam = pose_cam.transform.matrix() 117 | for i,frame in enumerate(frames): 118 | self.renderFrame(pcam,frame) 119 | 120 | # Render dynamic frame 121 | self.window_var["max_pts_size"] = self.max_pts_size_dyn 122 | self.window_var["min_pts_size"] = self.min_pts_size_dyn 123 | dynamic_frame = self.frameManager.getDynamicFrame() 124 | if dynamic_frame is not None: 125 | self.renderFrame(pcam,dynamic_frame) 126 | 127 | def renderScanDepth(self): 128 | # Render current pose 129 | self.window_var["max_pts_size"] = self.max_pts_size 130 | self.window_var["min_pts_size"] = self.min_pts_size 131 | 132 | frame = self.frameManager.getCurrentTrame(self.index) 133 | pose_cam = self.frameManager.getCameraPose(self.index) 134 | pcam = pose_cam.transform.matrix() 135 | self.renderFrame(pcam,frame) 136 | 137 | 138 | def renderFrame(self,pcam,frame): 139 | trame = frame["trame"] 140 | plidar = frame["pose"].transform.matrix() 141 | 142 | self.window_var["points"] = trame 143 | self.window_var["T"] = (np.linalg.inv(pcam) @ plidar).T 144 | 145 | gl.glEnable(gl.GL_DEPTH_TEST) 146 | gl.glDepthRange(0.0, 1.0) 147 | gl.glDepthFunc(gl.GL_LESS) 148 | self.window_var.draw(gl.GL_POINTS) 149 | 150 | def saveOutput(self,filename): 151 | if self.output_mode.lower() == "image": 152 | self.saveImage(filename) 153 | elif self.output_mode.lower() == "kitti": 154 | self.saveKitti(filename) 155 | else: 156 | self.saveDepth(filename) 157 | 158 | def saveImage(self,filename): 159 | rgb = np.zeros((self.height, self.width, 3), dtype=np.uint8) 160 | gl.glReadPixels(0, 0, self.width, self.height, 161 | gl.GL_RGB, gl.GL_UNSIGNED_BYTE, rgb) 162 | rgb = rgb[::-1,:] 163 | rgb = rgb.reshape((self.height,self.width,3)) 164 | 165 | rgb = cv2.cvtColor(rgb, cv2.COLOR_RGB2BGRA) 166 | 167 | invalid_mask = np.all(rgb==255,axis=-1) 168 | rgb[invalid_mask,3] = 0 169 | 170 | 171 | if not filename.endswith(".png") or not filename.endswith(".jpg"): 172 | filename += ".png" 173 | 174 | cv2.imwrite(filename,rgb) 175 | 176 | 177 | def saveDepth(self,filename): 178 | framebuffer = np.zeros((self.height, self.width), dtype=np.float32) 179 | gl.glReadPixels(0, 0, self.width, self.height, 180 | gl.GL_DEPTH_COMPONENT, gl.GL_FLOAT, framebuffer) 181 | framebuffer = framebuffer[::-1,:] 182 | framebuffer = framebuffer.reshape((self.height,self.width)) 183 | invalid_mask = framebuffer == 1 184 | 185 | im = framebuffer.astype("float32") * self.max_depth 186 | im[invalid_mask] = np.inf 187 | 188 | if not filename.endswith(".exr"): 189 | filename += ".exr" 190 | 191 | cv2.imwrite(filename,im,[cv2.IMWRITE_EXR_TYPE, cv2.IMWRITE_EXR_TYPE_HALF]) 192 | 193 | def saveKitti(self,filename): 194 | framebuffer = np.zeros((self.height, self.width), dtype=np.float32) 195 | gl.glReadPixels(0, 0, self.width, self.height, 196 | gl.GL_DEPTH_COMPONENT, gl.GL_FLOAT, framebuffer) 197 | framebuffer = framebuffer[::-1,:] 198 | framebuffer = framebuffer.reshape((self.height,self.width)) 199 | invalid_mask = framebuffer == 1 200 | 201 | im = framebuffer.astype("float32") * self.max_depth 202 | im[invalid_mask] = np.inf 203 | 204 | if not filename.endswith(".png"): 205 | filename += ".png" 206 | 207 | im = (im * 256).astype(np.uint16) 208 | cv2.imwrite(filename,im) 209 | 210 | 211 | def printStatus(self): 212 | fps = "{:.2f}".format(clock.get_fps()) 213 | print("FPS: ", fps, "| Image : ", self.index, "/", self.frameManager.len()) 214 | 215 | def run(self): 216 | app.run() 217 | -------------------------------------------------------------------------------- /Rendering/src/trajectory.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from plyfile import PlyData,PlyElement 3 | import quaternion 4 | from .pose import Pose 5 | 6 | class Trajectory: 7 | 8 | @staticmethod 9 | def subSample(trajectory,min_dist): 10 | 11 | output = [trajectory[0]] 12 | 13 | for i in range(1,len(trajectory)): 14 | if(np.linalg.norm(trajectory[i].transform.translation()-output[-1].transform.translation())>=min_dist): 15 | output.append(trajectory[i]) 16 | 17 | return output 18 | 19 | @staticmethod 20 | def matchTraj(ref_traj,tomatch_traj):#,method="dist"): 21 | output = [] 22 | i=0 23 | N = len(tomatch_traj) 24 | for p in ref_traj: 25 | u_ref = p.transform.translation() 26 | # if(method=="dist"): 27 | # while i np.linalg.norm(u_ref-tomatch_traj[i+1].T.translation()): 28 | # i+=1 29 | # else: 30 | while i abs(p.timestamp-tomatch_traj[i+1].timestamp): 31 | i+=1 32 | output.append(tomatch_traj[i]) 33 | 34 | return output 35 | 36 | @staticmethod 37 | def readTrajectory(filename): 38 | traj = [] 39 | ply = PlyData.read(filename) 40 | ply_props = [p.name for p in ply['vertex'].properties] 41 | i=0 42 | for vertex in ply['vertex']: 43 | #Retrive position 44 | u = np.array([vertex['x'], vertex['y'], vertex['z']]) 45 | q = quaternion.from_float_array([vertex['q_w'],vertex['q_x'],vertex['q_y'],vertex['q_z']]) 46 | timestamp = vertex['timestamp'] 47 | index = vertex['indices'] if 'indices' in ply_props else None 48 | 49 | if timestamp == 0: 50 | timestamp = i 51 | 52 | #Compute pose 53 | P = Pose.fromTranslationQuaternion(u,q,timestamp,index) 54 | 55 | #Store 56 | traj.append(P) 57 | i+=1 58 | 59 | return traj 60 | 61 | @staticmethod 62 | def writeTrajectory(traj,output_file): 63 | #formating values 64 | values = [] 65 | for p in traj: 66 | u = p.transform.translation() 67 | q = p.transform.quaternion() 68 | values.append((u[0],u[1],u[2],q.w,q.x,q.y,q.z,p.timestamp,p.index)) 69 | #print(p.timestamp) 70 | 71 | #writing vertex 72 | vertex = np.array(values,dtype=[('x','f8'),('y','f8'),('z','f8'),('q_w','f8'),('q_x','f8'),('q_y','f8'),('q_z','f8'),('timestamp','f8'),('indices','i4')]) 73 | el = PlyElement.describe(vertex,"vertex") 74 | 75 | #saving to file 76 | if not output_file.endswith(".ply"): 77 | output_file += ".ply" 78 | PlyData([el]).write(output_file) 79 | 80 | @staticmethod 81 | def interpolateFromTimestamp(ref_trajectory,timestamps): 82 | #lidar poses 83 | i=1 84 | pl1 = ref_trajectory[i-1] 85 | pl2 = ref_trajectory[i] 86 | 87 | N = len(ref_trajectory) 88 | traj = [] 89 | 90 | for j,t in enumerate(timestamps): 91 | #Finding matching timestamp between lidar and camera 92 | while(pl2.timestamp < t) and i0 and dist0: 125 | return traj,idx 126 | return traj -------------------------------------------------------------------------------- /Rendering/src/transform.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import quaternion 3 | 4 | class Transform: 5 | def __init__(self,T): 6 | self.T = T 7 | 8 | 9 | def translation(self): 10 | """ 11 | Returns translation vector block from SE3 (R|t) 4x4 matrix. 12 | """ 13 | return self.T[0:3, 3] 14 | 15 | def rotation(self): 16 | """ 17 | Returns rotation matrix block vector from SE3 (R|t) 4x4 matrix. 18 | """ 19 | return self.T[0:3, 0:3] 20 | 21 | 22 | def quaternion(self): 23 | """ 24 | Returns rotation matrix block vector from SE3 (R|t) 4x4 matrix. 25 | """ 26 | return quaternion.from_rotation_matrix(self.rotation()) 27 | 28 | @staticmethod 29 | def fromTranslationQuaternion(u,q): 30 | R = quaternion.as_rotation_matrix(q) 31 | #Ridig Transform 32 | T = np.eye(4) 33 | T[0:3,0:3] = R 34 | T[0:3, 3] = u 35 | return Transform(T) 36 | 37 | @staticmethod 38 | def fromTranslationRotation(u,R): 39 | #Ridig Transform 40 | T = np.eye(4) 41 | T[0:3,0:3] = R 42 | T[0:3, 3] = u 43 | return Transform(T) 44 | 45 | def matrix(self): 46 | """ 47 | Returns the SE3 matrix. 48 | """ 49 | return self.T -------------------------------------------------------------------------------- /Rendering/src/utils.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | from plyfile import PlyData,PlyElement 4 | 5 | def read_shader(file): 6 | with open(file) as f: 7 | return f.read() 8 | 9 | 10 | def getPlyName(indice): 11 | return "frame_" + "%06d"%indice + ".ply" 12 | 13 | def readPly(filename): 14 | ply = PlyData.read(filename) 15 | 16 | dt = ply['vertex'].dtype().descr 17 | dt_others = [d for d in dt if d[0] not in ["x","y","z"]] 18 | 19 | xyz = [ply["vertex"][p] for p in ["x","y","z"]] 20 | xyz = np.array(xyz).T 21 | 22 | others = [ply["vertex"][d[0]] for d in dt_others] 23 | others = np.array(others).T 24 | return xyz,others,dt_others 25 | 26 | def OpenCVMtxToOpenGLMTx(mtx,width,height,zfar,znear): 27 | fx = mtx[0,0] 28 | fy = mtx[1,1] 29 | cx = mtx[0,2] 30 | cy = mtx[1,2] 31 | 32 | new_mtx = np.zeros((4,4),np.float32) 33 | 34 | new_mtx[0,0] = 2.0 * fx / width 35 | new_mtx[1,0] = 0.0 36 | new_mtx[2,0] = 0.0 37 | new_mtx[3,0] = 0.0 38 | 39 | new_mtx[0,1] = 0.0 40 | new_mtx[1,1] = -2.0 * fy / height 41 | new_mtx[2,1] = 0.0 42 | new_mtx[3,1] = 0.0 43 | 44 | new_mtx[0,2] = 2.0 * cx / width - 1.0 45 | new_mtx[1,2] = 1.0 - 2.0 * cy / height 46 | new_mtx[2,2] = -(zfar + znear) / (znear - zfar) 47 | new_mtx[3,2] = 1.0 48 | 49 | 50 | new_mtx[0,3] = 0.0 51 | new_mtx[1,3] = 0.0 52 | new_mtx[2,3] = 2.0 * zfar * znear / (znear - zfar) 53 | new_mtx[3,3] = 0.0 54 | 55 | return new_mtx 56 | 57 | 58 | def findPoseIdx(pose,traj): 59 | for j,p in enumerate(traj): 60 | if p.index == pose.index: 61 | return j 62 | return 0 63 | 64 | def getPropertieIndex(dt_others,propertie): 65 | for i,d in enumerate(dt_others): 66 | if d[0] == propertie: 67 | return i 68 | return None 69 | 70 | def getImagesTimestamp(folder): 71 | return np.loadtxt(os.path.join(folder,"timestamps.txt")) 72 | -------------------------------------------------------------------------------- /assets/paper_concept.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SimondeMoreau/DOC-Depth/07646512ac96a673ec85fbc4544506de4ec1dddb/assets/paper_concept.png --------------------------------------------------------------------------------