longer95479@home:~$

Archive

About

Categories

Tags

RSS

Todo

  • 特征提取
    • 数据关联
    • 数据结构 frame::_features and frame::_track_id
    • 流程 [ ] 运动估计 [] 数据结构 [] 流程 [] 后端优化 [] 数据结构
  • 数据集读取
    • json
    • csv (包含 mag 数据)
  • 可视化：
    • 把地图显示在 rviz 上，方便调试
      • 尺度上的变换
      • 左上角为 (0, 0), x 向右为正，y 向下为正，x 指向北
      • 定义地图系（NED，相对于 rviz 的世界系）
      • 解决读取不到 .dae 文件的 bug：打开rviz的终端找不到 slampose包
      • mesh 不显示：尺度变换错误，尺度太小导致看不见
      • 确认 marker 的固联坐标系：见 手写笔记
  • 确定状态量、 观测量、因子（约束）：
    • 状态
      • geo_T_slamp
    • 观测量 与 因子
      • Local Factor(VIO)
      • GPS Factor
      • Magnetmeter Factor
  • 确定数据结构
    • 阅读 test_PGO
    • 安装源码版 pycolmap
  • 初始化：
    • NED 坐标系下轨迹各帧初始值 = SE(3) * slam_pose
    • 先将 localkit 的经纬度变换到 地理系
    • 确定平移部分：x、y 从 slampose 和 localkit 最近邻点对来获取，然后取个平均
    • 确定旋转部分：yaw从磁力计获取、或者从已有轨迹和地图对齐考虑
  • 约束与优化：
    • Local Factor(VIO): slampose 帧间位姿
    • wifi/GPS Factor: localkit 找时间上最近邻的帧施加约束
    • manhattan yaw
      • 施加 yaw offset
      • 读取 mgw 数据
      • 预处理 mgw 数据
      • 构建约束，和 location 一起
    • Magnetmeter Factor: 磁力计施加的 yaw 约束
    • huber 鲁棒核函数
  • 数据输出
    • 结果转换为 rpy
    • 结果打包到 list
    • 按指定格式，写到指定位置下的 csv
    • 0-6 数据集rosbag 图片 265 张，但 slampose 有 267 个，转成 rosbag 有点问题
  • 高频传播：
    • imu propagate

一些初步结论：

  • mgw 姿态比 slampose 的姿态好，因此输出只输出 mgw 的角度
  • mgw 姿态 和 slampse 的平移初始值共同构成 AbsPose 先验，这个先验的方差如果设置小，也就ishi比重大一些后，会导致整体的轨迹尺度发散，这就导致 mgw 的权重无法太大，因此变成了辅助定位

第一帧使用 mag_in_imu 计算的 T 有误，甚至会影响到尺度

  • 0 mgw slampose
    • 0: back b
    • 1: forward b
    • 2: b b
    • 3: f b
    • 4: b b
    • 5: f b
    • 6: b b
    • 7: b b
    • 8: f b

旋转错误

  • 0-4
  • 0-8
  • 1-0
  • 1-1

pytorch 与其他框架训练的网络如何交互：onnx 开放神经网络交换

分为用于实验的即时模式，和用于高性能执行的图形模式

反向模式自动微分，Chainer

  • 写一个子程序计算函数值 f(x)

A simple explanation of reverse-mode automatic differentiation

backpropagation algorithm:

  • 本质上是链式法则
n = 4, N = 7

1    2    3    4
 5(1,2)  6(1, 3, 4)
      7(5, 6)

x_i <- f_i(X_{/pi(i)})
i = n + 1, ... , N


dxN/dxN <- 1
dxN/dxi <- \sum_{k: i = /pi(k)} dxN/dxk * dfk/dxi
i = N-1, N-2, ... , 1



  graph (1)

1  x1   1
   | \ /
   v  v
2  x2 x3
    \ /
     v
3    x4

x1.need_grad()
1. x2 <- f2(x1) = x1
2. x3 <- f3(x1, 1) = x1 - 1
3. x4 <- f4(x2, x3) = x2 * x3

dx4/dx4 = 1
3. dx4/dx3 = dx4/dx4 * df4/dx3 = x2
2. dx4/dx2 = dx4/dx4 * df4/dx2 = x3
1. dx4/dx1 = dx4/dx2 * df2/dx1 + dx4/dx3 * df3/dx1
           = x3 * 1 + x2 * 1
           = x1 - 1 + x1
           = 2x1 - 1

  graph (2)

1  x1
   | \
   |  v
2  |  x2
   | /
   v
3  x3

x1.need_grad()
1. x2 <- f2(x1) = x1 - 1
2. x3 <- f3(x1, x2) = x1 * x2

dx3/dx3 = 1
2. dx3/dx2 = dx3/dx3 * df3/dx2 = x1
1. dx3/dx1 = dx3/dx3 * df3/dx1 + dx3/dx2 * df2/dx1
           = x2 + x1 * 1
           = x1 - 1 + x1
           = 2x1 - 1

graph

The biggest difference is that autodiff can differentiate algorithms, not just expressions

