# Efficient Second-Order Plane Adjustment

Lipu ZHou
Meituan, Beijing, China
<EMAIL_ADDRESS>
###### Abstract

Planes are generally used in 3D reconstruction for depth sensors, such as
RGB-D cameras and LiDARs. This paper focuses on the problem of estimating the
optimal planes and sensor poses to minimize the point-to-plane distance. The
resulting least-squares problem is referred to as plane adjustment (PA) in the
literature, which is the counterpart of bundle adjustment (BA) in visual
reconstruction. Iterative methods are adopted to solve these least-squares
problems. Typically, Newton’s method is rarely used for a large-scale least-
squares problem, due to the high computational complexity of the Hessian
matrix. Instead, methods using an approximation of the Hessian matrix, such as
the Levenberg-Marquardt (LM) method, are generally adopted. This paper
challenges this ingrained idea. We adopt the Newton’s method to efficiently
solve the PA problem. Specifically, given poses, the optimal planes have
close-form solutions. Thus we can eliminate planes from the cost function,
which significantly reduces the number of variables. Furthermore, as the
optimal planes are functions of poses, this method actually ensures that the
optimal planes for the current estimated poses can be obtained at each
iteration, which benefits the convergence. The difficulty lies in how to
efficiently compute the Hessian matrix and the gradient of the resulting cost.
This paper provides an efficient solution. Empirical evaluation shows that our
algorithm converges significantly faster than the widely used LM algorithm.

##  1 Introduction

Figure 1: We use Gaussian noises to perturb the poses of dataset D in Fig. 3.
The standard deviations for rotation and translation are $3^{\circ}$ and
$0.3m$, respectively. The resulting point cloud (a) is in a mess. Fig. (b)
shows the result from our algorithm. Our algorithm can quickly align the
planes, as shown in Fig. 6.

Planes ubiquitously exist in man-made environments, as demonstrated in Fig. 1.
Thus they are generally used in simultaneous localization and mapping (SLAM)
systems for depth sensors, such as RGB-D cameras [14, 10, 13, 15, 7] and
LiDARs [22, 17, 25, 27, 23]. Just as bundle adjustment (BA) [21, 4, 26, 9] is
important for visual reconstruction [2, 19, 20, 6], jointly optimizing planes
and depth sensor poses, which is called plane adjustment (PA) [25, 27], is
critical for 3D reconstruction using depth sensors. This paper focuses on
efficiently solving the large-scale PA problem.

The BA and PA problems both involve jointly optimizing 3D structures and
sensor poses. As the two problems are similar, it is straightforward to apply
the well-studied solutions for BA to PA, as done in [24, 27]. However, planes
in PA can be eliminated, so that the cost function of the PA problem only
depends on sensor poses, which significantly reduces the number of variables.
This property provides a promising direction to efficiently solve the PA
problem. However, it is difficult to compute the Hessian matrix and the
gradient vector of the resulting cost. Although this problem was studied in
several previous works [11, 17], no efficient solution has been proposed. This
paper seeks to solve this problem.

The main contribution of this paper is an efficient PA solution using Newton’s
method. We derive a closed-form solution for the Hessian matrix and the
gradient vector for the PA problem whose computational complexity is
independent of the number of points on the planes. Our experimental results
show that, in terms of the PA problem, Newton’s method outperforms the widely-
used Levenberg-Marquardt (LM) algorithm [18] with Schur complement trick [21].

##  2 Related Work

The PA problem is closely related to the BA problem. In BA, points and camera
poses are jointly optimized to minimize the reprojection error. Schur
complement [21, 4, 26] or the square root method [8, 9] is generally used to
solve the linear system of the iterative methods. The keypoint is to generate
a reduced camera system (RCS) which only relates to camera poses.

In PA, planes and poses are jointly optimized. Planes are the counterparts of
points in BA. Thus, the well-known solutions for the BA problem can be applied
to the PA problem [24, 25]. In the literature, two cost functions are used to
formulate the PA problem. The first one is the plane-to-plane distance which
measures the difference between two plane parameters [14, 13]. The value of
the plane-to-plane distance is related to the choice of the global coordinate
system, which means the selection of the global coordinate system will affect
the accuracy of the results. The second one is the point-to-plane distance,
whose value is invariant to choice of the global coordinate system. The
solutions of different choices of the global coordinate system are equivalent
up to a rigid transformation. Zhou et al. [24] show that the point-to-plane
distance can converge faster and lead to a more accurate result. But unlike BA
where each 3D point has only one 2D observation at a pose, a plane can
generate many points at a pose as demonstrated in Fig. 2. This means the
point-to-plane distance probably leads to a very large-scale least-squares
problem. Directly adopting the BA solutions is computationally infeasible for
a large-scale PA problem. Zhou et al. [24] propose to use the QR decomposition
to accelerate the computation.

For a general least-squares problem with $M$ variables, the computational
complexity of the Hessian matrix is $O(M^{2})$. Thus, in the computer vision
community, it is ingrained that Newton’s method is infeasible for a large-
scale optimization problem, as calculating the Hessian matrix is
computationally demanding. Instead, Gauss-Newton like iterative methods are
generally adopted. Suppose that $\mathbf{J}$ is the Jacobian matrix of the
residuals. Gauss-Newton like methods actually approximates the Hessian matrix
by $\mathbf{H}\approx\mathbf{J}^{T}\mathbf{J}$. In theory, Newton’s method can
lead to a better quadratic approximation to the original cost function, which
means the Newton’s step probably yields a more accurate result. This in turn
may reduce the number of iterations for convergence.

The PA problem has a special property that the optimal plane parameters are
determined by the poses. That is to say the point-to-plane cost actually only
depends on the poses. This property is attractive, as it significantly reduces
the number of variables, which makes using the Newton’s method possible.
Moreover, in the traditional framework, the correlation between the plane
parameters and the poses are ignored.Thus, after one iteration, there is no
guarantee that the new plane parameters are optimal for the new poses. Using
the property of the PA, it is possible to overcome this drawback, which may
lead to faster convergence. Several previous works seek to exploit this
property of PA. Ferrer [11] considered an algebraic point-to-plane distance
and provided a closed-form gradient for the resulting cost. The algebraic cost
may result in a suboptimal solution [5], and the first-order optimization
generally leads to slow convergence [21]. Liu et al. [17] provided analytic
forms of the Hessian matrix and the gradient of the genuine point-to-plane
cost. Assume that $N$ points are captured from a plane at a pose. The
computational complexity of the Hessian matrix related to these points is
$O(N^{2})$. Since $N$ can be large as shown in Fig. 2, this algorithm is
computationally demanding and infeasible for a large-scale problem.

In summary, the potential benefits of the special property of the PA problem
have not been manifested in previous works. The bottleneck is how to
efficiently compute the Hessian matrix and the gradient vector. This paper
focuses on solving this problem.

Figure 2: A schematic of the PA problem and the planes detected in a LiDAR
scan. Unlike BA where each 3D point only has one observation, many points can
be captured from a plane in PA. Assume that $N$ points are captured from a
plane. The computational complexity of the Hessian matrix related to these
points for BALM [17] is $O(N^{2})$. Thus, this method is infeasible for a
large-scale problem. In contrast, the computational complexity of our
algorithm is independent of $N$.

##  3 Problem Formulation

In this paper we use italic, boldfaced lowercase and boldfaced uppercase
letters to represent scalars, vectors and matrices, respectively.

###  3.1 Notations

Planes and Poses A plane can be represented by a four-dimensional vector
$\bm{\pi}=[\bm{n};d]$. We denote the rotational and translational components
from a depth sensor coordinate system to the global coordinate system as
$\mathbf{R}\in SO(3)$ and $\bm{t}\in\mathbb{R}^{3}$, respectively. To simplify
the notation in the following description, we also use the following two
matrices to represent a pose:

$\mathbf{X}=\begin{bmatrix}\mathbf{R},&\bm{t}\\\ \mathbf{0},&1\end{bmatrix}\in
SE(3)\ \text{and}\ \mathbf{T}=\begin{bmatrix}\mathbf{R},&\bm{t}\end{bmatrix}.$
(1)

As $\mathbf{R}\in SO(3)$, a certain parameterization is usually adopted in the
optimization [21]. In this paper, we use the Cayley-Gibbs-Rodriguez (CGR)
parameterization [12] to represent $\mathbf{R}$

$\mathbf{R}=\frac{{\mathbf{\bar{R}}}}{1+{{\bm{s}}^{T}}\bm{s}},\mathbf{\bar{R}}=\left(1-{{\bm{s}}^{T}}\bm{s}\right){{\mathbf{I}}_{3}}+2{{\left[\bm{s}\right]}_{\times}}+2\bm{s}{{\bm{s}}^{T}},$
(2)

where $\bm{s}=\left[s_{1};s_{2};s_{3}\right]$ is a three-dimensional vector.
We adopt the CGR parameterization as it is a minimal representation for
$\mathbf{R}$. Furthermore, unlike the angle-axis parameterization that is
singular at $\mathbf{I}_{3}$, the CGR parameterization is well-defined at
$\mathbf{I}_{3}$, and equals to $[0;0;0]$ which can accelerate the
computation. We parameterize $\mathbf{T}$ as a six-dimensional vector
$\bm{x}=[\bm{s};\bm{t}]$.

Newton’s method This paper adopts the damped Newton’s method in the
optimization. For a cost function $f(\bm{z})$, the damped Newton’s method
seeks to find its minimizer from an initial point. Assume that $\bm{z}_{n}$ is
the solution at the $n$th iteration. Given the Hessian matrix
$\mathbf{H}_{f}(\bm{z}_{n})$ and the gradient $\bm{g}_{f}(\bm{z}_{n})$ at
$\bm{z}_{n}$, $\bm{z}_{n}$ is updated by
$\bm{z}_{n+1}=\bm{z}_{n}+\varDelta\bm{z}$. Here $\varDelta\bm{z}$ is from

$(\mathbf{H}_{f}(\bm{z}_{n})+\mu\mathbf{I}))\varDelta{z}=-\bm{g}_{f}(\bm{z}_{n}),$
(3)

where $\mu$ is adjusted at each iteration to make the value of $f(\bm{z})$
reduce, as done in the LM algorithm [18].

Matrix Calculus In the following derivation, we will use vector-by-vector,
vector-by-scalar, scalar-by-vector derivatives. Here we provide their
definitions.

Assume $\bm{a}=[a_{1};\cdots;a_{N}]\in\mathbb{R}^{N}$ is a vector function of
$\bm{b}=[b_{1},\cdots,b_{M}]\in\mathbb{R}^{M}$. The first-order partial
derivatives of vector-by-vector $\frac{\partial\bm{a}}{\partial\bm{b}}$,
vector-by-scalar $\frac{\partial\bm{a}}{\partial b_{j}}$, and scalar-by-vector
$\frac{\partial a_{i}}{\partial\bm{b}}$ are defined as

$\frac{\partial\bm{a}}{\partial\bm{b}}=\begin{bmatrix}\frac{\partial
a_{1}}{\partial b_{1}}&\cdots&\frac{\partial a_{N}}{\partial b_{1}}\\\
\vdots&\ddots&\vdots\\\ \frac{\partial\bm{a}_{1}}{\partial
b_{M}}&\cdots&\frac{\partial\bm{a}_{N}}{\partial\bm{b}_{M}}\end{bmatrix},\frac{\partial\bm{a}}{\partial
b_{j}}=\begin{bmatrix}\frac{\partial a_{1}}{\partial b_{j}}\\\ \vdots\\\
\frac{\partial a_{N}}{\partial b_{j}}\end{bmatrix},\frac{\partial
a_{i}}{\partial\bm{b}}=\begin{bmatrix}\frac{\partial a_{i}}{\partial b_{1}}\\\
\vdots\\\ \frac{\partial a_{i}}{\partial b_{M}}\end{bmatrix}$ (4)

where $\frac{\partial\bm{a}}{\partial\bm{b}}$ is an $M\times N$ matrix with
$\frac{\partial a_{i}}{\partial b_{j}}$ as the $i$th row $j$th column element,
$\frac{\partial\bm{a}}{\partial b_{i}}$ is an $N$-dimensional vector whose
$i$th term is $\frac{\partial a_{i}}{\partial{b}_{j}}$, and $\frac{\partial
a_{i}}{\partial\bm{b}}$ is an $M$-dimensional vector whose $i$th term is
$\frac{\partial a_{i}}{\partial{b}_{i}}$.

