Exact Point Cloud Downsamplingfor Fast and Accurate Global Trajectory Optimization
Kenji Koide1 , Shuji Oishi1 , Masashi Yokozuka1 , and Atsuhiko Banno1

I. I NTRODUCTION
Global trajectory optimization is a crucial step for localization and mapping systems. Because it is unavoidable that
trajectory errors accumulate in online odometry estimationthat performs real-time optimization using local observations,
it is necessary to correct estimation drift by considering the
global consistency of the map.
Global registration error minimization is one of the mostaccurate approaches to global trajectory optimization [1].
Unlike the conventional pose graph optimization that minimizes the errors in the pose space [2], global registration
error minimization directly minimizes the multi-frame point
cloud registration errors over the entire map. This approach
avoids the Gaussian approximation of the relative pose constraint and enables accurate trajectory optimization by jointlyaligning all frames in the map [3]. However, it is known
to be computationally expensive compared to pose graphoptimization, as it requires a re-evaluation of registration
error functions that involve residual computations for many
points [4]. It also consumes a substantial amount of memory
in order to remember the point correspondences between
frames.
To mitigate the processing cost and memory consumptionof global registration error minimization, we propose a point
cloud downsampling algorithm based on an efficient andexact weighted coreset extraction algorithm [5]. A coresetis a subset of an input dataset selected such that the resultof an algorithm on the coreset approximates that on theoriginal set [6]. For example, considering a Hessian matrix
calculated from a Jacobian matrix (H = J T J and J ∈*This work was supported in part by JSPS KAKENHI Grant Number
23K16979 and a project commissioned by the New Energy and Industrial
Technology Development Organization (NEDO).
1 All the authors are with the Department of Information Technology and
Human Factors, the National Institute of Advanced Industrial Science andTechnology, Tsukuba, Ibaraki, Japan, k.koide@aist.go.jp

Quadratic

Target points
Registration
error function
Source points

Exact

=

Abstract— This paper presents a point cloud downsamplingalgorithm for fast and accurate trajectory optimization basedon global registration error minimization. The proposed algorithm selects a weighted subset of residuals of the input point
cloud such that the subset yields exactly the same quadratic
point cloud registration error function as that of the original
point cloud at the evaluation point. This method accurately
approximates the original registration error function with onlya small subset of input points (29 residuals at a minimum).
Experimental results using the KITTI dataset demonstratethat the proposed algorithm significantly reduces processing
time (by 87%) and memory consumption (by 99%) for global
registration error minimization while retaining accuracy.

D2D distance
Exact sampling

Source subset
Approximatedquadratic

Fig. 1: The proposed algorithm extracts a weighted subset
of input source points such that the subset yields the same
quadratic registration error function as that of the original
points at the evaluation point.

Fig. 2: Dense factor graph for global registration error
minimization. The proposed algorithm reduces memory consumption by 99% and processing time by 87% for the
optimization of the factor graph.
RN ×D ), a weighted coreset S ∈ RM ×D approximates theoriginal Hessian matrix with a subset of the Jacobian matrix(S T S ≈ H and S ⊂ J ). In this work, we employ a novel
coreset extraction algorithm, which finds a coreset to exactly
represent the original Hessian matrix in a time linear to thenumber of points [5].
Using this algorithm, we find a coreset of the residualsof the input point cloud such that it exactly reconstructs theoriginal quadratic registration error function at the evaluation
point (see Fig. 1). The proposed method needs only 29
residuals at a minimum to compose an exact coreset for aquadratic error function with six-dimensional input. It is alsoable to find a larger coreset with M residuals (e.g., M=256) toenhance the nonlinearity approximation accuracy. Note that
because one three-dimensional point yields three residuals
for point cloud registration, the computation of 29 residuals

