深蓝学院-多传感器融合定位-第3章作业

深蓝学院-多传感器融合定位-第三章作业

题目内容
第一题：残差模型推导
第二题: Ceres解析求导
- Ceres的使用
- Ceres解析求导：线特征残差
- Ceres解析求导：面特征残差
- Ceres和Eigen的坑点
第三题：EVO结果分析

题目内容

第一题：残差模型推导

补充：鉴于写代码时发现A-LOAM的更新的参数块是四元数param_q和平移矢量param_t，所以我重新写了关于R，t的李代数求导，这样就可以分别对R和t进行残差更新：

第二题: Ceres解析求导

大师兄：Ceres库有自动求导、数值求导和解析求导，A-LOAM中用的是自动求导但是效率会比较低。

本题代码就是要实现Ceres的解析求导，所以我们主要的工作是输入第一题中得到的Jacobian。
（听起来很简单对吧？）

首先我们可以看到在alom_laser_odometry_node.cpp中Line 382左右是线特征的CostFunction:

ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);

大师兄：这就是我们要着手的地方了，接着进入LidarEdgeFactor，看看这个它自动求导是怎么写的，以及我们如何自己手写解析求导。
小萝卜：大师兄，且慢！problem.AddResidualBlock()每个参数的含义是什么？
大师兄：小萝卜，这就对了，懂得问问题啦！
大师兄：这里的cost_function的我们要定义残差Residual和导数Jacobian的地方，loss_function其实就是Ceres的核函数（暂且不管他），para_q和para_t分别是我们要优化的参数块。

小萝卜：谢谢大师兄，我还有一问！为什么我们一开始手写的Jacobian是用了李代数，算Residual对Transformation矩阵的导数，现在怎么变成对q和t求导了呢？对四元数求导？我可不会呀！
大师兄：这个问题问得很好！老纳一开始也在这个问题上栽了根头。我就在纳闷儿，我们公式是对T求导，然后咱们分成了R和t求导，再后来还变成了q和t求导了呢！
大师兄：其实啊，就是咱们这里把Ceres对T的优化，拆分开了分别对R和t进行优化(SO3)，然而由于咱们的输入parameters是四元数q和t，所以咱们还要把R里面3维的旋转向量v转成4维的q。（当然也可以不拆分开用SE(3)）
大师兄：这一步，其实Ceres自带的LocalParameterization已经帮我们解决了！¹

那我们再来看看他是怎么定义的自动求导：

struct LidarEdgeFactor
{LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,Eigen::Vector3d last_point_b_, double s_): curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}template <typename T>bool operator()(const T *q, const T *t, T *residual) const{Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};q_last_curr = q_identity.slerp(T(s), q_last_curr);Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};Eigen::Matrix<T, 3, 1> lp;lp = q_last_curr * cp + t_last_curr;Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);Eigen::Matrix<T, 3, 1> de = lpa - lpb;residual[0] = nu.x() / de.norm();residual[1] = nu.y() / de.norm();residual[2] = nu.z() / de.norm();return true;}static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,const Eigen::Vector3d last_point_b_, const double s_){return (new ceres::AutoDiffCostFunction<LidarEdgeFactor, 3, 4, 3>(new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));}Eigen::Vector3d curr_point, last_point_a, last_point_b;double s;
};

Ceres的使用

小萝卜：大师兄大师兄，这个Ceres的代码好复杂，我看不懂呀！
大师兄：看不懂就对了！我也是参考了好几个大佬的博客才明白呢。

请各位同学参考这几个博客网页¹ ²来学习Ceres。

Ceres解析求导：线特征残差

// EdgeAnalyticCostFunction
class LidarEdgeAnalyticCostFunction : public ceres::SizedCostFunction<3, 4, 3> {public:LidarEdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,Eigen::Vector3d last_point_b_, double s_): curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}virtual bool Evaluate(double const* const* parameters,double* residuals,double** jacobians) const {Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);Eigen::Vector3d lp;Eigen::Vector3d lp_r;lp_r = q_last_curr * curr_point; // for computing Jacobian of Rotation: dp_by_drlp = q_last_curr * curr_point + t_last_curr; //new pointEigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b);Eigen::Vector3d de = last_point_a - last_point_b;residuals[0] = nu.x() / de.norm();residuals[1] = nu.y() / de.norm();residuals[2] = nu.z() / de.norm();if(jacobians != NULL){if(jacobians[0] != NULL){Eigen::Vector3d re = last_point_b - last_point_a;Eigen::Matrix3d skew_re = skew(re);//  RotationEigen::Matrix3d skew_lp_r = skew(lp_r);Eigen::Matrix<double, 3, 3> dp_by_dr;dp_by_dr.block<3,3>(0,0) = -skew_lp_r;Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor> > J_so3_r(jacobians[0]);J_so3_r.setZero();J_so3_r.block<3,3>(0,0) = skew_re * dp_by_dr / de.norm();// TranslationEigen::Matrix<double, 3, 3> dp_by_dt;(dp_by_dt.block<3,3>(0,0)).setIdentity();Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > J_so3_t(jacobians[1]);J_so3_t.setZero();J_so3_t.block<3,3>(0,0) = skew_re * dp_by_dt / de.norm();  }}return true;}protected:Eigen::Vector3d curr_point, last_point_a, last_point_b;double s;
};

这里写完以后，记得改problem的costFunction:

ceres::CostFunction *cost_function = new LidarEdgeAnalyticCostFunction(curr_point, last_point_a, last_point_b, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);

Ceres解析求导：面特征残差

