# 松耦合和紧耦合理解：松耦合，多个传感器把观测到的数据合并。紧耦合，如这里，把IMU的观测值作为点云的初始位置。
# 误差对状态量的导数在雅可比矩阵的位置：雅可比矩阵里对R和对p的导数位置没有强求，只要与dx的对应就行了。对于旋转量，是对李代数求导的，因此dx求得的也是李代数
# 左扰动和右扰动公式：设李代数为a。左扰动，d(R*p)/da = -（Rp）^ ；右扰动：d(R*p)/da =-R*p^
# 状态量 x = [p,v,R，bg，ba，g].transpose
# 罗德里格斯(Rodriguez formula)公式
# 这里的观测量是点云位置，一次扫描会存在上千个点，单纯使用卡尔曼滤波会导致上千个观测方程，因此观测需要优化。观测矩阵线性化之后就是误差对状态量的导数
# SMW恒等式：AB(D+CAB)^(-1) = (A^(-1)+BD^(-1)C)^(-1)BD^(-1)
# SVD解最小二乘法，主要利用左奇异和右奇异矩阵是酉矩阵，不用求逆，直接使用转置矩阵
# SVD求逆：SVD，A=UΣV^T, A^(-1)=VΣ^(-1)U^T, 对每个奇异值求倒数则是Σ^(-1)
# 第一个点云的位姿，是IMU从0开始积分得到的，与GNSS不对应
1、读取配置文件，bagpath（数据包位置）和激光参数路径lio_yaml_
2、初始化：
(1)LioIEKF初始化、IMU初始化,定义MessageSync里的回调函数
(2)读取激光参数，time_scale = 1e-3 ，lidar_type: 2（LidarType::VELO32） ，scan_line: 32，point_filter_num: 10
(3)读取IMU和雷达之间册变换位姿，外参TIL = [E 0 0 0.28]
3、先读取全部的RTK数据gnss_
4、取RTK第一个数据为原点，并且gnss_减去原点
5、每一次读取点云都进行：点云放入lidar_buffer_，时间放入time_buffer_，记录当前点云时间为last_timestamp_lidar_
6、判断IMU和lidar数据是否都有了，没有则继续读取
7、每一次读取到IMU数据：时间记为last_timestamp_imu_，IMU数据放入到imu_buffer_
8、直到读取到点云数据，运行第5步后，取出lidar_buffer_和time_buffer_第一个数据，lidar_pushed_标记为ture，代表数据已取出
lidar_begin_time_为time_buffer_第一个数据，lidar_end_time_为点云最后测量数据时间，measures_.lidar_为lidar_buffer_第一个数据
9、判断lidar_end_time_是否大于last_timestamp_imu_，保证在lidar_begin_time_和lidar_end_time_之间IMU数据齐全了
10、记录imu_buffer_第一个IMU数据的时间，将lidar_begin_time_到lidar_end_time_之间的IMU放入到measures_.imu_
11、删除已经取出来的数据和标记lidar_pushed_为ture
12、初始化IMU，计算重力加速度和测量噪声协方差和均值作为零偏，设置eskf初始状态
13、重新执行5-10，直到读取到点云，将eskf的状态放入imu_states_，这时候没预测过，应该都是0
14、根据所有的IMU数据更新eskf，并放入到状态放入imu_states_，参考第三章，所有的状态都放进去是为了后续矫正点云
15、读取IMU最后预测的位姿T_end
16、插值求取点云时候IMU的位姿(每个点的时间是不一样的，点云获取过程，机器仍在运动)：
(1)如果点云时间超过了IMU时间，且只超出一点点，则直接取IMU的位姿
(2)记录点的时间在IMU数据的位置[iter,iter+1]
(3)线性插值，可以旋转量直接调用eigen的函数slerp进行插值
17、因为点云获取过程，机器也在运动，因此每个点获取时，其实位姿都是不一样的，因此需要对每个点都矫正
p' = TLI * TIW * Ti * TIL * p,这里最后获取的是雷达坐标下的真实点云
18、点云转到IMU坐标下，current_scan_ = TIL * p
19、使用体素大小为0.5的过滤点云
20、ndt：以点整数为key，点为值添加到data，再把data的指针添加到grid，记录key到active_voxels，
如果key已经存在，则data在对应值后面添加，因为grid添加的是指针，因此grid不需要再更新。
把data最新的key移到最前面，因为大他顺序变了，grid更新data指针
21、ndt：求每个key（对应一个体素）data的均值和方差的逆，删除记录的点，避免下次计算时重新计算了。
22_1、第一次点云直接使用IMU预测的位置，结束第一帧点云，记为关键帧，并保存，类似IMU插值，使用点云的时间插值获取对应时间的GNSS位姿
22_2、下一次点云到来，设置为源点云(配准点云)，与前面设置好的目标点云(被配准点云)匹配，所有点云都转到了IMU坐标下了，且已去畸变
23、使用IMU预测的位姿为初始位置，源点云使用IUM预测值转到世界坐标
24、使用6邻域加上自身，共7个作为key，在grid中寻找key是否存在，取点与key的均值为误差e = p - mu，
使用方差来加权误差res = e.transpose() * v.info_ * e，因为初始位姿是使用IMU预测的，res不会太大，太大则是异常，应舍弃
25、计算雅可比矩阵，因为状态量是 x = [p,v,R，bg，ba，g]，18维，因此
J.setZero();
J.block<3, 3>(0, 0) = Mat3d::Identity(); // 对p
J.block<3, 3>(0, 6) = -pose.so3().matrix() * SO3::hat(q); // 对R，右扰动
26、 const double info_ratio = 0.01; // 每个点反馈的info因子
total_res += errors[idx].transpose() * infos[idx] * errors[idx];
HTVH += jacobians[idx].transpose() * infos[idx] * jacobians[idx] * info_ratio;
HTVr += -jacobians[idx].transpose() * infos[idx] * errors[idx] * info_ratio; //HTVH deltax = HTVr
27、旋转量投影：
Mat18T J = Mat18T::Identity();
J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat((R_.inverse() * start_R).log()); //误差重置为0后，旋转变量会有小变化
Pk = J * cov_ * J.transpose();
28、根据SMW恒等式，卡尔曼增益K = P* H^T * (H * P *H^T + V)^(-1)可以转为K = (P^(-1)+H^T*V^(-1)H)^(-1)*H^T*V^(-1)
则dx = K*（z-h(x)）= (P^(-1)+H^T*V^(-1)H)^(-1)*H^T*V^(-1)*（z-h(x)）
(1)P由27所得
(2)H是观测矩阵线性化的矩阵，其实就是雅可比矩阵jacobians
(3)V就是NDT体素的协方差
(4)H^T*V^(-1)H = HTVH
(5)H^T*V^(-1)*（z-h(x)）= HTVr
29、根据dx更新每个状态量
30、迭代运行24-29，直到dx小于设定的数，或者迭代次数到达一定数量
31、估计协方差更新：cov_ = （I - Kk * H ） * Pk = (Mat18T::Identity() - (Pk.inverse() + HTVH).inverse() * HTVH) * Pk;
32、投影：
Mat18T J = Mat18T::Identity();
Vec3d dtheta = (R_.inverse() * start_R).log();
J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dtheta);
cov_ = J * cov_ * J.inverse();
dx_.setZero();
32_2、判断点云与上一帧关键帧的位姿变换，较大则记为关键帧，并保存和记录GNSS插值的位姿
33、获取新点云后，使用当前位姿和之前的位姿来判断位姿变换是否较大，较大则执行20-21，如果之前体素已经计算过均值和方差，但是现在添加了新点，则需要更新均值和方差
更新公式 mu = (m*mu_old + n*mu_new)/(m+n)
var = (m*(var_old + (mu_old-mu)*(mu_old-mu)^T)+n*(var_new+(mu_new-mu)*(mu_new-mu)^T))/(m+n)
求取var的逆：对var奇异值分解，var^(-1)=VΣ^(-1)U^T。
34、执行22到32
35、重复执行33-34，直到数据处理完
36、保存关键帧的信息，id-1，时间-2，rtk状态-3-5，点云位姿-6-12（平移和旋转四元数），rtk位姿-13-19，//优化1位姿、优化2位姿
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
