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，意味着不会对异常点作额外的平滑，因此更具阶梯性

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

协方差矩阵在单向滤波的时候是变大的吗？

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

为什么全量回传的数据时间间隔大和时间不均匀会让 ESKF 不适用呢？ 首先时间不均匀不会影响 ESKF 的使用，因为动力方程离散化的时候引入了 delta t，是可以变化的，只要多花一步计算一下就可以了。 关键是时间间隔大，导致 动力学方程 一阶离散化 不再合理，因为一阶意味着假设 加速度 和 角速度 是恒定的， 这导致无论是 名义递推 还是 误差状态递推都是不准确，且误差状态递推是 时变线性系统，所以时变也不准确，进一步带来误差。

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-24

优化 PGO 求解器：

  • cost 函数内的循环内重复执行 ndarray 的初始化，耗时巨大 ``` @@ -362,9 +367,10 @@ class AutoOptimizer(object): self.costStructReset() edgelist = self.optholder.edgelist startcolumns = np.array(self.vertexholder.startcolumns)
  • ver_array = np.array(self.vertexholder.vertexlist) for edge in edgelist: sc = startcolumns[edge.vertexindices]
  • relatedverts = np.array(self.vertexholder.vertexlist)[edge.vertexindices]

  • relatedverts = ver_array[edge.vertexindices] err, jac = edge.calculate(relatedverts) self.augSparseJac(jac, sc, err) error = np.array(self.optholder.error).reshape(-1, 1) ```

  • 减少乘法次数 和 类型转换次数 ``` @@ -467,9 +528,12 @@ class AutoOptimizer(object):

       g = Jac.T.dot(error)
   if mu is None:
  • A = np.array([Jac[:, j0].reshape(1, -1).dot(Jac[:, j0].reshape(-1, 1))[0, 0]
  • for j0 in np.arange(Jac.shape[1])])
  • mu = tau * np.max(A)

  • A = np.array([Jac[:, j0].reshape(1, -1).dot(Jac[:, j0].reshape(-1, 1))[0, 0]

  • for j0 in np.arange(Jac.shape[1])])

  • mu = tau * np.max(A)

  • A = Jac.transpose().dot(Jac)
  • A_array = A.toarray()

  • mu = tau * np.max([A_array[i, i] for i in np.arange(Jac.shape[1])]) if mu < 1e-7: mu = 10.0 ```

  • 减少深拷贝，线上 16s -> 12s ``` @@ -488,9 +552,14 @@ class AutoOptimizer(object): print(‘iteration end: LM step length small enough’) break else:
  • oldVertlist = copy.deepcopy(self.vertexholder.vertexlist)

  • oldVertlist = copy.deepcopy(self.vertexholder.vertexlist)

    self.update(d) newRes = self.cost() newErr = newRes[0] newJ = newRes[1] rho = (error.T.dot(error) - newErr.T.dot(newErr)) / (0.5 * d.T.dot(mu * d + g)) @@ -515,7 +584,8 @@ class AutoOptimizer(object): nu = 2.0 break else:

  • self.vertexholder.vertexlist = oldVertlist

  • self.vertexholder.vertexlist = oldVertlist

  • self.update(-d) mu *= nu nu *= 2.0 innerNum += 1 ```

  • set -c 0 强制 numpy 单线程执行，从111s降到16s

  • @ 替换为 .dot()，缓存 R0.T 到 R0_T，缓存 p1 - p0 ``` @@ -38,16 +41,18 @@ class PoseGraphOpt(object): dR = dT[0:3, 0:3] dp = dT[0:3, 3:] error = np.zeros((6, 1))
  • error[0:3] = -su.logSO3(R0.T @ R1 @ dR.T)
  • error[3:6] = dp - R0.T @ (p1 - p0)
  • error = weight @ error
  • R0_T = R0.T
  • dp0 = p1 - p0
  • error[0:3] = -su.logSO3(R0_T.dot(R1.dot(dR.T)))
  • error[3:6] = dp - R0_T.dot((dp0))
  • error = weight.dot(error) jac = np.zeros((6, 12))
  • jac[0:3, 0:3] = -R0.T
  • jac[0:3, 6:9] = R0.T
  • jac[3:6, 0:3] = R0.T @ su.vec2skewMat(p1 - p0)
  • jac[3:6, 3:6] = -R0.T
  • jac[3:6, 9:12] = R0.T
  • jac = weight @ jac
  • jac[0:3, 0:3] = -R0_T
  • jac[0:3, 6:9] = R0_T
  • jac[3:6, 0:3] = R0_T.dot(su.vec2skewMat(dp0))
  • jac[3:6, 3:6] = -R0_T
  • jac[3:6, 9:12] = R0_T
  • jac = weight.dot(jac) return error, (jac[:, 0:6], jac[:, 6:])
 def _fix_constraint(self, vert: aopt.Vertex, T, weight=np.eye(6)): @@ -55,9 +60,9 @@ class PoseGraphOpt(object):
     R = T[0:3, 0:3]
     p = T[0:3, 3:]
     error = np.zeros((6, 1)) -        error[0:3] = -su.logSO3(R0 @ R.T) +        error[0:3] = -su.logSO3(R0.dot(R.T))
     error[3:6] = p - p0 -        error = weight @ error +        error = weight.dot(error)
     jac = weight
     return error, (jac, ) ```