2024-04-20

PyTorch is a Python-based scientific computing package serving two broad purposes:

A replacement for NumPy to use the power of GPUs and other accelerators.

An automatic differentiation library that is useful to implement neural networks.

Pi0

  1. 社区更好地掌握了创建可迁移通用模型的方法，使得微调或零样本迁移到其他机器人上成为可能
  2. 视觉语言模型，让机器人能够理解环境中的语义
  3. 强化学习领域的进步，使系统的健壮性、可靠性、性能得到突破

LLM：文字 -> 文字 VLM：图片+文字 -> 文字 VLA：图片+文字+Robot Action Data -> Robot Action Data

通用模型比专用系统表现更好 纳入异质数据源显著提升了繁华能力

训练基础模型的流程：

  • 预训练：爬虫、筛选巨大的预训练数据集 -> 设计网络架构（一般是 Transformer）-> 设计训练目标

视觉多模态理解模型基础

C 语言模板库得到的启示：

如何在 C 语言中构建 类 呢

  • 使用结构体 struct
    • property：需要初始化的成员变量
    • value：用于缓存
    • 函数指针

使用的步骤：

  • 核心思路是三步走：
    • 成员变量（普通变量+函数指针）初始化（赋初值）
    • 通过函数指针调用算法初始化函数
    • 再通过函数指针调用逻辑执行函数
  • C++ 有构造函数，而在 C 里，则需要单独写一个 全局函数，用于将 函数指针 绑定（被赋值）到某个具体的函数实现上

  • 把 DD 相关的写完，总结好

  • vins 初始化流程
    • c0bk 意味着是以 c0 为参考，因此尺度 s 不确定
  • 理解 plnet 的 pipline 以及 数据结构
    • pipline、基本公式（如果有的话）
    • 如何在代码上把 onnx 转成 engine
    • 数据结构

  • 自己实现一个智能指针

  • 初学 diffusion 原理
  • 初学 Transformer 原理

  • 初学 RL 原理

  • 复习 Pytorch
  • PCL 库学习

依赖：

  • NvOnnxParser.h
  • 3rdparty/tensorrtbuffer/include/buffers.h
  • Eigen/core
  • opencv.hpp

resize 图片相关:

  • input_width, input_height

keynote: The Real Problem of C++ - Klaus

c++ has a safety problem

  • yes if you still have the “classic C++” midset

always choose perfomence, cuz:

  • YES: safe over fast
  • NO: fast over safe

topic:

  • Bound Safety
  • Type Safety
  • Initialization Safety
  • Lifetime Safety
  • Undefined Behavior
  1. Bound Safety

The first part about ranges and not using loops, that’s mainly about using a declarative style - so that your code says what it does, but not how it does that, and every common part of the logic (like take) is a separate library function (tested billions times unlike the for loop you make instead).

  • the “Ranges” style. 
    std::ranges::sort()

和 std::sort 有什么区别?

  • No Raw Loops

代码示例见视频

  1. undedined Behavior

  • using constexpr -std=c++20 才好用，否则无法在 constexpr 的函数内创建 vector 等数据结构

  • complier explorer 是个好工具

  • 函数只能全特化，不能偏特化（全特化意味着template<>，也就是括号里得为空，然后在函数名后3所有具体的类型名）

explicit 关键字用于一个参数的函数，一般用于 构造函数，禁止隐式转换的发生

boost::bind 可以在运行时动态创建 可调用对象

如果函数不在类内，则

int add(int a, int b) { return a + b; }
auto add2 = boost::bind(add, 2, _1);
add2(3); // 5

否则：

one_class(){
  ...
  _one_thread = std::thread(std::bind(&one_class::method, this));
  ...
}

RAII(Resource Acquisition Is Initialization): 资源获取即初始化。利用存储在栈的对象，来管理资源

  1. 设计一个类封装资源
  2. 在构造函数中初始化资源
  3. 在析构函数中销毁资源
  4. 使用时声明一个该类的对象

c++经验之谈一：RAII原理介绍

一个 procedure A 调用另一个 procedure B 时，计算机需要干的事情：

  1. 转移控制
  2. 转移数据
  3. 分配和释放内存
int fact(int n)
{
  int result;
  if (n <= 1)
    result = 1;
  else
    result = n * fact(n-1);
  return result;
}

main() -> fact(n) -> fact(n-1) 即将-> fact(n-2) 的 call stack:

      (stack top)
  +-----------------+ <-- stack pointer (SP)
  |      n - 2      | \  
  +-----------------+  | 
  |      result     |   > frame for fact(n-1)
  +-----------------+  | 
  |  saved register | /  
  +-----------------+
  |  return address | \  
  +-----------------+  |  
  |      n - 1      |  | 
  +-----------------+   > frame for fact(n)
  |      result     |  | 
  +-----------------+  |
  | saved registers | /
  +-----------------+
  |  return address | \  
  +-----------------+  | 
  |        n        |   > frame for main()
  +-----------------+  | 
  |       ...       | /  
  +-----------------+
    (stack "bottom")

设计模式

