# BALO: A novel point-to-plane BAlanced Lidar
Odometry

Matteo Azzini, Ezio Malis, Philippe Martinet

► To cite this version:

Matteo Azzini, Ezio Malis, Philippe Martinet. BALO: A novel point-to-plane BAlanced Lidar Odometry. IV 2025 - IEEE Intelligent Vehicles Symposium, Jun 2025, Cluj-Napoca, Romania. hal-05055752

HAL Id: hal-05055752

https://hal.science/hal-05055752v1

Submitted on 5 May 2025

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# BALO: A novel point-to-plane BAlanced Lidar Odometry

Matteo Azzini, Ezio Malis and Philippe Martinet

*Abstract*— Nowadays, lidar technology is widely exploited thanks to the capability of providing a highly accurate 3D point cloud. In particular, in the context of autonomous navigation, the sensor data can be used to localize a vehicle in the environment. In this context, it is important to reduce as much as possible the unavoidable drift, especially for long trajectories. Traditional localization methods rely mainly on approaches inspired by the ICP algorithm, trying to extract reliable features in the incoming point cloud and performing a unidirectional matching with the reference point cloud. With such approach, not all the available information from two successive scans are exploited. Indeed, one could choose the symmetrical approach of extracting reliable features in the reference point cloud and performing a unidirectional matching with the current point cloud. In this work, BALO is presented, a novel point-to-plane lidar odometry, which exploits the symmetrical nature of the problem. By performing a bidirectional matching, the method is able to balance the error inherited in the features extraction phase and the noise from the data. The proposed method is evaluated on synthetic data from CARLA Simulator and on real data from the KITTI dataset. The results show that BALO outperforms the state-of-the-art point-to-plane methods and it is equivalent to the best point-to-point approaches across different real scenarios. Furthermore, on synthetic data, the proposed method showed higher accuracy and robustness to simulated noise, proving the potential superiority of point-to-plane correspondences over point-to-point ones, as expected from the theoretical point of view.

## I. INTRODUCTION

In recent years, Light Detection and Ranging (LIDAR) technology has emerged thanks to its capability of generating a precise 3D point cloud of the surrounding environment. This ability is exploited in different sectors, in particular for robot localization. In this context, the challenge is to refine as much as possible the pose estimation to avoid the so-called drift problem, which is the accumulation of errors over time. The state-of-the-art approaches take inspiration from the Iterative Closest Point (ICP) [1] algorithm. Despite its widespread use, the accuracy of this method is strictly dependent on the nature of the features involved and the formulation of the cost function adopted. In order to overcome those limitations, several modifications were proposed during the years. Among those, [2] came up with the idea of keeping the most simple formulation of the ICP algorithm, while including just small but crucial modifications, such as an adaptive threshold for the matching process and the introduction of an efficient local map to perform a scan-to-map matching to maintain a more robust reference. Similar

approaches are the continuous time formulations presented in [3], where they included a continuity pose constraint interframe and a velocity smoothness constraint intraframe into the point-to-point distances cost function, but also [4] where the authors proposed a new cost function that takes into account the uncertainty of the sensor and the motion model of the robot.

On the other side, some work focused more on the features to be considered. The most popular example is [5], which exploited a novel smoothness metric based on the point neighborhood curvature to extract lines and planes, used in the pose estimation. Later, [6] proposed the same strategy, but improved with a matching weight based on the local smoothness of the extracted features. The same idea of [5] was also exploited by [7], where the authors introduced first a ground point segmentation and then, considering the same type of features, they proposed an odometry pipeline split into two parts, one for the ground points and the other for the remaining ones. More recently, [8] presented a new complex planes extraction based on KD-tree and a new local map update strategy based on uncertainty.

Other works tried to further complexify the process considering different features, in [9]. In this work, the authors considered the Implicit Moving Least Squares (IMLS) surfaces into a scan-to-map registration. Furthermore, they employed a refined preprocessing phase, substituting the conventional voxel grid downsampling with a more intricate method based on the covariance of points along the sensor axes to enhance matching reliability. On the other side, some papers focused on the optimization process, like [10], where the authors proposed a modified version of the classical ICP including a weighted distribution based cost, obtained from a derivation of the KL-divergence.

Later, a different trend emerged, focusing on the semantic segmentation of the point cloud. In this context, the work of [11] proposed a model based multi-metric registration to classify the points and improve the matching process. With a similar idea [12] exploited a deep learning approach to refine the registration process and additionally ignore the dynamic objects.

Almost in parallel, to avoid having to come up with increasingly complex solutions and models, learning-based solutions have been gaining popularity [13] [14]. This type of approach allows each task in an odometry pipeline to be entrusted to machine learning algorithms, at the cost of an additional preprocessing step to prepare the data. However, the results are currently inferior to model-based solutions.

Very recently a new work [15], starting from the idea of [2], tried to propose a fusion between point-to-point

This work has been supported by the AISENSE project.
Authors are with ACENTAURI team at Centre Inria d'Université Côte d'Azur, Sophia-Antipolis, France
matteo.azzini@inria.fr ezio.malis@inria.fr
philippe.martinet@inria.fr
and point-to-plane correspondences. However, the method provided an improvement only in the case of degenerate scenarios like long corridors, while in the general case the performance was similar and even worse in some cases.

This paper, in contrast with the global tendency to complexify the lidar odometry pipeline, focuses on a simple planar features extraction, that allows for a theoretically more precise minimization with respect to the point-to-point correspondences. This is due to the fact that it is highly unlikely for a lidar sensor to hit the exact same point in two consecutive scans. Instead, lidar measurements tend to repeatedly capture the same planar surfaces, making plane based approaches inherently more reliable.