entails a processing cost approximately equal to that for 10
points.
For global trajectory optimization, we construct an extremely dense global registration error minimization factor
graph, as shown in Fig. 2. The graph contains 4,540 posevariables and 585,417 registration error factors. We evaluate
the residuals of approximately 10,000 points for each factor
at every optimization iteration. The optimization requires approximately 22 GB of memory and 13 hours on a CPU with
128 threads. The proposed algorithm drastically decreases
the number of residuals to be computed and reduces memory
consumption and processing time to approximately 0.25
GB and 1.7 hours, respectively, while retaining estimation
accuracy.
The main contributions of this work can be summarized
as follows:• We extend the exact coreset extraction algorithm in [5]so that it can extract coresets with an arbitrary numberof residuals. We also made several modifications toimprove the processing speed of the algorithm.
• Based on the extended exact coreset extraction algorithm, we propose a point cloud downsampling algorithm that exactly recovers the original quadratic
registration error function at the evaluation point. It also
shows good approximation accuracy for the nonlinearityof the original function.
• We release the code of the proposed coreset extraction
algorithm as open source.
II. R ELATED W ORK
A. Pose Graph Optimization
Pose graph optimization is the gold standard for global
trajectory optimization for range-based SLAM [2]. It models
the relative pose between frames estimated by scan matchingas a Gaussian distribution and optimizes the sensor poses via
maximum a posteriori estimation in the pose space. To model
the Gaussian relative pose constraint, an explicit estimateof the representative relative pose value (i.e., mean) and
its uncertainty (i.e., covariance) is required. However, scanmatching becomes unreliable when two frames have onlya small overlap, making it difficult to obtain an accurate
relative pose estimate, which, in turn, makes it difficult toconstrain the relative pose between small overlapping frameswith pose graph optimization [3]. The uncertainty estimationof a scan matching result is also difficult in practice [7],and many works use inappropriate covariance matrices toconstruct the relative pose constraints (e.g., using constantcovariance matrices [8] or a simple weighting scheme [9]).
These difficulties in the modeling of the relative pose constraint lead to inaccurate estimation results.
B. Bundle Adjustment
Bundle adjustment (BA) is another approach to global
trajectory optimization that simultaneously optimizes sensor
poses and environment parameters. For range-based SLAM,
BA is often formulated as the simultaneous estimation ofpose variables and plane or edge environment parameters[10]. Because the plane and edge parameters can be estimated from sensor poses and point coordinates, they canbe eliminated from the estimation variables, turning the BA
problem into a direct eigenvalue minimization of the accumulated points [11]. While this approach ensures a consistentmapping result, it can suffer from high computation cost anda small convergence basin due to the non-least-squares error
function. To mitigate these issues, it is necessary to combine
range-based BA with hierarchical pose graph optimization[12].
C. Global Registration Error Minimization
Global registration error minimization is an approach todirectly minimize multi-frame registration errors over theentire map [1]. Because it avoids the Gaussian approximationof pose graph optimization, it enables the construction of
accurate relative pose constraints between point clouds witha very small overlap. Furthermore, it naturally propagates
the per-point uncertainty of input data to the pose uncertainty
via re-linearization of point cloud registration errors. While itshows a substantially better accuracy compared to pose graphoptimization, it is computationally expensive because it reevaluates point cloud registration errors between all point
cloud pairs at every optimization iteration.
To mitigate the computation cost of global registration
error minimization, one may consider reducing the numberof input points by, for example, random sampling [4]. However, this may change the shape of the objective function,
depending on the selected points, and can negatively andsubstantially affect the optimization result. Although there
is another approach to speeding up global registration error
minimization with GPU-acceleration, the computation costis still high, and it requires submap-level registration [3].
D. Coreset Extraction
Geometric data summarization is an essential tool for
handling big data in computational geometry, and coresets
are one of the most important classes of summarization
methods. A coreset is a subset of an input dataset selected
such that the result of an algorithm on it approximates theresult on the original set [6]. When a coreset produces thesame output of an algorithm as that of the original set, it
is called exact. For example, Caratheodory’s theorem statesthat every point in a convex hull of points in RD can be
represented by a weighted sum of at most D + 1 points [13].
This means there always exists an exact coreset with only
D + 1 points to represent a point in a convex hull (a.k.a., a
Caratheodory set).
While Caratheodory provided an algorithm to find such a
coreset, the time required was O(N 2 D2 ), rendering it impractical for large-scale problems [13]. Although there wereseveral subsequent works proposing efficient approximating
algorithms to find a Caratheodory set, an exact algorithmwith a linear time complexity had, until recently, been
unavailable. However, in 2019, an epoch-making algorithm
was proposed by Maalouf et al., one that finds an exact
Caratheodory set in time linear to the number of input datapoints [5]. The method was applied to find an exact coreset
to reconstruct a Hessian matrix using only a small subset of
the input data for accelerating least squares optimization.
Our aim in this work is to introduce this novel data summarization technique to drastically reduce the computation
cost of point cloud registration while retaining the accuracyof the original set by extracting an exact coreset of input
points. To the best of our knowledge, this is the first work that
introduces this type of exact data summarization techniquein the context of point cloud registration.
III. M ETHODOLOGY
Given a sequence of point clouds Pi = {p1 , · · · , pN } andan initial guess of their poses T̆i , we estimate refined sensor
poses Ti by minimizing the registration errors between all
overlapping point clouds. The registration error function
f REG is defined as the sum of least square errors between
corresponding points:
X
f REG (Pi , Pj , Ti , Tj ) =∥f DIST (p′k , pk , Ti , Tj )∥2 , (1)pk ∈Pj(2)where p′k ∈ Pi is the corresponding point of pk ∈ Pj , f DIST
is a distance function between points that returns a residual
vector ek ∈ R3 , and e = [eT1 , eT2 , · · · ]T is a stack of ek .
In Gauss-Newton optimization, the error function f REG islinearized at the current estimate T̆ = (T̆i , T̆j ) and modeledin the quadratic formf REG (Pi , Pj , T̆ ⊞ ∆x) ≈ ∆xT H∆x + 2bT ∆x + c, (3)where c = f REG (Pi , Pj , T̆ ) is a constant, J = ∂e/∂T is theJacobian of residuals evaluated at T̆ , and b = J T e and H =J T J are the information vector and matrix, respectively.
Because the linearization of f REG involves residual computation for all input points in Pj , it is computationallyexpensive. It also consumes a substantial amount of memory
to remember the corresponding points for each point cloudpair.
To mitigate the memory consumption and processing costof global registration error minimization, we introduce adownsampling function that takes as input the error function,
point clouds, and current estimate of sensor poses, andoutputs a weighted subset of residuals:
f DOWN (f REG , Pi , Pj , T̆i , T̆j ) = (wij , gij ),b = b̃,c = c̃,(5)whereẽ = gij (e) ,˜TH̃ = J Wij J˜,∂ ẽ, Wij = diag(wij ),
J˜ =
∂T
T
b̃ = J˜ Wij ẽ, c̃ = ẽT Wij ẽ.

