longer95479@home:~$

Archive

About

Categories

Tags

RSS

note

tramjectory_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；
      • 若要复用，需修改：参数写死了，认为时间间隔远小于 30 ms
      • 粗到细的判定要求：dist1 是 相邻两帧的位移的模长；dist2 是 两种来源的位移的差异
        • dist1 = x2 - x1 > vel/30
        • dist2 = x2 - x1 - vel*dt > vel/30 * 0.5, 约为 15 ms走的距离
          • x2 - x1 - vel*dt >= x2 - x1 - vel*dt
      • predict 是用 当前帧 的 前后帧 取均值得到的 当前帧位置
      • predict_delta 是 预测值 和 测量值 之间的差距
        • if predict_delta > 0.2 && sta.type == “gps”: geos[i].pose = predict
      • dist2 有点类似于 predict_delta，只不过其对比的是 vel 推出来的
      • 根据跳变的距离，向前后扩展选取片段，按 max(15m, dist2 * 50) 向前后扩展，遇到零速的点也终止
  • RunBadGeodetic
    • geos 是入出参，imu 是入参，bad_case 是入参
    • 对 bad_cases 片段进行前向和后向的 ESKF，然后根据前后向求出的位置协方差，求位置的加权平均
  • UpdateVel
    • geo 是入出参，Imu 是入参， index 是入参
    • 不对总序列前后 25 个数据处理，因为 25 大小的滑窗
    • 前向的范围 [index -25, index + 1]，反向的范围 [index, index + 25]，前 25 和后 25 个数据点不会被滤波，只是参与滤波，只有 index 处的滤波结果会被刷到原始数组上
    • 区间内有零速，不更新
    • 进行正反向 ESKF，如果反向成功，则执行成功，则根据协方差求 速度 的加权平均，否则只使用前向的结果
  • time_lost and good_clips
    • 挑出时间间隔大于 0.1 s 的区间，存到 time_lost
    • 其余区间为 good_clips
  • UpdatePose
    • geo 是入出参，Imu 是入参，index 是入参
    • 不对总序列前后 25 个数据处理，因为 25 大小的滑窗
    • 前向的范围 [index -25, index]，反向的范围 [index, index + 25]，最前 25 和最后 25 个数据点不会被滤波，只是参与滤波
    • 区间内有零速，不更新
    • 进行正反向 ESKF，如果反向成功，则执行成功，则根据协方差求 位置 的加权平均，否则只使用前向的结果
  • UpdateZreoVel
    • geos 是入出参
    • 针对 零速 的区间的 位置 求平均，并重新赋值给这个区间内的所有帧
  • 确认源代码是否用了 IMU 的加速度和角速度
  • 使用 kinetic 替换加速度和角速度
  • Python 的优化方案
  • 自己写的优化方案

RTS 滤波

  • RunESKF(vector&, vector& Imu, vector&, int start, int end, int flag)
    • if (!sensorFusion.first_imu_get) 初始化 ba bg g, Q R cov_
    • if (gps && !first_gnss_set) 初始化第一帧 p v R/q
    • if (gps) 更新
    • if (imu) 预测

NOTE: 因为反向 ESKF 调用了 正向，所以取消观测更新是正反向都取消的

由于不论是正向还是反向，初始 cov_ = Mat18T::Identity() * 1e-4，并且最后的结果是对 正反向结果 的加权平均（根据协方差），因此导致最终结果一定是首尾在原处，且中间十分平滑

如果不执行 RunBadGeodetic，意味着不会对异常点作额外的平滑，因此更具阶梯性

如果只输出 正向 或 反向的结果，结果都很不均匀，因为设置的协方差十分信赖位置观测，而位置观测会有较大的切向偏移，自然单向的滤波结果也就不平滑。

因此，双向滤波结果平滑并不是单向的滤波过程带来的，而是双向使用协方差加权平均的结果。

trajectory_filter.cpp

