视觉里程计1

习题二:从E恢复R,t

我们在书中讲到了单目对极几何部分,可以通过本质矩阵E,得到旋转和平移R,t,但那时直接使用了OpenCV提供的函数。本题中,请你根据数学原理,完成从E到R,t的计算。程序框架见code/E2Rt.cpp
设Essential矩阵E的取值为(与书上实验数值相同):

$$ E=\begin{bmatrix} −0.0203618550523477 & −0.4007110038118445 & −0.03324074249824097 \\ 0.3939270778216369 & −0.03506401846698079 & 0.5857110303721015 \\ −0.006788487241438284 & −0.5815434272915686 &−0.01438258684486258 \\ \end{bmatrix} $$

请计算对应的R,t,流程如下:

  1. 对E作SVD分解: $$ E = U \Sigma V^T $$
  2. 处理$\Sigma$的奇异值。设$\Sigma = diag(\sigma_1,\sigma_2,\sigma_3)$,并且$\sigma_1 \geq \sigma_2 \geq \sigma_3$,那么处理后的$\Sigma$为: $$ \Sigma = diag(\frac{\sigma_1 + \sigma_2}{2}, \frac{\sigma_1 + \sigma_2}{2}, 0) $$
  3. 共存在四个可能的解:

$$ t_1^{∧} = UR_{Z(\frac{\pi}{2})}\Sigma U^T, R_1 = UR_{Z(\frac{\pi}{2})}V^T \\ t_2^{∧} = UR_{Z(-\frac{\pi}{2})}\Sigma U^T, R_2 = UR_{Z(-\frac{\pi}{2})}V^T $$

其中$R_{Z(\frac{\pi}{2})}$表示沿Z轴旋转90度得到的旋转矩阵。同时,由于-E和E等价,所以对任意的一个t或R取负号,也会得到同样的结果。因此,从E分解到t,R时,一共存在四个可能的解。请打印出这四个可能的R,t。

提示

  1. AngleAxisSophus::SO3计算$R_{Z(\frac{\pi}{2})}$。
  2. 实际当中,可以利用深度值判断哪个解是真正的解,不过本题不作要求,只需打印四个可能的解即可。同时,你也可以验证$t^∧R$应该与E只差一个乘法因子,并且与书上的实验结果亦只差一个乘法因子。

答案

  1. 本质矩阵E的奇异值必定是$[\sigma , \sigma , 0]$,这是本质矩阵的内在性质。
  2. 本质矩阵 $E = t^{\bigwedge} *R$,但是由于尺度等价性,他的自由度只有5。
  3. 此处还得注意本质矩阵和但应矩阵的对比,这一点后续还会补充。

最终的结果为:

完整程序链接:https://github.com/XLMaverick/Visual-Localization-Percessing/tree/master/vslam_fourteen_lectures/computerRt

相关代码为

    int main(int argc, char **argv) {

        // 给定Essential矩阵
        Matrix3d E;
        E << -0.0203618550523477, -0.4007110038118445, -0.03324074249824097,
                0.3939270778216369, -0.03506401846698079, 0.5857110303721015,
                -0.006788487241438284, -0.5815434272915686, -0.01438258684486258;

        // 待计算的R,t
        Matrix3d R;
        Vector3d t;

        JacobiSVD<MatrixXd> svd(E,ComputeThinU | ComputeThinV);
        Matrix3d U = svd.matrixU();
        Matrix3d V = svd.matrixV();
        Matrix3d Sigma = U.inverse()*E*V.transpose().inverse();
        // cout<<"U:\n"<<U<<"\nV:\n"<<V<<"\nSigma:\n"<<Sigma<<endl;
        vector<double>  tao = {Sigma(0,0),Sigma(1,1),Sigma(2,2)};
        sort(tao.begin(), tao.end());
        Matrix3d SigmaFix = Matrix3d::Zero();
        double tao_mead = (tao[1]+tao[2])*0.5;
        SigmaFix(0,0) = tao_mead;
        SigmaFix(1,1) = tao_mead;
        cout<<"Sigma after fix: \n"<<SigmaFix<<endl;

        Matrix3d R_Z1 = AngleAxisd(M_PI/2, Vector3d(0,0,1)).matrix();
        Matrix3d R_Z2 = AngleAxisd(-M_PI/2, Vector3d(0,0,1)).matrix();

        Matrix3d t_wedge1 = U*R_Z1*SigmaFix*U.transpose();
        Matrix3d t_wedge2 = U*R_Z2*SigmaFix*U.transpose();

        Matrix3d R1 = U*R_Z1*V.transpose();
        Matrix3d R2 = U*R_Z2*V.transpose(); 


        cout << "R1 = " << endl<<R1 << endl;
        cout << "R2 = " << endl<<R2 << endl;
        cout << "t1 = " <<endl<< Sophus::SO3::vee(t_wedge1) << endl;
        cout << "t2 = " <<endl<< Sophus::SO3::vee(t_wedge2) << endl;

        // check t^R=E up to scale
        Matrix3d tR = t_wedge1 * R1;
        cout << "t^R = " << tR << endl;

        return 0;
    }

