[ICP]手推SVD方法

该方法源于《Least-Squares Rigid Motion Using SVD》，原文推导十分详细，这里自己也仔细推导了一遍，有些地方加以注释整理。

问题定义

假设我们有两个点云集合(mathcal{P}=left{mathbf{p}_{1}, mathbf{p}_{2}, ldots, mathbf{p}_{n} ight})和(mathcal{Q}=left{mathbf{q}_{1}, mathbf{q}_{2}, ldots, mathbf{q}_{n} ight})，则我们定义的 ICP 问题是通过最小化点对之间距离获得相应的位姿((R,mathbf{t}))：

[(R, mathbf{t})=underset{R in SO(d), mathbf{t} in mathbb{R}^{d}}{operatorname{argmin}} sum_{i=1}^{n} w_{i}left|left(R mathbf{p}_{i}+mathbf{t} ight)-mathbf{q}_{i} ight|^{2} ag{1} ]

其中 (w_i) 代表每个点的权重。R 和 t 是我们所要求的旋转矩阵和平移向量。

计算平移向量

我们要优化的误差函数如下:
(F(R,mathbf{t})=sum_{i=1}^{n} w_{i}left|left(R mathbf{p}_{i}+mathbf{t} ight)-mathbf{q}_{i} ight|^{2})
首先来计算对t的导数
令(mathcal{l}=left|left(Rmathbf{p}_{i}+mathbf{t} ight)-mathbf{q}_{i} ight|^{2}=(Rmathbf{p}_i+mathbf{t}-mathbf{q}_i)^T(Rmathbf{p}_i+mathbf{t}-mathbf{q}_i))
(dl)的微分为：

[egin{aligned} dl &=d((Rmathbf{p}_i+mathbf{t}-mathbf{q}_i)^T)(Rmathbf{p}_i+mathbf{t}-mathbf{q}_i)+(Rmathbf{p}_i+mathbf{t}-mathbf{q}_i)^Td(Rmathbf{p}_i+mathbf{t}-mathbf{q}_i) \ &=underbrace{(d(Rmathbf{p}_i+mathbf{t}-mathbf{q}_i))^T}_{d(X^T) = (dX)^T}(Rmathbf{p}_i+mathbf{t}-mathbf{q}_i)+(Rmathbf{p}_i+mathbf{t}-mathbf{q}_i)^Tdmathbf{t} \ &=(dmathbf{t})^T(Rmathbf{p}_i+mathbf{t}-mathbf{q}_i)+(Rmathbf{p}_i+mathbf{t}-mathbf{q}_i)^Tdmathbf{t} \ &=underbrace{2(Rmathbf{p}_i+mathbf{t}-mathbf{q}_i)^Tdt}_{当A^TB为标量时，A^TB=B^TA} end{aligned} ]

对照(dl=frac{partial l}{partial t}^Tdt)，得(frac{partial l}{partial t}=2(Rmathbf{p}_i+mathbf{t}-mathbf{q}_i))。因此:

[egin{aligned} frac{partial F}{partial t} &=sum_{i=1}^{n} 2 w_{i}(Rmathbf{p}_i+mathbf{t}-mathbf{q}_i) \ &=2mathbf{t}sum_{i=1}^{n}w_i+2Rsum_{i=1}^{n}w_imathbf{p}_i-2sum_{i=1}^{n}w_imathbf{q}_iend{aligned} ag{2} ]

令(frac{partial F}{partial t}=0)，得

[mathbf{t}=frac {sum_{i=1}^{n}w_imathbf{q}_i-Rsum_{i=1}^{n}w_imathbf{p}_i}{sum_{i=1}^{n}w_i} ]

记：

[overline{mathbf{p}}=frac{sum_{i=1}^{n} w_{i} mathbf{p}_{i}}{sum_{i=1}^{n} w_{i}}, quad overline{mathbf{q}}=frac{sum_{i=1}^{n} w_{i} mathbf{q}_{i}}{sum_{i=1}^{n} w_{i}} ag{3} ]

也就是加权平均的质心，并再次带回 (2) 式可以得到：
(mathbf{t}=overline{mathbf{q}}-R overline{mathbf{p}} ag{4})

将(4)带回(1)可得：