This paper proposes a novel lidar odometry algorithm, named BALO, that includes the following contributions:

* a simple planes extraction method combined with a sample strategy, based on the normals, to ensure an even distribution of the considered features;

* a robust bilateral point-to-plane matching and a robust weighting method, based on the combination of point-to-plane and point-to-point distances of the correspondences;

* a Balanced ICP for precise pose estimation in a lidar odometry framework

* a nonlinear filter on the SE(3) group for pose estimation and prediction of the initial guess for each odometry step.

## II. THEORETICAL BACKGROUND

In this section, the basic elements of the point cloud registration problem are recalled, together with the concept of non bilateral correspondences in point-to-plane matching, from which the proposed method is derived.

### A. Features extraction

The objective of this first step is to find the most reliable features (points, lines, planes, geometric shapes) that can be correctly matched across successive frames and allow a precise pose estimation. In this paper, among all possible features, the planes are chosen, since it is a common assumption to consider a piecewise planar approximation of the 3D world. Traditionally, to classify a 3D point **m** as belonging to a local plane, the neighbor points are considered to compute a curvature score or to apply a plane fitting algorithm minimizing the point-to-plane distances $||\pi^\top \underline{m}||$, where $\pi = (\underline{n}, d)$ is the $4 \times 1$, with **n** the normal and **d** the distance from the origin, and $\underline{m} = (\underline{m}, 1)$ is the point expressed in homogeneous coordinates, thus a $4 \times 1$ vector.

The choice of planar features allows us to get theoretically a precise estimation of the pose between two point clouds in the absence of noise and false matching. This would not be possible using point-to-point correspondences, due to the fact that is almost impossible for a lidar laser to hit the same point in two successive scans, while it's true for a planar surface.

### B. Point cloud registration

Point cloud registration is the core task of the full architecture. Given the two point clouds as input, called reference and current, the transformation between those can be computed by a double-loop approach. In the outer loop, the features extracted from the current point cloud are warped on the reference one using an initial guess transformation. Then, the correspondences are established, generally, relying on the nearest neighbor search. Once the couples (feature, reference point) are obtained, a minimization problem is posed based on the distance between the elements of each correspondence. Traditionally, the solution is found in the inner loop by iterative methods, like the Gauss-Newton algorithm. Both the outer loop and the inner loop are repeated until the maximum number of iterations is reached or when the convergence criteria are met.

In the specific context of point-to-plane correspondences, considering the $n$ planes extracted $\pi_k$ with $k \in \{1, 2, ..., n\}$ from the incoming point cloud measured by the lidar, matched with the points $\underline{m}_k$ in the reference map, the traditional point cloud registration problem is posed as:

$$ \hat{\mathbf{T}} = \operatorname*{argmin}_{\sum_{k=1}^{n} \|\pi_k^\top \mathbf{T} \underline{\mathbf{m}}_k\|^2} \quad (1) $$

Then, the obtained pose as transformation homogeneous matrix $\mathbf{T} \in SE(3)$ is used to update the reference map adding the new points, transformed in the reference frame.

### C. Non bilateral correspondences in point-to-plane matching

The concept of non bilateral correspondences for different features was previously introduced in [16]. In this work, the focus is on the case of point-to-plane correspondences. Having $n_r$ planes in the reference frame and for each plane $\pi_{ri}$ a corresponding point in the current frame $\underline{m}_{ci}$, where $i \in \{1, 2, ..., n_r\}$. The objective is the estimation of the optimal transformation matrix $^r\mathbf{T}_c$ that minimizes the following weighted least squares problem:

$$ ^r\hat{\mathbf{T}}_c = \operatorname*{argmin}_{\sum_{i=1}^{n_r} w_{ri}^2 \|\pi_{ri}^\top {}^r\mathbf{T}_c \underline{\mathbf{m}}_{ci}\|^2} \quad (2) $$

where $w_{ri}$ are positive weights.

Having $n_c$ planes in the current frame and for each plane $\pi_{cj}$ a corresponding point in the reference frame $\underline{m}_{rj}$, where $j \in \{1, 2, ..., n_c\}$ the following problem can be posed:

$$ {}^c\hat{\mathbf{T}}_r = \operatorname*{argmin}_{\sum_{j=1}^{n_c} w_{cj}^2 \|\pi_{cj}^\top {}^c\mathbf{T}_r \underline{\mathbf{m}}_{rj}\|^2} \quad (3) $$

where $w_{cj}$ are positive weights.

The features are non bilateral, namely the result of the optimization depends on the direction of the cost function, in the sense that depends on which point cloud the planes are extracted and with which point cloud are matched. Therefore,
with noisy data, solving the two problems separately will give two estimated solutions such that:

$$r\hat{T}_c \neq c\hat{T}_r^{-1}$$

while the result should obviously show that the true poses verify $r\hat{T}_c = c\hat{T}_r^{-1}$.

In [16] it has been shown how that the estimation problem can be balanced by simultaneously optimising a global cost function that takes into account the correspondences in both directions.

### III. PROPOSED METHOD

This section provides a comprehensive overview of the proposed lidar odometry methodology BALO, represented by the diagram in Fig. 1, where the block with contributions are highlighted with the color green

<figure><img src="image_2.png" /><figcaption>Fig. 1: Block diagram of the proposed balanced lidar odometry architecture BALO</figcaption></figure>