工厂模式比较

  • 构建方法：返回一个对象的函数或方法 
    class number {
number(v): value(v) {}
unique_ptr<widget> nextFactory() {
  return make_unique<number>(value + 1);
}
private:
int value;
}
  • 简单工厂模式

简单工厂通常没有子类。 但当从一个简单工厂中抽取出子类后， 它看上去就会更像经典的工厂方法模式了。

class Button {/* ... */}; 
class WinButton: Botton {/* ... */};
class MacButton: Botton {/* ... */};

class ButtonFactory {
  unique_ptr<Button> create(string type) {
    switch (type) {
      case "Mac": return make_unique<MacButton>
      case "Win": return make_unique<WinButton>
      default: cout << "Wrong type." << endl; return nullptr;
    }
  }
}
  • 工厂方法模式 ```c++ class Button {/* … /}; class WinButton: Botton {/ … /}; class MacButton: Botton {/ … */};

class ButtonFactory { vitual unique_ptr createButton()=0; }

class MacButtonFactory: ButtonFactory{ unique_ptr createButton() { return make_unique(); } }

class WinButtonFactory: ButtonFactory{ unique_ptr createButton() { return make_unique(); } }



- 抽象工厂

抽象工厂 是一种创建型设计模式， 它能创建一系列相关或相互依赖的对象， 而无需指定其具体类。

什么是 “系列对象”？ 例如有这样一组的对象：运输工具+ 引擎+ 控制器 。 它可能会有几个变体：

1. 汽车+ 内燃机+ 方向盘
2. 飞机+ 喷气式发动机+ 操纵杆

如果你的程序中并不涉及产品系列的话， 那就不需要抽象工厂。

再次重申， 许多人分不清抽象工厂模式和声明为 abstract的简单工厂。 不要犯这个错误！

example:

产品结构：

class Button; // Abstract Class

class MacButton: public Button {};

class WinButton: public Button {};

class Border; // Abstract Class

class MacBorder: public Border {};

class WinBorder: public Border {};


对应的工厂：

class AbstractFactory { public: virtual Button* CreateButton() =0; virtual Border* CreateBorder() =0; };

class MacFactory: public AbstractFactory { public: MacButton* CreateButton() { return new MacButton; } MacBorder* CreateBorder() { return new MacBorder; } };

class WinFactory: public AbstractFactory { public: WinButton* CreateButton() { return new WinButton; } WinBorder* CreateBorder() { return new WinBorder; } };

那么客户可以根据需要选择 Mac 风格或者 Win 风格来创建 Button 或 Border:

AbstractFactory* fac; switch (style) { case MAC: fac = new MacFactory; break; case WIN: fac = new WinFactory; break; } Button* button = fac->CreateButton(); Border* border = fac->CreateBorder();


---


## lambda

1 ^ /
[=] () mutable throw() -> int { int n = x + y;

x = y; y = n;

return n; }


lambda 表达式虽然是一种语法糖，但它本质是一种 重载了 `operator()` 的类的对象，每个 lambda 表达式都对应着一个唯一的匿名类

之所以编译器默认可以将不带捕获的lambda转为函数指针，而不能将带捕获的lambda转为函数指针，是因为从根本上讲，带捕获的lambda被编译器解释成了对象，而不带捕获的lambda被解释为函数，是两种完全不同的概念！

带捕获的lambda可以看做一个对象，不带捕获的只能看成函数。此处二者的最大区别在于对象能够保存状态，而正常情况下函数没有状态的概念。当使用了static后，是相当于给函数赋予了保存状态的能力，此时也就能当成“带捕获的lambda”使用了

---

- [x] “发明内容“后半部分：增加三步骤的图；文字部分
- [x] ”具体实施方式二：边界跟踪算法“ 修改公式，以及配图
- [x] 权利要求书对应的公式
- [x] 更新表 1
- [ ] 解决 visio 图模糊


---

笔记丢失-重记

- 3D线的普吕克坐标：(n, v) = (x1 x x2, x2 - x1)
- 坐标系变换证明：从 Rx+t 入手，然后尝试构造出 x1 x x2 和 x1-x2
- 投影的三角化证明：记住投影后的 2D 线的表达是  (A,B,C)；



射影几何中的对称关系：
- 线-面-点
- 矩阵-4D向量-矩阵

点：[L]x = AB^T-BA^T

[plucker matrix](https://en.wikipedia.org/wiki/Pl%C3%BCcker_matrix)


---

- [ ] 熟悉shapely

- 基本对象
  - 点(Point)
  - 线(LineString)
  - 多边形(Polygons)

- 多个对象可以组合成一个对象

- 对象的一些属性
  - 面积、长度、边界、bounds、minimum_clearance、distance、hausdoof distance
  - 单谓语：has_z、is_empty、


- [ ] 坐标系如经纬度、莫卡托、enu

  - [x] 经纬度：本初子午线为 0 度经线，赤道为 0 度纬线，具体来说，某位置的经度是一个通过本初子午线的平面和一个通过南极、北极和该位置的平面所组成的二面角；纬度是线面角
  - [x] 莫卡托：是一种等角投影，但面积会发生形变，可以显示任意两点间的正确方位，指出真实的经纬度。[莫卡托直观解释](https://www.bilibili.com/video/BV1y14y1L7Tf/?spm_id_from=333.337.search-card.all.click&vd_source=e371652571b1539bbd501fb7adb6cfc4)；推导：经纬的小长方形长宽比等于变换后的长宽比
  - [x] 一种本地坐标系，东北天


- [ ] 滤波，尤其是针对轨迹跳变情况的处理

  - 可能的来源：1）滑窗优化带来的问题，外参参与优化导致的，有很多极小值；2）高频传播带来的问题；3）特征质量差，跟踪不连续，会闪烁跳变

  - 从传感器的角度入手，零偏估计不准，导致 imu propagation 会有较大漂移；解决思路：提高对零偏的估计精度；特征跟踪差，改进前端；先验信息入手，信号缓存➕状态机

  - 从信号滤波的角度入手，不要全反馈，而是部分反馈 VIO 给 imu 高频传播；低通滤波，参考比赛的解决方案；匹配滤波

    // b = 2 * PI * fc * dt, dt = 1 / fs
    // a = b / (1 + b)
    // y[n] = a * x[n] + (1-a) * y[n-1]


  - [ ] ESKF：旋转误差状态的参数个数等于自由度，避免了过度参数化，以及协方差矩阵的奇异；误差状态系统总是在原点附近工作，避免了

可以把 原始信号 视为 大信号 与 小信号 的叠加

nominal ---------inte-----------> next nominal
                                      ^
                                      | inject
error-state ---pred and update--> next error-state -> set zero

这个 inject 就会带来阶梯


在INS/GNSS组合导航KF滤波中，INS的频率高比如100Hz，而GNSS的频率相对较低比如1Hz，每到整秒时刻有GNSS量测，对INS误差状态做估计，估计结果反馈给INS，以提高INS精度。

通常采用全反馈方式，即在整秒时将KF状态估计全反馈给INS，同时将KF状态清零，这时往往会造成INS导航参数在整秒时刻台阶性跳变。如下图所示为某5dph的MEMS与米级单点GNSS组合，蓝色线为GNSS定位（1Hz），红色线为全反馈组合INS结果（10Hz输出），红线存在明显转折波动（经验证好像IE采用此方式）。若将KF反馈方法改为部分反馈（时间参数3s，每次反馈量约KF状态估计的0.01s/3s=0.3%），效果如洋红色线，这样就比较平滑了（至少表面上好看些，虽然不是最优的）。
[组合导航卡尔曼滤波部分反馈校正的效果展示](https://zhuanlan.zhihu.com/p/673939195)


KF滤波要求系统状态过程满足线性，高斯假设；EKF通过线性化在某种程度上改善了KF的缺陷；UKF通过无损变换确保的无损的变换，但是毕竟UKF融合属于单假设过程，不容易解决机器人绑架问题。在无人车定位问题中，单假设的数据融合在某种程度上没有多假设的数据融合稳定。现在包括2007年，2010年斯坦福大学无人车队的定位的论文都是基于单假设。2017年百度Apollo框架的定位算法也是基于KF的单假设融合。粒子滤波算法在位置估计方面的优势在于对系统没有线性化的要求。用一句话来归纳粒子滤波：采用多个粒子（假设）来估计无人车的位置（状态）。

[贝叶斯滤波体系](https://zhuanlan.zhihu.com/p/391078675)


---


1. 为什么一般滤波比优化速度更快

可以从 优化变量个数 和 优化迭代次数 两方面对比。

优化个数：滤波只考虑当前状态和下一状态，窗口2；优化则有更多的变量参与，窗口较大

迭代次数：卡尔曼滤波只线性化一次（预测和更新算一次），而优化有多次线性化

额外思考：

微分方程 -> 离散递推 -> 即两个状态间的约束（优化/因子图角度） | v 预测（滤波角度）



2. 旋转矩阵的一些特性，维度，列向量模长，含义等

特性：R^TR = I，det(R) > 0
维度：9 个参数，3 个自由度
列向量模长：1
含义：如果 R 能够把一个向量的坐标，从 B 坐标系 变换到 A 坐标系，那么 R 的三个列向量是 B 坐标系三个轴在 A 坐标系下的坐标/表达。


3. 粒子滤波，ICP，NDT，GICP 的区别，重定位问题怎么做的？

- [ ] 粒子滤波：非参数化的蒙特卡洛方法

重要性采样 q 是关键
为什么可以只保留 xt

```python