###  3.2 Optimal Plane Estimation

Given a set of $K$ points $\\{\bm{p}_{i}\\}$, the optimal plane
$\hat{\bm{\pi}}$ can be estimated by minimizing the sum of squared point-to-
plane distances

$\hat{\bm{\pi}}=\arg\min_{\bm{\pi}}\sum_{i}^{K}\left(\bm{n}^{T}\bm{p}_{i}+d\right)^{2},\
s.t.\ \left\|\bm{n}\right\|_{2}^{2}=1.$ (5)

There is a closed-form solution for $\hat{\bm{\pi}}$. Let us define

$\small\mathbf{M}=\sum_{i=1}^{K}\left(\bm{p}_{i}-\bar{\bm{p}}\right)\left(\bm{p}_{i}-\bar{\bm{p}}\right)^{T}=\mathbf{S}-K\bar{\bm{p}}\bar{\bm{p}}^{T},$
(6)

where $\mathbf{S}=\sum_{i=1}^{K}\bm{p}_{i}\bm{p}_{i}^{T}$ and
$\bar{\bm{p}}=\frac{1}{K}\sum_{i}^{K}{\bm{p}}_{i}$. Assume that
${\lambda}_{3}(\mathbf{M})$ and $\bm{\xi}_{3}(\mathbf{M})$ are the smallest
eigenvalue of $\mathbf{M}$ and the corresponding eigenvector, respectively.
Using these notations, we can write the optimal plane
$\hat{\bm{\pi}}=[\hat{\bm{n}};\hat{d}]$ as

$\hat{\bm{n}}=\bm{\xi}_{3}(\mathbf{M}),\
\hat{d}=-\hat{\bm{n}}^{T}\bar{\bm{p}}.$ (7)

Furthermore, the cost of (5) at $\hat{\bm{\pi}}$ equals to
${\lambda}_{3}(\mathbf{M})$, i.e.,

$\begin{split}{\lambda}_{3}(\mathbf{M})=\sum_{i=1}^{K}\left(\hat{\bm{n}}\bm{p}_{i}+\hat{d}\right)^{2}=\min_{\bm{\pi}}\sum_{i=1}^{K}\left({\bm{n}}\bm{p}_{i}+{d}\right)^{2}.\end{split}$
(8)

The above property will be used to eliminate planes in PA.

###  3.3 Plane Adjustment

Assume that there are $M$ planes and $N$ poses. According to section 3.1, the
$i$th plane can be represented by a four-dimensional vector
$\bm{\pi}_{i}=[\bm{n}_{i};d_{i}]$. The $j$th pose is denoted as $\bm{x}_{j}$.
The observation of $\bm{\pi}_{i}$ at $\bm{x}_{j}$ is a set of $N_{ij}$ points
$\mathbb{P}_{ij}=\\{\bm{p}_{ijk}\in\mathbb{R}^{3}\\}_{k=1}^{N_{ij}}$. For a 3D
point $\bm{p}_{ijk}$, we use $\tilde{\bm{p}}_{ijk}=[\bm{p}_{ijk};1]$ to
represent the homogeneous coordinates of $\bm{p}_{ijk}$. Then, the
transformation from the local coordinate system at $\bm{x}_{j}$ to the global
coordinate system can be represented as

$\bm{p}^{g}_{ijk}=\mathbf{R}_{j}\bm{p}_{ijk}+\bm{t}_{j}=\mathbf{T}_{j}\tilde{\bm{p}}_{ijk},$
(9)

where $\mathbf{T}_{j}$ is defined in (1). Then the distance $d_{ijk}$ from
$\bm{p}_{ijk}$ to $\bm{\pi}_{i}$ has the form

$\begin{split}d_{ijk}(\bm{\pi}_{i},\mathbf{x}_{j})&=\bm{n}_{i}^{T}\left(\mathbf{R}_{j}\bm{p}_{ijk}+\bm{t}_{j}\right)+d_{i}\\\
&=\bm{n}_{i}^{T}\mathbf{T}_{j}\tilde{\bm{p}}_{ijk}+d_{j}=\bm{\pi}^{T}_{i}\tilde{\bm{p}}_{ijk}^{g}.\end{split}$
(10)

The PA problem is to jointly adjust the $M$ planes
$\left\\{\bm{\pi}_{i}\right\\}$ and the $N$ sensor poses
$\left\\{\bm{x}_{j}\right\\}$ to minimize the sum of squared point-to-plane
distances. Specifically, using (10), we can formulate the cost function of the
PA problem as

$\small\min_{\left\\{\bm{\pi}_{i}\right\\},\left\\{\mathbf{x}_{j}\right\\}}\sum_{i=1}^{N}\underbrace{\sum_{j\in
obs(\bm{\pi}_{i})}\sum_{k=1}^{N_{ij}}d_{ijk}^{2}(\bm{\pi}_{i},\mathbf{x}_{j})}_{C_{i}\left(\bm{\pi}_{i},\mathbb{X}_{i}\right),\
\mathbb{X}_{i}=\\{\bm{x}_{j}|j\in
obs(\bm{\pi}_{i})\\}}=\min_{\\{\bm{\pi}_{i}\\},\\{\bm{x}_{j}\\}}\sum_{i=1}^{N_{i}}C_{i}\left(\bm{\pi}_{i},\mathbb{X}_{i}\right).$
(11)

where $obs(\bm{\pi}_{i})$ represents the indexes of poses where $\bm{\pi}_{i}$
can be observed, and $C_{i}(\bm{\pi}_{i},\mathbb{X}_{i})$ accumulates the
errors from $N_{i}=\sum_{j\in obs(\bm{\pi}_{i})}N_{ij}$ points captured at the
set of poses $\mathbb{X}_{i}$. According to (6) and (9), we get

$\small\begin{split}\mathbf{M}_{i}(\mathbb{X}_{i})&=\sum_{j\in
obs(\bm{\pi}_{i})}\mathbf{S}_{ij}-N_{i}\bar{\bm{p}}_{i}\bar{\bm{p}}_{i}^{T},\end{split}$
(12)

where $\bar{\bm{p}}_{i}=\frac{1}{N_{i}}\sum_{j\in
obs(\bm{\pi}_{i})}\sum_{k=1}^{N_{ij}}\bm{p}_{ijk}^{g}$ and
$\mathbf{S}_{ij}={\sum_{k=1}^{N_{ij}}\bm{p}_{ijk}^{g}\left({\bm{p}_{ijk}^{g}}\right)^{T}}$.
Here the elements in $\mathbf{M}_{i}$, $\mathbf{S}_{ij}$ and
$\bar{\bm{p}}_{i}$ in (12) are all functions of the poses in $\mathbb{X}_{i}$.
Substituting $\bm{p}_{ijk}^{g}$ in (9) into $\mathbf{S}_{ij}$ and
$\bar{\bm{p}}_{i}$ in (12), we have

$\small\begin{split}\mathbf{S}_{ij}&=\mathbf{T}_{j}\underbrace{\sum_{k=1}^{N_{ij}}\tilde{\bm{p}}_{ijk}\tilde{\bm{p}}_{ijk}^{T}}_{\mathbf{U}_{ij}}\mathbf{T}_{j}^{T}=\mathbf{T}_{j}\mathbf{U}_{ij}\mathbf{T}_{j}^{T},\\\
\bar{\bm{p}}_{i}&=\frac{1}{N_{i}}\sum_{j\in
obs(\bm{\pi}_{i})}\mathbf{T}_{j}\underbrace{\sum_{k=1}^{N_{ij}}\tilde{\bm{p}}_{ijk}}_{\tilde{\bm{p}}_{ij}}=\frac{1}{N_{i}}\sum_{j\in
obs(\bm{\pi}_{i})}\mathbf{T}_{j}\tilde{\bm{p}}_{ij}.\end{split}$ (13)

Here $\mathbf{U}_{ij}$ and $\tilde{\bm{p}}_{ij}$ in (13) are constants. We
only need to compute them once, and reuse them in the iteration.

According to (7), given poses in $\mathbb{X}_{i}$, the optimal solution for
$\bm{\pi}_{i}$ has a closed-form expression
$\hat{\bm{\pi}}_{i}=[\hat{\bm{n}}_{i};\hat{d}_{i}]$, where
$\hat{\bm{n}}_{i}=\bm{\xi}_{3}(\mathbf{M}_{i}(\mathbb{X}_{i}))$ and
$\hat{d}_{i}=-\hat{\bm{n}}\bar{\bm{p}}_{i}$. As $\mathbf{M}_{i}$ and
$\bar{\bm{p}}_{i}$ are functions of the poses in $\mathbb{X}_{i}$,
$\hat{\bm{\pi}}_{i}$ is also a function of the poses in $\mathbb{X}_{i}$. That
is to say $\hat{\bm{\pi}}_{i}$ is completely determined by the poses in
$\mathbb{X}_{i}$. To simplify the notation, let us define

$\lambda_{i,3}(\mathbb{X}_{i})=\lambda_{3}(\mathbf{M}_{i}(\mathbb{X}_{i})),$
(14)

which represents the smallest eigenvalue of $\mathbf{M}_{i}(\mathbb{X}_{i})$.

Substituting the optimal plane estimation $\hat{\bm{\pi}}_{i}$ into
$C_{i}(\bm{\pi}_{i},\mathbb{X}_{i})$ in (11) and using (8), we have

$\lambda_{i,3}(\mathbb{X}_{i})=C_{i}\left(\hat{\bm{\pi}}_{i},\mathbb{X}_{i}\right).$
(15)

Using (15), we can formulate the PA problem in (11) as

$\\{\hat{\mathbf{x}}_{j}\\}=\arg\min_{\\{\mathbf{X}_{j}\\}}\bm{\tau},\
\bm{\tau}=\sum_{i=1}^{M}\lambda_{i,3}(\mathbb{X}_{i}).$ (16)

The cost function (16) only depends on the sensor poses, which significantly
reduces the number of variables. However, as it is the sum of squared point-
to-plane distances, we cannot apply the widely used Gauss-Newton-like methods
to minimize it, where the Jacobian matrix of residuals are required. Here we
adopt the Newton’s method to solve it. The crux for applying the Newton’s
method to minimize (16) is how to compute the gradient and the Hessian matrix
of (16) efficiently. In the following sections, we provide a closed-form
solution for them. To simplify the notation, we omit the variables of
functions in the following description (e.g.,
$\lambda_{i,3}(\mathbb{X}_{i})\rightarrow\lambda_{i,3}$).

##  4 Newton’s Iteration for Plane Adjustment

Let us denote the gradient and the Hessian matrix of $\tau$ in (16) as
$\bm{g}$ and $\mathbf{H}$, and denote the 6-dimensional gradient vector for
$\bm{x}_{j}$ as $\bm{g}_{j}$ and the $6\times 6$ Hessian matrix block for
$\bm{x}_{j}$ and $\bm{x}_{k}$ as $\mathbf{H}_{jk}$ (note that here $i$ can
equal to $j$). Then $\bm{g}$ and $\mathbf{H}$ can be written in the block form
as $\bm{g}=(\bm{g}_{j})\in\mathbb{R}^{6N}$ and
$\mathbf{H}=(\mathbf{H}_{jk})\in\mathbb{R}^{6N\times 6N}$.

The $i$th plane $\bm{\pi}_{i}$ is observed by the poses in $\mathbb{X}_{i}$.
Assume $j$th pose $\bm{x}_{j}\in\mathbb{X}_{i}$ and the $k$th pose
$\bm{x}_{k}\in\mathbb{X}_{i}$. Let us define

$\bm{g}_{j}^{i}=\frac{\partial\lambda_{i,3}}{\partial\bm{x}_{j}},\
\mathbf{H}_{jk}^{i}=\frac{\partial^{2}\lambda_{i,3}}{\partial\bm{x}_{j}\partial\bm{x}_{k}}.$
(17)

According to (16), we have

$\bm{g}_{j}=\sum_{i\in\mathbb{P}_{j}}\bm{g}^{i}_{j},\
\mathbf{H}_{jk}=\sum_{i\in\mathbb{P}_{jk}}\mathbf{H}_{jk}^{i},$ (18)