(6)
(7)

For simplicity and efficiency, we assume that the initial
guess of sensor poses is reasonably accurate and use thepoint correspondences found at the initial state during optimization.
B. Registration Error Function

A. Problem Setting= eT e,H = H̃,(4)

where gij (e) = ẽ is a function to select M residuals frome (i.e., ẽ ⊂ e and ẽ ∈ RM ), and wij ∈ RM is the weights
for the selected residuals. During optimization, we use thesubset of residuals ẽ instead of the original set to re-linearize
the nonlinear error function f REG .
To accurately approximate the original error function with
only a small subset of the input data, we select a subset thatwill yield exactly the same quadratic error function as theoriginal function at the evaluation point x̆:The proposed downsampling algorithm only requires theerror function to be a first-order differentiable least squares
function. It thus can be applied to most of the common
registration error functions (e.g., point-to-point [14] andpoint-to-plane [15] distance functions). In this work, we use
the generalized ICP (GICP) error function [16] based on the
distribution-to-distribution distance.
GICP models each point as a Gaussian distribution pk =(µk , Ck ) representing the local surface shape around thepoint and calculates the distribution-to-distribution distance
between a point pk ∈ Pj and its corresponding pointp′k ∈ Pi as follows:f GICP (p′k , pk , Ti , Tj ) = dT Ωd,d = µ′k − Tij µk ,

Ω = Ck′ + Tij Ck TijT ,

(8)
(9)

where Tij = Ti−1 Tj is the relative pose between Ti and Tj .
For ease of the following downsampling process, we
decompose Ω = ΦΦT and use the following function f DIST
that gives the same least square errors as f GICP :f DIST (p′k , pk , Ti , Tj ) = ΦT d.

(10)

Because Ω is a symmetric positive definite matrix, thedecomposition Ω = ΦΦT can efficiently be found via
Cholesky decomposition. This modification does not change
the final objective function and thus does not affect the
optimization process.
C. Fast Coreset Extraction
To extract a weighted subset of input residuals that yields
the same quadratic error function as that of the original
set, we use an extended version of the efficient coreset set
extraction algorithm in [5].
Caratheodory’s theorem [13] states that every point in aconvex hull of points in RD can be represented as a weighted
sum of a subset of at most D + 1 points. Given a Jacobian
1×Dkmatrix J = [aT1 , · · · , aTN ]T , where ak = ∂e∂T ∈ R
represents derivatives of a residual
Hessian matrix
PNek , the
H = J T J can be given byaTk ak . We consider
k=1
2
flattened vectors
h ∈ RDof aTk ak and calculate their
PN k
PN
h
flatten(aT a )kmean µh = k=1= k=1 N k k . Because µh isNalways in the convex hull of hk , we can find a minimum
exact coreset (a.k.a., the Caratheodory set) of hk to representµh and scale it to recover the original Hessian matrix fromat most D2 + 1 row vectors in the original Jacobian matrix.

Algorithm 1 Caratheodory(P , u, M )Algorithm 3 Fast-Caratheodory-Quadratic(e, J , M )Input:
A set of points P = {p1 , · · · , pN } in RL
A weighting function u : P → [0, ∞]Target output size M
Output:A
P subset of inputPpoints and weighting function (S, w) s.t.
(u(pi ) · pi ) =(w(si ) · si ) and |S| = M
1: if N ≤ M then2:
return (P , u)
3: for pi ∈ {p2 , · · · , pN } do
4:ai ← pi − p1
5: A ← [a2 | · · · |aN ]▷ A ∈ RL×(N −1)
T
6: ComputePv = [v2 , · · · , vN ] ̸= 0 s.t. Av = 0
N
7: v1 ← − i=2 vi
8: α ← min{u(pi )/vi |i ∈ {1, · · · , N } and vi > 0}
9: w(pi ) ← u(pi ) − αvi for every i ∈ {1, · · · , N }10: S ← {pi |w(pi ) > 0 and i ∈ {1, · · · , N }}11: if |S| > M then12:
return C ARATHEODORY(S, w, M )13: return (S, w)