辅助工具，可以查看每行代码运行所消耗的时间：profiler

import numpy as np
from line_profiler import LineProfiler  # 注意这里导入的是LineProfiler

def my_func():
    a = np.random.rand(1000, 1000)
    a.dot(a)

# 创建分析器并添加函数
lp = LineProfiler()
lp_wrapper = lp(my_func)  # 包装要分析的函数

# 执行分析
lp_wrapper()

# 打印结果
lp.print_stats()

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

2025-07-22:

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

2025-07-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) {


  - 可以看到，callback 绑定了 HandleCollatedSensorData()，wrapped_trajectory_builder 指针由 CreateGlobalTrajcetoryBuilder2D() 函数返回值给出

类型： CollatedTrajectoryBuilder <- GlobalTrajectoryBuilder = LocalTrajectoryBuilder + PoseGraph2D + LocalSlamResultCallback 对象： 匿名 = local_trajectory_builder + pose_graph_ + local_slam_result_callback

其中 /**

  • @brief TrajectoryBuilderInterface是个接口类,没有具体实现
  • GlobalTrajectoryBuilder类 与 CollatedTrajectoryBuilder类 都继承了

  • TrajectoryBuilderInterface 并且都做了 AddSensorData() 的实现 */ class TrajectoryBuilderInterface { ```

  • 下面追踪了 HandleCollatedSensorData 执行的内容 ``` 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_); global_trajectory_builder.cc: 95 matching_result = local_trajectory_builder_->AddRangeData( 114 auto node_id = pose_graph_->AddNode( 126 matching_result->insertion_result->insertion_submaps.end())}); 132 local_slam_result_callback_(

  - 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()); ```
- 这里的 callback 就是上文绑定的 HandleCollatedSensorData()

总结：前后端算法的核心实现在 global_trajectory_builder.cc: 75 void AddSensorData(

接下来分析算法的具体实现，从前端 local_trajectory_builder_2d.cc: 162 LocalTrajectoryBuilder2D::AddRangeData() 入手

首先分析 LocalTrajectoryBuilder2D 数据结构，比较核心的是 struct MatchingResult，用于保存扫描匹配的结果，这也是 AddRangeData() 的返回值。

  • class LocalTrajectoryBuilder2D
    • struct MatchingResult
      • InsertionResult
        • std::shared_ptr<const TrajectoryNode::Data> constant_data;
        • std::vector<std::shared_ptr> insertion_submaps;

其中 constant_data 保存了 一些不会在后端优化中变化的量。insertion_submaps 一般会存两个相邻的 submap。

Submap2D:

  • timestamp
  • grid_
  • grid2_
  • conversion_tables_
  • node_pose_in_submap_

Grid2D: MapLimits limits_; // 地图大小边界, 包括x和y最大值, 分辨率, x和y方向栅格数

// 地图栅格值, 存储的是free的概率转成uint16后的[0, 32767]范围内的值, 0代表未知 std::vector correspondence_cost_cells_; float min_correspondence_cost_; float max_correspondence_cost_; std::vector update_indices_; // 记录已经更新过的索引

// Bounding box of known cells to efficiently compute cropping limits. Eigen::AlignedBox2i known_cells_box_; // 栅格的bounding box, 存的是像素坐标

// 将[0, 1~32767] 映射到 [0.9, 0.1~0.9] 的转换表 const std::vector* value_to_correspondence_cost_table_;

submap active 的设计逻辑：

submap_2d.h: 116
// The first active submap will be created on the insertion of the first range
// data. Except during this initialization when no or only one single submap
// exists, there are always two submaps into which range data is inserted: an
// old submap that is used for matching, and a new one, which will be used for
// matching next, that is being initialized.
//
// Once a certain number of range data have been inserted, the new submap is
// considered initialized: the old submap is no longer changed, the "new" submap
// is now the "old" submap and is used for scan-to-map matching. Moreover, a
// "new" submap gets created. The "old" submap is forgotten by this object.

submap 什么时候 finish，详见 submap_2d.cc: 381，在 InsertRangeData() 函数内

  // 第一个子图的节点数量等于2倍的num_range_data时,第二个子图节点数量应该等于num_range_data
  // note: 插入后，再判断是否结束前一个子图
  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {

    // // 调用 Submap2D 的结束函数
    submaps_.front()->Finish();
  }

调用情况：AddRangeData() -> AddAccumulatedRangeData() -> InsertIntoSubmap() -> InsertRangeData()

AddRangeData() 后会执行 pose_graph 的 AddNode() 函数，把 InsertionResult 加进后端

代码注释提到的 tracking frame 应该就是 scan frame local_pose 是 scan 相对于 submap 的位姿 global_pose 是 scan 相对于 某个统一的世界坐标系 的位姿

单条轨迹的整体数据结构： GlobalTrajectoryBuilder { LocalTrajectoryBuilder PoseGraph }

不同的 GlobalTrajectoryBuilder 用的是同一个 pose_graph_，详见 map_builder.cc: 216； 但是是不同的 local_trajectory_builder

结论：因子图是多条轨迹共享的，但每条轨迹维护各自的 submaps，

因此回环检测只可能在但条轨迹内触发，不会在不同轨迹见触发吗？ 不一定，因为会把 submaps 通过 PoseGraph::AddNode 加入到 posegraph 中，因此回环检测可能会涉及到不同轨迹之间的 submaps 关键是找到 执行回环检测 的部分，判断是否由 PoseGraph 调用

会在 pose_graph_2d.cc: 268 的 ComputeConstraintsForNode() 函数内进行回环（扫描匹配），并返回 是否执行优化 的结果

GlobalTrajactoryBuilder 总的流程大致为：

AddSensorData()
  - matching_result = local_trajectory_builder->AddRangeData()

  - pose_graph->AddNode( matching_result )

    - global->pose = global->local * local->pose

    - node_id = AppendNode( matching_reuslt, global->pose )
      // 干两件事：1.对应id轨迹添加节点信息 2.添加子图信息（指针）
      // 本质上就是添加信息到 data_

    - AddWorkItem( [=](){ return ComputeConstrainsForNode( matching_result ); } ) 
      // 返回值决定是否执行全局优化计算
      // 把计算约束的工作放入 工作队列 中，不是在当前时刻执行，而是后台执行

2025-07-29

搞清楚 程序主脉络 + 数据结构，基本就了解所有

2025-07-30

子图越大越好，搜索范围越大越好，但也有副作用。

好处：子图越大，越有唯一特征，越不容易退化 搜索范围越大，越能找到真实的位姿

搜索范围变大有副作用，横向更能匹配上，但会放大纵向退化的结果

2025-07-31: 实验发现子图越小，纵向匹配越准确，与前文的提到的“好处”相矛盾。

分析：子图越大，同时搜索范围也较大，而考虑到子图的特殊性–是车道，因此匹配的时候可以纵向滑动，而得到同样的分数，即退化； 而子图较小的时候，节点应该是挑选了一个最近的 submap 去尝试搜索回环，此时 submap 较小等价于约束了退化的范围，从而改善了建图的纵向效果

2025-08-01: 经测试发现，参数并没有可扩展性，适用于某个任务的参数，并不适用于另一个任务

无论是前端还是后端回环，都需要匹配，而匹配分为两步，第一步是 fast correlative scan matcher, 第二步是 ceres scan matcher。即先搜索，后优化。 因此在此处会涉及到优化。另一个涉及到优化的地方是，位姿图优化部分，是真正的后端优化。两部分的优化有各自独立的权重参数。

新问题：单轨迹内出现错位，因此首先需要在单轨迹内保证建图的连续性，再去考虑轨迹间的回环。 解决：

POSE_GRAPH.optimize_every_n_nodes = 0
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 0.1 -- 原本是 40
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40    -- 原本是 40

原因是过于信任先验值（外推结果）

如果都改成 0.1，意味着对里程计的相对位置和姿态都不信任，完全靠扫描搜索，结果会更模糊一些，针对目前看到的案例，不信任里程计相对位置，但信任相对姿态比较合理

!!!NOTE: 如果 POSE_GRAPH.optimize_every_n_nodes = 16，则结果仍是有较大横向跳变，为什么？ 又没有了，为何

修复轨迹内的横向跳变后，运行多程 SLAM 的结果仍有较大重影，观察 log 后猜测可能的原因是 轨迹间回环未触发

总结：严进宽出 -> 宽进严出。一系列筛选回环候选 -> 计算分数 -> 对最优值进行分数阈值筛选，如果在回环候选的时候就把真实的最优值筛选掉了，那么就绝对找不到最优值，也就是召回率很低。当前节点和不同轨迹的最近子图作回环是合理的，只限制在一条上，就已经把可能正确的回环候选排除掉了；此外 low_prec_corr = 1.5 也是拔高了分数下界，在分指定界阶段把可能的最优值给提前排除了

分辨率 0.05 反而不好

!!重要初步改动： pose_graph_2d.cc:649

    // std::vector<constraints::SubmapCand> accepted_cands =
    //     constraint_builder_.FilterGlobalSubmapCand(node_id, global_candidates);
    // DEBUG: add by longer
    std::vector<constraints::SubmapCand> accepted_cands = global_candidates;

原本过滤太暴力，global 回环只和在距离范围内的 id 最小的轨迹的子图去计算； 以上改动则是另一个极端，完全不作回环计算前的筛选，带来的后果则是有多个退化的回环被触发， 导致纵向上的重影。

应该改称 和每条轨迹最近的子图 尝试作匹配，已改动，效果：尝试计算的回环变多了

关于后端因子图优化的实验：

  • 1e10 量级优化效果差；
  • 只有子图内约束权重，无回环权重，效果差；
  • 只有回环权重，无子图内约束权重，效果差

2322191 任务的优化效果对 回环权重很敏感，在原来的基础上乘10或除以10,重影变大，目前最优值：

-- 子图内约束权重
POSE_GRAPH.matcher_translation_weight = 5e2 -- origin, do not use 5e3 from 3d
POSE_GRAPH.matcher_rotation_weight = 1.6e3 -- origin, do not use 1.6e4 from 3d
-- 闭环约束的权重
POSE_GRAPH.constraint_builder.loop_closure_translation_weight = 1e4 -- 1.1e4
POSE_GRAPH.constraint_builder.loop_closure_rotation_weight = 5e4 -- 1e5

把停止线、物理边界、人行道、箭头的点云都加入匹配: src/slam_bev_cartographer/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc:225

insert_map(grid, INTENSITY_TYPE_LINE, INTENSITY_TYPE_SHOW);

对于 1495847 任务，影响其效果的最大因素是 min_score 和 global_lovalization_min_score

34 扰乱最大，32 在轨迹内能够正常配准后对整体轨迹没有消极影响

cartographer 使用 并查集（或不交集森林 disjoint set forest）来跟踪不同轨迹间的连通性，详见源码 connected_components.cc

其后端数据集合 PoseGraphData data_ 还涉及一个底层数据结构，自定义的 MapById，是对 std::map 的重写，用 id + data 来表示一系列数据。

pose_graph_data.h:

// The current state of the submap in the background threads. After this
// transitions to 'kFinished', all nodes are tried to match
// against this submap. Likewise, all new nodes are matched against submaps in
// that state.
// 后台线程中子图的当前状态.在此转换为“kFinished”之后,
// 所有节点都将尝试与此子图匹配, 进行回环检测. 同样,
// 所有新节点都与kNoConstraintSearch状态下的子图匹配.
enum class SubmapState { kNoConstraintSearch, kFinished };

参考 ceres 的源码内的注释： problem.h, local_parameterization.h

cere 使用技巧：

  • 定义 ceres 问题
ceres::Problem problem
  • 配置求解器： 
    ceres::Solver::options options;
options.use_nonmonotonic_steps = xxx;
options.max_num_iterations = xxx;
...
  • 设计一个 CostFunction 单例类工厂，用于生成 CostFUnction
    • 重载 括号操作符
    • 私有化 构造函数，删除拷贝构造函数，删除 赋值操作符
    • 定义 工厂函数，用于返回 ceres::CostFunction*
// Computes the cost of translating 'pose' to 'target_translation'.
// Cost increases with the solution's distance from 'target_translation'.
class TranslationDeltaCostFunctor2D {
 public:
  // 静态成员函数, 返回CostFunction
  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const double scaling_factor, const Eigen::Vector2d& target_translation) {
    return new ceres::AutoDiffCostFunction<
        TranslationDeltaCostFunctor2D,  // 自定义的 costfunctor
        2 /* residuals */,
        3 /* pose variables */>(  // 残差维度 2维，输入是pose 3维
        new TranslationDeltaCostFunctor2D(scaling_factor, target_translation));
  }

  // 平移量残差的计算, (pose[0] - x_)的平方ceres会自动加上
  template <typename T>
  bool operator()(const T* const pose, T* residual) const {
    residual[0] = scaling_factor_ * (pose[0] - x_);
    residual[1] = scaling_factor_ * (pose[1] - y_);
    return true;
  }

 private:
  // Constructs a new TranslationDeltaCostFunctor2D from the given
  // 'target_translation' (x, y).
  explicit TranslationDeltaCostFunctor2D(
      const double scaling_factor, const Eigen::Vector2d& target_translation)
      : scaling_factor_(scaling_factor),
        x_(target_translation.x()),
        y_(target_translation.y()) {}

  TranslationDeltaCostFunctor2D(const TranslationDeltaCostFunctor2D&) = delete;
  TranslationDeltaCostFunctor2D& operator=(
      const TranslationDeltaCostFunctor2D&) = delete;

  const double scaling_factor_;
  const double x_;
  const double y_;
};
  • 自定义局部参数化 
    class CERES_EXPORT LocalParameterization {
 public:
virtual ~LocalParameterization();

// Generalization of the addition operation,
//
//   x_plus_delta = Plus(x, delta)
//
// with the condition that Plus(x, 0) = x.
virtual bool Plus(const double* x,
                  const double* delta,
                  double* x_plus_delta) const = 0;

// The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
//
// jacobian is a row-major GlobalSize() x LocalSize() matrix.
virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0;

// local_matrix = global_matrix * jacobian
//
// global_matrix is a num_rows x GlobalSize  row major matrix.
// local_matrix is a num_rows x LocalSize row major matrix.
// jacobian(x) is the matrix returned by ComputeJacobian at x.
//
// This is only used by GradientProblem. For most normal uses, it is
// okay to use the default implementation.
virtual bool MultiplyByJacobian(const double* x,
                                const int num_rows,
                                const double* global_matrix,
                                double* local_matrix) const;

// Size of x.
virtual int GlobalSize() const = 0;

// Size of delta.
virtual int LocalSize() const = 0;
};
  • 添加参数块（如果没有局部参数化，则可以省略，否则需要显式调用） 
    double ceres_pose_estimate[] = {x_, y_, theta_};
problem.AddParameterBlock(ceres_pose_estimate, 3, some_parameterization);
// some_parameterization 的类型是 LocalParameterization*
  • 添加残差块
  problem.AddResidualBlock(
      TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
          options_.translation_weight(), target_translation), // 位置的目标值
      nullptr /* loss function */, ceres_pose_estimate);      // 当前位置
  • 求解 与 结果总结
  ceres::Solver::Summary summary;
  // 根据配置进行求解
  ceres::Solve(ceres_solver_options_, &problem, &summary);

ceres 自动求导的原理，参考 ceres 的官方文档。 将实数 a 附加上 n 个无穷小量单位 e_i，之后重载基本函数在 Jet 上的加减乘除， 无穷小的部分则存储了导数，在 evaluate 实数的同时也 eveluate 对偶部分（即导数）， 有点像正向传播同时求导，但不考虑中间变量的导数，只考虑给定的 parameters 的导数。

证明则借助多元函数的泰勒展开 加上 e_i * e_j = 0 的性质。

\[x_i = a_i + v_i\]

令

\[v_i = \sum_{k} w_k*e_k = e_i\]

数值求导，则是施加扰动，然后求近似导数。

2025-08-11

LOAM SLAM原理之防止非线性优化解退化

cartographer 中的各个坐标系：

+------------------+      +--------------+
| global map frame |      | submap frame |
+------------------+      +--------------+
     ^
     |
global_pose
     |
     --------------------------------------------+
                                                 | 
+-----------------+                       +----------------+     +-----------------------+
| local map frame | <---local_pose------- | tracking frame |     | gravity aligned frame |   
+-----------------+                       +----------------+     +-----------------------+

定坐标系：global map frame, local map frame, submap fram 动坐标系：tracking frame

submap 中的第一帧 range data 的 tracking frame 的原点 作为 submap frame 的原点， submap frame 的姿态在构造函数中是单位阵。

tracking frame 2 local map frame: local_pose tracking frame 2 global map frame: global_pose submap frame 2 local map frame: local_submap_pose submap frame 2 global map frame: global_submap_pose tracking frame 2 gravity_aligned frame: gravity alignment gravity alignment 2 local frame: local_pose_2d gravity alignment 2 global frame: global_pose_2d

match_type 的设置

all_parallel has_stop_line is_turning 0xx -> match once: full 111 -> match once: full 101 -> MATCH_RETRY_RESTRICT/REVERSE 110 -> MATCH_RETRY_RESTRICT/REVERSE + MATCH_RETRY_LENGTHWISE 100 -> MATCH_RETRY_RESTRICT/REVERSE + MATCH_BOARDWISE_LIMIT

P.S.

// 在比例小于ratio_时返回true, ratio_设置为1时都返回true, 也就是说使用所有的数据
bool FixedRatioSampler::Pulse() {
  ++num_pulses_;
  // ?:频率为什么这么设置，动动笔写一下就明白了
  if (static_cast<double>(num_samples_) / num_pulses_ < ratio_) {
    ++num_samples_;
    return true;
  }
  // 返回false时代表数据可以不用,可以跳过计算
  return false;
}

bev_cartographer_bin 输出：

  • frame.txt
  • pose0.out
  • pose1.out
  • result.pbstream
  • result.pbstream.enutraj.track
  • result.submap
  • result.pose

bev_cartographer_optimization_bin:

  • result.pbstream_op.pstream
  • result.op.submap
  • result.op.pose

bev_mapping_bin:

  • traj_blh.txt
  • result.pbstream.enutraj.track
  • factor_reuslt/ output/ log/ view/ filtered/ lightFrame/ freespace/ frame/

2025-08-20 ~ 21

Break:

  • 推导了雅可比矩阵解析表达
  • 与自动求导对比了结果，注意点：尺度上，resolution，scale_weight

Eigen:Map 的作用：把连续内存的语义转化为指定大小的 Eigen 向量或矩阵。

带约束的优化两种思路：

  • 无约束优化，得到结果后再将结果投影
  • 增加对应约束后，再进行优化
  • 二者结合

实验：

  • 0.5权重的全向预测约束好像没起作用，权重变大(10)，纵向重影变大

  • 只有 10 权重的退化方向约束效果先好，调整mapping后变差，纵向重影变大

  • 只对优化结果投影，不加额外纵向约束，效果好

  • 二者结合效果很差，箭头纵向重影大到出现两个，和 权重调大（纵向或全向）的效果一样差

  • 小权重 + 无投影，即修改前的效果，一样是差的

总结：只有 仅投影 的效果是好的，调大 translation 的权重，无论约束是纵向还是全向，效果均变差。

2025-08-25

给停止线增加权重，效果变差

Total views. cowboys. Hits