《视觉SLAM十四讲》中第十章代码中雅克比的具体形式推导
在上一篇文章《视觉SLAM十四讲》第十章g2o代码的简化 中，求解BA的雅克比矩阵，我直接使用了g2o的数值导，因为这个雅克比矩阵自己写还真的很容易出错，不过还是有一些小伙伴想要了解具体的形式，所以在这一篇文章中会具体进行推导。
1 雅克比矩阵数学推导
如果对数学推导不感兴趣的，可以直接跳到代码那里看，另外这里会使用李群和李代数的知识，不清楚的可以看一下《视觉SLAM十四讲》，里面介绍的很基础，也可以去查阅其他文章。
1.1 误差函数形式
Bundle Ajustment in Large中给出了世界坐标下的空间点Pw、以及外参Rcw,tcw和内参f。
1) 我们要求误差，那么需要将空间点重投影到相机坐标系，一般形式如下：
其中 
为变换矩阵，
为世界坐标下的空间点，形式为 
，
类似
2) 将 归一化得到 
, 形式为 
,令 
,这里需要注意，官方数据集中
，写代码容易疏忽。
3） 去除畸变，并且转换到像素坐标系得到 
其中 
，而在这个Bundle Ajustment in Large 的数据集中，
,
。
4) 由于我们已知观测数据(数据集里给出) 
,误差函数的形式如下:
我们取前面两行
如果将官方的数据要求带入，那么我们误差形式应该如下：
1.2 误差函数的雅克比矩阵形式
在得到误差后，我们需要将误差 
降到最小，所以我们要找到误差函数的极小值，而且误差函数往往是非线性的，所以我们要泰勒展开转成线性的，然后迭代求解，假设大家对梯度下降法、高斯牛顿法，LM算法有一定的了解，所以对优化算法不具体讨论。
1.2.1 对误差函数一阶泰勒展开
其中 
为雅克比矩阵，也就是我们要求的量， 另外我们是对所有的位姿和所有的空间进行优化，那么它的雅克比矩阵就不会是这么简单的形式，它是由许许多多的雅克比矩阵构成的一个
矩阵，这个矩阵的维度很大，如果对其求逆，会非常耗时，对SLAM这种讲究实时性的系统而言是致命的，但是这个矩阵有个优点，它是一个稀疏矩阵，这有许多方法进行优化，在这里也不具体讨论，详细可以参考书本。
结合了所有位姿、内外参 (
) 和空间点(
)的误差函数如下：
其中
为9维的，
为对位姿、内外参的偏导数，
为对空间点的偏导数
1.2.2 、 的具体形式
我们需要对 
这五个量(9维)一起优化，另外我们还要对空间点(3维)优化，我们要写两个雅克比矩阵，它的样子如下
让我们开始一项项写出来：
1) 由于旋转矩阵
对加法不封闭，所以需要转化到李代数上来求导,这里是对左扰动求偏导,使用链式法则
对
的求导要特别细心才行，这一步运算很容易出错，而且这个矩阵会被使用多次，所以一定不能写错
每一项如下：
其中 
，
，会用到这个公式
对对左扰动的求导，这里直接套用书上的公式了，不做具体推导，这是它的形式 
。^ 符号为反对称矩阵。
2) 对 
求导
3) 对 
求导
4) 对 求导
由于
，则有
因此
其中 
在前面已经推导过了
到这里为止，我们已经推导完了，现在将进入代码介绍。
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();
}
};
注意，
是 平移在前，旋转在后，所以我们要将雅克比矩阵 
前三列跟 四五列对换，代码也已经体现，如果不调换，则出错。
2) 这个类也要替换
class VertexCameraBAL : public g2o::BaseVertex<9,Vector9d> 中的平移不是我们需要的，需要转换下，所以要
将李代数转换成李群再来左乘原来的值来进行更新，至于
直接相加就可以了
//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
运行结果如下：
这里会发现运行时间比上一篇的快很多
没有优化前
优化后明显好超级多
如果本文有什么错误的地方，请联系我，我及时修改。
转载请注明出处：http://blog.csdn.net/johnnyyeh/article/details/82315543