N 个采样点

for i in range(N):
  x_t[i] sample from p(x_t | x_{t-1}[i])
  w_t[i] = w_{t-1}[i] * p(z_t | x_t[i])
normalize w_t[i] to \sum_1^N w_t[i] = 1

o->o->o      o->o->o->o     o->o->o->o                                
|  |  |      |  |  |        |  |  |  |                                
v  v  V      v  v  V        v  v  V  v                                
o  o  o      o  o  o        o  o  o  o
w(xt) = p(xt z1:t) / q(xt z1:t)
= \int p(x1:t z1:t) dx1:t-1 / \int q(x1:t z1:t) dx1:t-1

  • ICP：最近邻找匹配，优化求解，重复以上两步骤

  • NDT

  • GICP

  • 重定位问题怎么做的：看一下 airslam 的重定位方案

  1. 高斯牛顿在什么时候失效
  • 初始值不准确 + 目标函数非凸
  • 可能被鞍点吸引

f(x) = r^T(x) r(x)

  • 牛顿法：相当于把目标函数 f(x) = f(x0) + \Nabla f^T(x0)(x-x0) + H 二阶展开，然后利用 梯度为0 去求解
  • 高斯牛顿：不直接求解海塞矩阵，而是进行近似
  1. 地面点分割的方法
  1. 预积分 bias 的处理

