《视觉SLAM十四讲》中第十章代码中雅克比的具体形式推导

 

        在上一篇文章《视觉SLAM十四讲》第十章g2o代码的简化 中,求解BA的雅克比矩阵,我直接使用了g2o的数值导,因为这个雅克比矩阵自己写还真的很容易出错,不过还是有一些小伙伴想要了解具体的形式,所以在这一篇文章中会具体进行推导。

1 雅克比矩阵数学推导

        如果对数学推导不感兴趣的,可以直接跳到代码那里看,另外这里会使用李群和李代数的知识,不清楚的可以看一下《视觉SLAM十四讲》,里面介绍的很基础,也可以去查阅其他文章。

 

1.1 误差函数形式

         Bundle Ajustment in Large中给出了世界坐标下的空间点Pw、以及外参Rcw,tcw和内参f。

1) 我们要求误差,那么需要将空间点重投影到相机坐标系,一般形式如下:

                                                                                     \small P_{c}=T_{cw}*P_{w}

其中 \small T_{cw} 为变换矩阵,\small P_{w} 为世界坐标下的空间点,形式为   \small \small \small \begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w} \\ 1 \end{bmatrix} ,\small P_{c} 类似\small P_{w} 

2) 将 \small P_{c} 归一化得到 \small p_{c}, 形式为  \small \begin{bmatrix} X_{c} /Z_{c}\\ Y_{c}/Z_{c} \\ 1 \end{bmatrix},令 \small x_{c} = X_{c}/Z_{c}, y_{c} = Y_{c}/Z_{c} ,这里需要注意,官方数据集中\small x_{c} = -X_{c}/Z_{c}, y_{c} = -Y_{c}/Z_{c},写代码容易疏忽。

3)\small p_{c} 去除畸变,并且转换到像素坐标系得到 \small p' 

                                                     \small p'=\begin{bmatrix}u' \\ v' \\ 1 \end{bmatrix}= \begin{bmatrix}f_{x} &0 &c_{x} \\0 &f_{y} &c_{y} \\0 &0 &1 \end{bmatrix}* \begin{bmatrix}x_{c}(1+k_{1}*||p_{c}||^{2}+k_{2}*||p_{c}||^{4}) \\ y_{c}(1+k_{1}*||p_{c}||^{2}+k_{2}*||p_{c}||^{4}) \\ 1 \end{bmatrix}

其中  \small ||p_{c}||^2 = x_{c}^{2}+y_{c}^{2} ,而在这个Bundle Ajustment in Large 的数据集中,\small c_{x} = c_{y} = 0 ,\small f_{x} = f_{y} = f

4) 由于我们已知观测数据(数据集里给出) \small p = \begin{bmatrix}u \\ v \\ 1 \end{bmatrix}  ,误差函数的形式如下:

                                                                                            \small e = p-p'

我们取前面两行

                                                           \small e=\begin{bmatrix}u\\ v \end{bmatrix}-\begin{bmatrix}f_{x}*x_{c}(1+k_{1}*||p_{c}||^{2}+k_{2}*||p_{c}||^{4}) + c_{x}\\ f_{y}*y_{c}(1+k_{1}*||p_{c}||^{2}+k_{2}*||p_{c}||^{4}) + c_{y} \end{bmatrix}

如果将官方的数据要求带入,那么我们误差形式应该如下:

                                                            \small e=\begin{bmatrix}u- f * (1+k_{1}*||p_{c}||^{2}+k_{2}*||p_{c}||^{4})*(-X_{c}/Z_{c})\\ v-f*(1+k_{1}*||p_{c}||^{2}+k_{2}*||p_{c}||^{4})*(-Y_{c}/Z_{c}) \end{bmatrix}

 

1.2 误差函数的雅克比矩阵形式

       在得到误差后,我们需要将误差 \small e 降到最小,所以我们要找到误差函数的极小值,而且误差函数往往是非线性的,所以我们要泰勒展开转成线性的,然后迭代求解,假设大家对梯度下降法、高斯牛顿法,LM算法有一定的了解,所以对优化算法不具体讨论。

1.2.1 对误差函数一阶泰勒展开

                                                                            \small e(x+\Delta x)\approx e(x)+J(x)\Delta x

