多传感器融合定位 第三讲 3D激光里程计2

一、理论

1.1 点云线特征

当 $p_i$为 sharp 时，在上一帧中搜索离$\widetilde{p_i}$最近的线特征点，并在相邻线上再找一个线特征点，组成直线。

点到直线的距离公式可以写成：$$d_{\epsilon }=\frac{\Vert (\widetilde{p_i}-p_b)\times (\widetilde{p_i}-p_a)\Vert }{\Vert p_a-p_b\Vert }$$
令$\acute{d_{\epsilon }}=\frac{(\widetilde{p_i}-p_b)\times (\widetilde{p_i}-p_a)}{\Vert p_a-p_b\Vert }$，一维残差对应的雅可比为:
$$\begin{array}{rl}{ȷ}_q& =\frac{\partial d_{\epsilon }}{\partial \acute{d_{\epsilon }}}\frac{\partial \acute{d_{\epsilon }}}{\partial \widetilde{p_i}}\frac{\partial \widetilde{p_i}}{\partial \theta }\frac{\partial \theta }{\partial q}\\ {ȷ}_t& =\frac{\partial d_{\epsilon }}{\partial \acute{d_{\epsilon }}}\frac{\partial \acute{d_{\epsilon }}}{\partial \widetilde{p_i}}\frac{\partial \widetilde{p_i}}{\partial t}\end{array}$$其中
∂ 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εT​dε​ ​​=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=[I3x3​01x3​​]T​三维残差对应的雅可比为：
$$\begin{array}{rl}{ȷ}_q& =\frac{\partial \acute{d_{\epsilon }}}{\partial \widetilde{p_i}}\frac{\partial \widetilde{p_i}}{\partial \theta }\frac{\partial \theta }{\partial q}\\ {ȷ}_t& =\frac{\partial \acute{d_{\epsilon }}}{\partial \widetilde{p_i}}\frac{\partial \widetilde{p_i}}{\partial t}\end{array}$$实际在代码计算雅可比的时候，最后一项$\frac{\partial \theta }{\partial q}$是通过自定义旋转参数块实现的。该式的雅可比计算也是在这种条件下成立的。参考博客

1.2 点云面特征

当$p_i$为flat时，在上一帧中搜索离$\widetilde{p_i}$最近的面特征点，并在相邻线上找两个面特征点，组成平面。

点到面的距离公式可以写成：
$$d_{\mathrm{H}}=\Vert (\widetilde{p_i}-p_j)\cdot \frac{(p_l-p_j)\times (p_m-p_a)}{\Vert (p_l-p_j)\times (p_m-p_a)\Vert }\Vert$$
令$\acute{d_{\mathrm{H}}}=(\widetilde{p_i}-p_j)\cdot \frac{(p_l-p_j)\times (p_m-p_a)}{\Vert (p_l-p_j)\times (p_m-p_a)\Vert }$，对应的雅可比为:
$$\begin{array}{rl}{ȷ}_q& =\frac{\partial d_{\mathrm{H}}}{\partial \acute{d_{\mathrm{H}}}}\frac{\partial \acute{d_{\mathrm{H}}}}{\partial \widetilde{p_i}}\frac{\partial \widetilde{p_i}}{\partial \theta }\frac{\partial \theta }{\partial q}\\ {ȷ}_t& =\frac{\partial d_{\mathrm{H}}}{\partial \acute{d_{\mathrm{H}}}}\frac{\partial \acute{d_{\mathrm{H}}}}{\partial \widetilde{p_i}}\frac{\partial \widetilde{p_i}}{\partial t}\end{array}$$其中由于$d_{\mathrm{H}}$是一维的，它的偏导数$\frac{\partial d_{\mathrm{H}}}{\partial \acute{d_{\mathrm{H}}}}$是正负1。$\frac{\partial \widetilde{p_i}}{\partial \theta }$、$\frac{\partial \theta }{\partial q}$、$\frac{\partial \widetilde{p_i}}{\partial t}$和上面一样。
$$\frac{\partial \acute{d_{\mathrm{H}}}}{\partial \widetilde{p_i}}=\frac{((p_l-p_j)\times (p_m-p_a){)}^T}{\Vert (p_l-p_j)\times (p_m-p_a)\Vert }$$

1.3 小结

1.通过最小化点到直线的距离，点到面距离来计算两帧之间的相对变化，实际上就是PLICP算法和PPICP算法的结合。通过提取一些比较明显的特征，进而用PPICP或PLICP来计算更精确的相对变化。LOAM通过曲率来划分在线上的点、面上的点。

2.部分雅可比的推导可能和PPT上面的有出入，比如$\frac{\partial \acute{d_{\mathrm{H}}}}{\partial \widetilde{p_i}}$相差一个转置，主要原因是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文件，好像根本就没运行修改的代码，一直都跑的是自动求导？

四、其他参考

1.第三章作业参考