Umeyama算法、ICP算法、Sim3优化闭式解
1.问题定义和公式推导Umeyama算法用于对齐两条轨迹（位置坐标对齐），ICP算法用于点云配准，Sim3优化用于回环检测减小漂移，三个不同的算法在闭式求解上有一个统一的优美的解决方法。ICP算法中，通常s固定为1，此时等同于Umeyama算法中的so3对齐，Sim3优化也是利用三维点对应来计算相对的位姿，同时优化尺度s，来减小漂移，设Umeyama一般性问题（se3对齐）定义如下：
设 \boldsymbol{x} 是需要评估的轨迹的位置坐标， \boldsymbol{y} 是gt的轨迹的位置坐标，此时需要找到一个尺度s，旋转矩阵 \boldsymbol{R} ，平移向量 \boldsymbol{t} ，使得待评估的轨迹和gt可以对齐，用公式可以写成如下形式：
\boldsymbol{y}=s\boldsymbol{R}\boldsymbol{x}+\boldsymbol{t} \\
定义误差函数为：
\boldsymbol{e}_i=\boldsymbol{y}_i - (s\boldsymbol{R}\boldsymbol{x}_i+\boldsymbol{t}) \\
\boldsymbol{y}_i,\boldsymbol{x}_i 分别表示gt的第i个pose的位置坐标以及待评估轨迹的第i个pose的位置坐标。
将所有的点对应写成最小二乘形式可以得到：
\min_{s,\boldsymbol{R},\boldsymbol{t}}\frac{1}{2}\sum_{i=1}^{n}\|{\boldsymbol{e}_i}\|^2_2=\min_{s,\boldsymbol{R},\boldsymbol{t}}\frac{1}{2}\sum_{i=1}^{n}\|\boldsymbol{y}_i - (s\boldsymbol{R}\boldsymbol{x}_i+\boldsymbol{t})\|^2_2\\
该问题就是要找到一组 s,\boldsymbol{R},\boldsymbol{t} 使得总体误差函数的二范式最小。
为了分析和求解该问题，首先定义轨迹 \boldsymbol{x} 和轨迹 \boldsymbol{y} 的均值（或中心）为：
\boldsymbol{\bar{x}} = \frac{1}{n}\sum_{i=1}^{n}\boldsymbol{{x}_i}\\
\boldsymbol{\bar{y}} = \frac{1}{n}\sum_{i=1}^{n}\boldsymbol{{y}_i}\\
此时总体目标函数可以表示为：
\begin{array}{l} \frac{1}{2}\sum_{i=1}^{n}\|\boldsymbol{y}_i - (s\boldsymbol{R}\boldsymbol{x}_i+\boldsymbol{t})\|^2&=\frac{1}{2}\sum_{i=1}^{n}\|\boldsymbol{y}_i - s\boldsymbol{R}\boldsymbol{x}_i-\boldsymbol{t}-\boldsymbol {\bar{y}}+s\boldsymbol{R}\boldsymbol{\bar x}+\boldsymbol {\bar{y}}-s\boldsymbol{R}\boldsymbol{\bar x}\|^2\\ &=\frac{1}{2}\sum_{i=1}^{n}\|(\boldsymbol{y}_i - \boldsymbol {\bar{y}}- s\boldsymbol{R}(\boldsymbol{x}_i-\boldsymbol{\bar x}))+(\boldsymbol{\bar y} - s\boldsymbol{R}\boldsymbol{\bar x}-\boldsymbol{t})\|^2\\ &=\frac{1}{2}\sum_{i=1}^{n} \left(\|\boldsymbol{y}_i - \boldsymbol {\bar{y}}- s\boldsymbol{R}(\boldsymbol{x}_i-\boldsymbol{\bar x})\|^2+\|\boldsymbol{\bar y} - s\boldsymbol{R}\boldsymbol{\bar x}-\boldsymbol{t}\|^2+2(\boldsymbol{y}_i - \boldsymbol {\bar{y}}- s\boldsymbol{R}(\boldsymbol{x}_i-\boldsymbol{\bar x}))^T(\boldsymbol{\bar y} - s\boldsymbol{R}\boldsymbol{\bar x}-\boldsymbol{t})\right) \end{array} \\
对于目标函数中的求和第三项，由于 (\boldsymbol{\bar y} - s\boldsymbol{R}\boldsymbol{\bar x}-\boldsymbol{t}) 可以当做公因子提出来，而 \boldsymbol{y}_i - \boldsymbol {\bar{y}}- s\boldsymbol{R}(\boldsymbol{x}_i-\boldsymbol{\bar x}))^T 求和之后等于 \boldsymbol{0}^T ，于是目标函数可以简化为如下形式：
\min_{s,\boldsymbol{R},\boldsymbol{t}}{F}=\frac{1}{2}\sum_{i=1}^{n} \left(\|\boldsymbol{y}_i - \boldsymbol {\bar{y}}- s\boldsymbol{R}(\boldsymbol{x}_i-\boldsymbol{\bar x})\|^2+\|\boldsymbol{\bar y} - s\boldsymbol{R}\boldsymbol{\bar x}-\boldsymbol{t}\|^2\right)\\
由于目标函数中的第一项只和 s,\boldsymbol{R} 有关，第二项才和 \boldsymbol{t} 有关，也就是说，对于每一组 s,\boldsymbol{R} ，我们都能找到一个 \boldsymbol{t} ，使得 \boldsymbol{\bar y} - s\boldsymbol{R}\boldsymbol{\bar x}-\boldsymbol{t}=\boldsymbol0^T ，此时有：
\boldsymbol{t} = \boldsymbol{\bar y} - s\boldsymbol{R}\boldsymbol{\bar x} \\
那么只需要保证目标函数第一项取得最小值即可，
\begin{array}{l} \min_{s,\boldsymbol{R}} {F}=\frac{1}{2}\sum_{i=1}^{n} \|\boldsymbol{y}_i - \boldsymbol {\bar{y}}- s\boldsymbol{R}(\boldsymbol{x}_i-\boldsymbol{\bar x})\|^2 \end{array} \\
定义每个点距离中心的坐标偏移为：
\boldsymbol{x^{'}}=\boldsymbol{x}_i - \boldsymbol {\bar{x}}\\
\boldsymbol{y^{'}}=\boldsymbol{y}_i - \boldsymbol {\bar{y}}\\
此时目标函数可以进一步写成：
\frac{1}{2}\sum_{i=1}^{n} \|\boldsymbol{y^{’}}_i - s\boldsymbol{R}\boldsymbol{x^{'}}_i\|^2=\frac{1}{2}\sum_{i=1}^{n} \left(\|\boldsymbol{y^{’}}_i\|^2-2s{\boldsymbol{y^{’}}_i}^T\boldsymbol{R}\boldsymbol{x^{'}}_i+s^2\|\boldsymbol{R}\boldsymbol{x^{'}}_i\|^2\right) \\
将目标函数都除以常数n，不会改变目标函数的极值，定义 \boldsymbol{x}，\boldsymbol{y} 的方差和协方差如下，同时对 \boldsymbol{H} 做SVD分解\sigma_y=\frac{1}{n}\sum_{i=1}^{n} \|\boldsymbol{y^{’}}_i\|^2 \\
\sigma_x=\frac{1}{n}\sum_{i=1}^{n} \|\boldsymbol{Rx^{’}}_i\|^2=\frac{1}{n}\sum_{i=1}^{n} \|\boldsymbol{x^{’}}_i\|^2\\
{D}=\frac{1}{n}\sum_{i=1}^{n}{\boldsymbol{y^{’}}_i}^T\boldsymbol{R}\boldsymbol{x^{'}}_i=\frac{1}{n}\sum_{i=1}^{n} \mathrm{tr}(\boldsymbol{R}\boldsymbol{x^{'}}_i {\boldsymbol{y^{’}}_i}^T) =\mathrm{tr}\left(\boldsymbol{R}*\frac{1}{n}\sum_{i=1}^{n} {\boldsymbol{x^{'}}_i\boldsymbol{y^{’}}_i}^T\right) \\
\boldsymbol{H}=\frac{1}{n}\sum_{i=1}^{n} {\boldsymbol{x^{'}}_i\boldsymbol{y^{’}}_i}^T=\boldsymbol{U\Sigma V^T}\\
注：矩阵 D 的相关变换推导可以查阅矩阵的迹的相关计算公式
于是目标函数可以写成：
\begin{array}{l} \min_{s,\boldsymbol{R}} {F}&=\sigma_y-2s{D}+s^2\sigma_x\\ &=(s\sqrt{\sigma_x}-{D}/\sqrt{\sigma_x})^2+(\sigma_y\sigma_x-{D}^2)/\sigma_x \end{array}\\
和 \boldsymbol{t} 的求解同理，第一项包含 s,\boldsymbol{R} ，对于每一个 \boldsymbol{R} ，都能找到一个 s 使得第一项等于0，同时第二项只和 \boldsymbol{R} 有关（矩阵 \boldsymbol{D} 只和 \boldsymbol{R} 有关），因此只需要找到一个让第二项最小的 \boldsymbol{R} ，就能使目标函数最小，此时有：
s\sqrt{\sigma_x}-D/\sqrt{\sigma_x}=0\\
s=\frac{D}{\sigma_x}\\
\max_R D =\max_R \mathrm{tr}\left(\boldsymbol{RH}\right)\\
\mathrm{tr} (\boldsymbol {RH}) =\mathrm{tr} (\boldsymbol {RU\Sigma V^T}) =\mathrm{tr} (\boldsymbol {\Sigma V^TRU})\\
定义矩阵
\boldsymbol{M}=\boldsymbol {V^TRU}= \begin{bmatrix} m_{11} & m_{12} & m_{13} \\ m_{21} & m_{22} & m_{23} \\ m_{31} & m_{32} & m_{33} \end{bmatrix}\\
于是有
\mathrm{tr} (\boldsymbol {\Sigma V^TRU})=\mathrm{tr} (\boldsymbol {\Sigma M})=\sigma_1 m_{11}+ \sigma_2 m_{22}+\sigma_3 m_{33}\\
由于 \boldsymbol {V^T}、\boldsymbol {R}、\boldsymbol {U} 都是正交矩阵，因此 \boldsymbol {M} 也是正交矩阵， \boldsymbol {M}\boldsymbol {}矩阵的所有元素都小于1。
\det(\boldsymbol{M})=\det(\boldsymbol{V^T})\det(\boldsymbol{U})\det(\boldsymbol{R})=\det(\boldsymbol{V^T})\det(\boldsymbol{U})=\pm 1 \\
当 \det({\boldsymbol {M}})=1 时，若 m_{11}=m_{22}=m_{33}=1 ，此时 \sigma_1 m_{11}+ \sigma_2 m_{22}+\sigma_3 m_{33}\ 显然取得最大值（ \sigma_1 > \sigma_2 >\sigma_3 >=0 ），此时有：
\boldsymbol {V^TRU}=\boldsymbol {I} \\
\boldsymbol {R}=\boldsymbol {VU^T}\\
当 \det(\boldsymbol{M})=- 1时，若 m_{11}=m_{22}=1，m_{33}=-1 ，此时 \sigma_1 m_{11}+ \sigma_2 m_{22}+\sigma_3 m_{33}\ 取得最大值，这里的证明省略，可参考论文证明，此时有：
\boldsymbol {M} = \begin{bmatrix} 1& 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & -1 \end{bmatrix} \\
于是联合两种形式，可以得到：
\boldsymbol {R^{*}}=\boldsymbol {V} \begin{bmatrix} 1& 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \det(\boldsymbol {VU^T} )\end{bmatrix}\boldsymbol {U^T}\\
\boldsymbol{D^{*}}=\sum_{i=1}^{n}{\boldsymbol{y^{’}}_i}^T\boldsymbol{R^{*}}\boldsymbol{x^{'}}_i=\mathrm{tr}(\boldsymbol{R^{*}H})= \mathrm{tr}(\boldsymbol{M\Sigma})\\
得到旋转矩阵 \boldsymbol {R} 的最优解之后，然后回代可以计算得到 s，\boldsymbol{t} ，到此计算结束。
2.完整的求解步骤如下：
[*]分别计算两组点的质心(均值 \boldsymbol{\bar{x}}、 \boldsymbol{\bar{y}}) 、去质心坐标( \boldsymbol{x^{'}} 、 \boldsymbol{y^{'}} )
\boldsymbol{\bar{x}} = \frac{1}{n}\sum_{i=1}^{n}\boldsymbol{{x}_i} ，\boldsymbol{\bar{y}} = \frac{1}{n}\sum_{i=1}^{n}\boldsymbol{{y}_i}， \boldsymbol{x^{'}}=\boldsymbol{x}_i - \boldsymbol {\bar{x}} ， \boldsymbol{y^{'}}=\boldsymbol{y}_i - \boldsymbol {\bar{y}} ，
2.分别计算两组点的方差系数()协方差矩阵H
\sigma_x=\frac{1}{n}\sum_{i=1}^{n} \|\boldsymbol{x^{’}}_i\|^2 ，
\sigma_y=\frac{1}{n}\sum_{i=1}^{n} \|\boldsymbol{y^{’}}_i\|^2
\boldsymbol{H}=\frac{1}{n}\sum_{i=1}^{n} {\boldsymbol{x^{'}}_i\boldsymbol{y^{’}}_i}^T
3.对 \boldsymbol{H} 进行SVD分解，求解最优旋转矩阵 \boldsymbol {R^{*}}
\boldsymbol{H}=\boldsymbol{U\Sigma V^T}
\boldsymbol {M} = \begin{bmatrix} 1& 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \det(\boldsymbol {VU^T}) \end{bmatrix}
\boldsymbol {R^{*}}=\boldsymbol {VMU^T}
4.利用最优解 \boldsymbol{R^{*}} 求解 s
s^*=\frac{\boldsymbol {D}^{*}}{\sigma_x}=\frac{\mathrm{tr}({\boldsymbol {M\Sigma}})}{\sigma_x}
5.利用最优解计算t
\boldsymbol{t^{*}} = \boldsymbol{\bar y} - s^{*}\boldsymbol{R^{*}}\boldsymbol{\bar x}
3. 代码参考
Python EVO Umeyama算法
def umeyama_alignment(x: np.ndarray, y: np.ndarray,
with_scale: bool = False) -> UmeyamaResult:
"""
Computes the least squares solution parameters of an Sim(m) matrix
that minimizes the distance between a set of registered points.
Umeyama, Shinji: Least-squares estimation of transformation parameters
between two point patterns. IEEE PAMI, 1991
:param x: mxn matrix of points, m = dimension, n = nr. of data points
:param y: mxn matrix of points, m = dimension, n = nr. of data points
:param with_scale: set to True to align also the scale (default: 1.0 scale)
:return: r, t, c - rotation matrix, translation vector and scale factor
"""
if x.shape != y.shape:
raise GeometryException("data matrices must have the same shape")
# m = dimension, n = nr. of data points
m, n = x.shape
# means, eq. 34 and 35
mean_x = x.mean(axis=1)
mean_y = y.mean(axis=1)
# variance, eq. 36
# "transpose" for column subtraction
sigma_x = 1.0 / n * (np.linalg.norm(x - mean_x[:, np.newaxis])**2)
# covariance matrix, eq. 38
outer_sum = np.zeros((m, m))
for i in range(n):
outer_sum += np.outer((y[:, i] - mean_y), (x[:, i] - mean_x))
cov_xy = np.multiply(1.0 / n, outer_sum)
# SVD (text betw. eq. 38 and 39)
u, d, v = np.linalg.svd(cov_xy)
if np.count_nonzero(d > np.finfo(d.dtype).eps) < m - 1:
raise GeometryException(&#34;Degenerate covariance rank, &#34;
&#34;Umeyama alignment is not possible&#34;)
# S matrix, eq. 43
s = np.eye(m)
if np.linalg.det(u) * np.linalg.det(v) < 0.0:
# Ensure a RHS coordinate system (Kabsch algorithm).
s = -1
# rotation, eq. 40
r = u.dot(s).dot(v)
# scale & translation, eq. 42 and 41
c = 1 / sigma_x * np.trace(np.diag(d).dot(s)) if with_scale else 1.0
t = mean_y - np.multiply(c, r.dot(mean_x))
return r, t, cORB-SLAM3 Sim3闭式求解(关于Sim3的求解会单独拎一篇文章出来分析，这里旋转矩阵的求解用四元数表示)
void Sim3Solver::ComputeSim3(cv::Mat &P1, cv::Mat &P2)
{
// Custom implementation of:
// Horn 1987, Closed-form solution of absolute orientataion using unit quaternions
// Step 1: Centroid and relative coordinates
cv::Mat Pr1(P1.size(),P1.type()); // Relative coordinates to centroid (set 1)
cv::Mat Pr2(P2.size(),P2.type()); // Relative coordinates to centroid (set 2)
cv::Mat O1(3,1,Pr1.type()); // Centroid of P1
cv::Mat O2(3,1,Pr2.type()); // Centroid of P2
ComputeCentroid(P1,Pr1,O1);
ComputeCentroid(P2,Pr2,O2);
// Step 2: Compute M matrix
cv::Mat M = Pr2*Pr1.t();
// Step 3: Compute N matrix
double N11, N12, N13, N14, N22, N23, N24, N33, N34, N44;
cv::Mat N(4,4,P1.type());
N11 = M.at<float>(0,0)+M.at<float>(1,1)+M.at<float>(2,2);
N12 = M.at<float>(1,2)-M.at<float>(2,1);
N13 = M.at<float>(2,0)-M.at<float>(0,2);
N14 = M.at<float>(0,1)-M.at<float>(1,0);
N22 = M.at<float>(0,0)-M.at<float>(1,1)-M.at<float>(2,2);
N23 = M.at<float>(0,1)+M.at<float>(1,0);
N24 = M.at<float>(2,0)+M.at<float>(0,2);
N33 = -M.at<float>(0,0)+M.at<float>(1,1)-M.at<float>(2,2);
N34 = M.at<float>(1,2)+M.at<float>(2,1);
N44 = -M.at<float>(0,0)-M.at<float>(1,1)+M.at<float>(2,2);
N = (cv::Mat_<float>(4,4) << N11, N12, N13, N14,
N12, N22, N23, N24,
N13, N23, N33, N34,
N14, N24, N34, N44);
// Step 4: Eigenvector of the highest eigenvalue
cv::Mat eval, evec;
cv::eigen(N,eval,evec); //evec is the quaternion of the desired rotation
cv::Mat vec(1,3,evec.type());
(evec.row(0).colRange(1,4)).copyTo(vec); //extract imaginary part of the quaternion (sin*axis)
// Rotation angle. sin is the norm of the imaginary part, cos is the real part
double ang=atan2(norm(vec),evec.at<float>(0,0));
vec = 2*ang*vec/norm(vec); //Angle-axis representation. quaternion angle is the half
mR12i.create(3,3,P1.type());
cv::Rodrigues(vec,mR12i); // computes the rotation matrix from angle-axis
// Step 5: Rotate set 2
cv::Mat P3 = mR12i*Pr2;
// Step 6: Scale
if(!mbFixScale)
{
double nom = Pr1.dot(P3);
cv::Mat aux_P3(P3.size(),P3.type());
aux_P3=P3;
cv::pow(P3,2,aux_P3);
double den = 0;
for(int i=0; i<aux_P3.rows; i++)
{
for(int j=0; j<aux_P3.cols; j++)
{
den+=aux_P3.at<float>(i,j);
}
}
ms12i = nom/den;
}
else
ms12i = 1.0f;
// Step 7: Translation
mt12i.create(1,3,P1.type());
mt12i = O1 - ms12i*mR12i*O2;
// Step 8: Transformation
// Step 8.1 T12
mT12i = cv::Mat::eye(4,4,P1.type());
cv::Mat sR = ms12i*mR12i;
sR.copyTo(mT12i.rowRange(0,3).colRange(0,3));
mt12i.copyTo(mT12i.rowRange(0,3).col(3));
// Step 8.2 T21
mT21i = cv::Mat::eye(4,4,P1.type());
cv::Mat sRinv = (1.0/ms12i)*mR12i.t();
sRinv.copyTo(mT21i.rowRange(0,3).colRange(0,3));
cv::Mat tinv = -sRinv*mt12i;
tinv.copyTo(mT21i.rowRange(0,3).col(3));
}
参考文献
[*]InsaneGuy：三维点云配准 -- ICP 算法原理及推导
[*]赵明明：「自动驾驶SLAM」 sim3求解过程推导以及代码对应
[*]ORB-SLAM2代码阅读笔记（十）：sim3求解_文科升的博客-CSDN博客_sim3算法
[*]Horn B K P. Closed-form solution of absolute orientation using unit quaternions. Josa a, 1987, 4(4): 629-642.
[*]Umeyama S. Least-squares estimation of transformation parameters between two point patterns. IEEE Transactions on Pattern Analysis & Machine Intelligence, 1991, 13(04): 376-380.
页:
[1]