深蓝学院-多传感器融合定位-第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已经帮我们解决了！1
那我们再来看看他是怎么定义的自动求导：
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 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的代码好复杂，我看不懂呀！
大师兄：看不懂就对了！我也是参考了好几个大佬的博客才明白呢。
请各位同学参考这几个博客网页1 2来学习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_dr
lp = q_last_curr * curr_point + t_last_curr; //new point
Eigen::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);
// Rotation
Eigen::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();
// Translation
Eigen::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_dr
lp = q_last_curr * curr_point + t_last_curr; //new point
Eigen::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);
// Rotation
Eigen::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;
// Translation
Eigen::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 —— 最小二乘建模 ↩︎