【旋转矩阵的求导及ICP问题的迭代求解与Maltab代码实现】

介绍

旋转矩阵的求导是在相机标定、ICP求解、PnP求解中经常涉及的问题。但是由于旋转矩阵本身特殊的约束存在，使得旋转矩阵的求导不像其它参数的求解那么简单。《视觉SLAM十四讲》从李群-李代数角度对旋转矩阵的求导做了详细的介绍，但笔者在看的时候，一直是云里雾里的，看了好几遍才看懂。实际上，旋转矩阵的求导涉及的核心是罗德里格斯公式，《Computer Vision: Algorithms and Applications》这本书里从罗德里格斯公式小角度旋转下的近似出发，对旋转矩阵的求导有一段精要的介绍，很方便小白的理解。这里结合个人理解进行分享，并以ICP问题求解为例，给出雅可比矩阵及迭代公式的形式，以及相应的Matlab代码实现。

旋转矩阵

三维空间中的旋转矩阵$R$是$3\times 3$的矩阵，满足如下约束
$$R{R}^T=I,\det (R)=1$$此性质说明，旋转矩阵是单位正交矩阵，且行列式值为1。但要注意的是，并非所有单位正交矩阵都是旋转矩阵，一些单位正交矩阵的行列式值为-1，这种矩阵是旋转镜像矩阵。

罗德里格斯公式

旋转有多种表示方法，旋转矩阵是其中一种，此外还有轴角（旋转向量）、欧拉角、四元数等，几种表示方法可以相互进行转换。
轴角把旋转描述为绕某一轴单位向量$n=[n_1,n_2,n_3{]}^T$旋转一定角度$\theta$的形式，或等效地用向量$\omega =\theta n$表示。罗德里格斯公式定义了从轴角到旋转矩阵的变换
$$R=\cos \theta I+(1-\cos \theta )n{n}^T+\sin \theta n^{\wedge }$$其中，符号$\wedge$表示向量的反对称矩阵，表示为
$$n^{\wedge }=\left[\begin{array}{ccc}0& -n_3& n_2\\ n_3& 0& -n_1\\ -n_2& n_1& 0\end{array}\right]$$

小角度近似

Computer Vision: Algorithms and Applications, 2nd ed, p47.

当$\theta$较小时，罗德里格斯公式近似为（对$\cos \theta ,\sin \theta$进行泰勒展开得到）
$$R=I+\theta n^{\wedge }+O({\theta }^2)\approx I+{\omega }^{\wedge }=\left[\begin{array}{ccc}1& -{\omega }_3& {\omega }_2\\ {\omega }_3& 1& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 1\end{array}\right]$$则旋转矩阵$R(\omega )$与点$p$的乘积在小角度旋转下可以近似表示为$$R(\omega )p\approx p+{\omega }^{\wedge }p=p-p^{\wedge }\omega$$则有$$\frac{\partial R(\omega )p}{\partial \omega }=-p^{\wedge }$$

ICP问题求解