// PlaneAnalyticCostFunction
class LidarPlaneAnalyticCostFunction : public ceres::SizedCostFunction<1, 4, 3> {public:LidarPlaneAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_): curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_), last_point_m(last_point_m_), s(s_) {}virtual bool Evaluate(double const* const* parameters,double* residuals,double** jacobians) const {Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);Eigen::Vector3d lp;Eigen::Vector3d lp_r;lp_r = q_last_curr * curr_point; // for computing Jacobian of Rotation: dp_drlp = q_last_curr * curr_point + t_last_curr; //new pointEigen::Vector3d de = (last_point_l-last_point_j).cross(last_point_m-last_point_j);double nu = (lp-last_point_j).dot(de);residuals[0] = nu / de.norm();if(jacobians != NULL){if(jacobians[0] != NULL){Eigen::Vector3d dX_dp = de / de.norm();double X = nu / de.norm();Eigen::Vector3d ddh_dp = X * dX_dp / std::abs(X);//  RotationEigen::Matrix3d skew_lp_r = skew(lp_r);Eigen::Matrix<double, 3, 3> dp_dr;dp_dr.block<3,3>(0,0) = -skew_lp_r;Eigen::Map<Eigen::Matrix<double, 1, 4, Eigen::RowMajor> > J_so3_r(jacobians[0]);J_so3_r.setZero();J_so3_r.block<1,3>(0,0) = ddh_dp.transpose() * dp_dr;// TranslationEigen::Matrix<double, 3, 3> dp_dt;(dp_dt.block<3,3>(0,0)).setIdentity();Eigen::Map<Eigen::Matrix<double, 1, 3, Eigen::RowMajor> > J_so3_t(jacobians[1]);J_so3_t.setZero();J_so3_t.block<1,3>(0,0) = ddh_dp.transpose() * dp_dt;}}return true;}protected:Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;double s;
};

Ceres和Eigen的坑点

Eigen Library requires the data type to be consistent, eg.

// Since de is Vector3d, thus nu must be double
Eigen::Vector3d de = (last_point_l-last_point_j).cross(last_point_m-last_point_j);
double nu = (lp-last_point_j).dot(de);

Eigen Vector product must use explicitly .dot() pr .cross()

第三题：EVO结果分析

APE
RPE

RVIZ中的结果，熟悉的Kitti图，可以看到在最后转弯的时候还是有点偏离绿色的GNSS轨迹，不过整体效果还是不错！

yuntian_li, Ceres详解（一） CostFunction ↩︎ ↩︎
leeayu, Ceres Tutotial —— 最小二乘建模 ↩︎

深蓝学院-多传感器融合定位-第3章作业相关推荐

深蓝学院-多传感器融合定位-第7章作业
深蓝学院-多传感器融合定位-第7章作业 1. 及格题 2. 良好题 Parameter - verison 1 Parameter - verison 2 Parameter - verison 3 ...
深蓝学院-多传感器融合定位课程-第2章-3D激光里程计I
专栏文章: 深蓝学院-多传感器融合定位课程-第1章-概述_goldqiu的博客-CSDN博客 github保存了相关代码和学习笔记: Shenlan-Course-Multi-Sensor-Fusio ...
深蓝学院-多传感器融合定位课程-第5章-惯性导航原理及误差分析
专栏文章: 深蓝学院-多传感器融合定位课程-第1章-概述_goldqiu的博客-CSDN博客深蓝学院-多传感器融合定位课程-第2章-3D激光里程计I_goldqiu的博客-CSDN博客深蓝学院-多 ...
多传感器融合定位第四章点云地图构建及基于点云地图定位
多传感器融合定位第四章点云地图构建及基于点云地图定位代码下载 https://github.com/kahowang/sensor-fusion-for-localization-and-map ...
多传感器融合定位第六章惯性导航结算及误差模型
多传感器融合定位第六章惯性导航结算及误差模型参考博客:深蓝学院-多传感器融合定位-第6章作业代码下载:https://github.com/kahowang/sensor-fusion-for ...
看深蓝学院多传感器融合课程的笔记
贝叶斯推断和贝叶斯公式是整个卡尔曼的核心. 贝叶斯滤波比卡尔曼滤波比卡尔曼滤波更宽泛. 误差作为状态量融合工程师很多时候是调参工程师,卡尔曼滤波的Q R,需要调参掌握观测方程的推导.13章第一 ...
多传感器融合定位第八章基于滤波的融合方法进阶
多传感器融合定位第八章基于滤波的融合方法进阶参考博客:深蓝学院-多传感器融合定位-第8章作业代码下载:https://github.com/kahowang/sensor-fusion-for ...
多传感器融合定位ch1
多传感器融合定位ch1 目录多传感器融合定位ch1 1 数据集下载 2 数据集测试深蓝学院多传感器融合定位课程笔记, 采用的软硬件环境如下硬件: 华擎DeskMini X300W迷你主机软件: ...
多传感器融合定位三-3D激光里程计其三：点云畸变补偿
多传感器融合定位三-3D激光里程计其三:点云畸变补偿 1. 产生原因 2. 补偿方法 Reference: 深蓝学院-多传感器融合多传感器融合定位理论基础文章跳转: 多传感器融合定位一-3D激光里 ...
多传感器融合定位五-点云地图构建及定位
多传感器融合定位五-点云地图构建及定位 1. 回环检测 1.1 基于Scan Context 1.2 基于直方图 2. 后端优化 2.1 后端优化基本原理 2.2 李群.李代数基本知识 2.3 李群. ...