[egin{aligned} sum_{i=1}^{n} w_{i}left|left(R mathbf{p}_{i}+mathbf{t} ight)-mathbf{q}_{i} ight|^{2} &=sum_{i=1}^{n} w_{i}left|R mathbf{p}_{i}+overline{mathbf{q}}-R overline{mathbf{p}}-mathbf{q}_{i} ight|^{2}=\ &=sum_{i=1}^{n} w_{i}left|Rleft(mathbf{p}_{i}-overline{mathbf{p}} ight)-left(mathbf{q}_{i}-overline{mathbf{q}} ight) ight|^{2} end{aligned} ag{5} ]

令(mathbf{x}_i:=mathbf{p}_i-overline{mathbf{p}},mathbf{y}_i:=mathbf{q}_i-overline{mathbf{q}})，则问题转变为：

[R=underset{R in S O(d)}{operatorname{argmin}} sum_{i=1}^{n} w_{i}left|R mathbf{x}_{i}-mathbf{y}_{i} ight|^{2} ag{6} ]

计算旋转矩阵

(6)式是不是很眼熟，还记得最小二乘问题吗？

[egin{aligned}left|R mathbf{x}_{i}-mathbf{y}_{i} ight|^{2} &=left(R mathbf{x}_{i}-mathbf{y}_{i} ight)^{ op}left(R mathbf{x}_{i}-mathbf{y}_{i} ight)=left(mathbf{x}_{i}^{ op} R^{ op}-mathbf{y}_{i}^{ op} ight)left(R mathbf{x}_{i}-mathbf{y}_{i} ight) \ &=underbrace{mathbf{x}_{i}^{ op}R^{ op}Rmathbf{x}_{i}}_{R是单位正交阵，R^{ op}R=mathbf{I}}-mathbf{y}_{i}^{ op} R mathbf{x}_{i}-mathbf{x}_{i}^{ op} R^{ op} mathbf{y}_{i}+mathbf{y}_{i}^{ op} mathbf{y}_{i} \ &=mathbf{x}_{i}^{ op} mathbf{x}_{i}-mathbf{y}_{i}^{ op}Rmathbf{x}_{i}-underbrace{(Rmathbf{x}_{i})^{ op}mathbf{y}_{i}}_{(AB)^{ op}=B^{ op}A^{ op}}+mathbf{y}_{i}^{ op} mathbf{y}_{i} \ &=mathbf{x}_{i}^{ op} mathbf{x}_{i}underbrace{-2mathbf{y}_{i}^{ op}Rmathbf{x}_{i}}_{当A^TB为标量时，A^TB=B^TA} + mathbf{y}_{i}^{ op}mathbf{y}_{i} end{aligned} ag{7} ]

因此问题转换为：

[egin{aligned}underset{Rin SO(d)}{operatorname{argmin}} sum_{i=1}^{n}w_ileft|R mathbf{x}_{i}-mathbf{y}_{i} ight|^{2}&=underset{Rin SO(d)}{operatorname{argmin}} :sum_{i=1}^{n}w_i(underbrace{mathbf{x}_{i}^{ op} mathbf{x}_{i}-2mathbf{y}_{i}^{ op}Rmathbf{x}_i + mathbf{y}_{i}^{ op}mathbf{y}_{i}}_{mathbf{x}_i,mathbf{y}_i与R无关}) \ &=underset{Rin SO(d)}{operatorname{argmax}} :sum_{i=1}^{n}w_imathbf{y}_{i}^{ op}Rmathbf{x}_i end{aligned} ag{8} ]

令(W_{n imes n}=left[egin{array}{ccccc}{w_{1}} & {} & {} & {} & {} \ {} & {w_{2}} & {} & {} & {} \ {} & {} & {} & {ddots} & {} \ {} & {} & {} & {} & {w_{n}}end{array} ight],Y^{ op}_{n imes 3} = left[egin{array}{c}{-mathbf{y}_{1}^{ op}-} \ {-mathbf{y}_{2}^{ op}-} \ {vdots} \ {-mathbf{y}_{n}^{ op}-}end{array} ight],X_{3 imes n}=egin{bmatrix}| & | & & |\mathbf{x}_1 & mathbf{x}_2 & cdots & mathbf{x}_n\ | & | & & |end{bmatrix})
有