The framework receives a 3D point cloud as input and perform a planes extraction and sampling process, as explained in III-A. Then, the obtained planes and feature points feed the Balanced ICP block, composed by a bilateral matching and a pose estimation block, presented respectively in III-B and III-C. The resulting pose is then used twice. First to predict the initial guess pose for the next frame by the nonlinear filter, detailed in III-D. Second, to update the local map where the reference point cloud, planes and feature points are stored, as explained in III-E.

#### A. Planes extraction and sampling

Considering an input 3D point cloud $P_c$, the objective is to find local planes in the scene. First, a downsampling by voxel grid of resolution $\alpha$ is applied to reduce the number of points in the scan for efficiency purposes. For each of those points, referred as feature points, the plane extraction is performed. The process consist in a neighbors search into $P_c$ for each feature point. For this process, an adaptive radius policy is adopted, starting from an initial size $\delta$, since, for a general lidar sensor, density is inversely proportional to the distance of the object hit by the laser. If the number of points found is greater than a defined minimum $N_{min}$, the Singular Value Decomposition (SVD) is applied by estimating the eigenvalues and eigenvectors of the covariance matrix of the group of points to the plane coefficients. From those the normalized normal vector and the distance from

the origin are extracted. A plane is accepted if its score, which is defined as the mean of the point-to-plane distances considering the points in the neighborhood, is less than a certain threshold $\gamma$.

The first contribution of this work is to employ a sampling technique within the spherical coordinates space, effectively partitioning the environment into $n_{az}$ by $n_{al}$ sectors, defined by azimuth and altitude angles. A graphical example can be observed in Fig. 2. This approach facilitates the systematic sampling of extracted planes based on their normals, ensuring a balanced representation of the environmental features. By dividing the spherical space, the risk of an uneven distribution of samples is mitigated, thereby enhancing the robustness and accuracy of the pose estimation.

<figure><img src="image_3.png" /><figcaption>Fig. 2: Example of sampling by spherical coordinates. The red lines show the sector division over the altitude and azimuth angles. The blue points represent the normal vectors of each plane. Here, an uneven distribution of planes can be observed, which can be mitigated by changing the maximum number of sectors and the number of planes per sector.</figcaption></figure>

Furthermore, this technique addresses the convergence issue encountered at the apex of the spherical coordinates system, where all sectors converge in such small sphere segments that they cancel out the partition of space. To alleviate this, a dome effect is introduced by unifying all sectors at the zenith, thus maintaining a consistent sampling strategy across the entire spherical space. For each sampling cell, a predetermined maximum number of normals is defined and the corresponding higher scored planes are kept. This modification enhances the effectiveness of the sampling approach while ensuring comprehensive coverage of the environment. At the end of the process, the resulting planes and their feature points are used in the pose estimation process. After that, the planes and the points are transformed in the reference frame and stored in the local map.

#### B. point-to-plane matching and weighting

This subsection describes the matching and weighting process for the point-to-plane correspondences. Note that the functioning presented below is the same both for the features extracted in the current point cloud and the features stored as reference in the local map, respectively matched with the points in the reference point cloud and with the points in the current point cloud.
Consequently, considering a set of planes and their feature points, the closest point in the other point cloud are sought by voxel based nearest neighbour search. For each feature point, a correspondence is accepted when the closest neighbor found is inside a radius. This threshold, starting from an initial value $\tau$, is adaptively updated as in [2]. Then, for each those $n$ correspondences, the point-to-point distance is saved and the point-to-plane distance is computed. Both, values are then used into a Geman-McClure robust M-estimator with adaptive kernel inspired by [2] to get the sets of weights $\mathbf{A}$ and $\mathbf{B}$. The final weights are found by combining the two sets as:

$$w_k = \frac{a_k b_k}{\sum a_k b_k}$$

with $a_i \in \mathbf{A}$, $b_i \in \mathbf{B}$ and $k \in \{1, 2, ..., n\}$.

### C. Pose estimation by Balanced ICP

The proposed pose estimation by Balanced ICP is detailed in the previous work [16]. By exploiting the concept of non bilateral features recalled in II-C, the presented approach performs the following simultaneous optimization of two contributions by defining the pose estimation problem in both directions. Consider the $n_r$ planes $\pi_{ri}$, with $i \in \{1, ..., n_c\}$, extracted from the reference point cloud and matched with the points $\mathbf{m}_{ci}$ in the current point cloud, and then consider the $n_c$ planes $\pi_{cj}$, with $j \in \{1, ..., n_r\}$, extracted from the current point cloud and matched with the points $\mathbf{m}_{rj}$ in the reference point cloud. The following minimization problem can be defined to find homogeneous transformation $^r\mathbf{T}_c$ between the current and the reference point cloud:

$$\min_{^r\mathbf{T}_c} \frac{n_r}{n_r + n_c} \|\mathbf{e}_r(^r\mathbf{T}_c)\|^2 + \frac{n_c}{n_r + n_c} \|\mathbf{e}_c(^r\mathbf{T}_c)\|^2 \quad (4)$$

where the error term $\mathbf{e}_r$ and $\mathbf{e}_c$ are respectively the weighted residuals of the point-to-plane matching of the features extracted from the current point cloud and the ones extracted from the local map. Both contributes are additionally weighted with a term dependent from the number of correspondences found $n_c$ and $n_r$. The pose is iteratively optimized starting from an initial guess. The problem is solved as a sensor-based control problem, named Virtual Lidar Servoing (VLS), by considering:

$$\dot{\mathbf{e}}_r = \mathbf{L}_r \mathbf{v}$$

$$\dot{\mathbf{e}}_c = \mathbf{L}_c \mathbf{v}$$

where $\mathbf{v} \in \mathbb{R}^6$ is the velocity of a virtual lidar and $\mathbf{L}_r$ and $\mathbf{L}_c$ are called the interaction matrices. Stacking together both errors and interaction matrices, the problem can be expressed as:

$$\mathbf{e} = \begin{bmatrix} \mathbf{e}_r \\ \mathbf{e}_c \end{bmatrix} \quad \mathbf{L} = \begin{bmatrix} \mathbf{L}_r \\ \mathbf{L}_c \end{bmatrix}$$

$$\dot{\mathbf{e}} = \mathbf{L} \mathbf{v}$$

thereby, the control law can be expressed as:

$$\mathbf{v} = -\lambda(\mathbf{L}^{\top}\mathbf{L})^{-1}\mathbf{L}^{\top}\mathbf{e}$$

where $\lambda$ is a constant positive scalar factor that tunes the amplitude of the incremental step. It is worth noticing that this is like a Gauss-Newton optimization approach. The new pose is then computed as:

$$\mathbf{T}_{k+1} = \mathbf{T}_k \exp([\mathbf{v}])$$

The point cloud is transformed using the estimated pose and the process is iterated until convergence, when the virtual velocity decrease under a given threshold $\epsilon$, or the maximum number of iterations $I$ is reached.

### D. Pose prediction and estimation by a non-linear filter

This section presents a non-linear filter on the Special Euclidean Lie Group SE(3) group adapted from the filter on the Special Linear Lie Group SL(3) proposed in [17]. The objective is to obtain a filtered estimation of both the pose and the velocity of the Lidar and a prediction of the pose in the next frame. Considering the homogeneous transformation matrix $\mathbf{T} \in SE(3)$, the pose estimated in the lidar odometry, and the matrix of the velocities $\mathbf{A} \in se_3$. Under the assumption of a constant velocity, very common in the lidar odometry context, to define the kinematic model of the system. Then, the non-linear system can be expressed as:

$$\dot{\mathbf{T}} = \mathbf{T}\mathbf{A} \quad (5)$$

$$\dot{\mathbf{A}} = 0 \quad (6)$$

The objective is to obtain an estimate $\hat{\mathbf{T}}$ of $\mathbf{T}$ and an estimate $\hat{\mathbf{A}}$ of $\mathbf{A}$. The model of the non-linear estimator is:

$$\dot{\hat{\mathbf{T}}} = \hat{\mathbf{T}}(\hat{\mathbf{A}} + \mathbf{C}_T) \quad (7)$$

$$\dot{\hat{\mathbf{A}}} = \mathbf{C}_A \quad (8)$$

where $\mathbf{C}_T$ and $\mathbf{C}_A$ are control inputs to be designed. By defining the left-invariant error between $\mathbf{T}$ and $\hat{\mathbf{T}}$ as:

$$\tilde{\mathbf{T}} = \hat{\mathbf{T}}^{-1}\mathbf{T}$$

Then, by defining the adjoint as:

$$\mathrm{Ad}_{\mathbf{T}}(\mathbf{A}) = \mathbf{T}\mathbf{A}\mathbf{T}^{-1}$$

and the projector $\mathbb{P}(\mathbf{T})$ as:

$$\mathbb{P}(\mathbf{T}) := \sum_{i=0}^{6} \mathrm{tr} \left( \mathbf{T}^{\mathrm{T}} \mathbf{B}_{i} \right) \mathbf{B}_{i} \quad (9)$$

where $\mathrm{tr}(\cdot)$ is the trace of the matrix and $\mathbf{B}_i$ are the basis of the Lie algebra $se(3)$.

As in [17], the final control laws can be derived as:

$$\mathbf{C}_T = -\hat{\mathbf{A}} + \mathrm{Ad}_{\hat{\mathbf{T}}} (\hat{\mathbf{A}} - k_t \mathbb{P}(\tilde{\mathbf{T}}^{\top}(\mathbf{I} - \tilde{\mathbf{T}}))) \quad (10)$$

$$\mathbf{C}_A = -k_a \mathbb{P}(\tilde{\mathbf{T}}^{\top}(\mathbf{I} - \tilde{\mathbf{T}})) \quad (11)$$

where $k_t$ and $k_a$ are positive gains. It can be shown, as in [17], that the filter converges to provide an estimation
of the pose and the velocity. With the estimated velocity, the future pose of the next point cloud can be predicted to initialize the outer loop (matching) of the ICP algorithm.

### E. Local mapping

The local map, built as a voxel grid of resolution $\beta$, has a pivotal role in scan-to-map point cloud registration since it allows the reduction of the error accumulation typical of an odometry approach. In this work, the starting position and orientation of the lidar sensor is considered as the global reference frame, thus the local map is always expressed in this frame. For efficient maintenance, the map range $R$ is fixed, characterized by an origin that always coincides with the current estimated sensor position, emulating a sliding window mechanism. To obtain this behavior, the reference point cloud is updated at each scan, first, moving the origin, thus removing points too far. Secondly, the current down-sampled point cloud is transformed in the reference frame using the estimated lidar pose and the points are added to the local map in the corresponding voxel until the maximum number per voxel $N_{vox}$ is reached. With the same strategy, also the extracted planes and feature points are transformed and stored in the local map.

## IV. EXPERIMENTS

