多传感器融合定位 第三讲
一、理论
1.1 点云线特征
当
p
i
p_i
pi为 sharp 时,在上一帧中搜索离
p
i
~
\tilde{p_i}
pi~最近的线特征点,并在相邻线上再找一个线特征点,组成直线。
点到直线的距离公式可以写成:
d
ε
=
∥
(
p
i
~
−
p
b
)
×
(
p
i
~
−
p
a
)
∥
∥
p
a
−
p
b
∥
d_\varepsilon = \frac{ \|(\tilde{p_i}-p_b)\times(\tilde{p_i}-p_a) \|}{\|p_a -p_b\|}
dε=∥pa−pb∥∥(pi~−pb)×(pi~−pa)∥
令
d
ε
ˊ
=
(
p
i
~
−
p
b
)
×
(
p
i
~
−
p
a
)
∥
p
a
−
p
b
∥
\acute{d_\varepsilon} = \frac{ (\tilde{p_i}-p_b)\times(\tilde{p_i}-p_a) }{\|p_a -p_b\|}
dεˊ=∥pa−pb∥(pi~−pb)×(pi~−pa),一维残差对应的雅可比为:
ȷ
q
=
∂
d
ε
∂
d
ε
ˊ
∂
d
ε
ˊ
∂
p
i
~
∂
p
i
~
∂
θ
∂
θ
∂
q
ȷ
t
=
∂
d
ε
∂
d
ε
ˊ
∂
d
ε
ˊ
∂
p
i
~
∂
p
i
~
∂
t
\begin{aligned} \jmath_q&= \frac{\partial d_\varepsilon}{\partial \acute{d_\varepsilon}} \frac{\partial \acute{d_\varepsilon}}{\partial \tilde{p_i}} \frac{\partial \tilde{p_i}}{\partial \theta} \frac{\partial \theta}{\partial q} \\ \jmath_t&= \frac{\partial d_\varepsilon}{\partial \acute{d_\varepsilon}} \frac{\partial \acute{d_\varepsilon}}{\partial \tilde{p_i}} \frac{\partial \tilde{p_i}}{\partial t} \end{aligned}
qt=∂dεˊ∂dε∂pi~∂dεˊ∂θ∂pi~∂q∂θ=∂dεˊ∂dε∂pi~∂dεˊ∂t∂pi~其中
∂
d
ε
∂
d
ε
ˊ
=
∂
d
ε
T
d
ε
∂
d
ε
ˊ
=
d
ε
T
ˊ
d
ε
∂
d
ε
ˊ
∂
p
i
~
=
∂
p
i
~
−
p
b
)
×
(
p
i
~
−
p
a
)
∥
p
a
−
p
b
∥
∂
p
i
~
=
1
∥
p
a
−
p
b
∥
{
p
i
~
−
p
b
∂
p
i
~
×
(
p
i
~
−
p
a
)
+
(
p
i
~
−
p
b
)
×
p
i
~
−
p
a
∂
p
i
~
}
=
1
∥
p
a
−
p
b
∥
(
p
a
−
p
b
)
×
∂
p
i
~
∂
θ
=
−
(
R
p
i
)
×
∂
p
i
~
∂
t
=
I
∂
θ
∂
q
=
[
I
3
x
3
0
1
x
3
]
T
\begin{aligned} \frac{\partial d_\varepsilon}{\partial \acute{d_\varepsilon}} &= \frac{\partial \sqrt{d^T_\varepsilon d_\varepsilon} }{\partial \acute{d_\varepsilon}} = \frac{\acute{d^T_\varepsilon} }{d_\varepsilon } \\ \frac{\partial \acute{d_\varepsilon}}{\partial \tilde{p_i}} &= \frac{\partial \frac{ \tilde{p_i}-p_b)\times(\tilde{p_i}-p_a) }{\|p_a -p_b\|}}{\partial \tilde{p_i} } \\&=\frac{1}{\|p_a-p_b\|} \{ \frac{\tilde{p_i}-p_b}{\partial \tilde{p_i}} \times (\tilde{p_i}-p_a)+ (\tilde{p_i}-p_b)\times \frac{\tilde{p_i}-p_a}{\partial \tilde{p_i}} \} \\ &=\frac{1}{\|p_a-p_b\|} (p_a-p_b)_\times \\ \frac{\partial \tilde{p_i}}{\partial \theta} &= -(R_{p_i})\times \\ \frac{\partial \tilde{p_i}}{\partial t} &= I \\ \frac{\partial \theta }{\partial q} &= {\begin{bmatrix} I_{3x3} \\ {0_{1x3}} \end{bmatrix}}^T \end{aligned}
∂dεˊ∂dε∂pi~∂dεˊ∂θ∂pi~∂t∂pi~∂q∂θ=∂dεˊ∂dεTdε=dεdεTˊ=∂pi~∂∥pa−pb∥pi~−pb)×(pi~−pa)=∥pa−pb∥1{∂pi~pi~−pb×(pi~−pa)+(pi~−pb)×∂pi~pi~−pa}=∥pa−pb∥1(pa−pb)×=−(Rpi)×=I=[I3x301x3]T三维残差对应的雅可比为:
ȷ
q
=
∂
d
ε
ˊ
∂
p
i
~
∂
p
i
~
∂
θ
∂
θ
∂
q
ȷ
t
=
∂
d
ε
ˊ
∂
p
i
~
∂
p
i
~
∂
t
\begin{aligned} \jmath_q&= \frac{\partial \acute{d_\varepsilon}}{\partial \tilde{p_i}} \frac{\partial \tilde{p_i}}{\partial \theta} \frac{\partial \theta}{\partial q} \\ \jmath_t&=\frac{\partial \acute{d_\varepsilon}}{\partial \tilde{p_i}} \frac{\partial \tilde{p_i}}{\partial t} \end{aligned}
qt=∂pi~∂dεˊ∂θ∂pi~∂q∂θ=∂pi~∂dεˊ∂t∂pi~实际在代码计算雅可比的时候,最后一项
∂
θ
∂
q
\frac{\partial \theta}{\partial q}
∂q∂θ是通过自定义旋转参数块实现的。该式的雅可比计算也是在这种条件下成立的。参考博客
1.2 点云面特征
当
p
i
p_i
pi为flat时,在上一帧中搜索离
p
i
~
\tilde{p_i}
pi~最近的面特征点,并在相邻线上找两个面特征点,组成平面。
点到面的距离公式可以写成:
d
H
=
∥
(
p
i
~
−
p
j
)
⋅
(
p
l
−
p
j
)
×
(
p
m
−
p
a
)
∥
(
p
l
−
p
j
)
×
(
p
m
−
p
a
)
∥
∥
d_\Eta= \|(\tilde{p_i}-p_j)\cdot\frac{ (p_l-p_j)\times(p_m-p_a) }{\|(p_l-p_j)\times(p_m-p_a)\|}\|
dH=∥(pi~−pj)⋅∥(pl−pj)×(pm−pa)∥(pl−pj)×(pm−pa)∥
令
d
H
ˊ
=
(
p
i
~
−
p
j
)
⋅
(
p
l
−
p
j
)
×
(
p
m
−
p
a
)
∥
(
p
l
−
p
j
)
×
(
p
m
−
p
a
)
∥
\acute{d_\Eta} = (\tilde{p_i}-p_j)\cdot\frac{ (p_l-p_j)\times(p_m-p_a) }{\|(p_l-p_j)\times(p_m-p_a)\|}
dHˊ=(pi~−pj)⋅∥(pl−pj)×(pm−pa)∥(pl−pj)×(pm−pa),对应的雅可比为:
ȷ
q
=
∂
d
H
∂
d
H
ˊ
∂
d
H
ˊ
∂
p
i
~
∂
p
i
~
∂
θ
∂
θ
∂
q
ȷ
t
=
∂
d
H
∂
d
H
ˊ
∂
d
H
ˊ
∂
p
i
~
∂
p
i
~
∂
t
\begin{aligned} \jmath_q&= \frac{\partial d_\Eta}{\partial \acute{d_\Eta}} \frac{\partial \acute{d_\Eta}}{\partial \tilde{p_i}} \frac{\partial \tilde{p_i}}{\partial \theta} \frac{\partial \theta}{\partial q} \\ \jmath_t&= \frac{\partial d_\Eta}{\partial \acute{d_\Eta}} \frac{\partial \acute{d_\Eta}}{\partial \tilde{p_i}} \frac{\partial \tilde{p_i}}{\partial t} \end{aligned}
qt=∂dHˊ∂dH∂pi~∂dHˊ∂θ∂pi~∂q∂θ=∂dHˊ∂dH∂pi~∂dHˊ∂t∂pi~其中由于
d
H
d_\Eta
dH是一维的,它的偏导数
∂
d
H
∂
d
H
ˊ
\frac{\partial d_\Eta}{\partial \acute{d_\Eta}}
∂dHˊ∂dH是正负1。
∂
p
i
~
∂
θ
\frac{\partial \tilde{p_i}}{\partial \theta}
∂θ∂pi~、
∂
θ
∂
q
\frac{\partial \theta}{\partial q}
∂q∂θ、
∂
p
i
~
∂
t
\frac{\partial \tilde{p_i}}{\partial t}
∂t∂pi~和上面一样。
∂
d
H
ˊ
∂
p
i
~
=
(
(
p
l
−
p
j
)
×
(
p
m
−
p
a
)
)
T
∥
(
p
l
−
p
j
)
×
(
p
m
−
p
a
)
∥
\frac{\partial \acute{d_\Eta}}{\partial \tilde{p_i}} =\frac{ ((p_l-p_j)\times(p_m-p_a))^T }{\|(p_l-p_j)\times(p_m-p_a)\|}
∂pi~∂dHˊ=∥(pl−pj)×(pm−pa)∥((pl−pj)×(pm−pa))T
1.3 小结
1.通过最小化点到直线的距离,点到面距离来计算两帧之间的相对变化,实际上就是PLICP算法和PPICP算法的结合。通过提取一些比较明显的特征,进而用PPICP或PLICP来计算更精确的相对变化。LOAM通过曲率来划分在线上的点、面上的点。
2.部分雅可比的推导可能和PPT上面的有出入,比如 ∂ d H ˊ ∂ p i ~ \frac{\partial \acute{d_\Eta}}{\partial \tilde{p_i}} ∂pi~∂dHˊ相差一个转置,主要原因是ceres里面的雅可比用的是行向量,而PPT里面的是列向量。另外在计算线特征的雅可比时,作业中只要求计算线特征一维残差下面的雅可比,而 ALOAM计算的是三维残差,因此也推导了三维残差下面的雅可比。后续的实验可以发现,计算三维残差下面的雅可比得到的解会更优,其中的原因我也不太清楚。
二、代码及实验
2.1 自动求导
实验效果:
2.2 解析求导(一维线特征残差)
核心代码:FILE aloam_factor.hpp ,把原来的 LidarEdgeFactor LidarPlaneFactor 注释掉即可
inline Eigen::Matrix<double,3,3> skew(Eigen::Matrix<double,3,1>& mat_in){
Eigen::Matrix<double,3,3> skew_mat;
skew_mat.setZero();
skew_mat(0,1) = -mat_in(2);
skew_mat(0,2) = mat_in(1);
skew_mat(1,2) = -mat_in(0);
skew_mat(1,0) = mat_in(2);
skew_mat(2,0) = -mat_in(1);
skew_mat(2,1) = mat_in(0);
return skew_mat;
}
class LidarEdgeFactor : public ceres::SizedCostFunction<1, 4, 3>{
public:
LidarEdgeFactor(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
const Eigen::Vector3d last_point_b_, const 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
{
// 存放 w x y z
Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);
Eigen::Vector3d pi = q_last_curr * curr_point + t_last_curr;
Eigen::Vector3d nu = (pi - last_point_b).cross(pi - last_point_a);
Eigen::Vector3d de = last_point_a - last_point_b;
double de_norm = de.norm();
// 残差为一维
residuals[0] = nu.norm() / de_norm;
// 残差为三维
// residuals[0] = nu.x() / de_norm;
// residuals[1] = nu.y() / de_norm;
// residuals[2] = nu.z() / de_norm;
if (jacobians != nullptr)
{
Eigen::Matrix3d skew_de = skew(de);
Eigen::Vector3d rp = q_last_curr*curr_point;
// 线的单位法向量
nu.normalize();
if (jacobians[0] != nullptr)
{
Eigen::Matrix3d neg_dp_dr = -skew(rp);
// 一维残差的雅可比
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) = nu.transpose() * skew_de * neg_dp_dr / de_norm;
// 三维残差的雅可比
// 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_de * neg_dp_dr / de_norm;
}
if(jacobians[1] != nullptr)
{
// 一维残差的雅可比
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) = nu.transpose() * skew_de / de_norm;
// 三维残差的雅可比
// 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_de / de_norm;
}
}
return true;
}
private:
Eigen::Vector3d curr_point, last_point_a, last_point_b;
double s;
public:
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 LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_);
}
};
class LidarPlaneFactor : public ceres::SizedCostFunction<1, 4, 3>
{
public:
LidarPlaneFactor(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,
const double s_)
:curr_point(curr_point_),
last_point_j(last_point_j_),
last_point_l(last_point_l_),
last_point_m(last_point_m_){}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const
{
Eigen::Vector3d ljm_norm = (last_point_l - last_point_j).cross(last_point_m - last_point_j);
// 平面单位法向量
ljm_norm.normalize();
Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);
Eigen::Vector3d pi = q_last_curr * curr_point + t_last_curr;
// 残差函数
double phi1 = (pi - last_point_j).dot(ljm_norm);
residuals[0] = std::fabs(phi1);
if(jacobians != nullptr)
{
if(residuals[0] != 0 && phi1 < 0)
{
// phi1 = phi1 / residuals[0];
phi1 = -phi1;
}
if(jacobians[0] != nullptr)
{
Eigen::Vector3d rp = q_last_curr * curr_point ;
Eigen::Matrix3d neg_dp_dr = -skew(rp);
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) = phi1 * ljm_norm.transpose() * neg_dp_dr;
}
if( jacobians[1] != nullptr)
{
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) = phi1 * ljm_norm.transpose();
}
}
return true;
}
private:
Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
Eigen::Vector3d ljm_norm;
double s;
public:
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,
const double s_)
{
return new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_);
}
};
实验效果:
2.3 解析求导(三维线特征残差)
实验效果:
2.4 解析求导 + 自定义旋转参数块(一维线特征残差)
核心代码:
/*
追加到 aloam_factor.hpp文件
另外修改 aloam_laser_odometry_node.cpp文件 288行附近的代码
ceres::LocalParameterization *q_parameterization =
// new ceres::EigenQuaternionParameterization();
new PoseSO3Parameterization();
*/
class PoseSO3Parameterization : public ceres::LocalParameterization {
public:
PoseSO3Parameterization() { }
virtual ~PoseSO3Parameterization() { }
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const
{
// 待更新的四元数
Eigen::Map<const Eigen::Quaterniond> quater(x);
// delta 值,使用流形 so3 更新
Eigen::Map<const Eigen::Vector3d> delta_so3(delta);
// 更新后的四元数
Eigen::Map<Eigen::Quaterniond> quter_plus(x_plus_delta);
// so3 转换位 delta_p 四元数
Eigen::Quaterniond delta_quater = Sophus::SO3d::exp(delta_so3).unit_quaternion();
// 旋转更新公式
quter_plus = (delta_quater*quater).normalized();
return true;
}
virtual bool ComputeJacobian(const double* x, double* jacobian) const // 四元数对so3的偏导数
{
Eigen::Map<Eigen::Matrix<double, 4, 3, Eigen::RowMajor>> j(jacobian);
(j.topRows(3)).setIdentity();
(j.bottomRows(1)).setZero();
return true;
}
// virtual bool MultiplyByJacobian(const double* x,
// const int num_rows,
// const double* global_matrix,
// double* local_matrix) const;//一般不用
virtual int GlobalSize() const {return 4;} // 参数的实际维数
virtual int LocalSize() const {return 3;} // 正切空间上的参数维数
};
实验效果:
2.5 解析求导 + 自定义旋转参数块(三维线特征残差)
实验效果:
2.6 小结
1.实验上看,自动求导和解析求导(三维线特征残差)的精度是差不多的。解析求导(一维线特征残差)效果好差。不太明白为什么解析求导(三维线特征残差)比解析求导(一维线特征残差)好这么多。
2.加入自定义的旋转参数块对解析求导(一维线特征残差)提升还是非常明显,轨迹正常了很多,精度和对解析求导(三维线特征残差)差不多了。对解析求导(三维线特征残差)提升不是很明显,毕竟原本已经相对精确了。
3.这个算法在拐弯的时候误差有点大,跑代码的时候记得清空trajectory里面的内容。
三、讨论及拓展
疑惑
1.为什么解析求导(三维线特征残差)比解析求导(一维线特征残差)好这么多?
2.增加自定义旋转参数块为什么对解析求导(一维线特征残差)影响这么大?
3.第三章作业参考跑的解析求导(一维线特征残差)的效果跟我实际效果差别挺大,看了一下源码,作者添加的位置都是不在aloam_factor.hpp文件,好像根本就没运行修改的代码,一直都跑的是自动求导?