其中 \small J(x) 为雅克比矩阵,也就是我们要求的量, 另外我们是对所有的位姿和所有的空间进行优化,那么它的雅克比矩阵就不会是这么简单的形式,它是由许许多多的雅克比矩阵构成的一个\small H矩阵,这个矩阵的维度很大,如果对其求逆,会非常耗时,对SLAM这种讲究实时性的系统而言是致命的,但是这个\small H矩阵有个优点,它是一个稀疏矩阵,这有许多方法进行优化,在这里也不具体讨论,详细可以参考书本。

结合了所有位姿、内外参 (\small \gamma) 和空间点(\small p)的误差函数如下:

                                                         \small \frac{1}{2}||e(x+\Delta x)||^{2}\approx \frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}||e_{ij}+F_{ij}\Delta \gamma +E_{ij}\Delta p_{j}||^{2}

其中\small \Delta \gamma 为9维的,\small F_{ij}为对位姿、内外参的偏导数,\small E_{ij}为对空间点的偏导数

1.2.2 \small F_{ij}、 \small E_{ij} 的具体形式

我们需要对 \small R,t,f,k1,k2 这五个量(9维)一起优化,另外我们还要对空间点(3维)优化,我们要写两个雅克比矩阵,它的样子如下

                                                                     \small X_{i}=F=\begin{bmatrix}\frac{\partial e}{\partial \delta \xi }_{2*6} & \frac{\partial e}{\partial f}_{2*1} &\frac{\partial e}{\partial k_{1}}_{2*1} &\frac{\partial e}{\partial k2}_{2*1} \end{bmatrix} _{2*9}

                                                                                      \small X_{j}=E=\begin{bmatrix}\frac{\partial e}{\partial P_{w} } \end{bmatrix} _{2*3}

让我们开始一项项写出来:

1) 由于旋转矩阵R对加法不封闭,所以需要转化到李代数上来求导,这里是对左扰动求偏导,使用链式法则

                                                                                     \frac{\partial e}{\partial \delta \xi } = \frac{\partial e}{\partial P_{c} } * \frac{\partial P_{c}}{\partial \delta \xi}

    对P_{c}的求导要特别细心才行,这一步运算很容易出错,而且这个矩阵会被使用多次,所以一定不能写错

                                                                                     \small \frac{\partial e}{\partial P_{c} } =\begin{bmatrix}\frac{\partial e_{u}}{\partial X_{c}} &\frac{\partial e_{u}}{\partial Y_{c}} &\frac{\partial e_{u}}{\partial Z_{c}} \\ \frac{\partial e_{v}}{\partial X_{c}} & \frac{\partial e_{v}}{\partial Y_{c}} & \frac{\partial e_{v}}{\partial Z_{c}} \end{bmatrix}

     每一项如下:

 \small \frac{\partial e_{u}}{\partial X_{c}} = \frac{f*r}{Z_{c}}+\frac{2*f*X_{c}^{2}}{Z_{c}^{3}}*k

 \small \frac{\partial e_{u}}{\partial Y_{c}} = \frac{2*f*X_{c}*Y_{c}}{Z_{c}^{3}}*k

 \small \frac{\partial e_{u}}{\partial Z_{c}} = -\frac{f*X_{c}*r}{Z_{c}^{2}}-\frac{2*f*X_{c}*||p_{c}||^2}{Z_{c}^2}*k

 \small \frac{\partial e_{u}}{\partial Y_{c}} = \frac{2*f*X_{c}*Y_{c}}{Z_{c}^{3}}*k

 \small \frac{\partial e_{u}}{\partial X_{c}} = \frac{f*r}{Z_{c}}+\frac{2*f*Y_{c}^{2}}{Z_{c}^{3}}*k

  \small \frac{\partial e_{u}}{\partial Z_{c}} = -\frac{f*Y_{c}*r}{Z_{c}^{2}}-\frac{2*f*Y_{c}*||p_{c}||^2}{Z_{c}^2}*k

其中 \small k = k_{1}+2*k_{2}*||p_{c}||^2\small r = 1+k_{1}*||p_{c}||^2+k_{2}*||p_{c}||^4,会用到这个公式(f(x)g(x))' = f(x)'g(x)+f(x)g(x)' 

    对P_{c}对左扰动的求导,这里直接套用书上的公式了,不做具体推导,这是它的形式 \begin{bmatrix}I & -P_{c}^{'\Lambda } \end{bmatrix} 。^ 符号为反对称矩阵。

                                                                      \frac{\partial P_{c}}{\partial \delta \xi } =\begin{bmatrix}1&0 &0 &0 &Z_{c} &-Y_{c} \\0 &1 &0 &-Z_{c} &0 &X_{c} \\0 &0 &1 &Y_{c} &-X_{c} &0 \end{bmatrix}

 

2) 对 f 求导

                                                                           \frac{\partial e}{\partial f } =\begin{bmatrix} \frac{\partial e_{u}}{\partial f } \\ \frac{\partial e_{v}}{\partial f } \end{bmatrix}=\begin{bmatrix} \frac{X_{c}}{Z_{c}} *r \\ \frac{Y_{c}}{Z_{c}} * r \end{bmatrix}