In this section, BALO is compared with state-of-the-art approaches. The current implementation runs at 5 Hz, which cannot be considered real-time, as most available lidar sensors operate at 10 Hz. However, the algorithm's structure is well suited for parallelization. In particular, plane extraction, point-to-plane matching, and the construction of the matrices for solving the minimization problem involve operations that are repeated for each point. In all three cases, these per-point operations are independent of one another, and thus, they can be efficiently parallelized.

To test the efficiency and robustness of the proposed approach, experiments are performed both on synthetic data generated by the Carla simulator [18] and on real data from the KITTI dataset [19]. In both cases it is supposed that the Lidar motion distortion has been corrected. The results are compared with the best ranked state-of-the-art lidar odometry methods.

### A. Parameters setup

To maintain the approach as simple as possible, the number of required parameters is minimized. The complete list of parameters is the following:

* Sampling voxel grid resolution $\alpha$: 0.50 m;

* Local map resolution $\beta$: 1.50 m;

* Local map range $R$: 80 m;

* Number of points per voxel $N_{vox}$: 20;

* Min number of points for plane extraction $N_{min}$: 10;

* Plane extraction threshold $\gamma$: 0.05 m;

* Initial radius search for plane extraction $\delta$: 0.50 m;

* Policy adaptive radius: double it after 50 m;

* Sectors in altitude for spherical sampling $n_{al}$: 18;

* Sectors in azimuth for spherical sampling $n_{az}$: 36;

* Max number of odometry iterations $I$: 50;

* Convergence threshold $\epsilon$: 0.001;

* Initial matching threshold $\tau$: 2.00 m;

### B. Carla simulator data

The Carla simulator [18] is a powerful tool for the development of autonomous driving algorithms. It allows the user to create a virtual environment with different sensors in urban and peri-urban scenarios. For this set of experiments, four different scenarios (Fig. 3) are considered, taken from the available maps in Carla official setup. For each scenario, the ground truth trajectory is generated along with the 3D point clouds simulated by a lidar sensor during the navigation. The generated 3D points are without motion distortion or noise. The original points range measurements are then perturbed with Gaussian noise in the direction of the lidar ray with different standard deviation values, such as 0.03 m, 0.05 m and 0.10 m. The values are chosen accordingly with the datasheets of real Ouster OS2 LiDAR and the Gaussian distribution is a good approximation of the actual noise behavior observed in real range measurements. The objective of these simulations is to show the robustness of BALO method in different scenarios and noise conditions with respect to a classic point-to-point approach. For this purpose, KISS-ICP [2] is taken as the baseline method, since it is most representative of a classical point-to-point approach, and it is shown in [4] having equivalent performances with respect to other methods.

<figure><img src="image_4.png" /><figcaption>Fig. 3: Carla simulator maps used for the experiments</figcaption></figure>

Results are shown for each map and for each noise value in tables I, II, III and IV in terms of Relative Translation Error (RTE) in percentage and Relative Rotation Error (RRE) [19] in degrees per meter. These metrics average the trajectory drift over segments of lengths ranging from 100 m to 800 m, thus it highlights the quality of the local pose estimation, which is fundamental for long trajectories and even small improvements have a big impact on the global pose. Best values, thus the lowest, are in bold. As can be observed, BALO outperforms KISS-ICP in all the metrics and noise conditions, showing higher precision and robustness. Furthermore, from the zero noise case, it can be observed a very
low error of BALO, which confirms the superior capability of minimization of the point-to-plane approach with respect to the point-to-point in urban or peri-urban scenarios due to the presence of planar surfaces in the environment. However, the error is not zero even in perfect conditions due to the fact that also not perfectly planar surfaces are approximated by planes, leading to a slightly imperfect pose estimation. A completely structured environment would lead to a zero error, but this is not the case in real scenarios.

<table><caption>TABLE I: Comparison Town01 (827 m).</caption><thead><tr><th>noise</th><th>KISS RTE</th><th>BALO RTE</th><th>KISS RRE</th><th>BALO RRE</th></tr></thead><tbody><tr><td>0.00m</td><td>0.143</td><td><strong>0.056</strong></td><td>0.0007</td><td><strong>0.0003</strong></td></tr><tr><td>0.03m</td><td>0.134</td><td><strong>0.066</strong></td><td>0.0006</td><td><strong>0.0003</strong></td></tr><tr><td>0.05m</td><td>0.139</td><td><strong>0.076</strong></td><td>0.0007</td><td><strong>0.0004</strong></td></tr><tr><td>0.10m</td><td>0.149</td><td><strong>0.111</strong></td><td><strong>0.0007</strong></td><td><strong>0.0004</strong></td></tr></tbody></table>

<table><caption>TABLE II: Comparison Town02 (1922 m).</caption><thead><tr><th>noise</th><th>KISS RTE</th><th>BALO RTE</th><th>KISS RRE</th><th>BALO RRE</th></tr></thead><tbody><tr><td>0.00m</td><td>0.090</td><td><strong>0.048</strong></td><td>0.0006</td><td><strong>0.0003</strong></td></tr><tr><td>0.03m</td><td>0.091</td><td><strong>0.049</strong></td><td>0.0006</td><td><strong>0.0003</strong></td></tr><tr><td>0.05m</td><td>0.086</td><td><strong>0.057</strong></td><td>0.0006</td><td><strong>0.0004</strong></td></tr><tr><td>0.10m</td><td>0.096</td><td><strong>0.067</strong></td><td>0.0007</td><td><strong>0.0004</strong></td></tr></tbody></table>

