├── LICENSE ├── README.md ├── image └── result.png └── script └── normal_distributions_transform.py /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 ryohei sasaki 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 | # Python sample code of 2D NDT Scan Matching 2 | This Python script demonstrates a 2D scan matching algorithm called Normal Distributions Transform (NDT) for aligning two point clouds. 3 | 4 | ## Usage 5 | 6 | ```bash 7 | python script/normal_distributions_transform.py 8 | ``` 9 | . 10 | ## Results 11 | The source points (red), target points (blue), and transformed source points (green). 12 | 13 | ``` 14 | True transform: [0.1 0.2 0.26179939] 15 | Estimated transform: [0.11679415 0.28055845 0.25984399] 16 | ``` 17 | 18 | ![Result](image/result.png) 19 | 20 | This code is inspired by [PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics). 21 | 22 | -------------------------------------------------------------------------------- /image/result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rsasaki0109/NormalDistributionTransform2D/da613d7b2bb34c5fe71f4dd53eea993b786616bc/image/result.png -------------------------------------------------------------------------------- /script/normal_distributions_transform.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from scipy.optimize import minimize 4 | 5 | def generate_src_points(n_points, field_length): 6 | px = (np.random.rand(n_points) - 0.5) * field_length 7 | py = (np.random.rand(n_points) - 0.5) * field_length 8 | return np.vstack((px, py)).T 9 | 10 | def apply_transformation(points, transform): 11 | R = np.array([[np.cos(transform[2]), -np.sin(transform[2])], 12 | [np.sin(transform[2]), np.cos(transform[2])]]) 13 | return points @ R.T + transform[:2] 14 | 15 | def add_noise(points, scale): 16 | return points + np.random.normal(scale=scale, size=points.shape) 17 | 18 | def ndt_match(src_points, dst_points, resolution=7, max_iter=20, tol=1e-5): 19 | src_points = np.asarray(src_points) 20 | dst_points = np.asarray(dst_points) 21 | 22 | # Build a grid of cells with the specified resolution. 23 | min_corner = np.min(dst_points, axis=0) - resolution 24 | max_corner = np.max(dst_points, axis=0) + resolution 25 | grid_shape = np.ceil((max_corner - min_corner) / resolution).astype(int) 26 | grid = np.zeros(grid_shape, dtype=[('mean', float, 2), ('cov', float, (2, 2)), ('count', int)]) 27 | 28 | # Fill the grid with destination points data. 29 | for p in dst_points: 30 | cell_idx = tuple(((p - min_corner) / resolution).astype(int)) 31 | grid[cell_idx]['count'] += 1 32 | grid[cell_idx]['mean'] += (p - grid[cell_idx]['mean']) / grid[cell_idx]['count'] 33 | grid[cell_idx]['cov'] += np.outer(p - grid[cell_idx]['mean'], p - grid[cell_idx]['mean']) 34 | 35 | # Calculate the covariance matrices for non-empty cells. 36 | for cell in grid.flat: 37 | if cell['count'] > 1: 38 | cell['cov'] /= cell['count'] - 1 39 | else: 40 | cell['cov'] = np.eye(2) * resolution 41 | 42 | # Optimization function to minimize. 43 | def objective_func(x): 44 | R = np.array([[np.cos(x[2]), -np.sin(x[2])], [np.sin(x[2]), np.cos(x[2])]]) 45 | transformed_points = src_points @ R.T + x[:2] 46 | 47 | cost = 0.0 48 | for p in transformed_points: 49 | cell_idx = tuple(((p - min_corner) / resolution).astype(int)) 50 | if 0 <= cell_idx[0] < grid_shape[0] and 0 <= cell_idx[1] < grid_shape[1]: 51 | cell = grid[cell_idx] 52 | if cell['count'] > 0: 53 | d = p - cell['mean'] 54 | cost += d @ np.linalg.pinv(cell['cov']) @ d 55 | 56 | return cost 57 | 58 | def objective_func_vectorized(x): 59 | R = np.array([[np.cos(x[2]), -np.sin(x[2])], [np.sin(x[2]), np.cos(x[2])]]) 60 | transformed_points = src_points @ R.T + x[:2] 61 | 62 | cell_idx = np.floor((transformed_points - min_corner) / resolution).astype(int) 63 | valid_mask = np.all(np.logical_and(cell_idx >= 0, cell_idx < grid_shape), axis=1) 64 | valid_cell_idx = cell_idx[valid_mask] 65 | 66 | valid_cells = grid[valid_cell_idx[:, 0], valid_cell_idx[:, 1]] 67 | count_mask = valid_cells['count'] > 0 68 | valid_cells = valid_cells[count_mask] 69 | valid_transformed_points = transformed_points[valid_mask][count_mask] 70 | 71 | d = valid_transformed_points - valid_cells['mean'] 72 | cov_inv = np.linalg.pinv(valid_cells['cov']) 73 | cost = np.sum(np.einsum('...ij,...j->...i', d @ cov_inv, d)) 74 | 75 | return cost 76 | 77 | 78 | # Optimize the transformation parameters. 79 | x0 = np.zeros(3) 80 | res = minimize(objective_func_vectorized, x0, method='BFGS', options={'maxiter': max_iter, 'gtol': tol}) 81 | return res.x 82 | 83 | def plot_results(src_points, dst_points, src_points_transformed): 84 | plt.scatter(src_points[:, 0], src_points[:, 1], c='r', label='Source Points') 85 | plt.scatter(dst_points[:, 0], dst_points[:, 1], c='b', label='Target Points') 86 | plt.scatter(src_points_transformed[:, 0], src_points_transformed[:, 1], c='g', label='Transformed Source Points') 87 | plt.legend() 88 | plt.axis('equal') 89 | plt.title('2D NDT Scan Matching') 90 | plt.show() 91 | 92 | def main(): 93 | n_points = 1000 94 | field_length = 50.0 95 | 96 | src_points = generate_src_points(n_points, field_length) 97 | true_transform = np.array([0.1, 0.2, np.pi / 12]) 98 | dst_points = apply_transformation(src_points, true_transform) 99 | 100 | estimated_transform = ndt_match(src_points, dst_points) 101 | 102 | src_points_transformed = apply_transformation(src_points, estimated_transform) 103 | 104 | plot_results(src_points, dst_points, src_points_transformed) 105 | 106 | print(f"True transform: {true_transform}") 107 | print(f"Estimated transform: {estimated_transform}") 108 | 109 | if __name__ == '__main__': 110 | main() --------------------------------------------------------------------------------