3) 对 k_{1},k_{2} 求导

                                                                          \frac{\partial e}{\partial k_{1} } =\begin{bmatrix} \frac{\partial e_{u}}{\partial k_{1}} \\ \frac{\partial e_{v}}{\partial k_{1} } \end{bmatrix}=\begin{bmatrix}f* \frac{X_{c}}{Z_{c}} * ||p_{c}||^2 \\f* \frac{Y_{c}}{Z_{c}} * ||p_{c}||^2 \end{bmatrix}

                                                                          \frac{\partial e}{\partial k_{2} } =\begin{bmatrix} \frac{\partial e_{u}}{\partial k_{2}} \\ \frac{\partial e_{v}}{\partial k_{2} } \end{bmatrix}=\begin{bmatrix}f* \frac{X_{c}}{Z_{c}} * ||p_{c}||^4 \\f* \frac{Y_{c}}{Z_{c}} * ||p_{c}||^4 \end{bmatrix}

4) 对 \small P_{w} 求导

                                                                                       \frac{\partial e}{\partial P_{w} } = \frac{\partial e}{\partial P_{c} } * \frac{\partial P_{c}}{\partial P_{w} }

 由于P_{c} = R_{cw}* P_{w}+t_{cw} ,则有

                                                                                              \frac{\partial P_{c}}{\partial P_{w} }= R_{cw}

 因此                                                                                             

                                                                                          \frac{\partial e}{\partial P_{w} } = \frac{\partial e}{\partial P_{c} } * R_{cw}

其中 \frac{\partial e}{\partial P_{c} } 在前面已经推导过了

到这里为止,我们已经推导完了,现在将进入代码介绍。

 

2 代码附上以及注释

1) 完整的代码上一篇文章已经给出,但是那一篇没有加上自己求导,我们需要替换原来的

class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d,VertexCameraBAL, VertexPointBAL>

这个类

//BAL 边
class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d,VertexCameraBAL, VertexPointBAL>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeObservationBAL() {}

    virtual bool read ( std::istream& in ) {return false;}
    virtual bool write ( std::ostream& out ) const {return false;}

    //使用G20数值导,不像书上调用ceres的自动求导功能.
    virtual void computeError() override
    {
        //这里将第0个顶点,相机位姿取出来。
        const VertexCameraBAL* cam = static_cast ( vertex ( 0 ) );
        //这里将第1个顶点,空间点位置取出来。
        const VertexPointBAL* point = static_cast ( vertex ( 1 ) );

        Eigen::Vector2d u;

        CamProjectionWithDistortion(cam->estimate(), point->estimate(), u );

        _error[0] = _measurement[0] - u[0];
        _error[1] = _measurement[1] - u[1];

    }