where $\mathbb{P}_{j}$ is the set of planes observed by $\bm{x}_{j}$, and
$\mathbb{P}_{jk}$ is the set of planes observed by $\bm{x}_{j}$ and
$\bm{x}_{k}$ simultaneously. If $j=k$, here $\mathbb{P}_{jk}$ equals to
$\mathbb{P}_{j}$. From (18), we know that the key point to get $\bm{g}$ and
$\mathbf{H}$ is to compute $\bm{g}_{j}^{i}$ and $\mathbf{H}_{jk}^{i}$ in (17).

###  4.1 Partial Derivatives of Eigenvalue

According to (17), $\lambda_{i,3}$ is a function of poses in $\mathbb{X}_{i}$,
and $\bm{x}_{j}\in\mathbb{X}_{i}$ and $\bm{x}_{k}\in\mathbb{X}_{i}$. Assume
that $x_{jm}$ and $x_{kn}$ are the $m$th and $n$th elements of $\bm{x}_{j}$
and $\bm{x}_{k}$, respectively. In this section, we first consider the first-
order partial derivation $\frac{\partial\lambda_{i,3}}{\partial x_{jm}}$ and
the second-order partial derivation $\frac{\partial^{2}\lambda_{i,3}}{\partial
x_{jm}\partial x_{kn}}$.

$\lambda^{3}_{i,3}$ is a root of the equation
$|\mathbf{M}_{i}(\mathbb{X}_{i})-\lambda_{i}\mathbf{I}_{3}|=0$, where
$|\cdot|$ denotes the determinant of a matrix. Assume $m_{ef}$ is the $e$th
row $f$th column term of $\mathbf{M}_{i}(\mathbb{X}_{i})$.
$|\mathbf{M}_{i}(\mathbb{X}_{i})-\lambda_{i}\mathbf{I}_{3}|=0$ is a cubic
equation with the following form

$-\lambda^{3}_{i,3}+a_{i}\lambda^{2}_{i,3}+b_{i}\lambda_{i,3}+c_{i}=0,$ (19)

where $a_{i}=m_{11}+m_{22}+m_{33}$,
$b_{i}=m_{12}^{2}+m_{13}^{2}+m_{23}^{2}-m_{11}m_{22}-m_{11}m_{33}-m_{22}m_{33}$,
and
$c_{i}=-m_{33}m_{12}^{2}+2m_{12}m_{13}m_{23}-m_{22}m_{13}^{2}-m_{11}m_{23}^{2}+m_{11}m_{22}m_{33}$.
Here $a_{i}$, $b_{i}$ and $c_{i}$ are all functions of the poses in
$\mathbb{X}_{i}$. It is known that the root of a cubic equation has a closed
form. One solution to compute $\frac{\partial\lambda_{i,3}}{\partial x_{jm}}$
and $\frac{\partial^{2}\lambda_{i,3}}{\partial x_{jm}\partial x_{kn}}$ is to
directly differentiate the root. However, the formula of the root is too
complicated. Here we introduce a simple way to compute them. Briefly, we
employ the implicit function theorem [16] to compute them. Let us define

$\bm{\chi}_{i}=\begin{bmatrix}\lambda_{i,3}^{2}\\\ \lambda_{i,3}\\\
1\end{bmatrix},\ \bm{\eta}_{i}=\begin{bmatrix}a_{i}\\\ b_{i}\\\
c_{i}\end{bmatrix}\bm{\kappa}_{i}=\begin{bmatrix}-3\\\ 2a_{i}\\\
b_{i}\end{bmatrix}.$ (20)

$\frac{\partial\lambda_{i,3}}{\partial x_{jm}}$ and
$\frac{\partial^{2}\lambda_{i,3}}{\partial x_{jm}\partial x_{kn}}$ are
presented in Lemma 1 and 2. The proofs of the following lemmas and theorems
are in the supplementary material.

######  Lemma 1.

$\frac{\partial\lambda_{i,3}}{\partial x_{jm}}$ has a closed-form expression
as

$\frac{\partial\lambda_{i,3}}{\partial
x_{jm}}=-\varphi_{i}\bm{\delta}_{jm}^{i}\cdot\bm{\chi}_{i},$ (21)

where $\cdot$ represents the dot product and
$\varphi_{i}=\left(\bm{\kappa}_{i}\cdot\bm{\chi}_{i}\right)^{-1}$ and
$\bm{\delta}_{jm}^{i}=\frac{\partial\bm{\eta}_{i}}{\partial x_{jm}}$.

######  Lemma 2.

$\frac{\partial^{2}{}\lambda_{i,3}}{\partial x_{jm}\partial x_{kn}}$ has a
closed-form expression as

$\small\frac{\partial^{2}\lambda_{i,3}}{\partial x_{jm}\partial
x_{kn}}=-\varphi_{i}\left(\bm{\delta}_{jm}^{i}\cdot\frac{\partial\bm{\chi}_{i}}{\partial
x_{kn}}+\bm{\chi}_{i}\cdot\frac{\partial\bm{\delta}_{jm}^{i}}{\partial
x_{kn}}-\frac{\partial\lambda_{i,3}}{\partial
x_{jm}}\frac{\partial\varphi_{i}^{-1}}{\partial x_{kn}}\right).$ (22)

Let us define

$\small\begin{split}&\bm{\alpha}_{j}^{i}=\frac{\partial
a_{i}}{\partial\bm{x}_{j}},\bm{\beta}_{j}^{i}=\frac{\partial
b_{i}}{\partial\bm{x}_{j}},\bm{\gamma}_{j}^{i}=\frac{\partial
c_{i}}{\partial\bm{x}_{j}},\mathbf{\Delta}_{j}^{i}=[\bm{\alpha}_{j}^{i},\bm{\beta}_{j}^{i},\bm{\gamma}_{j}^{i}],\\\
&\bm{\alpha}_{k}^{i}=\frac{\partial
a_{i}}{\partial\bm{x}_{k}},\bm{\beta}_{k}^{i}=\frac{\partial
b_{i}}{\partial\bm{x}_{k}},\bm{\gamma}_{k}^{i}=\frac{\partial
c_{i}}{\partial\bm{x}_{k}},\mathbf{\Delta}_{k}^{i}=[\bm{\alpha}_{k}^{i},\bm{\beta}_{k}^{i},\bm{\gamma}_{k}^{i}],\\\
&\mathbf{H}^{a_{i}}_{jk}=\frac{\partial^{2}a_{i}}{\partial\bm{x}_{j}\partial\bm{x}_{k}},\mathbf{H}^{b_{i}}_{jk}=\frac{\partial^{2}b_{i}}{\partial\bm{x}_{j}\partial\bm{x}_{k}},\mathbf{H}^{c_{i}}_{jk}=\frac{\partial^{2}c_{i}}{\partial\bm{x}_{j}\partial\bm{x}_{k}}.\end{split}$
(23)

Using the above lemmas and notations, we can derive $\bm{g}_{j}^{i}$ and
$\mathbf{H}_{jk}^{i}$.

######  Theorem 1.

$\bm{g}_{j}^{i}$ and $\mathbf{H}_{jk}^{i}$ have the forms

$\begin{split}\bm{g}_{j}^{i}&=-\varphi_{i}\bm{\Delta}_{j}^{i}\bm{\chi}_{i},\\\
\mathbf{H}_{jk}^{i}&=\varphi_{i}\left(\mathbf{K}_{jk}^{i}-{\lambda_{3,i}^{2}}\mathbf{H}^{a_{i}}_{jk}-\lambda_{3,i}\mathbf{H}^{b_{i}}_{jk}-\mathbf{H}^{c_{i}}_{jk}\right),\end{split}$
(24)

where
$\mathbf{K}_{jk}^{i}=\bm{g}_{j}^{i}{(\bm{u}_{k}^{i})}^{T}-\bm{v}_{j}^{i}({\bm{g}_{k}^{i}})^{T}$,
$\bm{u}_{k}^{i}=2\lambda_{i,3}\bm{\alpha}_{j}^{i}+\bm{\beta}_{j}^{i}+(2a-6\lambda_{i,3})\bm{g}_{k}^{i}$,
$\bm{v}_{j}^{i}=2\lambda_{i,3}\bm{\alpha}_{k}^{i}+\bm{\beta}_{k}^{i}$, and
similar to $\bm{g}_{j}^{i}$,
$\bm{g}_{k}^{i}=-\varphi_{i}\bm{\Delta}_{k}^{i}\bm{\chi}_{i}$ is the gradient
block for $\bm{x}_{k}$.

The formula of $\mathbf{H}_{jk}^{i}$ in (24) is applicable to the case that
$j=k$. From Theorem 1, we know that the key point to get $\bm{g}_{j}^{i}$ and
$\mathbf{H}_{jk}^{i}$ is to get the derivatives of $a_{i}$, $b_{i}$ and
$c_{i}$ in (23).

###  4.2 Partial Derivatives of $a_{i}$, $b_{i}$ and $c_{i}$

As shown in (19), $a_{i}$, $b_{i}$ and $c_{i}$ are functions of the elements
in $\mathbf{M}_{i}$. Using this relationship, we can easily derive the partial
derivatives in (23). For instance, as $a_{i}=m_{11}+m_{22}+m_{33}$, we have

$\frac{\partial a_{i}}{\partial\bm{x}_{j}}=\frac{\partial
m_{11}}{\partial\bm{x}_{j}}+\frac{\partial
m_{22}}{\partial\bm{x}_{j}}+\frac{\partial m_{33}}{\partial\bm{x}_{j}}.$ (25)

Thus, to get the first- and second-order partial derivatives of $a_{i}$,
$b_{i}$ and $c_{i}$ with respect to $\bm{x}_{j}$ and $\bm{x}_{k}$ in (23), we
need to derive the form of $\mathbf{M}_{i}$ in terms of $\bm{x}_{j}$ and
$\bm{x}_{k}$.

######  Lemma 3.

In terms of $\bm{x}_{j}$ and $\bm{x}_{k}$, $\bar{\bm{p}}_{i}$ in (13) has the
form

$\bar{\bm{p}}_{i}(\bm{x}_{j},\bm{x}_{k})=\mathbf{T}_{j}\bm{q}_{ij}+\mathbf{T}_{k}\bm{q}_{ik}+\bm{c}_{ijk},$
(26)

where $\bm{q}_{ij}=\frac{1}{N_{i}}\bm{\tilde{p}}_{ij}$,
$\bm{q}_{ik}=\frac{1}{N_{i}}\bm{\tilde{p}}_{ik}$, and
$\bm{c}_{ijk}=\frac{1}{N_{i}}\sum_{n\in\mathbb{O}_{jk}}\mathbf{T}_{n}\tilde{\bm{p}}_{in}$.
Here $\mathbb{O}_{jk}$ represents the set of poses where $\bm{\pi}_{i}$ can be
observed, excluding the $\bm{j}$th and $\bm{k}$th poses (i.e.,
$\mathbb{O}_{jk}=obs(\bm{\pi}_{i})-\\{j,k\\}$).

In terms of $\bm{x}_{j}$, $\bar{\bm{p}}_{i}$ has the form

$\bar{\bm{p}}_{i}(\bm{x}_{j})=\mathbf{T}_{j}\mathbf{q}_{ij}+\bm{c}_{ij},$ (27)

where $\bm{c}_{ij}=\mathbf{T}_{k}\mathbf{q}_{ik}+\bm{c}_{ijk}$.

Using Lemma 3, we can have the following theorem for $\mathbf{M}_{i}$ in (12).

######  Theorem 2.

In terms of $\bm{x}_{j}$, $\mathbf{M}_{i}$ in (12) can be written as

$\mathbf{M}_{i}(\bm{x}_{j})=\mathbf{T}_{j}\mathbf{Q}_{j}^{i}\mathbf{T}_{j}^{T}+\mathbf{T}_{j}\mathbf{K}_{j}^{i}+(\mathbf{K}_{j}^{i})^{T}\mathbf{T}_{j}^{T}+\mathbf{C}_{j}^{i},$
(28)

where $\mathbf{Q}_{j}^{i}=\mathbf{S}_{ij}-N_{j}\bm{q}_{ij}\bm{q}_{ij}^{T}$ and
$\mathbf{K}_{j}^{i}=-N_{i}\bm{q}_{ij}\bm{c}_{ij}^{T}$.

In terms of $\bm{x}_{j}$ and $\bm{x}_{k}$, $\mathbf{M}_{i}$ can be written as