习题三:用G-N实现Bundle Adjustment

Bundle Adjustment并不神秘,它仅是一个目标函数为重投影误差的最小二乘。我们演示了Bundle Adjustment可以由Ceresg2o实现,并可用于PnP当中的位姿估计。本题,你需要自己书写一个高斯牛顿法,实现用Bundle Adjustment优化位姿的功能,求出相机位姿。严格来说,这是Bundle Adjustment的一部分,因为我们仅考虑了位姿,没有考虑点的更新。完整的BA需要用到矩阵的稀疏性,我们留到第七节课介绍。
假设一组点的3D坐标为$P = {p_i}$,它们在相机中的坐标为$U = {u_i},∀i = 1, … n$。在文件 p3d.txtp2d.txt中给出了这两组点的值。同时,设待估计的位姿为$T ∈ SE(3)$,内参矩阵为:

$$ K = \begin{bmatrix} 520.9 & 0 & 325.1 \\ 521.0 & 249.7 & 0 \\ 1 & 0 & 0\
\end{bmatrix} $$

请你根据上述条件,用G-N法求出最优位姿,初始估计为$T_0 = I$。程序GN-BA.cpp文件提供了大致的框架,请填写剩下的内容。 在书写程序过程中,回答下列问题:

  1. 如何定义重投影误差?
  2. 该误差关于自变量的雅可比矩阵是什么?
  3. 解出更新量之后,如何更新至之前的估计上?

作为验证,最后估计得到的位姿应该接近:

$$ T^* = \begin{bmatrix} 0.9978 & 0.0506 & 0.0399 & −0.1272 \\\\ 0.0506 & 0.9983 & 0.0274 & −0.007 \\\\ −0.0412 & −0.0253 & 0.9977 & 0.0617 \\\\ 0 & 0 & 0 & 1 \\\\ \end{bmatrix} $$

这和书中使用g2o优化的结果很接近。

答案

1.首先我们需要明白PnP问题是的已知条件3D点以及在图像中的投影坐标,整个误差函数的简历是相机模型中坐标系的变换,利用归一化坐标系下的坐标作为中间变量,链式求导,进而求解雅各比矩阵的。整个过程是针对一帧而言。这和直接法中的不一样,直接法是前后两帧。
重投影误差定义为:

$$ e(\xi)=P_{u v}-\frac{1}{z_{c}} \operatorname{Kexp}\left(\xi^{\wedge}\right) P_{w} $$

2.关于误差的雅各比矩阵有两部分组成,误差关于重投影点的导数、变换后的点关于李代数的导数:

$$ J(\xi)=-\left[ \begin{array}{ccccc}{\frac{f_{x}}{z_{c}}} & {0} & {-\frac{f_{x} x_{c}}{z_{c}^{2}}} & {-\frac{f_{x} x_{c} y_{c}}{z_{c}^{2}}} & {f_{x}+\frac{f_{x} x_{c}^{2}}{Z_{c}^{2}}} & {-\frac{f_{x} y_{c}}{z_{c}}} \\ {0} & {\frac{f_{y}}{z_{c}}} & {-\frac{f_{y} y_{c}}{z_{c}^{2}}} & {-f_{y}-\frac{f_{y} y_{c}^{2}}{z_{c}^{2}}} & {\frac{f_{y} x_{c} y_{c}}{z_{c}^{2}}} & {\frac{f_{y} x_{c}}{z_{c}}}\end{array}\right] $$

3.解出更新量之后,更新至之前的估计上有两种方式,李代数上或者在变换矩阵上:

    T21 = Sophus::SE3::exp(update) * T21;

或者

    Vector6d T_origin = T_esti.log();
    Vector6d T_se = (T_origin + dx);
    T_esti = Sophus::SE3::exp(T_se);

最终结果为:

完整程序链接:https://github.com/XLMaverick/Visual-Localization-Percessing/tree/master/vslam_fourteen_lectures/BApnp

相关代码:

    int main(int argc, char **argv) {
        VecVector2d p2d;
        VecVector3d p3d;
        Matrix3d K;
        double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
        K << fx, 0, cx, 0, fy, cy, 0, 0, 1;

        ifstream p3dfile;
        p3dfile.open(p3d_file);
        if(!p3dfile)
        {
            cout<<"open p3d.txt error!"<<endl;
        }
        string p3dline;
        while(getline(p3dfile,p3dline) && !p3dline.empty())
        {
            istringstream p3dtempLine(p3dline);
            Vector3d p3dtemp;
            p3dtempLine>>p3dtemp[0]>>p3dtemp[1]>>p3dtemp[2];
            p3d.push_back(p3dtemp);
            
        }        
        ifstream p2dfile;
        p2dfile.open(p2d_file);
        if(!p2dfile)
        {
            cout<<"open p2d.txt error!"<<endl;
        }
        string p2dline;
        while(getline(p2dfile,p2dline) && !p2dline.empty())
        {
            istringstream p2dtempLine(p2dline);
            Vector2d p2dtemp;
            p2dtempLine>>p2dtemp[0]>>p2dtemp[1];
            p2d.push_back(p2dtemp);
            
        }
        assert(p3d.size() == p2d.size());
        int iterations = 100;
        double cost = 0, lastCost = 0;
        int nPoints = p3d.size();
        cout << "points: " << nPoints << endl;
        Sophus::SE3 T_esti(Matrix3d::Identity(),Vector3d::Zero()); // estimated pose
        for (int iter = 0; iter < iterations; iter++) 
        {
            Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();
            Vector6d b = Vector6d::Zero();
            Vector2d e;
            cost = 0;
            for (int i = 0; i < nPoints; i++) 
            {
                Vector3d pc = T_esti*p3d[i];
                Vector3d e_temp = Vector3d(p2d[i][0],p2d[i][1],1) - K*pc/pc[2];
                e[0] = e_temp[0];
                e[1] = e_temp[1];
                cost  += 0.5*e.transpose()*e; 
                double x = pc[0], y = pc[1], z = pc[2];
                Matrix<double, 2, 6> J;
                J(0,0) = -fx/z;
                J(0,2) = fx*x/(z*z);
                J(0,3) = fx*x*y/(z*z);
                J(0,4) = -fx - fx*(x*x)/(z*z);
                J(0,5) =  fx*y/z;
                J(1,1) = -fy/z;
                J(1,2) = fy*y/(z*z);
                J(1,3) = fy+fy*(y*y)/(z*z);
                J(1,4) = -fy*x*y/(z*z);
                J(1,5) = -fy*x/z;

                H +=J.transpose()*J;
                b +=-J.transpose()*e;
            }
            Vector6d dx;
            dx = H.ldlt().solve(b);

            if (isnan(dx[0])) 
            {
                cout << "result is nan!" << endl;
                break;
            }
            if (iter > 0 && cost >= lastCost) 
            {
                // cost increase, update is not good
                cout << "cost: " << cost << ", last cost: " << lastCost << endl;
                break;
            }
            T_esti = Sophus::SE3::exp(dx)*T_esti;            
            lastCost = cost;
            cout << "iteration " << iter << " cost=" << cout.precision(12) << cost << endl;
        }
        cout << "estimated pose: \n" << T_esti.matrix() << endl;
        return 0;
    }