[egin{align*}W_{n imes n}Y^{ op}_{n imes 3} R_{3 imes 3} X_{3 imes n} &= left[egin{array}{ccccc}{w_{1}} & {} & {} & {} & {} \ {} & {w_{2}} & {} & {} & {} \ {} & {} & {} & {ddots} & {} \ {} & {} & {} & {} & {w_{n}}end{array} ight] left[egin{array}{c}{-mathbf{y}_{1}^{ op}-} \ {-mathbf{y}_{2}^{ op}-} \ {vdots} \ {-mathbf{y}_{n}^{ op}-}end{array} ight] left[egin{array}{ccccc}{} & {} & {} \ {} & {R} & {} \ {} & {} & {} end{array} ight] egin{bmatrix}| & | & & |\mathbf{x}_1 & mathbf{x}_2 & cdots & mathbf{x}_n\ | & | & & |end{bmatrix}\ &= left[egin{array}{c}{-w_{1}mathbf{y}_{1}^{ op}-} \ {-w_{2}mathbf{y}_{2}^{ op}-} \ {vdots} \ {-w_{n}mathbf{y}_{n}^{ op}-}end{array} ight]_{n imes 3} egin{bmatrix}| & | & & |\Rmathbf{x}_1 & Rmathbf{x}_2 & cdots & Rmathbf{x}_n\ | & | & & |end{bmatrix}_{3 imes n}\ &= left[egin{array}{cccc}{w_{1} mathbf{y}_{1}^{ op} R mathbf{x}_{1}} & {} & {} & {*} \ {} & {w_{2} mathbf{y}_{2}^{ op} R mathbf{x}_{2}} & {} \ {} & {} & {ddots} & {} \ {*} & {} & {} & {w_{n} mathbf{y}_{n}^{ op} R mathbf{x}_{n}}end{array} ight] end{align*} ag{9} ]

因此

[egin{aligned}sum_{i=1}^{n} w_{i} mathbf{y}_{i}^{ op} R mathbf{x}_{i}&=operatorname{tr}left(W Y^{ op} R X ight)\ &=underbrace{tr(RXWY^{ op})}_{tr(AB)=tr(BA)} end{aligned} ag{10}]

令(S=XWY^{ op})，而(S_{SVD}=USigma V^{ op},U与V都是单位正交阵，即UU^{ op}=I,VV^{ op}=I,Sigma =left(egin{array}{cccc}{sigma_{1}} & {} & {} & {} \ {} & {sigma_{2}} & {} & {} \ {} & {} & {ddots} & {} \ {} & {} & {} & {sigma_{n}}end{array} ight)且sigma_{1}, sigma_{2}, ldots, sigma_{n} geq 0)，带入(10):

[tr(RXWY^{ op})=tr(RS)=tr(RUSigma V^{ op})=tr(Sigma V^{ op}RU) ag{11} ]

我们来看下(M=V^{ op}RU)，(V^{ op},R,U)均为单位正交阵，那么(M)也为单位正交阵（自己动手推导下，很简单的～），有(MM^{ op}=I)，即M中每行、每列的内积都是1。假设(mathbf{m}_j为M的列向量)，那么

[mathbf{m}_j^{ op}mathbf{m}_j=sum_{i}m_{ij}^2=1 ]

可见(forall i,jin[0,n],|m_{ij}|leqslant 1)。那么

[operatorname{tr}(Sigma M)=left(egin{array}{cccc}{sigma_{1}} & {} & {} & {} \ {} & {sigma_{2}} & {} & {} \ {} & {} & {ddots} & {} \ {} & {} & {} & {sigma_{n}}end{array} ight)left(egin{array}{c}{m_{11} m_{12} ldots m_{1 n}} \ {m_{21} m_{22} ldots m_{2 n}} \ {vdots} \ {m_{n 1} m_{n 2} ldots m_{n n}}end{array} ight)=sum_{i=1}^{n} sigma_{i} m_{i i} leq sum_{i=1}^{n} sigma_{i} ag{12} ]

显然(M=I)时，(tr(Sigma M))可以取到最大值，此时

[I=M=V^{ op} R U Rightarrow V=R U Rightarrow R=V U^{ op} ag{13} ]

反射修正

前文中推导的结果一定是一个单位正交阵，但是有一个问题，并不是所有的单位正交阵都是旋转矩阵。

镜面反射