/***
 * 自己写雅克比矩阵
 */
    virtual void linearizeOplus() override
    {
        //这里将第0个顶点,相机位姿取出来。
        const VertexCameraBAL* cam = static_cast ( vertex ( 0 ) );
        //这里将第1个顶点,空间点位置取出来。
        const VertexPointBAL* point = static_cast ( vertex ( 1 ) );

        //_jacobianOplusXi 为 e 对 T , f , k1, k2 求导 , 其中T为6维.故 Xi 为 2*9
        //_jacobianOplusXi = Eigen::Matrix::Zero();
        //_jacobianOplusXj 为 e 对 空间点 求导 ,故 Xj 为 2*3
        //_jacobianOplusXj = Eigen::Matrix::Zero();

        Eigen::Vector3d P_w;
        Eigen::Vector3d P_c;
        Vector9d camera_param;

        P_w = point->estimate();
        camera_param = cam->estimate();
        World2Camera(camera_param,P_w,P_c);

        double f = camera_param[6];
        double k1 = camera_param[7];
        double k2 = camera_param[8];

        double x_z =  P_c[0]/ P_c[2];
        double y_z =  P_c[1]/ P_c[2];

        double x =  P_c[0];
        double y =  P_c[1];
        double z =  P_c[2];

        double p2 = x_z*x_z + y_z*y_z;
        double r =  1.0 + k1 * p2 + k2 * p2 * p2;
        double fr = f * r;

        Eigen::Matrix de_dp;
        Eigen::Matrix dp_dE;
        Eigen::Vector2d de_df;
        Eigen::Vector2d de_dk1;
        Eigen::Vector2d de_dk2;

        double dr2_dx = 2 * x / (z * z);
        double dr2_dy = 2 * y / (z * z);
        //double dr2_dz = -2 * ( x*x + y*y)/ (z*z*z);

        //    | u |   | f * (-Xc/Zc) *(1 + k1 * ||p_c|| ^ 2 + k2 * ||p_c|| ^ 4 ) |
        //e = |   | - |                                                          |
        //    | v |   | f * (-Yc/Zc) *(1 + k1 * ||p_c|| ^ 2 + k2 * ||p_c|| ^ 4 ) |
        //
        // ||p_c|| = sqrt(x_c * x_c + y_c * y_c) , 代码中用p2表示

        // e[0] = u + f * (Xc/Zc) *(1 + k1 * ||p_c|| ^ 2 + k2 * ||p_c|| ^ 4 )
        // e[1] = v + f * (Yc/Zc) *(1 + k1 * ||p_c|| ^ 2 + k2 * ||p_c|| ^ 4 )

        // de      de       dP'
        //---- = ------ * ------
        // dE      dP'      dE
        // 请把这里的E看成为左扰动,这里矩阵大小为 2 * 6

        //e对相机坐标系下的空间点p求导, 这里矩阵大小为 2 * 3
        //具体形式如下:
        // du         f        f * Xc           2*Xc                             2*Xc
        //------ = ------*r + --------*( k1 * ------- + 2 * k2 * ||p_c|| ^ 2 * -------  )
        // dXc       Zc          Zc             Zc^2                             Zc^2

        // du       f * Xc           2*Yc                             2*Yc
        //----- = -------- * ( k1 * ------- + 2 * k2 * ||p_c|| ^ 2 * ------- )
        // dYc       Zc              Zc^2                             Zc^2

        // du       -f*r*Xc       - 2*f*Xc*p2
        //----- = ----------- + --------------- * ( k1 + 2 * k2 * p2 )
        // dZc        Zc^2            Zc^2
        double k = k1 + 2*k2*p2;

        de_dp(0,0) = fr/z + f* x_z * dr2_dx * k;
        de_dp(0,1) = f*x_z * dr2_dy * k;
        de_dp(0,2) = -fr*x/(z*z)- 2*f*x*p2 * k/(z*z);

        // dv/dXc dv/dYc dv/dZc 推导同上 ,如果对这个矩阵了解的话,其中规律也很容易看出来
        de_dp(1,0) = f*x_z * dr2_dy * k;
        de_dp(1,1) = fr/z + f*y_z * dr2_dy * k;
        de_dp(1,2) = -fr*y/(z*z) - 2*f*y*p2* k/(z*z);

        //p对左扰动求导,具体可以看书上的推导,dP'/dE 只取前三行 ,形式为 [I, -P'^],这里矩阵大小为 3 * 6
        //| 1 0 0   0    Zc -YC|
        //| 0 1 0  -Zc   0   Xc|
        //| 0 0 1  Yc   -Xc   0|
        dp_dE(0,0) = de_dp(0,1) * (-z) + de_dp(0,2) * y;
        dp_dE(0,1) = de_dp(0,0) * z    + de_dp(0,2) * (-x);
        dp_dE(0,2) = de_dp(0,0) * (-y) + de_dp(0,1) * x;

        dp_dE(1,0) = de_dp(1,1) * (-z) + de_dp(1,2) * y;
        dp_dE(1,1) = de_dp(1,0) * z    + de_dp(1,2) * (-x);
        dp_dE(1,2) = de_dp(1,0) * (-y) + de_dp(1,1) * x;

        //e 对 f 进行求导 ,这里矩阵大小为 2 * 1
        //       Xc
        // r * ------
        //       Zc
        de_df[0] = x/z*r;
        de_df[1] = y/z*r;

        //e 对 k1 进行求导 ,这里矩阵大小为 2 * 1
        //       Xc
        // f * ------  * ||p_c||^2
        //       Zc
        de_dk1[0] = f*x/z*p2;
        de_dk1[1] = f*y/z*p2;

        //e 对 k2 进行求导 ,这里矩阵大小为 2 * 1
        //       Xc
        // f * ------  * ||p_c||^4
        //       Zc
        de_dk2[0] = f*x/z*p2*p2;
        de_dk2[1] = f*y/z*p2*p2;

        _jacobianOplusXi.block<2,3>(0,0) = dp_dE;
        _jacobianOplusXi.block<2,3>(0,3) = de_dp;
        _jacobianOplusXi.block<2,1>(0,6) = de_df;
        _jacobianOplusXi.block<2,1>(0,7) = de_dk1;
        _jacobianOplusXi.block<2,1>(0,8) = de_dk2;

         Eigen::Vector3d angleAxis(camera_param[0],camera_param[1], camera_param[2]);
        _jacobianOplusXj = de_dp * Sophus::SO3::exp(angleAxis).matrix();

    }

};

