From bfc1fa1ecb21c5dbe1f51464b41bc89353653753 Mon Sep 17 00:00:00 2001 From: cc0006-coder Date: Fri, 16 Oct 2026 21:21:56 +0800 Subject: [PATCH] =?UTF-8?q?docs:=20explain=20NDT=20map=20equations=20and?= =?UTF-8?q?=20cite=20Biber=20and=20Stra=C3=9Fer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/components/mapping/ndt/ndt_map.py | 36 ++++++++++++++++++++++++--- 1 file changed, 32 insertions(+), 4 deletions(-) diff --git a/src/components/mapping/ndt/ndt_map.py b/src/components/mapping/ndt/ndt_map.py index 9f2af12b..574b47f3 100644 --- a/src/components/mapping/ndt/ndt_map.py +++ b/src/components/mapping/ndt/ndt_map.py @@ -18,7 +18,17 @@ class NdtMap: """ - NDT grid map class + Normal Distributions Transform (NDT) map of a 2D point cloud. + + Each populated grid cell represents its points by a normal distribution, + storing their mean position and 2-by-2 covariance matrix in an NdtGrid. + See update_map for the estimators used by this implementation. + + Reference: + P. Biber and W. Straßer, "The normal distributions transform: a new + approach to laser scan matching," Proceedings of the 2003 IEEE/RSJ + International Conference on Intelligent Robots and Systems (IROS), + pp. 2743-2748, 2003. DOI: 10.1109/IROS.2003.1249285. """ def __init__(self, width_m=60.0, height_m=60.0, resolution_m=5.0, @@ -57,9 +67,27 @@ def _create_grid_points_index_map(self, points_x_array, points_y_array): def update_map(self, points_x_list, points_y_list): """ - Function to update map - points_x_list: List of point cloud x coordinates - points_y_list: List of point cloud y coordinates + Update eligible cells using the supplied point cloud. + + For N points in one cell, let p_i = [x_i, y_i]^T be a column vector. + The mean vector mu and sample covariance matrix Sigma are: + + mu = (1 / N) * sum_{i=1}^{N} p_i + Sigma = (1 / (N - 1)) * sum_{i=1}^{N} + (p_i - mu) (p_i - mu)^T + + Here T denotes transpose, so each covariance summand is an outer + product. The components of mu are stored in mean_x_m and mean_y_m + (meters); Sigma is stored in covariance (square meters). The N - 1 + normalization matches the default np.cov estimator and requires N > 1. + + A cell is replaced only if N >= MIN_POINTS_NUM and N is at least its + previously stored point count. Statistics use only the current input + points in that cell; they are not merged with previous observations. + Other cells remain unchanged. + + points_x_list: List of point cloud x coordinates in meters + points_y_list: List of point cloud y coordinates in meters """ points_x_array, points_y_array = np.array(points_x_list), np.array(points_y_list)