$\mathbf{M}_{i}(\bm{x}_{j},\bm{x}_{k})=\mathbf{T}_{j}\mathbf{O}_{jk}^{i}\mathbf{T}_{k}^{T}+\mathbf{T}_{k}{(\mathbf{O}_{jk}^{i})}^{T}\mathbf{T}_{j}^{T}+\mathbf{C}_{jk}^{i}.$
(29)

where $\mathbf{O}_{jk}^{i}=-N_{i}\bm{q}_{ij}\bm{q}_{ik}^{T}$.

Here we do not provide the detailed formulas for $\mathbf{C}_{j}^{i}$ and
$\mathbf{C}_{jk}^{i}$, as they will be eliminated in the partial derivative.
Actually, only $\mathbf{Q}_{j}^{i}$, $\mathbf{K}_{j}^{i}$, and
$\mathbf{O}_{jk}^{i}$ are required to compute the partial derivatives in (23).
Equation (28) is used to compute the first- and second-order partial
derivatives of $a_{i}$, $b_{i}$ and $c_{i}$ with respect to $\bm{x}_{j}$.
Equation (29) is used to compute the second-order partial derivatives of
$a_{i}$, $b_{i}$ and $c_{i}$ with respect to $\bm{x}_{j}$ and $\bm{x}_{k}$.

###  4.3 Efficient Iteration

From Theorem 2, we can easily derive the elements of
$\mathbf{M}_{i}(\bm{x}_{j})$ and $\mathbf{M}_{i}(\bm{x}_{j},\bm{x}_{i})$.
Specifically, each element of them is a second-order polynomial in terms of
the elements in $\mathbf{T}_{j}$ and $\mathbf{T}_{k}$. Assume
$m_{ef}(\bm{x}_{j})$ and $m_{ef}(\bm{x}_{j},\bm{x}_{k})$ are the $e$th row
$f$th column element of $\mathbf{M}_{i}(\bm{x}_{j})$ and
$\mathbf{M}_{i}(\bm{x}_{j},\bm{x}_{k})$, respectively. $m_{ef}(\bm{x}_{j})$
and $m_{ef}(\bm{x}_{j},\bm{x}_{k})$ are linear combinations of monomials with
respect to the elements in $\mathbf{T}_{j}$ and $\mathbf{T}_{k}$. Substituting
(2) into $m_{ef}(\bm{x}_{j})$ and $m_{ef}(\bm{x}_{j},\bm{x}_{k})$, we have

$\begin{split}m_{ef}(\bm{x}_{j})&=\bm{c}_{ef}\cdot\bm{h}_{ef}(\bm{x}_{j}),\\\
m_{ef}(\bm{x}_{j},\bm{x}_{k})&=\bm{d}_{ef}\cdot\bm{g}_{ef}(\bm{x}_{j},\bm{x}_{k}),\end{split}$
(30)

where $\bm{c}_{ef}$ is determined by $\mathbf{Q}_{j}^{i}$ and
$\mathbf{K}^{i}_{j}$ in (28), $\bm{d}_{ef}$ is determined by
$\mathbf{O}_{jk}^{i}$ in (29), and $\bm{h}_{ef}$ and $\bm{g}_{ef}$ are two
vector functions. Let us first consider the first-order partial derivative of
$m_{ef}(\bm{x}_{j})$ with respect to $\bm{x}_{j}$. It has the form

$\frac{\partial
m_{ef}(\bm{x}_{j})}{\partial\bm{x}_{j}}=\frac{\partial\bm{h}_{ef}(\bm{x}_{j})}{\partial\bm{x}_{j}}\bm{c}_{ef},$
(31)

where the vector-by-vector derivative
$\frac{\partial\bm{h}_{ef}(\bm{x}_{j})}{\partial\bm{x}_{j}}$ is defined in
(4). To efficiently compute (31), we consider a special pose
$\mathbf{T}_{0}=[\mathbf{R}_{0},\bm{t}_{0}]$ where
$\mathbf{R}_{0}=\mathbf{I}_{3}$ and $\bm{t}_{0}=[0;0;0]$. Let us denote the
parameterization of $\mathbf{T}_{0}$ as $\bm{x}_{0}$. As the CGR
parameterization defined in (2) for $\mathbf{I}_{3}$ is $[0;0;0]$, we have
$\bm{x}_{0}=[0;0;0;0;0;0]$. For $\bm{x}_{j}=\bm{x}_{0}$, the matrix
$\left.\frac{\partial\bm{h}_{ef}(\mathbf{x}_{j})}{\partial\bm{x}_{j}}\right|_{{\mathbf{x}_{j}}=\mathbf{x}_{0}}$
has many zero terms. Similarly, the second-order partial derivatives of
$\bm{h}_{ef}(\bm{x}_{j})$ and $\bm{g}_{ef}(\bm{x}_{j})$ at
$\bm{x}_{j}=\bm{x}_{0}$ and $\bm{x}_{k}=\bm{x}_{0}$ are simple. As
$\bm{h}_{ef}(\bm{x}_{j})$ and $\bm{g}_{ef}(\bm{x}_{j},\bm{x}_{k})$ only
depends on $\bm{x}_{j}$ and $\bm{x}_{k}$, we can compute their partial
derivatives at $\bm{x}_{0}$ once, and then reuse them during the iteration.
Here we introduce a method to make the iteration stay at $\mathbf{x}_{0}$ for
each pose.

Assume that $\\{\mathbf{X}_{j}^{n}\\}$ are the poses after the $n$th
iteration. Then we can update $\mathbf{U}_{ij}$ and $\tilde{\bm{p}}_{ij}$ in
(13) by

$\mathbf{U}_{ij}^{n+1}=\mathbf{X}_{j}^{n}\mathbf{U}_{ij}(\mathbf{X}_{j}^{n})^{T}\
\text{and}\ \tilde{\bm{p}}_{ij}^{n+1}=\mathbf{X}_{j}^{n}\tilde{\bm{p}}_{ij}.$
(32)

Substituting $\mathbf{U}_{ij}^{n+1}$ and $\tilde{\bm{p}}_{ij}^{n+1}$ into
(12), we get a new matrix $\mathbf{M}_{i}(\mathbb{X}_{i})^{n+1}$, which can
finally lead to a new cost $\tau^{n+1}$ in (16). As the points have been
transformed by $\\{\mathbf{X}_{j}^{n}\\}$, each pose should start with
$\mathbf{X}_{0}$ for $\tau^{n+1}$. Assume that $\Delta\bm{x}^{n+1}_{j}$ is the
result from the $(n+1)$th iteration for the $j$th pose. We can obtain the
corresponding transformation matrix $\Delta\mathbf{X}_{j}^{n+1}$ using (2).
Then we can update $\mathbf{X}_{j}^{n}$ by

$\mathbf{X}_{j}^{n+1}=\Delta\mathbf{X}_{j}^{n+1}\mathbf{X}_{j}^{n}$ (33)

Furthermore, the update steps in (32) will not introduce additional
computation. This is because the damped Newton’s method requires to compute
the cost $\tau$ in (16) to adjust $\mu$ in (3) after each iteration, which
will perform the computation in (32).

Figure 3: The four datasets used in this paper. The four datasets have 472,
1355, 1606, 1184 poses, respectively. Roofs are removed to show the
trajectories. Figure 4: The point clouds of dataset C after the poses were
perturbs by the four noise levels.

Figure 5: The results of our algorithm and PA_LM [24] under different noise
levels. It is clear that our algorithm converges significantly faster than
PA_LM. Figure 6: The number of iterations for our algorithm and PA_LM [24].
Our algorithm requires much fewer number of iterations than PA_LM.

###  4.4 Algorithm Summary

We first construct $\mathbf{H}$ and $\bm{g}$. For each plane $\bm{\pi}_{i}$,
we solve the cubic equation (19), and select the smallest root
$\lambda_{i,3}$. For $\bm{x}_{j}$, we construct $\mathbf{M}(\bm{x}_{j})$ in
(28), and calculate the partial derivatives of $a_{i}$, $b_{i}$ and $c_{i}$
with respect to $\bm{x}_{j}$ in (23). Then, we use (24) to compute
$\bm{g}_{j}^{i}$ and $\mathbf{H}_{jj}^{i}$ and use (18) to update $\bm{g}_{j}$
and $\mathbf{H}_{jj}$. For $\bm{x}_{j}$ and $\bm{x}_{k}$,
$\mathbf{M}_{i}(\bm{x}_{j},\bm{x}_{k})$ is generated, and then the partial
derivatives of $a_{i}$, $b_{i}$ and $c_{i}$ with respect to $\bm{x}_{j}$ and
$\bm{x}_{j}$ in (23) are computed. Then, $\mathbf{H}_{jk}^{i}$ can be easily
obtained from (24), and $\mathbf{H}_{jk}$ in (18) is updated accordingly.
Using $\mathbf{H}$ and $\bm{g}$, we conduct the damped Newton’s step in (3).
After each iteration, $\mathbf{U}_{ij}$ and $\tilde{\bm{p}}_{ij}$ are updated
by (32). The proposed algorithm is summarized in Algorithm 1. Let us denote
the mean and variance of the number of observations per plane as $\bar{K}$ and
$\sigma^{2}$, respectively. According to [9], the computational complexity of
the Hessian matrix is $O(M(\bar{K}^{2}+\sigma^{2}))$, which is of the same
order as the Schur complement trick.

while  _not converge_ do

$\mathbf{H}=zeros(6N,6N)$, $\bm{g}=zeros(6N,1)$;

for  _$i\in[1,M]$_ do

/* Compute $\bm{g}$ and the diagonal terms of $\mathbf{H}.$ */

for  _$j\in obs(\bm{\pi}_{i})$_ do

Compute $\mathbf{M}_{i}(\bm{x}_{j})$ using (28);

Compute $\bm{\alpha}_{j}^{i}$, $\bm{\beta}_{j}^{i}$, $\bm{\gamma}_{j}^{i}$,
$\mathbf{H}_{jj}^{a_{i}}$, $\mathbf{H}_{jj}^{b_{i}}$,
$\mathbf{H}_{jj}^{c_{i}}$ using (23);

Compute $\mathbf{H}_{jj}^{i}$ and $\bm{g}_{j}^{i}$ using (24);

$\mathbf{H}_{jj}=\mathbf{H}_{jj}+\mathbf{H}_{jj}^{i}$,
$\bm{g}_{j}=\bm{g}_{j}+\bm{g}_{j}^{i}$;

/* Compute other terms of $\mathbf{H}.$ */

for  _$j\in obs(\bm{\pi}_{i})$_ do

for  _$k\in obs(\bm{\pi}_{i})$ and $k>j$_ do

Compute $\mathbf{H}_{jk}^{a_{i}}$, $\mathbf{H}_{jk}^{b_{i}}$,
$\mathbf{H}_{jk}^{c_{i}}$ using (23);

Compute $\mathbf{H}_{jk}^{i}$ using (24);

$\mathbf{H}_{jk}=\mathbf{H}_{jk}+\mathbf{H}_{jk}^{i}$;

$\mathbf{H}_{kj}=\mathbf{H}_{kj}+(\mathbf{H}_{jk}^{i})^{T}$;

Conduct the damped Newton’s step in (3) ;

Update $\mathbf{U}_{ij}$ and $\tilde{\bm{p}}_{ij}$ using (32);

Algorithm 1 Second-order plane adjustment for $N$ poses and $M$ planes

##  5 Experiments

###  5.1 Setup

In this section, we evaluate the performance of our algorithm and the
traditional LM algorithm [24] (PA_LM). We obtain the c++ code of PA_LM from
the author of [24], which was implemented by the Ceres library [3]. Our damped
Newton’s method is implemented according to the implementation of the LM
algorithm in Ceres. The damped Newton’s method and the LM algorithm are with
the same parameters. Specifically, the initial value of the damping factor
$\mu$ in (3) is set to $10^{-4}$. The maximum number of iterations is set to
200, and the early stopping tolerances (such as the cost value change and the
norm of gradient) are set to $10^{-7}$. All the experiments were conducted on
a desktop with an Intel i7 cpu and 64G memory.

###  5.2 Datasets