里面有重复的代码片段，相关变量是 bad_start, bad_end, bad_len, 都是用于找到 状态异常（status=2）的片段的起点和终点。

  • 先直接查 status=2 的片段，注释解释这是 RTK 与 地图不匹配的片段，然后把起止时间戳记录到输出文件 traj_filtered_out
  • 之后记录时间戳跳变的大于 0.1 秒的片段（相关的其实只有两个点）
  • 把状态都变成3，用于下一步的判定
  • 根据速度和位移差异判定准则，异常点的前后 50 点组成的片段设为状态 2，最后重复执行片段起止查找逻辑。

geodetic2enu geodetic2ecef -> uvw2enu

ecef2enu geodetic2ecef -> uvw2enu

二者的区别是 ecef2enu 的当前点输入是 ecef 的，而 geodetic2enu 的当前点输入是在 lla 下的； 二者的相同之处是 都需要 参考点 的 lla 坐标系。

xjt = xit \oplus ( uj + w ) xj = xi \oplus uj

xj \oplus \delta xj = xi \oplus \delta xi \oplus ( uj + w ) xj \oplus \delta xj = xi \oplus ( uj + w + Jl-1(uj+w) \delta xi ) xj \oplus \delta xj = xi \oplus uj \oplus Jr(uj) w \oplus ( Jr(uj+w) Jl-1(uj+w) \delta xi )

Exp( \delta xj ) = Exp( Jr(uj) w ) Exp ( Jr(uj+w) Jl-1(uj+w) \delta xi )

如果作最大程度的假设，uj, w, \delta xi 都很小，则满足交换律

\delta xj = \delta xi + w

  • 下载 csv 文件
    • 小米融合云
    • SQL 查询pipeline/test/multi-slam/230768
    • fastmap(tj)
    • 执行以下筛选语句 
      select *
