多传感器融合定位二-3D激光里程计其二：NDT
多传感器融合定位二-3D激光里程计其二：NDT
- 1. 经典NDT
- 2. 计算方式
- 2.1 2D场景求解:
- 2.2 3D场景求解：
- 3. 其他 NDT
Reference:
- 深蓝学院-多传感器融合
- 多传感器融合定位理论基础
文章跳转：
- 多传感器融合定位一-3D激光里程计其一：ICP
- 多传感器融合定位二-3D激光里程计其二：NDT
- 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
- 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
- 多传感器融合定位五-点云地图构建及定位
- 多传感器融合定位六-惯性导航原理及误差分析
- 多传感器融合定位七-惯性导航解算及误差分析其一
- 多传感器融合定位八-惯性导航解算及误差分析其二
- 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
- 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
- 多传感器融合定位十一-基于滤波的融合方法Ⅱ
- 多传感器融合定位十二-基于图优化的建图方法其一
- 多传感器融合定位十三-基于图优化的建图方法其二
- 多传感器融合定位十四-基于图优化的定位方法
- 多传感器融合定位十五-多传感器时空标定(综述)
1. 经典NDT
NDT 核心思想：基于概率的匹配。目标是将点集 Y Y Y 匹配到固定的点集 X X X 中。这里的联合概率说的是将 X X X 划分成栅格(如下图)，每个栅格里面都有很多点，这些点可以计算一个 均值&&协方差，这是一个概率的概念。而联合概率是： Y Y Y 往 X X X 上旋转的时候，它落在栅格中的哪个格子里是知道的。根据落在格子里的点，本身 X X X 格子已经有了一个 均值/协方差，而 Y Y Y 的这些落在格子里的点可以形成一个联合概率。这个联合概率会作为有没匹配好的一个指标。
点集：
X = { x 1 , x 2 , ⋯ , x N x } Y = { y 1 , y 2 , ⋯ , y N y } \begin{aligned} & X=\left\{x_1, x_2, \cdots, x_{N_x}\right\} \\ & Y=\left\{y_1, y_2, \cdots, y_{N_y}\right\} \end{aligned} X={x1,x2,⋯,xNx}Y={y1,y2,⋯,yNy}目标: max Ψ = max ∏ i = 1 N y f ( X , T ( p , y i ) ) \max \Psi=\max \prod_{i=1}^{N_y} f\left(X, T\left(p, y_i\right)\right) maxΨ=max∏i=1Nyf(X,T(p,yi))(与ICP这里有些区别，ICP的是点到点的距离，这里变成了联合概率)
2D模型: p = p 3 = [ t x t y ϕ z ] T \quad p=p_3=\left[\begin{array}{lll}t_x & t_y & \phi_z\end{array}\right]^{\mathrm{T}} p=p3=[txtyϕz]T
3D模型: p = p 6 = [ t x t y t z ϕ x ϕ y ϕ z ] T \quad p=p_6=\left[\begin{array}{llllll}t_x & t_y & t_z & \phi_x & \phi_y & \phi_z\end{array}\right]^{\mathrm{T}} p=p6=[txtytzϕxϕyϕz]T
2. 计算方式
均值和协方差：
μ = 1 N x ∑ i = 1 N x x i , Σ = 1 N x − 1 ∑ i = 1 N x ( x i − μ ) ( x i − μ ) T \mu=\frac{1}{N_x} \sum_{i=1}^{N_x} x_i, \quad \boldsymbol{\Sigma}=\frac{1}{N_x-1} \sum_{i=1}^{N_x}\left(x_i-\mu\right)\left(x_i-\mu\right)^{\mathrm{T}} μ=Nx1i=1∑Nxxi,Σ=Nx−11i=1∑Nx(xi−μ)(xi−μ)T根据预测的位姿，对点进行旋转和平移(这里的旋转和平移是一个初始预测值)：
y i ′ = T ( p , y i ) = R y i + t y_i^{\prime}=T\left(p, y_i\right)=R y_i+t yi′=T(p,yi)=Ryi+t旋转和平移后的点与目标点集中的点在同一坐标系下，此时可计算各点的联合概率：
f ( X , y i ′ ) = 1 2 π ∣ Σ ∣ exp ( − ( y i ′ − μ ) T Σ − 1 ( y i ′ − μ ) 2 ) f\left(X, y_i^{\prime}\right)=\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}} \exp \left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}\right) f(X,yi′)=2π ∣Σ∣ 1exp(−2(yi′−μ)TΣ−1(yi′−μ))所有点的联合概率(就是所有点的概率乘一起)：
Ψ = ∏ i = 1 N y f ( X , T ( p , y i ) ) = ∏ i = 1 N y 1 2 π ∣ Σ ∣ exp ( − ( y i ′ − μ ) T Σ − 1 ( y i ′ − μ ) 2 ) \begin{aligned} \Psi & =\prod_{i=1}^{N_y} f\left(X, T\left(p, y_i\right)\right) \\ & =\prod_{i=1}^{N_y} \frac{1}{\sqrt{2 \pi} \sqrt{|\mathbf{\Sigma}|}} \exp \left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}\right) \end{aligned} Ψ=i=1∏Nyf(X,T(p,yi))=i=1∏Ny2π ∣Σ∣ 1exp(−2(yi′−μ)TΣ−1(yi′−μ))目标是让所有点的联合概率最大。但是上式中的 R 和 t 是在 y i ′ y'_i yi′ 内的。这里还有一个exp，这个指数项会让公式变得复杂，需要消掉指数项。这时去对数可以解决。
取对数，简化问题(目标是让所有点的联合概率最大)：
ln Ψ = ∑ i = 1 N y ( − ( y i ′ − μ ) T Σ − 1 ( y i ′ − μ ) 2 + ln ( 1 2 π ∣ Σ ∣ ) ) \ln \Psi=\sum_{i=1}^{N_y}\left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}+\ln \left(\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}}\right)\right) lnΨ=i=1∑Ny(−2(yi′−μ)TΣ−1(yi′−μ)+ln(2π ∣Σ∣ 1))去除常数项 ln ( 1 2 π ∣ Σ ∣ ) \ln \left(\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}}\right) ln(2π ∣Σ∣ 1)，得到：
max Ψ = max ln Ψ = min Ψ 1 = min ∑ i = 1 N y ( y i ′ − μ ) T Σ − 1 ( y i ′ − μ ) \max \Psi=\max \ln \Psi=\min \Psi_1=\min \sum_{i=1}^{N_y}\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right) maxΨ=maxlnΨ=minΨ1=mini=1∑Ny(yi′−μ)TΣ−1(yi′−μ)因为之前是负数，我们上式只需要求最小值就行。(这里又变成马氏距离了)
这时就变成了一个优化问题了：
目标函数： min ∑ i = 1 N y ( y i ′ − μ ) T Σ − 1 ( y i ′ − μ ) \min \sum_{i=1}^{N_y}\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right) min∑i=1Ny(yi′−μ)TΣ−1(yi′−μ)
y i ′ = T ( p , y i ) = R y i + t \quad \quad \quad \quad y_i^{\prime}=T\left(p, y_i\right)=R y_i+t yi′=T(p,yi)=Ryi+t
待求参数： R , t R, t R,t
定义残差函数： f i ( p ) = y i ′ − μ f_i(p)=y_i^{\prime}-\mu fi(p)=yi′−μ
按照高斯牛顿法的流程，只需计算残差函数关于待求参数的雅可比，便可迭代优化。
J i = d f i ( p ) d p J_i=\frac{d f_i(p)}{d p} Ji=dpdfi(p)
2.1 2D场景求解:
p = [ t x t y ϕ z ] T y i ′ = T ( p , y i ) = [ cos ϕ z − sin ϕ z sin ϕ z cos ϕ z ] y i + [ t x t y ] \begin{aligned} p & =\left[\begin{array}{lll} t_x & t_y & \phi_z \end{array}\right]^{\mathrm{T}} \\ \\ y_i^{\prime} & =T\left(p, y_i\right) \\ & =\left[\begin{array}{cc} \cos \phi_z & -\sin \phi_z \\ \sin \phi_z & \cos \phi_z \end{array}\right] y_i+\left[\begin{array}{l} t_x \\ t_y \end{array}\right] \end{aligned} pyi′=[txtyϕz]T=T(p,yi)=[cosϕzsinϕz−sinϕzcosϕz]yi+[txty]雅可比：
J i = d f i ( p ) d p = [ 1 0 − y i 1 sin ϕ z − y i 2 cos ϕ z 0 1 y i 1 cos ϕ z − y i 2 sin ϕ z ] J_i=\frac{d f_i(p)}{d p}=\left[\begin{array}{ccc}1 & 0 & -y_{i 1} \sin \phi_z-y_{i 2} \cos \phi_z \\ 0 & 1 & y_{i 1} \cos \phi_z-y_{i 2} \sin \phi_z\end{array}\right] Ji=dpdfi(p)=[1001−yi1sinϕz−yi2cosϕzyi1cosϕz−yi2sinϕz]
2.2 3D场景求解：
p = [ t x t y t z ϕ x ϕ y ϕ z ] T y i ′ = T ( p , y i ) = R x R y R z y i + t = [ c y c z − c y s z s y c x s z + s x s y c z c x c z − s x s y s z − s x c y s x s z − c x s y c z c x s y s z + s x c z c x c y ] y i + [ t x t y t z ] \begin{aligned} & p=\left[\begin{array}{llllll} t_x & t_y & t_z & \phi_x & \phi_y & \phi_z \end{array}\right]^{\mathrm{T}} \\ & y_i^{\prime}=T\left(p, y_i\right)=R_x R_y R_z y_i+t=\left[\begin{array}{ccc} c_y c_z & -c_y s_z & s_y \\ c_x s_z+s_x s_y c_z & c_x c_z-s_x s_y s_z & -s_x c_y \\ s_x s_z-c_x s_y c_z & c_x s_y s_z+s_x c_z & c_x c_y \end{array}\right] y_i+\left[\begin{array}{c} t_x \\ t_y \\ t_z \end{array}\right] \end{aligned} p=[txtytzϕxϕyϕz]Tyi′=T(p,yi)=RxRyRzyi+t= cyczcxsz+sxsyczsxsz−cxsycz−cyszcxcz−sxsyszcxsysz+sxczsy−sxcycxcy yi+ txtytz 雅可比：
J i = [ 1 0 0 0 c f 0 1 0 a d g 0 0 1 b e h ] \begin{aligned} J_i=\left[\begin{array}{llllll} 1 & 0 & 0 & 0 & c & f \\ 0 & 1 & 0 & a & d & g \\ 0 & 0 & 1 & b & e & h \end{array}\right] \end{aligned} Ji= 1000100010abcdefgh 其中：
a = y i 1 ( − s x s z + c x s y c z ) + y i 2 ( − s x c z − c x s y s z ) + y i 3 ( − c x c y ) b = y i 1 ( c x s z + s x s y c z ) + y i 2 ( − s x s y s z + c x c z ) + y i 3 ( − s x c y ) c = y i 1 ( − s y c z ) + y i 2 ( s y s z ) + y i 3 ( c y ) d = y i 1 ( s x c y c z ) + y i 2 ( − s x c y s z ) + y i 3 ( s x s y ) e = y i 1 ( − c x c y c z ) + y i 2 ( c x c y s z ) + y i 3 ( − c x s y ) f = y i 1 ( − c y s z ) + y i 2 ( − c y c z ) g = y i 1 ( c x c z − s x s y s z ) + y i 2 ( − c x s z − s x s y c z ) h = y i 1 ( s x c z + c x s y s z ) + y i 2 ( c x s y c z − s x s z ) \begin{aligned} & a=y_{i 1}\left(-s_x s_z+c_x s_y c_z\right)+y_{i 2}\left(-s_x c_z-c_x s_y s_z\right)+y_{i 3}\left(-c_x c_y\right) \\ & b=y_{i 1}\left(c_x s_z+s_x s_y c_z\right)+y_{i 2}\left(-s_x s_y s_z+c_x c_z\right)+y_{i 3}\left(-s_x c_y\right) \\ & c=y_{i 1}\left(-s_y c_z\right)+y_{i 2}\left(s_y s_z\right)+y_{i 3}\left(c_y\right) \\ & d=y_{i 1}\left(s_x c_y c_z\right)+y_{i 2}\left(-s_x c_y s_z\right)+y_{i 3}\left(s_x s_y\right) \\ & e=y_{i 1}\left(-c_x c_y c_z\right)+y_{i 2}\left(c_x c_y s_z\right)+y_{i 3}\left(-c_x s_y\right) \\ & f=y_{i 1}\left(-c_y s_z\right)+y_{i 2}\left(-c_y c_z\right) \\ & g=y_{i 1}\left(c_x c_z-s_x s_y s_z\right)+y_{i 2}\left(-c_x s_z-s_x s_y c_z\right) \\ & h=y_{i 1}\left(s_x c_z+c_x s_y s_z\right)+y_{i 2}\left(c_x s_y c_z-s_x s_z\right) \end{aligned} a=yi1(−sxsz+cxsycz)+yi2(−sxcz−cxsysz)+yi3(−cxcy)b=yi1(cxsz+sxsycz)+yi2(−sxsysz+cxcz)+yi3(−sxcy)c=yi1(−sycz)+yi2(sysz)+yi3(cy)d=yi1(sxcycz)+yi2(−sxcysz)+yi3(sxsy)e=yi1(−cxcycz)+yi2(cxcysz)+yi3(−cxsy)f=yi1(−cysz)+yi2(−cycz)g=yi1(cxcz−sxsysz)+yi2(−cxsz−sxsycz)h=yi1(sxcz+cxsysz)+yi2(cxsycz−sxsz)上面是使用旋转矩阵求导，也可以使用李代数求解。
3. 其他 NDT
多传感器融合定位二-3D激光里程计其二：NDT相关推荐
- 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
多传感器融合定位三-3D激光里程计其三:点云畸变补偿 1. 产生原因 2. 补偿方法 Reference: 深蓝学院-多传感器融合 多传感器融合定位理论基础 文章跳转: 多传感器融合定位一-3D激光里 ...
- 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
多传感器融合定位四-3D激光里程计其四:点云线面特征提取 1. 点云线面特征提取 1.1 按线数分割 1.2 计算曲率(重要!) 1.3 按曲率大小筛选特征点 2. 基于线面特征的位姿变化 2.1 帧 ...
- 深蓝学院-多传感器融合定位课程-第2章-3D激光里程计I
专栏文章: 深蓝学院-多传感器融合定位课程-第1章-概述_goldqiu的博客-CSDN博客 github保存了相关代码和学习笔记: Shenlan-Course-Multi-Sensor-Fusio ...
- 多传感器融合定位（一）——3D激光里程计
目录 一.点云地图整体流程 二.激光里程计方案 2.1 ICP点到点 2.1.1 ICP推导 2.1.2 ICP改进 2.2 NDT 2.2.1 NDT推导 2.2.2 NDT改进 2.3 LOAM系 ...
- 激光SLAM源码解析S-LOAM（二）激光里程计的计算
10Hz激光雷达点云帧,相临帧的时间间隔是0.1秒.在这0.1秒内激光雷达的位姿变化(平移和旋转)可由这两帧点云的配准计算出来. 以某一时刻为起点,累计帧间位姿变换,得到各帧时刻激光雷达相对起点的位姿 ...
- 多传感器融合定位五-点云地图构建及定位
多传感器融合定位五-点云地图构建及定位 1. 回环检测 1.1 基于Scan Context 1.2 基于直方图 2. 后端优化 2.1 后端优化基本原理 2.2 李群.李代数基本知识 2.3 李群. ...
- 多传感器融合定位十四-基于图优化的定位方法
多传感器融合定位十四-基于图优化的定位方法 1. 基于图优化的定位简介 1.1 核心思路 1.2 定位流程 2. 边缘化原理及应用 2.1 边缘化原理 2.2 从滤波角度理解边缘化 3. 基于kitt ...
- 多传感器融合定位十五-多传感器时空标定(综述)
多传感器融合定位十五-多传感器时空标定 1. 多传感器标定简介 1.1 标定内容及方法 1.2 讲解思路 2. 内参标定 2.1 雷达内参标定 2.2 IMU内参标定 2.3 编码器内参标定 2.4 ...
- 多传感器融合定位七-惯性导航解算及误差分析其一
多传感器融合定位七-惯性导航解算及误差分析其一 1. 三维运动描述基础知识 1.1 概述 1.2 姿态描述方法 1.2.1 欧拉角 1.2.2 旋转矩阵 1.2.3 四元数 1.2.4 等效旋转矢量 ...
最新文章
- 嵌入式linux驱动开发之点亮led（驱动编程思想之初体验）
- spring aop 应用实例
- 100内奇数之和流程图_Python进阶记录之基础篇（四）
- HTTP与HttpServlet
- 编译器构造概述（详细）
- 世界领先的界面设计公司：The Skins Factory
- linux阻塞原语,Linux中的内存原语是什么？
- android 主线程调用,Android 主线程和线程之间相互发送消息
- MongoDB经典故障系列一：数据库频繁启动失败怎么办？
- 解决 sublime text 3 there are no packages available for installation 错误
- oracle 查询当年数据_查询ORACLE数据库TOP SQL使用情况
- 学计算机自考商务英语本科好吗,自考本科商务英语专业真的难吗
- 解决@media screen （自适应）IE浏览器不兼容问题
- Codeforces 1485D - Multiples and Power Differences （构造）
- 幼儿抽象逻辑思维举例_什么是幼儿的逻辑思维能力？训练好逻辑思维，让孩子学习事半功倍...
- 【转载】Cygwin安装
- 关于如何连接网络打印机
- 北大肖臻老师《区块链技术与应用》系列课程学习笔记[1]Bitcoin中用到的密码学原理和数据结构
- 计算机重启机箱亮显示器不亮,电脑重启显示器不亮如何解决
- linux切换独显,Ubuntu 16.04如何切换Intel集显与Nvidia独显