We collected four datasets using a VLP-16 LiDAR. We used the LiDAR SLAM
algorithm [25] to detect the planes and establish the plane association. Fig.
3 shows the four datasets. Similar to the evaluation of BA algorithms [4, 26,
9], we perturb the pose, and compare the convergence speed of different PA
algorithms. Specifically, we directly add Gaussian noises to the translation,
and randomly generate an angle-axis vector from a Gaussian distribution to
perturb the rotation. After the poses are perturb, we use (7) to get the
initial plane parameters for PA_LM [24].

We evaluate the performance of different algorithms under different noise
levels. Let us denote the standard deviation (std) of the Gaussian noises for
rotation and translation as $\sigma_{\mathbf{R}}$ and $\sigma_{\bm{t}}$,
respectively. We consider four noise levels: $\sigma_{\mathbf{R}}=0.1^{\circ}$
and $\sigma_{\bm{t}}=0.01m$, $\sigma_{\mathbf{R}}=1^{\circ}$ and
$\sigma_{\bm{t}}=0.1m$, $\sigma_{\mathbf{R}}=2^{\circ}$ and
$\sigma_{\bm{t}}=0.2m$, and $\sigma_{\mathbf{R}}=3^{\circ}$ and
$\sigma_{\bm{t}}=0.3m$. Fig. 4 demonstrates the point clouds of dataset C
after the poses are perturbed by the four noise levels.

###  5.3 Results

Fig. 5 and Fig. 6 illustrates the results. It is clear that our algorithm
converges faster than PA_LM. PA_LM works well at small noises (such as
$\sigma_{\mathbf{R}}=0.1^{\circ}$ and $\sigma_{\bm{t}}=0.01m$). As the noise
level increases, PA_LM tends to converge slower. For dataset A and B, PA_LM
does not converge before the maximum number of iterations reaches when
$\sigma_{\mathbf{R}}=3^{\circ}$ and $\sigma_{\bm{t}}=3m$. For dataset C and D,
the performance of PA_LM gets worse. It converges very slowly after the noise
level reaches $\sigma_{\mathbf{R}}=1^{\circ}$ and $\sigma_{\bm{t}}=0.1m$. In
contrast, the impact of the noise level on our algorithm is small. Our
algorithm is more robust to the noise.

###  5.4 Discussion

Our algorithm converges faster than PA_LM for all the noise levels. This is
because our algorithm takes advantage of the special relationship between
planes and poses in (7). This does not only significantly reduce the number of
variables, but only ensures planes obtain the optimal estimation with respect
to the current pose estimation after each iteration. Although PA_LM jointly
optimizes planes and poses, it cannot guarantee planes achieve the optimal
value after each iteration. That is to say even if our algorithm and PA_LM get
the same poses, our algorithm can reach a smaller cost. Thus our algorithm
converges faster.

##  6 Conclusion

In the computer vision community, Newton’s method is generally considered too
expensive for a large-scale least-squares problem. This paper adopts the
Newton’s method to efficiently solve the PA problem. Our algorithm takes
advantage of the fact that the optimal planes are determined by the poses, so
that the number of unknowns can be significantly reduced. Furthermore, this
property can ensure to obtain the optimal planes when we update the poses. The
difficulty lies in how to efficiently compute the Hessian matrix and the
gradient vector. The key contribution of this paper is to provide a closed-
form solution for them. The experimental results show that our algorithm can
converge faster than the LM algorithm.

##  7 Supplementary Material

###  7.1 Implicit Function Theorem

Here we introduce the implicit function theorem [1]. We used it to derive
Lemma 1.

Implicit Function Theorem Let
$\bm{f}:\mathbb{R}^{n+m}\rightarrow\mathbb{R}^{m}$ be a continuously
differentiable function, and let $\mathbb{R}^{n+m}$ have coordinates
$\left[{\bm{x}},{\bm{y}}\right]$. Fix a point
$\left[\bm{a},\bm{b}\right]=\left[a_{1},\cdots a_{n},b_{1}\cdots b_{m}\right]$
with $f(\bm{a},\bm{b})=\mathbf{0}$, where $\mathbf{0}\in\mathbb{R}^{m}$ is the
zero vector. If the Jacobian matrix of $f$ with respect to $\bm{y}$ is
invertible at $\left[\bm{a},\bm{b}\right]$, then there exists an open set
$\mathbb{U}\subset\mathbb{R}^{n}$ containing $\bm{a}$ such that there exists a
unique continuously differentiable function
$\bm{g}:\mathbb{U}\rightarrow\mathbf{R}^{m}$ such that
$\bm{g}(\bm{a})=\bm{b}$, and $\bm{f}(\bm{x},\bm{g}(\bm{x}))=0$ for all
$\bm{x}\in\mathbb{U}$. Moreover, the Jacobian matrix of $\bm{g}$ in
$\mathbb{U}$ with respect to $\bm{x}$ is given by the matrix product:

$\frac{\partial\bm{g}}{\partial\bm{x}}(\bm{x})=-{\mathbf{J}_{\bm{f},\bm{y}}(\bm{x},\bm{g}(\bm{x}))}^{-1}\mathbf{J}_{\bm{f},\bm{x}}(\bm{x},\bm{g}(\bm{x}))$
(34)

where ${\mathbf{J}_{\bm{f},\bm{y}}(\bm{x},\bm{g}(\bm{x}))}$ is the Jacobian
matrix of $\bm{f}$ with respect to $\bm{y}$ at
$\left[\bm{x},\bm{g}(\bm{x})\right]$, and
${\mathbf{J}_{\bm{f},\bm{x}}(\bm{x},\bm{g}(\bm{x}))}$ is the Jacobian matrix
of $\bm{f}$ with respect to $\bm{x}$ at $\left[\bm{x},\bm{g}(\bm{x})\right]$.

From the implicit function theorem, we know that we can get
$\frac{\partial\bm{g}}{\partial\bm{x}}\left(\bm{x}\right)$ without knowing the
exact form of $\bm{g}(\bm{x})$.

###  7.2 Proof of Lemma 1

###### Proof.

$a_{i}$, $b_{i}$, $c_{i}$ are functions of poses in $\mathbb{X}_{i}$. Here we
only consider one variable $x_{jm}$ of $\bm{x}_{j}$ (i.e., the $m$th entry of
$\bm{x}_{j}\in\mathbb{X}_{i}$). To compute
$\frac{\partial\lambda_{i,3}}{\partial x_{jm}}$, we treat $x_{jm}$ as the only
unknown and other variables in $\mathbb{X}_{i}$ as constants. Thus, $a_{i}$,
$b_{i}$, $c_{i}$ are functions of $x_{jm}$. Then, we define

$f(x_{jm},\lambda_{i,3})=-\lambda^{3}_{i,3}+a_{i}\lambda^{2}_{i,3}+b_{i}\lambda_{i,3}+c_{i}.$
(35)

Then we have

$\begin{split}\frac{\partial
f}{\partial\lambda_{i,3}}&=-3\lambda_{i,3}^{2}+2a_{i}\lambda_{i,3}+b_{i},\\\
\frac{\partial f}{\partial x_{jm}}&=\lambda_{i,3}^{2}\frac{\partial
a_{i}}{\partial x_{jm}}+\lambda_{i,3}\frac{\partial b_{i}}{\partial
x_{jm}}+\frac{\partial c_{i}}{\partial x_{jm}}.\end{split}$ (36)

According to the definition of $\bm{\delta}_{jm}^{i}$ in (21), it has the form

$\bm{\delta}_{jm}^{i}=\frac{\partial\bm{\eta}_{j}}{\partial
x_{jm}}=\begin{bmatrix}\frac{\partial a_{i}}{{x}_{jm}}\\\ \frac{\partial
b_{i}}{x_{jm}}\\\ \frac{\partial c_{i}}{x_{jm}}\end{bmatrix}$ (37)

Substituting the definitions of $\varphi_{i}$, $\bm{\chi}_{i}$,
$\bm{\kappa}_{i}$, and $\bm{\delta}_{jm}$ into (36), we have

$\begin{split}\frac{\partial
f}{\partial\lambda_{i,3}}&=\bm{\kappa}_{i}\cdot\bm{\chi}_{i}=\varphi_{i}^{-1},\\\
\frac{\partial f}{\partial
x_{jm}}&=\bm{\delta}_{jm}\cdot\bm{\chi}_{i}.\end{split}$ (38)

Using the implicit function theorem, for $f(x_{jm},\lambda_{i,3})=0$, we have

$\frac{\partial\lambda_{i,3}}{\partial x_{jm}}=-\frac{\frac{\partial
f}{\partial x_{jm}}}{\frac{\partial f}{\partial\lambda_{i,3}}}$ (39)

Substituting (38) into (39), we finally get

$\frac{\partial\lambda_{i,3}}{\partial
x_{jm}}=-\varphi_{i}\bm{\delta}_{jm}^{i}\cdot\bm{\chi}_{i}.$ (40)

∎

###  7.3 Proof of Lemma 2

###### Proof.

We first compute the partial derivative of
$\frac{\partial\lambda_{i,3}}{\partial x_{jm}}$ in (21) with respect to
$x_{kn}$. According to the production rule of calculus, we have

$\frac{\partial^{2}\lambda_{i,3}}{\partial x_{jm}\partial
x_{km}}=-\varphi_{i}\bm{\delta}_{jm}^{i}\cdot\frac{\partial\bm{\chi}_{i}}{\partial
x_{kn}}-\varphi_{i}\bm{\chi}_{i}\cdot\frac{\partial\bm{\delta}_{jm}^{i}}{\partial
x_{kn}}-\bm{\delta}_{jm}^{i}\cdot\bm{\chi}_{i}\frac{\partial\varphi_{i}}{\partial
x_{kn}}.$ (41)

Let us first focus on the term $\frac{\partial\varphi_{i}^{-1}}{\partial
x_{kn}}$ in (41). As
$\varphi_{i}=\left(\bm{\kappa}_{i}\cdot\bm{\chi}_{i}\right)$, we have

$\frac{\partial\varphi_{i}}{\partial
x_{kn}}=-\left(\bm{\kappa}_{i}\cdot\bm{\chi}_{i}\right)^{-2}\frac{\partial\left(\bm{\kappa}_{i}\cdot\bm{\chi}_{i}\right)}{\partial
x_{kn}}=-\varphi_{i}^{2}\frac{\partial\varphi_{i}^{-1}}{\partial x_{kn}}.$
(42)

Now let us consider
$\bm{\delta}_{jm}^{i}\cdot\bm{\chi}_{i}\frac{\partial\varphi_{i}}{\partial
x_{kn}}$ that is the third term in (41). Using (42), we have

$\begin{split}\bm{\delta}_{jm}^{i}\cdot\bm{\chi}_{i}\frac{\partial\varphi_{i}}{\partial
x_{kn}}&=-\bm{\delta}_{jm}^{i}\cdot\bm{\chi}_{i}\varphi_{i}^{2}\frac{\partial\varphi_{i}^{-1}}{\partial
x_{kn}}\\\
&=-\varphi_{i}\underbrace{\left(\varphi_{i}\bm{\delta_{jm}^{i}}\cdot\bm{\chi}_{i}\right)}_{\frac{\partial\lambda_{i,3}}{\partial
x_{jm}}}\frac{\partial\varphi_{i}^{-1}}{\partial x_{kn}}\\\
&=-\varphi_{i}\frac{\partial\lambda_{i,3}}{\partial
x_{jm}}\frac{\partial\varphi_{i}^{-1}}{\partial x_{kn}}\end{split}$ (43)

Substituting (43) into (41), we have

$\small\begin{split}\frac{\partial^{2}\lambda_{i,3}}{\partial x_{jm}\partial
x_{km}}&=-\varphi_{i}\bm{\delta}_{jm}^{i}\cdot\frac{\partial\bm{\chi}_{i}}{\partial
x_{kn}}-\varphi_{i}\bm{\chi}_{i}\cdot\frac{\partial\bm{\delta}_{jm}^{i}}{\partial
x_{kn}}+\varphi_{i}\frac{\partial\lambda_{i,3}}{\partial
x_{jm}}\frac{\partial\varphi_{i}^{-1}}{\partial x_{kn}}\\\
&=-\varphi_{i}\left(\bm{\delta}_{jm}^{i}\cdot\frac{\partial\bm{\chi}_{i}}{\partial
x_{kn}}+\bm{\chi}_{i}\cdot\frac{\partial\bm{\delta}_{jm}^{i}}{\partial
x_{kn}}-\frac{\partial\lambda_{i,3}}{\partial
x_{jm}}\frac{\partial\varphi_{i}^{-1}}{\partial x_{kn}}\right)\end{split}$
(44)