以点云对齐ICP问题为例介绍旋转矩阵的求导，已知初始点云$p_1,p_2,...,p_N$和经过位姿变换后匹配的点云$p_1^{\prime },p_2^{\prime },...,p_N^{\prime }$，求旋转矩阵$R(\omega )$和平移向量$t$。优化问题表示为
$$\underset{\omega ,t}{\min }\sum _n\Vert R(\omega )p_n+t-p_n^{\prime }{\Vert }_2^2$$尽管此问题可通过SVD直接求解，但本文介绍通过迭代求解的方法，方便读者对旋转矩阵求导的理解。令$f(\omega ,t)=R(\omega )p+t-p^{\prime }$，则有$$\begin{gathered} \frac{\partial f(\omega ,t)}{\partial \omega }=\underset{\Delta \omega \to 0}{\lim }\frac{R(\Delta \omega )R(w)p+t-p^{\prime }-(R(w)p+t-p^{\prime })}{\Delta \omega } \\ =\underset{\Delta \omega \to 0}{\lim }\frac{R(\Delta \omega )R(w)p-R(w)p}{\Delta \omega } \end{gathered}$$带入上述小角度近似公式$$\begin{gathered} \frac{\partial f(\omega ,t)}{\partial \omega }\approx \frac{(I+(\Delta \omega {)}^{\wedge })R(w)p-R(w)p}{\Delta \omega } \\ =\frac{(\Delta {\omega }^{\wedge })R(w)p}{\Delta \omega } \\ =\frac{-(R(w)p{)}^{\wedge }\Delta \omega }{\Delta \omega } \\ =-(R(\omega )p{)}^{\wedge } \end{gathered}$$对于平移向量，易得$$\frac{\partial f(\omega ,t)}{\partial t}=I$$注意$f,w,t$均为$3\times 1$的向量，雅可比矩阵$$J_f=\left[\begin{array}{cccccc}\frac{\partial f_1}{\partial {\omega }_1}& \frac{\partial f_1}{\partial {\omega }_2}& \frac{\partial f_1}{\partial {\omega }_3}& \frac{\partial f_1}{\partial t_1}& \frac{\partial f_1}{\partial t_2}& \frac{\partial f_1}{\partial t_3}\\ \frac{\partial f_2}{\partial {\omega }_1}& \frac{\partial f_2}{\partial {\omega }_2}& \frac{\partial f_2}{\partial {\omega }_3}& \frac{\partial f_2}{\partial t_1}& \frac{\partial f_2}{\partial t_2}& \frac{\partial f_2}{\partial t_3}\\ \frac{\partial f_3}{\partial {\omega }_1}& \frac{\partial f_3}{\partial {\omega }_2}& \frac{\partial f_3}{\partial {\omega }_3}& \frac{\partial f_3}{\partial t_1}& \frac{\partial f_3}{\partial t_2}& \frac{\partial f_3}{\partial t_3}\end{array}\right]=\left[\begin{array}{cc}-(R(\omega )p{)}^{\wedge }& I\end{array}\right]$$

可以使用高斯牛顿法求解优化问题，迭代增量为
$$\begin{gathered} \Delta x=(J^T(x)J(x){)}^{-1}{J}^T(x)F(x) \\ J=[J_{f_1},\cdots ,J_{f_N}{]}^T \\ F=[f_1,\cdots ,f_N{]}^T \\ \Downarrow \\ \left[\begin{array}{c}\Delta \omega \\ \Delta t\end{array}\right]=(\sum _n{J}_{f_n}^T{J}_{f_n}{)}^{-1}\cdot \sum _n{J}_{f_n}^T{f}_n \end{gathered}$$旋转矩阵和平移向量的增量更新方式为
$$\begin{gathered} R=R(\Delta \omega {)}^{T}R \\ t=t-\Delta t \end{gathered}$$

Matlab代码

clear
clc
close all

% 读取点云
load('xyzPoints');
xyz1 = xyzPoints';
% 随机变换位姿
trans = randn(3,1);
rotm = rodrigues(randn(3,1));
xyz2 = rotm*xyz1+trans;
% 显示点云
figure,subplot(1,2,1),pcshowpair(pointCloud(xyz1'),pointCloud(xyz2')),title('优化前')

% ICP，高斯牛顿
R = eye(3);
t = zeros(3,1);
i = 1;
while(1)
    % 梯度
    grad = zeros(6,1);
    % J^T*J
    jtj = zeros(6,6);
    for j = 1:size(xyz1,2)
        p1 = xyz1(:,j);
        p2 = xyz2(:,j);
        f = R*p1+t-p2;
        jcb = [-ssm(R*p1),eye(3)];
        g = jcb'*f;
        grad = grad+g;
        jtj = jtj+jcb'*jcb;
    end
    deltaRt = jtj\grad;
    dR = rodrigues(deltaRt(1:3));
    dt = deltaRt(4:6);
    R = dR'*R;
    t = t-dt;
    err(i) = mean((R*xyz1+t-xyz2).^2,'all');
    if err(i)<1e-10
        break
    end
    i = i+1;
end
subplot(1,2,2),pcshowpair(pointCloud(xyz2'),pointCloud((R*xyz1+t)')),title('优化后')
figure,plot(err),xlabel('迭代次数'),ylabel('目标函数值')

% 反对称矩阵
function m = ssm(a)
m = [0 -a(3) a(2); a(3) 0 -a(1);-a(2) a(1) 0];
end
% 罗德里格斯公式
function m = rodrigues(rvec)
theta = sqrt(rvec'*rvec);
n = rvec/theta;
m = cos(theta)*eye(3)+(1-cos(theta))*(n*n')+sin(theta)*ssm(n);
end