参考
(A=egin{bmatrix} ext{cos} heta & ext{sin} heta \ ext{sin} heta & - ext{cos} hetaend{bmatrix})
也是一正交矩阵，仔细观察两个基的变化，它相当于逆时针旋转( heta)后再把(y') 轴对折，物理上若不对折，无论如何旋转也达不到依运算所得的结果，显然这类正交矩阵既包括旋转还包括了镜面反射。这里是二维的情况，对于三维同样有效，因此求解出R后还需要进行一些检测：
如果(operatorname{det}left(V U^{ op} ight)=-1)，则所求的 R 包含了旋转和镜像；
如果 (operatorname{det}left(V U^{ op} ight)=1)，则所求的 R 是我们所求的旋转矩阵。

假设包含了旋转和镜像，对于上节的结论：

[M=V^{ op}RU=left(egin{array}{ccccc}{1} \ {} & {1} \ {} & {} & {ddots} & {} \ {} & {} & {} & {-1}end{array} ight) Rightarrow R=Vleft(egin{array}{ccccc}{1} \ {} & {1} \ {} & {} & {ddots} & {} \ {} & {} & {} & {-1}end{array} ight) U^{ op} ag{13} ]

整理上述两种情况就可以统一成以下表达式：

[R=V egin{pmatrix}1 & 0 & 0\ 0 & 1 & 0\ 0 & 0 & operatorname{det}left(V U^{ op} ight)end{pmatrix} U^{ op} ag{14} ]

平移向量(mathbf{t}=overline{mathbf{q}}-R overline{mathbf{p}} ag{31})

实践

void pose_estimation_svd (
        const vector<pair<Vec3_t, Vec3_t>>& match_pairs,
        Mat33_t& R, Vec3_t& t)
{
    //假设每个点的权重都是1.0
    // overline{mathbf{p}}=frac{sum_{i=1}^{n} w_{i} mathbf{p}_{i}}{sum_{i=1}^{n} w_{i}} = frac {sum_{i=1}^{n} mathbf{p}_{i}}{n}
    // overline{mathbf{q}}=frac{sum_{i=1}^{n} w_{i} mathbf{q}_{i}}{sum_{i=1}^{n} w_{i}} = frac {sum_{i=1}^{n} mathbf{q}_{i}}{n}

    //1. 计算overline{mathbf{p}}，overline{mathbf{q}}
    Vec3_t p{0.,0.,0.}, q{0.,0.,0.};
    int N = match_pairs.size();
    for ( int i=0; i<N; i++ )
    {
        p += match_pairs[i].first;  // sum_{i=1}^{n} mathbf{p}_{i}
        q += match_pairs[i].second; // sum_{i=1}^{n} mathbf{q}_{i}
    }
    p /= N; //frac {sum_{i=1}^{n} mathbf{p}_{i}}{n}
    q /= N; //frac {sum_{i=1}^{n} mathbf{1}_{i}}{n}
    //2. 计算mathbf{x},mathbf{y}
    vector<Vec3_t> X( N ), Y( N ); // remove the center
    for ( int i=0; i<N; i++ )
    {
        X[i] = match_pairs[i].first - p;
        Y[i] = match_pairs[i].second - q;
    }

    //3. S=XWY^{	op} W=E, 因此S=XY^{	op}
    Eigen::Matrix3d S = Eigen::Matrix3d::Zero();
    for ( int i=0; i<N; i++ )
    {
        S += X[i] * Y[i].transpose();
    }
    cout<<"S="<<S<<endl;

    //4. S进行SVD 奇异值分解
    Eigen::JacobiSVD<Eigen::Matrix3d> svd ( S, Eigen::ComputeFullU|Eigen::ComputeFullV );
    const Eigen::Matrix3d U = svd.matrixU();
    const Eigen::Matrix3d V = svd.matrixV();
    cout<<"U="<<U<<endl;
    cout<<"V="<<V<<endl;
    //5. 构造去镜像矩阵
    Eigen::Matrix3d remove_mirror{Eigen::Matrix3d::Identity()};
    remove_mirror(2,2) = (V*U.transpose()).determinant();
    cout<<"remove_mirror="<<remove_mirror<<endl;

    //6. R=V*remove_mirror*U^{	op}
    R = V*remove_mirror*U.transpose();

    //7. 平移向量$mathbf{t}=overline{mathbf{q}}-R overline{mathbf{p}}	ag{31}$
    t = q - R * p;

    cout<< "SVD method:"<<endl;
    cout<<"R="<<R<<endl;
    cout<<"t="<<t.transpose()<<endl;

}

参考

1. 使用 SVD 方法求解 ICP 问题