∎

###  7.4 Proof of Theorem 1

Figure 7: Summary of our algorithm. $\mathbb{X}_{i}$ is the set of poses which
can see $\bm{\pi}_{i}$. Here $\bm{x}_{j}\in\mathbb{X}_{i}$ and
$\bm{x}_{k}\in\mathbb{X}_{i}$. The key point of our algorithm is to get
$\bm{g}_{j}^{i}=\frac{\partial\lambda_{i,3}}{\partial\bm{x}_{j}}$,
$\mathbf{H}_{jj}^{i}=\frac{\partial^{2}\lambda_{i,3}}{\partial\bm{x}_{j}^{2}}$,
and
$\mathbf{H}_{jk}^{i}=\frac{\partial^{2}\lambda_{i,3}}{\partial\bm{x}_{j}\partial\bm{x}_{k}}$.
Theorem 1 provides their formulas. From Theorem 1, we know that the partial
derivatives of $a_{i}$, $b_{i}$, and $c_{i}$ with respect to $\bm{x}_{j}$ and
$\bm{x}_{k}$ in (23) are crucial. As $a_{i}$, $b_{i}$, and $c_{i}$ are
polynomials with respect to $m_{ef}$ ($e=1,2,3$ and $f=1,2,3$), to get the
partial derivatives of $a_{i}$, $b_{i}$, and $c_{i}$ with respect to
$\bm{x}_{j}$ and $\bm{x}_{k}$, we need to compute the partial derivatives of
$m_{ef}$ with respect to $\bm{x}_{j}$ and $\bm{x}_{k}$. Section 7.7 proves
these partial derivatives of $m_{ef}$.

###### Proof.

Expanding the definition of $\bm{\Delta}_{j}^{i}$ in (23), we have

$\bm{\Delta}_{j}^{i}=\begin{bmatrix}\frac{\partial a_{i}}{\partial
x_{j1}}&\frac{\partial b_{i}}{\partial x_{j1}}&\frac{\partial c_{i}}{\partial
x_{j1}}\\\ \vdots&\vdots&\vdots\\\ \frac{\partial a_{i}}{\partial
x_{jm}}&\frac{\partial b_{i}}{\partial x_{jm}}&\frac{\partial c_{i}}{\partial
x_{jm}}\\\ \vdots&\vdots&\vdots\\\ \frac{\partial a_{i}}{\partial
x_{j6}}&\frac{\partial b_{i}}{\partial x_{j6}}&\frac{\partial c_{i}}{\partial
x_{j6}}\end{bmatrix}\in\mathbb{R}^{6\times 6}$ (45)

Substituting the definition of $\bm{\delta}_{jm}^{i}$ in (37) into (45), we
can write $\bm{\Delta}_{j}^{i}$ as

$\bm{\Delta}_{j}^{i}=\begin{bmatrix}{\bm{\delta}_{j1}^{i}}^{T}\\\ \vdots\\\
{\bm{\delta}_{jm}^{i}}^{T}\\\ \vdots\\\
{\bm{\delta}_{j6}^{i}}^{T}\end{bmatrix}$ (46)

Assume $x_{jm}$ is the $m$th variable of $\bm{x}_{j}$. Then $\bm{g}_{j}^{i}$
can be written as

$\bm{g}_{j}^{i}=\frac{\partial\lambda_{i,3}}{\partial\bm{x}_{j}}=\begin{bmatrix}\frac{\partial\lambda_{i,3}}{\partial{x}_{j1}}\\\
\vdots\\\ \frac{\partial\lambda_{i,3}}{\partial{x}_{jm}}\\\ \vdots\\\
\frac{\partial\lambda_{i,3}}{\partial{x}_{j6}}\end{bmatrix}\in\mathbb{R}^{6}.$
(47)

Here $\frac{\partial\lambda_{i,3}}{\partial x_{jm}}$ is the $m$th element of
$\bm{g}_{j}^{i}$. Substituting (46) into $\bm{g}_{j}^{i}$ in (24), we can
obtain the formula of $\frac{\partial\lambda_{i,3}}{\partial x_{jm}}$ as

$\frac{\partial\lambda_{i,3}}{\partial
x_{jm}}=-\varphi_{i}{\bm{\delta}_{jm}^{i}}^{T}\bm{\chi}_{i}=-\varphi_{i}{\bm{\delta}_{jm}^{i}}\cdot\bm{\chi}_{i}.$
(48)

The above formula is what we proved in Lemma 1. Using Lemma 1, we known that
the formula of $\bm{g}_{j}^{i}$ in (24) is correct.

Now we consider the Hessian matrix. According to the definitions of
$\bm{\delta}_{kn}^{i}$, $\bm{\chi}_{i}$, and $\bm{\delta}_{jm}^{i}$, we have

$\small\begin{split}\bm{\delta}_{kn}^{i}&=\begin{bmatrix}\frac{\partial
a_{i}}{\partial x_{kn}}\\\ \frac{\partial b_{i}}{\partial x_{kn}}\\\
\frac{\partial c_{i}}{\partial
x_{kn}}\end{bmatrix},\frac{\partial\bm{\chi}_{i}}{\partial
x_{j,m}}=\begin{bmatrix}2\lambda_{i,3}^{2}\frac{\partial\lambda_{i,3}}{\partial
x_{j,m}}\\\ \frac{\partial\lambda_{i,3}}{\partial x_{j,m}}\\\
0\end{bmatrix},\frac{\partial\bm{\delta}_{jm}^{i}}{\partial
x_{kn}}=\begin{bmatrix}\frac{\partial^{2}a_{i}}{\partial x_{jm}\partial
x_{kn}}\\\ \frac{\partial^{2}b_{i}}{\partial x_{jm}\partial x_{kn}}\\\
\frac{\partial^{2}c_{i}}{\partial x_{jm}\partial
x_{kn}}\end{bmatrix}\end{split}$ (49)

In addition, using the definition of $\varphi_{i}$ in (21), we obtain

$\begin{split}\frac{\partial\varphi_{i}^{-1}}{\partial
x_{km}}&=\bm{\chi}_{i}\cdot\frac{\partial\bm{\kappa}_{i}}{\partial
x_{km}}+\bm{\kappa}_{i}\cdot\frac{\partial\bm{\chi}_{i}}{\partial x_{km}}\\\
&=2\lambda_{i,3}\frac{\partial a_{i}}{\partial x_{kn}}+\frac{\partial
b_{i}}{\partial
x_{kn}}+\left(2a-6\lambda_{i,3}\right)\frac{\partial\lambda_{i,3}}{\partial
x_{kn}}\end{split}$ (50)

Let us denote the entry in the $m$th row and $n$th column of
$\mathbf{H}_{jk}^{i}$ as $\mathbf{H}_{jk}^{i}(m,n)$. Using the formula of
$\mathbf{H}_{jk}^{i}$ in (24) and the variables in (49) and (50), we have

$\small\begin{split}\mathbf{H}_{jk}^{i}(m,n)=&-\underbrace{\varphi_{i}\frac{\partial\lambda_{i,3}}{\partial
x_{kn}}\left(2\lambda_{i,3}\frac{\partial a_{i}}{\partial
x_{jm}}+\frac{\partial b_{i}}{\partial
x_{jm}}\right)}_{\varphi_{i}\bm{\delta}_{kn}^{i}\cdot\frac{\partial\bm{\chi}_{i}}{\partial
x_{jm}}}-\\\
&\underbrace{\varphi_{i}\left(\lambda_{i,3}^{2}\frac{\partial^{2}a_{i}}{\partial
x_{jm}\partial x_{kn}}+\lambda_{i,3}\frac{\partial^{2}b_{i}}{\partial
x_{jm}\partial x_{kn}}+\frac{\partial^{2}c_{i}}{\partial x_{jm}\partial
x_{kn}}\right)}_{\varphi_{i}\bm{\chi}_{i}\cdot\frac{\partial\bm{\delta}_{jm}^{i}}{\partial
x_{kn}}}+\\\ &\underbrace{\varphi_{i}\frac{\partial\lambda_{i,3}}{\partial
x_{jm}}\left(2\lambda_{i,3}\frac{\partial a_{i}}{\partial
x_{kn}}+\frac{\partial b_{i}}{\partial
x_{kn}}+\left(2a-6\lambda_{i,3}\right)\frac{\partial\lambda_{i,3}}{\partial
x_{kn}}\right)}_{\varphi_{i}\frac{\partial\lambda_{i,3}}{\partial
x_{jm}}\frac{\partial\varphi_{i}^{-1}}{\partial x_{km}}}\\\
=&-\varphi_{i}\left(\bm{\delta}_{jm}^{i}\cdot\frac{\partial\bm{\chi}_{i}}{\partial
x_{kn}}+\bm{\chi}_{i}\cdot\frac{\partial\bm{\delta}_{jm}^{i}}{\partial
x_{kn}}-\frac{\partial\lambda_{i,3}}{\partial
x_{jm}}\frac{\partial\varphi_{i}^{-1}}{\partial x_{km}}\right)\end{split}$
(51)

On the other hand, we know

$\mathbf{H}_{jk}^{i}(m,n)=\frac{\partial^{2}\lambda_{i,3}}{\partial
x_{jm}\partial x_{kn}}.$ (52)

Comparing (22) and (51), we know that the formula of $\mathbf{H}_{jk}^{i}$ in
(24) is correct. ∎

###  7.5 Proof of Lemma 3

###### Proof.

For $j\in obs(\bm{\pi}_{i})$ and $k\in obs(\bm{\pi}_{i})$, we take
$\frac{1}{N_{i}}\mathbf{T}_{j}\tilde{\bm{p}}_{ij}$ and
$\frac{1}{N_{i}}\mathbf{T}_{k}\tilde{\bm{p}}_{ik}$ out the summation. Then, we
can write $\bar{\bm{p}}_{i}$ as

$\begin{split}\bar{\bm{p}}_{i}(\bm{x}_{j},\bm{x}_{k})&=\mathbf{T}_{j}\underbrace{\frac{1}{N_{i}}\tilde{\bm{p}}_{ij}}_{\bm{q}_{ij}}+\mathbf{T}_{k}\underbrace{\frac{1}{N_{i}}\tilde{\bm{p}}_{ik}}_{\bm{q}_{ik}}+\underbrace{\frac{1}{N_{i}}\sum_{n\in\mathbb{O}_{jk}}\mathbf{T}_{n}\tilde{\bm{p}}_{in}}_{\bm{c}_{ijk}}.\\\
&=\mathbf{T}_{j}\bm{q}_{ij}+\mathbf{T}_{k}\bm{q}_{ik}+\bm{c}_{ijk}.\end{split}$
(53)

For $j\in obs(\bm{\pi}_{i})$, we can write (53) as

$\begin{split}\bar{\bm{p}}_{i}(\bm{x}_{j},\bm{x}_{k})&=\mathbf{T}_{j}\bm{q}_{ij}+\underbrace{\mathbf{T}_{k}\bm{q}_{ik}+\bm{c}_{ijk}}_{\bm{c}_{ij}}\\\
&=\mathbf{T}_{j}\bm{q}_{ij}+\bm{c}_{ij}\end{split}$ (54)

∎

###  7.6 Proof of Theorem 2

###### Proof.

Substituting (54) into (12) and using the formula of
$\mathbf{S}_{ij}=\mathbf{T}_{j}\mathbf{U}\mathbf{T}_{j}^{T}$ in (13), we have

