多传感器集成定位 三章 3D激光里程计2
参考博客：
slam中ceres总结常用用法
参考代码：
激光里程计基于线性特征
代码下载：
https://github.com/kahowang/sensor-fusion-for-localization-and-mapping/tree/main/第三章 3D激光里程计2/激光里程计lidar_localization
环境配置：
Sophus 编译
问题1:系统找不到Sophus 库
问题2：缺少 #include <sophus/so3.hpp>
解决办法：
均是因为sophus由于库安装不当，可以选择安装更通用、更广泛的版本，高博slam14讲中的sophus版本
#下载高博slam14讲的sophus版本后 cd Sophus mkdir build cd build make -j8 sudo make install 问题3：缺少 libglog.so
参考博客：
make[2]: *** 没有规则可以创建shared/libaplcommon-pdr.so目标/usr/lib/x86_64-linux-gnu/libcrypto.so”。 停止。
解决办法：
sudo apt-get install libgoogle-glog-dev 基于线面特征的位姿优化
公式推导
请注意，这里的推导形式是单独更新R ，t ，与 直接更新转换矩阵T的导数会有所不同
参考博客：基于线面特征的激光里程计
点到线雅克比推导:
点到线的距离 :
d ε = ∣ ( p i ′ ? p b ) × ( p i ′ ? p a ) ∣ ∣ p a ? p b ∣ d_{\varepsilon}=\frac{\left|\left(p_{i}^{\prime}-p_{b}\right) \times\left(p_{i}^{\prime}-p_{a}\right)\right|}{\left|p_{a}-p_{b}\right|} dε=∣pa?pb∣∣(pi′−pb)×(pi′−pa)∣ 线特征残差雅可比： J ε = α d ε α T = α d ε α p i ′ α p i ′ α T J_{\varepsilon}=\frac{\alpha d_{\varepsilon}}{\alpha T}=\frac{\alpha d_{\varepsilon}}{\alpha p_{i}^{\prime}} \frac{\alpha p_{i}^{\prime}}{\alpha T} Jε=αTαdε=αpi′αdεαTαpi′ 对平移的雅可比： α p i ′ α t = I \frac{\alpha p_{i}^{\prime}}{\alpha t}=I αtαpi′=I 对旋转的雅可比： α p i ′ α R = − ( R p i ) × \frac{\alpha p_{i}^{\prime}}{\alpha R}=-\left(R p_{i}\right)_{\times} αRαpi′=−(Rpi)× 求等号右边第一项的偏导数, 令 X = ( p i ′ − p b ) × ( p i ′ − p a ) X=\left(p_{i}^{\prime}-p_{b}\right) \times\left(p_{i}^{\prime}-p_{a}\right) X=(pi′−pb)×(pi′−pa) 推导得： α d ε α p i = α d ε α ∣ X ∣ α ∣ X ∣ α X α X α p i = 1 ∣ p a − p b ∣ α ∣ X ∣ α X α X α p i ′ = 1 ∣ p a − p b ∣ X ∣ X ∣ α X α p i ′ = 1 ∣ p a − p b ∣ ( p i ′ − p b ) × ( p i ′ − p a ) ∣ ( p i ′ − p b ) × ( p i ′ − p a ) ∣ ( − ( p i ′ − p a ) × + ( p i ′ + p b ) × ) = ( p i ′ − p b ) × ( p i ′ − p a ) ∣ ( p i ′ − p b ) × ( p i ′ − p a ) ∣ ( p a − p b ) × ∣ p a − p b ∣ \begin{gathered} \frac{\alpha d_{\varepsilon}}{\alpha p_{i}}=\frac{\alpha d_{\varepsilon}}{\alpha|X|} \frac{\alpha|X|}{\alpha X} \frac{\alpha X}{\alpha p_{i}} \\ =\frac{1}{\left|p_{a}-p_{b}\right|} \frac{\alpha|X|}{\alpha X} \frac{\alpha X}{\alpha p_{i}^{\prime}}=\frac{1}{\left|p_{a}-p_{b}\right|} \frac{X}{|X|} \frac{\alpha X}{\alpha p_{i}^{\prime}} \\ =\frac{1}{\mid p_{a}-p_{b}} \mid \frac{\left(p_{i}^{\prime}-p_{b}\right) \times\left(p_{i}^{\prime}-p_{a}\right)}{\left|\left(p_{i}^{\prime}-p_{b}\right) \times\left(p_{i}^{\prime}-p_{a}\right)\right|}\left(-\left(p_{i}^{\prime}-p_{a}\right)_{\times}+\left(p_{i}^{\prime}+p_{b}\right)_{\times}\right) \\ =\frac{\left(p_{i}^{\prime}-p_{b}\right) \times\left(p_{i}^{\prime}-p_{a}\right)}{\left|\left(p_{i}^{\prime}-p_{b}\right) \times\left(p_{i}^{\prime}-p_{a}\right)\right|} \frac{\left(p_{a}-p_{b}\right)_{\times}}{\left|p_{a}-p_{b}\right|} \end{gathered} αpiαdε=α∣X∣αdεαXα∣X∣αpiαX=∣pa−pb∣1αXα∣X∣αpi′αX=∣pa−pb∣1∣X∣Xαpi′αX=∣pa−pb1∣∣(pi′−pb)×(pi′−pa)∣(pi′−pb)×(pi′−pa)(−(pi′−pa)×+(pi′+pb)×)= 标签： cd22激光传感器371395传感器线束z系列激光传感器