先梳理一下预积分的思路

  1. 状态的微分方程组 -> 积分形式
  2. 假设 a 和 w 短时间恒定，简化积分形式
  3. 代入 a 和 w 的测量模型，假设短时间 R 没变，得到 递推式
  4. 将递推写成求和形式，把 pvR 移到同一边，得到 最初始预积分公式
  5. 将 \delta pvR 变换到 第 i 帧下（我们称相邻两帧为第 i，j 帧），从而将测量侧的 Rwk 变成 Rik，但此时测量侧还有状态量 vik
  6. p 等式左右减去 vii，再根据 v 的 预积分测量部分形式，减去 g
  7. 对 模型预测 一侧进行噪声分离
原始预测约束（原始测量 ，状态）= 0
               | 
     ----------- f
     |
     v 
预积分测量 = 预积分预测模型（状态）

用 f(b) 表示预积分原始测量到预积分测量的映射，当 b 在迭代过程中变化较大时，就该更新 f(b) <- f(b) + J^f_b \delta b

如果任何远程修改与本地未提交的修改重叠，合并将被自动取消，工作目录树不会被改动。 一般来说，最好是在拉取之前把任何本地的修改弄到工作状态，或者用 git-stash[1] 把它们贮藏起来。

Git 小技巧 - 忽略不想要提交的本地修改

git update-index --skip-worktree <file>

git ls-files -v |grep '^S'

S config_scipt/global_config_indoor.yaml
S config_scipt/global_config_outdoor.yaml
S shfiles/record.sh
S shfiles/record_csi_rs_imu.sh
S shfiles/record_vins.sh
S src/auto_search/search_plan/launch/search_plan.launch
S src/auto_search/target_merge/launch/target_merge.launch
S src/planner/plan_manage/launch/advanced_param_exp.xml
S src/planner/plan_manage/launch/single_run_in_exp.launch
S src/realflight_modules/VINS-Fusion-gpu/config/fast_drone_250.yaml
S src/realflight_modules/det-reg-pnp/detect_box_pnp/yaml/camera_param.yaml
S src/realflight_modules/px4ctrl/config/ctrl_param_fpv.yaml

需要增加：

  • 协同前进逻辑

  • 增加 search plan 的 drone_id, total_drone_num
    • 在 launch 文件里加
    • 在 定义处 和 初始化处 增加 drone_id、total_drone_num

  • 定义 ros msg，lcm data type，话题名字

  • 订阅 “/start_stage_colla/receive_colla_signal”
    • [x]并写回调函数
  • 注册 “/start_stage_colla/send_colla_signal”
    • 并在 send_HV 和 sent_FW 处调用
  • sent_HV(),
    • msg 赋值
    • 传 msg 给 话题 “/start_stage_colla/send_colla_signal”，lcm_node 里会订阅
  • sent_FW()
    • msg 赋值
    • 传 msg 给 话题 “/start_stage_colla/send_colla_signal”，lcm_node 里会订阅
  • 增加标志变量
    • is_kplus1_drone_hovered
    • is_kminus1drone_forward
    • enable_colla_mode_
  • 工具实现
    • 在状态机中重复执行某件事：本质上需要一个计数器
  • 单元测试
    • 生成测试数据的 node
  1. ros pub “/traj_start_trigger” geometry_msgs::PoseStampedPtr
  2. lcm pub “colla_signal” lcm_node::CollaSignal HV
  3. lcm sub “colla_signal”

部署到无人机上：

  • modified: search_plan/CMakeLists.txt
  • modified: search_plan/package.xml
  • modified: search_plan/launch/search_plan.launch
  • modified: search_plan/include/search_plan/search_plan_fsm.h

  • modified: search_plan/src/search_plan_fsm.cpp

  • untracked files: lcm_node/

!!!NOTE: 使用 路由器 不需要执行 readme 里的两条命令，使用 3070 则需要

!!!NOTE: 无人机 pull 之后一定要重新执行 set_config 指令

!!!NOTE: 新的 ubuntu 默认防火墙是开着的，需要关闭才能使用 lcm_node


interval = 3
times = 5

o o o o o o o o o o o o o
|     |     |     |     |


input interval = 3,
      times = 5