Input:A residual vector e ∈ RN = [e1 , · · · , eN ]TJacobian matrix J ∈ RN ×D = [a1 , · · · , aN ]TTarget output size M
Output:A function g to select a subset of e
Weights w for the selected subset
g and w satisfy J˜T W J˜ = J T J , J˜T W ẽ = J T e, ẽT W ẽ =eT e, where ẽ = g(e), J˜ = g(J ), and W = diag(w)
1: for i ∈ {1, · · · , N } do2:hi ← upper triangular elements of aTi ai
3:bi ← aTi ei
4:ci ← e2i5:pi ← [hTi , bTi , ci ]T6:u(pi ) ← 1/N
7: P ← {p1 , · · · , pN }8: (C, w) ← FAST-C ARATHEODORY(P , u, K, M )9: g ← A function to select entries of e exist in C
10: w ← [w(ci )|ci ∈ g(e)]11: return (g, w)

Algorithm 2 Fast-Caratheodory(P , u, K, M )Caratheodory set of the cluster means and eliminates clusters
that are not selected to be in the Caratheodory set. It recovers
the set of input data from the clusters in the extracted
Caratheodory set and repeats this process until the numberof data points becomes less than or equal to a target output
size. The clustering strategy significantly improves the timecomplexity and can find a Caratheodory set in time linear
to the number of input data points O(N L). Algorithm 3
takes as input a residual vector e and its Jacobian matrix
J , computes flattened vectors of H, b, c, and passes them to
Algorithm 2 to obtain a weighted subset of input residuals
to reconstruct the original quadratic function.
The modifications we made to fit the algorithms in [5] toour problem can be summarized as follows:

Input:
A set of points P = {p1 , · · · , pN } in RL
A weighting function u : P → [0, ∞]Number of clusters K >= L + 2
Target output size M
Output:A subset of inputPpoints and weighting function (C, w) s.t.
P(u(pi ) · pi ) =(w(ci ) · ci )1: if |N ≤ M | then2:
return (P , u)
3: {P1 , · · · , PK } = equally divided disjoint subsets of P
4: for i ∈ {1,P· · · , K} dou(p)·p▷ Compute mean of the cluster5:µi ← Pp∈Pi u(p)
p∈P
i
P
6:
u′ (µi ) ← p∈Pi u(p)7: (µ̃, w̃) ← C ARATHEODORY({µ1 , · · · , µK }, u′ )8: C ← ∪µi ∈µ̃ Pi▷ Recover points from selected clusters
9: for µi ∈ µ̃ and p ∈ Pi do
10:w(p) ← Pw̃(µi )u(p)
u(p)
p∈Pi

11: return FAST-C ARATHEODORY(C, w, K, M )Algorithms 1, 2, and 3 describe the proposed coreset
extraction algorithm extending the algorithms in [13], [5].
Due to space limitations, we provide only a brief explanationof the algorithms and highlight the modifications we made.
For details of the original algorithms, we refer the reader to[5]. The code for the proposed algorithm is available online
so that the reader can confirm the details 1 .
Algorithm 1 is Caratheodory’s classic algorithm for finding a minimum exact coreset from a weighted input set [13].
This algorithm finds and eliminates redundant data points one
by one. Because the time required to execute the algorithmis O(N 2 L2 ), it cannot be directly applied to large datasets.
To reduce the time complexity, Algorithm 2 divides the input
set into K disjoint subsets (e.g., K = 64) and computes themeans of the clusters. It then applies Algorithm 1 to extract a
1 https://github.com/koide3/caratheodory2

1) While the original algorithm selects only a minimum
number of data points to compose a Caratheodory set,
we modified Algorithms 1 and 2 so that they canchange the target number of data points M to control the trade-off between approximation accuracy andcomputation speed. By increasing M , the algorithm
extracts more data that are redundant to compose a
Caratheodory set but help approximate the nonlinearityof the original registration error function. Note thatthe proposed algorithm does not extract exactly M
points but rather extracts points in the range [max(M −K, 29), M ]. While we can easily modify Algorithm 2
to select exactly M points, we avoided doing so tosave processing time.
2) In the original implementation of Algorithm 1, most of
the computation time was taken by SVD to find v ̸= 0such that Av = 0. To improve the processing speed,
we use an efficient LU decomposition method to findthe nullspace of A instead of SVD.
3) While [5] recovers only the Hessian matrix H, werecover b and c in addition to H to approximate thequadratic function in Eq. 3. Although this modification
increases the dimension of the input vectors from D2

Algorithm 4 Exact-Downsampling(Pi , Pj , T̆i , T̆j , f REG , M )Input:
Point clouds Pi and Pj
Poses T̆i and T̆j to evaluate errors between Pi and Pj
Registration error function f REG
Target output size M
Output:A function g to select a subset of residuals of input points
A weight vector w
1: Shuffle points in Pj
2: eT e ← f REG (Pi , Pj , xi , xj )3: J ← ∂e/∂Tj
4: (g, w) ← FAST-C ARATHEODORY-Q UADRATIC(e, J , M )5: return (g, w)

