note
trajectory_smooth 使用RTK、轮速计、IMU实现轨迹平滑
1.trajectory_filter.cpp 轨迹筛选源文件，输出轨迹筛选段
2.trajectory_perfect.cpp 基于group_id的轨迹平滑源文件，输入为traj_smooth_from_group.py生成的过程文件 数据源：Geodetic的车速、经纬高(以第一帧为起点)、Kinetic的IMU信息
3.trajectory_perfect_for_mapless.cpp 轻图mapless_release上线的源文件，输入为过程文件路径 数据源：Geodetic的车速、经纬高(以第一帧为起点)、Kinetic的IMU信息
4.vel_odom.cpp 车速积分轨迹
5.ape.cpp 计算ATE
trajectory_perfect_for_mapless.cpp
pipline:
- read files
- checkBadGeodetic
- RunBadGeodetic
- UpdateVel
- good_clips 筛选时间戳连续片段，实际好像没用到
- UpdatePose 时间序列分段优化
- UpdateZeroVel 更新完速度之后，应该首先对Bad的区间内，进行双向ESKF，优化中间量
- fileout « timestamp, lng, lat, alt, w, x, y, z
NOTE: state.pose 实际是 pos，经纬高
trajectory_smooth/include/trajectory_smooth/dataprocess.h
ss >> time_str >> sensor_type >> gps_status >> gps_long >> gps_lati >> gps_alti;
ss >> q_w_str >> q_x_str >> q_y_str >> q_z_str; // 读取四元数
ss >> heading >> roll_str >> pitch_str >> a_x >> a_y >> a_z >> w_x >> w_y >> w_z >> vel_east_str >> vel_north_str >> vel_up_str;
只有 fileInput 被使用了，kinetic 在代码中未被执行。
x,dx -> SensorDataFusion q,w,ddx -> imu_msg
geos 是一个 vector，存了所有从 csv 提取出来的状态信息。 geos.size() = 5995
-
100hz 数据中，5576 是 ‘gps,2
，有 419 个是gps,5` -
100 hz 序列没有 bad_case
这个可能是问题所在：
// in sensorType
if cur_time - last_gps < 0.1: sensor = "imu"
else if cur_time - last_gps >= 0.1: sensor = "gps"
-
这假设了 gps 以 10 HZ 触发，imu 的频率高于 gps；但这对于距离触发回传并不成立，需要更多关于 如何区分新序列中的 imu 和 gps 的信息
-
理解 bool checkBadGeodetic(vector
&geos, int idx, vector<vector > &bad_cases) -
可视化，是否有现有代码？
-
执行低频数据集出现 Sophus 出现读到 NA 的情况，因而终止
-
checkBadGeodetic
- 返回值 bool 未使用
- geos 是入出参，i 是入参，bad_case 是出参
- bad 的准则：eps = vel / 30; if vel < 6: eps = 0.25；
- dist1 是 相邻两帧的位移的模长；dist2 是 两种来源的位移的差异
-
dist1 = x2 - x1 > vel/30 -
dist2 = x2 - x1 - vel*dt > vel/30
-
- predict 是用 当前帧 的 前后帧 取均值得到的 当前帧位置
- predict_delta 是 预测值 和 测量值 之间的差距
- dist1 是 相邻两帧的位移的模长；dist2 是 两种来源的位移的差异
if predict_delta > 0.2 && sta.type == “gps”: geos[i].pose = predict
-
dist1 = x2 - x1 > vel/30 -
dist2 = x2 - x1 - vel*dt > vel/30 * 0.5
| x2 - x1 - vel*dt | >= | x2 - x1 | - | vel*dt |