<table><caption>TABLE III: Comparison on Town03 (752 m).</caption><thead><tr><th>noise</th><th>KISS RTE</th><th>BALO RTE</th><th>KISS RRE</th><th>BALO RRE</th></tr></thead><tbody><tr><td>0.00m</td><td>0.087</td><td><strong>0.014</strong></td><td><strong>0.0003</strong></td><td><strong>0.0003</strong></td></tr><tr><td>0.03m</td><td>0.088</td><td><strong>0.013</strong></td><td><strong>0.0003</strong></td><td><strong>0.0003</strong></td></tr><tr><td>0.05m</td><td>0.095</td><td><strong>0.019</strong></td><td>0.0004</td><td><strong>0.0003</strong></td></tr><tr><td>0.10m</td><td>0.111</td><td><strong>0.034</strong></td><td>0.0005</td><td><strong>0.0004</strong></td></tr></tbody></table>

<table><caption>TABLE IV: Comparison on Town05 (1365 m).</caption><thead><tr><th>noise</th><th>KISS RTE</th><th>BALO RTE</th><th>KISS RRE</th><th>BALO RRE</th></tr></thead><tbody><tr><td>0.00m</td><td>0.066</td><td><strong>0.022</strong></td><td>0.0003</td><td><strong>0.0002</strong></td></tr><tr><td>0.03m</td><td>0.063</td><td><strong>0.024</strong></td><td>0.0003</td><td><strong>0.0002</strong></td></tr><tr><td>0.05m</td><td>0.064</td><td><strong>0.026</strong></td><td>0.0003</td><td><strong>0.0002</strong></td></tr><tr><td>0.10m</td><td>0.073</td><td><strong>0.031</strong></td><td>0.0004</td><td><strong>0.0002</strong></td></tr></tbody></table>

### C. KITTI dataset

The KITTI dataset [19] provides a set of 11 sequences with the ground truth poses in different driving scenarios (urban, peri-urban, country, highway) recorded with a Vehodyne HDL-64E laser scanner, GPS, IMU and stereo vision system. Lidar points are already corrected by applying a motion compensation. Before running the test, an additional correction of 0.205° is required to rectify the calibration errors in the raw point cloud registration, as suggested in [4]. The obtained results are compared with the state-of-the-art point-to-plane methods (LOAM [5], FLOAM [6] and MAD-ICP [8]), and with the state-of-the-art point-to-point methods (KISS-ICP [2], CT-ICP [3] and Traj-LO [4]). The results of each state-of-the-art method for each of 11 scenes are those mentioned in the reference papers. The comparison is done in terms of Relative Translation Error (RTE), the official metric from KITTI dataset. Relative Rotation Error (RRE) is not considered here since none of the methods provide it in the original papers, probably due to the fact that the variations are very small and not significant. When computing the score on multiple sequences, the RTE average (Avg) is weighted over all the segments of all sequences and mirrors KITTI's

benchmark evaluation. The results are presented in Table V and Table VI. Best values, thus the lowest, are in bold. Second best values are underlined. As can be observed from the tables V and VI, the proposed method outperforms the point-to-plane methods and it provides equivalent results with the point-to-point approaches across different scenarios. As expected from the theoretical point of view and confirmed by the results on synthetic data shown in IV-B, the point-to-plane can potentially provide better pose estimation with respect to the point-to-point approach.

<table><caption>TABLE V: RTE comparison with point-to-plane approaches</caption><thead><tr><th>Scene</th><th>FLOAM</th><th>LOAM</th><th>MAD-ICP</th><th>BALO</th></tr></thead><tbody><tr><td>00</td><td><strong>0.71</strong></td><td>0.78</td><td>0.73</td><td><strong>0.50</strong></td></tr><tr><td>01</td><td><strong>0.71</strong></td><td>1.43</td><td>0.85</td><td>0.76</td></tr><tr><td>02</td><td><strong>0.73</strong></td><td>0.92</td><td>0.76</td><td><strong>0.51</strong></td></tr><tr><td>03</td><td>0.98</td><td>0.86</td><td>0.84</td><td><strong>0.61</strong></td></tr><tr><td>04</td><td><strong>0.57</strong></td><td>0.71</td><td>0.69</td><td><strong>0.39</strong></td></tr><tr><td>05</td><td>0.62</td><td>0.57</td><td>0.48</td><td><strong>0.26</strong></td></tr><tr><td>06</td><td>0.33</td><td>0.65</td><td>0.48</td><td><strong>0.28</strong></td></tr><tr><td>07</td><td>0.47</td><td>0.63</td><td>0.45</td><td><strong>0.32</strong></td></tr><tr><td>08</td><td>1.04</td><td>1.12</td><td>1.21</td><td><strong>0.81</strong></td></tr><tr><td>09</td><td>0.88</td><td>0.77</td><td>1.02</td><td><strong>0.49</strong></td></tr><tr><td>10</td><td>1.02</td><td>0.79</td><td>0.96</td><td><strong>0.58</strong></td></tr><tr><td>Avg</td><td>0.77</td><td>0.86</td><td>0.81</td><td><strong>0.53</strong></td></tr></tbody></table>