$\small\begin{split}\mathbf{M}_{i}(\bm{x}_{i})=&\sum_{j\in
obs(\bm{\pi}_{i})}\mathbf{S}_{ij}-N_{i}(\mathbf{T}_{j}\bm{q}_{ij}+\bm{c}_{ij})(\mathbf{T}_{j}\bm{q}_{ij}+\bm{c}_{ij})^{T}\\\
=&\mathbf{S}_{ij}-\mathbf{T}_{j}\left(N_{i}\bm{q}_{ij}\bm{q}_{ij}^{T}\right)\mathbf{T}_{j}^{T}-\mathbf{T}_{j}\underbrace{\left(N_{j}\bm{q}_{ij}\bm{c}_{ij}^{T}\right)}_{-\mathbf{K}_{j}^{i}}-\\\
&\underbrace{\left(N_{j}\bm{c}_{ij}\bm{p}^{T}_{ij}\right)}_{\left(-\mathbf{K}_{j}^{i}\right)^{T}}\mathbf{T}_{j}^{T}+\underbrace{\sum_{\begin{subarray}{c}n\in
obs(\bm{\pi}_{i})\\\ n\neq
j\end{subarray}}\mathbf{S}_{in}-N_{j}\bm{c}_{ij}\bm{c}_{ij}^{T}}_{\mathbf{C}_{j}^{i}}\\\
=&\mathbf{T}_{j}\mathbf{U}_{ij}\mathbf{T}_{j}^{T}-\mathbf{T}_{j}\left(N_{i}\bm{q}_{ij}\bm{q}_{ij}^{T}\right)\mathbf{T}_{j}^{T}+\mathbf{T}_{j}\mathbf{K}_{j}^{i}+\left(\mathbf{K}_{j}^{i}\right)^{T}\mathbf{T}_{j}^{T}+\mathbf{C}_{j}^{i}\\\
=&\mathbf{T}_{j}\underbrace{\left(\mathbf{U}_{ij}-N_{j}\bm{q}_{ij}\bm{q}_{ij}^{T}\right)}_{\mathbf{Q}_{j}^{i}}\mathbf{T}_{j}^{T}+\mathbf{T}_{j}\mathbf{K}_{j}^{i}+\left(\mathbf{K}_{j}^{i}\right)^{T}\mathbf{T}_{j}^{T}+\mathbf{C}_{j}^{i}\\\
=&\mathbf{T}_{j}\mathbf{Q}_{j}^{i}\mathbf{T}_{j}^{T}+\mathbf{T}_{j}\mathbf{K}_{j}^{i}+(\mathbf{K}_{j}^{i})^{T}\mathbf{T}_{j}^{T}+\mathbf{C}_{j}^{i}\end{split}$

Thus we get the formula of $\mathbf{M}_{i}(\bm{x}_{j})$ in (28). Now let us
prove (29). Let us define

$\begin{split}\mathbf{E}_{j}^{i}&=\mathbf{T}_{j}\bm{q}_{ij}\left(\mathbf{T}_{j}\bm{q}_{ij}+\bm{c}_{ijk}\right)^{T}\\\
\mathbf{E}_{k}^{i}&=\mathbf{T}_{k}\bm{q}_{ik}\left(\mathbf{T}_{k}\bm{q}_{ik}+\bm{c}_{ijk}\right)^{T}\end{split}$
(55)

Substituting (53) into (12), we obtain

$\begin{split}\mathbf{M}_{i}(\bm{x}_{j},\bm{x}_{k})=&\mathbf{T}_{j}\underbrace{\left(-N\bm{q}_{ij}\bm{q}_{ik}^{T}\right)}_{\mathbf{O}_{j}^{i}}\mathbf{T}_{k}^{T}+\mathbf{T}_{j}\underbrace{\left(-N\bm{q}_{ik}\bm{q}_{ij}^{T}\right)}_{\left(\mathbf{O}_{j}^{i}\right)^{T}}\mathbf{T}_{k}^{T}+\\\
&\underbrace{\sum_{j\in
obs(\bm{\pi}_{i})}\mathbf{S}_{ij}-N_{i}\left(\mathbf{E}_{j}^{i}+\mathbf{E}_{k}^{i}+\bm{c}_{ijk}\bar{\bm{p}}_{i}(\bm{x}_{j},\bm{x}_{k})^{T}\right)}_{\mathbf{C}_{jk}^{i}}\\\
=&\mathbf{T}_{j}\mathbf{O}_{jk}^{i}\mathbf{T}_{k}^{T}+\mathbf{T}_{k}{(\mathbf{O}_{jk}^{i})}^{T}\mathbf{T}_{j}^{T}+\mathbf{C}_{jk}^{i}\end{split}$
(56)

Thus we get the formula of $\mathbf{M}_{i}(\bm{x}_{j},\bm{x}_{k})$ in (29). ∎

###  7.7 Partial Derivatives of Entries in Mi

As illustrated in Fig. 7, the derivatives of $a_{i}$, $b_{i}$ and $c_{i}$ in
(23) are the crux to get $\bm{g}_{j}^{i}$, $\mathbf{H}_{jj}^{i}$, and
$\mathbf{H}_{jk}^{i}$. The $a_{i}$, $b_{i}$ and $c_{i}$ in (19) are first-,
second-, and third-order polynomials with respect to the elements in
$\mathbf{M}_{i}$, respectively. Let us denote the $e$th row and $f$th column
entry of $\mathbf{M}_{i}$ as $m_{ef}$. According to the chain rule in
calculus, to compute the partial derivatives in (23), we have to calculate

$\frac{\partial
m_{ef}}{\partial\bm{x}_{j}},\frac{\partial^{2}m_{ef}}{\partial\bm{x}_{j}^{2}},\
\text{and}\ \frac{\partial^{2}m_{ef}}{\partial\bm{x}_{j}\partial\bm{x}_{k}}.$
(57)

From our paper, we know that we only need to compute their value at
$\bm{x}_{0}=[0;0;0;0;0;0]$. Assume ${q}_{ef}$, ${k}_{ef}$, and $o_{ef}$ are
the $e$th row and $f$th column entries of $\mathbf{Q}_{j}^{i}$,
$\mathbf{K}_{j}^{i}$, and $\mathbf{O}_{jk}^{i}$, respectively. Then, the
values of $\frac{\partial m_{ef}}{\partial\bm{x}_{j}}$,
$\frac{\partial^{2}m_{ef}}{\partial\bm{x}_{j}\partial\bm{x}_{k}}$, and
$\frac{\partial^{2}m_{ef}}{\partial\bm{x}_{j}^{2}}$ at $\bm{x}_{0}$ have the
forms in Table 1, Table 2, and Table 3, respectively.