During Gauss-Newton optimization, we iteratively relinearize the error function f REG with the extracted subset
to obtain a quadratic error function.
Fig. 3 (c) and (d) show examples of downsampling results.
The colors of the points indicate the selected axes of thesampled points (R=X, G=Y, B=Z). Because the proposed
algorithm works on a per-residual basis, it happens that only
one or two axes of a point are selected (Fig. 3 (c)). Because
the algorithm tends to drop contiguous residuals, most of
the selected points have all selected axes when the numberof target residuals is sufficiently large (Fig. 3 (d)). Both thesampled points with M = 29 and M = 512 shown in Fig.
3 (c) and (d) exactly recover the quadratic registration error
function between the original input point clouds shown in
Fig. 3 (a) and (b).
IV. E XPERIMENTS
A. Numerical Validation(a) Target points(b) Source points (N = 10,000)(c) Sampled residuals (M = 29) (d) Sampled residuals (M = 512)

Fig. 3: Example of downsampling results. Source points(b) and sampled points with different target numbers ofresiduals (M=29 (c), and M=512 (d)) all yield the same
quadratic registration error function for the target points
at the evaluation point. The colors of the sampled points
represent the selected axes of residuals (R=X, G=Y, B=Z).
to D2 + D + 1, we can omit the non-upper-triangularelements of H, since it is symmetric. As a result, fora function with six-dimensional input, the input vector
dimension becomes 21+6+1 = 28, and we need only
28 + 1 = 29 residuals to exactly recover the original
quadratic function at a minimum.
As we will show in Sec. IV-A, modifications 2 and 3 reduce
processing time by a factor of 10. Note that because theregistration error function is symmetric for Ti and Tj , thesubset extracted for Tj exactly reconstructs Hii , Hij , andbi as well.
D. Point Cloud Downsampling
The proposed exact point cloud downsampling algorithm
is summarized as in Algorithm 4. Because Algorithm 2
reduces the dataset size by dropping contiguous items, wefirst shuffle the input points to avoid bias in the sampling
result. We then evaluate the residuals e and Jacobian J of
the registration error function at T̆i and T̆j . We input e and
J to Algorithm 3 and obtain a subset of the residuals andweights.

We first verified the proposed algorithm through numericalvalidation. We began by randomly generating J ∈ RN ×6
and e ∈ RN (N = 30,000 ≈ 10,000 points) and applying the proposed downsampling algorithm to obtain aweighted subset of the residuals (g, w). We then calculated
the quadratic function approximation error as max(∥J T J −J˜T W J˜∥, ∥J T e − J˜T W ẽ∥, eT e − ẽT W ẽ), where J˜ =g(J ), ẽ = g(e), and W = diag(w). We repeated this numerical validation 100 times 2 .
To evaluate the speed gain of the modifications proposed
in Sec. III-C, we ran the algorithm with three configurations:
1) Naive implementation of [5] with the expanded input
dimension D2 + D + 1 = 43
2) Naive implementation of [5] with the compact input
dimension without the non-upper-triangular elementsof the Hessian ((D + 1) × (D/2) + D + 1 = 28)
3) Using the compact input dimension (28) and LU
decomposition instead of SVD
All three configurations produced approximation errors
below 10−10 for all trials, showing the validity of the
proposed algorithm. Configuration 1 took approximately 125
ms for each trial. With configuration 2, the processing timedecreased to 76 ms by using the compact input representation. Finally, with configuration 3, the processing timefurther decreased to 7 ms, or roughly 5.6% of the time for
configuration 1.
We then evaluated the change in processing time forthe proposed algorithm when the target output size varies
from 29 to 1024. Again, all the selected subsets showedreconstruction errors below 10−10 for all the settings. Table I
summarizes the average processing times with several target
numbers of residuals M . As shown, when the target numberof residuals was increased, the processing time also grew,albeit gradually. While all the settings exactly reconstructed
the original quadratic error function at the evaluation point,
as we will show in the following Sec. IV-B, a larger number
2 The code for reproducing this experiment is available at the GitHub
repository.

TABLE I: Processing time for the exact sampling algorithm
Number of residuals (M)Processing time [ms]

29
6.87

64
13.39

128
19.34

256
25.03

512
29.99

1024
34.19

TABLE II: Hessian approximation accuracy evaluation result≈ 30

MethodNum. of points / residuals

Normalized KLD

Random sampling

10 points ≈ 30 residuals
64 points ≈ 192 residuals
256 points ≈ 768 residuals
1024 points ≈ 3072 residuals
10149 points (original points)

0.996 ± 0.026
0.437 ± 0.213
0.107 ± 0.062
0.024 ± 0.013
0.000 ± 0.000

Exact sampling

29 - 3072 residuals

0.000 ± 0.000

