├── README.md ├── config ├── HDL-64.json └── VLP-16.json ├── example.png ├── src ├── data_loader.py ├── feature_extract.py ├── laser_mapping.py ├── laser_odometry.py ├── main_kitti.py ├── main_loam.py └── utils.py └── utils └── rosbag2scans.py /README.md: -------------------------------------------------------------------------------- 1 | # PyLOAM 2 | ## Python version LOAM 3 | 4 | PyLOAM is implemented based on LOAM (J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time). 5 | 6 | 7 | ## Requirements 8 | - Python 3.6.9 9 | - Numpy 1.19.1 10 | - Open3D 0.10.0.0 11 | - pypcd 0.1.1 12 | 13 | ## Usage 14 | 15 | Currently I convert scans to `.npy` files. 16 | 17 | `python3 src/main_loam.py --npy_path ${DATA_PATH}` 18 | 19 | ## References 20 | - [LOAM: Lidar Odometry and Mapping in Real-time](https://frc.ri.cmu.edu/~zhangji/publications/JFR_2018.pdf) 21 | - [loam_velodyne](https://github.com/daobilige-su/loam_ve) 22 | - [A-LOAM](https://github.com/HKUST-Aerial-Robotics/A-LOAM) 23 | 24 | ## TODOs 25 | 26 | Many stuff are still under development: 27 | 28 | - Configuration class 29 | - Visualization 30 | - Evaluation 31 | 32 | ## Example 33 | 34 | Mapped feature points from [NSH indoor outdor](http://wiki.ros.org/loam_velodyne) dataset. 35 | 36 | ![](example.png) -------------------------------------------------------------------------------- /config/HDL-64.json: -------------------------------------------------------------------------------- 1 | { 2 | "feature" : { 3 | "line_num" : 64, 4 | "ring_index" : 4, 5 | "dist_thresh" : 5, 6 | "ring_init" : false 7 | }, 8 | "odometry" : { 9 | "dist_threshold" : 5, 10 | "ring_index" : 4, 11 | "nearby_scan" : 2.5, 12 | "optim_iteration" : 25, 13 | "voxel_size" : 0.2 14 | }, 15 | "mapping" : { 16 | "cloud_width" : 21, 17 | "cloud_height" : 21, 18 | "cloud_depth" : 11, 19 | "corner_voxel_size" : 0.4, 20 | "surf_voxel_size" : 0.8, 21 | "map_voxel_size" : 0.6, 22 | "stack_num" : 2, 23 | "optim_iteration" : 5 24 | } 25 | } -------------------------------------------------------------------------------- /config/VLP-16.json: -------------------------------------------------------------------------------- 1 | { 2 | "feature" : { 3 | "line_num" : 16, 4 | "ring_index" : 4, 5 | "dist_thresh" : 0.2, 6 | "ring_init" : true 7 | }, 8 | "odometry" : { 9 | "dist_threshold" : 5, 10 | "ring_index" : 4, 11 | "nearby_scan" : 2.5, 12 | "optim_iteration" : 25, 13 | "voxel_size" : 0.2 14 | }, 15 | "mapping" : { 16 | "cloud_width" : 21, 17 | "cloud_height" : 21, 18 | "cloud_depth" : 11, 19 | "corner_voxel_size" : 0.2, 20 | "surf_voxel_size" : 0.4, 21 | "map_voxel_size" : 0.6, 22 | "stack_num" : 2, 23 | "optim_iteration" : 5 24 | } 25 | } -------------------------------------------------------------------------------- /example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lizimo061/PyLOAM/bf5cdee0dbd3187e73702b76f9d2842dd0843359/example.png -------------------------------------------------------------------------------- /src/data_loader.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | from pypcd import pypcd 4 | import numpy as np 5 | import open3d as o3d 6 | # import rosbag 7 | # import sensor_msgs.point_cloud2 8 | 9 | def viz_scan(scan): 10 | o3d_cloud = o3d.geometry.PointCloud() 11 | o3d_cloud.points = o3d.utility.Vector3dVector(scan[:, :3]) 12 | o3d.visualization.draw_geometries([o3d_cloud]) 13 | 14 | class DataLoader: 15 | def __init__(self, path, name=None): 16 | self.name = name 17 | self.path = path 18 | self.file_list = os.listdir(path) 19 | self.scan_num = len(self.file_list) 20 | self.sort_files() 21 | 22 | def __len__(self): 23 | return self.scan_num 24 | 25 | def __getitem__(self, index): 26 | return self.get_pc(index) 27 | 28 | def get_pc(self, index): 29 | pass 30 | 31 | def sort_files(self): 32 | pass 33 | 34 | 35 | class PCDLoader(DataLoader): 36 | def get_pc(self, index): 37 | if index < self.scan_num: 38 | pcd = pypcd.PointCloud.from_path(os.path.join(self.path, self.file_list[index])) 39 | raw_data = pcd.pc_data 40 | return raw_data 41 | else: 42 | print("Access out of range!") 43 | 44 | def sort_files(self): 45 | self.file_list.sort(key=lambda file: int(file[:-4])) 46 | 47 | class KittiLoader(DataLoader): 48 | def get_pc(self, index): 49 | if index < self.scan_num: 50 | scan = np.fromfile(os.path.join(self.path, self.file_list[index]), dtype=np.float32).reshape(-1,4) 51 | return scan 52 | else: 53 | print("Access out of range!") 54 | 55 | def sort_files(self): 56 | self.file_list.sort(key=lambda file: int(file[:-4])) 57 | 58 | class NPYLoader(DataLoader): 59 | def get_pc(self, index): 60 | if index < self.scan_num: 61 | scan = np.load(os.path.join(self.path, self.file_list[index])) 62 | return scan 63 | else: 64 | print("Access out of range!") 65 | 66 | def sort_files(self): 67 | self.file_list.sort(key=lambda file: int(file[:-4])) 68 | 69 | def viz(self): 70 | for i in range(self.scan_num): 71 | scan = self.get_pc(i) 72 | viz_scan(scan) 73 | 74 | """ 75 | class ROSBagLoader: 76 | def __init__(self, path, topic): 77 | bag = rosbag.Bag(path) 78 | self.scans = [] 79 | for topic_name, msg, t in bag.read_messages(topics=[topic]): 80 | cloud = sensor_msgs.point_cloud2.read_points_list(msg) 81 | cloud = self.point_list_to_cloud(cloud) 82 | self.scans.append(cloud) 83 | self.scan_num = len(self.scans) 84 | print('Loaded rosbag:', path) 85 | 86 | def __getitem__(self, index): 87 | if index < self.scan_num: 88 | return self.scans[index] 89 | else: 90 | print("Access out of range!") 91 | 92 | def __len__(self): 93 | return self.scan_num 94 | 95 | def point_list_to_cloud(self, pts_list): 96 | cloud = [] 97 | for pt in pts_list: 98 | pt_np = np.array([pt.x, pt.y, pt.z, pt.intensity, pt.ring]) 99 | cloud.append(pt_np) 100 | cloud = np.vstack(cloud) 101 | return cloud 102 | """ -------------------------------------------------------------------------------- /src/feature_extract.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import threading 3 | import queue 4 | import math 5 | 6 | class FeatureExtract: 7 | def __init__(self, config=None): 8 | self.used_line_num = None 9 | self.config = config 10 | if config is None: 11 | self.LINE_NUM = 16 12 | self.RING_INDEX = 4 13 | self.RING_INIT = True # If ring index is given 14 | self.THRES = 0.2 15 | else: 16 | self.LINE_NUM = config['feature']['line_num'] 17 | self.RING_INDEX = config['feature']['ring_index'] 18 | self.RING_INIT = config['feature']['ring_init'] 19 | self.THRES = config['feature']['dist_thresh'] 20 | 21 | def get_scan_id(self, cloud): 22 | xy_dist = np.sqrt(np.sum(np.square(cloud[:, :2]), axis=1)) 23 | angles = np.arctan(cloud[:, 2]/xy_dist) * 180/math.pi 24 | if self.LINE_NUM == 16: 25 | scan_ids = (angles + 15)/2 + 0.5 26 | elif self.LINE_NUM == 32: 27 | scan_ids = int((angles + 93./3.) * 3./4.) 28 | elif self.LINE_NUM == 64: 29 | scan_ids = self.LINE_NUM / 2 + (-8.83 - angles) * 2. + .5 30 | upper = np.where(angles >= -8.83) 31 | scan_ids[upper] = (2 - angles[upper]) * 3.0 + 0.5 32 | else: 33 | print("Specific line number not supported!") 34 | return 35 | scan_ids = scan_ids.astype(int) 36 | if self.LINE_NUM == 64: 37 | correct_id = np.where(np.logical_and(scan_ids >= 0, scan_ids < 50)) # Only use 50 lines 38 | else: 39 | correct_id = np.where(np.logical_and(scan_ids >= 0, scan_ids < self.LINE_NUM)) 40 | scan_ids = scan_ids[correct_id] 41 | cloud = cloud[correct_id] 42 | scan_ids = np.expand_dims(scan_ids, axis=1) 43 | return cloud, scan_ids 44 | 45 | def get_rel_time(self, cloud): 46 | start_ori = -np.arctan2(cloud[0, 1], cloud[0, 0]) 47 | end_ori = -np.arctan2(cloud[-1, 1], cloud[-1, 0]) + 2*math.pi 48 | if end_ori - start_ori > 3 * math.pi: 49 | end_ori -= 2*math.pi 50 | elif end_ori - start_ori < math.pi: 51 | end_ori += 2*math.pi 52 | 53 | half_passed = False 54 | rel_time = np.zeros(cloud.shape[0]) 55 | 56 | for i in range(cloud.shape[0]): 57 | pt = np.array([cloud[i, 1], cloud[i, 2], cloud[i, 0]]) 58 | cloud[i, :3] = pt 59 | ori = -np.arctan2(pt[0], pt[2]) 60 | if not half_passed: 61 | if ori < start_ori - math.pi/2: 62 | ori += 2*math.pi 63 | elif ori > start_ori + math.pi*3/2: 64 | ori -= 2*math.pi 65 | if ori - start_ori > math.pi: 66 | half_passed = True 67 | else: 68 | ori += 2*math.pi 69 | if ori < end_ori - math.pi *3/2: 70 | ori += 2*math.pi 71 | elif ori > end_ori + math.pi/2: 72 | ori -= 2*math.pi 73 | rel_time[i] = (ori - start_ori)/(end_ori - start_ori) 74 | return rel_time 75 | 76 | def remove_close_points(self, cloud, thres): 77 | """ Input size: N*3 """ 78 | dists = np.sum(np.square(cloud[:, :3]), axis=1) 79 | cloud_out = cloud[dists > thres*thres] 80 | return cloud_out 81 | 82 | def divide_lines(self, cloud): 83 | line_num = np.max(cloud[:, self.RING_INDEX]) + 1 84 | self.used_line_num = int(line_num) 85 | clouds_by_line = [cloud[cloud[:, self.RING_INDEX] == val, :] for val in range(0, self.used_line_num)] 86 | cloud_out = np.concatenate(clouds_by_line, axis=0) 87 | return cloud_out 88 | 89 | def compute_curvatures(self, cloud): 90 | kernel = np.ones(11) 91 | kernel[5] = -10 92 | curvatures = np.apply_along_axis(lambda x: np.convolve(x, kernel, 'same'), 0, cloud[:, :3]) 93 | curvatures = np.sum(np.square(curvatures), axis=1) 94 | scan_start_id = [np.where(cloud[:, self.RING_INDEX] == val)[0][0] + 5 for val in range(0, self.used_line_num)] 95 | scan_end_id = [np.where(cloud[:, self.RING_INDEX] == val)[0][-1] - 5 for val in range(0, self.used_line_num)] 96 | return curvatures, scan_start_id, scan_end_id 97 | 98 | def remove_occluded(self, cloud): 99 | num_points = cloud.shape[0] 100 | depth = np.sqrt(np.sum(np.square(cloud[:, :3]), axis=1)) 101 | picked_list = np.zeros(num_points, dtype=int) 102 | for i in range(5, num_points-6): 103 | diff = np.sum(np.square(cloud[i, :3] - cloud[i+1, :3])) 104 | if diff > 0.1: 105 | if depth[i] > depth[i+1]: 106 | depth_diff = cloud[i+1, :3] - cloud[i, :3] * (depth[i+1]/depth[i]) 107 | depth_diff = np.sqrt(np.sum(np.square(depth_diff))) 108 | if depth_diff/depth[i+1] < 0.1: 109 | picked_list[i-5:i+1] = 1 110 | else: 111 | depth_diff = cloud[i+1, :3] * (depth[i]/depth[i+1]) - cloud[i, :3] 112 | depth_diff = np.sqrt(np.sum(np.square(depth_diff))) 113 | if depth_diff/depth[i] < 0.1: 114 | picked_list[i+1:i+7] = 1 115 | 116 | diff_prev = np.sum(np.square(cloud[i, :3] - cloud[i-1, :3])) 117 | if diff > 0.0002 * depth[i] * depth[i] and diff_prev > 0.0002 * depth[i] * depth[i]: 118 | picked_list[i] = 1 119 | 120 | return picked_list 121 | 122 | def feature_classification(self, cloud, curvatures, picked_list, scan_start_id, scan_end_id): 123 | corner_sharp = [] 124 | corner_less = [] 125 | surf_flat = [] 126 | surf_less = [] 127 | cloud_labels = np.zeros(cloud.shape[0]) 128 | index = np.arange(cloud.shape[0]) 129 | index = np.expand_dims(index, axis=1).astype('float64') 130 | curvatures = np.expand_dims(curvatures, axis=1) 131 | curv_index = np.hstack((curvatures, index)) 132 | for scan_id in range(self.used_line_num): 133 | """ TODO: Avoid empty line """ 134 | for i in range(6): 135 | sp = int((scan_start_id[scan_id] * (6-i) + scan_end_id[scan_id] * i) / 6) 136 | ep = int((scan_start_id[scan_id] * (5-i) + scan_end_id[scan_id] * (i+1)) / 6 - 1) 137 | curv_seg = curv_index[sp:ep+1, :] 138 | sorted_curv = curv_seg[np.argsort(curv_seg[:, 0])] 139 | picked_num = 0 140 | 141 | for j in range(ep, sp-1, -1): 142 | sorted_ind = j - sp 143 | ind = int(sorted_curv[sorted_ind, 1]) 144 | curv = sorted_curv[sorted_ind, 0] 145 | if picked_list[ind] == 0 and curv > 0.1: 146 | picked_num += 1 147 | if picked_num <= 2: 148 | cloud_labels[ind] = 2 149 | corner_sharp.append(ind) 150 | corner_less.append(ind) 151 | elif picked_num <= 20: 152 | cloud_labels[ind] = 1 153 | corner_less.append(ind) 154 | else: 155 | break 156 | 157 | picked_list[ind] = 1 158 | 159 | for l in range(1,6): 160 | diff = np.sum(np.square(cloud[ind+l, :3] - cloud[ind+l-1, :3])) 161 | if diff > 0.05: 162 | break 163 | picked_list[ind+l] = 1 164 | 165 | for l in range(-1, -6, -1): 166 | diff = np.sum(np.square(cloud[ind+l, :3] - cloud[ind+l+1, :3])) 167 | if diff > 0.05: 168 | break 169 | picked_list[ind+l] = 1 170 | 171 | picked_num = 0 172 | for j in range(sp, ep+1): 173 | sorted_ind = j - sp 174 | ind = int(sorted_curv[sorted_ind, 1]) 175 | curv = sorted_curv[sorted_ind, 0] 176 | if picked_list[ind] == 0 and curv < 0.1: 177 | cloud_labels[ind] = -1 178 | surf_flat.append(ind) 179 | picked_num += 1 180 | 181 | if picked_num >= 4: 182 | break 183 | 184 | picked_list[ind] = 1 185 | 186 | for l in range(1,6): 187 | diff = np.sum(np.square(cloud[ind+l, :3] - cloud[ind+l-1, :3])) 188 | if diff > 0.05: 189 | break 190 | picked_list[ind+l] = 1 191 | 192 | for l in range(-1, -6, -1): 193 | diff = np.sum(np.square(cloud[ind+l, :3] - cloud[ind+l+1, :3])) 194 | if diff > 0.05: 195 | break 196 | picked_list[ind+l] = 1 197 | 198 | for j in range(sp, ep+1): 199 | sorted_ind = j - sp 200 | ind = int(sorted_curv[sorted_ind, 1]) 201 | if cloud_labels[ind] <= 0: 202 | surf_less.append(ind) 203 | 204 | return corner_sharp, corner_less, surf_flat, surf_less 205 | 206 | def feature_extract(self, cloud): 207 | if self.RING_INIT is False: 208 | cloud, line_id = self.get_scan_id(cloud) 209 | cloud = np.hstack((cloud, line_id.astype(np.float32))) 210 | self.RING_INDEX = cloud.shape[1]-1 211 | rel_time = self.get_rel_time(cloud) 212 | rel_time = np.expand_dims(rel_time, axis=1) 213 | cloud = np.hstack((cloud, rel_time)) 214 | cloud = self.remove_close_points(cloud, self.THRES) 215 | cloud = self.divide_lines(cloud) 216 | curvatures, scan_start_id, scan_end_id = self.compute_curvatures(cloud) 217 | picked_list = self.remove_occluded(cloud) 218 | corner_sharp, corner_less, surf_flat, surf_less = self.feature_classification(cloud, curvatures, picked_list, scan_start_id, scan_end_id) 219 | return cloud[corner_sharp, :], cloud[corner_less, :], cloud[surf_flat, :], cloud[surf_less, :] 220 | 221 | def feature_extract_id(self, cloud): 222 | if self.RING_INIT is False: 223 | cloud, line_id = self.get_scan_id(cloud) 224 | cloud = np.hstack((cloud, line_id.astype(np.float32))) 225 | self.RING_INDEX = cloud.shape[1]-1 226 | 227 | cloud = self.remove_close_points(cloud, self.THRES) 228 | cloud = self.divide_lines(cloud) 229 | curvatures, scan_start_id, scan_end_id = self.compute_curvatures(cloud) 230 | picked_list = self.remove_occluded(cloud) 231 | corner_sharp, corner_less, surf_flat, surf_less = self.feature_classification(cloud, curvatures, picked_list, scan_start_id, scan_end_id) 232 | return [corner_sharp, corner_less, surf_flat, surf_less] 233 | 234 | class FeatureManager(threading.Thread): 235 | feature_queue = [] 236 | process_id = 0 237 | process_lock = threading.Lock() 238 | 239 | def __init__(self, loader, config=None): 240 | threading.Thread.__init__(self) 241 | self.feature_extractor = FeatureExtract(config=config) 242 | self.data_loader = loader 243 | 244 | def run(self): 245 | while FeatureManager.process_id < len(self.data_loader): 246 | with FeatureManager.process_lock: 247 | feature_idx = self.feature_extractor.feature_extract_id(self.data_loader[FeatureManager.process_id]) 248 | print("Feature processed: ", FeatureManager.process_id) 249 | FeatureManager.process_id += 1 250 | FeatureManager.feature_queue.append(feature_idx) 251 | 252 | @classmethod 253 | def get_feature(self): 254 | feature = [] 255 | with FeatureManager.process_lock: 256 | if len(FeatureManager.feature_queue) is 0: 257 | print("Error: No feature processed") 258 | else: 259 | feature = FeatureManager.feature_queue.pop(0) 260 | return feature 261 | -------------------------------------------------------------------------------- /src/laser_mapping.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import open3d as o3d 3 | from utils import * 4 | 5 | class Mapper: 6 | def __init__(self, config=None): 7 | self.rot_wmap_wodom = np.eye(3) 8 | self.trans_wmap_wodom = np.zeros((3,1)) 9 | self.frame_count = 0 10 | if config is None: 11 | self.CLOUD_WIDTH = 21 12 | self.CLOUD_HEIGHT = 21 13 | self.CLOUD_DEPTH = 11 14 | self.CORNER_VOXEL_SIZE = 0.2 15 | self.SURF_VOXEL_SIZE = 0.4 16 | self.MAP_VOXEL_SIZE = 0.6 17 | self.STACK_NUM = 2 18 | self.OPTIM_ITERATION = 5 19 | else: 20 | self.CLOUD_WIDTH = config['mapping']['cloud_width'] 21 | self.CLOUD_HEIGHT = config['mapping']['cloud_height'] 22 | self.CLOUD_DEPTH = config['mapping']['cloud_depth'] 23 | self.CORNER_VOXEL_SIZE = config['mapping']['corner_voxel_size'] 24 | self.SURF_VOXEL_SIZE = config['mapping']['surf_voxel_size'] 25 | self.MAP_VOXEL_SIZE = config['mapping']['map_voxel_size'] 26 | self.STACK_NUM = config['mapping']['stack_num'] 27 | self.OPTIM_ITERATION = config['mapping']['optim_iteration'] 28 | 29 | self.CUBE_NUM = self.CLOUD_DEPTH * self.CLOUD_HEIGHT * self.CLOUD_WIDTH 30 | 31 | self.cloud_center_width = int(self.CLOUD_WIDTH/2) 32 | self.cloud_center_height = int(self.CLOUD_HEIGHT/2) 33 | self.cloud_center_depth = int(self.CLOUD_DEPTH/2) 34 | self.valid_index = [-1] * 125 35 | self.surround_index = [-1] * 125 36 | 37 | self.cloud_corner_array = [[] for i in range(self.CUBE_NUM)] 38 | self.cloud_surf_array = [[] for i in range(self.CUBE_NUM)] 39 | self.frame_count = 0 40 | 41 | self.rot_wodom_curr = np.eye(3) 42 | self.trans_wodom_curr = np.zeros((3,1)) 43 | self.rot_wmap_wodom = np.eye(3) 44 | self.trans_wmap_wodom = np.zeros((3,1)) 45 | self.rot_w_curr = np.eye(3) 46 | self.trans_w_curr = np.zeros((3,1)) 47 | self.transform = np.zeros(6) 48 | 49 | def transform_associate_to_map(self, rot_wodom_curr, trans_wodom_curr): 50 | self.rot_wodom_curr = rot_wodom_curr 51 | self.trans_wodom_curr = trans_wodom_curr 52 | self.rot_w_curr = np.matmul(self.rot_wmap_wodom, rot_wodom_curr) 53 | self.trans_w_curr = np.matmul(self.rot_wmap_wodom, trans_wodom_curr).reshape(3,1) + self.trans_wmap_wodom 54 | rx, ry, rz = get_euler_angles(self.rot_w_curr) 55 | self.transform = np.array([rx, ry, rz, self.trans_w_curr[0][0], self.trans_w_curr[1][0], self.trans_w_curr[2][0]]) 56 | 57 | def point_associate_to_map(self, pt): 58 | pt_out = np.matmul(self.rot_w_curr, pt.T).T + self.trans_w_curr.reshape(1, 3) 59 | return pt_out.squeeze() 60 | 61 | def transform_update(self): 62 | self.rot_wmap_wodom = np.matmul(self.rot_w_curr, self.rot_wodom_curr.T) 63 | self.trans_wmap_wodom = self.trans_w_curr - np.matmul(self.rot_wmap_wodom, self.trans_wodom_curr.reshape(3,1)) 64 | 65 | def transform_convert(self): 66 | self.rot_w_curr = get_rotation(self.transform[0], self.transform[1], self.transform[2]) 67 | self.trans_w_curr = self.transform[3:].reshape(3,1) 68 | 69 | def map_frame(self, odom, corner_last, surf_last): 70 | print('Mapping frame: ', self.frame_count) 71 | if self.frame_count % self.STACK_NUM != 0: 72 | self.frame_count += 1 73 | return None 74 | rot_wodom_curr = odom[:3, :3] 75 | trans_wodom_curr = odom[:3, 3].reshape(3,1) 76 | self.transform_associate_to_map(rot_wodom_curr, trans_wodom_curr) 77 | cube_center_i = int((self.trans_w_curr[0][0] + 25.0) / 50.0) + self.cloud_center_width 78 | cube_center_j = int((self.trans_w_curr[1][0] + 25.0) / 50.0) + self.cloud_center_height 79 | cube_center_k = int((self.trans_w_curr[2][0] + 25.0) / 50.0) + self.cloud_center_depth 80 | 81 | if self.trans_w_curr[0][0] + 25.0 < 0: 82 | cube_center_i -= 1 83 | if self.trans_w_curr[1][0] + 25.0 < 0: 84 | cube_center_j -= 1 85 | if self.trans_w_curr[2][0] + 25.0 < 0: 86 | cube_center_k -= 1 87 | 88 | is_degenerate = False 89 | 90 | while cube_center_i < 3: 91 | for j in range(self.CLOUD_HEIGHT): 92 | for k in range(self.CLOUD_DEPTH): 93 | i = self.CLOUD_WIDTH - 1 94 | ind = j * self.CLOUD_WIDTH + k * self.CLOUD_WIDTH * self.CLOUD_HEIGHT 95 | corner_tmp = self.cloud_corner_array[i+ind] 96 | surf_tmp = self.cloud_surf_array[i+ind] 97 | 98 | for tmp in range(i, 0, -1): 99 | self.cloud_corner_array[tmp+ind] = self.cloud_corner_array[tmp+ind-1] 100 | self.cloud_surf_array[tmp+ind] = self.cloud_surf_array[tmp+ind-1] 101 | self.cloud_corner_array[ind] = corner_tmp 102 | self.cloud_surf_array[ind] = surf_tmp 103 | 104 | cube_center_i += 1 105 | self.cloud_center_width += 1 106 | 107 | while cube_center_i >= self.CLOUD_WIDTH - 3: 108 | for j in range(self.CLOUD_HEIGHT): 109 | for k in range(self.CLOUD_DEPTH): 110 | i = 0 111 | ind = j * self.CLOUD_WIDTH + k * self.CLOUD_WIDTH * self.CLOUD_HEIGHT 112 | corner_tmp = self.cloud_corner_array[i+ind] 113 | surf_tmp = self.cloud_surf_array[i+ind] 114 | 115 | for tmp in range(0, self.CLOUD_WIDTH-1): 116 | self.cloud_corner_array[tmp+ind] = self.cloud_corner_array[tmp+ind+1] 117 | self.cloud_surf_array[tmp+ind] = self.cloud_surf_array[tmp+ind+1] 118 | self.cloud_corner_array[ind + self.CLOUD_WIDTH - 1] = corner_tmp 119 | self.cloud_surf_array[ind + self.CLOUD_WIDTH - 1] = surf_tmp 120 | 121 | cube_center_i -= 1 122 | self.cloud_center_width -= 1 123 | 124 | while cube_center_j < 3: 125 | for i in range(self.CLOUD_WIDTH): 126 | for k in range(self.CLOUD_DEPTH): 127 | j = self.CLOUD_HEIGHT - 1 128 | ind = i + k * self.CLOUD_WIDTH * self.CLOUD_HEIGHT 129 | corner_tmp = self.cloud_corner_array[j * self.CLOUD_WIDTH + ind] 130 | surf_tmp = self.cloud_surf_array[j * self.CLOUD_WIDTH + ind] 131 | 132 | for tmp in range(j, 0, -1): 133 | self.cloud_corner_array[tmp*self.CLOUD_WIDTH+ind] = self.cloud_corner_array[(tmp-1)*self.CLOUD_WIDTH+ind] 134 | self.cloud_surf_array[tmp*self.CLOUD_WIDTH+ind] = self.cloud_surf_array[(tmp-1)*self.CLOUD_WIDTH+ind] 135 | self.cloud_corner_array[ind] = corner_tmp 136 | self.cloud_surf_array[ind] = surf_tmp 137 | 138 | cube_center_j += 1 139 | self.cloud_center_height += 1 140 | 141 | while cube_center_j >= self.CLOUD_HEIGHT - 3: 142 | for i in range(self.CLOUD_WIDTH): 143 | for k in range(self.CLOUD_DEPTH): 144 | j = 0 145 | ind = i + k * self.CLOUD_WIDTH * self.CLOUD_HEIGHT 146 | corner_tmp = self.cloud_corner_array[j*self.CLOUD_WIDTH+ind] 147 | surf_tmp = self.cloud_surf_array[j*self.CLOUD_WIDTH+ind] 148 | 149 | for tmp in range(0, self.CLOUD_HEIGHT-1): 150 | self.cloud_corner_array[tmp*self.CLOUD_WIDTH+ind] = self.cloud_corner_array[(tmp+1)*self.CLOUD_WIDTH+ind] 151 | self.cloud_surf_array[tmp*self.CLOUD_WIDTH+ind] = self.cloud_corner_array[(tmp+1)*self.CLOUD_WIDTH+ind] 152 | self.cloud_corner_array[ind + (self.CLOUD_HEIGHT-1)*self.CLOUD_WIDTH] = corner_tmp 153 | self.cloud_surf_array[ind + (self.CLOUD_HEIGHT-1)*self.CLOUD_WIDTH] = surf_tmp 154 | 155 | cube_center_j -= 1 156 | self.cloud_center_height -= 1 157 | 158 | while cube_center_k < 3: 159 | for i in range(self.CLOUD_WIDTH): 160 | for j in range(self.CLOUD_HEIGHT): 161 | k = self.CLOUD_DEPTH - 1 162 | ind = i + j * self.CLOUD_WIDTH 163 | corner_tmp = self.cloud_corner_array[k * self.CLOUD_WIDTH * self.CLOUD_HEIGHT + ind] 164 | surf_tmp = self.cloud_surf_array[k * self.CLOUD_WIDTH * self.CLOUD_HEIGHT + ind] 165 | 166 | for tmp in range(k, 0, -1): 167 | self.cloud_corner_array[tmp * self.CLOUD_WIDTH * self.CLOUD_HEIGHT + ind] = self.cloud_corner_array[(tmp-1) * self.CLOUD_WIDTH * self.CLOUD_HEIGHT + ind] 168 | self.cloud_surf_array[tmp * self.CLOUD_WIDTH * self.CLOUD_HEIGHT + ind] = self.cloud_surf_array[(tmp-1) * self.CLOUD_WIDTH * self.CLOUD_HEIGHT + ind] 169 | 170 | self.cloud_corner_array[ind] = corner_tmp 171 | self.cloud_surf_array[ind] = surf_tmp 172 | 173 | cube_center_k += 1 174 | self.cloud_center_depth += 1 175 | 176 | while cube_center_k >= self.CLOUD_DEPTH - 3: 177 | for i in range(self.CLOUD_WIDTH): 178 | for j in range(self.CLOUD_HEIGHT): 179 | k = 0 180 | ind = i + j * self.CLOUD_WIDTH 181 | corner_tmp = self.cloud_corner_array[k * self.CLOUD_WIDTH * self.CLOUD_HEIGHT + ind] 182 | surf_tmp = self.cloud_surf_array[k * self.CLOUD_WIDTH * self.CLOUD_HEIGHT + ind] 183 | 184 | for tmp in range(0, self.CLOUD_DEPTH - 1): 185 | self.cloud_corner_array[tmp * self.CLOUD_WIDTH * self.CLOUD_HEIGHT + ind] = self.cloud_corner_array[(tmp+1) * self.CLOUD_WIDTH * self.CLOUD_HEIGHT + ind] 186 | self.cloud_surf_array[tmp * self.CLOUD_WIDTH * self.CLOUD_HEIGHT + ind] = self.cloud_surf_array[(tmp+1) * self.CLOUD_WIDTH * self.CLOUD_HEIGHT + ind] 187 | self.cloud_corner_array[ind + (self.CLOUD_DEPTH-1) * self.CLOUD_WIDTH * self.CLOUD_HEIGHT] = corner_tmp 188 | self.cloud_surf_array[ind + (self.CLOUD_DEPTH-1) * self.CLOUD_WIDTH * self.CLOUD_HEIGHT] = surf_tmp 189 | 190 | cube_center_k -= 1 191 | self.cloud_center_depth -= 1 192 | 193 | valid_cloud_num = 0 194 | surround_cloud_num = 0 195 | 196 | for i in range(cube_center_i - 2, cube_center_i + 3): 197 | for j in range(cube_center_j - 2, cube_center_j + 3): 198 | for k in range(cube_center_k - 1, cube_center_k + 2): 199 | if i>=0 and i=0 and j=0 and k 0: 212 | corner_from_map = np.vstack(map_corner_list) 213 | 214 | if len(map_surf_list) > 0: 215 | surf_from_map = np.vstack(map_surf_list) 216 | 217 | _, corner_last_ds = downsample_filter(corner_last, self.CORNER_VOXEL_SIZE) 218 | _, surf_last_ds = downsample_filter(surf_last, self.SURF_VOXEL_SIZE) 219 | 220 | if len(map_corner_list) > 0 and len(map_surf_list) > 0 and corner_from_map.shape[0] > 10 and surf_from_map.shape[0] > 100: 221 | corner_map_tree = o3d.geometry.KDTreeFlann(np.transpose(corner_from_map[:, :3])) 222 | surf_map_tree = o3d.geometry.KDTreeFlann(np.transpose(surf_from_map[:, :3])) 223 | 224 | for iter_num in range(self.OPTIM_ITERATION): 225 | coeff_list = [] 226 | pt_list = [] 227 | 228 | # Find corner correspondences 229 | for i in range(corner_last_ds.shape[0]): 230 | point_sel = self.point_associate_to_map(corner_last_ds[i, :3]) 231 | [_, ind, dist] = corner_map_tree.search_knn_vector_3d(point_sel, 5) 232 | if dist[4] < 1.0: 233 | center, cov = get_mean_cov(corner_from_map[ind, :3]) 234 | vals, vecs = np.linalg.eig(cov) 235 | idx = vals.argsort() # Sort ascending 236 | vals = vals[idx] 237 | vecs = vecs[:, idx] 238 | 239 | if vals[2] > 3 * vals[1]: 240 | point_a = center + 0.1 * vecs[:, 2] 241 | point_b = center - 0.1 * vecs[:, 2] 242 | edge_normal = np.cross((point_sel - point_a), (point_sel - point_b), axis=0) 243 | edge_norm = np.linalg.norm(edge_normal) 244 | ab = point_a - point_b 245 | ab_norm = np.linalg.norm(ab) 246 | 247 | la = (ab[1]*edge_normal[2] + ab[2]*edge_normal[1]) / (ab_norm*edge_norm) 248 | lb = -(ab[0]*edge_normal[2] + ab[2]*edge_normal[0]) / (ab_norm*edge_norm) 249 | lc = (ab[0]*edge_normal[1] - ab[1]*edge_normal[0]) / (ab_norm*edge_norm) 250 | 251 | ld = edge_norm / ab_norm 252 | 253 | s = 1 - 0.9 * np.abs(ld) 254 | 255 | if s > 0.1: 256 | pt_list.append(corner_last_ds[i, :3]) 257 | coeff_list.append(np.array([s*la, s*lb, s*lc, s*ld])) 258 | 259 | # Find surface correspondences 260 | for i in range(surf_last_ds.shape[0]): 261 | point_sel = self.point_associate_to_map(surf_last_ds[i, :3]) 262 | [_, ind, dist] = surf_map_tree.search_knn_vector_3d(point_sel, 5) 263 | 264 | if dist[4] < 1.0: 265 | surf_normal, _, _, _ = np.linalg.lstsq(surf_from_map[ind, :3], -np.ones((5,1)), rcond=None) 266 | surf_norm = np.linalg.norm(surf_normal) 267 | coeff = np.append(surf_normal, 1.0) / surf_norm 268 | 269 | surf_homo = np.concatenate((surf_from_map[ind, :3], np.ones((5,1))), axis=1) 270 | plane_residual = np.abs(np.matmul(surf_homo, coeff.reshape(4,1))) 271 | 272 | if np.any(plane_residual > 0.2): 273 | continue 274 | 275 | pd2 = np.dot(np.append(point_sel, 1), coeff) 276 | s = 1 - 0.9 * np.abs(pd2) / np.sqrt(np.linalg.norm(point_sel)) 277 | 278 | coeff[3] = pd2 279 | coeff = s*coeff 280 | 281 | if s > 0.1: 282 | coeff_list.append(coeff) 283 | pt_list.append(surf_last_ds[i, :3]) 284 | 285 | if len(coeff_list) < 50: 286 | print("Warning: Few matches") 287 | continue 288 | 289 | srx = np.sin(self.transform[0]) 290 | crx = np.cos(self.transform[0]) 291 | sry = np.sin(self.transform[1]) 292 | cry = np.cos(self.transform[1]) 293 | srz = np.sin(self.transform[2]) 294 | crz = np.cos(self.transform[2]) 295 | 296 | A_mat = [] 297 | B_mat = [] 298 | 299 | for i in range(len(coeff_list)): 300 | A_tmp = np.zeros((1,6)) 301 | B_tmp = np.zeros((1,1)) 302 | A_tmp[0, 0] = (crx*sry*srz*pt_list[i][0] + crx*crz*sry*pt_list[i][1] - srx*sry*pt_list[i][2]) * coeff_list[i][0] \ 303 | + (-srx*srz*pt_list[i][0] - crz*srx*pt_list[i][1] - crx*pt_list[i][2]) * coeff_list[i][1] \ 304 | + (crx*cry*srz*pt_list[i][0] + crx*cry*crz*pt_list[i][1] - cry*srx*pt_list[i][2]) * coeff_list[i][2] 305 | 306 | A_tmp[0, 1] = ((cry*srx*srz - crz*sry)*pt_list[i][0] \ 307 | + (sry*srz + cry*crz*srx)*pt_list[i][1] + crx*cry*pt_list[i][2]) * coeff_list[i][0] \ 308 | + ((-cry*crz - srx*sry*srz)*pt_list[i][0] \ 309 | + (cry*srz - crz*srx*sry)*pt_list[i][1] - crx*sry*pt_list[i][2]) * coeff_list[i][2] 310 | 311 | A_tmp[0, 2] = ((crz*srx*sry - cry*srz)*pt_list[i][0] + (-cry*crz-srx*sry*srz)*pt_list[i][1])*coeff_list[i][0] \ 312 | + (crx*crz*pt_list[i][0] - crx*srz*pt_list[i][1]) * coeff_list[i][1] \ 313 | + ((sry*srz + cry*crz*srx)*pt_list[i][0] + (crz*sry-cry*srx*srz)*pt_list[i][1])*coeff_list[i][2] 314 | 315 | A_tmp[0, 3] = coeff_list[i][0] 316 | A_tmp[0, 4] = coeff_list[i][1] 317 | A_tmp[0, 5] = coeff_list[i][2] 318 | 319 | B_tmp[0,0] = -coeff_list[i][3] 320 | 321 | A_mat.append(A_tmp) 322 | B_mat.append(B_tmp) 323 | 324 | A_mat = np.vstack(A_mat) 325 | B_mat = np.vstack(B_mat) 326 | AtA = np.matmul(A_mat.T, A_mat) 327 | AtB = np.matmul(A_mat.T, B_mat) 328 | X_mat = np.linalg.solve(AtA, AtB) 329 | 330 | if iter_num == 0: 331 | vals, vecs = np.linalg.eig(AtA) 332 | eigen_vec = vecs.copy() 333 | for i in range(6): 334 | if vals[i] < 100: 335 | print("Warning: Degenerate!") 336 | is_degenerate = True 337 | eigen_vec[:, i] = np.zeros(6) 338 | else: 339 | break 340 | P_mat = np.matmul(np.linalg.inv(vecs), eigen_vec) 341 | 342 | if is_degenerate: 343 | X_mat = np.matmul(P_mat, X_mat) 344 | 345 | self.transform += np.squeeze(X_mat) 346 | self.transform_convert() 347 | 348 | delta_r = np.linalg.norm(np.rad2deg(X_mat[:3])) 349 | delta_t = np.linalg.norm(X_mat[3:] * 100) 350 | # print("{} frame, {} iter, [{},{},{}] delta translation".format(self.frame_count, iter_num, self.transform[3], self.transform[4], self.transform[5])) 351 | if delta_r < 0.05 and delta_t < 0.05: 352 | print("Mapping converged.") 353 | break 354 | 355 | self.transform_update() 356 | print("Frame: {}, Transform after mapping: {}".format(self.frame_count, self.transform)) 357 | else: 358 | print("Few corner and edges in map.") 359 | 360 | new_points = [] 361 | for i in range(corner_last_ds.shape[0]): 362 | point_sel = self.point_associate_to_map(corner_last_ds[i, :3]) 363 | cube_i = int((point_sel[0] + 25.0) / 50.0) + self.cloud_center_width 364 | cube_j = int((point_sel[1] + 25.0) / 50.0) + self.cloud_center_height 365 | cube_k = int((point_sel[2] + 25.0) / 50.0) + self.cloud_center_depth 366 | 367 | if point_sel[0] + 25 < 0: 368 | cube_i -= 1 369 | if point_sel[1] + 25 < 0: 370 | cube_j -= 1 371 | if point_sel[2] + 25 < 0: 372 | cube_k -= 1 373 | 374 | if cube_i >=0 and cube_i < self.CLOUD_WIDTH and cube_j >= 0 and cube_j < self.CLOUD_HEIGHT and cube_k >= 0 and cube_k < self.CLOUD_DEPTH: 375 | cube_ind = cube_i + cube_j * self.CLOUD_WIDTH + cube_k * self.CLOUD_WIDTH * self.CLOUD_HEIGHT 376 | self.cloud_corner_array[cube_ind].append(point_sel) 377 | new_points.append(point_sel) 378 | 379 | for i in range(surf_last_ds.shape[0]): 380 | point_sel = self.point_associate_to_map(surf_last_ds[i, :3]) 381 | 382 | cube_i = int((point_sel[0] + 25.0) / 50.0) + self.cloud_center_width 383 | cube_j = int((point_sel[1] + 25.0) / 50.0) + self.cloud_center_height 384 | cube_k = int((point_sel[2] + 25.0) / 50.0) + self.cloud_center_depth 385 | 386 | if point_sel[0] + 25 < 0: 387 | cube_i -= 1 388 | if point_sel[1] + 25 < 0: 389 | cube_j -= 1 390 | if point_sel[2] + 25 < 0: 391 | cube_k -= 1 392 | 393 | if cube_i >=0 and cube_i < self.CLOUD_WIDTH and cube_j >= 0 and cube_j < self.CLOUD_HEIGHT and cube_k >= 0 and cube_k < self.CLOUD_DEPTH: 394 | cube_ind = cube_i + cube_j * self.CLOUD_WIDTH + cube_k * self.CLOUD_WIDTH * self.CLOUD_HEIGHT 395 | self.cloud_surf_array[cube_ind].append(point_sel) 396 | new_points.append(point_sel) 397 | 398 | for i in range(valid_cloud_num): 399 | ind = self.valid_index[i] 400 | if len(self.cloud_corner_array[ind]) > 0: 401 | _, ds_corner = downsample_filter(np.vstack(self.cloud_corner_array[ind]), 0.4) 402 | self.cloud_corner_array[ind] = cloud_to_list(ds_corner) 403 | 404 | if len(self.cloud_surf_array[ind]) > 0: 405 | _, ds_surf = downsample_filter(np.vstack(self.cloud_surf_array[ind]), 0.8) 406 | self.cloud_surf_array[ind] = cloud_to_list(ds_surf) 407 | 408 | if self.frame_count % 20 == 0: 409 | map_pts = [] 410 | for i in range(self.CUBE_NUM): 411 | map_pts += self.cloud_surf_array[i] 412 | map_pts += self.cloud_corner_array[i] 413 | map_pts = np.vstack(map_pts) 414 | np.savetxt('Mapped_frame_' + str(self.frame_count) + '.txt', map_pts, fmt='%.8f') 415 | 416 | self.frame_count += 1 417 | return self.trans_w_curr 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | -------------------------------------------------------------------------------- /src/laser_odometry.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import open3d as o3d 3 | from feature_extract import FeatureExtract 4 | from scipy.spatial.transform import Rotation as R 5 | from scipy.spatial.transform import Slerp 6 | from utils import * 7 | import math 8 | 9 | class Odometry: 10 | def __init__(self, config=None): 11 | self.config = config 12 | self.init = False 13 | self.surf_last = None 14 | self.corner_last = None 15 | self.feature_extractor = FeatureExtract(config=config) 16 | self.rot_w_curr = np.eye(3) 17 | self.trans_w_curr = np.zeros((3,1)) 18 | self.transform = np.array([0., 0., 0., 0., 0., 0.]) # rx, ry, rz, tx, ty, tz 19 | self.frame_count = 0 20 | if config is None: 21 | self.DIST_THRES = 5 22 | self.RING_INDEX = 4 23 | self.NEARBY_SCAN = 2.5 24 | self.OPTIM_ITERATION = 25 25 | self.VOXEL_SIZE = 0.2 26 | else: 27 | self.DIST_THRES = config['odometry']['dist_threshold'] 28 | self.RING_INDEX = config['odometry']['ring_index'] 29 | self.NEARBY_SCAN = config['odometry']['nearby_scan'] 30 | self.OPTIM_ITERATION = config['odometry']['optim_iteration'] 31 | self.VOXEL_SIZE = config['odometry']['voxel_size'] 32 | 33 | # For test 34 | self.trans_list = [] 35 | 36 | def grab_frame(self, cloud): 37 | corner_sharp, corner_less, surf_flat, surf_less = self.feature_extractor.feature_extract(cloud) 38 | is_degenerate = False 39 | T_w_curr = np.eye(4) 40 | print("Processing frame: ", self.frame_count) 41 | if not self.init: 42 | self.init = True 43 | else: 44 | P_mat = np.identity(6) 45 | if self.surf_last.shape[0] < 100 or self.corner_last.shape[0] < 10: 46 | print("Warning: too few points in last frame") 47 | return self.surf_last, self.corner_last, T_w_curr 48 | 49 | for opt_iter in range(self.OPTIM_ITERATION): 50 | if opt_iter % 5 == 0: 51 | corner_points, corner_points_a, corner_points_b = self.get_corner_correspondences(corner_sharp) 52 | surf_points, surf_points_a, surf_points_b, surf_points_c = self.get_surf_correspondences(surf_flat) 53 | edge_A, edge_B = self.get_edge_mat(corner_points, corner_points_a, corner_points_b, opt_iter) 54 | surf_A, surf_B = self.get_plane_mat(surf_points, surf_points_a, surf_points_b, surf_points_c, opt_iter) 55 | 56 | A_mat = np.vstack((edge_A, surf_A)) 57 | B_mat = np.vstack((edge_B, surf_B)) * -0.05 # Reference to original LOAM 58 | 59 | if A_mat.shape[0] < 10: 60 | print('Warning: too few matches') 61 | continue 62 | 63 | AtA = np.matmul(A_mat.T, A_mat) 64 | AtB = np.matmul(A_mat.T, B_mat) 65 | X_mat = np.linalg.solve(AtA, AtB) 66 | 67 | if opt_iter == 0: 68 | vals, vecs = np.linalg.eig(AtA) 69 | eigen_vec = vecs.copy() 70 | for i in range(6): 71 | if vals[i] < 10: 72 | print("Warning: Degenerate!") 73 | is_degenerate = True 74 | eigen_vec[:, i] = np.zeros(6) 75 | else: 76 | break 77 | P_mat = np.matmul(np.linalg.inv(vecs), eigen_vec) 78 | 79 | if is_degenerate: 80 | X_mat = np.matmul(P_mat, X_mat) 81 | 82 | self.transform += np.squeeze(X_mat) 83 | 84 | delta_r = np.linalg.norm(np.rad2deg(X_mat[:3])) 85 | delta_t = np.linalg.norm(X_mat[3:] * 100) 86 | # print("{} frame, {} iter, [{},{},{}] delta translation".format(self.frame_count, opt_iter, self.transform[3], self.transform[4], self.transform[5])) 87 | if delta_r < 0.1 and delta_t < 0.1: 88 | print("Odometry converged.") 89 | break 90 | 91 | print("Transform: ", self.transform) 92 | T_last_curr = np.eye(4) 93 | T_w_last = np.eye(4) 94 | T_last_curr[0:3, 0:3] = get_rotation(self.transform[0], self.transform[1], self.transform[2]).T 95 | T_last_curr[0:3, 3] = -np.matmul(T_last_curr[0:3, 0:3], self.transform[3:].reshape(3,1)).reshape(3) 96 | T_w_last[0:3, 0:3] = self.rot_w_curr 97 | T_w_last[0:3, 3] = self.trans_w_curr.reshape(3) 98 | T_w_curr = np.matmul(T_w_last, T_last_curr) 99 | 100 | self.rot_w_curr = T_w_curr[0:3, 0:3] 101 | self.trans_w_curr = T_w_curr[0:3, 3].reshape(3,1) 102 | self.trans_list.append(self.trans_w_curr) 103 | 104 | # Transform surf_less and corner_less to the end 105 | for i in range(surf_less.shape[0]): 106 | surf_less[i, :3] = self.transform_to_end(surf_less[i, :3], surf_less[i, -1]).T 107 | for i in range(corner_less.shape[0]): 108 | corner_less[i, :3] = self.transform_to_end(corner_less[i, :3], corner_less[i, -1]).T 109 | 110 | if surf_less.shape[0] > 100 and corner_less.shape[0] > 10: 111 | self.surf_last = surf_less 112 | self.corner_last = corner_less 113 | self.frame_count += 1 114 | return self.surf_last, self.corner_last, T_w_curr 115 | 116 | def get_corner_correspondences(self, corner_sharp): 117 | curr_points = [] 118 | points_a = [] 119 | points_b = [] 120 | corner_last_tree = o3d.geometry.KDTreeFlann(np.transpose(self.corner_last[:, :3])) 121 | 122 | for i in range(corner_sharp.shape[0]): 123 | point_sel = self.transform_to_start(corner_sharp[i, :3], corner_sharp[i, -1]) 124 | [_, ind, dist] = corner_last_tree.search_knn_vector_3d(point_sel, 1) 125 | closest_ind = -1 126 | min_ind2 = -1 127 | if dist[0] < self.DIST_THRES: 128 | closest_ind = ind[0] 129 | closest_scan_id = self.corner_last[ind[0], self.RING_INDEX] 130 | min_sq_dist2 = self.DIST_THRES * self.DIST_THRES 131 | 132 | for j in range(closest_ind+1, corner_sharp.shape[0]): 133 | if self.corner_last[j, self.RING_INDEX] <= closest_scan_id: 134 | continue 135 | if self.corner_last[j, self.RING_INDEX] > closest_scan_id + self.NEARBY_SCAN: 136 | break 137 | 138 | point_sq_dist = np.sum(np.square(self.corner_last[j, :3].reshape(3,1) - point_sel)) 139 | if point_sq_dist < min_sq_dist2: 140 | min_sq_dist2 = point_sq_dist 141 | min_ind2 = j 142 | 143 | for j in range(closest_ind-1, -1, -1): 144 | if self.corner_last[j, self.RING_INDEX] >= closest_scan_id: 145 | continue 146 | if self.corner_last[j, self.RING_INDEX] < closest_scan_id - self.NEARBY_SCAN: 147 | break 148 | 149 | point_sq_dist = np.sum(np.square(self.corner_last[j, :3].reshape(3,1) - point_sel)) 150 | if point_sq_dist < min_sq_dist2: 151 | min_sq_dist2 = point_sq_dist 152 | min_ind2 = j 153 | 154 | if min_ind2 >= 0: 155 | ab_dist = np.sum(np.square(self.corner_last[min_ind2, :3]-self.corner_last[closest_ind, :3])) 156 | if ab_dist < 1e-3: 157 | continue 158 | curr_points.append(corner_sharp[i, :]) 159 | points_a.append(self.corner_last[closest_ind, :]) 160 | points_b.append(self.corner_last[min_ind2, :]) 161 | 162 | return curr_points, points_a, points_b 163 | 164 | def get_surf_correspondences(self, surf_flat): 165 | curr_points = [] 166 | points_a = [] 167 | points_b = [] 168 | points_c = [] 169 | surf_last_tree = o3d.geometry.KDTreeFlann(np.transpose(self.surf_last[:, :3])) 170 | for i in range(surf_flat.shape[0]): 171 | point_sel = self.transform_to_start(surf_flat[i,:3], surf_flat[i, -1]) 172 | [_, ind, dist] = surf_last_tree.search_knn_vector_3d(point_sel, 1) 173 | closest_ind = -1 174 | min_ind2 = -1 175 | min_ind3 = -1 176 | if dist[0] < self.DIST_THRES: 177 | closest_ind = ind[0] 178 | closest_scan_id = self.surf_last[ind[0], self.RING_INDEX] 179 | min_sq_dist2 = self.DIST_THRES * self.DIST_THRES 180 | min_sq_dist3 = self.DIST_THRES * self.DIST_THRES 181 | 182 | for j in range(closest_ind+1, surf_flat.shape[0]): 183 | if self.surf_last[j, self.RING_INDEX] > closest_scan_id + self.NEARBY_SCAN: 184 | break 185 | point_sq_dist = np.sum(np.square(self.surf_last[j, :3].reshape(3,1) - point_sel)) 186 | if self.surf_last[j, self.RING_INDEX] <= closest_scan_id and point_sq_dist < min_sq_dist2: 187 | min_sq_dist2 = point_sq_dist 188 | min_ind2 = j 189 | elif point_sq_dist < min_sq_dist3: 190 | min_sq_dist3 = point_sq_dist 191 | min_ind3 = j 192 | 193 | for j in range(closest_ind-1, -1, -1): 194 | if self.surf_last[j, self.RING_INDEX] < closest_scan_id - self.NEARBY_SCAN: 195 | break 196 | point_sq_dist = np.sum(np.square(self.surf_last[j, :3].reshape(3,1) - point_sel)) 197 | if self.surf_last[j, self.RING_INDEX] >= closest_scan_id and point_sq_dist < min_sq_dist2: 198 | min_sq_dist2 = point_sq_dist 199 | min_ind2 = j 200 | elif point_sq_dist < min_sq_dist3: 201 | min_sq_dist3 = point_sq_dist 202 | min_ind3 = j 203 | 204 | if min_ind2 >= 0 and min_ind3 >= 0: 205 | ab_dist = np.sum(np.square(self.surf_last[min_ind2, :3]-self.surf_last[closest_ind, :3])) 206 | ac_dist = np.sum(np.square(self.surf_last[min_ind3, :3]-self.surf_last[closest_ind, :3])) 207 | if ab_dist < 1e-3 or ac_dist < 1e-3: 208 | continue 209 | curr_points.append(surf_flat[i, :]) 210 | points_a.append(self.surf_last[closest_ind, :]) 211 | points_b.append(self.surf_last[min_ind2, :]) 212 | points_c.append(self.surf_last[min_ind3, :]) 213 | 214 | return curr_points, points_a, points_b, points_c 215 | 216 | def get_downsample_cloud(self, cloud): 217 | o3d_cloud = o3d.geometry.PointCloud() 218 | o3d_cloud.points = o3d.utility.Vector3dVector(cloud[:, :3]) 219 | max_bound = o3d_cloud.get_max_bound() + self.VOXEL_SIZE * 0.5 220 | min_bound = o3d_cloud.get_min_bound() - self.VOXEL_SIZE * 0.5 221 | out = o3d_cloud.voxel_down_sample_and_trace(self.VOXEL_SIZE, min_bound, max_bound, False) 222 | index_ds = [cubic_index[0] for cubic_index in out[2]] 223 | return index_ds 224 | 225 | def transform_to_start(self, pt, s=1.0): 226 | scaled_transform = s * self.transform 227 | rot_mat = get_rotation(scaled_transform[0], scaled_transform[1], scaled_transform[2]) 228 | translation = scaled_transform[3:6] 229 | 230 | if len(pt.shape) == 1: 231 | pt = pt.reshape(3,-1) 232 | if pt.shape[0] != 3: 233 | pt = pt.T 234 | 235 | undistorted_pt = np.transpose(rot_mat).dot(pt - translation.reshape(3,1)) 236 | return undistorted_pt 237 | 238 | def transform_to_end(self, pt, s=1.0): 239 | un_point = self.transform_to_start(pt, s) 240 | rot_mat = get_rotation(self.transform[0], self.transform[1], self.transform[2]) 241 | translation = self.transform[3:] 242 | pt_end = rot_mat.dot(un_point) + translation.reshape(3,1) 243 | return pt_end 244 | 245 | def get_plane_mat(self, surf_points, surf_points_a, surf_points_b, surf_points_c, iter_num): 246 | A_mat = [] 247 | B_mat = [] 248 | 249 | srx = np.sin(self.transform[0]) 250 | crx = np.cos(self.transform[0]) 251 | sry = np.sin(self.transform[1]) 252 | cry = np.cos(self.transform[1]) 253 | srz = np.sin(self.transform[2]) 254 | crz = np.cos(self.transform[2]) 255 | tx = self.transform[3] 256 | ty = self.transform[4] 257 | tz = self.transform[5] 258 | 259 | weight = 1.0 260 | 261 | for i in range(len(surf_points)): 262 | s = surf_points[i][-1] 263 | pt = surf_points[i][:3].reshape(3,1) 264 | pt_a = surf_points_a[i][:3].reshape(3,1) 265 | pt_b = surf_points_b[i][:3].reshape(3,1) 266 | pt_c = surf_points_c[i][:3].reshape(3,1) 267 | pt_sel = self.transform_to_start(pt, s) 268 | plane_norm = np.cross((pt_b - pt_a), (pt_c - pt_a), axis=0) 269 | norm = np.linalg.norm(plane_norm) 270 | plane_norm = plane_norm / norm 271 | dist = np.dot(np.transpose(plane_norm),(pt_sel - pt_a)) 272 | 273 | if iter_num >= 5: 274 | weight = 1 - 1.8 * abs(dist) / np.sqrt(np.linalg.norm(pt_sel)) 275 | 276 | if norm < 1e-5 or weight <= 0.1: 277 | continue 278 | 279 | plane_norm = weight * plane_norm 280 | A_tmp = np.zeros((1,6)) 281 | B_tmp = np.zeros((1,1)) 282 | 283 | B_tmp[0, 0] = dist * weight 284 | A_tmp[0, 0] = (-crx*sry*srz*pt[0] + crx*crz*sry*pt[1] + srx*sry*pt[2] \ 285 | + tx*crx*sry*srz - ty*crx*crz*sry - tz*srx*sry) * plane_norm[0] \ 286 | + (srx*srz*pt[0] - crz*srx*pt[1] + crx*pt[2] \ 287 | + ty*crz*srx - tz*crx - tx*srx*srz) * plane_norm[1] \ 288 | + (crx*cry*srz*pt[0] - crx*cry*crz*pt[1] - cry*srx*pt[2] \ 289 | + tz*cry*srx + ty*crx*cry*crz - tx*crx*cry*srz) * plane_norm[2] 290 | A_tmp[0, 1] = ((-crz*sry - cry*srx*srz)*pt[0] \ 291 | + (cry*crz*srx - sry*srz)*pt[1] - crx*cry*pt[2] \ 292 | + tx*(crz*sry + cry*srx*srz) + ty*(sry*srz - cry*crz*srx) \ 293 | + tz*crx*cry) * plane_norm[0] \ 294 | + ((cry*crz - srx*sry*srz)*pt[0] \ 295 | + (cry*srz + crz*srx*sry)*pt[1] - crx*sry*pt[2] \ 296 | + tz*crx*sry - ty*(cry*srz + crz*srx*sry) \ 297 | - tx*(cry*crz - srx*sry*srz)) * plane_norm[2] 298 | A_tmp[0, 2] = ((-cry*srz - crz*srx*sry)*pt[0] + (cry*crz - srx*sry*srz)*pt[1] \ 299 | + tx*(cry*srz + crz*srx*sry) - ty*(cry*crz - srx*sry*srz)) * plane_norm[0] \ 300 | + (-crx*crz*pt[0] - crx*srz*pt[1] \ 301 | + ty*crx*srz + tx*crx*crz) * plane_norm[1] \ 302 | + ((cry*crz*srx - sry*srz)*pt[0] + (crz*sry + cry*srx*srz)*pt[1] \ 303 | + tx*(sry*srz - cry*crz*srx) - ty*(crz*sry + cry*srx*srz)) * plane_norm[2] 304 | A_tmp[0, 3] = -(cry*crz - srx*sry*srz) * plane_norm[0] + crx*srz * plane_norm[1] \ 305 | - (crz*sry + cry*srx*srz) * plane_norm[2] 306 | A_tmp[0, 4] = -(cry*srz + crz*srx*sry) * plane_norm[0] - crx*crz * plane_norm[1] \ 307 | - (sry*srz - cry*crz*srx) * plane_norm[2] 308 | A_tmp[0, 5] = crx*sry * plane_norm[0] - srx * plane_norm[1] - crx*cry * plane_norm[2] 309 | 310 | A_mat.append(A_tmp) 311 | B_mat.append(B_tmp) 312 | 313 | if len(A_mat) != 0 and len(B_mat) !=0: 314 | A_mat = np.vstack(A_mat) 315 | B_mat = np.vstack(B_mat) 316 | else: 317 | A_mat = np.zeros((0,6)) 318 | B_mat = np.zeros((0,1)) 319 | 320 | return A_mat, B_mat 321 | 322 | def get_edge_mat(self, corner_points, corner_points_a, corner_points_b, iter_num): 323 | A_mat = [] 324 | B_mat = [] 325 | 326 | srx = np.sin(self.transform[0]) 327 | crx = np.cos(self.transform[0]) 328 | sry = np.sin(self.transform[1]) 329 | cry = np.cos(self.transform[1]) 330 | srz = np.sin(self.transform[2]) 331 | crz = np.cos(self.transform[2]) 332 | tx = self.transform[3] 333 | ty = self.transform[4] 334 | tz = self.transform[5] 335 | 336 | weight = 1.0 337 | 338 | for i in range(len(corner_points)): 339 | s = corner_points[i][-1] 340 | pt = corner_points[i][:3].reshape(3,1) 341 | pt_a = corner_points_a[i][:3].reshape(3,1) 342 | pt_b = corner_points_b[i][:3].reshape(3,1) 343 | pt_sel = self.transform_to_start(pt, s) 344 | edge_normal = np.cross((pt_sel - pt_a), (pt_sel - pt_b), axis=0) 345 | ab = pt_a - pt_b 346 | ab_norm = np.linalg.norm(ab) 347 | edge_norm = np.linalg.norm(edge_normal) 348 | 349 | if iter_num >= 5: 350 | weight = 1 - 1.8 * abs(edge_norm / ab_norm) 351 | 352 | if edge_norm < 1e-5 or weight <= 0.1: 353 | continue 354 | 355 | la = weight * (ab[1]*edge_normal[2] + ab[2]*edge_normal[1]) / (ab_norm*edge_norm) 356 | lb = -weight * (ab[0]*edge_normal[2] + ab[2]*edge_normal[0]) / (ab_norm*edge_norm) 357 | lc = weight * (ab[0]*edge_normal[1] - ab[1]*edge_normal[0]) / (ab_norm*edge_norm) 358 | 359 | A_tmp = np.zeros((1,6)) 360 | B_tmp = np.zeros((1,1)) 361 | 362 | B_tmp[0, 0] = weight * (edge_norm / ab_norm) 363 | A_tmp[0, 0] = (-crx*sry*srz*pt[0] + crx*crz*sry*pt[1] + srx*sry*pt[2] \ 364 | + tx*crx*sry*srz - ty*crx*crz*sry - tz*srx*sry) * la \ 365 | + (srx*srz*pt[0] - crz*srx*pt[1] + crx*pt[2] \ 366 | + ty*crz*srx - tz*crx - tx*srx*srz) * lb \ 367 | + (crx*cry*srz*pt[0] - crx*cry*crz*pt[1] - cry*srx*pt[2] \ 368 | + tz*cry*srx + ty*crx*cry*crz - tx*crx*cry*srz) * lc 369 | A_tmp[0, 1] = ((-crz*sry - cry*srx*srz)*pt[0] \ 370 | + (cry*crz*srx - sry*srz)*pt[1] - crx*cry*pt[2] \ 371 | + tx*(crz*sry + cry*srx*srz) + ty*(sry*srz - cry*crz*srx) \ 372 | + tz*crx*cry) * la \ 373 | + ((cry*crz - srx*sry*srz)*pt[0] \ 374 | + (cry*srz + crz*srx*sry)*pt[1] - crx*sry*pt[2] \ 375 | + tz*crx*sry - ty*(cry*srz + crz*srx*sry) \ 376 | - tx*(cry*crz - srx*sry*srz)) * lc 377 | A_tmp[0, 2] = ((-cry*srz - crz*srx*sry)*pt[0] + (cry*crz - srx*sry*srz)*pt[1] \ 378 | + tx*(cry*srz + crz*srx*sry) - ty*(cry*crz - srx*sry*srz)) * la \ 379 | + (-crx*crz*pt[0] - crx*srz*pt[1] \ 380 | + ty*crx*srz + tx*crx*crz) * lb \ 381 | + ((cry*crz*srx - sry*srz)*pt[0] + (crz*sry + cry*srx*srz)*pt[1] \ 382 | + tx*(sry*srz - cry*crz*srx) - ty*(crz*sry + cry*srx*srz)) * lc 383 | A_tmp[0, 3] = -(cry*crz - srx*sry*srz) * la + crx*srz * lb \ 384 | - (crz*sry + cry*srx*srz) * lc 385 | A_tmp[0, 4] = -(cry*srz + crz*srx*sry) * la - crx*crz * lb \ 386 | - (sry*srz - cry*crz*srx) * lc 387 | A_tmp[0, 5] = crx*sry * la - srx * lb - crx*cry * lc 388 | 389 | A_mat.append(A_tmp) 390 | B_mat.append(B_tmp) 391 | 392 | if len(A_mat) != 0 and len(B_mat) !=0: 393 | A_mat = np.vstack(A_mat) 394 | B_mat = np.vstack(B_mat) 395 | else: 396 | A_mat = np.zeros((0,6)) 397 | B_mat = np.zeros((0,1)) 398 | 399 | return A_mat, B_mat 400 | 401 | def get_trans_list(self): 402 | return self.trans_list -------------------------------------------------------------------------------- /src/main_kitti.py: -------------------------------------------------------------------------------- 1 | from data_loader import KittiLoader 2 | from laser_odometry import Odometry 3 | from laser_mapping import Mapper 4 | import argparse 5 | import json 6 | import numpy as np 7 | 8 | parser = argparse.ArgumentParser() 9 | parser.add_argument('--kitti_path', type=str, help='Input folder of KITTI .bin files') 10 | parser.add_argument('--config_path', type=str, help='Configuration file') 11 | 12 | if __name__== '__main__': 13 | args = parser.parse_args() 14 | loader = KittiLoader(path=args.kitti_path, name='Kitti dataset') 15 | 16 | config = None 17 | if args.config_path is not None: 18 | with open(args.config_path) as config_file: 19 | config_data = config_file.read() 20 | config = json.loads(config_data) 21 | 22 | odometry = Odometry(config=config) 23 | mapper = Mapper(config=config) 24 | skip_frame = 5 25 | res_mapped = [] 26 | res_odom = [] 27 | 28 | for i in range(len(loader)): 29 | cloud = loader[i] 30 | surf_pts, corner_pts, odom = odometry.grab_frame(cloud) 31 | res_odom.append(odom[0:3, 3].reshape(3)) 32 | if i % skip_frame == 0: 33 | trans = mapper.map_frame(odom, corner_pts, surf_pts) 34 | if trans is not None: 35 | res_mapped.append(trans.reshape(3)) 36 | 37 | res = np.vstack(res_mapped) 38 | np.savetxt("mapped.txt", res, fmt='%.8f') 39 | 40 | res = np.vstack(res_odom) 41 | np.savetxt("odom.txt", res, fmt='%.8f') -------------------------------------------------------------------------------- /src/main_loam.py: -------------------------------------------------------------------------------- 1 | from data_loader import NPYLoader 2 | from laser_odometry import Odometry 3 | from laser_mapping import Mapper 4 | import argparse 5 | import json 6 | 7 | parser = argparse.ArgumentParser() 8 | parser.add_argument('--npy_path', type=str, help='Input folder of npy files') 9 | parser.add_argument('--config_path', type=str, help='Configuration file') 10 | 11 | if __name__== '__main__': 12 | args = parser.parse_args() 13 | loader = NPYLoader(path=args.npy_path, name='NSH indoor') 14 | 15 | config = None 16 | if args.config_path is not None: 17 | with open(args.config_path) as config_file: 18 | config_data = config_file.read() 19 | config = json.loads(config_data) 20 | 21 | odometry = Odometry(config=config) 22 | mapper = Mapper(config=config) 23 | 24 | for i in range(len(loader)): 25 | cloud = loader[i] 26 | surf_pts, corner_pts, odom = odometry.grab_frame(cloud) 27 | trans = mapper.map_frame(odom, corner_pts, surf_pts) -------------------------------------------------------------------------------- /src/utils.py: -------------------------------------------------------------------------------- 1 | from scipy.spatial.transform import Rotation as R 2 | from scipy.spatial.transform import Slerp 3 | import numpy as np 4 | import open3d as o3d 5 | 6 | def get_rotation(rx, ry, rz): 7 | r = R.from_euler('yxz', [ry, rx, rz], degrees=False) 8 | return r.as_dcm() 9 | 10 | def get_euler_angles(rot_mat): 11 | r = R.from_dcm(rot_mat) 12 | deg = r.as_euler('yxz', degrees=False) 13 | return deg[1], deg[0], deg[2] 14 | 15 | def downsample_filter(cloud, voxel_size): 16 | o3d_cloud = o3d.geometry.PointCloud() 17 | o3d_cloud.points = o3d.utility.Vector3dVector(cloud[:, :3]) 18 | max_bound = o3d_cloud.get_max_bound() + voxel_size * 0.5 19 | min_bound = o3d_cloud.get_min_bound() - voxel_size * 0.5 20 | out = o3d_cloud.voxel_down_sample_and_trace(voxel_size, min_bound, max_bound, False) 21 | index_ds = [cubic_index[0] for cubic_index in out[2]] 22 | cloud_ds = cloud[index_ds, :] 23 | return index_ds, cloud_ds 24 | 25 | def get_mean_cov(cloud): 26 | """ 27 | Assuming N by 3 input 28 | """ 29 | o3d_cloud = o3d.geometry.PointCloud() 30 | o3d_cloud.points = o3d.utility.Vector3dVector(cloud[:, :3]) 31 | mean_cov = o3d_cloud.compute_mean_and_covariance() 32 | return mean_cov[0], mean_cov[1] 33 | 34 | def cloud_to_list(cloud): 35 | return [cloud[i, :3].reshape(1,3) for i in range(cloud.shape[0])] -------------------------------------------------------------------------------- /utils/rosbag2scans.py: -------------------------------------------------------------------------------- 1 | import rosbag 2 | import sys, os 3 | import sensor_msgs.point_cloud2 4 | import numpy as np 5 | 6 | def point_list_to_cloud(pts_list): 7 | cloud = [] 8 | for pt in pts_list: 9 | pt_np = np.array([pt.x, pt.y, pt.z, pt.intensity, pt.ring]) 10 | cloud.append(pt_np) 11 | cloud = np.vstack(cloud) 12 | return cloud 13 | 14 | if __name__=="__main__": 15 | bag_name = sys.argv[1] 16 | output_dir = sys.argv[2] 17 | bag = rosbag.Bag(bag_name) 18 | counter = 0 19 | for topic, msg, t in bag.read_messages(topics=['/velodyne_points']): 20 | cloud = sensor_msgs.point_cloud2.read_points_list(msg) 21 | cloud = point_list_to_cloud(cloud) 22 | np.save(os.path.join(output_dir, str(counter) + '.npy'), cloud) 23 | counter += 1 --------------------------------------------------------------------------------