<table><caption>TABLE VI: RTE comparison with point-to-point approaches</caption><thead><tr><th>Scene</th><th>KISS-ICP</th><th>CT-ICP</th><th>Traj-LO</th><th>BALO</th></tr></thead><tbody><tr><td>00</td><td>0.52</td><td><strong>0.49</strong></td><td>0.50</td><td>0.50</td></tr><tr><td>01</td><td><strong>0.72</strong></td><td>0.76</td><td>0.81</td><td>0.76</td></tr><tr><td>02</td><td>0.53</td><td><strong>0.52</strong></td><td>0.52</td><td><strong>0.51</strong></td></tr><tr><td>03</td><td>0.65</td><td>0.72</td><td>0.67</td><td><strong>0.61</strong></td></tr><tr><td>04</td><td><strong>0.35</strong></td><td>0.39</td><td>0.40</td><td>0.39</td></tr><tr><td>05</td><td>0.30</td><td><strong>0.25</strong></td><td><strong>0.25</strong></td><td>0.26</td></tr><tr><td>06</td><td><strong>0.26</strong></td><td>0.27</td><td>0.27</td><td>0.28</td></tr><tr><td>07</td><td>0.33</td><td>0.31</td><td><strong>0.30</strong></td><td>0.32</td></tr><tr><td>08</td><td><strong>0.81</strong></td><td><strong>0.81</strong></td><td><strong>0.81</strong></td><td><strong>0.81</strong></td></tr><tr><td>09</td><td>0.49</td><td>0.49</td><td><strong>0.45</strong></td><td>0.49</td></tr><tr><td>10</td><td>0.54</td><td><strong>0.48</strong></td><td>0.55</td><td>0.58</td></tr><tr><td>Avg</td><td><strong>0.54</strong></td><td><strong>0.53</strong></td><td><strong>0.53</strong></td><td><strong>0.53</strong></td></tr></tbody></table>

Consequently, future works will address a deeper analysis and modeling of the environment to further improve the results obtained on the real data. To further clarify the contributions of the method, an additional ablation study is conducted. The aim is to show the importance of each contribution. Results are shown in Table VII, in terms of RTE in percentage for each scene and in average, as before. Best values, thus the lowest, are in bold. Second best values are underlined. The full implementation proposed in this work, is compared with four different modifications: without plane sampling (w/o pl.s.), without combined weights (w/o c.w.), replacing the balanced ICP with the classical ICP (w/o bal ICP). For completeness, also the case without the non-linear filter for pose prediction (w/o filter) is included, but in this case the expectation is to find minimal variations, due to the smoothness of the velocity in the KITTI dataset.

The results show that the proposed method is the best in all the scenarios and in average, confirming the importance of each contribution to the final results. The results confirm the importance of each contribution in the proposed BALO framework. While some modifications individually achieve
competitive results in specific scenarios, the full implementation consistently delivers the best performance on average.

Removing plane sampling (w/o pl.s.) leads to slight performance degradation, highlighting its role in improving correspondence selection. Similarly, excluding combined weights (w/o c.w.) results in increased RTE across most sequences, confirming that the weighting strategy enhances robustness. Replacing balanced ICP with standard ICP (w/o bal ICP) significantly worsens performance, especially in challenging scenarios, proving the necessity of a more informed correspondence weighting scheme. Interestingly, the absence of the non-linear filter for pose prediction (w/o filter) results in nearly identical accuracy to the full BALO implementation. This is expected, as the selected KITTI sequences exhibit smooth trajectories without abrupt velocity changes. In such cases, the filter's predicted motion remains close to a simple extrapolation from the previous step. However, in more dynamic environments, the filter would provide a significant advantage by refining the motion estimate under sudden accelerations or external disturbances. In conclusion, while each component individually contributes to better accuracy, their combined effect leads to the best overall results. The full BALO implementation achieves the lowest average RTE, demonstrating that the synergy between plane sampling, combined weighting, balanced ICP, and non-linear filtering results in the most robust and precise lidar odometry solution.

<table><caption>TABLE VII: Ablation study of BALO on KITTI dataset.</caption><thead><tr><th>Scene</th><th>w/o pl.s.</th><th>w/o c.w.</th><th>w/o bal ICP</th><th>w/o filter</th><th>BALO</th></tr></thead><tbody><tr><td>00</td><td>0.505</td><td><strong>0.500</strong></td><td>0.509</td><td>0.506</td><td><strong>0.504</strong></td></tr><tr><td>01</td><td><strong>0.756</strong></td><td>0.761</td><td>0.777</td><td>0.760</td><td><strong>0.757</strong></td></tr><tr><td>02</td><td>0.527</td><td>0.587</td><td>0.869</td><td><strong>0.515</strong></td><td><strong>0.512</strong></td></tr><tr><td>03</td><td>0.605</td><td><strong>0.601</strong></td><td><strong>0.598</strong></td><td>0.604</td><td>0.605</td></tr><tr><td>04</td><td><strong>0.393</strong></td><td><strong>0.393</strong></td><td><strong>0.392</strong></td><td><strong>0.392</strong></td><td>0.394</td></tr><tr><td>05</td><td>0.261</td><td>0.256</td><td><strong>0.259</strong></td><td>0.261</td><td>0.260</td></tr><tr><td>06</td><td><strong>0.279</strong></td><td><strong>0.277</strong></td><td>0.279</td><td>0.280</td><td><strong>0.279</strong></td></tr><tr><td>07</td><td><strong>0.318</strong></td><td>0.324</td><td>0.324</td><td><strong>0.318</strong></td><td><strong>0.318</strong></td></tr><tr><td>08</td><td>0.815</td><td>0.820</td><td><strong>0.807</strong></td><td><strong>0.806</strong></td><td>0.809</td></tr><tr><td>09</td><td>0.490</td><td>0.498</td><td>0.496</td><td><strong>0.488</strong></td><td><strong>0.489</strong></td></tr><tr><td>10</td><td>0.583</td><td>0.599</td><td><strong>0.575</strong></td><td><strong>0.582</strong></td><td>0.583</td></tr><tr><td>Avg</td><td>0.532</td><td>0.545</td><td>0.601</td><td><strong>0.528</strong></td><td><strong>0.528</strong></td></tr></tbody></table>