initially counter = 0;

if counter % interval == 0:
  do first thing

if counter == interval * (times - 1):
  do second thing: change state / stop
  counter = -1
else if counter != -1:
  counter++



package StartStageCollaboration;

struct CollaSignal
{
  int64_t timestamp;
  int8_t  from_id;
  int8_t  to_id;
  string  signal_name;
  boolean is_ready;
}

lcm_node/msg/CollaSignal.msg

std_msgs/Header header
float64 timestamp
uint8 from_id
uint8 to_id
string signal_name
bool is_ready


GroundControl
  | TK  \    \______       ...
  |      \          \      
  v       v          v
drone1 <- drone2 <- drone3 ... <- droneN
  |    HV2  |    HV3  | 
  v         v         v
drone1 -> drone2 -> drone3 ... -> droneN
  |    FW1  |    FW2  |
  v         v         v
  go        go        go


TK：起飞信号
HV_{k}：第 k 号无人机 起飞完成已悬停 的信号
FW_{k}：第 k 号无人机 马上要向前飞行 的信号


 search_plan_node    至于 from_id 等等是不是想要的逻辑，写在顶层节点
   ^ | ros topic msg
   | v
  lcm_node           起到的作用类似透传
   ^ | lcm msg  
   | v
  other lcm_node
   ^ | ros topic msg
   | v
 other search_plan_node 


search_plan_node
  |  topic name: "/start_stage_colla/send_colla_signal"
  V
lcm_node

search_plan_node
  ^  topic name: "/start_stage_colla/receive_colla_signal"
  |
lcm_node

----------------------------------------------------
initially ID = k,
          state = TAKING_OFF,

if have_trigger:
  if ID == N:
    repeat sent HV_N T sec / or several times
    change state from TAKING_OFF to WAIT_FOR_START

  else if: 
    if HV_{k+1} == true:
      if ID != 1:
        repeat sent HV_k T sec / or several times
      else if ID == 1:
        wait T sec
      change state from TAKING_OFF to WAIT_FOR_START
    else :
      if timeout: take land
----------------------------------------------------

----------------------------------------------------
initially ID = k, 
          state = WAIT_FOR_START, 
          drone_num = N, 
          timeout_time = t0

if ID == 1:
  repeat send FW1 T sec
  just go forward

else if:
  if FW_{k-1} == true:
    if ID != N: repeat send FW_{k} T sec
    else if ID == N: wait T sec
    go forward
  else:
    if timeout: take land
----------------------------------------------------



  o__o__o     o           o       
  |  |  |     |           |
  |  |  |     |__o        |
  |  |  |     |  |        |__o
  ^  ^  ^     ^  |__o     ^  |
                 |  |        |
                 ^  |        |__o
                    |        ^  |
                    ^           |
                                |
                                ^

双机测试，成功。

1 号的 lcm_node 的打印如下

[ INFO] [1747815961.070115659]: [LCM->ROS] Takeoff/Land Command: 1
[ INFO] [1747815961.993584193]: [LCM->ROS] Takeoff/Land Command: 1
[ INFO] [1747815963.015122472]: [LCM->ROS] Takeoff/Land Command: 1
[ INFO] [1747815964.048727105]: [LCM->ROS] Takeoff/Land Command: 1
[ INFO] [1747815965.062666531]: [LCM->ROS] Takeoff/Land Command: 1
[ INFO] [1747815974.277258699]: [LCM->ROS] - From: 2, To: 1, Signal Name: HV, Ready: 1
[ INFO] [1747815988.613312717]: [LCM->ROS] - From: 2, To: 1, Signal Name: HV, Ready: 1
[ INFO] [1747815989.455305203]: [ROS->LCM] - From: 1, To: 2, Signal Name: FW, Ready: 1
[ INFO] [1747815989.455493594]: [LCM->ROS] - From: 1, To: 2, Signal Name: FW, Ready: 1
[ INFO] [1747815989.644815500]: [LCM->ROS] - From: 2, To: 1, Signal Name: HV, Ready: 1
[ INFO] [1747815990.468040941]: [ROS->LCM] - From: 1, To: 2, Signal Name: FW, Ready: 1
[ INFO] [1747815990.468145535]: [LCM->ROS] - From: 1, To: 2, Signal Name: FW, Ready: 1
[ INFO] [1747815990.668593675]: [LCM->ROS] - From: 2, To: 1, Signal Name: HV, Ready: 1
[ INFO] [1747815991.481560749]: [ROS->LCM] - From: 1, To: 2, Signal Name: FW, Ready: 1
[ INFO] [1747815991.481659712]: [LCM->ROS] - From: 1, To: 2, Signal Name: FW, Ready: 1
[ INFO] [1747815992.494322232]: [ROS->LCM] - From: 1, To: 2, Signal Name: FW, Ready: 1
[ INFO] [1747815992.494429545]: [LCM->ROS] - From: 1, To: 2, Signal Name: FW, Ready: 1
[ INFO] [1747815993.507451537]: [ROS->LCM] - From: 1, To: 2, Signal Name: FW, Ready: 1
[ INFO] [1747815993.507608988]: [LCM->ROS] - From: 1, To: 2, Signal Name: FW, Ready: 1