of residuals leads to better approximation accuracy underpose displacements. Thus, setting the target number of residuals parameter involves a trade-off between approximation
accuracy and processing speed.
B. Nonlinearity Approximation Accuracy Analysis
We next compared the approximation accuracy of the
proposed exact sampling with that of the commonly used
random sampling approach. Here, we took two consecutive
frames in the sequence 00 of the KITTI dataset [17], asshown in Fig. 3. We aligned the frames by using standard
GICP scan matching and then sampled subsets of the input
points and residuals using random sampling and the proposed
exact sampling method while changing the target output size.
We computed the Hessian matrices of f REG by using thesampled subsets and compared them with that computed
using the original input points. To compare Hessian matrices,
we used the normalized KLD metric defined as follows:


|H|
1
log(
) + tr(H −1 H̃) , (11)KLD(H, H̃) =2|H̃|NormedKLD(H, H̃) = 1 − exp(−KLD(H, H̃)). (12)We repeated the evaluation 100 times while shuffling theorder of the points.
Table II summarizes the normalized KLDs for the random
and exact sampling results. The random sampling results
showed large errors when the number of points was small.
In particular, when the number of points was set to 10,
we observed that the Hessian matrix computed from thesampled points sometimes degenerated. As the number ofpoints increased, the approximation accuracy of the random
sampling results improved. When and only when the target
number of points was set to the same number as the numberof input points, the approximation error became zero. This
result suggests that random sampling would not be a good
choice for sampling point cloud registration errors, since it
substantially changes the objective function shape when thenumber of sampling points is small. On the other hand,
as verified in Sec. IV-A, the proposed algorithm exactly
recovered the original Hessian matrix with a small numberof residuals and thus the normalized KLD was always zeroat the evaluation point.
We then evaluated the changes in function approximation
accuracy of the two sampling methods under pose displace-(a) Random sampling(b) Exact sampling

Fig. 4: Errors of the displacement vector ∆x = H −1 b ofrandom and exact sampling results under rotation noise.
ments. We applied random rotation noise and re-evaluated theregistration error function using the sampled subsets to obtain
H and b. We then computed the displacement vector claimedby the linearized system (∆x = H −1 b) and compared itwith that computed using the original points.
Fig. 4 summarizes the displacement vector errors. Because
the residuals sampled by the proposed algorithm exactly
recovered the original quadratic error function, the proposed
algorithm showed zero displacement vector errors with all
the settings when the noise was zero. As the rotation noise
became larger and the current estimate separated further fromthe evaluation point, the displacement vector errors became
gradually worse due to the nonlinearity of the registration
error function. However, we can see that the results of the
proposed exact sampling show substantially smaller errors
compared to the random sampling results with equivalent
output size settings.
C. Application to Global Trajectory Optimization
We applied the proposed exact downsampling to global
trajectory optimization on the longest four sequences (00,02, 05, 08) in the KITTI dataset [17] in order to compare
its trajectory estimation accuracy and associated processing
time with those of the conventional pose graph optimization.
Evaluation protocol: To obtain an initial guess of sensor
poses, we applied GICP scan matching between consecutive
frames (i.e., odometry estimation). Meanwhile, we ran ScanContext [18] to detect loop candidates and validated themvia GICP scan matching. We constructed a pose graph withrelative pose constraints between the consecutive frames andloop pairs and performed optimization using the LevenbergMarquardt optimizer in GTSAM [19]. The objective functionof pose graph optimization is defined as follows:X
f PGO (X ) =∥f RP (Ti , Tj , T̂ij )∥2 , (13)(Ti ,Tj ,T̂ij )

f

RP


(Ti , Tj , T̂ij ) = ρ dTij Hij dij ,
dij = log T̂ij−1 Ti−1 Tj ,

(14)
(15)

TABLE III: ATEs [m] of pose graph optimization and global registration error minimization results
MethodSequence
05

00

02

Pose graph optimization (Identity)
Pose graph optimization (Hessian)Pose graph optimization (Hessian + Dense)

1.6257 ± 0.7529
1.3777 ± 0.6051
1.1846 ± 0.5625

23.9856 ± 8.2752
9.3406 ± 3.1490
9.3393 ± 3.1486

1.5149 ± 0.6760
1.6249 ± 0.8489
1.6240 ± 0.8488

9.3636 ± 3.2890
5.0532 ± 2.5991
5.0520 ± 2.5993

Registration error minimization + Exact sampling (M=29)

0.9553 ± 0.4650

8.9679 ± 3.0856

0.2917 ± 0.1060

4.4394 ± 2.5294