习题三:用ICP实现轨迹对齐

在实际当中,我们经常需要比较两条轨迹之间的误差。第三节课习题中,你已经完成了两条轨迹之间的RMSE 误差计算。但是,由于ground-truth轨迹与相机轨迹很可能不在一个参考系中,它们得到的轨迹并不能直接比较。这时,我们可以用ICP来计算两条轨迹之间的相对旋转与平移,从而估计出两个参考系之间的差异。 设真实轨迹为 $T_ g$ ,估计轨迹为 $T_ e$ ,二者皆以 $T_ {WC}$ 格式存储。但是真实轨迹的坐标原点定义于外部 某参考系中(取决于真实轨迹的采集方式,如Vicon系统可能以某摄像头中心为参考系,而估计轨迹则以相机出发点为参考系(在视觉SLAM中很常见)。由于这个原因,理论上的真实轨迹点与估计轨迹点应满足:

$$ \mathbf{T}_{g, i}=\mathbf{T}_{g e} \mathbf{T}_{e, i} $$

其中i表示轨迹中的第i条记录,$T_ {ge} ∈ SE(3)$为两个坐标系之间的变换矩阵,该矩阵在整条轨迹中保持不变。$T_ {ge}$可以通过两条轨迹数据估计得到,但方法可能有若干种:

1.认为初始化时两个坐标系的差异就是$T_{ge},即:

$$ \mathbf{T}_{g e}=\mathbf{T}_{g, 1} \mathbf{T}_{e, 1}^{-1} $$

2.在整条轨迹上利用最小二乘计算$T_ge$:

$$ \mathbf{T}_{g e}=\arg \min _{\mathbf{T}_{g c}} \sum_{i=1}^{n}\left\|\log \left(\mathbf{T}_{g i}^{-1} \mathbf{T}_{g e} \mathbf{T}_{e, i}\right)^{\mathrm{v}}\right\|_{2} $$

3.把两条轨迹的平移部分看作点集,然后求点集之间的ICP,得到两组点之间的变换。

其中第三种也是实践中用的最广的一种。现在请你书写ICP程序,估计两条轨迹之间的差异。轨迹文 件在compare.txt文件中,格式为:

$$ _{e}, \mathbf{t}_{e}, \mathbf{q}_{e}, \text { time }_{g}, \mathbf{t}_{g}, \mathbf{q}_{g} $$

其中t表示平移,q表示单位四元数。请计算两条轨迹之间的变换,然后将它们统一到一个参考系,并画在pangolin中。轨迹的格式与先前相同,即以时间,平移,旋转四元数方式存储。 本题不提供代码框架,你可以利用之前的作业完成本题。

答案:

  1. ICP有两种解决办法,SVD和非线性优化的方法,这一次我们尝试两种办法,其中非线性优化的方法我们采用g2o优化库。
  2. 画图部分依然采用pangolin,和前几讲的方式一样。
  3. g2o的版本问题需要注意,最新版本有些地方是不兼容的。
  4. 还是g2o的问题,g2o中李代数的定义顺序和我们一样不一样。通常前三维为平移,后三维为旋转,但是g2o的李代数和Sohpus的李代数定义就不一样,g2o中是前三维为旋转,后三维为平移,所以对应的雅各比矩阵交换了顺序,这一点尤其需要注意。

首先是SVD方法的结果:

非线性优化的结果:

完整程序链接:https://github.com/XLMaverick/Visual-Localization-Percessing/tree/master/vslam_fourteen_lectures/icp