4 号的打印如下：

process[lcm_node-1]: started with pid [853]
[ INFO] [1747815961.042192186]: [LCM->ROS] Takeoff/Land Command: 1
[ INFO] [1747815961.965872858]: [LCM->ROS] Takeoff/Land Command: 1
[ INFO] [1747815962.997611415]: [LCM->ROS] Takeoff/Land Command: 1
[ INFO] [1747815964.021437248]: [LCM->ROS] Takeoff/Land Command: 1
[ INFO] [1747815965.039179071]: [LCM->ROS] Takeoff/Land Command: 1
[ INFO] [1747815974.188075823]: [ROS->LCM] - From: 2, To: 1, Signal Name: HV, Ready: 1
[ INFO] [1747815974.188161810]: [LCM->ROS] - From: 2, To: 1, Signal Name: HV, Ready: 1
[ INFO] [1747815975.201402070]: [ROS->LCM] - From: 2, To: 1, Signal Name: HV, Ready: 1
[ INFO] [1747815975.201514778]: [LCM->ROS] - From: 2, To: 1, Signal Name: HV, Ready: 1
[ INFO] [1747815988.538923666]: [LCM->ROS] - From: 2, To: 1, Signal Name: HV, Ready: 1
[ INFO] [1747815988.539103190]: [ROS->LCM] - From: 2, To: 1, Signal Name: HV, Ready: 1
[ INFO] [1747815989.510299331]: [LCM->ROS] - From: 1, To: 2, Signal Name: FW, Ready: 1
[ INFO] [1747815989.550854464]: [ROS->LCM] - From: 2, To: 1, Signal Name: HV, Ready: 1
[ INFO] [1747815989.551098949]: [LCM->ROS] - From: 2, To: 1, Signal Name: HV, Ready: 1
[ INFO] [1747815990.543507956]: [LCM->ROS] - From: 1, To: 2, Signal Name: FW, Ready: 1
[ INFO] [1747815990.562956255]: [ROS->LCM] - From: 2, To: 1, Signal Name: HV, Ready: 1
[ INFO] [1747815990.563423881]: [LCM->ROS] - From: 2, To: 1, Signal Name: HV, Ready: 1
[ INFO] [1747815991.562839441]: [LCM->ROS] - From: 1, To: 2, Signal Name: FW, Ready: 1
[ INFO] [1747815992.582230110]: [LCM->ROS] - From: 1, To: 2, Signal Name: FW, Ready: 1
[ INFO] [1747815993.504735561]: [LCM->ROS] - From: 1, To: 2, Signal Name: FW, Ready: 1

route -n 查看路由表

route add 224.0.0.0 netmask 240.0.0.0 dev wlan1 得保证两台机子的 dev 是在同一个 子网下，lcm 消息才能传通

UDP 单播：两个主机一对一 广播：一对所有 多播：一对一部分

对于多播，消息只是发送到一个多播地址，网络知识将数据分发给哪些表示想要接收发送到该多播地址的数据的主机。

多播的地址是特定的，D类地址用于多播。D类IP地址就是多播IP地址，即224.0.0.0至239.255.255.255之间的IP地址，并被划分为局部连接多播地址、预留多播地址和管理权限多播地址3类：

　　1、局部多播地址：在224.0.0.0～224.0.0.255之间，这是为路由协议和其他用途保留的地址，路由器并不转发属于此范围的IP包。

　　2、预留多播地址：在224.0.1.0～238.255.255.255之间，可用于全球范围（如Internet）或网络协议。

　　3、管理权限多播地址：在239.0.0.0～239.255.255.255之间，可供组织内部使用，类似于私有IP地址，不能用于Internet，可限制多播范围。

所以 export LCM_DEFAULT_URL=udpm://239.255.76.67:7667?ttl=1 是管理权限多播地址

IP 地址 = 网段（网络号）+ 机器号

网络号由 子网掩码 确定，未被掩盖的就是网络号，表示一个子网

e.g.:

  • 192.168.1 或 192.168.1.0\24（24 表示 IP 地址中有 24 位未被遮掩）

  • 192.168.1.160\27

对可控和可观的好解释

可以用大白话解释一下控制中可控和可观的概念? V777 V777​知乎知识会员 液压机械臂智能搬砖 Aurelian 等 27 人赞同了该回答 能控能观还真不能纯靠记线性情况结论来理解，那只是毫无益处的技术性结论，应付考试的。要把握其核心。对一个系统dx=f(x,u)，及其观测y=h(x)来说，有两个非常重要的映射：

一是u到x，二是x到y。

能控性是指前者是onto的，改变u能导致x取满Rn。能观是指后者是one-to-one的，使任何y都能唯一确认一个x。

是不是能理解为什么要研究这两个概念了？任何从零开始发展控制理论的人，都不可避免地研究这两个映射及其性质。