注意,se3 是 平移在前,旋转在后,所以我们要将雅克比矩阵 X_{i} 前三列跟 四五列对换,代码也已经体现,如果不调换,则出错。

2) 这个类也要替换

class VertexCameraBAL : public g2o::BaseVertex<9,Vector9d>

se3 中的平移不是我们需要的,需要转换下,所以要expexp 将李代数转换成李群再来左乘原来的值来进行更新,至于f,k_{1},k_{2}直接相加就可以了

//BAL相机顶点
class VertexCameraBAL : public g2o::BaseVertex<9,Vector9d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexCameraBAL() {}
    virtual void setToOriginImpl() {}

    virtual void oplusImpl ( const double* update )
    {
        Vector9d::ConstMapType v ( update );

        Sophus::Vector6d SE3_Rt;
		//se3是平移在前,旋转在后,另外这里的平移不是我们平时使用的平移
        SE3_Rt << _estimate[3],_estimate[4],_estimate[5] ,_estimate[0],_estimate[1], _estimate[2];

        Sophus::Vector6d update_se3;
        update_se3 << update[3],update[4],update[5],update[0],update[1],update[2];

        Sophus::SE3  Update_SE3 = Sophus::SE3::exp(update_se3)*Sophus::SE3::exp(SE3_Rt);
        Vector9d u;
        u << Update_SE3.log()[3],Update_SE3.log()[4],Update_SE3.log()[5],
                Update_SE3.log()[0],Update_SE3.log()[1],Update_SE3.log()[2],
                _estimate[6] + v[6],_estimate[7] +v[7],_estimate[8] +v[8];
        _estimate = u;
    }

    virtual bool read ( std::istream& in ) { return false;}
    virtual bool write ( std::ostream& out ) const {return false;}
};

3) 对原来的一些地方进行替换,用来加速

一开始这里是用opencv来将旋转向量转成旋转矩阵,其实这一步是可以不用,旋转向量其实就是李代数,所以只用Sophus来运算就可以了

void World2Camera(const Vector9d camera, const Eigen::Vector3d P_w, Eigen::Vector3d& P_c)
{

    //这里的非齐次坐标的变换要注意
    Vector4d Pw(P_w[0],P_w[1],P_w[2],1.0);
    Sophus::Vector6d se3_RT;
    se3_RT << camera[3],camera[4], camera[5],camera[0],camera[1], camera[2];

    Vector4d P = Sophus::SE3::exp(se3_RT).matrix() * Pw;
    P_c[0] = P[0];
    P_c[1] = P[1];
    P_c[2] = P[2];
}

这里同理

void LoadBALProblem::Camera2World(const Eigen::Vector3d angleAxis, const Eigen::Vector3d P_c, Eigen::Vector3d& P_w)
{
    cv::Mat Rcw;
    Sophus::Vector6d Tcw;
    Tcw <

完整代码链接:https://pan.baidu.com/s/15wvmxXcxQlvL5bxHb50-3Q 密码:vup9

运行结果如下:

这里会发现运行时间比上一篇的快很多

《视觉SLAM十四讲》中第十章代码中雅克比的具体形式推导_第1张图片

《视觉SLAM十四讲》中第十章代码中雅克比的具体形式推导_第2张图片

 

没有优化前

《视觉SLAM十四讲》中第十章代码中雅克比的具体形式推导_第3张图片

优化后明显好超级多

《视觉SLAM十四讲》中第十章代码中雅克比的具体形式推导_第4张图片

如果本文有什么错误的地方,请联系我,我及时修改。

转载请注明出处:http://blog.csdn.net/johnnyyeh/article/details/82315543

你可能感兴趣的:(SLAM)