where X is the set of sensor poses, T̂ij is the relative posemeasurement given by scan matching, and ρ is Cauchy’s
robust kernel. For Hij , we used two settings: 1) setting
the identity matrix I6×6 to Hij , and 2) setting the Hessian
matrix at the last iteration of GICP scan matching to Hij .
After performing pose graph optimization, we found all
overlapping point cloud pairs by voxel-based overlap assessment [3] and constructed a densely connected graph withfactors between all detected overlapping pairs, as shown in
Fig. 2. With this dense graph structure, we again optimized
the sensor poses using pose graph optimization and global
registration error minimization. The objective function for
global registration error minimization is defined asX
f GR (X ) =∥f REG (Pi , Pj , Ti , Tj )∥2 ,(16)(Ti ,Tj )∈X O

where X O is the set of point cloud pairs with an overlap.
For pose graph optimization, we applied GICP scan matching for all frame pairs and created relative pose constraints
from the scan matching results. For global registration error
minimization, we applied the exact sampling for all frame
pairs with the target number of residuals M = 29 and optimized the sensor poses by minimizing the global registration
errors computed with the sampled residuals. For the sequence00, we also ran the same algorithm with M=256 and M=1024
and the original input points to determine the effect thatchanging the number of samples has on processing time andestimation accuracy.
Comparison with pose graph optimization: Table III
summarizes the absolute trajectory errors (ATEs) [20] forpose graph optimization and global registration error minimization. We used the evo toolkit 3 to measure the ATEs.
For most of the sequences, pose graph optimization withidentity Hessian matrices produced much worse results thanwhen Hessian matrices composed of the scan matching
results were used. This suggests the importance of appropriately modeling the relative pose constraints for better
estimation results.
Although pose graph optimization on the dense factor
graph showed better ATEs than those for the sparse graph,the accuracy gain was not very significant. Fig. 5 shows
the dense pose graph optimization graph. The colors of thefactors indicate the magnitudes of the relative pose errors. We
can see that the factors between frames at a distance tend toshow large errors because the GICP scan matching failed on
small overlapping frames. This suggests that increasing the3 https://github.com/MichaelGrupp/evo

08

Fig. 5: Dense pose graph optimization result. Color indicatesthe magnitude of the relative pose errors of each factor(Green: small error, Red: large error).
TABLE IV: ATEs of global registration error minimization
with different sampling settings for the sequence 00
MethodATE [m]Optimization time [h]Exact sampling (M=29)
Exact sampling (M=256)
Exact sampling (M=1024)

0.9553 ± 0.4650
0.9549 ± 0.4646
0.9549 ± 0.4647

1.67
1.88
2.36

Original points (10,000 points)

0.9549 ± 0.4647

13.21

number of factors does not always improve the estimation
accuracy of pose graph optimization.
The global registration error minimization approach
greatly improved the ATEs of all the sequences comparedto those of pose graph optimization. Because this approachdirectly minimizes registration errors over the entire map
while avoiding the Gaussian approximation of the relativepose constraint, it can accurately constrain the relative pose
between frames with small overlap.
Effect of the exact sampling: Table IV summarizes
the ATEs of global registration error minimization withdifferent sampling settings. While the ATEs improved as
the number of residuals increased, the smallest number ofresiduals M=29 showed an ATE very close to that of theoriginal points. Because we started the optimization froma sufficiently good initial guess, the approximation errors
were sufficiently small for the small displacements with theminimum number of residuals (M=29) as shown in Sec. IVB. We consider that, if the initial guess had larger errors, theresults from the smallest sampling setting might deteriorate
and the other sampling settings with more residual samples
would show better results.
Memory consumption: Table V summarizes the memory
consumption of global registration error minimization foreach sampling setting. For the case in which the originalTABLE V: Memory consumption
Sampling method

Num. residuals

Memory consumption [GB]Correspondences
Sampled residuals

Exact sampling

29
256
1024

0.11
0.43
1.52

0.14
1.15
4.87

All residuals∼30,000

25.13

0.00

Fig. 7: Optimization time breakdown.
ATE

R EFERENCES

Fig. 6: Global registration error and ATE change during
global registration error minimization.
points are used, where we need to remember the point
correspondences for each factor, approximately 25 GB ofmemory was required. With exact sampling, where, in addition to remembering the point correspondences, we needto remember the selected residual samples, total memory
consumption was substantially smaller. For the setting M=29,
only 0.11 GB was needed for the point correspondences andonly 0.14 GB was needed for the sampled residuals.
Processing time: Fig. 6 shows the decreasing patternof both the global registration error and the ATE during
optimization. With this large-scale factor graph optimization,
more than several hundred iterations were needed before
convergence. Fig. 7 shows a breakdown of the optimization
times for the different sampling settings. With the original
points, the optimization took approximately 13.2 hours on
a CPU with 128 threads. We can see that the linearizationand cost evaluation of the registration error function consumed a majority of the optimization time. Although theproposed exact sampling introduced additional processing
time for downsampling of approximately 130 s, it drasticallydecreased the processing time for linearization and cost
evaluation and reduced the total optimization time (1.67
hours with M=29, 2.36 hours with M=1024). Note that while
we used the multi-threaded MULTIFRONTAL CHOLESKY
linear solver in GTSAM, we observed that it did not fully
utilize all the available CPU cores. We consider that the total
optimization time can be further improved by using a linear
solver dedicated to multi-threading or GPU computing.
V. C ONCLUSION
This work proposed a point cloud downsampling algorithm based on an algorithm for efficient exact coreset
extraction. Experimental results showed that this approachdrastically reduces the linearization cost of the point cloud
registration error function without sacrificing accuracy. We
plan to apply the proposed algorithm to extremely large cityor nation-scale mapping problems and other types of data
(e.g., dense full BA for visual SLAM).