## V. CONCLUSIONS

This work presented BALO, a novel point-to-plane lidar odometry algorithm. The method exploits a simple but effective planes extraction, then sampled with a new technique based on the spherical projection of the normals. Additionally, a novel weighting policy has been introduced to mix the point-to-point and the point-to-plane distances obtained in the matching process. The pose estimation is performed by Balanced ICP and used to maintain the reference local map. The proposed method is evaluated on synthetic data from CARLA Simulator and on real data from the KITTI dataset. The results show that the proposed method outperforms the state-of-the-art point-to-plane methods and it is equivalent to the best point-to-point approaches across different real scenarios. Furthermore, on synthetic data, the proposed method showed higher robustness to noise and accuracy, proving the potential superiority of point-to-plane correspondences over

point-to-point ones, as expected from the theoretical point of view. Future work will address a deeper analysis and modeling of the environment to further improve the results obtained on the real data and the optimization of the method for a real-time application.

## ACKNOWLEDGMENTS

This work has been supported by EDSTIC d'Université Côte d'Azur. This research is also part of the SAMURAI project, funded by the French National Research Agency (ANR-22-CE33-0014) and of the AISENSE international team funded by Inria.

## REFERENCES

[1] P. Besl and N. D. McKay, "A method for registration of 3-d shapes," *IEEE Transactions on Pattern Analysis and Machine Intelligence*, 1992.

[2] I. Vizzo, T. Guadagnino, B. Mersch, L. Wiesmann, J. Behley, and C. Stachniss, "Kiss-icp: In defense of point-to-point icp - simple, accurate, and robust registration if done the right way," *IEEE Robotics and Automation Letters*, 2023.

[3] P. Dellenbach, J.-E. Deschaud, B. Jacquet, and F. Goulette, "Ct-icp: Real-time elastic lidar odometry with loop closure," in *International Conference on Robotics and Automation*, 2022.

[4] X. Zheng and J. Zhu, "Traj-lo: In defense of lidar-only odometry using an effective continuous-time trajectory," *IEEE Robotics and Automation Letters*, 2024.

[5] J. Zhang and S. Singh, "Loam : Lidar odometry and mapping in real-time," *Robotics: Science and Systems Conference*, 2014.

[6] H. Wang, C. Wang, C. Chen, and L. Xie, "F-loam : Fast lidar odometry and mapping," in *2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2020.

[7] T. Shan and B. Englot, "Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain," in *IEEE/RSJ International Conference on Intelligent Robots and Systems*, 2018.

[8] S. Ferrari, L. D. Giammarino, L. Brizi, and G. Grisetti, "Mad-icp: It is all about matching data – robust and informed lidar odometry," *IEEE Robotics and Automation Letters*, 2024.

[9] J.-E. Deschaud, "IMLS-SLAM: scan-to-model matching based on 3D data," in *IEEE International Conference on Robotics and Automation (ICRA)*, 2018.

[10] M. Yokozuka, K. Koide, S. Oishi, and A. Banno, "Litamin2: Ultra light lidar-based slam using geometric approximation applied with kl-divergence," in *IEEE International Conference on Robotics and Automation*, 2021.

[11] Y. Pan, P. Xiao, Y. He, Z. Shao, and Z. Li, "Mulls: Versatile lidar slam via multi-metric linear least square," in *IEEE International Conference on Robotics and Automation*, 2021.

[12] X. Chen, A. Milioto, E. Palazzolo, P. Giguère, J. Behley, and C. Stachniss, "Suma++: Efficient lidar-based semantic slam," in *IEEE/RSJ International Conference on Intelligent Robots and Systems*, 2019.

[13] Y. Xu, Z. Huang, K.-Y. Lin, X. Zhu, J. Shi, H. Bao, G. Zhang, and H. Li, "Selfvoxelo: Self-supervised lidar odometry with voxel-based deep neural networks," *Conference on Robot Learning*, 2020.

[14] Q. Li, S. Chen, C. Wang, X. Li, C. Wen, M. Cheng, and J. Li, "Lo-net: Deep real-time lidar odometry," *2019 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR)*, 2019.

[15] D. Lee, H. Lim, and S. Han, "GenZ-ICP: Generalizable and Degeneracy-Robust LiDAR Odometry Using an Adaptive Weighting," *IEEE Robotics and Automation Letters (RA-L)*, 2025.

[16] M. Azzini, E. Malis, and P. Martinet, "Balanced icp for precise lidar odometry from non bilateral correspondences," in *2024 IEEE Intelligent Vehicles Symposium (IV)*, 2024.

[17] E. Malis, T. Hamel, R. Mahony, and P. Morin, *Estimation of Homography Dynamics on the Special Linear Group*, 2010.

[18] A. Dosovitskiy, G. Ros, F. Codevilla, A. Lopez, and V. Koltun, "CARLA: An open urban driving simulator," 2017.

[19] A. Geiger, P. Lenz, and R. Urtasun, "Are we ready for autonomous driving? the kitti vision benchmark suite," in *IEEE Conference on Computer Vision and Pattern Recognition*, 2012.