├── LICENSE ├── README.md ├── automatic_labeling ├── draw_semantic_label_sample.py └── semi_automated_labeling_framework.py ├── dataset └── README.md ├── demos └── semantic_lidar_data_2000.png └── manually_labeling ├── .labelmerc ├── dataset_collection.py ├── labelme_example.svg ├── labelme_output ├── img.png ├── label.png ├── label_names.txt └── label_viz.png └── semantic_data_collection_ws └── src └── laser_line_extraction ├── CMakeLists.txt ├── LICENSE ├── README.md ├── images └── line_extraction.gif ├── include └── laser_line_extraction │ ├── line.h │ ├── line_extraction.h │ ├── line_extraction_ros.h │ └── utilities.h ├── launch ├── debug.launch └── example.launch ├── msg ├── LineSegment.msg └── LineSegmentList.msg ├── package.xml └── src ├── line.cpp ├── line_extraction.cpp ├── line_extraction_node.cpp └── line_extraction_ros.cpp /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Temple Robotics and Artificial Intelligence Lab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Semantic2D: A Semantic Dataset for 2D Lidar Semantic Segmentation 2 | Implementation code for our paper ["Semantic2D: A Semantic Dataset for 2D Lidar Semantic Segmentation"](https://arxiv.org/pdf/2409.09899). 3 | Video demos can be found at [multimedia demonstrations](https://youtu.be/a6Xl00loInY). 4 | The Semantic2D dataset can be found and downloaded at: https://doi.org/10.5281/zenodo.13730200. 5 | 6 | ## Requirements: 7 | * Ubuntu 20.04 8 | * ROS-Noetic 9 | * Python 3.8 10 | * Labelme 11 | 12 | ## Installation: 13 | Install Labelme package: 14 | ``` 15 | git clone https://github.com/TempleRAIL/semantic2d.git 16 | cd semantic2d 17 | pip install labelme 18 | cp manually_labeling/.labelmerc ~/.labelmerc 19 | ``` 20 | ## Semantic2D Dataset Description: 21 | * ``scans_lidar": each *.npy file contains a 1081x1 array 2D lidar range data 22 | * ``intensities_lidar": each *.npy file contains 1081x1 array 2D lidar intensity data 23 | * ``line_segments": each *.npy file contains a point list of line segments in the 2D lidar data 24 | * ``positions": each *.npy file contains a 3x1 robot position data (x, y, z) 25 | * ``velocities": each *.npy file contains a 2x1 robot velocity data (Vx, Wz) 26 | 27 | ## Semi-Automatic Labeling Usage: 28 | * Dataset Collection: collect and save the data from the rosbag file: assume you have already created the environment map using a mapping package like amcl and collected the raw rosbag data "xx.bag" 29 | ``` 30 | # complie the workspace: 31 | cd manually_labeling/semantic_data_collection_ws 32 | catkin_make 33 | source devel/setup.sh 34 | # read the rosbag and collect and save the data: 35 | roslaunch laser_line_extraction example.launch 36 | rosbag play xx.bag 37 | cd manually_labeling 38 | python dataset_collection.py 39 | ``` 40 | 41 | * Manually Labeling: use the Labelme tool to manually label the environment map and save the *.json file, as shown in 42 | ![Labelme example](./manually_labeling/labelme_example.svg "labelme_example") 43 | 44 | Then, export the labeled map images: 45 | ``` 46 | labelme_export_json *.json -o labelme_output 47 | ``` 48 | You will then get an environment map image and its semantically labeled map image, as shown in [labelme_output](./manually_labeling/labelme_output). 49 | 50 | * Automatic Labeling: 51 | Please modify the following configuration in the [semi_automated_labeling_framework.py](./automatic_labeling/semi_automated_labeling_framework.y) according to your robot and environment configuration: 52 | ``` 53 | # dataset: 54 | DATASET_ODIR = "/home/xzt/data/semantic_lidar_v2/2024-04-04-12-16-41" # the directory path of the raw data 55 | DATASET_NAME = "train" # select the train, dev, and test 56 | # map: parameters from the map configuration file 57 | MAP_ORIGIN = np.array([-21.200000, -34.800000, 0.000000]) 58 | MAP_RESOLUTION = 0.025000 59 | # labeling: 60 | MAP_LABEL_PATH = '../manually_labeling/labelme_output/label.png' 61 | MAP_PATH = '../manually_labeling/labelme_output/img.png' 62 | # lidar sensor: 63 | POINTS = 1081 # the number of lidar points 64 | LIDAR_BASE_DIS = -0.12 # the distance from lidar_mount to base_link 65 | ``` 66 | 67 | Start the automatic labeling: 68 | ``` 69 | cd ../automatic_labeling 70 | python semi_automated_labeling_framework.py 71 | ``` 72 | 73 | Plot the labeled semantic lidar data: you also need to modify the similar configuration as [semi_automated_labeling_framework.py](./automatic_labeling/semi_automated_labeling_framework.y) in the [draw_semantic_label_sample.py](./automatic_labeling/draw_semantic_label_sample.y) 74 | ``` 75 | python draw_semantic_label_sample.py 76 | ``` 77 | ![semantic_lidar_data](demos/semantic_lidar_data_2000.png "semantic_lidar_data_2000") 78 | 79 | 80 | 81 | ## Citation 82 | ``` 83 | @article{xie2024semantic2d, 84 | title={Semantic2D: A Semantic Dataset for 2D Lidar Semantic Segmentation}, 85 | author={Xie, Zhanteng and Dames, Philip}, 86 | journal={arXiv preprint arXiv:2409.09899}, 87 | year={2024} 88 | } 89 | ``` 90 | -------------------------------------------------------------------------------- /automatic_labeling/draw_semantic_label_sample.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # file: $ISIP_EXP/tuh_dpath/exp_0074/scripts/decode.py 4 | # 5 | # revision history: 6 | # 20190925 (TE): first version 7 | # 8 | # usage: 9 | # python decode.py odir mfile data 10 | # 11 | # arguments: 12 | # odir: the directory where the hypotheses will be stored 13 | # mfile: input model file 14 | # data: the input data list to be decoded 15 | # 16 | # This script decodes data using a simple MLP model. 17 | #------------------------------------------------------------------------------ 18 | 19 | # import pytorch modules 20 | # 21 | import torch 22 | from tqdm import tqdm 23 | 24 | # visualize: 25 | import matplotlib.pyplot as plt 26 | import numpy as np 27 | import matplotlib 28 | matplotlib.style.use('ggplot') 29 | import sys 30 | import os 31 | 32 | 33 | ################ customized parameters ################# 34 | ################ please modify them based on your dataset ################# 35 | DATASET_ODIR = "/home/xzt/data/semantic_lidar_v2/2024-04-04-12-16-41" # the directory path of the raw data 36 | DATASET_NAME = "train" # select the train, dev, and test 37 | SEMANTIC_MASK_ODIR = "./output" 38 | # lidar sensor: 39 | POINTS = 1081 # the number of lidar points 40 | 41 | 42 | ################# read dataset ################### 43 | NEW_LINE = "\n" 44 | # for reproducibility, we seed the rng 45 | # 46 | class Semantic2DLidarDataset(torch.utils.data.Dataset): 47 | def __init__(self, img_path, file_name): 48 | # initialize the data and labels 49 | # read the names of image data: 50 | self.scan_file_names = [] 51 | self.intensity_file_names = [] 52 | self.vel_file_names = [] 53 | self.label_file_names = [] 54 | # parameters: 55 | self.s_max = 30 56 | self.s_min = 0 57 | # open train.txt or dev.txt: 58 | fp_file = open(img_path+'/'+file_name+'.txt', 'r') 59 | 60 | # for each line of the file: 61 | for line in fp_file.read().split(NEW_LINE): 62 | if('.npy' in line): 63 | self.scan_file_names.append(img_path+'/scans_lidar/'+line) 64 | self.intensity_file_names.append(img_path+'/intensities_lidar/'+line) 65 | self.label_file_names.append(img_path+'/semantic_label/'+line) 66 | # close txt file: 67 | fp_file.close() 68 | self.length = len(self.scan_file_names) 69 | 70 | print("dataset length: ", self.length) 71 | 72 | 73 | def __len__(self): 74 | return self.length 75 | 76 | def __getitem__(self, idx): 77 | # get the index of start point: 78 | scan = np.zeros((1, POINTS)) 79 | intensity = np.zeros((1, POINTS)) 80 | label = np.zeros((1, POINTS)) 81 | 82 | # get the scan data: 83 | scan_name = self.scan_file_names[idx] 84 | scan = np.load(scan_name) 85 | 86 | # get the intensity data: 87 | intensity_name = self.intensity_file_names[idx] 88 | intensity = np.load(intensity_name) 89 | 90 | # get the semantic label data: 91 | label_name = self.label_file_names[idx] 92 | label = np.load(label_name) 93 | 94 | # initialize: 95 | scan[np.isnan(scan)] = 0. 96 | scan[np.isinf(scan)] = 0. 97 | 98 | intensity[np.isnan(intensity)] = 0. 99 | intensity[np.isinf(intensity)] = 0. 100 | 101 | scan[scan >= 15] = 0. 102 | 103 | label[np.isnan(label)] = 0. 104 | label[np.isinf(label)] = 0. 105 | 106 | # transfer to pytorch tensor: 107 | scan_tensor = torch.FloatTensor(scan) 108 | intensity_tensor = torch.FloatTensor(intensity) 109 | label_tensor = torch.FloatTensor(label) 110 | 111 | data = { 112 | 'scan': scan_tensor, 113 | 'intensity': intensity_tensor, 114 | 'label': label_tensor, 115 | } 116 | 117 | return data 118 | 119 | #------------------------------------------------------------------------------ 120 | # 121 | # the main program starts here 122 | # 123 | #------------------------------------------------------------------------------ 124 | 125 | # function: main 126 | # 127 | # arguments: none 128 | # 129 | # return: none 130 | # 131 | # This method is the main function. 132 | # 133 | if __name__ == '__main__': 134 | # input parameters: 135 | dataset_odir = DATASET_ODIR 136 | dataset_name = DATASET_NAME 137 | semantic_mask_odir = SEMANTIC_MASK_ODIR 138 | # create the folder for the semantic label mask: 139 | if not os.path.exists(semantic_mask_odir): 140 | os.makedirs(semantic_mask_odir) 141 | 142 | # read dataset: 143 | eval_dataset = Semantic2DLidarDataset(dataset_odir, dataset_name) 144 | eval_dataloader = torch.utils.data.DataLoader(eval_dataset, batch_size=1, num_workers=2, \ 145 | shuffle=False, drop_last=True, pin_memory=True) 146 | 147 | # for each batch in increments of batch size: 148 | cnt = 0 149 | cnt_m = 0 150 | # get the number of batches (ceiling of train_data/batch_size): 151 | num_batches = int(len(eval_dataset)/eval_dataloader.batch_size) 152 | for i, batch in tqdm(enumerate(eval_dataloader), total=num_batches): 153 | # collect the samples as a batch: 10 timesteps 154 | if(i % 200 == 0): 155 | scans = batch['scan'] 156 | scans = scans.detach().cpu().numpy() 157 | labels = batch['label'] 158 | labels = labels.detach().cpu().numpy() 159 | 160 | # lidar data: 161 | r = scans.reshape(POINTS) 162 | theta = np.linspace(-(135*np.pi/180), 135*np.pi/180, POINTS, endpoint='true') 163 | 164 | ## plot semantic label: 165 | fig = plt.figure(figsize=(12, 12)) 166 | ax = fig.add_subplot(1,1,1, projection='polar', facecolor='seashell') 167 | smap = labels.reshape(POINTS) 168 | 169 | # add the background label: 170 | theta = np.insert(theta, -1, np.pi) 171 | r = np.insert(r, -1, 1) 172 | smap = np.insert(smap, -1, 0) 173 | label_val = np.unique(smap).astype(int) 174 | print("label_values: ", label_val) 175 | 176 | colors = smap 177 | area = 6 178 | scatter = ax.scatter(theta, r, c=colors, s=area, cmap='nipy_spectral', alpha=0.95, linewidth=10) 179 | ax.set_xticks(np.linspace(-(135*np.pi/180), 135*np.pi/180, 8, endpoint='true')) 180 | ax.set_thetamin(-135) 181 | ax.set_thetamax(135) 182 | ax.set_yticklabels([]) 183 | # produce a legend with the unique colors from the scatter 184 | classes = ['Other', 'Chair', 'Door', 'Elevator', 'Person', 'Pillar', 'Sofa', 'Table', 'Trash bin', 'Wall'] 185 | plt.xticks(fontsize=16) 186 | plt.yticks(fontsize=16) 187 | plt.legend(handles=scatter.legend_elements(num=[j for j in label_val])[0], labels=[classes[j] for j in label_val], bbox_to_anchor=(0.5, -0.08), loc='lower center', fontsize=18) 188 | ax.grid(False) 189 | ax.set_theta_offset(np.pi/2) 190 | 191 | input_img_name = semantic_mask_odir + "/semantic_mask" + str(i)+ ".png" 192 | plt.savefig(input_img_name, bbox_inches='tight') 193 | plt.show() 194 | 195 | print(i) 196 | 197 | -------------------------------------------------------------------------------- /automatic_labeling/semi_automated_labeling_framework.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import os 3 | import numpy as np 4 | import PIL.Image 5 | import torch 6 | from tqdm import tqdm 7 | from typing import Optional, Tuple, Union 8 | from sklearn.neighbors import NearestNeighbors 9 | 10 | ################ customized parameters ################# 11 | ################ please modify them based on your dataset ################# 12 | # dataset: 13 | DATASET_ODIR = "/home/xzt/data/semantic_lidar_v2/2024-04-04-12-16-41" # the directory path of the raw data 14 | DATASET_NAME = "train" # select the train, dev, and test 15 | # map: parameters from the map configuration file 16 | MAP_ORIGIN = np.array([-21.200000, -34.800000, 0.000000]) 17 | MAP_RESOLUTION = 0.025000 18 | # labeling: 19 | MAP_LABEL_PATH = '../manually_labeling/labelme_output/label.png' 20 | MAP_PATH = '../manually_labeling/labelme_output/img.png' 21 | # lidar sensor: 22 | POINTS = 1081 # the number of lidar points 23 | LIDAR_BASE_DIS = -0.12 # the distance from lidar_mount to base_link 24 | 25 | ################# read dataset ################### 26 | NEW_LINE = "\n" 27 | class Semantic2DLidarDataset(torch.utils.data.Dataset): 28 | def __init__(self, img_path, file_name): 29 | # initialize the data and labels 30 | # read the names of image data: 31 | self.scan_file_names = [] 32 | self.line_file_names = [] 33 | self.pos_file_names = [] 34 | self.vel_file_names = [] 35 | # open train.txt or dev.txt: 36 | fp_file = open(img_path+'/'+file_name+'.txt', 'r') 37 | # for each line of the file: 38 | for line in fp_file.read().split(NEW_LINE): 39 | if('.npy' in line): 40 | self.scan_file_names.append(img_path+'/scans_lidar/'+line) 41 | self.line_file_names.append(img_path+'/line_segments/'+line) 42 | self.pos_file_names.append(img_path+'/positions/'+line) 43 | self.vel_file_names.append(img_path+'/velocities/'+line) 44 | # close txt file: 45 | fp_file.close() 46 | self.length = len(self.scan_file_names) 47 | 48 | print("dataset length: ", self.length) 49 | 50 | def __len__(self): 51 | return self.length 52 | 53 | def __getitem__(self, idx): 54 | scan = np.zeros(POINTS) 55 | position = np.zeros(3) 56 | vel = np.zeros(2) 57 | # get the scan data: 58 | scan_name = self.scan_file_names[idx] 59 | scan = np.load(scan_name) 60 | 61 | # get the line segments data: 62 | line_name = self.line_file_names[idx] 63 | line_segs = np.load(line_name) 64 | 65 | # get the scan_ur data: 66 | pos_name = self.pos_file_names[idx] 67 | position = np.load(pos_name) 68 | 69 | # get the velocity data: 70 | vel_name = self.vel_file_names[idx] 71 | vel = np.load(vel_name) 72 | 73 | # initialize: 74 | scan[np.isnan(scan)] = 0. 75 | scan[np.isinf(scan)] = 0. 76 | scan[scan==60] = 0. 77 | 78 | position[np.isnan(position)] = 0. 79 | position[np.isinf(position)] = 0. 80 | 81 | vel[np.isnan(vel)] = 0. 82 | vel[np.isinf(vel)] = 0. 83 | 84 | # transfer to pytorch tensor: 85 | scan_tensor = torch.FloatTensor(scan) 86 | line_tensor = torch.FloatTensor(line_segs) 87 | pose_tensor = torch.FloatTensor(position) 88 | vel_tensor = torch.FloatTensor(vel) 89 | 90 | data = { 91 | 'scan': scan_tensor, 92 | 'line': line_tensor, 93 | 'position': pose_tensor, 94 | 'velocity': vel_tensor, 95 | } 96 | 97 | return data 98 | 99 | ################# ICP algorithm ################### 100 | class MapICP: 101 | def __init__(self, map_pts): 102 | # map_pts: 2xN numpy.ndarray of points 103 | self.map_pts = map_pts 104 | self.neighbors = NearestNeighbors(n_neighbors=1).fit(self.map_pts.T) 105 | 106 | def setMapPoints(self, pts: np.ndarray) -> None: 107 | ''' 108 | Initializes a set of points to match against 109 | Inputs: 110 | pts: 2xN numpy.ndarray of 2D points 111 | ''' 112 | assert pts.shape[0] == 2 113 | self.map_pts = pts 114 | self.neighbors = NearestNeighbors(n_neighbors=1).fit(pts.T) 115 | 116 | def nearestNeighbor(self, pts: np.ndarray) -> Tuple[np.array, np.array]: 117 | ''' 118 | Find the nearest (Euclidean) neighbor in for each point in pts 119 | Input: 120 | pts: 2xN array of points 121 | Output: 122 | distances: np.array of Euclidean distances of the nearest neighbor 123 | indices: np.array of indices of the nearest neighbor 124 | ''' 125 | assert pts.shape[0] == 2 126 | distances, indices = self.neighbors.kneighbors(pts.T, return_distance=True) 127 | 128 | return distances.ravel(), indices.ravel() 129 | 130 | def bestFitTransform(self, pts: np.array, map_pts: np.array) -> np.ndarray: #indices: np.array) -> np.ndarray: 131 | ''' 132 | Calculates the least-squares best-fit transform that maps pts on to map_pts 133 | Input: 134 | pts: 2xN numpy.ndarray of points 135 | # indices: 1xN numpy.array of corresponding map_point indices 136 | map_pts: 2xN numpy.ndarray of points 137 | Returns: 138 | T: 3x3 homogeneous transformation matrix that maps pts on to map_pts 139 | ''' 140 | # get number of dimensions 141 | m = pts.shape[0] 142 | assert m == 2 143 | 144 | # Extract points 145 | map = map_pts # self.map_pts[:,indices] 146 | assert pts.shape == map.shape 147 | 148 | # translate points to their centroids 149 | centroid_pts = np.mean(pts, axis=1) 150 | centroid_map = np.mean(map, axis=1) 151 | PTS = pts - centroid_pts.reshape((-1,1)) 152 | MAP = map - centroid_map.reshape((-1,1)) 153 | 154 | # rotation matrix 155 | H = MAP @ PTS.T 156 | U, _, Vt = np.linalg.svd(H) 157 | R = U @ Vt 158 | 159 | # special reflection case 160 | if np.linalg.det(R) < 0: 161 | Vt[:,-1] *= -1 162 | R = U @ Vt 163 | 164 | # translation 165 | t = centroid_map - R @ centroid_pts 166 | 167 | # homogeneous transformation 168 | T = np.identity(m+1) 169 | T[:m, :m] = R 170 | T[:m, m] = t 171 | 172 | return T 173 | 174 | def icp(self, pts: np.ndarray, init_pose: Optional[Union[np.array, None]]=None, max_iterations: Optional[int]=20, tolerance: Optional[float]=0.05) -> Tuple[np.ndarray, np.array, int]: 175 | ''' 176 | The Iterative Closest Point method: finds best-fit transform that maps points A on to points B 177 | Input: 178 | pts: 2xN numpy.ndarray of source points 179 | init_pose: 3x3 homogeneous transformation 180 | max_iterations: exit algorithm after max_iterations 181 | tolerance: convergence criteria 182 | Outputs: 183 | T: final homogeneous transformation that maps pts on to map_pts 184 | distances: Euclidean distances (errors) of the nearest neighbor 185 | i: number of iterations to converge 186 | ''' 187 | # Get number of dimensions 188 | m = pts.shape[0] 189 | assert m == 2 190 | 191 | # Make points homogeneous, copy them to maintain the originals 192 | src = np.ones((m+1, pts.shape[1])) 193 | dst = np.ones((m+1, self.map_pts.shape[1])) 194 | src[:m,:] = np.copy(pts) 195 | dst[:m,:] = np.copy(self.map_pts) 196 | 197 | # Apply the initial pose estimate 198 | T = np.eye(3) 199 | if init_pose is not None: 200 | src = init_pose @ src 201 | T = init_pose @ T 202 | 203 | # Run ICP 204 | prev_error = 1e6 205 | for i in range(max_iterations): 206 | # find the nearest neighbors between the current source and destination points 207 | distances, indices = self.nearestNeighbor(src[:m,:]) 208 | map_pts = self.map_pts[:, indices] 209 | # compute the transformation between the current source and nearest destination points 210 | T_delta = self.bestFitTransform(src[:m,:], map_pts) #indices) 211 | 212 | # update the current source and transform 213 | src = T_delta @ src 214 | T = T_delta @ T 215 | 216 | # check error 217 | mean_error = np.mean(distances) 218 | if np.abs(prev_error - mean_error) < tolerance: 219 | break 220 | prev_error = mean_error 221 | 222 | #T = self.bestFitTransform(pts, src[:m,:]) 223 | # Calculate final transformation 224 | return T, distances, i 225 | 226 | ################# map converter: homogeneous transformation ################### 227 | class MapConverter: 228 | # Constructor 229 | def __init__(self, label_file, origin=np.array([-12.200000, -12.200000, 0.000000]), resolution=0.05): 230 | # map parameters 231 | self.origin = origin 232 | self.resolution = resolution 233 | # homogeneous transformation matrix: 234 | self.map_T_pixel = np.array([[np.cos(self.origin[2]), -np.sin(self.origin[2]), self.origin[0]], 235 | [np.sin(self.origin[2]), np.cos(self.origin[2]), self.origin[1]], 236 | [0, 0, 1] 237 | ]) 238 | self.pixel_T_map = np.linalg.inv(self.map_T_pixel) 239 | # load semantic map 240 | label_map = np.asarray(PIL.Image.open(label_file)) 241 | self.label_map = label_map.T # transpose 90 242 | self.x_lim = self.label_map.shape[0] 243 | self.y_lim = self.label_map.shape[1] 244 | print(self.x_lim, self.y_lim) 245 | 246 | def coordinate2pose(self, px, py): 247 | pixel_pose = np.array([px*self.resolution, py*self.resolution, 1]) 248 | map_pose = np.matmul(self.map_T_pixel, pixel_pose.T) 249 | x = map_pose[0] 250 | y = map_pose[1] 251 | 252 | return x, y 253 | 254 | def pose2coordinate(self, x, y): 255 | map_pose = np.array([x, y, 1]) 256 | pixel_pose = np.matmul(self.pixel_T_map, map_pose.T) 257 | px = int(pixel_pose[0] / self.resolution) 258 | py = int(pixel_pose[1] / self.resolution) 259 | 260 | return px, py 261 | 262 | def get_semantic_label(self, x, y): 263 | px, py = self.pose2coordinate(x, y) 264 | py = self.y_lim - py # the y axis is inverse 265 | label_loc = np.zeros(10) # 10 labels 266 | # filtering: 267 | for i in range(px-2, px+2): 268 | for j in range(py-2, py+2): 269 | if(i >= 0 and i < self.x_lim and j >=0 and j < self.y_lim): 270 | label = self.label_map[i, j] 271 | if(label != 0): 272 | label_loc[label] += 1 273 | 274 | if(np.sum(label_loc) == 0): # people 275 | semantic_label = 4 276 | else: # static objects 277 | semantic_label = np.argmax(label_loc) 278 | 279 | return semantic_label 280 | 281 | def lidar2map(self, lidar_pos, lidar_scan): 282 | # get laser points: polar to cartesian 283 | lidar_points = np.zeros((POINTS, 2)) 284 | angles = np.linspace(-(135*np.pi/180), 135*np.pi/180, num=POINTS) 285 | dis = lidar_scan 286 | lidar_points[:, 0] = dis*np.cos(angles) 287 | lidar_points[:, 1] = dis*np.sin(angles) 288 | #lidar_points[:, 2] = 1 289 | 290 | # coordinate transformation: lidar -> map 291 | lidar_points_in_map = np.zeros((POINTS, 2)) 292 | lidar_points_in_map[:, 0] = lidar_points[:, 0]*np.cos(lidar_pos[2]) - lidar_points[:, 1]*np.sin(lidar_pos[2]) + lidar_pos[0] 293 | lidar_points_in_map[:, 1] = lidar_points[:, 0]*np.sin(lidar_pos[2]) + lidar_points[:, 1]*np.cos(lidar_pos[2]) + lidar_pos[1] 294 | 295 | return lidar_points_in_map, lidar_points 296 | 297 | 298 | if __name__ == '__main__': 299 | # input parameters: 300 | dataset_odir = DATASET_ODIR 301 | dataset_name = DATASET_NAME 302 | scan_label_odir = dataset_odir + "/" + "semantic_label" 303 | if not os.path.exists(scan_label_odir): 304 | os.makedirs(scan_label_odir) 305 | 306 | map_path = MAP_PATH 307 | map_label_path = MAP_LABEL_PATH 308 | 309 | map_origin = MAP_ORIGIN 310 | map_resolution = MAP_RESOLUTION 311 | 312 | # initialize semantic scan label: 313 | scan_label = np.zeros(POINTS) 314 | 315 | # initialize the map converter: homogeneous transformation 316 | mc = MapConverter(map_label_path, origin=map_origin, resolution=map_resolution) 317 | 318 | ## extract the valid map points fromt the map image: 319 | # load map image: 320 | map_img = np.asarray(PIL.Image.open(map_path)) 321 | map_img = map_img.T # transpose 90 322 | print(map_img.shape) 323 | # get map valid points: 324 | map_idx = np.where(map_img == 0) 325 | map_idx_x = map_idx[0] 326 | map_idx_y = map_idx[1] 327 | map_points = [] 328 | for n in range(len(map_idx_x)): 329 | px = map_idx_x[n] 330 | py = map_idx_y[n] 331 | py = map_img.shape[1] - py # the x axis is inverse 332 | [x, y] = mc.coordinate2pose(px, py) 333 | map_points.append([x, y]) 334 | map_pts = np.array(map_points) 335 | 336 | # initialize ICP: 337 | icp = MapICP(map_pts.T) 338 | 339 | # load dataset: 340 | eval_dataset = Semantic2DLidarDataset(dataset_odir, dataset_name) 341 | eval_dataloader = torch.utils.data.DataLoader(eval_dataset, batch_size=1, \ 342 | shuffle=False, drop_last=True) #, pin_memory=True) 343 | 344 | # get the number of batches (ceiling of train_data/batch_size): 345 | num_batches = int(len(eval_dataset)/eval_dataloader.batch_size) 346 | for i, batch in tqdm(enumerate(eval_dataloader), total=num_batches): 347 | # collect the samples as a batch: 348 | scan = batch['scan'] 349 | scan = scan.detach().cpu().numpy() 350 | lines = batch['line'] 351 | lines = lines.detach().cpu().numpy() 352 | position = batch['position'] 353 | position = position.detach().cpu().numpy() 354 | 355 | # transfer lidar points: 356 | lidar_pos = position.reshape(3) 357 | lidar_pos[0] += LIDAR_BASE_DIS # the distance from lidar_mount to base_link 358 | lidar_scan = scan.reshape(POINTS) 359 | lidar_points_in_map, lidar_points = mc.lidar2map(lidar_pos, lidar_scan) 360 | 361 | # use line segments (key line features) to remove outliers in the lidar scan: 362 | correspondences = [] 363 | for line in lines[0]: 364 | x1, y1, x2, y2 = line 365 | # construct line formular: y = ax +b 366 | a = (y2 - y1) / (x2 - x1 + 1e-10) 367 | b = y1 - a*x1 368 | for n in range(POINTS): 369 | x, y = lidar_points[n, :] 370 | if(x >= x1-1 and x <= x2+1 and y >= y1-1 and y <= y2+1): # in the area of the line 371 | if(abs(y - (a*x+b)) <= 0.3): # on the line 372 | correspondences.append([x, y]) 373 | 374 | correspondences = np.array(correspondences) 375 | correspondences_length = len(correspondences) 376 | 377 | if(correspondences_length > 280): # reliable correspondences 378 | # coordinate transformation: lidar -> map 379 | correspondences_in_map = np.zeros((correspondences_length, 2)) 380 | correspondences_in_map[:, 0] = correspondences[:, 0]*np.cos(lidar_pos[2]) - correspondences[:, 1]*np.sin(lidar_pos[2]) + lidar_pos[0] 381 | correspondences_in_map[:, 1] = correspondences[:, 0]*np.sin(lidar_pos[2]) + correspondences[:, 1]*np.cos(lidar_pos[2]) + lidar_pos[1] 382 | 383 | # use ICP scan matching to correct lidar pose: 384 | mapc_T_map, _, _ = icp.icp(correspondences_in_map.T, max_iterations=500, tolerance=1e-6) 385 | 386 | # corrected lidar pose: 387 | lidar_pose_corrected = np.matmul(mapc_T_map, np.array([lidar_pos[0], lidar_pos[1], 1])) 388 | lidar_points_in_map_old = np.concatenate((lidar_points_in_map, np.ones((POINTS, 1))), axis=1) 389 | point_corrected = np.matmul(mapc_T_map, lidar_points_in_map_old.T) 390 | else: # no icp correction 391 | lidar_pose_corrected = lidar_pos 392 | point_corrected = lidar_points_in_map.T 393 | 394 | # semantic pixel mataching: 395 | for j in range(POINTS): 396 | if(point_corrected[0, j] == lidar_pose_corrected[0] and point_corrected[1, j] == lidar_pose_corrected[1]): # scan == 0 397 | scan_label[j] = 0 398 | else: 399 | scan_label[j] = mc.get_semantic_label(point_corrected[0, j], point_corrected[1, j]) 400 | 401 | # write scan_label in lidar frame into np.array: 402 | scan_label_name = scan_label_odir + "/" + str(i).zfill(7) 403 | np.save(scan_label_name, scan_label) 404 | -------------------------------------------------------------------------------- /dataset/README.md: -------------------------------------------------------------------------------- 1 | # Semantic2D: A Semantic Dataset for 2D Lidar Semantic Segmentation 2 | The Semantic2D Dataset and its raw rosbag data can be downloaded at: https://doi.org/10.5281/zenodo.13730200. 3 | -------------------------------------------------------------------------------- /demos/semantic_lidar_data_2000.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TempleRAIL/semantic2d/6a36bf5cb4e4db35c183a6641fbdb934ad5081af/demos/semantic_lidar_data_2000.png -------------------------------------------------------------------------------- /manually_labeling/.labelmerc: -------------------------------------------------------------------------------- 1 | auto_save: false 2 | display_label_popup: true 3 | store_data: true 4 | keep_prev: false 5 | keep_prev_scale: false 6 | keep_prev_brightness: false 7 | keep_prev_contrast: false 8 | logger_level: info 9 | 10 | flags: null 11 | label_flags: null 12 | labels: [Chair, Door, Elevator, Pillar, Sofa, Table, Trash bin, Wall, People] # null 13 | file_search: null 14 | sort_labels: false #true 15 | validate_label: null 16 | 17 | default_shape_color: [0, 255, 0] 18 | shape_color: manual #auto # null, 'auto', 'manual' 19 | shift_auto_shape_color: 0 20 | label_colors: {Chair:[109, 0, 156], Door:[0, 46, 221], Elevator:[0, 164, 187], Pillar:[0, 155, 18], Sofa:[0, 225, 0], Table:[203, 249, 0], Trash bin:[255, 173, 0], Wall:[227, 0, 0], Peole:[204, 204, 204]} #null 21 | 22 | shape: 23 | # drawing 24 | line_color: [0, 255, 0, 128] 25 | fill_color: [0, 0, 0, 64] 26 | vertex_fill_color: [0, 255, 0, 255] 27 | # selecting / hovering 28 | select_line_color: [255, 255, 255, 255] 29 | select_fill_color: [0, 255, 0, 155] 30 | hvertex_fill_color: [255, 255, 255, 255] 31 | point_size: 8 32 | 33 | # main 34 | flag_dock: 35 | show: true 36 | closable: true 37 | movable: true 38 | floatable: true 39 | label_dock: 40 | show: true 41 | closable: true 42 | movable: true 43 | floatable: true 44 | shape_dock: 45 | show: true 46 | closable: true 47 | movable: true 48 | floatable: true 49 | file_dock: 50 | show: true 51 | closable: true 52 | movable: true 53 | floatable: true 54 | 55 | # label_dialog 56 | show_label_text_field: true 57 | label_completion: startswith 58 | fit_to_content: 59 | column: true 60 | row: false 61 | 62 | # canvas 63 | epsilon: 10.0 64 | canvas: 65 | fill_drawing: true 66 | # None: do nothing 67 | # close: close polygon 68 | double_click: close 69 | # The max number of edits we can undo 70 | num_backups: 10 71 | # show crosshair 72 | crosshair: 73 | polygon: false 74 | rectangle: true 75 | circle: false 76 | line: false 77 | point: false 78 | linestrip: false 79 | ai_polygon: false 80 | 81 | shortcuts: 82 | close: Ctrl+W 83 | open: Ctrl+O 84 | open_dir: Ctrl+U 85 | quit: Ctrl+Q 86 | save: Ctrl+S 87 | save_as: Ctrl+Shift+S 88 | save_to: null 89 | delete_file: Ctrl+Delete 90 | 91 | open_next: [D, Ctrl+Shift+D] 92 | open_prev: [A, Ctrl+Shift+A] 93 | 94 | zoom_in: [Ctrl++, Ctrl+=] 95 | zoom_out: Ctrl+- 96 | zoom_to_original: Ctrl+0 97 | fit_window: Ctrl+F 98 | fit_width: Ctrl+Shift+F 99 | 100 | create_polygon: Ctrl+N 101 | create_rectangle: Ctrl+R 102 | create_circle: null 103 | create_line: null 104 | create_point: null 105 | create_linestrip: null 106 | edit_polygon: Ctrl+J 107 | delete_polygon: Delete 108 | duplicate_polygon: Ctrl+D 109 | copy_polygon: Ctrl+C 110 | paste_polygon: Ctrl+V 111 | undo: Ctrl+Z 112 | undo_last_point: Ctrl+Z 113 | add_point_to_edge: Ctrl+Shift+P 114 | edit_label: Ctrl+E 115 | toggle_keep_prev_mode: Ctrl+P 116 | remove_selected_point: [Meta+H, Backspace] 117 | -------------------------------------------------------------------------------- /manually_labeling/dataset_collection.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from random import choice 3 | import rospy 4 | import tf 5 | # custom define messages: 6 | from geometry_msgs.msg import Point, PoseStamped, Twist, TwistStamped, Pose, PoseArray, PoseWithCovarianceStamped 7 | from sensor_msgs.msg import Image, LaserScan 8 | from nav_msgs.msg import Odometry, OccupancyGrid, Path 9 | from laser_line_extraction.msg import LineSegmentList, LineSegment 10 | # python: 11 | import os 12 | import numpy as np 13 | import threading 14 | 15 | 16 | class DataCollection: 17 | # Constructor 18 | def __init__(self): 19 | # initialize data: 20 | self.scan_lidar = np.zeros(1081) 21 | self.intensities_lidar = np.zeros(1081) 22 | self.lines = [] 23 | self.vel_cmd = np.zeros(2) 24 | self.pos = np.zeros(3) 25 | self.record = True 26 | self.rosbag_cnt = 0 27 | self.rosbag_cnt_reg = np.array([-4, -3, -2, -1]) 28 | self.reg_cnt = 0 29 | 30 | # store directory: 31 | data_odir = "/home/xzt/data/semantic_lidar_v2/2024-04-11-15-24-29" 32 | self.scan_lidar_odir = data_odir + "/" + "scans_lidar" 33 | if not os.path.exists(self.scan_lidar_odir): 34 | os.makedirs(self.scan_lidar_odir) 35 | self.intensities_lidar_odir = data_odir + "/" + "intensities_lidar" 36 | if not os.path.exists(self.intensities_lidar_odir): 37 | os.makedirs(self.intensities_lidar_odir) 38 | self.line_odir = data_odir + "/" + "line_segments" 39 | if not os.path.exists(self.line_odir): 40 | os.makedirs(self.line_odir) 41 | self.vel_odir = data_odir + "/" + "velocities" 42 | if not os.path.exists(self.vel_odir): 43 | os.makedirs(self.vel_odir) 44 | self.pos_odir = data_odir + "/" + "positions" 45 | if not os.path.exists(self.pos_odir): 46 | os.makedirs(self.pos_odir) 47 | 48 | # timer: 49 | self.timer = None 50 | self.rate = 20 # 20 Hz velocity controller 51 | self.idx = 0 52 | # Lock 53 | self.lock = threading.Lock() 54 | 55 | # initialize ROS objects: 56 | self.tf_listener = tf.TransformListener() 57 | self.scan_sub = rospy.Subscriber("scan", LaserScan, self.scan_callback) 58 | self.line_sub = rospy.Subscriber("line_segments", LineSegmentList, self.line_segments_callback) 59 | self.dwa_cmd_sub = rospy.Subscriber('bluetooth_teleop/cmd_vel', Twist, self.dwa_cmd_callback) #, queue_size=1) 60 | self.robot_pose_pub = rospy.Subscriber('robot_pose', PoseStamped, self.robot_pose_callback) 61 | 62 | 63 | # Callback function for the scan measurement subscriber 64 | def scan_callback(self, laserScan_msg): 65 | # get the laser scan data: 66 | scan_data = np.array(laserScan_msg.ranges, dtype=np.float32) 67 | scan_data[np.isnan(scan_data)] = 0. 68 | scan_data[np.isinf(scan_data)] = 0. 69 | self.scan_lidar = scan_data 70 | # get the laser intensity data: 71 | intensity_data = np.array(laserScan_msg.intensities, dtype=np.float32) 72 | intensity_data[np.isnan(intensity_data)] = 0. 73 | intensity_data[np.isinf(intensity_data)] = 0. 74 | self.intensities_lidar = intensity_data 75 | self.rosbag_cnt += 1 76 | 77 | # Callback function for the line segments subscriber 78 | def line_segments_callback(self, lineSeg_msg): 79 | self.lines = [] 80 | # get the laser line segments data: 81 | line_segs = lineSeg_msg.line_segments 82 | for line_seg in line_segs: 83 | line = [line_seg.start[0], line_seg.start[1], line_seg.end[0], line_seg.end[1]] 84 | self.lines.append(line) 85 | 86 | # Callback function for the local map subscriber 87 | def dwa_cmd_callback(self, robot_vel_msg): 88 | self.vel_cmd = np.array([robot_vel_msg.linear.x, robot_vel_msg.angular.z]) 89 | 90 | # Callback function for the current robot pose subscriber 91 | def robot_pose_callback(self, robot_pose_msg): 92 | # Cartesian coordinate: 93 | #self.pos = np.array([robot_pose_msg.pose.position.x, robot_pose_msg.pose.position.y, robot_pose_msg.pose.orientation.z]) 94 | quaternion = [ 95 | robot_pose_msg.pose.orientation.x, robot_pose_msg.pose.orientation.y, 96 | robot_pose_msg.pose.orientation.z, robot_pose_msg.pose.orientation.w 97 | ] 98 | roll, pitch, yaw = tf.transformations.euler_from_quaternion(quaternion) 99 | self.pos = np.array([robot_pose_msg.pose.position.x, robot_pose_msg.pose.position.y, yaw]) 100 | 101 | # start the timer if this is the first path received 102 | if self.timer is None: 103 | self.start() 104 | 105 | # Start the timer that calculates command velocities 106 | def start(self): 107 | # initialize timer for controller update 108 | self.timer = rospy.Timer(rospy.Duration(1./self.rate), self.timer_callback) 109 | 110 | # function that runs every time the timer finishes to ensure that vae data are sent regularly 111 | def timer_callback(self, event): 112 | # lock data: 113 | #self.lock.acquire() 114 | scan_ranges = self.scan_lidar 115 | scan_intensities = self.intensities_lidar 116 | line_segs = self.lines 117 | rob_pos = self.pos 118 | vel_cmd = self.vel_cmd 119 | #self.lock.release() 120 | # check if the rosbag is done: 121 | if(self.reg_cnt > 3): 122 | self.reg_cnt = 0 123 | 124 | self.rosbag_cnt_reg[self.reg_cnt] = self.rosbag_cnt 125 | self.reg_cnt += 1 126 | cnt_unique = np.unique(self.rosbag_cnt_reg) 127 | # write array into npy: 128 | if(self.record and len(cnt_unique)>1): #and self.idx < 15000): 129 | # write lidar scan in lidar frame into np.array: 130 | scan_name = self.scan_lidar_odir + "/" + str(self.idx).zfill(7) 131 | np.save(scan_name, scan_ranges) 132 | # write lidar intensity in lidar frame into np.array: 133 | intensity_name = self.intensities_lidar_odir + "/" + str(self.idx).zfill(7) 134 | np.save(intensity_name, scan_intensities) 135 | # write line segments in lidar frame into np.array: 136 | line_name = self.line_odir + "/" + str(self.idx).zfill(7) 137 | np.save(line_name, line_segs) 138 | # write velocoties into np.array: 139 | vel_name = self.vel_odir + "/" + str(self.idx).zfill(7) 140 | np.save(vel_name, vel_cmd) 141 | # write robot position into np.array: 142 | pos_name = self.pos_odir + "/" + str(self.idx).zfill(7) 143 | np.save(pos_name, rob_pos) 144 | 145 | #img.show() 146 | self.idx += 1 147 | print("idx: ", self.idx) 148 | else: 149 | print("idx: ", self.idx) 150 | 151 | if __name__ == '__main__': 152 | rospy.init_node('data_collection') 153 | data = DataCollection() 154 | rospy.spin() 155 | 156 | 157 | -------------------------------------------------------------------------------- /manually_labeling/labelme_output/img.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TempleRAIL/semantic2d/6a36bf5cb4e4db35c183a6641fbdb934ad5081af/manually_labeling/labelme_output/img.png -------------------------------------------------------------------------------- /manually_labeling/labelme_output/label.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TempleRAIL/semantic2d/6a36bf5cb4e4db35c183a6641fbdb934ad5081af/manually_labeling/labelme_output/label.png -------------------------------------------------------------------------------- /manually_labeling/labelme_output/label_names.txt: -------------------------------------------------------------------------------- 1 | _background_ 2 | Chair 3 | Door 4 | Elevator 5 | People 6 | Pillar 7 | Sofa 8 | Table 9 | Trash bin 10 | Wall 11 | -------------------------------------------------------------------------------- /manually_labeling/labelme_output/label_viz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TempleRAIL/semantic2d/6a36bf5cb4e4db35c183a6641fbdb934ad5081af/manually_labeling/labelme_output/label_viz.png -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(laser_line_extraction) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | cmake_modules 6 | geometry_msgs 7 | message_generation 8 | roscpp 9 | rospy 10 | sensor_msgs 11 | visualization_msgs 12 | ) 13 | 14 | find_package(Eigen3 REQUIRED) 15 | 16 | add_message_files( 17 | FILES 18 | LineSegment.msg 19 | LineSegmentList.msg 20 | ) 21 | 22 | generate_messages( 23 | DEPENDENCIES 24 | sensor_msgs 25 | ) 26 | 27 | catkin_package( 28 | INCLUDE_DIRS include 29 | LIBRARIES line line_extraction line_extraction_ros 30 | CATKIN_DEPENDS geometry_msgs message_runtime roscpp sensor_msgs visualization_msgs 31 | ) 32 | 33 | add_library(line src/line.cpp) 34 | target_link_libraries(line ${catkin_LIBRARIES}) 35 | 36 | add_library(line_extraction src/line_extraction.cpp) 37 | target_link_libraries(line_extraction ${catkin_LIBRARIES}) 38 | 39 | add_library(line_extraction_ros src/line_extraction_ros.cpp) 40 | target_link_libraries(line_extraction_ros line line_extraction ${catkin_LIBRARIES}) 41 | add_dependencies(line_extraction_ros laser_line_extraction_generate_messages_cpp) 42 | 43 | add_executable(line_extraction_node src/line_extraction_node.cpp) 44 | target_link_libraries(line_extraction_node line_extraction_ros ${catkin_LIBRARIES}) 45 | 46 | include_directories(include ${catkin_INCLUDE_DIRS}) 47 | include_directories(include ${EIGEN3_INCLUDE_DIRS}) 48 | 49 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_laser_line_extraction.cpp) 50 | 51 | install(TARGETS line_extraction_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 52 | install(TARGETS line_extraction_ros line_extraction line ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 53 | install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 54 | -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, Marc Gallant 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the {organization} nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/README.md: -------------------------------------------------------------------------------- 1 | # Laser Line Extraction 2 | Laser Line Extraction is a [Robot Operating System (ROS)](http://www.ros.org) package that extracts line segments form [LaserScan](http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html) messages. Created by [Marc Gallant](http://marcgallant.ca), originally for use in the [Mining Systems Laboratory](http://msl.engineering.queensu.ca). Here is what the Laser Line Extraction package looks like in action: 3 | 4 | ![Laser line extraction](images/line_extraction.gif) 5 | 6 | In the above image, the white dots are points in a LaserScan message, and the red lines are what is extracted by Laser Line Extraction. This data was collected by driving a robot through Beamish-Munro Hall at Queen's University. A SICK LMS111 laser scanner was mounted to the robot. The extraction algorithm is very configurable; the above image used the parameters configured in the `example.launch` launch file. 7 | 8 | After applying some filters to remove outlying points, Laser Line Extraction implements a split-and-merge algorithm to determine which points belong to lines. Next, it implements the weighted line fitting algorithm by Pfister *et al.* [1] to find the best fit lines and their respective covariance matrices. 9 | 10 | ## Usage 11 | I recommend making a copy of `example.launch` in the launch directory and configuring the parameters until you reach a desirable outcome. The parameters in `example.launch` are a good starting point. Then simply use roslaunch, e.g., 12 | 13 | ``` 14 | roslaunch laser_line_extraction example.launch 15 | ``` 16 | 17 | ## Messages 18 | Laser Line Extraction has two messages types: 19 | 20 | #### LineSegment.msg 21 | ``` 22 | float32 radius 23 | float32 angle 24 | float32[4] covariance 25 | float32[2] start 26 | float32[2] end 27 | ``` 28 | `radius` (m) and `angle` (rad) are the polar parameterization of the line segment. `covariance` is the 2x2 covariance matrix of `radius` and `angle` (listed in row-major order). Finally `start` and `end` are the (x, y) coordinates of the start and end of the line segment. 29 | 30 | #### LineSegmentList.msg 31 | ``` 32 | Header header 33 | LineSegment[] line_segments 34 | ``` 35 | An array of LineSegment.msg with a header. 36 | 37 | ## Topics 38 | 39 | Laser Line Extraction subscribes to a single topic and publishes one or two topics. 40 | 41 | ### Subscribed topics 42 | - `/scan` ([sensor_msgs/LaserScan](http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html)) 43 | - The name of this topic can be configured (see Parameters). 44 | 45 | ### Published topics 46 | - `/line_segments` (laser\_line\_extraction/LineSegmentList) 47 | - A list of line segments extracted from a laser scan. 48 | - `/line_markers` ([visualization_msgs/Marker](http://docs.ros.org/api/visualization_msgs/html/msg/Marker.html)) 49 | - (optional) Markers so that the extracted lines can be visualized in rviz (see above image). Can be toggled (see Parameters). 50 | 51 | ## Parameters 52 | The parameters are listed in alphabetical order. 53 | 54 | - `bearing_std_dev` (default: 0.001) 55 | - The standard deviation of bearing uncertainty in the laser scans (rad). 56 | - `frame_id` (default: "laser") 57 | - The frame in which the line segments are published. 58 | - `least_sq_angle_thresh` (default: 0.0001) 59 | - Change in angle (rad) threshold to stop iterating least squares (`least_sq_radius_thresh` must also be met). 60 | - `least_sq_radius_thresh` (default: 0.0001) 61 | - Change in radius (m) threshold to stop iterating least squares (`least_sq_angle_thresh` must also be met). 62 | - `max_line_gap` (default: 0.4) 63 | - The maximum distance between two points in the same line (m). 64 | - `min_line_length` (default: 0.5) 65 | - Lines shorter than this are not published (m). 66 | - `min_line_points` (default: 9) 67 | - Lines with fewer points than this are not published. 68 | - `min_range` (default: 0.4) 69 | - Points closer than this are ignored (m). 70 | - `max_range` (default: 10000.0) 71 | - Points farther than this are ignored (m). 72 | - `min_split_dist` (default: 0.05) 73 | - When performing "split" step of split and merge, a split between two points results when the two points are at least this far apart (m). 74 | - `outlier_dist` (default: 0.05) 75 | - Points who are at least this distance from all their neighbours are considered outliers (m). 76 | - `publish_markers` (default: false) 77 | - Whether or not markers are published. 78 | - `range_std_dev` (default: 0.02) 79 | - The standard deviation of range uncertainty in the laser scans (m). 80 | - `scan_topic` (default: "scan") 81 | - The LaserScan topic. 82 | 83 | ## References 84 | [1] S. T. Pfister, S. I. Roumeliotis, and J. W. Burdick, "Weighted line fitting algorithms for mobile robot map building and efficient data representation" in Proc. IEEE Intl. Conf. on Robotics and Automation (ICRA), Taipei, Taiwan, 14-19 Sept., 2003. 85 | -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/images/line_extraction.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TempleRAIL/semantic2d/6a36bf5cb4e4db35c183a6641fbdb934ad5081af/manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/images/line_extraction.gif -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/include/laser_line_extraction/line.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_EXTRACTION_LINE_H 2 | #define LINE_EXTRACTION_LINE_H 3 | 4 | #include 5 | #include 6 | #include "laser_line_extraction/utilities.h" 7 | 8 | namespace line_extraction 9 | { 10 | 11 | class Line 12 | { 13 | 14 | public: 15 | // Constructor / destructor 16 | Line(const CachedData&, const RangeData&, const Params&, std::vector); 17 | Line(double angle, double radius, const boost::array &covariance, 18 | const boost::array &start, const boost::array &end, 19 | const std::vector &indices); 20 | ~Line(); 21 | // Get methods for the line parameters 22 | double getAngle() const; 23 | const boost::array& getCovariance() const; 24 | const boost::array& getEnd() const; 25 | const std::vector& getIndices() const; 26 | double getRadius() const; 27 | const boost::array& getStart() const; 28 | // Methods for line fitting 29 | double distToPoint(unsigned int); 30 | void endpointFit(); 31 | void leastSqFit(); 32 | double length() const; 33 | unsigned int numPoints() const; 34 | void projectEndpoints(); 35 | 36 | private: 37 | std::vector indices_; 38 | // Data structures 39 | CachedData c_data_; 40 | RangeData r_data_; 41 | Params params_; 42 | PointParams p_params_; 43 | // Point variances used for least squares 44 | std::vector point_scalar_vars_; 45 | std::vector > point_covs_; 46 | double p_rr_; 47 | // Line parameters 48 | double angle_; 49 | double radius_; 50 | boost::array start_; 51 | boost::array end_; 52 | boost::array covariance_; 53 | // Methods 54 | void angleFromEndpoints(); 55 | void angleFromLeastSq(); 56 | double angleIncrement(); 57 | void calcCovariance(); 58 | void calcPointCovariances(); 59 | void calcPointParameters(); 60 | void calcPointScalarCovariances(); 61 | void radiusFromEndpoints(); 62 | void radiusFromLeastSq(); 63 | }; // class Line 64 | 65 | } // namespace line_extraction 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/include/laser_line_extraction/line_extraction.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_EXTRACTION_H 2 | #define LINE_EXTRACTION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "laser_line_extraction/utilities.h" 9 | #include "laser_line_extraction/line.h" 10 | 11 | namespace line_extraction 12 | { 13 | 14 | class LineExtraction 15 | { 16 | 17 | public: 18 | // Constructor / destructor 19 | LineExtraction(); 20 | ~LineExtraction(); 21 | // Run 22 | void extractLines(std::vector&); 23 | // Data setting 24 | void setCachedData(const std::vector&, const std::vector&, 25 | const std::vector&, const std::vector&); 26 | void setRangeData(const std::vector&); 27 | // Parameter setting 28 | void setBearingVariance(double); 29 | void setRangeVariance(double); 30 | void setLeastSqAngleThresh(double); 31 | void setLeastSqRadiusThresh(double); 32 | void setMaxLineGap(double); 33 | void setMinLineLength(double); 34 | void setMinLinePoints(unsigned int); 35 | void setMinRange(double); 36 | void setMaxRange(double); 37 | void setMinSplitDist(double); 38 | void setOutlierDist(double); 39 | 40 | private: 41 | // Data structures 42 | CachedData c_data_; 43 | RangeData r_data_; 44 | Params params_; 45 | // Indices after filtering 46 | std::vector filtered_indices_; 47 | // Line data 48 | std::vector lines_; 49 | // Methods 50 | double chiSquared(const Eigen::Vector2d&, const Eigen::Matrix2d&, 51 | const Eigen::Matrix2d&); 52 | double distBetweenPoints(unsigned int index_1, unsigned int index_2); 53 | void filterCloseAndFarPoints(); 54 | void filterOutlierPoints(); 55 | void filterLines(); 56 | void mergeLines(); 57 | void split(const std::vector&); 58 | }; 59 | 60 | } // namespace line_extraction 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/include/laser_line_extraction/line_extraction_ros.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_EXTRACTION_ROS_H 2 | #define LINE_EXTRACTION_ROS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "laser_line_extraction/LineSegment.h" 11 | #include "laser_line_extraction/LineSegmentList.h" 12 | #include "laser_line_extraction/line_extraction.h" 13 | #include "laser_line_extraction/line.h" 14 | 15 | namespace line_extraction 16 | { 17 | 18 | class LineExtractionROS 19 | { 20 | 21 | public: 22 | // Constructor / destructor 23 | LineExtractionROS(ros::NodeHandle&, ros::NodeHandle&); 24 | ~LineExtractionROS(); 25 | // Running 26 | void run(); 27 | 28 | private: 29 | // ROS 30 | ros::NodeHandle nh_; 31 | ros::NodeHandle nh_local_; 32 | ros::Subscriber scan_subscriber_; 33 | ros::Publisher line_publisher_; 34 | ros::Publisher marker_publisher_; 35 | // Parameters 36 | std::string frame_id_; 37 | std::string scan_topic_; 38 | bool pub_markers_; 39 | // Line extraction 40 | LineExtraction line_extraction_; 41 | bool data_cached_; // true after first scan used to cache data 42 | // Members 43 | void loadParameters(); 44 | void populateLineSegListMsg(const std::vector&, laser_line_extraction::LineSegmentList&); 45 | void populateMarkerMsg(const std::vector&, visualization_msgs::Marker&); 46 | void cacheData(const sensor_msgs::LaserScan::ConstPtr&); 47 | void laserScanCallback(const sensor_msgs::LaserScan::ConstPtr&); 48 | }; 49 | 50 | } // namespace line_extraction 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/include/laser_line_extraction/utilities.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_EXTRACTION_UTILITIES_H 2 | #define LINE_EXTRACTION_UTILITIES_H 3 | 4 | #include 5 | #include 6 | 7 | namespace line_extraction 8 | { 9 | 10 | struct CachedData 11 | { 12 | std::vector indices; 13 | std::vector bearings; 14 | std::vector cos_bearings; 15 | std::vector sin_bearings; 16 | }; 17 | 18 | struct RangeData 19 | { 20 | std::vector ranges; 21 | std::vector xs; 22 | std::vector ys; 23 | }; 24 | 25 | struct Params 26 | { 27 | double bearing_var; 28 | double range_var; 29 | double least_sq_angle_thresh; 30 | double least_sq_radius_thresh; 31 | double max_line_gap; 32 | double min_line_length; 33 | double min_range; 34 | double max_range; 35 | double min_split_dist; 36 | double outlier_dist; 37 | unsigned int min_line_points; 38 | }; 39 | 40 | struct PointParams 41 | { 42 | std::vector a; 43 | std::vector ap; 44 | std::vector app; 45 | std::vector b; 46 | std::vector bp; 47 | std::vector bpp; 48 | std::vector c; 49 | std::vector s; 50 | }; 51 | 52 | // Inlining this function will be faster 53 | // and also get rid of multiple definitions 54 | // error 55 | inline double pi_to_pi(double angle) 56 | { 57 | angle = fmod(angle, 2 * M_PI); 58 | if (angle >= M_PI) 59 | angle -= 2 * M_PI; 60 | return angle; 61 | } 62 | 63 | } // namespace line_extraction 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/launch/debug.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/launch/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/msg/LineSegment.msg: -------------------------------------------------------------------------------- 1 | float32 radius 2 | float32 angle 3 | float32[4] covariance 4 | float32[2] start 5 | float32[2] end 6 | -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/msg/LineSegmentList.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | LineSegment[] line_segments 3 | -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | laser_line_extraction 4 | 0.1.0 5 | 6 | A ROS package to extract line segments from LaserScan messages. 7 | 8 | Marc Gallant 9 | BSD 10 | catkin 11 | cmake_modules 12 | eigen 13 | geometry_msgs 14 | message_generation 15 | roscpp 16 | rospy 17 | sensor_msgs 18 | visualization_msgs 19 | geometry_msgs 20 | message_runtime 21 | roscpp 22 | rospy 23 | sensor_msgs 24 | visualization_msgs 25 | 26 | -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/src/line.cpp: -------------------------------------------------------------------------------- 1 | #include "laser_line_extraction/line.h" 2 | 3 | namespace line_extraction 4 | { 5 | 6 | /////////////////////////////////////////////////////////////////////////////// 7 | // Constructor / destructor 8 | /////////////////////////////////////////////////////////////////////////////// 9 | Line::Line(const CachedData &c_data, const RangeData &r_data, const Params ¶ms, 10 | std::vector indices): 11 | c_data_(c_data), 12 | r_data_(r_data), 13 | params_(params), 14 | indices_(indices) 15 | { 16 | } 17 | 18 | Line::Line(double angle, double radius, const boost::array &covariance, 19 | const boost::array &start, const boost::array &end, 20 | const std::vector &indices): 21 | angle_(angle), 22 | radius_(radius), 23 | covariance_(covariance), 24 | start_(start), 25 | end_(end), 26 | indices_(indices) 27 | { 28 | } 29 | 30 | Line::~Line() 31 | { 32 | } 33 | 34 | /////////////////////////////////////////////////////////////////////////////// 35 | // Get methods for line parameters 36 | /////////////////////////////////////////////////////////////////////////////// 37 | double Line::getAngle() const 38 | { 39 | return angle_; 40 | } 41 | 42 | const boost::array& Line::getCovariance() const 43 | { 44 | return covariance_; 45 | } 46 | 47 | const boost::array& Line::getEnd() const 48 | { 49 | return end_; 50 | } 51 | 52 | const std::vector& Line::getIndices() const 53 | { 54 | return indices_; 55 | } 56 | 57 | double Line::getRadius() const 58 | { 59 | return radius_; 60 | } 61 | 62 | const boost::array& Line::getStart() const 63 | { 64 | return start_; 65 | } 66 | 67 | /////////////////////////////////////////////////////////////////////////////// 68 | // Utility methods 69 | /////////////////////////////////////////////////////////////////////////////// 70 | double Line::distToPoint(unsigned int index) 71 | { 72 | double p_rad = sqrt(pow(r_data_.xs[index], 2) + pow(r_data_.ys[index], 2)); 73 | double p_ang = atan2(r_data_.ys[index], r_data_.xs[index]); 74 | return fabs(p_rad * cos(p_ang - angle_) - radius_); 75 | } 76 | 77 | double Line::length() const 78 | { 79 | return sqrt(pow(start_[0] - end_[0], 2) + pow(start_[1] - end_[1], 2)); 80 | } 81 | 82 | unsigned int Line::numPoints() const 83 | { 84 | return indices_.size(); 85 | } 86 | 87 | void Line::projectEndpoints() 88 | { 89 | double s = -1.0 / tan(angle_); 90 | double b = radius_ / sin(angle_); 91 | double x = start_[0]; 92 | double y = start_[1]; 93 | start_[0] = (s * y + x - s * b) / (pow(s, 2) + 1); 94 | start_[1] = (pow(s, 2) * y + s * x + b) / (pow(s, 2) + 1); 95 | x = end_[0]; 96 | y = end_[1]; 97 | end_[0] = (s * y + x - s * b) / (pow(s, 2) + 1); 98 | end_[1] = (pow(s, 2) * y + s * x + b) / (pow(s, 2) + 1); 99 | } 100 | 101 | /////////////////////////////////////////////////////////////////////////////// 102 | // Methods for endpoint line fitting 103 | /////////////////////////////////////////////////////////////////////////////// 104 | void Line::endpointFit() 105 | { 106 | start_[0] = r_data_.xs[indices_[0]]; 107 | start_[1] = r_data_.ys[indices_[0]]; 108 | end_[0] = r_data_.xs[indices_.back()]; 109 | end_[1] = r_data_.ys[indices_.back()]; 110 | angleFromEndpoints(); 111 | radiusFromEndpoints(); 112 | } 113 | 114 | void Line::angleFromEndpoints() 115 | { 116 | double slope; 117 | if (fabs(end_[0] - start_[0]) > 1e-9) 118 | { 119 | slope = (end_[1] - start_[1]) / (end_[0] - start_[0]); 120 | angle_ = pi_to_pi(atan(slope) + M_PI/2); 121 | } 122 | else 123 | { 124 | angle_ = 0.0; 125 | } 126 | } 127 | 128 | void Line::radiusFromEndpoints() 129 | { 130 | radius_ = start_[0] * cos(angle_) + start_[1] * sin(angle_); 131 | if (radius_ < 0) 132 | { 133 | radius_ = -radius_; 134 | angle_ = pi_to_pi(angle_ + M_PI); 135 | } 136 | } 137 | 138 | /////////////////////////////////////////////////////////////////////////////// 139 | // Methods for least squares line fitting 140 | /////////////////////////////////////////////////////////////////////////////// 141 | void Line::leastSqFit() 142 | { 143 | calcPointCovariances(); 144 | double prev_radius = 0.0; 145 | double prev_angle = 0.0; 146 | while (fabs(radius_ - prev_radius) > params_.least_sq_radius_thresh || 147 | fabs(angle_ - prev_angle) > params_.least_sq_angle_thresh) 148 | { 149 | prev_radius = radius_; 150 | prev_angle = angle_; 151 | calcPointScalarCovariances(); 152 | radiusFromLeastSq(); 153 | angleFromLeastSq(); 154 | } 155 | calcCovariance(); 156 | projectEndpoints(); 157 | } 158 | 159 | void Line::angleFromLeastSq() 160 | { 161 | calcPointParameters(); 162 | angle_ += angleIncrement(); 163 | } 164 | 165 | double Line::angleIncrement() 166 | { 167 | const std::vector &a = p_params_.a; 168 | const std::vector &ap = p_params_.ap; 169 | const std::vector &app = p_params_.app; 170 | const std::vector &b = p_params_.b; 171 | const std::vector &bp = p_params_.bp; 172 | const std::vector &bpp = p_params_.bpp; 173 | const std::vector &c = p_params_.c; 174 | const std::vector &s = p_params_.s; 175 | 176 | double numerator = 0; 177 | double denominator = 0; 178 | for (std::size_t i = 0; i < a.size(); ++i) 179 | { 180 | numerator += (b[i] * ap[i] - a[i] * bp[i]) / pow(b[i], 2); 181 | denominator += ((app[i] * b[i] - a[i] * bpp[i]) * b[i] - 182 | 2 * (ap[i] * b[i] - a[i] * bp[i]) * bp[i]) / pow(b[i], 3); 183 | } 184 | return -(numerator/denominator); 185 | } 186 | 187 | void Line::calcCovariance() 188 | { 189 | covariance_[0] = p_rr_; 190 | 191 | const std::vector &a = p_params_.a; 192 | const std::vector &ap = p_params_.ap; 193 | const std::vector &app = p_params_.app; 194 | const std::vector &b = p_params_.b; 195 | const std::vector &bp = p_params_.bp; 196 | const std::vector &bpp = p_params_.bpp; 197 | const std::vector &c = p_params_.c; 198 | const std::vector &s = p_params_.s; 199 | 200 | double G = 0; 201 | double A = 0; 202 | double B = 0; 203 | double r, phi; 204 | for (std::size_t i = 0; i < a.size(); ++i) 205 | { 206 | r = r_data_.ranges[indices_[i]]; // range 207 | phi = c_data_.bearings[indices_[i]]; // bearing 208 | G += ((app[i] * b[i] - a[i] * bpp[i]) * b[i] - 2 * (ap[i] * b[i] - a[i] * bp[i]) * bp[i]) / pow(b[i], 3); 209 | A += 2 * r * sin(angle_ - phi) / b[i]; 210 | B += 4 * pow(r, 2) * pow(sin(angle_ - phi), 2) / b[i]; 211 | } 212 | covariance_[1] = p_rr_ * A / G; 213 | covariance_[2] = covariance_[1]; 214 | covariance_[3] = pow(1.0 / G, 2) * B; 215 | } 216 | 217 | void Line::calcPointCovariances() 218 | { 219 | point_covs_.clear(); 220 | double r, phi, var_r, var_phi; 221 | for (std::vector::const_iterator cit = indices_.begin(); cit != indices_.end(); ++cit) 222 | { 223 | r = r_data_.ranges[*cit]; // range 224 | phi = c_data_.bearings[*cit]; // bearing 225 | var_r = params_.range_var; // range variance 226 | var_phi = params_.bearing_var; // bearing variance 227 | boost::array Q; 228 | Q[0] = pow(r, 2) * var_phi * pow(sin(phi), 2) + var_r * pow(cos(phi), 2); 229 | Q[1] = -pow(r, 2) * var_phi * sin(2 * phi) / 2.0 + var_r * sin(2 * phi) / 2.0; 230 | Q[2] = Q[1]; 231 | Q[3] = pow(r, 2) * var_phi * pow(cos(phi), 2) + var_r * pow(sin(phi), 2); 232 | point_covs_.push_back(Q); 233 | } 234 | } 235 | 236 | void Line::calcPointParameters() 237 | { 238 | p_params_.a.clear(); 239 | p_params_.ap.clear(); 240 | p_params_.app.clear(); 241 | p_params_.b.clear(); 242 | p_params_.bp.clear(); 243 | p_params_.bpp.clear(); 244 | p_params_.c.clear(); 245 | p_params_.s.clear(); 246 | 247 | double r, phi, var_r, var_phi; 248 | double a, ap, app, b, bp, bpp, c, s; 249 | for (std::vector::const_iterator cit = indices_.begin(); cit != indices_.end(); ++cit) 250 | { 251 | r = r_data_.ranges[*cit]; // range 252 | phi = c_data_.bearings[*cit]; // bearing 253 | var_r = params_.range_var; // range variance 254 | var_phi = params_.bearing_var; // bearing variance 255 | c = cos(angle_ - phi); 256 | s = sin(angle_ - phi); 257 | a = pow(r * c - radius_, 2); 258 | ap = -2 * r * s * (r * c - radius_); 259 | app = 2 * pow(r, 2) * pow(s, 2) - 2 * r * c * (r * c - radius_); 260 | b = var_r * pow(c, 2) + var_phi * pow(r, 2) * pow(s, 2); 261 | bp = 2 * (pow(r, 2) * var_phi - var_r) * c * s; 262 | bpp = 2 * (pow(r, 2) * var_phi - var_r) * (pow(c, 2) - pow(s, 2)); 263 | p_params_.a.push_back(a); 264 | p_params_.ap.push_back(ap); 265 | p_params_.app.push_back(app); 266 | p_params_.b.push_back(b); 267 | p_params_.bp.push_back(bp); 268 | p_params_.bpp.push_back(bpp); 269 | p_params_.c.push_back(c); 270 | p_params_.s.push_back(s); 271 | } 272 | } 273 | 274 | void Line::calcPointScalarCovariances() 275 | { 276 | point_scalar_vars_.clear(); 277 | double P; 278 | double inverse_P_sum = 0; 279 | for (std::vector >::const_iterator cit = point_covs_.begin(); 280 | cit != point_covs_.end(); ++cit) 281 | { 282 | P = (*cit)[0] * pow(cos(angle_), 2) + 2 * (*cit)[1] * sin(angle_) * cos(angle_) + 283 | (*cit)[3] * pow(sin(angle_), 2); 284 | inverse_P_sum += 1.0 / P; 285 | point_scalar_vars_.push_back(P); 286 | } 287 | p_rr_ = 1.0 / inverse_P_sum; 288 | } 289 | 290 | void Line::radiusFromLeastSq() 291 | { 292 | radius_ = 0; 293 | double r, phi; 294 | for (std::vector::const_iterator cit = indices_.begin(); cit != indices_.end(); ++cit) 295 | { 296 | r = r_data_.ranges[*cit]; // range 297 | phi = c_data_.bearings[*cit]; // bearing 298 | radius_ += r * cos(angle_ - phi) / point_scalar_vars_[cit - indices_.begin()]; // cit to index 299 | } 300 | 301 | radius_ *= p_rr_; 302 | } 303 | 304 | } // namespace line_extraction 305 | -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/src/line_extraction.cpp: -------------------------------------------------------------------------------- 1 | #include "laser_line_extraction/line_extraction.h" 2 | #include 3 | #include 4 | #include 5 | 6 | namespace line_extraction 7 | { 8 | 9 | /////////////////////////////////////////////////////////////////////////////// 10 | // Constructor / destructor 11 | /////////////////////////////////////////////////////////////////////////////// 12 | LineExtraction::LineExtraction() 13 | { 14 | } 15 | 16 | LineExtraction::~LineExtraction() 17 | { 18 | } 19 | 20 | /////////////////////////////////////////////////////////////////////////////// 21 | // Main run function 22 | /////////////////////////////////////////////////////////////////////////////// 23 | void LineExtraction::extractLines(std::vector& lines) 24 | { 25 | // Resets 26 | filtered_indices_ = c_data_.indices; 27 | lines_.clear(); 28 | 29 | // Filter indices 30 | filterCloseAndFarPoints(); 31 | filterOutlierPoints(); 32 | 33 | // Return no lines if not enough points left 34 | if (filtered_indices_.size() <= std::max(params_.min_line_points, static_cast(3))) 35 | { 36 | return; 37 | } 38 | 39 | // Split indices into lines and filter out short and sparse lines 40 | split(filtered_indices_); 41 | filterLines(); 42 | 43 | // Fit each line using least squares and merge colinear lines 44 | for (std::vector::iterator it = lines_.begin(); it != lines_.end(); ++it) 45 | { 46 | it->leastSqFit(); 47 | } 48 | 49 | // If there is more than one line, check if lines should be merged based on the merging criteria 50 | if (lines_.size() > 1) 51 | { 52 | mergeLines(); 53 | } 54 | 55 | lines = lines_; 56 | } 57 | 58 | /////////////////////////////////////////////////////////////////////////////// 59 | // Data setting 60 | /////////////////////////////////////////////////////////////////////////////// 61 | void LineExtraction::setCachedData(const std::vector& bearings, 62 | const std::vector& cos_bearings, 63 | const std::vector& sin_bearings, 64 | const std::vector& indices) 65 | { 66 | c_data_.bearings = bearings; 67 | c_data_.cos_bearings = cos_bearings; 68 | c_data_.sin_bearings = sin_bearings; 69 | c_data_.indices = indices; 70 | } 71 | 72 | void LineExtraction::setRangeData(const std::vector& ranges) 73 | { 74 | r_data_.ranges = ranges; 75 | r_data_.xs.clear(); 76 | r_data_.ys.clear(); 77 | for (std::vector::const_iterator cit = c_data_.indices.begin(); 78 | cit != c_data_.indices.end(); ++cit) 79 | { 80 | r_data_.xs.push_back(c_data_.cos_bearings[*cit] * ranges[*cit]); 81 | r_data_.ys.push_back(c_data_.sin_bearings[*cit] * ranges[*cit]); 82 | } 83 | } 84 | 85 | /////////////////////////////////////////////////////////////////////////////// 86 | // Parameter setting 87 | /////////////////////////////////////////////////////////////////////////////// 88 | void LineExtraction::setBearingVariance(double value) 89 | { 90 | params_.bearing_var = value; 91 | } 92 | 93 | void LineExtraction::setRangeVariance(double value) 94 | { 95 | params_.range_var = value; 96 | } 97 | 98 | void LineExtraction::setLeastSqAngleThresh(double value) 99 | { 100 | params_.least_sq_angle_thresh = value; 101 | } 102 | 103 | void LineExtraction::setLeastSqRadiusThresh(double value) 104 | { 105 | params_.least_sq_radius_thresh = value; 106 | } 107 | 108 | void LineExtraction::setMaxLineGap(double value) 109 | { 110 | params_.max_line_gap = value; 111 | } 112 | 113 | void LineExtraction::setMinLineLength(double value) 114 | { 115 | params_.min_line_length = value; 116 | } 117 | 118 | void LineExtraction::setMinLinePoints(unsigned int value) 119 | { 120 | params_.min_line_points = value; 121 | } 122 | 123 | void LineExtraction::setMinRange(double value) 124 | { 125 | params_.min_range = value; 126 | } 127 | 128 | void LineExtraction::setMaxRange(double value) 129 | { 130 | params_.max_range = value; 131 | } 132 | 133 | void LineExtraction::setMinSplitDist(double value) 134 | { 135 | params_.min_split_dist = value; 136 | } 137 | 138 | void LineExtraction::setOutlierDist(double value) 139 | { 140 | params_.outlier_dist = value; 141 | } 142 | 143 | /////////////////////////////////////////////////////////////////////////////// 144 | // Utility methods 145 | /////////////////////////////////////////////////////////////////////////////// 146 | double LineExtraction::chiSquared(const Eigen::Vector2d &dL, const Eigen::Matrix2d &P_1, 147 | const Eigen::Matrix2d &P_2) 148 | { 149 | return dL.transpose() * (P_1 + P_2).inverse() * dL; 150 | } 151 | 152 | double LineExtraction::distBetweenPoints(unsigned int index_1, unsigned int index_2) 153 | { 154 | return sqrt(pow(r_data_.xs[index_1] - r_data_.xs[index_2], 2) + 155 | pow(r_data_.ys[index_1] - r_data_.ys[index_2], 2)); 156 | } 157 | 158 | /////////////////////////////////////////////////////////////////////////////// 159 | // Filtering points 160 | /////////////////////////////////////////////////////////////////////////////// 161 | void LineExtraction::filterCloseAndFarPoints() 162 | { 163 | std::vector output; 164 | for (std::vector::const_iterator cit = filtered_indices_.begin(); 165 | cit != filtered_indices_.end(); ++cit) 166 | { 167 | const double& range = r_data_.ranges[*cit]; 168 | if (range >= params_.min_range && range <= params_.max_range) 169 | { 170 | output.push_back(*cit); 171 | } 172 | } 173 | filtered_indices_ = output; 174 | } 175 | 176 | void LineExtraction::filterOutlierPoints() 177 | { 178 | if (filtered_indices_.size() < 3) 179 | { 180 | return; 181 | } 182 | 183 | std::vector output; 184 | unsigned int p_i, p_j, p_k; 185 | for (std::size_t i = 0; i < filtered_indices_.size(); ++i) 186 | { 187 | 188 | // Get two closest neighbours 189 | 190 | p_i = filtered_indices_[i]; 191 | if (i == 0) // first point 192 | { 193 | p_j = filtered_indices_[i + 1]; 194 | p_k = filtered_indices_[i + 2]; 195 | } 196 | else if (i == filtered_indices_.size() - 1) // last point 197 | { 198 | p_j = filtered_indices_[i - 1]; 199 | p_k = filtered_indices_[i - 2]; 200 | } 201 | else // middle points 202 | { 203 | p_j = filtered_indices_[i - 1]; 204 | p_k = filtered_indices_[i + 1]; 205 | } 206 | 207 | // Check if point is an outlier 208 | 209 | if (fabs(r_data_.ranges[p_i] - r_data_.ranges[p_j]) > params_.outlier_dist && 210 | fabs(r_data_.ranges[p_i] - r_data_.ranges[p_k]) > params_.outlier_dist) 211 | { 212 | // Check if it is close to line connecting its neighbours 213 | std::vector line_indices; 214 | line_indices.push_back(p_j); 215 | line_indices.push_back(p_k); 216 | Line line(c_data_, r_data_, params_, line_indices); 217 | line.endpointFit(); 218 | if (line.distToPoint(p_i) > params_.min_split_dist) 219 | { 220 | continue; // point is an outlier 221 | } 222 | } 223 | 224 | output.push_back(p_i); 225 | } 226 | 227 | filtered_indices_ = output; 228 | } 229 | 230 | /////////////////////////////////////////////////////////////////////////////// 231 | // Filtering and merging lines 232 | /////////////////////////////////////////////////////////////////////////////// 233 | void LineExtraction::filterLines() 234 | { 235 | std::vector output; 236 | for (std::vector::const_iterator cit = lines_.begin(); cit != lines_.end(); ++cit) 237 | { 238 | if (cit->length() >= params_.min_line_length && cit->numPoints() >= params_.min_line_points) 239 | { 240 | output.push_back(*cit); 241 | } 242 | } 243 | lines_ = output; 244 | } 245 | 246 | void LineExtraction::mergeLines() 247 | { 248 | std::vector merged_lines; 249 | 250 | for (std::size_t i = 1; i < lines_.size(); ++i) 251 | { 252 | // Get L, P_1, P_2 of consecutive lines 253 | Eigen::Vector2d L_1(lines_[i-1].getRadius(), lines_[i-1].getAngle()); 254 | Eigen::Vector2d L_2(lines_[i].getRadius(), lines_[i].getAngle()); 255 | Eigen::Matrix2d P_1; 256 | P_1 << lines_[i-1].getCovariance()[0], lines_[i-1].getCovariance()[1], 257 | lines_[i-1].getCovariance()[2], lines_[i-1].getCovariance()[3]; 258 | Eigen::Matrix2d P_2; 259 | P_2 << lines_[i].getCovariance()[0], lines_[i].getCovariance()[1], 260 | lines_[i].getCovariance()[2], lines_[i].getCovariance()[3]; 261 | 262 | // Merge lines if chi-squared distance is less than 3 263 | if (chiSquared(L_1 - L_2, P_1, P_2) < 3) 264 | { 265 | // Get merged angle, radius, and covariance 266 | Eigen::Matrix2d P_m = (P_1.inverse() + P_2.inverse()).inverse(); 267 | Eigen::Vector2d L_m = P_m * (P_1.inverse() * L_1 + P_2.inverse() * L_2); 268 | // Populate new line with these merged parameters 269 | boost::array cov; 270 | cov[0] = P_m(0,0); 271 | cov[1] = P_m(0,1); 272 | cov[2] = P_m(1,0); 273 | cov[3] = P_m(1,1); 274 | std::vector indices; 275 | const std::vector &ind_1 = lines_[i-1].getIndices(); 276 | const std::vector &ind_2 = lines_[i].getIndices(); 277 | indices.resize(ind_1.size() + ind_2.size()); 278 | indices.insert(indices.end(), ind_1.begin(), ind_1.end()); 279 | indices.insert(indices.end(), ind_2.begin(), ind_2.end()); 280 | Line merged_line(L_m[1], L_m[0], cov, lines_[i-1].getStart(), lines_[i].getEnd(), indices); 281 | // Project the new endpoints 282 | merged_line.projectEndpoints(); 283 | lines_[i] = merged_line; 284 | } 285 | else 286 | { 287 | merged_lines.push_back(lines_[i-1]); 288 | } 289 | 290 | if (i == lines_.size() - 1) 291 | { 292 | merged_lines.push_back(lines_[i]); 293 | } 294 | } 295 | lines_ = merged_lines; 296 | } 297 | 298 | /////////////////////////////////////////////////////////////////////////////// 299 | // Splitting points into lines 300 | /////////////////////////////////////////////////////////////////////////////// 301 | void LineExtraction::split(const std::vector& indices) 302 | { 303 | // Don't split if only a single point (only occurs when orphaned by gap) 304 | if (indices.size() <= 1) 305 | { 306 | return; 307 | } 308 | 309 | Line line(c_data_, r_data_, params_, indices); 310 | line.endpointFit(); 311 | double dist_max = 0; 312 | double gap_max = 0; 313 | double dist, gap; 314 | int i_max, i_gap; 315 | 316 | // Find the farthest point and largest gap 317 | for (std::size_t i = 1; i < indices.size() - 1; ++i) 318 | { 319 | dist = line.distToPoint(indices[i]); 320 | if (dist > dist_max) 321 | { 322 | dist_max = dist; 323 | i_max = i; 324 | } 325 | gap = distBetweenPoints(indices[i], indices[i+1]); 326 | if (gap > gap_max) 327 | { 328 | gap_max = gap; 329 | i_gap = i; 330 | } 331 | } 332 | 333 | // Check for gaps at endpoints 334 | double gap_start = distBetweenPoints(indices[0], indices[1]); 335 | if (gap_start > gap_max) 336 | { 337 | gap_max = gap_start; 338 | i_gap = 1; 339 | } 340 | double gap_end = distBetweenPoints(indices.rbegin()[1], indices.rbegin()[0]); 341 | if (gap_end > gap_max) 342 | { 343 | gap_max = gap_end; 344 | i_gap = indices.size() - 1; 345 | } 346 | 347 | // Check if line meets requirements or should be split 348 | if (dist_max < params_.min_split_dist && gap_max < params_.max_line_gap) 349 | { 350 | lines_.push_back(line); 351 | } 352 | else 353 | { 354 | int i_split = dist_max >= params_.min_split_dist ? i_max : i_gap; 355 | std::vector first_split(&indices[0], &indices[i_split - 1]); 356 | std::vector second_split(&indices[i_split], &indices.back()); 357 | split(first_split); 358 | split(second_split); 359 | } 360 | 361 | } 362 | 363 | } // namespace line_extraction 364 | -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/src/line_extraction_node.cpp: -------------------------------------------------------------------------------- 1 | #include "laser_line_extraction/line_extraction_ros.h" 2 | #include 3 | 4 | int main(int argc, char **argv) 5 | { 6 | 7 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) 8 | { 9 | ros::console::notifyLoggerLevelsChanged(); 10 | } 11 | 12 | ROS_DEBUG("Starting line_extraction_node."); 13 | 14 | ros::init(argc, argv, "line_extraction_node"); 15 | ros::NodeHandle nh; 16 | ros::NodeHandle nh_local("~"); 17 | line_extraction::LineExtractionROS line_extractor(nh, nh_local); 18 | 19 | double frequency; 20 | nh_local.param("frequency", frequency, 25); 21 | ROS_DEBUG("Frequency set to %0.1f Hz", frequency); 22 | ros::Rate rate(frequency); 23 | 24 | while (ros::ok()) 25 | { 26 | line_extractor.run(); 27 | ros::spinOnce(); 28 | rate.sleep(); 29 | } 30 | return 0; 31 | } 32 | 33 | -------------------------------------------------------------------------------- /manually_labeling/semantic_data_collection_ws/src/laser_line_extraction/src/line_extraction_ros.cpp: -------------------------------------------------------------------------------- 1 | #include "laser_line_extraction/line_extraction_ros.h" 2 | #include 3 | #include 4 | 5 | 6 | namespace line_extraction 7 | { 8 | 9 | /////////////////////////////////////////////////////////////////////////////// 10 | // Constructor / destructor 11 | /////////////////////////////////////////////////////////////////////////////// 12 | LineExtractionROS::LineExtractionROS(ros::NodeHandle& nh, ros::NodeHandle& nh_local): 13 | nh_(nh), 14 | nh_local_(nh_local), 15 | data_cached_(false) 16 | { 17 | loadParameters(); 18 | line_publisher_ = nh_.advertise("line_segments", 1); 19 | scan_subscriber_ = nh_.subscribe(scan_topic_, 1, &LineExtractionROS::laserScanCallback, this); 20 | if (pub_markers_) 21 | { 22 | marker_publisher_ = nh_.advertise("line_markers", 1); 23 | } 24 | } 25 | 26 | LineExtractionROS::~LineExtractionROS() 27 | { 28 | } 29 | 30 | /////////////////////////////////////////////////////////////////////////////// 31 | // Run 32 | /////////////////////////////////////////////////////////////////////////////// 33 | void LineExtractionROS::run() 34 | { 35 | // Extract the lines 36 | std::vector lines; 37 | line_extraction_.extractLines(lines); 38 | 39 | // Populate message 40 | laser_line_extraction::LineSegmentList msg; 41 | populateLineSegListMsg(lines, msg); 42 | 43 | // Publish the lines 44 | line_publisher_.publish(msg); 45 | 46 | // Also publish markers if parameter publish_markers is set to true 47 | if (pub_markers_) 48 | { 49 | visualization_msgs::Marker marker_msg; 50 | populateMarkerMsg(lines, marker_msg); 51 | marker_publisher_.publish(marker_msg); 52 | } 53 | } 54 | 55 | /////////////////////////////////////////////////////////////////////////////// 56 | // Load ROS parameters 57 | /////////////////////////////////////////////////////////////////////////////// 58 | void LineExtractionROS::loadParameters() 59 | { 60 | 61 | ROS_DEBUG("*************************************"); 62 | ROS_DEBUG("PARAMETERS:"); 63 | 64 | // Parameters used by this node 65 | 66 | std::string frame_id, scan_topic; 67 | bool pub_markers; 68 | 69 | nh_local_.param("frame_id", frame_id, "laser"); 70 | frame_id_ = frame_id; 71 | ROS_DEBUG("frame_id: %s", frame_id_.c_str()); 72 | 73 | nh_local_.param("scan_topic", scan_topic, "scan"); 74 | scan_topic_ = scan_topic; 75 | ROS_DEBUG("scan_topic: %s", scan_topic_.c_str()); 76 | 77 | nh_local_.param("publish_markers", pub_markers, false); 78 | pub_markers_ = pub_markers; 79 | ROS_DEBUG("publish_markers: %s", pub_markers ? "true" : "false"); 80 | 81 | // Parameters used by the line extraction algorithm 82 | 83 | double bearing_std_dev, range_std_dev, least_sq_angle_thresh, least_sq_radius_thresh, 84 | max_line_gap, min_line_length, min_range, max_range, min_split_dist, outlier_dist; 85 | int min_line_points; 86 | 87 | nh_local_.param("bearing_std_dev", bearing_std_dev, 1e-3); 88 | line_extraction_.setBearingVariance(bearing_std_dev * bearing_std_dev); 89 | ROS_DEBUG("bearing_std_dev: %f", bearing_std_dev); 90 | 91 | nh_local_.param("range_std_dev", range_std_dev, 0.02); 92 | line_extraction_.setRangeVariance(range_std_dev * range_std_dev); 93 | ROS_DEBUG("range_std_dev: %f", range_std_dev); 94 | 95 | nh_local_.param("least_sq_angle_thresh", least_sq_angle_thresh, 1e-4); 96 | line_extraction_.setLeastSqAngleThresh(least_sq_angle_thresh); 97 | ROS_DEBUG("least_sq_angle_thresh: %f", least_sq_angle_thresh); 98 | 99 | nh_local_.param("least_sq_radius_thresh", least_sq_radius_thresh, 1e-4); 100 | line_extraction_.setLeastSqRadiusThresh(least_sq_radius_thresh); 101 | ROS_DEBUG("least_sq_radius_thresh: %f", least_sq_radius_thresh); 102 | 103 | nh_local_.param("max_line_gap", max_line_gap, 0.4); 104 | line_extraction_.setMaxLineGap(max_line_gap); 105 | ROS_DEBUG("max_line_gap: %f", max_line_gap); 106 | 107 | nh_local_.param("min_line_length", min_line_length, 0.5); 108 | line_extraction_.setMinLineLength(min_line_length); 109 | ROS_DEBUG("min_line_length: %f", min_line_length); 110 | 111 | nh_local_.param("min_range", min_range, 0.4); 112 | line_extraction_.setMinRange(min_range); 113 | ROS_DEBUG("min_range: %f", min_range); 114 | 115 | nh_local_.param("max_range", max_range, 10000.0); 116 | line_extraction_.setMaxRange(max_range); 117 | ROS_DEBUG("max_range: %f", max_range); 118 | 119 | nh_local_.param("min_split_dist", min_split_dist, 0.05); 120 | line_extraction_.setMinSplitDist(min_split_dist); 121 | ROS_DEBUG("min_split_dist: %f", min_split_dist); 122 | 123 | nh_local_.param("outlier_dist", outlier_dist, 0.05); 124 | line_extraction_.setOutlierDist(outlier_dist); 125 | ROS_DEBUG("outlier_dist: %f", outlier_dist); 126 | 127 | nh_local_.param("min_line_points", min_line_points, 9); 128 | line_extraction_.setMinLinePoints(static_cast(min_line_points)); 129 | ROS_DEBUG("min_line_points: %d", min_line_points); 130 | 131 | ROS_DEBUG("*************************************"); 132 | } 133 | 134 | /////////////////////////////////////////////////////////////////////////////// 135 | // Populate messages 136 | /////////////////////////////////////////////////////////////////////////////// 137 | void LineExtractionROS::populateLineSegListMsg(const std::vector &lines, 138 | laser_line_extraction::LineSegmentList &line_list_msg) 139 | { 140 | for (std::vector::const_iterator cit = lines.begin(); cit != lines.end(); ++cit) 141 | { 142 | laser_line_extraction::LineSegment line_msg; 143 | line_msg.angle = cit->getAngle(); 144 | line_msg.radius = cit->getRadius(); 145 | line_msg.covariance = cit->getCovariance(); 146 | line_msg.start = cit->getStart(); 147 | line_msg.end = cit->getEnd(); 148 | line_list_msg.line_segments.push_back(line_msg); 149 | } 150 | line_list_msg.header.frame_id = frame_id_; 151 | line_list_msg.header.stamp = ros::Time::now(); 152 | } 153 | 154 | void LineExtractionROS::populateMarkerMsg(const std::vector &lines, 155 | visualization_msgs::Marker &marker_msg) 156 | { 157 | marker_msg.ns = "line_extraction"; 158 | marker_msg.id = 0; 159 | marker_msg.type = visualization_msgs::Marker::LINE_LIST; 160 | marker_msg.scale.x = 0.1; 161 | marker_msg.color.r = 1.0; 162 | marker_msg.color.g = 0.0; 163 | marker_msg.color.b = 0.0; 164 | marker_msg.color.a = 1.0; 165 | for (std::vector::const_iterator cit = lines.begin(); cit != lines.end(); ++cit) 166 | { 167 | geometry_msgs::Point p_start; 168 | p_start.x = cit->getStart()[0]; 169 | p_start.y = cit->getStart()[1]; 170 | p_start.z = 0; 171 | marker_msg.points.push_back(p_start); 172 | geometry_msgs::Point p_end; 173 | p_end.x = cit->getEnd()[0]; 174 | p_end.y = cit->getEnd()[1]; 175 | p_end.z = 0; 176 | marker_msg.points.push_back(p_end); 177 | } 178 | marker_msg.header.frame_id = frame_id_; 179 | marker_msg.header.stamp = ros::Time::now(); 180 | } 181 | 182 | /////////////////////////////////////////////////////////////////////////////// 183 | // Cache data on first LaserScan message received 184 | /////////////////////////////////////////////////////////////////////////////// 185 | void LineExtractionROS::cacheData(const sensor_msgs::LaserScan::ConstPtr &scan_msg) 186 | { 187 | std::vector bearings, cos_bearings, sin_bearings; 188 | std::vector indices; 189 | const std::size_t num_measurements = std::ceil( 190 | (scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment); 191 | for (std::size_t i = 0; i < num_measurements; ++i) 192 | { 193 | const double b = scan_msg->angle_min + i * scan_msg->angle_increment; 194 | bearings.push_back(b); 195 | cos_bearings.push_back(cos(b)); 196 | sin_bearings.push_back(sin(b)); 197 | indices.push_back(i); 198 | } 199 | 200 | line_extraction_.setCachedData(bearings, cos_bearings, sin_bearings, indices); 201 | ROS_DEBUG("Data has been cached."); 202 | } 203 | 204 | /////////////////////////////////////////////////////////////////////////////// 205 | // Main LaserScan callback 206 | /////////////////////////////////////////////////////////////////////////////// 207 | void LineExtractionROS::laserScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg) 208 | { 209 | if (!data_cached_) 210 | { 211 | cacheData(scan_msg); 212 | data_cached_ = true; 213 | } 214 | 215 | std::vector scan_ranges_doubles(scan_msg->ranges.begin(), scan_msg->ranges.end()); 216 | line_extraction_.setRangeData(scan_ranges_doubles); 217 | } 218 | 219 | } // namespace line_extraction 220 | 221 | --------------------------------------------------------------------------------