以线性情况举例。比如能控gramian就是把上述映射写出来，发现是一个矩阵指数，然后要求它必须行满秩。如果不是能控的，上述映射得到的矩阵可以自然诱导出一个分解，得到能控子空间和不能控子空间。

非线性情况的推广也没复杂到哪去，完全一样的思考过程。

这样的理解框架有很多好处，比如参数辨识/自适应里的PE条件和能观性是一模一样的，都是考察一段时间内x到w的映射是否one-one，即核空间是否平凡。因此没太有必要区分状态的能观性和参数的可辨识性。

Edsger Dijkstra - Turing Award Lecture - The Humble Programmer - 1972 - No Intro

简洁优雅地思考、描述、实现算法

仍然提到了重要的、强大的工具–抽象，能够让我们用有限的步骤去描述无穷的情况。

Nerf

有一个静态空间，在空间之外，发射一条射线，我们想要查询:

  1. 这条射线在空间中的每个点 (x, y, z) 的密度 $/sigma$，以及在该射线角度 $(\theta, \phi)$ 角度下呈现出来的颜色

  2. 然后沿这个方向做某种积分，得到某个像素的值。生成所有像素颜色后，也就生成了一张图片

\[F: (x, y, z, \theta, \phi) -> (R, G, B, \sigma)\]

第一步可以用 神经网络 来表示表示，第二步则是 体渲染过程

神经辐射场

本质：将单个神经网络过度拟合到这个特定的场景中。

因此，场景的信息被保存在 MLP 的参数中

NeRF将 \sigma(s) 密度建模为一个仅和采样点三维坐标相关的量，将 c(s) 颜色建模成一个和采样点三维坐标以及相机光线方向都有关的量，这种约束体现在如下的网络设计中

  x
  |
  v
+-----------+
|   MLP1    |
+-----------+
  |       |
  v       v
\sigma    L      d
          |      |
          v      v
        +-----------+
        |   MLP2    |
        +-----------+
              |
              v
              C

trick1: positional encoding

深层网络更倾向于学习低频函数[35]。

解决这个问题的一种方法是：位置编码。

把原本的信息映射到高维空间中

\[y: R -> R^{2L} y(p) = [sin(2^0 \pi p), cos(2^0 \pi p), ... , sin(2^L \pi p), cos(2^L \pi p)]\]

这样低频的三角函数刻画了低频变化，高频的三角函数刻画的高频变化

L = 10 for x and L = 4 for d.

NOTE: 这里的位置编码的作用和 transformer 里不一样，这里是为了让 MLP 也能拟合出高频变化，而 transformer 里的位置编码是为了给序列中的 tokens 提供离散的位置，使得实际的输入可以视为一个无序的集合，因为位置已经刻画再元素中了。

trick2: hierarchical volume sampling

有两个网络：一个粗，一个细

先进行粗采样，然后根据粗采样处的密度或者其他性质，决定是否进行细采样

coarse
---x------x-------x--------->

fine
---x----xxxxx---x-x-x------->

体渲染

\[C(r) = \int T(s) \sigma(s) c(s) ds\]

$T(s) \sigma(s)$ 可以视为 射线在 s 处击中粒子的概率

\sigma (s):
            __
           /  \
          /    \  
    o---------------------------------> s

\int \sigma (s):
            _________________________
            |
           /
    o---------------------------------> s
    
T(s):
    ______
          \
           \
    o---------------------------------> s

NOTE：推导过程有一个假设–光线没有二次反射，击中就会显示颜色

一方面，NeRF将 \sigma(s) 密度建模为一个仅和采样点三维坐标相关的量，将 c(s) 颜色建模成一个和采样点三维坐标以及相机光线方向都有关的量；另外一方面，在实际计算的时候，往往会选择Ray上一个最近的点、一个最远的点，只计算两点之间的粒子对最终颜色的贡献。根据上述两个方面便可得到NeRF的最终公式。

离散化：$\sum_{i=1}^N c_i T_i (1-e^{\sigma_i \delta_i})$

为什么可微分？

从训练流程（见下文）入手，推导出 C hat 对 c 和 delta 可导，也就证明了可微分。

如何与 SLAM 结合

假设我未阅读任何文献，如何把二者结合起来呢

NeRF: infer:

(x, y, z, \theta, \phi) -> MLP -> C, \sigma

train:

(x, d) -> MLP -> (c, \sigma) -> voxel_rendering -> pred_img
           ^                                          |
           |                                          v
           +-------------------------------------- compare 
                                                      ^
                                                      |
origin_img -------------------------------------------+

SLAM:

img + imu -> pose + map

combine:

 +-- origin_img -> VIO -> (x, d) 
 |       |                   |      
 |       v                   v 
 v     +-----------------------+
 - --> |    M     L     P      |
 ^     +-----------------------+ 
 |                |
 |                v
 |            (c, \sigma)
 |                |
 |                v
 |        +-----------------+
 |        | voxel_rendering | <- (x, d)
 |        +-----------------+
 |                |
 |                v
 +------------- syn_img

ref

NeRF: A Volume Rendering Perspective

3D-GS

Total views. cowboys. Hits