[1] F. Lu and E. Milios, “Globally consistent range scan alignment forenvironment mapping,” Autonomous robots, vol. 4, pp. 333–349, 1997.
[2] G. Grisetti, R. Kummerle, C. Stachniss, and W. Burgard, “A tutorial
on graph-based SLAM,” IEEE Intelligent Transportation Systems
Magazine, vol. 2, no. 4, pp. 31–43, 2010.
[3] K. Koide, M. Yokozuka, S. Oishi, and A. Banno, “Globally consistent 3d LiDAR mapping with GPU-accelerated GICP matching cost
factors,” IEEE Robotics and Automation Letters, vol. 6, no. 4, pp.
8591–8598, Oct. 2021.
[4] V. Reijgwart, A. Millane, H. Oleynikova, R. Siegwart, C. Cadena, andJ. Nieto, “Voxgraph: Globally consistent, volumetric mapping using
signed distance function submaps,” IEEE Robotics and Automation
Letters, vol. 5, no. 1, pp. 227–234, Jan. 2020.
[5] A. Maalouf, I. Jubran, and D. Feldman, “Fast and accurate least-meansquares solvers,” in Conference on Neural Information Processing
Systems, vol. 32. Curran Associates, Inc., 2019.
[6] J. M. Phillips, “Coresets and sketches,” in Handbook of discrete andcomputational geometry. Chapman and Hall/CRC, 2017, pp. 1269–
1288.
[7] D. Landry, F. Pomerleau, and P. Giguere, “CELLO-3d: Estimating thecovariance of ICP in the real world,” in IEEE International Conferenceon Robotics and Automation. IEEE, May 2019, pp. 8190–8196.
[8] J. Behley and C. Stachniss, “Efficient surfel-based slam using 3d laser
range data in urban environments.” in Robotics: Science and Systems,
vol. 2018, 2018, p. 59.
[9] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus,
“LIO-SAM: Tightly-coupled lidar inertial odometry via smoothing andmapping,” in IEEE/RSJ International Conference on Intelligent Robots
and Systems. IEEE, Oct. 2020, pp. 5135–5142.
[10] D. Wisth, M. Camurri, and M. Fallon, “VILENS: Visual, inertial, lidar,
and leg odometry for all-terrain legged robots,” IEEE Transactions on
Robotics, vol. 39, no. 1, pp. 309–326, Feb. 2023.
[11] Z. Liu and F. Zhang, “BALM: Bundle adjustment for lidar mapping,”
IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3184–3191,
Apr. 2021.
[12] X. Liu, Z. Liu, F. Kong, and F. Zhang, “Large-scale LiDAR consistentmapping using hierarchical LiDAR bundle adjustment,” IEEE Robotics
and Automation Letters, vol. 8, no. 3, pp. 1523–1530, Mar. 2023.
[13] W. Cook and R. Webster, “Caratheodory’s theorem,” Canadian Mathematical Bulletin, vol. 15, no. 2, pp. 293–293, 1972.
[14] D. Chetverikov, D. Svirko, D. Stepanov, and P. Krsek, “The trimmed
iterative closest point algorithm,” in Object recognition supported byuser interaction for service robots. IEEE, 2002, pp. 545–548.
[15] W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “FAST-LIO2: Fast direct
LiDAR-inertial odometry,” IEEE Transactions on Robotics, vol. 38,
no. 4, pp. 2053–2073, Aug. 2022.
[16] A. Segal, D. Haehnel, and S. Thrun, “Generalized-icp.” in Robotics:
science and systems, vol. 2, no. 4. Seattle, WA, 2009, p. 435.
[17] A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, “Vision meets robotics:
The kitti dataset,” The International Journal of Robotics Research,
vol. 32, no. 11, pp. 1231–1237, 2013.
[18] G. Kim and A. Kim, “Scan context: Egocentric spatial descriptor
for place recognition within 3d point cloud map,” in IEEE/RSJ
International Conference on Intelligent Robots and Systems. IEEE,
Oct. 2018, pp. 4802–4809.
[19] F. Dellaert and G. Contributors, “borglab/gtsam,” May 2022. [Online].
Available: https://github.com/borglab/gtsam)[20] Z. Zhang and D. Scaramuzza, “A tutorial on quantitative trajectory
evaluation for visual(-inertial) odometry,” in IEEE/RSJ International
Conference on Intelligent Robots and Systems. IEEE, Oct. 2018, pp.
7244–7251.