$\left.\frac{\partial m_{11}}{\partial\bm{x}_{j}}\right|_{\bm{x}_{j}=\bm{x}_{0}}=\begin{bmatrix}0\\\ -2k_{31}-2q_{13}\\\ 2k_{21}+2q_{12}\\\ -4k_{32}-4q_{23}\\\ 2k_{22}-2k_{33}+2q_{22}-2q_{33}\\\ 4k_{23}+4q_{23}\\\ \end{bmatrix}$ | $\left.\frac{\partial m_{12}}{\partial\bm{x}_{j}}\right|_{\bm{x}_{j}=\bm{x}_{0}}=\begin{bmatrix}4k_{31}+4q_{13}\\\ 2k_{32}+2q_{23}\\\ 2k_{33}-2k_{11}-2q_{11}+2q_{33}\\\ 0\\\ -2k_{12}-2q_{12}\\\ -4k_{13}-4q_{13}\\\ \end{bmatrix}$
---|---
$\left.\frac{\partial m_{13}}{\partial\bm{x}_{j}}\right|_{\bm{x}_{j}=\bm{x}_{0}}=\begin{bmatrix}-4k_{21}-4q_{12}\\\ 2k_{11}-2k_{22}+2q_{11}-2q_{22}\\\ -2k_{23}-2q_{23}\\\ 4k_{12}+4q_{12}\\\ 2k_{13}+2q_{13}\\\ 0\\\ \end{bmatrix}$ | $\left.\frac{\partial m_{22}}{\partial\bm{x}_{j}}\right|_{\bm{x}_{j}=\bm{x}_{0}}=\begin{bmatrix}2k_{41}+2q_{14}\\\ k_{42}+q_{24}\\\ k_{43}+q_{34}\\\ 0\\\ 0\\\ 0\\\ \end{bmatrix}$
$\left.\frac{\partial m_{23}}{\partial\bm{x}_{j}}\right|_{\bm{x}_{j}=\bm{x}_{0}}=\begin{bmatrix}0\\\ k_{41}+q_{14}\\\ 0\\\ 2k_{42}+2q_{24}\\\ k_{43}+q_{34}\\\ 0\end{bmatrix}$ | $\left.\frac{\partial m_{33}}{\partial\bm{x}_{j}}\right|_{\bm{x}_{j}=\bm{x}_{0}}=\begin{bmatrix}0\\\ 0\\\ k_{41}+q_{14}\\\ 0\\\ k_{42}+q_{24}\\\ 2k_{43}+2q_{34}\end{bmatrix}$
Table 1: $\frac{\partial m_{ef}}{\partial\bm{x}_{j}}$ at $\bm{x}_{j}=\bm{x}_{0}$.  $\left.\frac{\partial^{2}m_{11}}{\partial\bm{x}_{j}\partial\bm{x}_{k}}\right|_{\begin{subarray}{c}\bm{x}_{j}=\bm{x}_{0}\\\ \bm{x}_{k}=\bm{x}_{0}\end{subarray}}=\begin{bmatrix}0&0&0&0&0&0\\\ 0&8o_{33}&-8o_{32}&4o_{34}&0&0\\\ 0&-8o_{23}&8o_{22}&-4o_{24}&0&0\\\ 0&4o_{43}&-4o_{42}&2o_{44}&0&0\\\ 0&0&0&0&0&0\\\ 0&0&0&0&0&0\\\ \end{bmatrix}$ | $\left.\frac{\partial^{2}m_{13}}{\partial\bm{x}_{j}\partial\bm{x}_{k}}\right|_{\begin{subarray}{c}\bm{x}_{j}=\bm{x}_{0}\\\ \bm{x}_{k}=\bm{x}_{0}\end{subarray}}=\begin{bmatrix}0&4o_{23}&-4o_{22}&2o_{24}&0&0\\\ 4o_{32}&-4o_{13}-4o_{31}&4o_{12}&-2o_{14}&0&2o_{34}\\\ -4o_{22}&4o_{21}&0&0&0&-2o_{24}\\\ 2o_{42}&-2o_{41}&0&0&0&o_{44}\\\ 0&0&0&0&0&0\\\ 0&2o_{43}&-2o_{42}&o_{44}&0&0\\\ \end{bmatrix}$
---|---
$\left.\frac{\partial^{2}m_{22}}{\partial\bm{x}_{j}\partial\bm{x}_{k}}\right|_{\begin{subarray}{c}\bm{x}_{j}=\bm{x}_{0}\\\ \bm{x}_{k}=\bm{x}_{0}\end{subarray}}=\begin{bmatrix}8o_{33}&0&-8o_{31}&0&-4o_{34}&0\\\ 0&0&0&0&0&0\\\ -8o_{13}&0&8o_{11}&0&4o_{14}&0\\\ 0&0&0&0&0&0\\\ -4o_{43}&0&4o_{41}&0&2o_{44}&0\\\ 0&0&0&0&0&0\\\ \end{bmatrix}$ | $\left.\frac{\partial^{2}m_{12}}{\partial\bm{x}_{j}\partial\bm{x}_{k}}\right|_{\begin{subarray}{c}\bm{x}_{j}=\bm{x}_{0}\\\ \bm{x}_{k}=\bm{x}_{0}\end{subarray}}=\begin{bmatrix}0&-4o_{33}&4o_{32}&-2o_{34}&0&0\\\ -4o_{33}&0&4o_{31}&0&2o_{34}&0\\\ 4o_{23}&4o_{13}&-4o_{12}-4o_{21}&2o_{14}&-2o_{24}&0\\\ -2o_{43}&0&2o_{41}&0&o_{44}&0\\\ 0&2o_{43}&-2o_{42}&o_{44}&0&0\\\ 0&0&0&0&0&0\\\ \end{bmatrix}$
$\left.\frac{\partial^{2}m_{33}}{\partial\bm{x}_{j}\partial\bm{x}_{k}}\right|_{\begin{subarray}{c}\bm{x}_{j}=\bm{x}_{0}\\\ \bm{x}_{k}=\bm{x}_{0}\end{subarray}}=\begin{bmatrix}8o_{22}&-8o_{21}&0&0&0&4o_{24}\\\ -8o_{12}&8o_{11}&0&0&0&-4o_{14}\\\ 0&0&0&0&0&0\\\ 0&0&0&0&0&0\\\ 0&0&0&0&0&0\\\ 4o_{42}&-4o_{41}&0&0&0&2o_{44}\\\ \end{bmatrix}$ | $\left.\frac{\partial^{2}m_{23}}{\partial\bm{x}_{j}\partial\bm{x}_{k}}\right|_{\begin{subarray}{c}\bm{x}_{j}=\bm{x}_{0}\\\ \bm{x}_{k}=\bm{x}_{0}\end{subarray}}=\begin{bmatrix}-4o_{23}-4o_{32}&4o_{31}&4o_{21}&0&2o_{24}&-2o_{34}\\\ 4o_{13}&0&-4o_{11}&0&-2o_{14}&0\\\ 4o_{12}&-4o_{11}&0&0&0&2o_{14}\\\ 0&0&0&0&0&0\\\ 2o_{42}&-2o_{41}&0&0&0&o_{44}\\\ -2o_{43}&0&2o_{41}&0&o_{44}&0\\\ \end{bmatrix}$
Table 2: $\frac{\partial^{2}m_{ef}}{\partial\bm{x}_{j}\partial\bm{x}_{k}}$ at
$\bm{x}_{j}=\bm{x}_{0}$ and $\bm{x}_{k}=\bm{x}_{0}$.
$\left.\frac{\partial^{2}m_{11}}{\partial\bm{x}_{j}^{2}}\right|_{\bm{x}_{j}=\bm{x}_{0}}=\begin{bmatrix}0&4k_{21}+4q_{12}&4k_{31}+4q_{13}&0&0&0\\\
4k_{21}+4q_{12}&8q_{33}-8q_{11}-8k_{11}&-8q_{23}&4q_{34}&0&0\\\
4k_{31}+4q_{13}&-8q_{23}&8q_{22}-8q_{11}-8k_{11}&-4q_{24}&0&0\\\
0&4q_{34}&-4q_{24}&2q_{44}&0&0\\\ 0&0&0&0&0&0\\\ 0&0&0&0&0&0\end{bmatrix}$
---
$\left.\frac{\partial^{2}m_{12}}{\partial\bm{x}_{j}^{2}}\right|_{\bm{x}_{j}=\bm{x}_{0}}=\begin{bmatrix}-4k_{21}-4q_{12}&2k_{11}+2k_{22}+2q_{11}+2q_{22}-4q_{33}&2k_{32}+6q_{23}&-2q_{34}&0&0\\\
2k_{11}+2k_{22}+2q_{11}+2q_{22}-4q_{33}&-4k_{12}-4q_{12}&2k_{31}+6q_{13}&0&2q_{34}&0\\\
2k_{32}+6q_{23}&2k_{31}+6q_{13}&-4k_{12}-4k_{21}-16q_{12}&2q_{14}&-2q_{24}&0\\\
-2q_{34}&0&2q_{14},0&q_{44},0\\\ 0&2q_{34}&-2q_{24}&q_{44}&0&0\\\
0&0&0&0&0&0\\\ \end{bmatrix}$
$\left.\frac{\partial^{2}m_{13}}{\partial\bm{x}_{j}^{2}}\right|_{\bm{x}_{j}=\bm{x}_{0}}=\begin{bmatrix}-4k_{31}-4q_{13}&2k_{23}+6q_{23}&2k_{11}+2k_{33}+2q_{11}-4q_{22}+2q_{33}&2q_{24}&0&0\\\
2k_{23}+6q_{23}&-4k_{13}-4k_{31}-16q_{13}&2k_{21}+6q_{12}&-2q_{14}&0&2q_{34}\\\
2k_{11}+2k_{33}+2q_{11}-4q_{22}+2q_{33}&2k_{21}+6q_{12}&-4k_{13}-4q_{13}&0&0&-2q_{24}\\\
2q_{24}&-2q_{14}&0&0&0&q_{44}\\\ 0&0&0&0&0&0\\\
0&2q_{34}&-2q_{24}&q_{44}&0&0\\\ \end{bmatrix}$
$\left.\frac{\partial^{2}m_{22}}{\partial\bm{x}_{j}^{2}}\right|_{\bm{x}_{j}=\bm{x}_{0}}=\begin{bmatrix}8q_{33}-8q_{22}-8k_{22}&4k_{12}+4q_{12}&-8q_{13}&0&-4q_{34}&0\\\
4k_{12}+4q_{12}&0&4k_{32}+4q_{23}&0&0&0\\\
-8q_{13}&4k_{32}+4q_{23}&8q_{11}-8k_{22}-8q_{22}&0&4q_{14}&0\\\ 0&0&0&0&0&0\\\
-4q_{34}&0&4q_{14}&0&2q_{44}&0\\\ 0&0&0&0&0&0\\\ \end{bmatrix}$
$\left.\frac{\partial^{2}m_{23}}{\partial\bm{x}_{j}^{2}}\right|_{\bm{x}_{j}=\bm{x}_{0}}=\begin{bmatrix}-4k_{23}-4k_{32}-16q_{23}&2k_{13}+6q_{13}&2k_{12}+6q_{12}&0&2q_{24}&-2q_{34}\\\
2k_{13}+6q_{13}&-4k_{32}-4q_{23}&2k_{22}+2k_{33}-4q_{11}+2q_{22}+2q_{33}&0&-2q_{14}&0\\\
2k_{12}+6q_{12}&2k_{22}+2k_{33}-4q_{11}+2q_{22}+2q_{33}&-4k_{23}-4q_{23}&0&0&2q_{14}1\\\
0&0&0&0&0&0\\\ 2q_{24}&-2q_{14}&0&0&0&q_{44}\\\
-2q_{34}&0&2q_{14}&0&q_{44}&0\\\ \end{bmatrix}$
$\left.\frac{\partial^{2}m_{33}}{\partial\bm{x}_{j}^{2}}\right|_{\bm{x}_{j}=\bm{x}_{0}}=\begin{bmatrix}8q_{22}-8k_{33}-8q_{33}&-8q_{12}&4k_{13}+4q_{13}&0&0&4q_{24}\\\
-8q_{12}&8q_{11}-8k_{33}-8q_{33}&4k_{23}+4q_{23}&0&0&-4q_{14}\\\
4k_{13}+4q_{13}&4k_{23}+4q_{23}&0&0&0&0\\\ 0&0&0&0&0&0\\\ 0&0&0&0&0&0\\\
4q_{24}&-4q_{14}&0&0&0&2q_{44}\\\ \end{bmatrix}$
Table 3: $\frac{\partial^{2}m_{ef}}{\partial\bm{x}_{j}^{2}}$ at
$\bm{x}_{j}=\bm{x}_{0}$.

## References

  * [1] Implicit Funtion Theorem.  https://en.wikipedia.org/wiki/Implicit_function_theorem.
  * [2] Sameer Agarwal, Yasutaka Furukawa, Noah Snavely, Ian Simon, Brian Curless, Steven M Seitz, and Richard Szeliski.  Building rome in a day.  Communications of the ACM, 54(10):105–112, 2011.
  * [3] Sameer Agarwal, Keir Mierle, and The Ceres Solver Team.  Ceres Solver, 3 2022.
  * [4] Sameer Agarwal, Noah Snavely, Steven M Seitz, and Richard Szeliski.  Bundle adjustment in the large.  In European conference on computer vision, pages 29–42. Springer, 2010.
  * [5] Alex M Andrew.  Multiple view geometry in computer vision.  Kybernetes, 2001.
  * [6] Carlos Campos, Richard Elvira, Juan J Gómez Rodríguez, José MM Montiel, and Juan D Tardós.  Orb-slam3: An accurate open-source library for visual, visual–inertial, and multimap slam.  IEEE Transactions on Robotics, 2021.
  * [7] Danpeng Chen, Shuai Wang, Weijian Xie, Shangjin Zhai, Nan Wang, Hujun Bao, and Guofeng Zhang.  Vip-slam: An efficient tightly-coupled rgb-d visual inertial planar slam.  In 2022 International Conference on Robotics and Automation (ICRA), pages 5615–5621. IEEE, 2022.
  * [8] Nikolaus Demmel, David Schubert, Christiane Sommer, Daniel Cremers, and Vladyslav Usenko.  Square root marginalization for sliding-window bundle adjustment.  In Proceedings of the IEEE/CVF International Conference on Computer Vision, pages 13260–13268, 2021.
  * [9] Nikolaus Demmel, Christiane Sommer, Daniel Cremers, and Vladyslav Usenko.  Square root bundle adjustment for large-scale reconstruction.  In Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, pages 11723–11732, 2021.
  * [10] Hakim Elchaoui Elghor, David Roussel, Fakhreddine Ababsa, and El Houssine Bouyakhf.  Planes detection for robust localization and mapping in rgb-d slam systems.  In 2015 International Conference on 3D Vision, pages 452–459. IEEE, 2015.
  * [11] Gonzalo Ferrer.  Eigen-factors: Plane estimation for multi-frame and time-continuous point cloud alignment.  In 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 1278–1284. IEEE, 2019.
  * [12] Joel A Hesch and Stergios I Roumeliotis.  A direct least-squares (dls) method for pnp.  In 2011 International Conference on Computer Vision, pages 383–390. IEEE, 2011.
  * [13] Ming Hsiao, Eric Westman, Guofeng Zhang, and Michael Kaess.  Keyframe-based dense planar slam.  In 2017 IEEE International Conference on Robotics and Automation (ICRA), pages 5110–5117. IEEE, 2017.
  * [14] Michael Kaess.  Simultaneous localization and mapping with infinite planes.  In 2015 IEEE International Conference on Robotics and Automation (ICRA), pages 4605–4611. IEEE, 2015.
  * [15] Pyojin Kim, Brian Coltin, and H Jin Kim.  Linear rgb-d slam for planar environments.  In Proceedings of the European Conference on Computer Vision (ECCV), pages 333–348, 2018.
  * [16] Steven George Krantz and Harold R Parks.  The implicit function theorem: history, theory, and applications.  Springer Science & Business Media, 2002.
  * [17] Zheng Liu and Fu Zhang.  BALM: Bundle Adjustment for LiDAR Mapping.  IEEE Robotics and Automation Letters, 6(2):3184–3191, 2021.
  * [18] Jorge J Moré.  The levenberg-marquardt algorithm: implementation and theory.  In Numerical analysis, pages 105–116. Springer, 1978.
  * [19] Raul Mur-Artal, Jose Maria Martinez Montiel, and Juan D Tardos.  Orb-slam: a versatile and accurate monocular slam system.  IEEE transactions on robotics, 31(5):1147–1163, 2015.
  * [20] Johannes L Schonberger and Jan-Michael Frahm.  Structure-from-motion revisited.  In Proceedings of the IEEE conference on computer vision and pattern recognition, pages 4104–4113, 2016.
  * [21] Bill Triggs, Philip F McLauchlan, Richard I Hartley, and Andrew W Fitzgibbon.  Bundle adjustment—a modern synthesis.  In International workshop on vision algorithms, pages 298–372. Springer, 1999.
  * [22] Ji Zhang and Sanjiv Singh.  Loam: Lidar odometry and mapping in real-time.  In Robotics: Science and Systems, volume 2, 2014.
  * [23] Lipu Zhou, Guoquan Huang, Yinian Mao, Jincheng Yu, Shengze Wang, and Michael Kaess.  Plc-lislam: Lidar slam with planes, lines and cylinders.  IEEE Robotics and Automation Letters, 2022.
  * [24] Lipu Zhou, Daniel Koppel, Hul Ju, Frank Steinbruecker, and Michael Kaess.  An efficient planar bundle adjustment algorithm.  In 2020 IEEE International Symposium on Mixed and Augmented Reality (ISMAR), pages 136–145, 2020.
  * [25] Lipu Zhou, Daniel Koppel, and Michael Kaess.  Lidar slam with plane adjustment for indoor environment.  IEEE Robotics and Automation Letters, 6(4):7073–7080, 2021.
  * [26] Lei Zhou, Zixin Luo, Mingmin Zhen, Tianwei Shen, Shiwei Li, Zhuofei Huang, Tian Fang, and Long Quan.  Stochastic bundle adjustment for efficient and scalable 3d reconstruction.  In European Conference on Computer Vision, pages 364–379. Springer, 2020.
  * [27] Lipu Zhou, Shengze Wang, and Michael Kaess.  $\pi$-lsam: Lidar smoothing and mapping with planes.  In 2021 IEEE International Conference on Robotics and Automation (ICRA), pages 5751–5757. IEEE, 2021.