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

最新推荐文章于 2023-03-31 10:33:41 发布

夜下乘凉

最新推荐文章于 2023-03-31 10:33:41 发布

阅读量3.8k

点赞数 7

分类专栏： SLAM

本文链接：https://blog.csdn.net/JohnnyYeh/article/details/82315543

版权

SLAM 专栏收录该内容

3 篇文章 0 订阅

订阅专栏

在上一篇文章《视觉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<const VertexCameraBAL*> ( vertex ( 0 ) );
        //这里将第1个顶点，空间点位置取出来。
        const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( 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<const VertexCameraBAL*> ( vertex ( 0 ) );
        //这里将第1个顶点，空间点位置取出来。
        const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );

        //_jacobianOplusXi 为 e 对 T , f , k1, k2 求导 , 其中T为6维.故 Xi 为 2*9
        //_jacobianOplusXi = Eigen::Matrix<double,2,9>::Zero();
        //_jacobianOplusXj 为 e 对 空间点 求导 ,故 Xj 为 2*3
        //_jacobianOplusXj = Eigen::Matrix<double,2,3>::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<double,2,3> de_dp;
        Eigen::Matrix<double,2,3> 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$ 中的平移不是我们需要的，需要转换下，所以要 $exp$ $exp$ 将李代数转换成李群再来左乘原来的值来进行更新，至于 $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 <<P_c[0], P_c[1], P_c[2], angleAxis[0],angleAxis[1],angleAxis[2];
    Eigen::Matrix4d Twc;
    //Twc = Tcw^-1
    Twc = Sophus::SE3::exp(Tcw).matrix().inverse();
    P_w[0] = Twc(0,3);
    P_w[1] = Twc(1,3);
    P_w[2] = Twc(2,3);
}

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

运行结果如下：

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

没有优化前

优化后明显好超级多

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

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

夜下乘凉

关注

7
点赞
踩
29

收藏

觉得还不错? 一键收藏
13
评论
《视觉SLAM十四讲》中第十章代码中雅克比的具体形式推导

在上一篇文章《视觉SLAM十四讲》第十章g2o代码的简化中，求解BA的雅克比矩阵，我直接使用了g2o的数值导，因为这个雅克比矩阵自己写还真的很容易出错，不过还是有一些小伙伴想要了解具体的形式，所以在这一篇文章中会具体进行推导。1 雅克比矩阵数学推导如果对数学推导不感兴趣的，可以直接跳到代码那里看，另外这里会使用李群和李代数的知识，不清楚的可以看一下《...
复制链接

扫一扫