from fastmap.crossing_task
# where sid = 12298192
    • 导出pipeline/test/multi-slam/230768成 csv: /home/longer95479/Downloads/zeus_6_5_2025_crossing_task_1749111720615.csv
  • 批量下载 slam_tracks.json
    • ks3 = KS3Client(bucket_name="hdmap-process-public")
    • (hdmap-process-public – 金山云)[https://ks3.console.ksyun.com/#/bucket/single/hdmap-process-public/file]
    • 批量解析 vector_ks3_dir 列下的 slam_tracks.json 路径
    • 调用 utils.ks3client 来下载 slam_track.json
  • 批量解析 slam_track.json，下载获取对应的 config.json 文件
    • 解析 slam_track.json 文件，获取 config.json 文件的云端路径
    • 使用解析出来的路径下载 config.json 文件
  • 利用 config.json 下载 .pb 文件
    • 从 config.json 文件里解析得到 geodetic/kinetic 的云端目录和 .pb 文件名
    • 使用解析出来的路径下载 .pb 文件
  • 批量解析 .pb 文件
    • 构造 csv 文件目录和文件名字
    • 转换

NOTE:

config.json 表示一个 task，里面有多条子轨迹，需要拼接起来:

  • 下载 geodetic.pb 文件处修改，按任务名字构建子目录，然后把 .pb 存里面
  • 后续读取每个子目录，pb 转 csv 并进行拼接，然后输出到上一级目录

一个 slam_tracks.json 文件是一个任务 一个 config.json 文件是一条轨迹，但被分成了多个片段，需按时间戳拼接

crossing_task.csv
  /     |      \
  v     v      v
s_t.js  ...   s_t.js    
 / |
v  v
c.js ...
 | pb2csv and combine
 v
csv

批量运行

  • 生成输出文件目录及名称
  • os.system(“”) 执行命令

下载完 csv 文件后的使用方法：

  1. 在容器内目录 /home/trajectory_smooth 下执行 batch_run.sh，可修改存储路径
  2. 在容器外的 python 虚拟环境下执行 python batch_enu2wgs84.py。如果缺少依赖则在虚拟环境内安装

分析 kinetic 数据含义

  • 可视化以便于理解和 debug
    • 3d 图要保证三个轴的单位长度一致
    • 数据的三个轴的量纲要一致

kinetic_csv 的每一列的含义由 data-collect/core/trajectory_smooth/traj_smooth_from_bins.py 内函数实现给定

kinetic 由 .pb 转 .csv 文件有 bug，本应是 velocity_z_mps，错写成了 velocity_vehicle_y_mps

def extract_kinetic_data(bin_file):
    kinetic_array = kinetic_localization_pb2.KineticLocalizationArray()
    with open(bin_file, "rb") as f:
        kinetic_array.ParseFromString(f.read())
    extracted_data = []
    cur_time = 0
    last_time = 0
    for kinetic_data in kinetic_array.datas:
        timestamp_ns = kinetic_data.header.timestamp_ns
        timestamp_ns = int(float(timestamp_ns) * 1e-3)
        cur_time = timestamp_ns  #时间戳
        if cur_time == last_time:
            continue
        extracted_data.append((kinetic_data.header.timestamp_ns, kinetic_data.position_x_m, kinetic_data.position_y_m,
                               kinetic_data.position_z_m, kinetic_data.velocity_x_mps, kinetic_data.velocity_y_mps,
                               kinetic_data.velocity_vehicle_y_mps, kinetic_data.status))
        extracted_data.sort(key=lambda x: x[0])
    return extracted_data
  • 增加 下载 .pb 文件并转换成 .csv 文件的功能
    • 确定要解析的内容：timestamp_ns, position_x_m, position_y_m, position_z_m，
  • 对比 kinetic 和 geodetic 的 imu 数据区别
  • 用 geodetic 的 imu 纯预测，不用更新，看看效果
  • dataprocess.h: 120 处单位转换可能有错误，需要验证
    • 对位置没什么影响
gnss_pvt -> geodetic_localization -> state_machine
  • 叠加地图可判断相对正确性
  • 观测量中有位置和速度，如果只使用速度，则可恢复出原始轨迹形状，但绝对位置可能有偏移，起到拼接恢复的效果

2025-06-16

my first commit:

  • add confidence_fileout
  • fix wgs84_to_enu bug
  • fix prediction fomulars without gravity
  • fix angular velocity unit bug

2025-06-17

task_data_prepare.py

def gen_task_info:

  • 获取 lla 点集中的第一个点，作为 lla2enu 的 anchor
  • 将所有任务点从 lla 转为 enu
  • 求圆形扩展或多边形扩展
  • 求外边界的中心
  • 将该中心变会 lla，然后查找对应 .json 文件

时间同步与pgo接口

  • 将 一对 kinetic 和 geodetic 的 csv 文件转换为一个 .json

    • 时间不完全同步：以 geodetic 为参考，找最近的 kinetic，然后 插值，对于四元数使用 slerp；NOTE: 插值不能超出范围

    • json 数据定义: 见下

    • 确定每 frame 需要的原始数据
      • anchor_rad: 可以用 geodetic 第一帧，注意单位是 rad
      • pose_dr: 只需要 kinetic 的 p q
      • pose_rfu2enu: lla 存 geodetic 的 lla 即可，status 存 geodetic 的status，transform 存 geodetic2enu 的结果
      • ts_nsec: 存 gedetic 原始时间戳
    • 写批量合并 json 脚本
      • 需适配 pgo 接口假设的文件结构
  • 跑通 pgo
    • 基本只迭代一次就终止了，优化过程打印信息见下。已找到原因：初始值由 geodetic 给定，而绝对位置约束的权重矩阵又很大，导致一开始误差就足够小，直接停止优化
    • 将绝对位姿约束权重调小(0.25->1e-2)，将相对位姿约束的权重调大(1e-2->2.5)，这样就会更信任相对位姿，迭代次数也会变多。
    • 但此时并没有出现完全不信任状态为5的约束，而是出现折中的结果，感觉是陷入了局部最小值
  • 将 pgo 的输出结果转换为 csv，并可视化
    • 与输入的 geodetic 基本无差别，原因同上
原始输入：
          ____________3
_________
5

一些局部最小值：
         _____________
________/
    __________________
___/


全局最小值：
_____________________

数据存储的文件结构：

|-[root_data_dir]
  |-data
  | |-config_json
  | |-geodetic_csv
  | |-geodetic_pb
  | |-geo_kin_json
  | | |-pre_opt_csv   // pgo json 输出文件转换为 csv 便于显示
  | | |-kmls
  | | |-jsons
  | | | |-pre_opt
  | | |   |-*.json    // pgo 输出 json 文件
  | | | |-raw
  | | |   |-*.json    // pgo 的 json 输入会被复制到这里
  | | | 
  | | |-*.json        // geo_kin_json 输入放这里
  | |
  | |-kinetic_csv
  | |-kinetic_pb
  | |-slam_tracks_json
  |
  |-output
  |-output_84

!!!新思路：因子图此时没有约束车体运动学，也就是车头朝向应和运动方向一致；观察发现

0.001 s 精度，与整数时间戳的结果对比，此时未丢失额外信息，但可说明 kinetic 存在重复时间戳问题

geo_dict len:  834
geo_pv len:  834
kin_dict len:  929
kin_pv len:  1000
commn_timestamps len:  471

0.01 s 精度，此时 kinetic 存在混淆了（kin_dict_0.01_len < kin_dict_0.001_len），说明存在小于等于 0.00999… s （小于 0.01 s）的时间间隔，例如 0.005 和 0.014999… 都会变成 0.01

但数目不变小，不意味着不存在小于 0.01s 的间隔，例如 0.006 和 0.015 间隔为 0.009，但前者会变成 0.01，后者变成 0.02

geo_dict len:  834
geo_pv len:  834
kin_dict len:  794
kin_pv len:  1000
commn_timestamps len:  688

错误权重矩阵：

(pgo_env) longer95479@longer95479-ThinkCentre-M760t:~/vector_mapping$ sh pgo.sh 
INFO:root:Found 18 JSON files to process
cost value:  [2.03028489e-05]
the 1 iteration
err gain meet threshold: 1e-07
INFO:root:Optimization completed!

正确权重矩阵：

(pgo_env) longer95479@longer95479-ThinkCentre-M760t:~/vector_mapping$ sh pgo.sh 
INFO:root:Found 18 JSON files to process
cost value:  [0.00419494]
cost value:  [0.00507571]
the 1 iteration
cost value:  [0.00249075]
the 2 iteration
cost value:  [0.00163374]
the 3 iteration
cost value:  [0.0011336]
the 4 iteration
cost value:  [0.00072728]
the 5 iteration
cost value:  [0.00045428]
the 6 iteration
cost value:  [0.0003223]
the 7 iteration
err gain meet threshold: 1e-07
INFO:root:Optimization completed!


{ 
  "anchor_rad": [ lon_rad, lat_rad, alt ], 
  "frames": [ 
    { 
      "pose_dr": { 
        "transform": [12x1]
       } 
      "pose_rfu2enu": {
        "lla": [, , ],
        "status": 3,
        "transform": [12x1],
      },
      "ts_nsec": 1739325700370903567
    }, 
    {
      ...
    }
   ] 
}

note: kinetic 和 geodetic 时间戳不是完全相同，且 Kinetic 存在 时间戳重复 和 间隔不均匀 的情况

0822序列 0.56 重叠率，0905序列 0.97 重叠率

e.g.:

1724307403.072
1724307403.082
1724307403.092
1724307403.104
1724307403.112
1724307403.124
1724307403.132
1724307403.152
1724307403.152
1724307403.164
1724307403.182
1724307403.184
1724307403.192
1724307403.202
1724307403.222

how to use glog

Severity Level: INFO 0, WARNING 1, ERROR 2, FATAL 3

c 与 c++ 使用输出流，输出内容到文件

  • 定义一个 stream
    • c: FILE* stream = fopen("mylib/myfile", "w");
    • cpp: ofstream my_ofs("mylib/myfile");
  • 写入内容
    • c: fprintf(stream, "something");
    • cpp: my_ofs << setprecision(13) << some_var << "some_str" << endl;
  • 关闭流
    • c: `fclose(stream);’
    • cpp: my_ofs.close();

c 与 c++ 从输入流读取文件内容

执行一个shell命令行时通常会自动打开三个标准文件：

  • 标准输入文件（stdin），通常对应终端的键盘；
  • 标准输出文件（stdout）和标准错误输出文件（stderr），这两个文件都对应终端的屏幕。

进程将从标准输入文件中得到输入数据，将正常输出数据输出到标准输出文件，而将错误信息送到标准错误文件中。

1）输入重定向

输入重定向是指把命令（或可执行程序）的标准输入重定向到指定的文件中。也就是说，输入可以不来自键盘，而来自一个指定的文件。

使用“ < ”符号将标准输入重定向到文件中

2）输出重定向

输出重定向是指把命令（或可执行程序）的标准输出或标准错误输出重新定向到指定文件中。这样，该命令的输出就不显示在屏幕上，而是写入到指定文件中。

  • 使用 “ > ”符号，将标准输出重定向到文件中。形式为：命令>文件名

  • 使用“ » ”符号，将标准输出结果追加到指定文件后面。形式为：命令»文件名

  • 使用“ 2> ”符号，将标准错误输出重定向到文件中。形式为：命令 2> 文件名

  • 使用“ 2» ”符号，将标准错误输出追加到指定文件后面。形式为：命令 2»文件名

  • 使用“ 2>&1 ”符号或“ &> ”符号，将把标准错误输出stderr重定向到标准输出stdout

  • 使用“ >/dev/null ”符号，将命令执行结果重定向到空设备中，也就是不显示任何信息。

3）几个基本符号及其含义

  • /dev/null 表示空设备文件
  • 0 表示stdin标准输入
  • 1 表示stdout标准输出
  • 2 表示stderr标准错误

linux下的stdin,stdout和stderr理解

euler angle

Our intrinsic example: Yaw-Pitch’-Roll’’ (z-y’-x’’), that is, 1) rotation about the global z-axis 2) rotation about the new y’-axis 3) rotation about the new x’’-axis Matrix multiplication: R = Rotation1 ⋅ Rotation2 ⋅ Rotation3 Our extrinsic example: Roll-Pitch-Yaw (x-y-z), that is, 1) rotation about the global x-axis 2) rotation about the global y-axis 3) rotation about the global z-axis Matrix multiplication: R=Rotation3 ⋅ Rotation2 ⋅ Rotation1

Two method will yeild the same rotation matrix.

Extrinsic & intrinsic rotation: Do I multiply from right or left?

slerp

q = q0 o+ t ( q1 o- q0 ) )
  = q0 Exp( t Log( q0^-1 q1 ) )
  = q0 Exp( t v\Omega )
  = q0 ( cos( t \Omega ) + v sin( t \Omega ) )

SLerp - wiki

autoOptimizer.py

class:

  • Vertex
  • Edge
  • VertexHolder
  • OptHolder
  • AutoOptimizer
\[\sum_i a_i = tr(A)\] \[\prod_i a_i =\]

计算机上的有符号整数运算

62553 = 0xf459
= 0000 0000 0000 0000 1111 0100 0101 1001 原

1 << 16 = 0000 0000 0000 0001 0000 0000 0000 0000 补

62553 - (1 << 16) = -2938

62553(补) + (- 1 << 16)(补)

运算都是补码层面

 0000 0000 0000 0000 1111 0100 0101 1001 补
+1111 1111 1111 1111 0000 0000 0000 0000 补
---------------------------------------- 
 1111 1111 1111 1111 1111 0100 0101 1001 补

 1000 0000 0000 0000 0000 1011 1010 0111 原码

对于 compress_experience_data.py 里 morton_decode y = y - (1 « 16) 是有问题的， 因为 y 是 uint16 的类型，而 1 « 16 会被隐式转为 uint16 而溢出

全量回传数据目前 pb 解析结果有问题，具体为 geodetic_csv 解析出的四元数不正确，四元数xyz处其实是速度。

  • 原因：旧版全量回传数据 pb 定义：vx=10, vy=11, vz=12；新版 pb 定义: qw = 9, qx=10, qy=11, qz=12；使用新版的 pb 解析函数去解析旧定义的 pb 轨迹文件
  • fix geodetic csv data: 用欧拉角计算四元数后填到正确的列，同时把速度移动到正确的列上

全量回传的优化的损失函数特别大，分析原因：

  • 初始损失函数：12.10254726
  • 无相对姿态：12.10254723
  • 无相对位置：0.00073307 可以看到，主要由相对位置损失项引起的。经可视化发现，是geo的轨迹姿态从欧拉角转四元数错误导致的。 修复后，损失值回到零点几的正常水平。

2025-07-03

  • 轨迹显示增加 pgo 输出结果
  • 修改轨迹异常判断逻辑，1.2倍时间差
    • 对尖角 GPS 的判断会进行修改原本的位置，导致和上个点的 delta 不匹配。

2025-07-04

  • 测试 1000 条轨迹，主要寻找大跳变轨迹，查看优化结果
    • 自动挑选 142 条
    • 优化消耗总时长: 16 min，10 个线程

2025-07-07

  • 1000 条轨迹耗时对比
    • 1000 条滤波: 998.11719799s，16.6min；单条 0.998s
    • 979 条优化：117.78 min；单条 7.217 s
      • 时间同步和格式转换耗时：571.7688806056976 s，9.53 min
      • 纯优化耗时：6494.998217821121 s，108.25 min
      • 输出json转换成csv耗时：29.422772645950317 s
    • 考虑输入数据转换：单条轨迹耗时 优化 是 滤波 的 7.23 倍
    • 仅考虑核心计算部分：单条轨迹耗时 优化 是 滤波 的 6.64 倍
  • batch_geo_kin_csv2json.py 转换成功 979/1000，需要定位一下失败原因
    • 含有空格的文件名有 21 个

2025-07-07

如果 SciPy 是用 Python 这样的解释型语言编写的，它如何能做到快速运行？

实际上，时间关键的循环通常用 C、C++ 或 Fortran 实现。SciPy 的部分内容是基于 https://www.netlib.org/ 免费提供的科学例程的薄层代码。Netlib 是一个巨大的存储库，里面包含了用 C 和 Fortran 编写的极其有价值且健壮的科学算法。重写这些算法并调试它们将是愚蠢且耗时多年的。SciPy 使用各种方法为这些算法生成“包装器”，以便它们可以在 Python 中使用。一些包装器是手工用 C 编写的。其余的则是使用 SWIG 或 f2py 生成的。SciPy 的一些新贡献要么完全用 Cython 或 Pythran 编写，要么用它们进行包装。

第二个答案是，对于困难的问题，一个更好的算法可以在解决问题所需的时间上产生巨大的差异。因此，使用 SciPy 的内置算法可能比用 C 编写的简单算法快得多。

scipy faq

2025-07-09

  • 将数据加载与优化合并，写成单轨迹模式
  • 输出转换需修改成 无表头 + enu 坐标系
  • 由于时间同步，数据存在首位截断，导致 lla2enu 和 enu2lla 的 anchor 不一致，修复成按原始第一帧作参考点
  • python 版本的跳变检测

2025-07-10

  • 修复 interpolator_pose 对 scipy v1.13.1 的兼容性问题

2025-07-10

双三次插值:

scan-to-submap: 本质上是把 scan 的点集 旋转平移到 submap 上，使得该点集的置信和最高。匹配先粗后细。

粗匹配：argmax_{x_t} p(z_t | x_t, m_{t-1})，网格暴力搜索 细匹配：ceres 优化

cartographer 里的回环检测使用了 BnB，原问题是求最大值问题。对于非叶子节点，构造了一个高效的上界计算方法，则怎么松弛呢，其实是假设每个点都独自优化，从而松弛原问题。怎么快速求解松弛后的问题呢，本质上是用空间换时间，核心思想：点 A 移动构成的集合是否覆盖点 B，可以让点 A 不动，B 反向运动，看 A是否落在 B 反向运动的集合内

{B} IN/NOTIN {A} + D <=> {A} IN/NOTIN {B} - D

对于一个含有m个变量的模型，如果每个变量是连续变量，每个变量的范围无论是[-5,100]、[5,500]、[0,1]……，都可以作为LP问题在多项式时间内求解。

现在增加一个条件，模型中有n个变量是{0,1}变量。如果将变量松弛为[0,1]之间的连续变量，作为LP问题求解，这n个变量的取值可能是介于[0,1]区间的任何数，而无法保证是{0,1}。

分支定界是在这个背景下运用的。

我们以0-1整数规划举例：

首先，我们将0-1整数规划模型松弛为LP模型求最优解，对于最小化问题，该LRP的最优解是原问题最优解的下界，i.e. 原问题的最优解一定不小于LRP的最优解。

其次，我们令这n个变量在满足{0,1}约束的前提下求出一个可行解，该解可能不是最优解，一定是最优解的上界，i.e. 原问题的最优解一定不大于该可行解。

分支的树的中间节点表示可继续分的子问题，根节点表示无法继续分的子问题，对中间节点使用 lower_bound_function 评估，对根节点使用 objective_function 评估。当前最小值就是解的 upper bound。以上假设是求最小值问题。

1. Using a heuristic, find a solution xh to the optimization problem. Store its value, B = f(x_h). 
(If no heuristic is available, set B to infinity.) B will denote the best solution found so far, 
and will be used as an upper bound on candidate solutions.

2. Initialize a queue to hold a partial solution with none of the variables of the problem assigned.

3. Loop until the queue is empty:

	3.1. Take a node N off the queue.
	
	3.2. If N represents a single candidate solution x and f(x) < B, then x is the best solution so far. Record it and set B ← f(x).
	
	3.3. Else, branch on N to produce new nodes Ni. For each of these:

		3.3.1. If bound(N_i) > B, do nothing; since the lower bound on this node is greater than the upper bound of the problem, it will never lead to the optimal solution, and can be discarded.
		
		3.3.2. Else, store Ni on the queue.

// C++-like implementation of branch and bound, 
// assuming the objective function f is to be minimized
CombinatorialSolution branch_and_bound_solve(
    CombinatorialProblem problem, 
    ObjectiveFunction objective_function /*f*/,
    BoundingFunction lower_bound_function /*g*/) 
{
    // Step 1 above
    double problem_upper_bound = std::numeric_limits<double>::infinity; // = B
    CombinatorialSolution heuristic_solution = heuristic_solve(problem); // x_h
    problem_upper_bound = objective_function(heuristic_solution); // B = f(x_h)
    CombinatorialSolution current_optimum = heuristic_solution;
    // Step 2 above
    queue<CandidateSolutionTree> candidate_queue;
    // problem-specific queue initialization
    candidate_queue = populate_candidates(problem);
    while (!candidate_queue.empty()) { // Step 3 above
        // Step 3.1
        CandidateSolutionTree node = candidate_queue.pop();
        // "node" represents N above
        if (node.represents_single_candidate()) { // Step 3.2
            if (objective_function(node.candidate()) < problem_upper_bound) {
                current_optimum = node.candidate();
                problem_upper_bound = objective_function(current_optimum);
            }
            // else, node is a single candidate which is not optimum
        }
        else { // Step 3.3: node represents a branch of candidate solutions
            // "child_branch" represents N_i above
            for (auto&& child_branch : node.candidate_nodes) {
                if (lower_bound_function(child_branch) <= problem_upper_bound) {
                    candidate_queue.enqueue(child_branch); // Step 3.3.2
                }
                // otherwise, g(N_i) > B so we prune the branch; step 3.3.1
            }
        }
    }
    return current_optimum;
}

cartographer 编译需要注意的点

find /opt/ros/noetic/ -name librosbag-storage.so
source /opt/ros/noetic/setup.bash

vscode extensions: dev-container

07-16

  • 排查测试环境 PGO 报错：老问题，旧 pb 轨迹文件 用 新版 pb 更新后的工具解析
    • 解决方案：使用旧版和新版都有的欧拉角来计算四元数

07-17

  • 解决轨迹对比显示问题
    • 现象：轨迹序号对应错位，且输出的轨迹相较于原始轨迹发生整体偏移
    • 原因：launch.json 文件轨迹顺序与 mapping_bag_filenames.txt 内不一致，对应错误导致参考点不一致，从而产生偏移
  • 轨迹转到统一 ENU 系下，并在 flatpak 上下载 cloudcompare 可视化

07-21

  • 理清 bev_cartographer_optimization_bin 的代码思路
    • 加载 lua 配置
    • 设有 0, 1, … , N-1 号轨迹，会依次从 bag 包读取轨迹信息，需要确认优化 k 号轨迹时，是否会用 0～k-1
      • 以 0 号轨迹第一帧作为参考点
      • 每条优化的时候是否会用到已有的结果，需要确认 的优化后的信息
    • 最后执行一次全局优化，但目前观察到的是未检测出新约束，也就是没有新回环出现

std::function

7.22:

SubmapId 由两部分组成，轨迹 id 和 子图 id

7.23:

  • 回调函数的绑定：自顶向下 ``` offline_node.cc: 369 node.AddOfflineTrajectory() node.cc: 306 map_builder_bridge_.AddTrajectory() map_builder_bridge: 94 map_builder_->AddTrajectoryBuilder() map_builder.cc: 212 absl::make_unique( ... , CreateGlobalTrajcetoryBuilder2D) collated_trajectory_builder.cc: 55 CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( ... , wrapped_trajectory_builder)

collated_trajectory_builder.cc: 75 sensor_collator_->AddTrajectory( … , HandleCollatedSensorData()) collator.cc: 29 void Collator::AddTrajectory( … , const Callback& callback) {

collated_trajectory_builder.cc: 93 void CollatedTrajectoryBuilder::HandleCollatedSensorData( … ) { … } collated_trajectory_builder.cc: 127 data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get()); dispatchable.h: 38 trajectory_builder->AddSensorData(sensor_id_, data_);


  - AddTrajectoryBuilder 是初始化的核心入口
  - CollatedTrajectoryBuilder 通过 CreateGlobalTrajectoryBuilder2D 打包了 2D 前端和 2D 位姿图
    - LocalTrajactoryBuiler 是前端，不带 Loop Closure；具体使用的是 LocalTrajectoryBuilder2D
    - mapping::PoseGraph2D 用于后端优化


- scan 处理流程：自顶向下

           offline_node.cc: 392  node.HandleLaserScanMessage()
                   node.cc: 520  map_builder_bridge_.sensor_bridge(trajectory_id)->HandleLaserScanMessage(sensor_id, msg);
          sensor_bridge.cc: 211  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
          sensor_bridge.cc: 285  HandleRangefinder()
          sensor_bridge.cc: 311  trajectory_builder_->AddSensorData()  collated_trajectory_builder.h: 62   AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data)); collated_trajectory_builder.cc: 84   sensor_collator_->AddSensorData(trajectory_id_, std::move(data)); 
               collator.cc: 55   queue_.Add(std::move(queue_key), std::move(data));
    ordered_multi_queue.cc: 90   Dispatch()
    ordered_multi_queue.cc: 193  next_queue->callback(next_queue->queue.Pop()); ```
Total views. cowboys. Hits