雯欂の修仙笔记

障碍物用如下公式表示 O(m)={y∈Rn∣A(m)y⪯Kb(m)}\mathbb{O}^{(m)} = \left\{ \bm{y} \in \mathbb{R} ^n \mid \bm{A}^{(m)} \bm{y} \preceq _{\mathcal{K}} \bm{b}^{(m)}\right\} O(m)={y∈Rn∣A(m)y⪯K​b(m)} 上标(m)^{(m)}(m)表示在时间步为mmm时的障碍物位置；A(m)y⪯Kb(m)\bm{A}^{(m)} \bm{y} \preceq _{\mathcal{K}} \bm{b}^{(m)}A(m)y⪯K​b(m)可以展开为b(m)−A(m)y∈K\bm{b}^{(m)} - \bm{A}^{(m)} \bm{y} \in \mathcal{K}b(m)−A(m)y∈K。若令K=R+l\mathcal{K} = \mathbb{R} ^l_+K=R+l​即非负象限锥，则此广义不等式变为逐元素不等式≤\leq≤，此时障碍物由不同的半空间组成，是一个多面体；若令锥为二阶锥，此时障碍物是一个椭球 被控物体用符号E(xk)\...

1. 前言 随机排列算法，目的是将一个数组中的元素随机打乱随机排列 2. Fisher-Yates洗牌算法 2.1 思路 获取数组长度为nnn 随机选取[0,n−1][0,n-1][0,n−1]位置上的数据，并与第一个数据交换 随机选取[1,n−1][1,n-1][1,n−1]位置上的数据，并与第二个数据交换 重复这个过程 2.2 代码参考 void rand_permutation(const int n, int *p) { typedef std::uniform_int_distribution<int> rand_int; typedef rand_int::param_type rand_range; static std::mt19937_64 gen; static rand_int rdi(0, 1); int j, k; for (int i = 0; i...

1. 前言 重点介绍Seidel’s线性规划算法，适用于低纬度线性规划 完整代码详见github 2. 代码详解 2.1 linprog函数 求解的问题为 min⁡C⊤xs.t.Ax≤b\begin{gather*} \min \bm{C}^{\top}\bm{x} \\ s.t. \bm{Ax} \leq \bm{b} \end{gather*} minC⊤xs.t.Ax≤b​ prev存储的是链表中第i个元素的前一个元素的索引 next存储的是链表中第i个元素的后一个元素的索引 perm存储的是随机排列数列 2.2 rand_permutation函数 随机排列，参考随机排列算法博客

cwiseAbs()方法 逐元素取绝对值，返回一个新的同尺寸矩阵 maxCoeff()方法 找到矩阵中数值最大的那个元素，并返回该值 topRightCorner(m,n)方法 取矩阵的右上角子矩阵，大小为m行×n列 bottomRightCorner(m,n)方法 取矩阵的右下角子矩阵，大小为m行×n列 colwise().操作()方法 对矩阵的每一列单独执行同一个操作 normalize()/normalized()方法 对向量归一化，normalize()修改向量本身；normalized()返回新向量，不修改原向量本身 head(d)方法 取向量的前d个元素，引用取出，可修改原值

1. 低维线性规划 1.1 问题描述 最大化（最小化）目标函数 f(x1,x2⋯xd)=c1x1+c2x2+⋯+cdxdf(x_1,x_2 \cdots x_d) = c_1 x_1 + c_2 x_2 + \cdots + c_d x_d f(x1​,x2​⋯xd​)=c1​x1​+c2​x2​+⋯+cd​xd​ 需满足的约束为 a1,1x1+⋯+a1,dxd≤b1a2,1x1+⋯+a2,dxd≤b2⋮⋮⋮an,1x1+⋯+an,dxd≤bna_{1,1}x_1 + \cdots + a_{1,d}x_d \le b_1 \\ a_{2,1}x_1 + \cdots + a_{2,d}x_d \le b_2 \\ \vdots \quad\quad\quad\quad \vdots \quad\quad\quad \vdots \\ a_{n,1}x_1 + \cdots + a_{n,d}x_d \le b_n a1,1​x1​+⋯+a1,d​xd​≤b1​a2,1​x1​+⋯+a2,d​xd​≤b2​⋮⋮⋮an,1​x1​+⋯+an,d​xd​≤bn​ 此线性规划的维度为...

1. 代码结构 1.1 lidar_model 这里的体素索引依靠的是水平角度索引+垂直角度索引，二者的分辨率都是按每2度递增。其中垂直角度索引可以看成行索引，水平角度索引可以看成列索引 水平角度是以map坐标系下的x方向为零，在代码中将-x方向为索引的开始，逆时针为正；垂直角度是以map坐标系下的z方向为零，但是此方向一般为激光雷达的盲区，在代码中将视场角的上边作为索引的开始，通过kVerticalAngleOffset实现偏移 GetHorizontalNeighborNum和GetVerticalNeighborNum像是经验公式，即射线的邻居数量与距离成反比（距离越远，点云越稀疏）与分辨率成反比（分辨率越大，体素越稀疏） 1.2 viewpoint 定义了ViewPoint类用来表示视点，每一个视点中都有一个lidar_model，用来存储这个视点观测到的体素距离 1.2 grid 维护了一组栅格，并实现了线性索引、向量索引、维度下标索引和坐标值间的互相转换 1.3 rolling_grid 实现了3D滚动网格的功能，只维护机器人周围一个固定大小的局部网格。注意这里的网格...

1. 前言 光线投射算法(Ray Casting)是指在一个空间体素模型中，已知起点和终点，求光线从起点到终点经过的所有体素 此算法与直线绘制算法的不同之处在于：一个是寻找点，一个是寻找体素。因此当直线穿过对角点时，直线绘制算法直接加入这个点就行了；光线投射算法需要把对角线周围的体素加入进去 2. 快速体素遍历算法 2.1 思路 已知起点P0P_0P0​和终点PnP_nPn​，可以分别计算两点距离在x轴方向和y轴方向的投影Δx,Δy\Delta x,\Delta yΔx,Δy。可以把每个栅格的边长设为1(栅格索引以1为单位进行移动)，则移动一个格子时的比例（或者称为时间）为tΔx=1/Δxt_{\Delta x}=1/\Delta xtΔx​=1/Δx或tΔy=1/Δyt_{\Delta y}=1/\Delta ytΔy​=1/Δy。分别累积计算水平方向穿过网格边和竖直方向穿过网格边的ttt值，记为txt_{x}tx​和tyt_{y}ty​ 如果tx<tyt_{x} < t_{y}tx​<ty​，那么光线先穿过垂直边界，那么向x方向移动一步 如果tx≥t...

1. 前言 数学上直线是连续的，由无穷多点构成；但在计算机显示中，屏幕是离散的像素网格，只能用整数坐标的像素点去逼近连续直线。因此需要算法来确定哪些整数像素点最能代表这条直线，这就是直线绘制算法的核心作用 算法的输入是起点和终点的坐标，算法的输出是两点连成的直线经过的像素网格坐标序列 2. DDA算法 全称为数字差分分析器(Digital Differential Analyzer)算法 2.1 思路 直线的斜截式方程微分形式为 Δy=kΔx\Delta y = k \Delta x Δy=kΔx 那么有 {xi+1=xi+Δxyi+1=yi+Δy=yi+kΔx\left\{ \begin{align*} & x_{i+1} = x_i + \Delta x \\ & y_{i+1} = y_i + \Delta y = y_i + k \Delta x \end{align*} \right. {​xi+1​=xi​+Δxyi+1​=yi​+Δy=yi​+kΔx​ 当斜率为0≤k<10 \leq k <10≤k<1，且Δx\Delta xΔx...

1. 代码依赖 volumetric_mapping：项目地址 minkindr：项目地址 minkindr_ros：项目地址 2. 代码结构 2.1 dsvplanner包 2.1.1 drrtp_node 入口文件为，此文件会启动drrtp 2.1.2 exploration 订阅： /state_estimation里程计信息 /navigation_boundary整个算法的边界信息 客户端： /drrtPlannerSrv发送请求规划时间点 /cleanFrontierSrv 2.1.3 drrtp 订阅： /state_estimation里程计信息 /navigation_boundary整个算法的边界信息 服务端： /drrtPlannerSrv返回一系列目标点以及模式（探索或返航） /cleanFrontierSrv 2.1.4 dual_state_froniter 获取备选边界点 自动执行的操作 定时器： getFrontiers()维护局部和全局边界点 关键函数解析： getFrontiers() flowchart TB subg...

1. 异步通信 1.1 异步变量 promise： promise提供存储异步通信的值 , 再通过其对象创建的future异步获得结果 promise创建的一个对象只能用一次set_value设置传递值；也只能用一次get_future()获取future对象 创建子线程时需要使用move将promise对象传给子线程 future： future对象的get()方法会阻塞等待promise::set_value()的值 代码示例： #include <iostream> #include <future> #include <thread> using namespace std; void TestFuture(promise<string> p) { p.set_value("TestFuture value\n"); } int main(int argc, char* argv[]) { promise<string> p;/*异步传输变量存储*/ auto f...