-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathatom.xml
533 lines (249 loc) · 224 KB
/
atom.xml
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
<?xml version="1.0" encoding="utf-8"?>
<feed xmlns="http://www.w3.org/2005/Atom">
<title>某飞行员的随笔</title>
<link href="https://peitianyu.github.io/blog/atom.xml" rel="self"/>
<link href="https://peitianyu.github.io/blog/"/>
<updated>2022-11-30T05:50:55.636Z</updated>
<id>https://peitianyu.github.io/blog/</id>
<author>
<name>某飞行员</name>
</author>
<generator uri="https://hexo.io/">Hexo</generator>
<entry>
<title>西瓜书学习</title>
<link href="https://peitianyu.github.io/blog/2022/11/18/2022-11-18-%E8%A5%BF%E7%93%9C%E4%B9%A6%E5%AD%A6%E4%B9%A0/"/>
<id>https://peitianyu.github.io/blog/2022/11/18/2022-11-18-%E8%A5%BF%E7%93%9C%E4%B9%A6%E5%AD%A6%E4%B9%A0/</id>
<published>2022-11-18T04:30:12.000Z</published>
<updated>2022-11-30T05:50:55.636Z</updated>
<content type="html"><![CDATA[<h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址"></a>参考网址</h1><p><a href="https://www.bilibili.com/video/BV1Mh411e7VU?p=2&vd_source=745fd1b1f3e42bb544237f6d0bf78bb2">第3章-一元线性回归_哔哩哔哩_bilibili</a></p><h1 id="一元线性回归"><a href="#一元线性回归" class="headerlink" title="一元线性回归"></a>一元线性回归</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br></pre></td><td class="code"><pre><span class="line">算法原理</span><br><span class="line">线性回归与极大似然估计</span><br><span class="line">求解w与b</span><br><span class="line">凸函数优化(优化角度定义)</span><br></pre></td></tr></table></figure><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">线性回归损失函数: E(w, b) = Σ(yi - f(xi))^2</span><br><span class="line">极大似然估计: L(theta) = ⫪P(xi; theta) </span><br></pre></td></tr></table></figure><h2 id="例题"><a href="#例题" class="headerlink" title="例题"></a>例题</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">样本符合正态分布 X ~ N(u, sigma^2)</span><br><span class="line"></span><br><span class="line">总体思路是,先假设为一个一元线性回归, y = wx + b + sigma,然后由于符合正态分布,转化为对数函数,然后求换成一下形式</span><br></pre></td></tr></table></figure><p><a href="https://imgse.com/i/znrPNn"><img src="https://s1.ax1x.com/2022/11/18/znrPNn.png" alt="znrPNn.png"></a></p><p><a href="https://imgse.com/i/znrdUA"><img src="https://s1.ax1x.com/2022/11/18/znrdUA.png" alt="znrdUA.png"></a></p><h2 id="机器学习三要素"><a href="#机器学习三要素" class="headerlink" title="机器学习三要素"></a>机器学习三要素</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">模型: 根据具体问题, 确定假设空间</span><br><span class="line">策略: 根据评价标准, 确定最优策略(通常会产生一个损失函数)</span><br><span class="line">算法: 求解损失函数, 确定最优模型</span><br></pre></td></tr></table></figure><h1 id="多元线性回归"><a href="#多元线性回归" class="headerlink" title="多元线性回归"></a>多元线性回归</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">由最小二乘求解损失函数Ew</span><br><span class="line">求解w</span><br></pre></td></tr></table></figure><h1 id="对数几率"><a href="#对数几率" class="headerlink" title="对数几率"></a>对数几率</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">算法原理</span><br><span class="line">损失函数极大似然估计推导</span><br><span class="line">损失函数信息论推导</span><br></pre></td></tr></table></figure><h2 id="信息论概念"><a href="#信息论概念" class="headerlink" title="信息论概念"></a>信息论概念</h2><h2 id="相对熵-描述两个分布的差异"><a href="#相对熵-描述两个分布的差异" class="headerlink" title="相对熵(描述两个分布的差异)"></a>相对熵(描述两个分布的差异)</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line">最小化相对熵的方式达到最接近模拟分布, 最小化型对上等价于最小化交叉熵</span><br></pre></td></tr></table></figure><h1 id="决策树"><a href="#决策树" class="headerlink" title="决策树"></a>决策树</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br></pre></td><td class="code"><pre><span class="line">算法原理</span><br><span class="line">ID3决策树</span><br><span class="line">C4.5决策树</span><br><span class="line">CART决策树</span><br></pre></td></tr></table></figure><h2 id="原理"><a href="#原理" class="headerlink" title="原理"></a>原理</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">从逻辑角度一堆的if else语句组合</span><br><span class="line">从几何角度: 根据魔种规则划分特征空间</span><br><span class="line">最终目的: 将样本越分越纯</span><br></pre></td></tr></table></figure><h2 id="ID3决策树"><a href="#ID3决策树" class="headerlink" title="ID3决策树"></a>ID3决策树</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br></pre></td><td class="code"><pre><span class="line">通过信息熵来表现其样本纯度,主要思路是,由于条件越来越多,对于它的描述越来越清晰,也就越来越纯,也就是信息熵最小</span><br><span class="line">H(X) = -Σp(x)logbp(x) 越大表示越不确定</span><br><span class="line">信息熵</span><br><span class="line">Ent(D) = -Σpklog2pk</span><br><span class="line">条件熵</span><br><span class="line">Σ|Dv|/|D| * Ent(Dv) </span><br><span class="line">信息增益</span><br><span class="line">Gain = Ent(D) - Σ|Dv|/|D| * Ent(Dv) </span><br><span class="line">以信息增益为准则来划分属性的决策树</span><br></pre></td></tr></table></figure><h2 id="C4-5对ID3的优化"><a href="#C4-5对ID3的优化" class="headerlink" title="C4.5对ID3的优化"></a>C4.5对ID3的优化</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br></pre></td><td class="code"><pre><span class="line">使用ID3时采样样本太少,取值数目过多,会导致过拟合</span><br><span class="line"></span><br><span class="line"></span><br><span class="line">类似使用增益率代替信息增益</span><br><span class="line">Gain_ratio(D, a) = Gain(D, a) / IV(a)</span><br><span class="line"></span><br><span class="line">IV(a) = -Σ|Dv|/|D| * log2|Dv|/|D|</span><br></pre></td></tr></table></figure><h2 id="CART"><a href="#CART" class="headerlink" title="CART"></a>CART</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line">基尼值越小, 遇到异类的概率越小, 纯度越高</span><br></pre></td></tr></table></figure><h1 id="神经网络"><a href="#神经网络" class="headerlink" title="神经网络"></a>神经网络</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br></pre></td><td class="code"><pre><span class="line">M-P神经元</span><br><span class="line">输入n个输入加权和,进行比较,经过激活函数(阶跃函数, sigmoid),得到输出</span><br><span class="line">y = f(wTx + b)</span><br><span class="line">感知机(sgn作为激活函数)</span><br><span class="line">神经网络(多个神经元神经网络)</span><br></pre></td></tr></table></figure><h2 id="感知机"><a href="#感知机" class="headerlink" title="感知机"></a>感知机</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br></pre></td><td class="code"><pre><span class="line">y = sgn(wTx + b)</span><br><span class="line">构建一个超平面,也就是说只可以求解线性可分数据集</span><br><span class="line"></span><br><span class="line">法相空间,法向正空间与负空间,也就是说对应slam地图中我们可以将地图分为墙外空闲区域与机器人无法直接到达区域</span><br><span class="line"></span><br><span class="line"></span><br><span class="line">问题在于,搭建单层网络指挥导致只可以区分线性可分数据集</span><br></pre></td></tr></table></figure><h2 id="神经网络-1"><a href="#神经网络-1" class="headerlink" title="神经网络"></a>神经网络</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br></pre></td><td class="code"><pre><span class="line">本质是通过多层y=f(x)网络, 拟合任意线性不可分数据集</span><br><span class="line"></span><br><span class="line">问题:</span><br><span class="line">1. 多深多宽</span><br><span class="line">2. 结构怎样设计</span><br><span class="line">3. 输出结果如何解释, 一般只有直觉解释</span><br><span class="line"></span><br><span class="line">多层前馈网络:</span><br><span class="line">每层于下一层全互连,神经元不存在本层互连</span><br><span class="line"></span><br><span class="line">提取有用特征,向全自动分析迈步</span><br><span class="line"></span><br><span class="line">BP算法: 通过梯度下降求局部最小</span><br></pre></td></tr></table></figure><h1 id="支持向量机"><a href="#支持向量机" class="headerlink" title="支持向量机"></a>支持向量机</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br></pre></td><td class="code"><pre><span class="line">算法原理</span><br><span class="line">:线性可分数据集, 找距离正负样本都最远的超平面,相对于感知机解唯一,且不偏不倚,泛化性能更好</span><br><span class="line">超平面:</span><br><span class="line">: wTx + b = 0 (w为法向量, b为平移向量)</span><br><span class="line">几何间隔</span><br><span class="line">: r = yi(wTxi + b)/||w||</span><br><span class="line">数据集几何间隔: 所有几何间隔最小值</span><br><span class="line">支持向量机</span><br><span class="line">: y = sign(wTx + b)</span><br><span class="line"></span><br><span class="line">*注意学习凸优化问题: 拉格朗日乘子法</span><br></pre></td></tr></table></figure><h1 id="软间隔与支持向量机回归"><a href="#软间隔与支持向量机回归" class="headerlink" title="软间隔与支持向量机回归"></a>软间隔与支持向量机回归</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br></pre></td><td class="code"><pre><span class="line">算法原理</span><br><span class="line">: 允许部分样本犯错,弹药尽可能少</span><br><span class="line">软间隔</span><br><span class="line">: min 0.5||w||^2 + CΣl/1(0/1)(yi(wTxi + b) - 1)</span><br><span class="line">支持向量机回归</span><br><span class="line">: 找到一个超平面使所有点到其距离最小</span><br></pre></td></tr></table></figure><h1 id="贝叶斯分类器"><a href="#贝叶斯分类器" class="headerlink" title="贝叶斯分类器"></a>贝叶斯分类器</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br></pre></td><td class="code"><pre><span class="line">贝叶斯决策论</span><br><span class="line">: 在概率框架下实施决策(概率与误判损失) 条件风险 总体条件风险最小</span><br><span class="line">: h*(x) = argminR(c|x) h*为贝叶斯最优分类器</span><br><span class="line">: 最大化后验概率 = 最小化条件风险</span><br><span class="line">生成式模型和判别式模型</span><br><span class="line">朴素贝叶斯分类器</span><br><span class="line">: 假设特征相互独立</span><br><span class="line">: 实际对应slam的贝叶斯滤波,关注先验概率,最大似然得到最大后验概率</span><br><span class="line">半朴素贝叶斯分类器</span><br></pre></td></tr></table></figure><h1 id="集成学习"><a href="#集成学习" class="headerlink" title="集成学习"></a>集成学习</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br></pre></td><td class="code"><pre><span class="line">个体与集成</span><br><span class="line">: 通过多个个体学习器预测进行融合, 使用个体学习器众数作为结果,返回</span><br><span class="line">: 君子和而不同</span><br><span class="line">: 个体学习器数量越大越好,个体学习器误差不等于0.5</span><br><span class="line">Adaboost算法</span><br><span class="line">: Hx = Σa*h</span><br><span class="line">: 目的是为了增量式优化各个学习器权重a,最终优化所有权重,有点类似于贝叶斯滤波,加权传播的形式</span><br><span class="line">: 二分类的</span><br><span class="line">Bagging </span><br><span class="line">随机森林</span><br><span class="line">多样性增强</span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址"></a>参考网址</h1><p><a href="https://www.bilibili.com/video/BV1Mh411e7VU?p=2&vd_so</summary>
<category term="机器学习" scheme="https://peitianyu.github.io/blog/categories/%E6%9C%BA%E5%99%A8%E5%AD%A6%E4%B9%A0/"/>
<category term="机器学习" scheme="https://peitianyu.github.io/blog/tags/%E6%9C%BA%E5%99%A8%E5%AD%A6%E4%B9%A0/"/>
</entry>
<entry>
<title>2D激光slam数据集的下载与测试</title>
<link href="https://peitianyu.github.io/blog/2022/11/11/2022-11-11-2D%E6%BF%80%E5%85%89slam%E6%95%B0%E6%8D%AE%E9%9B%86%E7%9A%84%E4%B8%8B%E8%BD%BD%E4%B8%8E%E6%B5%8B%E8%AF%95/"/>
<id>https://peitianyu.github.io/blog/2022/11/11/2022-11-11-2D%E6%BF%80%E5%85%89slam%E6%95%B0%E6%8D%AE%E9%9B%86%E7%9A%84%E4%B8%8B%E8%BD%BD%E4%B8%8E%E6%B5%8B%E8%AF%95/</id>
<published>2022-11-11T04:30:12.000Z</published>
<updated>2022-11-30T05:45:26.816Z</updated>
<content type="html"><![CDATA[<h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址"></a>参考网址</h1><p><a href="https://blog.csdn.net/qq_21830903/article/details/111048937">(144条消息) 2D激光slam数据集的下载与测试_快乐飞奔的小菜鸡的博客-CSDN博客_2d slam数据集</a></p><p><a href="https://blog.csdn.net/ld1461575230/article/details/124093307?spm=1001.2101.3001.6650.5&utm_medium=distribute.pc_relevant.none-task-blog-2~default~CTRLIST~Rate-5-124093307-blog-107925921.pc_relevant_multi_platform_whitelistv6&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2~default~CTRLIST~Rate-5-124093307-blog-107925921.pc_relevant_multi_platform_whitelistv6&utm_relevant_index=6">(144条消息) 二维激光雷达SLAM数据集_哈哈哈的嘎嘎嘎的博客-CSDN博客_二维激光雷达</a></p><h1 id="代码"><a href="#代码" class="headerlink" title="代码"></a>代码</h1><figure class="highlight python"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br><span class="line">25</span><br><span class="line">26</span><br><span class="line">27</span><br><span class="line">28</span><br><span class="line">29</span><br><span class="line">30</span><br><span class="line">31</span><br><span class="line">32</span><br><span class="line">33</span><br><span class="line">34</span><br><span class="line">35</span><br><span class="line">36</span><br><span class="line">37</span><br><span class="line">38</span><br><span class="line">39</span><br><span class="line">40</span><br><span class="line">41</span><br><span class="line">42</span><br><span class="line">43</span><br><span class="line">44</span><br><span class="line">45</span><br><span class="line">46</span><br><span class="line">47</span><br><span class="line">48</span><br><span class="line">49</span><br><span class="line">50</span><br><span class="line">51</span><br><span class="line">52</span><br><span class="line">53</span><br><span class="line">54</span><br><span class="line">55</span><br><span class="line">56</span><br><span class="line">57</span><br><span class="line">58</span><br><span class="line">59</span><br><span class="line">60</span><br><span class="line">61</span><br><span class="line">62</span><br><span class="line">63</span><br><span class="line">64</span><br><span class="line">65</span><br><span class="line">66</span><br><span class="line">67</span><br><span class="line">68</span><br><span class="line">69</span><br><span class="line">70</span><br><span class="line">71</span><br><span class="line">72</span><br><span class="line">73</span><br><span class="line">74</span><br><span class="line">75</span><br><span class="line">76</span><br><span class="line">77</span><br><span class="line">78</span><br><span class="line">79</span><br><span class="line">80</span><br><span class="line">81</span><br><span class="line">82</span><br><span class="line">83</span><br><span class="line">84</span><br><span class="line">85</span><br><span class="line">86</span><br></pre></td><td class="code"><pre><span class="line"><span class="comment"># convert.py</span></span><br><span class="line"><span class="comment">#!/usr/bin/env python</span></span><br><span class="line"><span class="comment">#coding=utf8</span></span><br><span class="line"></span><br><span class="line"></span><br><span class="line"><span class="string">'''This is a converter for the Intel Research Lab SLAM dataset</span></span><br><span class="line"><span class="string"> ( http://kaspar.informatik.uni-freiburg.de/~slamEvaluation/datasets/intel.clf )</span></span><br><span class="line"><span class="string"> to rosbag'''</span></span><br><span class="line"></span><br><span class="line"><span class="keyword">import</span> rospy</span><br><span class="line"><span class="keyword">import</span> rosbag</span><br><span class="line"><span class="keyword">from</span> sensor_msgs.msg <span class="keyword">import</span> LaserScan</span><br><span class="line"><span class="keyword">from</span> nav_msgs.msg <span class="keyword">import</span> Odometry</span><br><span class="line"><span class="keyword">from</span> math <span class="keyword">import</span> pi</span><br><span class="line"><span class="keyword">from</span> tf2_msgs.msg <span class="keyword">import</span> TFMessage</span><br><span class="line"><span class="keyword">from</span> geometry_msgs.msg <span class="keyword">import</span> TransformStamped</span><br><span class="line"><span class="keyword">import</span> tf</span><br><span class="line"><span class="keyword">import</span> sys</span><br><span class="line"><span class="keyword">import</span> logging</span><br><span class="line">logging.basicConfig()</span><br><span class="line"></span><br><span class="line"><span class="function"><span class="keyword">def</span> <span class="title">make_tf_msg</span>(<span class="params">x, y, theta, t,base,base0</span>):</span></span><br><span class="line"> trans = TransformStamped()</span><br><span class="line"> trans.header.stamp = t</span><br><span class="line"> trans.header.frame_id = base</span><br><span class="line"> trans.child_frame_id = base0</span><br><span class="line"> trans.transform.translation.x = x</span><br><span class="line"> trans.transform.translation.y = y</span><br><span class="line"> q = tf.transformations.quaternion_from_euler(<span class="number">0</span>, <span class="number">0</span>, theta)</span><br><span class="line"> trans.transform.rotation.x = q[<span class="number">0</span>]</span><br><span class="line"> trans.transform.rotation.y = q[<span class="number">1</span>]</span><br><span class="line"> trans.transform.rotation.z = q[<span class="number">2</span>]</span><br><span class="line"> trans.transform.rotation.w = q[<span class="number">3</span>]</span><br><span class="line"></span><br><span class="line"> msg = TFMessage()</span><br><span class="line"> msg.transforms.append(trans)</span><br><span class="line"> <span class="keyword">return</span> msg</span><br><span class="line"><span class="keyword">if</span> __name__ == <span class="string">"__main__"</span>:</span><br><span class="line"> <span class="keyword">if</span> <span class="built_in">len</span>(sys.argv) < <span class="number">3</span>:</span><br><span class="line"> <span class="built_in">print</span> <span class="string">"请输入dataset文件名。"</span> </span><br><span class="line"> exit()</span><br><span class="line"> <span class="built_in">print</span> <span class="string">"正在处理"</span> + sys.argv[<span class="number">1</span>] + <span class="string">"..."</span></span><br><span class="line"> <span class="keyword">with</span> <span class="built_in">open</span>(sys.argv[<span class="number">1</span>]) <span class="keyword">as</span> dataset:</span><br><span class="line"> <span class="keyword">with</span> rosbag.Bag(sys.argv[<span class="number">2</span>], <span class="string">'w'</span>) <span class="keyword">as</span> bag:</span><br><span class="line"> i = <span class="number">1</span></span><br><span class="line"> <span class="keyword">for</span> line <span class="keyword">in</span> dataset.readlines():</span><br><span class="line"> line = line.strip()</span><br><span class="line"> tokens = line.split(<span class="string">' '</span>)</span><br><span class="line"> <span class="keyword">if</span> <span class="built_in">len</span>(tokens) <= <span class="number">2</span>:</span><br><span class="line"> <span class="keyword">continue</span></span><br><span class="line"> <span class="keyword">if</span> tokens[<span class="number">0</span>] == <span class="string">'FLASER'</span>:</span><br><span class="line"> msg = LaserScan()</span><br><span class="line"> num_scans = <span class="built_in">int</span>(tokens[<span class="number">1</span>])</span><br><span class="line"></span><br><span class="line"> <span class="keyword">if</span> num_scans != <span class="number">180</span> <span class="keyword">or</span> <span class="built_in">len</span>(tokens) < num_scans + <span class="number">9</span>:</span><br><span class="line"> <span class="comment">#rospy.logwarn("unsupported scan format")</span></span><br><span class="line"> <span class="keyword">continue</span></span><br><span class="line"></span><br><span class="line"> msg.header.frame_id = <span class="string">'base_laser_link'</span></span><br><span class="line"> t = rospy.Time(<span class="built_in">float</span>(tokens[(num_scans + <span class="number">8</span>)]))</span><br><span class="line"> msg.header.stamp = t</span><br><span class="line"> msg.header.seq = i</span><br><span class="line"> i += <span class="number">1</span></span><br><span class="line"> msg.angle_min = -<span class="number">90.0</span> / <span class="number">180.0</span> * pi</span><br><span class="line"> msg.angle_max = <span class="number">90.0</span> / <span class="number">180.0</span> * pi</span><br><span class="line"> msg.angle_increment = pi / num_scans</span><br><span class="line"> msg.time_increment = <span class="number">0.2</span> / <span class="number">360.0</span></span><br><span class="line"> msg.scan_time = <span class="number">0.2</span></span><br><span class="line"> msg.range_min = <span class="number">0.001</span></span><br><span class="line"> msg.range_max = <span class="number">50.0</span></span><br><span class="line"> msg.ranges = [<span class="built_in">float</span>(r) <span class="keyword">for</span> r <span class="keyword">in</span> tokens[<span class="number">2</span>:(num_scans + <span class="number">2</span>)]]</span><br><span class="line"> msg.ranges.append(<span class="built_in">float</span>(<span class="string">'inf'</span>)) <span class="comment">#我修改了这</span></span><br><span class="line"></span><br><span class="line"> bag.write(<span class="string">'scan'</span>, msg, t)</span><br><span class="line"></span><br><span class="line"> odom_x, odom_y, odom_theta = [<span class="built_in">float</span>(r) <span class="keyword">for</span> r <span class="keyword">in</span> tokens[(num_scans + <span class="number">2</span>):(num_scans + <span class="number">5</span>)]]</span><br><span class="line"> tf_msg = make_tf_msg(odom_x, odom_y, odom_theta, t,<span class="string">'odom'</span>,<span class="string">'base_link'</span>)</span><br><span class="line"> bag.write(<span class="string">'tf'</span>, tf_msg, t)</span><br><span class="line"></span><br><span class="line"> <span class="keyword">elif</span> tokens[<span class="number">0</span>] == <span class="string">'ODOM'</span>:</span><br><span class="line"> odom_x, odom_y, odom_theta = [<span class="built_in">float</span>(t) <span class="keyword">for</span> t <span class="keyword">in</span> tokens[<span class="number">1</span>:<span class="number">4</span>]]</span><br><span class="line"> t = rospy.Time(<span class="built_in">float</span>(tokens[<span class="number">7</span>]))</span><br><span class="line"> tf_msg = make_tf_msg(<span class="number">0</span>, <span class="number">0</span>, <span class="number">0</span>, t,<span class="string">'base_link'</span>,<span class="string">'base_laser_link'</span>)</span><br><span class="line"> bag.write(<span class="string">'tf'</span>, tf_msg, t)</span><br><span class="line"></span><br><span class="line"></span><br></pre></td></tr></table></figure><h1 id="使用"><a href="#使用" class="headerlink" title="使用"></a>使用</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">cd script //进入文件夹</span><br><span class="line">python convert.py ../clf/ACES.clf ../clf/ACES.bag //可修改路径调整自己的文件位置</span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址"></a>参考网址</h1><p><a href="https://blog.csdn.net/qq_21830903/article/details/111</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="常用工具" scheme="https://peitianyu.github.io/blog/categories/slam/%E5%B8%B8%E7%94%A8%E5%B7%A5%E5%85%B7/"/>
<category term="slam学习笔记" scheme="https://peitianyu.github.io/blog/tags/slam%E5%AD%A6%E4%B9%A0%E7%AC%94%E8%AE%B0/"/>
</entry>
<entry>
<title>因子图优化</title>
<link href="https://peitianyu.github.io/blog/2022/11/11/%E5%9B%A0%E5%AD%90%E5%9B%BE%E4%BC%98%E5%8C%96/"/>
<id>https://peitianyu.github.io/blog/2022/11/11/%E5%9B%A0%E5%AD%90%E5%9B%BE%E4%BC%98%E5%8C%96/</id>
<published>2022-11-11T04:30:12.000Z</published>
<updated>2022-11-30T05:47:16.236Z</updated>
<content type="html"><![CDATA[<h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址"></a>参考网址</h1><p> <a href="https://www.bilibili.com/video/BV1C4411772G?from=search&seid=522536518070545967&spm_id_from=333.337.0.0&vd_source=745fd1b1f3e42bb544237f6d0bf78bb2">【泡泡机器人公开课】第五十六课：gtsam_tutorial-董靖_哔哩哔哩_bilibili</a></p><p><a href="https://www.bilibili.com/video/BV1Y54y197VW">T5 - 图优化 (第一节课)_哔哩哔哩_bilibili</a></p><p><a href="https://zhuanlan.zhihu.com/p/106049035">战疫时期的算法课 - 知乎 (zhihu.com)</a></p><p><a href="https://blog.csdn.net/Darlingqiang/article/details/121947727?ops_request_misc=&request_id=&biz_id=102&utm_term=GTSAM%E8%AE%BA%E6%96%87&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-1-121947727.142%5Ev47%5Ebody_digest,201%5Ev3%5Eadd_ask&spm=1018.2226.3001.4187">(164条消息) 【GTSAM】GTSAM/iSAM1/2资源整理_guoqiang_sunshine的博客-CSDN博客_gtsam isam</a></p><p><a href="https://github.com/dongjing3309/minisam">dongjing3309/minisam: A general and flexible factor graph non-linear least square optimization framework (github.com)</a></p><h1 id="写一个简单的因子图库"><a href="#写一个简单的因子图库" class="headerlink" title="写一个简单的因子图库"></a>写一个简单的因子图库</h1><p><a href="https://github.com/peitianyu/factor_graph">peitianyu/factor_graph (github.com)</a></p><h1 id="简介"><a href="#简介" class="headerlink" title="简介"></a>简介</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br></pre></td><td class="code"><pre><span class="line">此程序主要学习了mini_sam思路,讲真mini_sam库真心清晰,明了,干净.</span><br><span class="line">程序包含</span><br><span class="line">Core:主函数在graph_optimize进行优化,然后通过sparsity_pattern保存稀疏矩阵缓存,variable与factor构造虚函数</span><br><span class="line">types: 构造二维空间下的variable与factor</span><br><span class="line">slam:简单的写了几个在二维下做的因子图</span><br><span class="line">test: 测试主函数</span><br></pre></td></tr></table></figure><h1 id="架构"><a href="#架构" class="headerlink" title="架构"></a>架构</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br></pre></td><td class="code"><pre><span class="line">variable: 相当于图优化里边的顶点</span><br><span class="line">factor: 通过variable构造成一元边或者二元边, 并且将求解Jacobian也放在此类中(用的是数值求解雅各比)</span><br><span class="line">factor_graph: 将构造好的variable与factor添加入因子图,由于之后的优化</span><br><span class="line">graph_optimize: 优化构造好的因子图,这里由于稀疏化技巧所以分出一个sparsity_pattern稀疏缓存器模块</span><br><span class="line">sparsity_pattern: 包含两个部分一个是variable搜索引导一个是H与b稀疏矩阵缓存</span><br></pre></td></tr></table></figure><h1 id="测试"><a href="#测试" class="headerlink" title="测试"></a>测试</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br></pre></td><td class="code"><pre><span class="line">主要测试了:</span><br><span class="line">1. 纯2d_point的优化</span><br><span class="line">2. 2d环境下point与pose的优化(比如, 反光板与里程计)</span><br><span class="line">3. 纯2d_pose的优化,(主要展示了读取g2o文件,并优化的效果)</span><br></pre></td></tr></table></figure><h1 id="BUG"><a href="#BUG" class="headerlink" title="BUG"></a>BUG</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line">1. 加入sqr_info后发现优化速度明显变慢,没找的什么原因</span><br></pre></td></tr></table></figure><h1 id="TODO"><a href="#TODO" class="headerlink" title="TODO"></a>TODO</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">1. 边缘化操作</span><br><span class="line">2. 如果H并不是稀疏矩阵,通过近似最小度排序变为稀疏矩阵</span><br></pre></td></tr></table></figure><h1 id="Build"><a href="#Build" class="headerlink" title="Build"></a>Build</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br></pre></td><td class="code"><pre><span class="line">cd factor_graph_optimize && mkdir build</span><br><span class="line">cd build && cmake ..</span><br><span class="line">make -j</span><br><span class="line">./g2o_slam2d</span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址"></a>参考网址</h1><p> <a href="https://www.bilibili.com/video/BV1C4411772G?from=sea</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="理论学习" scheme="https://peitianyu.github.io/blog/categories/slam/%E7%90%86%E8%AE%BA%E5%AD%A6%E4%B9%A0/"/>
<category term="slam" scheme="https://peitianyu.github.io/blog/tags/slam/"/>
</entry>
<entry>
<title>ransac学习</title>
<link href="https://peitianyu.github.io/blog/2022/11/07/2022-11-7-ransac%E5%AD%A6%E4%B9%A0/"/>
<id>https://peitianyu.github.io/blog/2022/11/07/2022-11-7-ransac%E5%AD%A6%E4%B9%A0/</id>
<published>2022-11-07T04:30:12.000Z</published>
<updated>2022-11-30T05:47:10.005Z</updated>
<content type="html"><![CDATA[<h1 id="Ransac"><a href="#Ransac" class="headerlink" title="Ransac"></a>Ransac</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">流程:</span><br><span class="line">在所有point中选择一组计算line参数,通过设置k_min与k_max保留局内点,</span><br><span class="line">下一轮中重新选点,若得分更高则认为该模型更好,如此循环直到达到设置阈值或者最大迭代</span><br></pre></td></tr></table></figure><figure class="highlight cc"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br><span class="line">25</span><br><span class="line">26</span><br><span class="line">27</span><br><span class="line">28</span><br><span class="line">29</span><br><span class="line">30</span><br><span class="line">31</span><br><span class="line">32</span><br><span class="line">33</span><br><span class="line">34</span><br><span class="line">35</span><br><span class="line">36</span><br><span class="line">37</span><br><span class="line">38</span><br><span class="line">39</span><br><span class="line">40</span><br><span class="line">41</span><br><span class="line">42</span><br><span class="line">43</span><br><span class="line">44</span><br><span class="line">45</span><br><span class="line">46</span><br><span class="line">47</span><br><span class="line">48</span><br><span class="line">49</span><br><span class="line">50</span><br><span class="line">51</span><br><span class="line">52</span><br><span class="line">53</span><br><span class="line">54</span><br><span class="line">55</span><br><span class="line">56</span><br><span class="line">57</span><br><span class="line">58</span><br><span class="line">59</span><br><span class="line">60</span><br><span class="line">61</span><br><span class="line">62</span><br></pre></td><td class="code"><pre><span class="line"><span class="comment">//RANSAC 拟合2D 直线</span></span><br><span class="line"><span class="comment">//输入参数：points--输入点集</span></span><br><span class="line"><span class="comment">// iterations--迭代次数</span></span><br><span class="line"><span class="comment">// sigma--数据和模型之间可接受的差值,车道线像素宽带一般为10左右</span></span><br><span class="line"><span class="comment">// （Parameter use to compute the fitting score）</span></span><br><span class="line"><span class="comment">// k_min/k_max--拟合的直线斜率的取值范围.</span></span><br><span class="line"><span class="comment">// 考虑到左右车道线在图像中的斜率位于一定范围内，</span></span><br><span class="line"><span class="comment">// 添加此参数，同时可以避免检测垂线和水平线</span></span><br><span class="line"><span class="comment">//输出参数:line--拟合的直线参数,It is a vector of 4 floats</span></span><br><span class="line"><span class="comment">// (vx, vy, x0, y0) where (vx, vy) is a normalized</span></span><br><span class="line"><span class="comment">// vector collinear to the line and (x0, y0) is some</span></span><br><span class="line"><span class="comment">// point on the line.</span></span><br><span class="line"><span class="comment">//返回值：无</span></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">fitLineRansac</span><span class="params">(<span class="keyword">const</span> std::vector<cv::Point2f>& points,</span></span></span><br><span class="line"><span class="params"><span class="function"> cv::Vec4f &line,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="keyword">int</span> iterations = <span class="number">1000</span>,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="keyword">double</span> sigma = <span class="number">1.</span>,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="keyword">double</span> k_min = <span class="number">-7.</span>,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="keyword">double</span> k_max = <span class="number">7.</span>)</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> <span class="keyword">unsigned</span> <span class="keyword">int</span> n = points.<span class="built_in">size</span>();</span><br><span class="line"></span><br><span class="line"> <span class="keyword">if</span>(n<<span class="number">2</span>)</span><br><span class="line"> {</span><br><span class="line"> <span class="keyword">return</span>;</span><br><span class="line"> }</span><br><span class="line"></span><br><span class="line"> cv::RNG rng;</span><br><span class="line"> <span class="keyword">double</span> bestScore = <span class="number">-1.</span>;</span><br><span class="line"> <span class="keyword">for</span>(<span class="keyword">int</span> k=<span class="number">0</span>; k<iterations; k++)</span><br><span class="line"> {</span><br><span class="line"> <span class="keyword">int</span> i1=<span class="number">0</span>, i2=<span class="number">0</span>;</span><br><span class="line"> <span class="keyword">while</span>(i1==i2)</span><br><span class="line"> {</span><br><span class="line"> i1 = <span class="built_in">rng</span>(n);</span><br><span class="line"> i2 = <span class="built_in">rng</span>(n);</span><br><span class="line"> }</span><br><span class="line"> <span class="keyword">const</span> cv::Point2f& p1 = points[i1];</span><br><span class="line"> <span class="keyword">const</span> cv::Point2f& p2 = points[i2];</span><br><span class="line"></span><br><span class="line"> cv::Point2f dp = p2-p1;<span class="comment">//直线的方向向量</span></span><br><span class="line"> dp *= <span class="number">1.</span>/<span class="built_in">norm</span>(dp);</span><br><span class="line"> <span class="keyword">double</span> score = <span class="number">0</span>;</span><br><span class="line"></span><br><span class="line"> <span class="keyword">if</span>(dp.y/dp.x<=k_max && dp.y/dp.x>=k_min )</span><br><span class="line"> {</span><br><span class="line"> <span class="keyword">for</span>(uint i=<span class="number">0</span>; i<n; i++)</span><br><span class="line"> {</span><br><span class="line"> cv::Point2f v = points[i]-p1;</span><br><span class="line"> <span class="keyword">double</span> d = v.y*dp.x - v.x*dp.y;<span class="comment">//向量a与b叉乘/向量b的摸.||b||=1./norm(dp),形成模越小说明匹配度越高</span></span><br><span class="line"> <span class="comment">//score += exp(-0.5*d*d/(sigma*sigma));//误差定义方式的一种</span></span><br><span class="line"> <span class="keyword">if</span>( <span class="built_in">fabs</span>(d)<sigma )</span><br><span class="line"> score += <span class="number">1</span>;</span><br><span class="line"> }</span><br><span class="line"> }</span><br><span class="line"> <span class="keyword">if</span>(score > bestScore)</span><br><span class="line"> {</span><br><span class="line"> line = cv::<span class="built_in">Vec4f</span>(dp.x, dp.y, p1.x, p1.y);</span><br><span class="line"> bestScore = score;</span><br><span class="line"> }</span><br><span class="line"> }</span><br><span class="line">}</span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="Ransac"><a href="#Ransac" class="headerlink" title="Ransac"></a>Ransac</h1><figure class="highlight plaintext"><table><tr><td class=</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="理论学习" scheme="https://peitianyu.github.io/blog/categories/slam/%E7%90%86%E8%AE%BA%E5%AD%A6%E4%B9%A0/"/>
<category term="slam学习笔记" scheme="https://peitianyu.github.io/blog/tags/slam%E5%AD%A6%E4%B9%A0%E7%AC%94%E8%AE%B0/"/>
</entry>
<entry>
<title>每日学习问答笔记</title>
<link href="https://peitianyu.github.io/blog/2022/11/04/2022-11-4-%E6%AF%8F%E6%97%A5%E5%AD%A6%E4%B9%A0%E9%97%AE%E7%AD%94%E7%AC%94%E8%AE%B0/"/>
<id>https://peitianyu.github.io/blog/2022/11/04/2022-11-4-%E6%AF%8F%E6%97%A5%E5%AD%A6%E4%B9%A0%E9%97%AE%E7%AD%94%E7%AC%94%E8%AE%B0/</id>
<published>2022-11-04T04:30:12.000Z</published>
<updated>2022-11-30T05:47:09.829Z</updated>
<content type="html"><![CDATA[<h1 id="每日学习笔记"><a href="#每日学习笔记" class="headerlink" title="每日学习笔记"></a>每日学习笔记</h1><h2 id="222-11-3"><a href="#222-11-3" class="headerlink" title="222-11-3"></a>222-11-3</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">Q: 关于slam重定位的讨论</span><br><span class="line">A: 使用粒子滤波时通过增加历史权重的方式获得一定scan的历史信息,当信息量达到一定程度时,开始做匹配重定位,最后留下一个pose作为最终定位pose,但实际上这种通过增加历史权重的方式获得历史信息的方式依旧会损失很多scan信息,所以可以通过scan叠加图后通过叠成更大图的方式最后进行匹配,尽可能保留scan数据</span><br></pre></td></tr></table></figure><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br></pre></td><td class="code"><pre><span class="line">问题起因: 当厂家给我们传感器误差时的意思实际上就是说他们的传感器可以近似拟合高斯分布</span><br><span class="line">Q1: 为什么可以拟合成高斯分布?</span><br><span class="line">A1: 因为没有办法,只有高斯分布可以在传播过程中依然保持高斯分布,这种特性是其他分布所没有的,只能如此,所以这也就导致了这样一个问题,我们虽然拟合成高斯分布,但实际上传感器误差不可能仅仅遵循高斯分布,因此可以短期相信高斯分布的结果,但不可能完全相信它</span><br><span class="line">Q2: 考虑amcl中观测模型求解权重的情况,为什么其似然域模型可以包含随机误差与最大误差的影响</span><br><span class="line">A2: 因为这里的权重模型仅考虑单次本轮内比较,并不存在与下一轮或者上一轮比较的情况,因此可以用</span><br><span class="line">Q3: 为什么似然域模型需要将p *= p; 换成 p += qz*qz*qz; ?</span><br><span class="line">A3: 因为发现原有公式算出来的差距过于大,对于计算机并不友好,所以换种方式降低这种差距</span><br></pre></td></tr></table></figure><h2 id="2022-11-4"><a href="#2022-11-4" class="headerlink" title="2022-11-4"></a>2022-11-4</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br></pre></td><td class="code"><pre><span class="line">Q:关于长走廊与隧道问题</span><br><span class="line">A:</span><br><span class="line">1. 其观测协方差矩阵并不会发生变化,原因由于,特征匹配出现的问题,拿icp举例,两个在长走廊上的icp匹配可能得到的d_pose非常小,原因实际上就是选取的特征点存在问题,比如将其换成带信息的二维码则不会存在这样的问题</span><br><span class="line">2. 为此可以判断处于长走廊,比如,通过法向量或者提线判断,最终决定于你对于长走廊的定义</span><br><span class="line">3. 为弥补这种特征退化造成的影响,可能需要其他传感器的辅助(odom, imu)</span><br></pre></td></tr></table></figure><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">Q: 关于法向量的思考</span><br><span class="line">A: 可以通过scan.points[i]左右两个点进行叠加选择计算我的切线方向(甚至与可以通过所有的点进行拟合),从而求出相对于我车体的法向量</span><br></pre></td></tr></table></figure><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">Q: 对于scantomap中map的理解</span><br><span class="line">A: 实际上就是所有scan的信息整合,不过这里的map相当于所有scan进行了一次降采样,一方面过滤掉了一部分scan坏的点另一部分也减少了总体scan的信息量</span><br></pre></td></tr></table></figure><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">Q: 简述说明icp过程?</span><br><span class="line">A: 1. 搜索最近点 2. 获得点对 3. 最小二乘迭代 4. 评估icp匹配质量(这里通过匹配点对/scan.size, 但实际上这只是一种无奈操作,我无法判断是80/100更好, 还是160/200更好)</span><br></pre></td></tr></table></figure><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br></pre></td><td class="code"><pre><span class="line">Q1: 简述ndt过程?</span><br><span class="line">A1: 1. 构建ndt栅格 2. 计算每一个ndt栅格左边与协方差 3. 通过协方差加权最小二乘迭代 4. 评估匹配质量 </span><br><span class="line">Q2: 思考ndt与icp区别?</span><br><span class="line">A2: ndt本质上是从icp衍生而来,只不过为了简化计算量,将前一个scan做了预处理,然后通过加权最小二乘实现迭代</span><br></pre></td></tr></table></figure><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">Q: 思考GPS,反光板,二维码与land_mark在图优化中的作用</span><br><span class="line">A: 提供绝对约束,或者一元边</span><br></pre></td></tr></table></figure><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">Q: 思考图优化中滑窗与边缘化</span><br><span class="line">A: 边缘化的概念是希望删除一些不需要的关键帧,但保留其约束,这样就可以与滑窗联系起来,通过滑窗将不需要的关键帧删除,这就势必舒尔补导致将原有的稀疏矩阵变为稠密矩阵,而vins的做法是保持信息矩阵维持在比较合理的尺寸</span><br></pre></td></tr></table></figure><h2 id="2022-11-5"><a href="#2022-11-5" class="headerlink" title="2022-11-5"></a>2022-11-5</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br></pre></td><td class="code"><pre><span class="line">Q: 关于重定位与slam关系思考</span><br><span class="line">A: 可以将slam想成前端重定位作为后端两个线程</span><br><span class="line">--------------->slam线程---------->重定位线程(类似于后端优化线程)------------------->ret(pose, map)</span><br><span class="line"> pose|map</span><br><span class="line">SmallRelocate|FastRelocate|DynamicRelocate|Mapping</span><br><span class="line">slam</span><br></pre></td></tr></table></figure><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br><span class="line">25</span><br><span class="line">26</span><br><span class="line">27</span><br><span class="line">28</span><br><span class="line">29</span><br></pre></td><td class="code"><pre><span class="line">Q: 多线程数据传输</span><br><span class="line">A: 可以通过跨线程类传输数据,比如</span><br><span class="line"></span><br><span class="line">class Map2d</span><br><span class="line">{</span><br><span class="line">public:</span><br><span class="line">GetCellValue()</span><br><span class="line">{</span><br><span class="line"> lock(m_lock);</span><br><span class="line"> // do something.....</span><br><span class="line"> unlock(m_lock);</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line">SetCellValue()</span><br><span class="line">{</span><br><span class="line">lock(m_lock);</span><br><span class="line"> // do something.....</span><br><span class="line"> unlock(m_lock);</span><br><span class="line">}</span><br><span class="line">private:</span><br><span class="line">mutex m_lock;</span><br><span class="line">};</span><br><span class="line"></span><br><span class="line">int main()</span><br><span class="line">{</span><br><span class="line">Map2D map;</span><br><span class="line">slam Thread(&map);</span><br><span class="line">Relocate Thread(&map);</span><br><span class="line">}</span><br></pre></td></tr></table></figure><h2 id="2022-11-9"><a href="#2022-11-9" class="headerlink" title="2022-11-9"></a>2022-11-9</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br></pre></td><td class="code"><pre><span class="line">Q1: 在做最小二乘时,error做损失函数的目的是?</span><br><span class="line">A1: 为了让训练模型的特征更容易达到我们所期望的效果,具体可参考:</span><br><span class="line">https://www.bilibili.com/video/BV1vg411172u/?spm_id_from=333.337.search-card.all.click&vd_source=745fd1b1f3e42bb544237f6d0bf78bb2</span><br><span class="line"></span><br><span class="line">Q2: g2o文件中信息矩阵为何要开方处理</span><br><span class="line">A2: 由于g2o默认为符合高斯分布,因此可以通过高斯分布的损失函数求解,这里的开方的目的是方便处理,具体参考</span><br><span class="line">https://github.com/alademm/micro-graph-optimizer</span><br><span class="line">https://github.com/dongjing3309/minisam</span><br></pre></td></tr></table></figure><h2 id="2022-11-10"><a href="#2022-11-10" class="headerlink" title="2022-11-10"></a>2022-11-10</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">Q: 对于vector等STL与智能指针来讲,对于内存消耗与运行速度思考</span><br><span class="line">A: 相较于最简单的数组,使用STL势必会占用更多的内存与时间,所有如果是非常频繁的使用尽量使用最简单表达</span><br></pre></td></tr></table></figure><h2 id="2022-11-11"><a href="#2022-11-11" class="headerlink" title="2022-11-11"></a>2022-11-11</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br></pre></td><td class="code"><pre><span class="line">Q1: 在进行QR稀疏矩阵求解或者Cholesky分解前由于矩阵为稠密矩阵,消耗非常大怎么办?</span><br><span class="line">A1: 先通过近似最小度排序将稠密矩阵转为稀疏矩阵.</span><br><span class="line"></span><br><span class="line">Q2: 为什么矩阵会变为稠密矩阵?</span><br><span class="line">A2: 因为在回环检测后如果依旧在原有的路径上行走,会出现很多回环点,导致矩阵越来越稠密.</span><br></pre></td></tr></table></figure><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br></pre></td><td class="code"><pre><span class="line">Q: Chou_Liu Tree理解?(可以理解为将稠密矩阵转化为稀疏矩阵的一种方式)</span><br><span class="line">A: 目的是将联合概率分布转化为二维分布连乘形式(近似): </span><br><span class="line">寻找一棵以互信息为权值、变量为节点的最大生成树作为依赖树</span><br><span class="line">参考: http://www.bewindoweb.com/232.html</span><br><span class="line"> https://zhuanlan.zhihu.com/p/437028454 </span><br><span class="line"> https://blog.csdn.net/qq_38023849/article/details/110306178?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166555644916782425127805%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=166555644916782425127805&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~rank_v31_ecpm-3-110306178-null-null.142^v53^control_1,201^v3^control&utm_term=chow-liu%E6%A0%91&spm=1018.2226.3001.4187</span><br><span class="line"> https://blog.csdn.net/qq_35201208/article/details/126289355?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166555644916782425157603%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=166555644916782425157603&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-1-126289355-null-null.142^v53^control_1,201^v3^control&utm_term=chow-liu%E6%A0%91&spm=1018.2226.3001.4187</span><br></pre></td></tr></table></figure><h2 id="2022-11-16"><a href="#2022-11-16" class="headerlink" title="2022-11-16"></a>2022-11-16</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">Q: 对于map更新思考</span><br><span class="line">A: 由于map整体上占用比较大,一般使用增量式更新,一方面便于自己使用,另一方面便于给显示使用</span><br></pre></td></tr></table></figure><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">Q: 关于二维地图修正</span><br><span class="line">A: 常规思路是提线,不过由于只是为了将地图摆正,所以只需要在地图范围下对其进行矫正(数格子,连在一块最多的格子,则认为是墙壁)</span><br></pre></td></tr></table></figure><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">Q: 关于Eigen库稀疏矩阵使用</span><br><span class="line">A: 一般操作是先将其储存在三角矩阵中最后通过稀疏化提取放入到我的稀疏矩阵中,具体可参考:</span><br><span class="line">https://github.com/qixianyu-buaa/EigenChineseDocument/blob/master/Eigen/Chapter3_SparseLinearAlgebra/Section1_SparseMatrixManipulations.hpp</span><br></pre></td></tr></table></figure><h2 id="2022-11-17"><a href="#2022-11-17" class="headerlink" title="2022-11-17"></a>2022-11-17</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br></pre></td><td class="code"><pre><span class="line">Q1: 关于单层神经网络流程理解</span><br><span class="line">A1: 总体上分为前向传播(输出分析结果)与后向传播(更新神经网络权重)</span><br><span class="line">前向传播:</span><br><span class="line">1. 与权重点乘获得结果</span><br><span class="line">2. 归一化用于传播</span><br><span class="line">后向传播:</span><br><span class="line">1. 计算误差</span><br><span class="line">2. 计算学习率(根据激活函数)</span><br><span class="line">3. 更新网络权重</span><br><span class="line"></span><br><span class="line">Q1: 关于多层神经网络流程理解</span><br><span class="line">A1: 总体上分为前向传播(输出分析结果)与后向传播(更新神经网络权重)</span><br><span class="line">前向传播:</span><br><span class="line">1. 与权重点乘获得下层结果</span><br><span class="line">2. 归一化用于传播</span><br><span class="line">后向传播:</span><br><span class="line">1. 计算上层误差</span><br><span class="line">2. 计算学习率(根据激活函数)</span><br><span class="line">3. 更新网络权重</span><br></pre></td></tr></table></figure><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">Q: 数值最小二乘与最大似然估计相关性</span><br><span class="line">A: 由于一般联合概率分布为多概率乘积形式,因此会先通过ln转换为连加,而根据正太分布可知,这两者本质上是相同的(最优化理论与基础)</span><br></pre></td></tr></table></figure><p><a href="https://imgse.com/i/zexLpq"><img src="https://s1.ax1x.com/2022/11/17/zexLpq.png" alt="zexLpq.png"></a></p><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">Q: 对数几率回归应用</span><br><span class="line">A: 又称sigmoid函数,主要是在slam的mapping中使用,目的是将每个栅格值做归一化处理</span><br><span class="line">由于sigmiod的分布在0.5附近变化很大,所以也可以很快区分占用与空闲栅格</span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="每日学习笔记"><a href="#每日学习笔记" class="headerlink" title="每日学习笔记"></a>每日学习笔记</h1><h2 id="222-11-3"><a href="#222-11-3" class="headerlink" </summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="理论学习" scheme="https://peitianyu.github.io/blog/categories/slam/%E7%90%86%E8%AE%BA%E5%AD%A6%E4%B9%A0/"/>
<category term="slam学习笔记" scheme="https://peitianyu.github.io/blog/tags/slam%E5%AD%A6%E4%B9%A0%E7%AC%94%E8%AE%B0/"/>
</entry>
<entry>
<title>哈工大原航模协会技术总监教你总体设计 3-1</title>
<link href="https://peitianyu.github.io/blog/2022/10/18/2022-10-18-%E5%93%88%E5%B7%A5%E5%A4%A7%E5%8E%9F%E8%88%AA%E6%A8%A1%E5%8D%8F%E4%BC%9A%E6%8A%80%E6%9C%AF%E6%80%BB%E7%9B%91%E6%95%99%E4%BD%A0%E6%80%BB%E4%BD%93%E8%AE%BE%E8%AE%A1%203-1/"/>
<id>https://peitianyu.github.io/blog/2022/10/18/2022-10-18-%E5%93%88%E5%B7%A5%E5%A4%A7%E5%8E%9F%E8%88%AA%E6%A8%A1%E5%8D%8F%E4%BC%9A%E6%8A%80%E6%9C%AF%E6%80%BB%E7%9B%91%E6%95%99%E4%BD%A0%E6%80%BB%E4%BD%93%E8%AE%BE%E8%AE%A1%203-1/</id>
<published>2022-10-18T04:30:12.000Z</published>
<updated>2022-11-30T05:48:31.921Z</updated>
<content type="html"><![CDATA[<h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址"></a>参考网址</h1><p><a href="https://www.bilibili.com/video/BV1sS4y117Sk/?spm_id_from=333.788&vd_source=745fd1b1f3e42bb544237f6d0bf78bb2">【硬核航模教程】02哈工大原航模协会技术总监教你总体设计3-1_哔哩哔哩_bilibili</a></p><h1 id="总体设计流程"><a href="#总体设计流程" class="headerlink" title="总体设计流程"></a>总体设计流程</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">input -> 需求分析 -> 参数设计 -> 布局设计 -> 动力设计 -> 翼型选择 -> 重量估计 -> 操纵性能验证 -> output</span><br><span class="line"> ^ |</span><br><span class="line"> |_____________________________________________________|</span><br></pre></td></tr></table></figure><h1 id="需求分析"><a href="#需求分析" class="headerlink" title="需求分析"></a>需求分析</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">对场景分析,提炼功能与信息指标</span><br><span class="line">飞行场景分为: 起飞, 爬升, 巡航, 任务, 下降, 降落, 存放和运输 7个阶段</span><br></pre></td></tr></table></figure><h2 id="起飞"><a href="#起飞" class="headerlink" title="起飞"></a>起飞</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br></pre></td><td class="code"><pre><span class="line">起飞重量</span><br><span class="line">起飞距离</span><br><span class="line">起飞功率</span><br><span class="line">横风能力(稳定性能)</span><br></pre></td></tr></table></figure><h2 id="爬升"><a href="#爬升" class="headerlink" title="爬升"></a>爬升</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">近地面爬升率</span><br><span class="line">升限(爬升率还剩余0.5m/s时飞行高度)</span><br></pre></td></tr></table></figure><h2 id="巡航过程"><a href="#巡航过程" class="headerlink" title="巡航过程"></a>巡航过程</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br></pre></td><td class="code"><pre><span class="line">航程</span><br><span class="line">航时</span><br><span class="line">巡航速度</span><br><span class="line">巡航高度</span><br><span class="line">环境适应性</span><br><span class="line">最小失速速度与最大失速迎角(安全飞行范围)</span><br></pre></td></tr></table></figure><h2 id="任务阶段"><a href="#任务阶段" class="headerlink" title="任务阶段"></a>任务阶段</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br></pre></td><td class="code"><pre><span class="line">稳定性要求:</span><br><span class="line">纵向短周期模态</span><br><span class="line">纵向长周期模态(一般不提)</span><br><span class="line">荷兰滚模态</span><br><span class="line">滚转模态</span><br><span class="line">螺旋模态(一般不提)</span><br><span class="line">横滚侧滑角</span><br><span class="line">操纵性要求:</span><br><span class="line">副翼阶跃响应</span><br><span class="line">升降舵阶跃响应</span><br><span class="line">方向舵阶跃响应</span><br><span class="line">最小盘旋半径</span><br><span class="line">任务要求:</span><br><span class="line">抛投</span><br><span class="line">侦察</span><br><span class="line">空战</span><br></pre></td></tr></table></figure><h2 id="下降"><a href="#下降" class="headerlink" title="下降"></a>下降</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">最大下滑率(匀速阶段)</span><br><span class="line">最小进近速度</span><br></pre></td></tr></table></figure><h2 id="降落阶段"><a href="#降落阶段" class="headerlink" title="降落阶段"></a>降落阶段</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">重量</span><br><span class="line">距离</span><br><span class="line">横风能力</span><br></pre></td></tr></table></figure><h2 id="存放和运输"><a href="#存放和运输" class="headerlink" title="存放和运输"></a>存放和运输</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br></pre></td><td class="code"><pre><span class="line">尺寸限制</span><br><span class="line">快装</span><br><span class="line">易维护</span><br><span class="line">运输载荷</span><br></pre></td></tr></table></figure><h1 id="参数设计"><a href="#参数设计" class="headerlink" title="参数设计"></a>参数设计</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br></pre></td><td class="code"><pre><span class="line">根据需求分析中有关的重量, 气动性能, 动力限制要求, 初步确定升阻参数, 动力配置, 电池,或燃油占比</span><br><span class="line">参数设计过程并不包含,气动外形设计, 共嗯那个设计, 操纵性能设计等,仅通过参考其他资料或引用经验公式进行参数数值确认.</span><br><span class="line"></span><br><span class="line">input:</span><br><span class="line">载荷要求, 动力限制, 巡航性能要求</span><br><span class="line"></span><br><span class="line">output:</span><br><span class="line">各部分系统重量</span><br><span class="line">起飞升力系数</span><br><span class="line">推重比</span><br><span class="line">翼载荷</span><br><span class="line">翼面积</span><br><span class="line">巡航升力系数</span><br><span class="line">巡航升阻比或功率因子</span><br><span class="line">....</span><br></pre></td></tr></table></figure><h2 id="通过能量法估算1kg各个参数"><a href="#通过能量法估算1kg各个参数" class="headerlink" title="通过能量法估算1kg各个参数"></a>通过能量法估算1kg各个参数</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br></pre></td><td class="code"><pre><span class="line">限制:</span><br><span class="line">输入功率:11.1v 40a-> 444w</span><br><span class="line">有效载荷不低于7kg</span><br><span class="line">25m以内加速到最小起飞速度</span><br><span class="line">地面至少剩余0.5m/s爬升率</span><br></pre></td></tr></table></figure><p><a href="https://imgse.com/i/xrMaP1"><img src="https://s1.ax1x.com/2022/10/18/xrMaP1.png" alt="xrMaP1.png"></a></p><h1 id="气动布局"><a href="#气动布局" class="headerlink" title="气动布局"></a>气动布局</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br></pre></td><td class="code"><pre><span class="line">机身:</span><br><span class="line">特征直径</span><br><span class="line">特征长度</span><br><span class="line">机翼:</span><br><span class="line">位置</span><br><span class="line">形状</span><br><span class="line">舵面</span><br><span class="line">翼型</span><br><span class="line">平尾:</span><br><span class="line">位置</span><br><span class="line">形状</span><br><span class="line">舵面</span><br><span class="line">翼型</span><br><span class="line">垂尾:</span><br><span class="line">位置</span><br><span class="line">形状</span><br><span class="line">舵面</span><br><span class="line">翼型</span><br></pre></td></tr></table></figure><h2 id="气动布局的影响"><a href="#气动布局的影响" class="headerlink" title="气动布局的影响"></a>气动布局的影响</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br></pre></td><td class="code"><pre><span class="line">操纵性和稳定性</span><br><span class="line">起降性能</span><br><span class="line">部件与载荷安装位置</span><br><span class="line">结构和工艺</span><br><span class="line">气动性能</span><br></pre></td></tr></table></figure><h1 id="翼型选择"><a href="#翼型选择" class="headerlink" title="翼型选择"></a>翼型选择</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br></pre></td><td class="code"><pre><span class="line">重要结论:</span><br><span class="line">薄翼型在失速前升力线斜率都是0.11/°</span><br><span class="line">正弯度翼型力矩系数一般为负,且迎角引起的力矩变化与弯度变化的力矩变化很小.(在小迎角范围,翼型的力矩系数变化不大, 可以近似用零升迎角时的力矩系数计算)</span><br><span class="line">不同翼型区别:</span><br><span class="line">最大升力系数 - 起飞/降落性能</span><br><span class="line">失速性能 - 最大机动系数</span><br><span class="line">力矩系数 - 操纵性能</span><br><span class="line">低阻范围 - 气动性能</span><br></pre></td></tr></table></figure><h1 id="动力选型"><a href="#动力选型" class="headerlink" title="动力选型"></a>动力选型</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br><span class="line">25</span><br><span class="line">26</span><br><span class="line">27</span><br><span class="line">28</span><br><span class="line">29</span><br></pre></td><td class="code"><pre><span class="line">动力装置:</span><br><span class="line">发动机:</span><br><span class="line">电压</span><br><span class="line">kv</span><br><span class="line">重量</span><br><span class="line">螺旋桨</span><br><span class="line">直径</span><br><span class="line">螺距</span><br><span class="line">重量</span><br><span class="line">减速组</span><br><span class="line">减速比</span><br><span class="line">重量</span><br><span class="line">电调</span><br><span class="line">电流</span><br><span class="line">重量</span><br><span class="line">电池</span><br><span class="line">电压</span><br><span class="line">放电倍率</span><br><span class="line">容量</span><br><span class="line">重量</span><br><span class="line">动力参数:</span><br><span class="line">推重比</span><br><span class="line">拉力曲线</span><br><span class="line">功率曲线</span><br><span class="line">力效曲线</span><br><span class="line">位置:</span><br><span class="line">几何位置</span><br><span class="line">安装角</span><br><span class="line">侧拉角</span><br></pre></td></tr></table></figure><h1 id="操纵性与稳定性可以通过X5分析"><a href="#操纵性与稳定性可以通过X5分析" class="headerlink" title="操纵性与稳定性可以通过X5分析"></a>操纵性与稳定性可以通过X5分析</h1>]]></content>
<summary type="html"><h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址"></a>参考网址</h1><p><a href="https://www.bilibili.com/video/BV1sS4y117Sk/?spm_id_f</summary>
<category term="飞机" scheme="https://peitianyu.github.io/blog/categories/%E9%A3%9E%E6%9C%BA/"/>
<category term="飞机" scheme="https://peitianyu.github.io/blog/tags/%E9%A3%9E%E6%9C%BA/"/>
</entry>
<entry>
<title>航模设计制作入门</title>
<link href="https://peitianyu.github.io/blog/2022/10/18/2022-10-18-%E8%88%AA%E6%A8%A1%E8%AE%BE%E8%AE%A1%E5%88%B6%E4%BD%9C%E5%85%A5%E9%97%A8/"/>
<id>https://peitianyu.github.io/blog/2022/10/18/2022-10-18-%E8%88%AA%E6%A8%A1%E8%AE%BE%E8%AE%A1%E5%88%B6%E4%BD%9C%E5%85%A5%E9%97%A8/</id>
<published>2022-10-18T04:30:12.000Z</published>
<updated>2022-11-30T05:48:32.612Z</updated>
<content type="html"><![CDATA[<h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址"></a>参考网址</h1><p><a href="https://www.bilibili.com/video/BV1ny4y127nZ/?spm_id_from=333.999.0.0&vd_source=745fd1b1f3e42bb544237f6d0bf78bb2">航模设计制作入门_哔哩哔哩_bilibili</a></p><h1 id="总体设计确定参数"><a href="#总体设计确定参数" class="headerlink" title="总体设计确定参数"></a>总体设计确定参数</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br></pre></td><td class="code"><pre><span class="line">1. 起飞总重</span><br><span class="line">2. 最大升力系数</span><br><span class="line">3. 零升阻力系数 : 预估飞机阻力</span><br><span class="line">4. 推重比 T/W</span><br><span class="line">5. 翼载荷 W/S</span><br></pre></td></tr></table></figure><h2 id="起飞总重"><a href="#起飞总重" class="headerlink" title="起飞总重"></a>起飞总重</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line">空载重量 + 任务载重</span><br></pre></td></tr></table></figure><h2 id="最大升力系数"><a href="#最大升力系数" class="headerlink" title="最大升力系数"></a>最大升力系数</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line">确定最大能够给飞机提供的升力</span><br></pre></td></tr></table></figure><h2 id="零升阻力系数-预估飞机阻力"><a href="#零升阻力系数-预估飞机阻力" class="headerlink" title="零升阻力系数 : 预估飞机阻力"></a>零升阻力系数 : 预估飞机阻力</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">1. 摩擦 材料外形</span><br><span class="line">2. 压差 材料外形</span><br><span class="line">3. 诱导阻力 翼梢形成下洗气流 (大展弦比, 翼梢小翼)</span><br></pre></td></tr></table></figure><h2 id="推重比"><a href="#推重比" class="headerlink" title="推重比"></a>推重比</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br></pre></td><td class="code"><pre><span class="line">1. 起飞滑跑距离</span><br><span class="line">2. 根据平飞状态</span><br><span class="line">3. 根据爬升性能</span><br><span class="line">4. 根据最大平飞速度</span><br><span class="line"></span><br><span class="line">综合考虑后选择最大得推重比</span><br></pre></td></tr></table></figure><h2 id="翼载荷"><a href="#翼载荷" class="headerlink" title="翼载荷"></a>翼载荷</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br></pre></td><td class="code"><pre><span class="line">影响时速速度, 爬升率, 起飞着陆距离, 盘旋性能, 决定了设计升力系数</span><br><span class="line">翼载对确定飞机起飞总重有很大影响,翼载小机翼大,能改善性能</span><br><span class="line">但机翼大同时会导致阻力与空机重量增大,导致起飞总重增大</span><br><span class="line"></span><br><span class="line">这里注意参考实战飞机:</span><br><span class="line">通用航空单发: 83 g/dm2 </span><br><span class="line">通用航空双发: 127g/dm2</span><br><span class="line">航模一般 100g/dm2比较好飞</span><br><span class="line"></span><br><span class="line">1. 根据失速速度</span><br><span class="line">2. 根据起飞距离计算</span><br><span class="line">3. 根据航程计算</span><br><span class="line">4. 根据航时确定</span><br><span class="line"></span><br><span class="line">综合考虑确定最小翼载荷,好飞</span><br></pre></td></tr></table></figure><h1 id="机翼设计"><a href="#机翼设计" class="headerlink" title="机翼设计"></a>机翼设计</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br></pre></td><td class="code"><pre><span class="line">1. 机翼面积</span><br><span class="line">2. 翼型</span><br><span class="line">: 最大升力系数</span><br><span class="line">3. 展弦比</span><br><span class="line">4. 后掠角 1/4弦线</span><br><span class="line">5. 根梢比</span><br><span class="line">6. 上反角 : 横向稳定性</span><br><span class="line">7. 几何扭转及气动扭转(希望翼根先失速)</span><br><span class="line">8. 增升装置 开缝襟翼</span><br><span class="line">9. 横向操纵面尺寸布局</span><br><span class="line">10. 确定布局</span><br><span class="line">11. 零升迎角确定安装角</span><br></pre></td></tr></table></figure><h1 id="副翼"><a href="#副翼" class="headerlink" title="副翼"></a>副翼</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br></pre></td><td class="code"><pre><span class="line">一般机翼面积比 Sa/S = 0.05 - 0.07</span><br><span class="line">弦长一般取 0.2 ~ 0.25</span><br><span class="line">沿展向长度得 0.3 ~ 0.4</span><br><span class="line">副翼偏转角一般向上-25°,向下15°</span><br></pre></td></tr></table></figure><h1 id="机翼受力分析"><a href="#机翼受力分析" class="headerlink" title="机翼受力分析"></a>机翼受力分析</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br></pre></td><td class="code"><pre><span class="line">剪力 : 主要由翼梁腹板承受</span><br><span class="line">扭矩 : 翼梁缘条承受</span><br><span class="line">弯矩 : 翼梁, 加强翼肋与张线组成得桁架承受</span><br><span class="line">做一架轻木航模教程</span><br></pre></td></tr></table></figure><h1 id="尾翼"><a href="#尾翼" class="headerlink" title="尾翼"></a>尾翼</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br></pre></td><td class="code"><pre><span class="line">尾翼:</span><br><span class="line">垂尾:</span><br><span class="line">Sv = Kv * S * b / LV</span><br><span class="line">KV : 0.05</span><br><span class="line">b : 翼展</span><br><span class="line">LV : 重心到水尾气动中心距离</span><br><span class="line">稍微增大展弦比</span><br><span class="line">根梢比</span><br><span class="line">后掠</span><br><span class="line"></span><br><span class="line">水尾:</span><br><span class="line">展弦比小于机翼得展弦比</span><br><span class="line">根梢比</span><br><span class="line">后掠</span><br><span class="line">翼型,对称翼型</span><br><span class="line">Sh = Kh * S* t/LH</span><br><span class="line">Kh = 0.6(飞行) ~ 1.2(滞空)</span><br><span class="line">t : 气动弦长</span><br><span class="line">重心到水尾气动中心距离</span><br><span class="line">水尾面积 = 0.6 * 主翼面积 * 平均气动弦长/重心到尾翼得距离</span><br></pre></td></tr></table></figure><h1 id="配平"><a href="#配平" class="headerlink" title="配平"></a>配平</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">重心升力点 俯仰力矩</span><br><span class="line">由于下洗 几何迎角小于气动迎角</span><br></pre></td></tr></table></figure><h1 id="动力"><a href="#动力" class="headerlink" title="动力"></a>动力</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">电动功率: 功率大, 反应灵敏, 续航差</span><br><span class="line">单发,双发</span><br></pre></td></tr></table></figure><h1 id="起落架"><a href="#起落架" class="headerlink" title="起落架"></a>起落架</h1><h1 id="快拆"><a href="#快拆" class="headerlink" title="快拆"></a>快拆</h1><p>4.2.299</p><p>new_slam</p><ol><li> slam踢动优化</li><li>动态重定位添加一些打印</li></ol><h1 id="材料密度记录"><a href="#材料密度记录" class="headerlink" title="材料密度记录"></a>材料密度记录</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br></pre></td><td class="code"><pre><span class="line">eps 0.03 g/cm3</span><br><span class="line">高密度泡沫挤塑板 0.23 g/cm3</span><br><span class="line">钢 7.9 g/cm3</span><br><span class="line">合金铝 2.72、2.73、2.82 g/cm3</span><br><span class="line">桐木 0.23 ~ 0.40 g/cm3</span><br><span class="line">松木0.4 ~ 0.7 g/cm3</span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址"></a>参考网址</h1><p><a href="https://www.bilibili.com/video/BV1ny4y127nZ/?spm_id_f</summary>
<category term="飞机" scheme="https://peitianyu.github.io/blog/categories/%E9%A3%9E%E6%9C%BA/"/>
<category term="飞机" scheme="https://peitianyu.github.io/blog/tags/%E9%A3%9E%E6%9C%BA/"/>
</entry>
<entry>
<title>Chow_Liu Tree</title>
<link href="https://peitianyu.github.io/blog/2022/10/12/chow_liu%20tree/"/>
<id>https://peitianyu.github.io/blog/2022/10/12/chow_liu%20tree/</id>
<published>2022-10-12T04:30:12.000Z</published>
<updated>2022-11-30T05:47:10.100Z</updated>
<content type="html"><![CDATA[<h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址:"></a>参考网址:</h1><p><a href="https://blog.csdn.net/qq_35201208/article/details/126289355?ops_request_misc=%7B%22request_id%22:%22166555644916782425157603%22,%22scm%22:%2220140713.130102334..%22%7D&request_id=166555644916782425157603&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-1-126289355-null-null.142%5Ev53%5Econtrol_1,201%5Ev3%5Econtrol&utm_term=chow-liu%E6%A0%91&spm=1018.2226.3001.4187">(170条消息) SLAM中的位姿图稀疏化_qq_35201208的博客-CSDN博客</a></p><p><a href="https://blog.csdn.net/qq_38023849/article/details/110306178?ops_request_misc=%7B%22request_id%22:%22166555644916782425127805%22,%22scm%22:%2220140713.130102334.pc_all.%22%7D&request_id=166555644916782425127805&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~rank_v31_ecpm-3-110306178-null-null.142%5Ev53%5Econtrol_1,201%5Ev3%5Econtrol&utm_term=chow-liu%E6%A0%91&spm=1018.2226.3001.4187">(170条消息) FAB_MAP算法的理论_宅男不宅的博客-CSDN博客_fabmap</a></p><p><a href="http://www.bewindoweb.com/232.html">毕业论文整理（二）：用依赖树近似离散概率分布 | 文章 | BEWINDOWEB</a></p><p><a href="https://zhuanlan.zhihu.com/p/437028454">高仙IROS论文分享丨适用于变化环境下的长期建图定位系统 - 知乎 (zhihu.com)</a></p><p><a href="https://sci-hub.se/10.1109/TIT.1968.1054142">Sci-Hub | Approximating discrete probability distributions with dependence trees. IEEE Transactions on Information Theory, 14(3), 462–467 | 10.1109/TIT.1968.1054142</a></p><h1 id="周刘树理解"><a href="#周刘树理解" class="headerlink" title="周刘树理解"></a>周刘树理解</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line">二阶分布的乘积近似原联合分布,使用最大互信息,求得依赖树</span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址:"></a>参考网址:</h1><p><a href="https://blog.csdn.net/qq_35201208/article/details/1</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="理论学习" scheme="https://peitianyu.github.io/blog/categories/slam/%E7%90%86%E8%AE%BA%E5%AD%A6%E4%B9%A0/"/>
<category term="slam" scheme="https://peitianyu.github.io/blog/tags/slam/"/>
</entry>
<entry>
<title>1. 坐标变换</title>
<link href="https://peitianyu.github.io/blog/2022/09/21/2022-9-21-%E5%9D%90%E6%A0%87%E5%8F%98%E6%8D%A2/"/>
<id>https://peitianyu.github.io/blog/2022/09/21/2022-9-21-%E5%9D%90%E6%A0%87%E5%8F%98%E6%8D%A2/</id>
<published>2022-09-21T04:30:12.000Z</published>
<updated>2022-11-30T05:47:08.970Z</updated>
<content type="html"><![CDATA[<h1 id="代码仓库"><a href="#代码仓库" class="headerlink" title="代码仓库"></a>代码仓库</h1><p><a href="https://github.com/peitianyu/2d_slam_learning.git">https://github.com/peitianyu/2d_slam_learning.git</a></p><h1 id="简介"><a href="#简介" class="headerlink" title="简介"></a>简介</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br></pre></td><td class="code"><pre><span class="line">2维坐标变换相对简单,实际上主要可分为两部分,求解转换矩阵或者转换向量/求解转换后坐标</span><br><span class="line">这里主要用两个函数表示</span><br><span class="line">- TransformFrom()</span><br><span class="line">- TransformAdd()</span><br><span class="line">这里提供了两种思路</span><br><span class="line">- 根据公式直接代入,硬解: 对于2dslam,建议使用,比较简单</span><br><span class="line">- 齐次坐标使用Eigen库求解</span><br><span class="line">- 使用四元数更新(待添加)</span><br><span class="line">位姿线性差分:</span><br><span class="line">- 对差分向量进行线性差分</span><br></pre></td></tr></table></figure><h1 id="测试"><a href="#测试" class="headerlink" title="测试"></a>测试</h1><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br><span class="line">25</span><br><span class="line">26</span><br><span class="line">27</span><br><span class="line">28</span><br><span class="line">29</span><br><span class="line">30</span><br><span class="line">31</span><br><span class="line">32</span><br><span class="line">33</span><br><span class="line">34</span><br><span class="line">35</span><br><span class="line">36</span><br><span class="line">37</span><br><span class="line">38</span><br><span class="line">39</span><br><span class="line">40</span><br><span class="line">41</span><br><span class="line">42</span><br><span class="line">43</span><br><span class="line">44</span><br><span class="line">45</span><br><span class="line">46</span><br><span class="line">47</span><br><span class="line">48</span><br><span class="line">49</span><br><span class="line">50</span><br><span class="line">51</span><br><span class="line">52</span><br><span class="line">53</span><br><span class="line">54</span><br><span class="line">55</span><br><span class="line">56</span><br><span class="line">57</span><br><span class="line">58</span><br><span class="line">59</span><br><span class="line">60</span><br></pre></td><td class="code"><pre><span class="line"><span class="comment">// 直接求解变换向量与变换后坐标</span></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">TestDirectSolution</span><span class="params">()</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> <span class="function">Eigen::Vector3f <span class="title">old_pose</span><span class="params">(<span class="number">1.0</span>, <span class="number">1.0</span>, <span class="number">1.0</span>)</span></span>;</span><br><span class="line"> <span class="function">Eigen::Vector3f <span class="title">new_pose</span><span class="params">(<span class="number">2.0</span>, <span class="number">2.0</span>, <span class="number">2.0</span>)</span></span>;</span><br><span class="line"></span><br><span class="line"> Eigen::Vector3f d_pose = Transform::<span class="built_in">TransformFrom</span>(old_pose, new_pose); <span class="comment">// 求解变换向量</span></span><br><span class="line"> std::cout<<<span class="string">"d_pose: "</span><<d_pose.<span class="built_in">transpose</span>()<<std::endl;</span><br><span class="line"></span><br><span class="line"> std::cout<<<span class="string">"transformed pose: "</span><<Transform::<span class="built_in">TransformAdd</span>(old_pose, d_pose).<span class="built_in">transpose</span>()<<std::endl; <span class="comment">// 求解变换后坐标</span></span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="comment">// 齐次坐标使用Eigen求解</span></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">TestEigenSolution</span><span class="params">()</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> <span class="function">Eigen::Vector3f <span class="title">old_pose</span><span class="params">(<span class="number">1.0</span>, <span class="number">1.0</span>, <span class="number">1.0</span>)</span></span>;</span><br><span class="line"> <span class="function">Eigen::Vector3f <span class="title">new_pose</span><span class="params">(<span class="number">2.0</span>, <span class="number">2.0</span>, <span class="number">2.0</span>)</span></span>;</span><br><span class="line"></span><br><span class="line"> Eigen::Matrix3f t_d_pose = Transform::<span class="built_in">TransformFromWithEigen</span>(old_pose, new_pose); <span class="comment">// 求解变换向量</span></span><br><span class="line"> std::cout<<<span class="string">"t_d_pose: \n"</span><<t_d_pose<<std::endl;</span><br><span class="line"> Eigen::Vector3f d_pose = Transform::<span class="built_in">T2V</span>(t_d_pose);</span><br><span class="line"> std::cout<<<span class="string">"d_pose: "</span><<d_pose.<span class="built_in">transpose</span>()<<std::endl;</span><br><span class="line"></span><br><span class="line"> std::cout<<<span class="string">"transformed pose: "</span><<Transform::<span class="built_in">TransformAddWithEigen</span>(old_pose, t_d_pose).<span class="built_in">transpose</span>()<<std::endl; <span class="comment">// 求解变换后坐标</span></span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="comment">// 测试差分坐标</span></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">TestDiffPose</span><span class="params">()</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> <span class="function">Eigen::Vector3f <span class="title">old_pose</span><span class="params">(<span class="number">1.0</span>, <span class="number">1.0</span>, <span class="number">1.0</span>)</span></span>;</span><br><span class="line"> <span class="function">Eigen::Vector3f <span class="title">new_pose</span><span class="params">(<span class="number">2.0</span>, <span class="number">2.0</span>, <span class="number">2.0</span>)</span></span>;</span><br><span class="line"></span><br><span class="line"> Eigen::Vector3f d_pose = Transform::<span class="built_in">TransformFrom</span>(old_pose, new_pose); </span><br><span class="line"> std::cout<<<span class="string">"d_pose: "</span><<d_pose.<span class="built_in">transpose</span>()<<std::endl;</span><br><span class="line"></span><br><span class="line"> std::cout<<<span class="string">"transformed pose: "</span><<Transform::<span class="built_in">TransformAdd</span>(old_pose, d_pose/<span class="number">3.0</span>).<span class="built_in">transpose</span>()<<std::endl;</span><br><span class="line"></span><br><span class="line"> std::cout<<<span class="string">"transformed pose: "</span><<Transform::<span class="built_in">TransformAdd</span>(old_pose, <span class="number">2.0</span>*d_pose/<span class="number">3.0</span>).<span class="built_in">transpose</span>()<<std::endl;</span><br><span class="line"></span><br><span class="line"> std::cout<<<span class="string">"transformed pose: "</span><<Transform::<span class="built_in">TransformAdd</span>(old_pose, d_pose).<span class="built_in">transpose</span>()<<std::endl;</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="comment">// 测试差分坐标</span></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">TestDiffPoseWithEigen</span><span class="params">()</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> <span class="function">Eigen::Vector3f <span class="title">old_pose</span><span class="params">(<span class="number">1.0</span>, <span class="number">1.0</span>, <span class="number">1.0</span>)</span></span>;</span><br><span class="line"> <span class="function">Eigen::Vector3f <span class="title">new_pose</span><span class="params">(<span class="number">2.0</span>, <span class="number">2.0</span>, <span class="number">2.0</span>)</span></span>;</span><br><span class="line"></span><br><span class="line"> Eigen::Matrix3f t_d_pose = Transform::<span class="built_in">TransformFromWithEigen</span>(old_pose, new_pose);</span><br><span class="line"> std::cout<<<span class="string">"t_d_pose: \n"</span><<t_d_pose<<std::endl;</span><br><span class="line"> Eigen::Vector3f d_pose = Transform::<span class="built_in">T2V</span>(t_d_pose);</span><br><span class="line"> std::cout<<<span class="string">"d_pose: "</span><<d_pose.<span class="built_in">transpose</span>()<<std::endl;</span><br><span class="line"></span><br><span class="line"> std::cout<<<span class="string">"transformed pose: "</span><<Transform::<span class="built_in">TransformAddWithEigen</span>(old_pose, Transform::<span class="built_in">V2T</span>(d_pose/<span class="number">3.0</span>)).<span class="built_in">transpose</span>()<<std::endl;</span><br><span class="line"></span><br><span class="line"> std::cout<<<span class="string">"transformed pose: "</span><<Transform::<span class="built_in">TransformAddWithEigen</span>(old_pose, Transform::<span class="built_in">V2T</span>(<span class="number">2.0</span>*d_pose/<span class="number">3.0</span>)).<span class="built_in">transpose</span>()<<std::endl;</span><br><span class="line"></span><br><span class="line"> std::cout<<<span class="string">"transformed pose: "</span><<Transform::<span class="built_in">TransformAddWithEigen</span>(old_pose, t_d_pose).<span class="built_in">transpose</span>()<<std::endl;</span><br><span class="line">}</span><br><span class="line"></span><br></pre></td></tr></table></figure><h1 id="结果"><a href="#结果" class="headerlink" title="结果"></a>结果</h1><figure class="highlight shell"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br></pre></td><td class="code"><pre><span class="line">-------------TestDirectSolution--------------</span><br><span class="line">d_pose: 1.38177 -0.301169 1</span><br><span class="line">transformed pose: 2 2 2</span><br><span class="line">-------------TestEigenSolution--------------</span><br><span class="line">t_d_pose:</span><br><span class="line"> 0.540302 -0.841471 1.38177</span><br><span class="line"> 0.841471 0.540302 -0.301169</span><br><span class="line"> 0 0 1</span><br><span class="line">d_pose: 1.38177 -0.301169 1</span><br><span class="line">transformed pose: 2 2 2</span><br><span class="line">-------------TestDiffPose--------------</span><br><span class="line">d_pose: 1.38177 -0.301169 1</span><br><span class="line">transformed pose: 1.33333 1.33333 1.33333</span><br><span class="line">transformed pose: 1.66667 1.66667 1.66667</span><br><span class="line">transformed pose: 2 2 2</span><br><span class="line">-------------TestDiffPoseWithEigen--------------</span><br><span class="line">t_d_pose:</span><br><span class="line"> 0.540302 -0.841471 1.38177</span><br><span class="line"> 0.841471 0.540302 -0.301169</span><br><span class="line"> 0 0 1</span><br><span class="line">d_pose: 1.38177 -0.301169 1</span><br><span class="line">transformed pose: 1.33333 1.33333 1.33333</span><br><span class="line">transformed pose: 1.66667 1.66667 1.66667</span><br><span class="line">transformed pose: 2 2 2</span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="代码仓库"><a href="#代码仓库" class="headerlink" title="代码仓库"></a>代码仓库</h1><p><a href="https://github.com/peitianyu/2d_slam_learning.git">ht</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="理论学习" scheme="https://peitianyu.github.io/blog/categories/slam/%E7%90%86%E8%AE%BA%E5%AD%A6%E4%B9%A0/"/>
<category term="2d激光slam学习笔记" scheme="https://peitianyu.github.io/blog/tags/2d%E6%BF%80%E5%85%89slam%E5%AD%A6%E4%B9%A0%E7%AC%94%E8%AE%B0/"/>
</entry>
<entry>
<title>0. 2d_slam_learning简介</title>
<link href="https://peitianyu.github.io/blog/2022/09/21/2022-9-21-0.-2d_slam_learning%E7%AE%80%E4%BB%8B/"/>
<id>https://peitianyu.github.io/blog/2022/09/21/2022-9-21-0.-2d_slam_learning%E7%AE%80%E4%BB%8B/</id>
<published>2022-09-21T04:30:12.000Z</published>
<updated>2022-11-30T05:44:55.960Z</updated>
<content type="html"><![CDATA[<h1 id="代码仓库"><a href="#代码仓库" class="headerlink" title="代码仓库"></a>代码仓库</h1><p><a href="https://github.com/peitianyu/2d_slam_learning.git">https://github.com/peitianyu/2d_slam_learning.git</a></p><h1 id="简介"><a href="#简介" class="headerlink" title="简介"></a>简介</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">纸上得来终觉浅,绝知此事要躬行.</span><br><span class="line">学习slam已经有一年多,总感觉对所学掌握的不够扎实,所以通过写教程的方式记录,方便以后复习之用</span><br></pre></td></tr></table></figure><h1 id="章节"><a href="#章节" class="headerlink" title="章节"></a>章节</h1><p><a href="">坐标变换</a></p><p>[里程计标定]</p><p>[激光数据处理]</p><p>[卡尔曼滤波]</p><p>[非线性优化]</p><p>[激光配准]</p><p>[概率地图]</p><p>[回环检测]</p><p>[图优化]</p><p>[因子图优化]</p><p>[重定位]</p>]]></content>
<summary type="html"><h1 id="代码仓库"><a href="#代码仓库" class="headerlink" title="代码仓库"></a>代码仓库</h1><p><a href="https://github.com/peitianyu/2d_slam_learning.git">ht</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="2d_slam_learning" scheme="https://peitianyu.github.io/blog/categories/slam/2d-slam-learning/"/>
<category term="2d激光slam学习笔记" scheme="https://peitianyu.github.io/blog/tags/2d%E6%BF%80%E5%85%89slam%E5%AD%A6%E4%B9%A0%E7%AC%94%E8%AE%B0/"/>
</entry>
<entry>
<title>huogh_transform</title>
<link href="https://peitianyu.github.io/blog/2022/09/19/%E7%9B%B4%E7%BA%BF%E6%A3%80%E6%B5%8B-huogh_transform/"/>
<id>https://peitianyu.github.io/blog/2022/09/19/%E7%9B%B4%E7%BA%BF%E6%A3%80%E6%B5%8B-huogh_transform/</id>
<published>2022-09-19T04:30:12.000Z</published>
<updated>2022-11-30T05:47:16.554Z</updated>
<content type="html"><![CDATA[<h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址:"></a>参考网址:</h1><p><a href="https://www.bilibili.com/video/BV1T44y1Y7kL/?spm_id_from=333.788&vd_source=745fd1b1f3e42bb544237f6d0bf78bb2"> 霍夫线变换原理_哔哩哔哩_bilibili</a></p><p><a href="https://github.com/Forrest-Z/mars_mapping/blob/master/occ_line_detection/src/occ_line_detector.cpp">mars_mapping/occ_line_detector.cpp at master · Forrest-Z/mars_mapping (github.com)</a></p><p><a href="https://blog.csdn.net/leonardohaig/article/details/87907462?ops_request_misc=&request_id=&biz_id=102&utm_term=%E9%9C%8D%E5%A4%AB%E5%8F%98%E5%8C%96%E6%A3%80%E6%B5%8B%E7%9B%B4%E7%BA%BF&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-1-87907462.142%5Ev47%5Ebody_digest,201%5Ev3%5Eadd_ask&spm=1018.2226.3001.4187">(164条消息) 霍夫变换直线检测（Line Detection）原理及示例_leonardohaig的博客-CSDN博客_霍夫变换直线检测原理</a></p><h1 id="原理"><a href="#原理" class="headerlink" title="原理"></a>原理</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">通过霍夫变换,得到霍夫空间下的(p, theta),对不同角度进行累加,若得到相近的p,则认为处于同一直线</span><br><span class="line">==> 霍夫变换: p = x*cos(theta) + y*sin(theta), 细分theta与p(这里一般指地图分辨率)</span><br></pre></td></tr></table></figure><h1 id="代码实现"><a href="#代码实现" class="headerlink" title="代码实现"></a>代码实现</h1><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br><span class="line">25</span><br><span class="line">26</span><br><span class="line">27</span><br><span class="line">28</span><br><span class="line">29</span><br><span class="line">30</span><br><span class="line">31</span><br></pre></td><td class="code"><pre><span class="line"><span class="meta">#<span class="meta-keyword">pragma</span> omp parallel for</span></span><br><span class="line"><span class="keyword">for</span>(<span class="keyword">int</span> i = <span class="number">0</span>;i < height;i++) <span class="comment">// 遍历地图</span></span><br><span class="line">{</span><br><span class="line"> <span class="keyword">for</span>(<span class="keyword">int</span> j = <span class="number">0</span>;j < width;j++)</span><br><span class="line"> {</span><br><span class="line"> <span class="keyword">int</span> index = j + i * width;</span><br><span class="line"> <span class="keyword">if</span>(i == <span class="number">0</span> && j == <span class="number">0</span>)</span><br><span class="line"> <span class="keyword">continue</span>;</span><br><span class="line"></span><br><span class="line"> <span class="keyword">if</span>(map_->data[index] >= <span class="number">60</span>) <span class="comment">// 占用值大于60,认为障碍物</span></span><br><span class="line"> {</span><br><span class="line"> <span class="keyword">double</span> x_ = (<span class="keyword">double</span>)j + <span class="number">0.5</span>;</span><br><span class="line"> <span class="keyword">double</span> y_ = (<span class="keyword">double</span>)i + <span class="number">0.5</span>;</span><br><span class="line"></span><br><span class="line"> <span class="keyword">double</span> phi = <span class="built_in">atan2</span>(y_,x_); <span class="comment">// 计算角度</span></span><br><span class="line"> <span class="keyword">double</span> R = <span class="built_in">sqrt</span>(<span class="built_in">pow</span>(x_,<span class="number">2</span>) + <span class="built_in">pow</span>(y_,<span class="number">2</span>)) * resolution; <span class="comment">// 计算距离</span></span><br><span class="line"></span><br><span class="line"> <span class="comment">// #pragma omp parallel for</span></span><br><span class="line"> <span class="keyword">for</span>(<span class="keyword">int</span> k = <span class="number">0</span>;k < angle_size; k++) <span class="comment">// 关键</span></span><br><span class="line"> {</span><br><span class="line"> <span class="keyword">double</span> angle = (<span class="keyword">double</span>)k * angle_resolution; <span class="comment">// 角度分辨率</span></span><br><span class="line"> <span class="keyword">double</span> r = R*<span class="built_in">cos</span>( angle/<span class="number">180.0</span>*M_PI - phi);</span><br><span class="line"> <span class="comment">// int theta_index = (angle-origin_a)/angle_resolution;</span></span><br><span class="line"> <span class="keyword">int</span> theta_index = k;</span><br><span class="line"> <span class="keyword">int</span> r_index = (r-origin_r)/r_resolution;</span><br><span class="line"> <span class="keyword">int</span> hough_index = r_index*angle_size + theta_index;</span><br><span class="line"> hough_map[hough_index]++; <span class="comment">// 累加器</span></span><br><span class="line"> }</span><br><span class="line"> }</span><br><span class="line"> }</span><br><span class="line">} </span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址:"></a>参考网址:</h1><p><a href="https://www.bilibili.com/video/BV1T44y1Y7kL/?spm_id</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="理论学习" scheme="https://peitianyu.github.io/blog/categories/slam/%E7%90%86%E8%AE%BA%E5%AD%A6%E4%B9%A0/"/>
<category term="直线检测" scheme="https://peitianyu.github.io/blog/tags/%E7%9B%B4%E7%BA%BF%E6%A3%80%E6%B5%8B/"/>
</entry>
<entry>
<title>滤波与优化思路梳理</title>
<link href="https://peitianyu.github.io/blog/2022/09/16/%E6%BB%A4%E6%B3%A2%E4%B8%8E%E4%BC%98%E5%8C%96%E6%80%9D%E8%B7%AF%E6%A2%B3%E7%90%86/"/>
<id>https://peitianyu.github.io/blog/2022/09/16/%E6%BB%A4%E6%B3%A2%E4%B8%8E%E4%BC%98%E5%8C%96%E6%80%9D%E8%B7%AF%E6%A2%B3%E7%90%86/</id>
<published>2022-09-16T04:30:12.000Z</published>
<updated>2022-11-30T05:47:14.927Z</updated>
<content type="html"><![CDATA[<h1 id="重新思考关于滤波-关于优化"><a href="#重新思考关于滤波-关于优化" class="headerlink" title="重新思考关于滤波,关于优化"></a>重新思考关于滤波,关于优化</h1><figure class="highlight shell"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">1. 优化与滤波本质上都是对数据的一种处理方式</span><br><span class="line">- 优化考虑的是批量数据, 批量处理, 对总体数据有约束作用</span><br><span class="line">- 滤波考虑的是k-1时刻的数据,实时处理</span><br></pre></td></tr></table></figure><h1 id="简单例子"><a href="#简单例子" class="headerlink" title="简单例子"></a>简单例子</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br></pre></td><td class="code"><pre><span class="line">对于这样一组数求它的均值: 12.5 12.4 12.5 12.6 12.8 12.7 12.3 12.4 12.5 12.2</span><br><span class="line">滤波做法:</span><br><span class="line">u = M(k-1)*(1-1/k) + Mk*(1/k)</span><br><span class="line">优化做法:</span><br><span class="line">u = (M1 + M2 + ... + Mk)/k</span><br><span class="line">=> 这里实际上可以加入权重的概念,也就变成了</span><br><span class="line">u = (w1*M1 + w2*M2 + ... + wk*Mk)/k</span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="重新思考关于滤波-关于优化"><a href="#重新思考关于滤波-关于优化" class="headerlink" title="重新思考关于滤波,关于优化"></a>重新思考关于滤波,关于优化</h1><figure class="highlight shel</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="理论学习" scheme="https://peitianyu.github.io/blog/categories/slam/%E7%90%86%E8%AE%BA%E5%AD%A6%E4%B9%A0/"/>
<category term="slam" scheme="https://peitianyu.github.io/blog/tags/slam/"/>
</entry>
<entry>
<title>lio_sam安装使用</title>
<link href="https://peitianyu.github.io/blog/2022/09/15/lio_sam%E5%AE%89%E8%A3%85%E4%BD%BF%E7%94%A8/"/>
<id>https://peitianyu.github.io/blog/2022/09/15/lio_sam%E5%AE%89%E8%A3%85%E4%BD%BF%E7%94%A8/</id>
<published>2022-09-15T04:30:12.000Z</published>
<updated>2022-11-30T05:46:38.801Z</updated>
<content type="html"><![CDATA[<h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址:"></a>参考网址:</h1><p><a href="https://blog.csdn.net/QLeelq/article/details/111088136?ops_request_misc=%7B%22request_id%22:%22166315977616800180680290%22,%22scm%22:%2220140713.130102334..%22%7D&request_id=166315977616800180680290&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~top_positive~default-1-111088136-null-null.142%5Ev47%5Ebody_digest,201%5Ev3%5Eadd_ask&utm_term=LIO-SAM&spm=1018.2226.3001.4187">(161条消息) LIO-sam：安装 - 运行 - 论文介绍 - 源码阅读_非晚非晚的博客-CSDN博客_lio-sam</a></p><h1 id="安装运行"><a href="#安装运行" class="headerlink" title="安装运行"></a>安装运行</h1><figure class="highlight shell"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br><span class="line">25</span><br><span class="line">26</span><br></pre></td><td class="code"><pre><span class="line"><span class="meta">#</span><span class="bash"> 安装gtsam</span></span><br><span class="line">wget -O ~/Downloads/gtsam.zip https://github.com/borglab/gtsam/archive/4.0.2.zip</span><br><span class="line">cd ~/Downloads/ && unzip gtsam.zip -d ~/Downloads/</span><br><span class="line">cd ~/Downloads/gtsam-4.0.2/</span><br><span class="line">mkdir build && cd build</span><br><span class="line">cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF ..</span><br><span class="line">sudo make install -j6 //根据自己的cpu核数定，越多越快，本人是6核的，注意不能超出自己电脑的核数</span><br><span class="line"><span class="meta"></span></span><br><span class="line"><span class="meta">#</span><span class="bash"> 配置环境</span></span><br><span class="line">nano ~/.bashrc</span><br><span class="line">export LD_LIBRARY_PATH=/usr/local/lib/:$LD_LIBRARY_PATH</span><br><span class="line">source ~/.bashrc</span><br><span class="line"><span class="meta"></span></span><br><span class="line"><span class="meta">#</span><span class="bash"> 安装并编译lio_sam</span></span><br><span class="line">mkdir ~/catkin_ws/src </span><br><span class="line">cd ~/catkin_ws/src </span><br><span class="line">git clone https://github.com/TixiaoShan/LIO-SAM.git </span><br><span class="line">cd ..</span><br><span class="line">catkin_make</span><br><span class="line"><span class="meta"></span></span><br><span class="line"><span class="meta">#</span><span class="bash"> 下载数据</span></span><br><span class="line">数据链接：链接：https://pan.baidu.com/s/1CbDJPIupCm3yROmM2qOsOw 提取码：ato7</span><br><span class="line"><span class="meta"></span></span><br><span class="line"><span class="meta">#</span><span class="bash"> 运行</span></span><br><span class="line">roslaunch lio_sam run.launch</span><br><span class="line">rosbag play casual_walk.bag</span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址:"></a>参考网址:</h1><p><a href="https://blog.csdn.net/QLeelq/article/details/111088</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="开源框架" scheme="https://peitianyu.github.io/blog/categories/slam/%E5%BC%80%E6%BA%90%E6%A1%86%E6%9E%B6/"/>
<category term="lio" scheme="https://peitianyu.github.io/blog/tags/lio/"/>
</entry>
<entry>
<title>协方差矩阵与椭圆</title>
<link href="https://peitianyu.github.io/blog/2022/09/14/%E5%8D%8F%E6%96%B9%E5%B7%AE%E7%9F%A9%E9%98%B5%E4%B8%8E%E6%A4%AD%E5%9C%86/"/>
<id>https://peitianyu.github.io/blog/2022/09/14/%E5%8D%8F%E6%96%B9%E5%B7%AE%E7%9F%A9%E9%98%B5%E4%B8%8E%E6%A4%AD%E5%9C%86/</id>
<published>2022-09-14T04:30:12.000Z</published>
<updated>2022-11-30T05:47:16.143Z</updated>
<content type="html"><![CDATA[<h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址"></a>参考网址</h1><p><a href="https://www.bilibili.com/video/BV1gg41117ha?spm_id_from=333.337.search-card.all.click&vd_source=745fd1b1f3e42bb544237f6d0bf78bb2">程序员的数学2（概率统计2020）：协方差矩阵与椭圆的关系.mp4_哔哩哔哩_bilibili</a></p><p><a href="https://www.bilibili.com/video/BV1ib411t7YR?spm_id_from=333.337.search-card.all.click">线性代数视频教程</a></p><p><a href="https://zhuanlan.zhihu.com/p/372315811">路径规划 | 随机采样算法：Informed-RRT* - 知乎 (zhihu.com)</a></p><h1 id="椭圆求解"><a href="#椭圆求解" class="headerlink" title="椭圆求解"></a>椭圆求解</h1><p><a href="https://imgse.com/i/vx9nsI"><img src="https://s1.ax1x.com/2022/09/14/vx9nsI.png" alt="vx9nsI.png"></a></p><h1 id="椭圆采样"><a href="#椭圆采样" class="headerlink" title="椭圆采样"></a>椭圆采样</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br></pre></td><td class="code"><pre><span class="line"># 根据长轴,短轴采样,(x, y)</span><br><span class="line">(x^2 / a^2) + (y^2 / b^2) < 1</span><br><span class="line"># 根据转移向量与旋转矩阵变换坐标</span><br><span class="line">X1 = R * X0 + T</span><br><span class="line"># 求解旋转矩阵,theata = atan(y/x)</span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址"></a>参考网址</h1><p><a href="https://www.bilibili.com/video/BV1gg41117ha?spm_id_fr</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="理论学习" scheme="https://peitianyu.github.io/blog/categories/slam/%E7%90%86%E8%AE%BA%E5%AD%A6%E4%B9%A0/"/>
<category term="slam" scheme="https://peitianyu.github.io/blog/tags/slam/"/>
</entry>
<entry>
<title>c++那些事</title>
<link href="https://peitianyu.github.io/blog/2022/09/05/c++11%E5%AD%A6%E4%B9%A0/"/>
<id>https://peitianyu.github.io/blog/2022/09/05/c++11%E5%AD%A6%E4%B9%A0/</id>
<published>2022-09-05T04:30:12.000Z</published>
<updated>2022-11-30T05:51:56.883Z</updated>
<content type="html"><![CDATA[<h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址:"></a>参考网址:</h1><p><a href="https://en.cppreference.com/w/cpp/11">C++11 - cppreference.com</a></p><h1 id="示例"><a href="#示例" class="headerlink" title="示例"></a>示例</h1><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br><span class="line">25</span><br><span class="line">26</span><br><span class="line">27</span><br><span class="line">28</span><br><span class="line">29</span><br><span class="line">30</span><br><span class="line">31</span><br><span class="line">32</span><br><span class="line">33</span><br><span class="line">34</span><br><span class="line">35</span><br><span class="line">36</span><br><span class="line">37</span><br><span class="line">38</span><br><span class="line">39</span><br><span class="line">40</span><br><span class="line">41</span><br><span class="line">42</span><br><span class="line">43</span><br><span class="line">44</span><br><span class="line">45</span><br><span class="line">46</span><br><span class="line">47</span><br><span class="line">48</span><br><span class="line">49</span><br><span class="line">50</span><br><span class="line">51</span><br><span class="line">52</span><br><span class="line">53</span><br><span class="line">54</span><br><span class="line">55</span><br><span class="line">56</span><br><span class="line">57</span><br><span class="line">58</span><br><span class="line">59</span><br><span class="line">60</span><br><span class="line">61</span><br><span class="line">62</span><br><span class="line">63</span><br><span class="line">64</span><br><span class="line">65</span><br><span class="line">66</span><br><span class="line">67</span><br><span class="line">68</span><br><span class="line">69</span><br><span class="line">70</span><br><span class="line">71</span><br><span class="line">72</span><br><span class="line">73</span><br><span class="line">74</span><br><span class="line">75</span><br><span class="line">76</span><br><span class="line">77</span><br><span class="line">78</span><br><span class="line">79</span><br><span class="line">80</span><br><span class="line">81</span><br><span class="line">82</span><br><span class="line">83</span><br><span class="line">84</span><br><span class="line">85</span><br><span class="line">86</span><br><span class="line">87</span><br><span class="line">88</span><br><span class="line">89</span><br><span class="line">90</span><br><span class="line">91</span><br><span class="line">92</span><br><span class="line">93</span><br><span class="line">94</span><br><span class="line">95</span><br><span class="line">96</span><br><span class="line">97</span><br><span class="line">98</span><br><span class="line">99</span><br><span class="line">100</span><br><span class="line">101</span><br><span class="line">102</span><br><span class="line">103</span><br><span class="line">104</span><br><span class="line">105</span><br><span class="line">106</span><br><span class="line">107</span><br><span class="line">108</span><br><span class="line">109</span><br><span class="line">110</span><br><span class="line">111</span><br><span class="line">112</span><br><span class="line">113</span><br><span class="line">114</span><br><span class="line">115</span><br><span class="line">116</span><br><span class="line">117</span><br><span class="line">118</span><br><span class="line">119</span><br><span class="line">120</span><br><span class="line">121</span><br><span class="line">122</span><br><span class="line">123</span><br><span class="line">124</span><br><span class="line">125</span><br><span class="line">126</span><br><span class="line">127</span><br><span class="line">128</span><br><span class="line">129</span><br><span class="line">130</span><br><span class="line">131</span><br><span class="line">132</span><br><span class="line">133</span><br><span class="line">134</span><br><span class="line">135</span><br><span class="line">136</span><br><span class="line">137</span><br><span class="line">138</span><br><span class="line">139</span><br><span class="line">140</span><br><span class="line">141</span><br><span class="line">142</span><br><span class="line">143</span><br><span class="line">144</span><br><span class="line">145</span><br><span class="line">146</span><br><span class="line">147</span><br><span class="line">148</span><br><span class="line">149</span><br><span class="line">150</span><br><span class="line">151</span><br><span class="line">152</span><br><span class="line">153</span><br><span class="line">154</span><br><span class="line">155</span><br><span class="line">156</span><br><span class="line">157</span><br><span class="line">158</span><br><span class="line">159</span><br><span class="line">160</span><br><span class="line">161</span><br><span class="line">162</span><br><span class="line">163</span><br><span class="line">164</span><br><span class="line">165</span><br><span class="line">166</span><br><span class="line">167</span><br><span class="line">168</span><br><span class="line">169</span><br><span class="line">170</span><br><span class="line">171</span><br><span class="line">172</span><br><span class="line">173</span><br><span class="line">174</span><br><span class="line">175</span><br><span class="line">176</span><br><span class="line">177</span><br><span class="line">178</span><br><span class="line">179</span><br><span class="line">180</span><br><span class="line">181</span><br><span class="line">182</span><br><span class="line">183</span><br><span class="line">184</span><br><span class="line">185</span><br><span class="line">186</span><br><span class="line">187</span><br><span class="line">188</span><br></pre></td><td class="code"><pre><span class="line"><span class="keyword">auto</span> <span class="keyword">and</span> <span class="keyword">decltype</span></span><br><span class="line">{</span><br><span class="line"><span class="keyword">int</span> x = <span class="number">8</span>;</span><br><span class="line"><span class="keyword">int</span> y = <span class="number">9</span>;</span><br><span class="line"><span class="keyword">auto</span> z = x + y; <span class="comment">// 一般用于类型过长</span></span><br><span class="line"> </span><br><span class="line"><span class="keyword">decltype</span>(z) a = x + y; <span class="comment">// 推导类型</span></span><br><span class="line"></span><br><span class="line"><span class="function"><span class="keyword">template</span><<span class="keyword">typename</span> A, <span class="keyword">typename</span> B></span></span><br><span class="line"><span class="function"><span class="title">decltype</span><span class="params">(a+b)</span> <span class="title">Add</span><span class="params">(A& a, B& b)</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"><span class="keyword">return</span> a + b;</span><br><span class="line">}</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line">trailing <span class="keyword">return</span> type</span><br><span class="line">{</span><br><span class="line"><span class="keyword">template</span><<span class="keyword">typename</span> T, <span class="keyword">typename</span> Y></span><br><span class="line"><span class="keyword">auto</span> <span class="built_in">add</span>(T v1, Y v2) -> <span class="keyword">decltype</span>(v1 + v2)</span><br><span class="line">{</span><br><span class="line"><span class="keyword">return</span> v1 + v2;</span><br><span class="line">}</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"></span><br><span class="line"><span class="keyword">override</span> <span class="keyword">and</span> <span class="keyword">final</span></span><br><span class="line">{</span><br><span class="line"><span class="class"><span class="keyword">struct</span> <span class="title">Base</span></span></span><br><span class="line"><span class="class">{</span></span><br><span class="line"><span class="function"><span class="keyword">virtual</span> <span class="keyword">void</span> <span class="title">func</span><span class="params">()</span></span>;</span><br><span class="line">};</span><br><span class="line"></span><br><span class="line"><span class="class"><span class="keyword">struct</span> <span class="title">A</span>:</span>Base</span><br><span class="line">{</span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">func</span><span class="params">()</span> <span class="keyword">final</span></span>;</span><br><span class="line">};</span><br><span class="line"></span><br><span class="line"><span class="class"><span class="keyword">struct</span> <span class="title">B</span>:</span>A</span><br><span class="line">{</span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">func</span><span class="params">()</span> <span class="keyword">override</span></span>; <span class="comment">// error</span></span><br><span class="line">};</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line">lvalue <span class="keyword">and</span> rvalue</span><br><span class="line">{</span><br><span class="line"><span class="keyword">int</span> x = <span class="number">8</span>; <span class="comment">// 坐标为左值右边为右值</span></span><br><span class="line">std::cout<<<span class="string">"x: "</span><<x<<std::endl;</span><br><span class="line">{</span><br><span class="line"><span class="number">1.</span> 左值可变,右值不可变(<span class="keyword">const</span> type)</span><br><span class="line"><span class="number">2.</span> 左值生存周期可以很长,右值只用一次</span><br><span class="line"><span class="number">3.</span> 左值储存在内存区,右值没有固定地址,用完即释放</span><br><span class="line"><span class="number">4.</span> 左值可以取地址,右值不可以</span><br><span class="line">}</span><br><span class="line">使用:</span><br><span class="line"><span class="keyword">int</span> a = <span class="number">8</span>;</span><br><span class="line"><span class="keyword">int</span> &x = a; <span class="comment">// 复制引用传参</span></span><br><span class="line"><span class="keyword">int</span> &&x = a; <span class="comment">// error</span></span><br><span class="line"><span class="keyword">int</span> &&x = <span class="number">8</span>; <span class="comment">// 移动右值传参</span></span><br><span class="line"></span><br><span class="line"><span class="keyword">const</span> <span class="keyword">int</span> &x = <span class="number">8</span>; <span class="comment">// ok, 但实际上是将8转化为临时变量, 然后左值引用</span></span><br><span class="line"></span><br><span class="line">有什么用:</span><br><span class="line"></span><br><span class="line"><span class="class"><span class="keyword">struct</span> <span class="title">A</span>{</span></span><br><span class="line"><span class="built_in">A</span>(<span class="keyword">int</span> &&x){</span><br><span class="line">cout<<<span class="string">"x: "</span><<x<<endl;</span><br><span class="line">}</span><br><span class="line">};</span><br><span class="line"></span><br><span class="line"><span class="function">A <span class="title">a</span><span class="params">(<span class="number">1</span>)</span></span>;</span><br><span class="line"></span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="keyword">default</span> <span class="keyword">and</span> <span class="keyword">delete</span></span><br><span class="line">{</span><br><span class="line"><span class="class"><span class="keyword">class</span> <span class="title">A</span></span></span><br><span class="line"><span class="class">{</span></span><br><span class="line"><span class="built_in">A</span>() = <span class="keyword">default</span>;</span><br><span class="line">~<span class="built_in">A</span>() = <span class="keyword">default</span>;</span><br><span class="line"><span class="built_in">A</span>(<span class="keyword">const</span> A&) = <span class="keyword">default</span>; <span class="comment">// 复制传参</span></span><br><span class="line"><span class="built_in">A</span>(A&&) = <span class="keyword">default</span>; <span class="comment">// 移动传参</span></span><br><span class="line">A& <span class="keyword">operator</span>=(<span class="keyword">const</span> A&) = <span class="keyword">default</span>; <span class="comment">// 可以连等</span></span><br><span class="line">A& <span class="keyword">operator</span>=(A&&) = <span class="keyword">default</span>; </span><br><span class="line">};</span><br><span class="line"></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">func</span><span class="params">()</span> </span>= <span class="keyword">delete</span>;</span><br><span class="line"></span><br><span class="line"><span class="class"><span class="keyword">class</span> <span class="title">B</span></span></span><br><span class="line"><span class="class">{</span></span><br><span class="line">B& <span class="keyword">operator</span>=(<span class="keyword">const</span> B&) = <span class="keyword">delete</span>;</span><br><span class="line">}</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line">move constructors</span><br><span class="line">{</span><br><span class="line"><span class="class"><span class="keyword">struct</span> <span class="title">B</span>;</span></span><br><span class="line">B b;</span><br><span class="line">B a = std::<span class="built_in">move</span>(b);</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"></span><br><span class="line">scoped enums</span><br><span class="line">{</span><br><span class="line"><span class="class"><span class="keyword">enum</span> <span class="keyword">class</span> :</span> <span class="keyword">int</span>{</span><br><span class="line">a = <span class="number">0</span>,</span><br><span class="line">b,</span><br><span class="line">c,</span><br><span class="line">d</span><br><span class="line">};</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="keyword">constexpr</span></span><br><span class="line">{</span><br><span class="line"><span class="keyword">constexpr</span> <span class="keyword">int</span> x = <span class="number">8</span>; <span class="comment">// 常量</span></span><br><span class="line">}</span><br><span class="line"></span><br><span class="line">delegating <span class="keyword">and</span> inherited constructors</span><br><span class="line">{</span><br><span class="line"><span class="class"><span class="keyword">struct</span> <span class="title">A</span></span></span><br><span class="line"><span class="class">{</span></span><br><span class="line"><span class="built_in">A</span>(<span class="keyword">const</span> in& x, <span class="keyword">const</span> <span class="keyword">int</span>& y)</span><br><span class="line">:<span class="built_in">m_x</span>(x),<span class="built_in">m_y</span>(y)</span><br><span class="line">{}</span><br><span class="line"></span><br><span class="line"><span class="built_in">A</span>(<span class="keyword">const</span> <span class="keyword">int</span>& y)</span><br><span class="line">:<span class="built_in">A</span>(<span class="number">2</span>,y)</span><br><span class="line">{}</span><br><span class="line"><span class="keyword">private</span>:</span><br><span class="line"><span class="keyword">int</span> m_x;</span><br><span class="line"><span class="keyword">int</span> m_y;</span><br><span class="line">};</span><br><span class="line"></span><br><span class="line"><span class="class"><span class="keyword">struct</span> <span class="title">B</span> :</span> A&{</span><br><span class="line"><span class="built_in">B</span>(<span class="keyword">const</span> in& x, <span class="keyword">const</span> <span class="keyword">int</span>& y)</span><br><span class="line">:<span class="built_in">A</span>(x, y)</span><br><span class="line">{}</span><br><span class="line">};</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line">type aliases</span><br><span class="line">{</span><br><span class="line"><span class="keyword">using</span> Degree = Angle<is_degree>;</span><br><span class="line"><span class="keyword">using</span> Radian = Angle<is_radian>;</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line">variadic templates </span><br><span class="line">{</span><br><span class="line"><span class="keyword">template</span><class... Types></span><br><span class="line"><span class="class"><span class="keyword">struct</span> <span class="title">Tuple</span> {</span>};</span><br><span class="line"> </span><br><span class="line">Tuple<> t0; <span class="comment">// Types contains no arguments</span></span><br><span class="line">Tuple<<span class="keyword">int</span>> t1; <span class="comment">// Types contains one argument: int</span></span><br><span class="line">Tuple<<span class="keyword">int</span>, <span class="keyword">float</span>> t2; <span class="comment">// Types contains two arguments: int and float</span></span><br><span class="line">Tuple<<span class="number">0</span>> t3; <span class="comment">// error: 0 is not a type</span></span><br><span class="line"></span><br><span class="line"><span class="function"><span class="keyword">template</span><class... Types></span></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">f</span><span class="params">(Types... args)</span></span>; <span class="comment">// </span></span><br><span class="line"> </span><br><span class="line"><span class="built_in">f</span>(); <span class="comment">// OK: args contains no arguments</span></span><br><span class="line"><span class="built_in">f</span>(<span class="number">1</span>); <span class="comment">// OK: args contains one argument: int</span></span><br><span class="line"><span class="built_in">f</span>(<span class="number">2</span>, <span class="number">1.0</span>); <span class="comment">// OK: args contains two arguments: int and double</span></span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"></span><br><span class="line">lambda</span><br><span class="line">{</span><br><span class="line"><span class="keyword">auto</span> a = []()->type{};</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line">range-<span class="keyword">for</span></span><br><span class="line">{</span><br><span class="line"><span class="keyword">int</span> a[<span class="number">4</span>] = {<span class="number">1</span>, <span class="number">2</span>, <span class="number">3</span>, <span class="number">4</span>};</span><br><span class="line"><span class="keyword">for</span>(<span class="keyword">int</span> i : a)</span><br><span class="line">{</span><br><span class="line">cout<<<span class="string">" "</span><<i;</span><br><span class="line">}</span><br><span class="line">cout<<endl;</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="keyword">static_assert</span></span><br><span class="line">{</span><br><span class="line"><span class="built_in"><span class="keyword">static_assert</span></span>(<span class="literal">true</span>, <span class="string">"success pass!"</span>);</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="keyword">alignof</span></span><br><span class="line">{</span><br><span class="line"><span class="function">std::<span class="keyword">size_t</span> <span class="title">alignof</span><span class="params">(type)</span></span></span><br><span class="line"><span class="function">}</span></span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址:"></a>参考网址:</h1><p><a href="https://en.cppreference.com/w/cpp/11">C++11 - cppre</summary>
<category term="语言" scheme="https://peitianyu.github.io/blog/categories/%E8%AF%AD%E8%A8%80/"/>
<category term="cpp" scheme="https://peitianyu.github.io/blog/categories/%E8%AF%AD%E8%A8%80/cpp/"/>
<category term="cpp" scheme="https://peitianyu.github.io/blog/tags/cpp/"/>
</entry>
<entry>
<title>lite_slam - 文章索引</title>
<link href="https://peitianyu.github.io/blog/2022/09/05/lite_slam-%E6%96%87%E7%AB%A0%E7%B4%A2%E5%BC%95/"/>
<id>https://peitianyu.github.io/blog/2022/09/05/lite_slam-%E6%96%87%E7%AB%A0%E7%B4%A2%E5%BC%95/</id>
<published>2022-09-05T04:30:12.000Z</published>
<updated>2022-11-30T05:44:57.393Z</updated>
<content type="html"><![CDATA[<h1 id="文章索引"><a href="#文章索引" class="headerlink" title="文章索引"></a>文章索引</h1><p><a href="https://blog.csdn.net/qq_30554779/article/details/126752121?spm=1001.2014.3001.5502">从零开始构建2d激光slam-0.整体框架简介</a></p><p><a href="https://blog.csdn.net/qq_30554779/article/details/126752232?spm=1001.2014.3001.5502">从零开始构建2d激光slam-1.概率地图</a></p><p><a href="https://blog.csdn.net/qq_30554779/article/details/126752249?spm=1001.2014.3001.5502">从零开始构建2d激光slam-2.激光里程计</a></p><p><a href="https://blog.csdn.net/qq_30554779/article/details/126752338?spm=1001.2014.3001.5502">从零开始构建2d激光slam-3.回环检测</a></p><p><a href="https://blog.csdn.net/qq_30554779/article/details/126752287?spm=1001.2014.3001.5502">从零开始构建2d激光slam-4.图优化</a></p>]]></content>
<summary type="html"><h1 id="文章索引"><a href="#文章索引" class="headerlink" title="文章索引"></a>文章索引</h1><p><a href="https://blog.csdn.net/qq_30554779/article/details/126</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="lite_slam" scheme="https://peitianyu.github.io/blog/categories/slam/lite-slam/"/>
<category term="lite_slam" scheme="https://peitianyu.github.io/blog/tags/lite-slam/"/>
</entry>
<entry>
<title>lite_slam - 搭建测试环境</title>
<link href="https://peitianyu.github.io/blog/2022/09/05/lite_slam0%20-%201/"/>
<id>https://peitianyu.github.io/blog/2022/09/05/lite_slam0%20-%201/</id>
<published>2022-09-05T04:30:12.000Z</published>
<updated>2022-11-30T05:44:56.348Z</updated>
<content type="html"><![CDATA[<h1 id="环境"><a href="#环境" class="headerlink" title="环境"></a>环境</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">- Ubuntu18</span><br><span class="line">- Eigen</span><br><span class="line">- opencv(仅用于显示)</span><br></pre></td></tr></table></figure><h1 id="安装环境"><a href="#安装环境" class="headerlink" title="安装环境"></a>安装环境</h1><figure class="highlight shell"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br></pre></td><td class="code"><pre><span class="line"><span class="meta">#</span><span class="bash"> g++ / gcc</span></span><br><span class="line">sudo apt install g++</span><br><span class="line"><span class="meta">#</span><span class="bash"> cmake</span></span><br><span class="line">sudo apt install cmake</span><br><span class="line"><span class="meta">#</span><span class="bash"> Eigen</span></span><br><span class="line"> #1.安装：</span><br><span class="line"> sudo apt-get install libeigen3-dev</span><br><span class="line"> # 2 调整，默认安装路径是：/usr/include/eigen3 </span><br><span class="line"> cd /usr/include/eigen3</span><br><span class="line"> ls</span><br><span class="line"> # $ Eigen signature_of_eigen3_matrix_library unsupported </span><br><span class="line"> # 3.需要执行复制命令，将Eigen文件夹放在/usr/include 下面</span><br><span class="line"> sudo cp Eigen/ .. -R</span><br><span class="line"><span class="meta">#</span><span class="bash"> opencv</span></span><br><span class="line">sudo apt-get install libcv-dev</span><br><span class="line">sudo apt-get install libopencv-dev</span><br></pre></td></tr></table></figure><h1 id="安装测试数据"><a href="#安装测试数据" class="headerlink" title="安装测试数据"></a>安装测试数据</h1><h2 id="使用测试data"><a href="#使用测试data" class="headerlink" title="使用测试data"></a>使用测试data</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br></pre></td><td class="code"><pre><span class="line">1. 百度云下载:</span><br><span class="line">链接：https://pan.baidu.com/s/19gm9azasi70CXTWbQ7dGrw?pwd=jned </span><br><span class="line">提取码：jned </span><br><span class="line">2. 将其复制于文件夹中</span><br><span class="line">3. mkdir build</span><br><span class="line">4. cd build && cmake ..</span><br><span class="line">5. make -j4</span><br></pre></td></tr></table></figure><h2 id="文件夹结构"><a href="#文件夹结构" class="headerlink" title="文件夹结构"></a>文件夹结构</h2><p><a href="https://imgse.com/i/vLMkuQ"><img src="https://s1.ax1x.com/2022/09/09/vLMkuQ.png" alt="vLMkuQ.png"></a></p><h2 id="使用自己的rosbag"><a href="#使用自己的rosbag" class="headerlink" title="使用自己的rosbag"></a>使用自己的rosbag</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br></pre></td><td class="code"><pre><span class="line"># 下载下来</span><br><span class="line">cd ~/catkin_ws/src</span><br><span class="line">git clone https://ghproxy.com/https://github.com/peitianyu/bag2txt</span><br><span class="line">cd .. && catkin_make</span><br><span class="line"># 修改launch文件,并运行</span><br><span class="line"><launch></span><br><span class="line"> <node name="odom2txt" pkg="ros_data2txt" type="odom2txt" respawn="false" output="screen" ></span><br><span class="line"> <param name="file_name" type="string" value="/mnt/d/file_ws/Learning/ros/bag2txt_ws/log/9_2_odom.txt"/> // 修改</span><br><span class="line"> <param name="topic_name" type="string" value="/odom"/> </span><br><span class="line"> </node></span><br><span class="line"></span><br><span class="line"> <node name="laser_scan2txt" pkg="ros_data2txt" type="laser_scan2txt" respawn="false" output="screen" ></span><br><span class="line"> <param name="file_name" type="string" value="/mnt/d/file_ws/Learning/ros/bag2txt_ws/log/9_2_scan.txt"/> // 修改</span><br><span class="line"> <param name="topic_name" type="string" value="/scan"/> </span><br><span class="line"> </node></span><br><span class="line"></span><br><span class="line"> <node name="playbag" pkg="rosbag" type="play"</span><br><span class="line"> args="--clock /mnt/d/file_ws/Learning/ros/bag2txt_ws/bag/scan_odom_2022_9_2.bag" /> // 修改</span><br><span class="line"></launch></span><br><span class="line"></span><br><span class="line"># 运行</span><br><span class="line">roslaunch ros_data2txt bag2txt.launch 自动生成</span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="环境"><a href="#环境" class="headerlink" title="环境"></a>环境</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><s</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="lite_slam" scheme="https://peitianyu.github.io/blog/categories/slam/lite-slam/"/>
<category term="lite_slam" scheme="https://peitianyu.github.io/blog/tags/lite-slam/"/>
</entry>
<entry>
<title>lite_slam - grid_map模块</title>
<link href="https://peitianyu.github.io/blog/2022/09/05/lite_slam1/"/>
<id>https://peitianyu.github.io/blog/2022/09/05/lite_slam1/</id>
<published>2022-09-05T04:30:12.000Z</published>
<updated>2022-11-30T05:44:56.651Z</updated>
<content type="html"><![CDATA[<h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址:"></a>参考网址:</h1><p><a href="https://github.com/peitianyu/lite_slam">peitianyu/lite_slam (github.com)</a></p><h1 id="GridMap框架"><a href="#GridMap框架" class="headerlink" title="GridMap框架"></a>GridMap框架</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">1. GridMapBase作为基类派生GridMapDownSamlple,GridMapUtils</span><br><span class="line">2. GridManage用于读取,显示,保存GridMap</span><br><span class="line">3. GridSubMap实现子图功能</span><br></pre></td></tr></table></figure><h1 id="GridMapBase"><a href="#GridMapBase" class="headerlink" title="GridMapBase"></a>GridMapBase</h1><h3 id="参考网址-1"><a href="#参考网址-1" class="headerlink" title="参考网址"></a>参考网址</h3><p><a href="https://zhuanlan.zhihu.com/p/339812617">如何理解概率栅格地图（Probability Grid Map）& 概率更新公式 - 知乎 (zhihu.com)</a></p><h3 id="配置文件"><a href="#配置文件" class="headerlink" title="配置文件"></a>配置文件</h3><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br></pre></td><td class="code"><pre><span class="line"><span class="comment">// 关于配置参数</span></span><br><span class="line"><span class="class"><span class="keyword">struct</span> <span class="title">Params</span></span></span><br><span class="line"><span class="class">{</span></span><br><span class="line"> <span class="keyword">float</span> resolution = <span class="number">0.05</span>; <span class="comment">// meter / pixel</span></span><br><span class="line"> Eigen::Vector2i size = Eigen::<span class="built_in">Vector2i</span>(<span class="number">2000</span>, <span class="number">2000</span>); <span class="comment">//地图尺寸</span></span><br><span class="line"> Eigen::Vector2f origin = Eigen::<span class="built_in">Vector2f</span>(<span class="number">-60.0f</span>, <span class="number">-60.0f</span>); <span class="comment">// 右上角为(0, 0)的建图原点</span></span><br><span class="line"> <span class="keyword">float</span> log_odds_p_occ = <span class="number">0.6f</span>; <span class="comment">// 更新地图格式为log(odds), 遇到障碍物会增加此值,反之减小此值,具体接下来代码与文献有讲解</span></span><br><span class="line"> <span class="keyword">float</span> log_odds_p_free = <span class="number">0.4f</span>;</span><br><span class="line">};</span><br></pre></td></tr></table></figure><h3 id="对于概率图理解"><a href="#对于概率图理解" class="headerlink" title="对于概率图理解"></a>对于概率图理解</h3><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br></pre></td><td class="code"><pre><span class="line">Q: 为何使用log-odds表示概率图</span><br><span class="line">A: 由于激光打在障碍物上的概率符合正太分布,因此可以写成标准正太分布形式,</span><br><span class="line"> 又由于exp()相乘计算复杂,所以将其保存为log-odds形式进行加法运算,加快计算速度.</span><br><span class="line"> 当然还有一个考虑,在log-odds在0附近变化剧烈,这样可以尽快将占用与空闲分开</span><br></pre></td></tr></table></figure><h3 id="代码体现"><a href="#代码体现" class="headerlink" title="代码体现"></a>代码体现</h3><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br></pre></td><td class="code"><pre><span class="line"><span class="comment">// 保存格式为MatrixXf m_data</span></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">GridMapBase::SetCellOccupied</span><span class="params">(<span class="keyword">const</span> Eigen::Vector2i& cell_index)</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> <span class="keyword">if</span>(<span class="built_in">IsValid</span>(cell_index))</span><br><span class="line"> <span class="built_in">m_data</span>(<span class="built_in">cell_index</span>(<span class="number">0</span>), <span class="built_in">cell_index</span>(<span class="number">1</span>)) += m_params.log_odds_p_occ; <span class="comment">// 只是对对应位置的log-odds进行加减操作,减小了计算量</span></span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">GridMapBase::SetCellFree</span><span class="params">(<span class="keyword">const</span> Eigen::Vector2i& cell_index)</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> <span class="keyword">if</span>(<span class="built_in">IsValid</span>(cell_index))</span><br><span class="line"> <span class="built_in">m_data</span>(<span class="built_in">cell_index</span>(<span class="number">0</span>), <span class="built_in">cell_index</span>(<span class="number">1</span>)) -= m_params.log_odds_p_free; <span class="comment">// 只是对对应位置的log-odds进行加减操作,减小了计算量</span></span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="function"><span class="keyword">float</span> <span class="title">GridMapBase::GetCellProb</span><span class="params">(<span class="keyword">const</span> Eigen::Vector2i& cell_index)</span> <span class="keyword">const</span> </span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> <span class="keyword">if</span>(!<span class="built_in">IsValid</span>(cell_index)) <span class="keyword">return</span> <span class="number">0.5f</span>;</span><br><span class="line"></span><br><span class="line"> <span class="keyword">float</span> odds = std::<span class="built_in">exp</span>(<span class="built_in">GetCellLogOdds</span>(cell_index)); <span class="comment">// log-odds2odds</span></span><br><span class="line"> <span class="keyword">float</span> prob = (odds / (odds + <span class="number">1</span>)); <span class="comment">// odds2prob</span></span><br><span class="line"> <span class="keyword">if</span>(std::<span class="built_in">isnan</span>(prob)) <span class="keyword">return</span> <span class="number">0</span>;</span><br><span class="line"> <span class="keyword">return</span> prob;</span><br><span class="line">}</span><br></pre></td></tr></table></figure><h3 id="对于map的常见工具"><a href="#对于map的常见工具" class="headerlink" title="对于map的常见工具"></a>对于map的常见工具</h3><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br></pre></td><td class="code"><pre><span class="line"><span class="comment">// 获得非0最大矩阵</span></span><br><span class="line"><span class="function">Eigen::Matrix2i <span class="title">GridMapBase::GetMapLimit</span><span class="params">()</span> <span class="keyword">const</span> <span class="comment">// top-bottom, left-right</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> Eigen::Matrix2i map_limit = Eigen::Matrix2i::<span class="built_in">Zero</span>();</span><br><span class="line"> map_limit << m_params.<span class="built_in">size</span>(<span class="number">1</span>) , <span class="number">0</span>, m_params.<span class="built_in">size</span>(<span class="number">0</span>) , <span class="number">0</span>; </span><br><span class="line"></span><br><span class="line"> <span class="keyword">for</span>(<span class="keyword">int</span> i = <span class="number">0</span>; i < m_params.<span class="built_in">size</span>(<span class="number">0</span>); i++){</span><br><span class="line"> <span class="keyword">for</span>(<span class="keyword">int</span> j = <span class="number">0</span>; j < m_params.<span class="built_in">size</span>(<span class="number">1</span>); j++){</span><br><span class="line"> <span class="keyword">if</span>(<span class="built_in">fabs</span>(<span class="built_in">m_data</span>(i,j)) > <span class="number">1.0f</span>){</span><br><span class="line"> <span class="keyword">if</span>(i < <span class="built_in">map_limit</span>(<span class="number">0</span>,<span class="number">0</span>)) {<span class="built_in">map_limit</span>(<span class="number">0</span>,<span class="number">0</span>) = i;}</span><br><span class="line"> <span class="keyword">if</span>(i > <span class="built_in">map_limit</span>(<span class="number">0</span>,<span class="number">1</span>)) {<span class="built_in">map_limit</span>(<span class="number">0</span>,<span class="number">1</span>) = i;}</span><br><span class="line"> <span class="keyword">if</span>(j < <span class="built_in">map_limit</span>(<span class="number">1</span>,<span class="number">0</span>)) {<span class="built_in">map_limit</span>(<span class="number">1</span>,<span class="number">0</span>) = j;}</span><br><span class="line"> <span class="keyword">if</span>(j > <span class="built_in">map_limit</span>(<span class="number">1</span>,<span class="number">1</span>)) {<span class="built_in">map_limit</span>(<span class="number">1</span>,<span class="number">1</span>) = j;}</span><br><span class="line"> }</span><br><span class="line"> }</span><br><span class="line"> }</span><br><span class="line"> <span class="keyword">return</span> map_limit;</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="comment">// 判断是否合法</span></span><br><span class="line"><span class="function"><span class="keyword">bool</span> <span class="title">GridMapBase::IsValid</span><span class="params">(<span class="keyword">const</span> Eigen::Vector2i& cell_index)</span> <span class="keyword">const</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> <span class="keyword">return</span> (<span class="built_in">cell_index</span>(<span class="number">0</span>) >= <span class="number">0</span> && <span class="built_in">cell_index</span>(<span class="number">0</span>) < m_params.<span class="built_in">size</span>(<span class="number">0</span>) && <span class="built_in">cell_index</span>(<span class="number">1</span>) >= <span class="number">0</span> && <span class="built_in">cell_index</span>(<span class="number">1</span>) < m_params.<span class="built_in">size</span>(<span class="number">1</span>));</span><br><span class="line">}</span><br></pre></td></tr></table></figure><h1 id="GridMapUtils"><a href="#GridMapUtils" class="headerlink" title="GridMapUtils"></a>GridMapUtils</h1><h2 id="框架理解"><a href="#框架理解" class="headerlink" title="框架理解"></a>框架理解</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">1. 更新概率地图 - bresenham(好处: 只用了整形加减,降低计算量)</span><br><span class="line">2. 世界坐标与地图坐标相互转换</span><br></pre></td></tr></table></figure><h2 id="更新概率地图"><a href="#更新概率地图" class="headerlink" title="更新概率地图"></a>更新概率地图</h2><h3 id="代码框架"><a href="#代码框架" class="headerlink" title="代码框架"></a>代码框架</h3><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br></pre></td><td class="code"><pre><span class="line">输入: 当前robot_in_world_pose与scan_points</span><br><span class="line">0. robot_in_world_pose转化为地图坐标作为起始点</span><br><span class="line">1. 对于每一个scan_point转化为世界坐标作为终点</span><br><span class="line">2. 使用bresenham画线</span><br></pre></td></tr></table></figure><h3 id="参考网址-2"><a href="#参考网址-2" class="headerlink" title="参考网址"></a>参考网址</h3><p><a href="https://www.jianshu.com/p/d63bf63a0e28">Bresenham画线算法 - 简书 (jianshu.com)</a></p><h3 id="代码体现-1"><a href="#代码体现-1" class="headerlink" title="代码体现"></a>代码体现</h3><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br><span class="line">25</span><br><span class="line">26</span><br><span class="line">27</span><br><span class="line">28</span><br><span class="line">29</span><br><span class="line">30</span><br><span class="line">31</span><br><span class="line">32</span><br><span class="line">33</span><br><span class="line">34</span><br><span class="line">35</span><br><span class="line">36</span><br><span class="line">37</span><br><span class="line">38</span><br><span class="line">39</span><br><span class="line">40</span><br><span class="line">41</span><br><span class="line">42</span><br><span class="line">43</span><br><span class="line">44</span><br><span class="line">45</span><br><span class="line">46</span><br><span class="line">47</span><br><span class="line">48</span><br><span class="line">49</span><br><span class="line">50</span><br><span class="line">51</span><br></pre></td><td class="code"><pre><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">GridMapUtils::UpdateByScan</span><span class="params">(<span class="keyword">const</span> Eigen::Vector3f& pose_in_world, <span class="keyword">const</span> std::vector<Eigen::Vector2f>& scan_points)</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line">Eigen::Vector3f pose_in_map = <span class="built_in">WorldToMapFloat</span>(pose_in_world); <span class="comment">// 转化为地图坐标</span></span><br><span class="line">Eigen::Vector2i begin_point_in_map_int = Eigen::<span class="built_in">Vector2i</span>(<span class="built_in">pose_in_map</span>(<span class="number">0</span>), <span class="built_in">pose_in_map</span>(<span class="number">1</span>));</span><br><span class="line"></span><br><span class="line"><span class="keyword">for</span> (Eigen::Vector2f point : scan_points)</span><br><span class="line">{</span><br><span class="line">Eigen::Vector2f end_point_in_world = <span class="built_in">LaserPointToWorld</span>(point, pose_in_world);</span><br><span class="line"></span><br><span class="line">Eigen::Vector2f end_point_in_map = <span class="built_in">WorldToMapFloat</span>(end_point_in_world);</span><br><span class="line"></span><br><span class="line"><span class="function">Eigen::Vector2i <span class="title">end_point_in_map_int</span><span class="params">(<span class="keyword">static_cast</span><<span class="keyword">int</span>>( ::round(end_point_in_map(<span class="number">0</span>))), <span class="keyword">static_cast</span><<span class="keyword">int</span>>( ::round(end_point_in_map(<span class="number">1</span>))))</span></span>; <span class="comment">// 转化为世界坐标系下的地图坐标</span></span><br><span class="line"></span><br><span class="line"><span class="keyword">if</span>(begin_point_in_map_int != end_point_in_map_int)</span><br><span class="line"><span class="built_in">InverseModel</span>(begin_point_in_map_int, end_point_in_map_int); <span class="comment">// bresenham画线</span></span><br><span class="line">}</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">GridMapUtils::InverseModel</span><span class="params">(<span class="keyword">const</span> Eigen::Vector2i &p0, <span class="keyword">const</span> Eigen::Vector2i &p1)</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"><span class="built_in">BresenhamCellOccupied</span>(p1); <span class="comment">// 更新占用</span></span><br><span class="line"></span><br><span class="line"><span class="built_in">BresenhamCellFree</span>(p0, p1); <span class="comment">// 更新空闲</span></span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">GridMapUtils::BrasenHam</span><span class="params">(<span class="keyword">int</span> x0, <span class="keyword">int</span> y0, <span class="keyword">int</span> x1, <span class="keyword">int</span> y1)</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"><span class="keyword">int</span> dx = ::<span class="built_in">abs</span>( x1 - x0 );</span><br><span class="line"><span class="keyword">int</span> dy = ::<span class="built_in">abs</span>( y1 - y0 );</span><br><span class="line"><span class="keyword">bool</span> inter_change = <span class="literal">false</span>;</span><br><span class="line"><span class="keyword">int</span> e = -dx;<span class="comment">// error</span></span><br><span class="line"><span class="keyword">int</span> signX = x1 > x0 ? <span class="number">1</span> : ( ( x1 < x0 ) ? <span class="number">-1</span> : <span class="number">0</span> );</span><br><span class="line"><span class="keyword">int</span> signY = y1 > y0 ? <span class="number">1</span> : ( ( y1 < y0 ) ? <span class="number">-1</span> : <span class="number">0</span> );</span><br><span class="line"><span class="keyword">if</span> (dy > dx) {</span><br><span class="line"><span class="keyword">int</span> temp = dx; dx = dy; dy = temp; inter_change = <span class="literal">true</span>;</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="keyword">int</span> x = x0, y = y0;</span><br><span class="line"><span class="keyword">for</span> (<span class="keyword">int</span> i = <span class="number">1</span>; i <= dx; i++) { </span><br><span class="line">m_down_map-><span class="built_in">SetCellFree</span>(Eigen::<span class="built_in">Vector2i</span>(x, y)); <span class="comment">// 更新空闲</span></span><br><span class="line"></span><br><span class="line"><span class="keyword">if</span> (!inter_change) {x += signX;}</span><br><span class="line"><span class="keyword">else</span> {y += signY;}</span><br><span class="line">e += <span class="number">2</span> * dy;</span><br><span class="line"><span class="keyword">if</span> (e >= <span class="number">0</span>) {</span><br><span class="line"><span class="keyword">if</span> (!inter_change) {y += signY;}</span><br><span class="line"><span class="keyword">else</span> {x += signX;}</span><br><span class="line">e -= <span class="number">2</span> * dx;</span><br><span class="line">}</span><br><span class="line">}</span><br><span class="line">}</span><br></pre></td></tr></table></figure><h2 id="世界坐标与地图坐标相互转换"><a href="#世界坐标与地图坐标相互转换" class="headerlink" title="世界坐标与地图坐标相互转换"></a>世界坐标与地图坐标相互转换</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br></pre></td><td class="code"><pre><span class="line">1. 根据对地图的定义,实现世界坐标与地图坐标转换</span><br><span class="line">2. 优化: 填加根据不同地图方向转化坐标的功能</span><br></pre></td></tr></table></figure><h3 id="代码实现-没啥技术含量-直接看源码"><a href="#代码实现-没啥技术含量-直接看源码" class="headerlink" title="代码实现(没啥技术含量, 直接看源码)"></a>代码实现(没啥技术含量, 直接看源码)</h3><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br></pre></td><td class="code"><pre><span class="line"><span class="function">Eigen::Vector2f <span class="title">LaserInScaledLaser</span><span class="params">(<span class="keyword">const</span> Eigen::Vector2f& laser_point)</span></span>;</span><br><span class="line"></span><br><span class="line"><span class="function">Eigen::Vector2f <span class="title">LaserPointToWorld</span><span class="params">(<span class="keyword">const</span> Eigen::Vector2f& point_in_laser, <span class="keyword">const</span> Eigen::Vector3f& pose_in_world)</span></span>;</span><br><span class="line"></span><br><span class="line"><span class="function">Eigen::Vector3f <span class="title">WorldToMapFloat</span><span class="params">(<span class="keyword">const</span> Eigen::Vector3f& pose_in_world)</span> <span class="keyword">const</span></span>; </span><br><span class="line"></span><br><span class="line"><span class="function">Eigen::Vector2f <span class="title">WorldToMapFloat</span><span class="params">(<span class="keyword">const</span> Eigen::Vector2f& point_in_world)</span> <span class="keyword">const</span></span>;</span><br><span class="line"></span><br><span class="line"><span class="function">Eigen::Vector3f <span class="title">MapToWorldFloat</span><span class="params">(<span class="keyword">const</span> Eigen::Vector3f& pose_in_map)</span> <span class="keyword">const</span></span>;</span><br></pre></td></tr></table></figure><h1 id="GridMapDownSamlple"><a href="#GridMapDownSamlple" class="headerlink" title="GridMapDownSamlple"></a>GridMapDownSamlple</h1><h2 id="降采样地图理解"><a href="#降采样地图理解" class="headerlink" title="降采样地图理解"></a>降采样地图理解</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">0. 继承GridMapBase</span><br><span class="line">1. 对上层地图长宽降采样为原来一半</span><br><span class="line">2. 采用上层地图四方格log-odds中最大的作为下层地图log-odds值(可根据自己需要定义,比如采用高斯核降采样等)</span><br></pre></td></tr></table></figure><h3 id="代码体现-2"><a href="#代码体现-2" class="headerlink" title="代码体现"></a>代码体现</h3><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br><span class="line">25</span><br><span class="line">26</span><br><span class="line">27</span><br><span class="line">28</span><br><span class="line">29</span><br><span class="line">30</span><br><span class="line">31</span><br><span class="line">32</span><br><span class="line">33</span><br><span class="line">34</span><br><span class="line">35</span><br></pre></td><td class="code"><pre><span class="line">DownSampleMap::<span class="built_in">DownSampleMap</span>(<span class="keyword">const</span> Params& p)</span><br><span class="line">:<span class="built_in">GridMapBase</span>(p)</span><br><span class="line">{}</span><br><span class="line"></span><br><span class="line"><span class="comment">// 若输入原始地图,则将其作为降采样地图 </span></span><br><span class="line">DownSampleMap::<span class="built_in">DownSampleMap</span>(<span class="keyword">const</span> GridMapBase& grid_map)</span><br><span class="line">{</span><br><span class="line"> m_params = grid_map.<span class="built_in">GetParams</span>();</span><br><span class="line"> m_data.<span class="built_in">setZero</span>();</span><br><span class="line"> m_data = grid_map.<span class="built_in">GetData</span>();</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="comment">// 若输入降采样地图,则将其降采样 </span></span><br><span class="line">DownSampleMap::<span class="built_in">DownSampleMap</span>(<span class="keyword">const</span> DownSampleMap& down_map)</span><br><span class="line">{</span><br><span class="line"> <span class="comment">// 重新设置配置文件</span></span><br><span class="line"> m_params = down_map.<span class="built_in">GetParams</span>();</span><br><span class="line"> m_params.resolution = down_map.<span class="built_in">GetResolution</span>() * <span class="number">2.0</span>;</span><br><span class="line"> m_params.size = Eigen::<span class="built_in">Vector2i</span>(<span class="keyword">static_cast</span><<span class="keyword">int</span>>(down_map.<span class="built_in">GetSize</span>()(<span class="number">0</span>) / <span class="number">2.0</span> + <span class="number">0.5</span>), <span class="keyword">static_cast</span><<span class="keyword">int</span>>(down_map.<span class="built_in">GetSize</span>()(<span class="number">1</span>) / <span class="number">2.0</span> + <span class="number">0.5</span>));</span><br><span class="line"> </span><br><span class="line"> <span class="comment">// 根据配置文件重新设置地图大小,并初始化</span></span><br><span class="line"> m_data.<span class="built_in">resize</span>(m_params.<span class="built_in">size</span>(<span class="number">0</span>), m_params.<span class="built_in">size</span>(<span class="number">1</span>)); </span><br><span class="line"> m_data.<span class="built_in">setConstant</span>(<span class="number">0</span>); </span><br><span class="line"></span><br><span class="line"> <span class="keyword">for</span> (<span class="keyword">int</span> row = <span class="number">0</span>; row < m_params.<span class="built_in">size</span>(<span class="number">0</span>); row++){ <span class="comment">// 实现了四个方格中取最大值操作</span></span><br><span class="line"> <span class="keyword">for</span> (<span class="keyword">int</span> col = <span class="number">0</span>; col < m_params.<span class="built_in">size</span>(<span class="number">1</span>); col++){</span><br><span class="line"> <span class="keyword">for</span> (<span class="keyword">int</span> i = <span class="number">0</span>; i < <span class="number">2</span>; i++){</span><br><span class="line"> <span class="keyword">for</span> (<span class="keyword">int</span> j = <span class="number">0</span>; j < <span class="number">2</span>; j++){</span><br><span class="line"> <span class="built_in">m_data</span>(row, col) = (<span class="built_in">m_data</span>(row, col) < <span class="built_in">fabs</span>(down_map.<span class="built_in">GetCellLogOdds</span>(Eigen::<span class="built_in">Vector2i</span>(<span class="number">2</span> * row + i, <span class="number">2</span> * col + j)))) ? </span><br><span class="line"> down_map.<span class="built_in">GetCellLogOdds</span>(Eigen::<span class="built_in">Vector2i</span>(<span class="number">2</span> * row + i, <span class="number">2</span> * col + j)):<span class="built_in">m_data</span>(row, col); </span><br><span class="line"> } </span><br><span class="line"> } </span><br><span class="line"> }</span><br><span class="line"> }</span><br><span class="line">}</span><br></pre></td></tr></table></figure><h1 id="GridManage"><a href="#GridManage" class="headerlink" title="GridManage"></a>GridManage</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line">主要用于保存,显示,读取概率地图</span><br></pre></td></tr></table></figure><h3 id="代码实现-没啥技术含量-直接看源码-1"><a href="#代码实现-没啥技术含量-直接看源码-1" class="headerlink" title="代码实现(没啥技术含量, 直接看源码)"></a>代码实现(没啥技术含量, 直接看源码)</h3><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br></pre></td><td class="code"><pre><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">SaveGridMap</span><span class="params">(<span class="keyword">const</span> std::string& datafile, std::shared_ptr<GridMapBase> grid_map)</span></span>;</span><br><span class="line"></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">SaveProbMap</span><span class="params">(<span class="keyword">const</span> std::string& datafile, std::shared_ptr<GridMapBase> grid_map)</span></span>;</span><br><span class="line"></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">LoadGridMap</span><span class="params">(<span class="keyword">const</span> std::string& datafile, std::shared_ptr<GridMapBase>& grid_map)</span></span>;</span><br><span class="line"></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">DisplayGridMap</span><span class="params">(std::shared_ptr<GridMapBase> grid_map, <span class="keyword">const</span> uint& type)</span></span>;</span><br><span class="line"></span><br><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">SaveMapPng</span><span class="params">(std::shared_ptr<GridMapBase> grid_map, <span class="keyword">const</span> std::string& datafile)</span></span>;</span><br></pre></td></tr></table></figure><h1 id="GridSubMap"><a href="#GridSubMap" class="headerlink" title="GridSubMap"></a>GridSubMap</h1><h2 id="总体思路-待完成"><a href="#总体思路-待完成" class="headerlink" title="总体思路(待完成)"></a>总体思路(待完成)</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br></pre></td><td class="code"><pre><span class="line">1. </span><br><span class="line">struct{</span><br><span class="line">uint id;</span><br><span class="line">Eigen::Vector3f map_pose;</span><br><span class="line">std::shared_ptr<GridMapBase> gird_map;</span><br><span class="line">};</span><br><span class="line"></span><br><span class="line">2. submap回环:</span><br><span class="line">1.</span><br><span class="line">- 通过scan_context查找周围几个submap判断回环,减低计算量</span><br><span class="line">- 根据回环点优化相关submap并更新子图与key_frames</span><br><span class="line">2.</span><br><span class="line">- csm + 分支定界 </span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址:"></a>参考网址:</h1><p><a href="https://github.com/peitianyu/lite_slam">peitianyu/l</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="lite_slam" scheme="https://peitianyu.github.io/blog/categories/slam/lite-slam/"/>
<category term="lite_slam" scheme="https://peitianyu.github.io/blog/tags/lite-slam/"/>
</entry>
<entry>
<title>lite_slam - 整体框架简介</title>
<link href="https://peitianyu.github.io/blog/2022/09/05/lite_slam0/"/>
<id>https://peitianyu.github.io/blog/2022/09/05/lite_slam0/</id>
<published>2022-09-05T04:30:12.000Z</published>
<updated>2022-11-30T05:44:56.438Z</updated>
<content type="html"><![CDATA[<h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址:"></a>参考网址:</h1><p><a href="https://github.com/peitianyu/lite_slam">peitianyu/lite_slam (github.com)</a></p><h1 id="环境"><a href="#环境" class="headerlink" title="环境"></a>环境</h1><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">- Ubuntu18</span><br><span class="line">- Eigen</span><br><span class="line">- opencv(仅用于显示)</span><br></pre></td></tr></table></figure><h1 id="框架"><a href="#框架" class="headerlink" title="框架"></a>框架</h1><h2 id="建图框架"><a href="#建图框架" class="headerlink" title="建图框架"></a>建图框架</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br></pre></td><td class="code"><pre><span class="line">- grid_map作为概率地图</span><br><span class="line">- map_manage用于显示,保存,读取grid_map</span><br><span class="line">- down_sample_map用于降采样概率图</span><br><span class="line">- scantomap作为匹配</span><br><span class="line">- 为加快速度,采用多层地图,即down_sample_map</span><br><span class="line">- 为加快速度,采用scan_points体素滤波</span><br><span class="line">- 为加快速度,当迭代到一定程度时会提前退出迭代</span><br><span class="line">- scan_context作为回环检测模块</span><br><span class="line">- 通过开源nanoflann库进行kdtree搜索得到先验pose</span><br><span class="line">- 然后通过scantomap进行精确定位得到回环pose</span><br><span class="line">- graph_optimize图优化模块,图优化完后,更新关键帧与地图</span><br></pre></td></tr></table></figure><h2 id="代码实现"><a href="#代码实现" class="headerlink" title="代码实现"></a>代码实现</h2><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br></pre></td><td class="code"><pre><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">Run</span><span class="params">(<span class="keyword">const</span> Pose2d& prior_pose = Pose2d())</span> <span class="keyword">override</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> <span class="keyword">static</span> uint is_first = <span class="number">0</span>;</span><br><span class="line"> <span class="keyword">if</span>(is_first++ < <span class="number">20</span>){</span><br><span class="line"> <span class="built_in">Init</span>(prior_pose); <span class="comment">// 建图初始化</span></span><br><span class="line"> <span class="keyword">return</span>;</span><br><span class="line"> }</span><br><span class="line"></span><br><span class="line"> <span class="built_in">Location</span>(); <span class="comment">// 定位</span></span><br><span class="line"> </span><br><span class="line"> <span class="built_in">UpdateKeyFrame</span>(); <span class="comment">// 更新keyframe与更新地图</span></span><br><span class="line"></span><br><span class="line"> <span class="comment">// 显示pose与显示grid_map</span></span><br><span class="line"> <span class="keyword">static</span> uint id = <span class="number">0</span>;</span><br><span class="line"> std::cout<<id++<<<span class="string">" m_estimate_pose: "</span><<m_estimate_pose.<span class="built_in">transpose</span>()<<std::endl;</span><br><span class="line"> m_map_manager-><span class="built_in">DisplayGridMap</span>(m_scan_matcher-><span class="built_in">GetGridMap</span>(), <span class="number">1</span>);</span><br><span class="line">}</span><br></pre></td></tr></table></figure><h2 id="纯定位框架"><a href="#纯定位框架" class="headerlink" title="纯定位框架"></a>纯定位框架</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br></pre></td><td class="code"><pre><span class="line">- 保存scan_context相关的KeyFrame到txt文本中</span><br><span class="line">- 保存原始grid_map用于纯定位</span><br><span class="line">- scan_context用于重定位,由于场景时agv,所以路线一般固定,可以直接进行kdtree搜索得到先验pose,后scantomap得到精确pose</span><br><span class="line">- scantomap用于纯定位</span><br><span class="line">- 根据不同需求更新地图</span><br></pre></td></tr></table></figure><h2 id="代码实现-1"><a href="#代码实现-1" class="headerlink" title="代码实现"></a>代码实现</h2><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br></pre></td><td class="code"><pre><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">Run</span><span class="params">(<span class="keyword">const</span> Pose2d& prior_pose = Pose2d())</span> <span class="keyword">override</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"><span class="keyword">static</span> uint is_first = <span class="number">0</span>;</span><br><span class="line"><span class="keyword">if</span>(is_first++ == <span class="number">20</span>) {<span class="built_in">Init</span>(prior_pose);} <span class="comment">// 纯定位初始化</span></span><br><span class="line"></span><br><span class="line"><span class="keyword">static</span> <span class="keyword">bool</span> is_loop_closure = <span class="literal">false</span>;</span><br><span class="line"><span class="keyword">if</span>(!is_loop_closure){</span><br><span class="line"><span class="keyword">if</span>(<span class="built_in">LoopClosure</span>() != <span class="number">-1</span>) {is_loop_closure = <span class="literal">true</span>;} <span class="comment">// 重定位,若重定位成功</span></span><br><span class="line">}<span class="keyword">else</span>{ </span><br><span class="line"><span class="built_in">Location</span>(); <span class="comment">// 定位 </span></span><br><span class="line"><span class="built_in">UpdateKeyFrame</span>(); <span class="comment">// 更新keyframe与更新地图</span></span><br><span class="line"></span><br><span class="line"> <span class="comment">// 显示</span></span><br><span class="line"><span class="keyword">static</span> uint id = <span class="number">0</span>;</span><br><span class="line">std::cout<<id++<<<span class="string">" m_estimate_pose: "</span><<m_estimate_pose.<span class="built_in">transpose</span>()<<std::endl;</span><br><span class="line">m_map_manager-><span class="built_in">DisplayGridMap</span>(m_scan_matcher-><span class="built_in">GetGridMap</span>(), <span class="number">1</span>);</span><br><span class="line">}</span><br><span class="line">}</span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址:"></a>参考网址:</h1><p><a href="https://github.com/peitianyu/lite_slam">peitianyu/l</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="lite_slam" scheme="https://peitianyu.github.io/blog/categories/slam/lite-slam/"/>
<category term="lite_slam" scheme="https://peitianyu.github.io/blog/tags/lite-slam/"/>
</entry>
<entry>
<title>lite_slam - ScanMatch</title>
<link href="https://peitianyu.github.io/blog/2022/09/05/lite_slam2/"/>
<id>https://peitianyu.github.io/blog/2022/09/05/lite_slam2/</id>
<published>2022-09-05T04:30:12.000Z</published>
<updated>2022-11-30T05:44:56.951Z</updated>
<content type="html"><![CDATA[<h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址:"></a>参考网址:</h1><p><a href="https://github.com/peitianyu/lite_slam">peitianyu/lite_slam (github.com)</a></p><p><a href="https://www.cnblogs.com/cyberniklee/p/8484104.html">Hector SLAM解读（1）原文翻译 - cyberniklee - 博客园 (cnblogs.com)</a></p><h2 id="ScanMatch"><a href="#ScanMatch" class="headerlink" title="ScanMatch"></a>ScanMatch</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br></pre></td><td class="code"><pre><span class="line">目的主要是输出激光里程计,这里使用的是scantomap</span><br><span class="line">采用的数据集是r2000数据包,因此对于数据预处理比较简单,且定位精度足够</span><br><span class="line">为加快速度:</span><br><span class="line">- 多层地图(避免局部最优)</span><br><span class="line">- 多种分辨率滤波后的激光</span><br><span class="line">- 迭代残差足够小退出迭代</span><br><span class="line">整体流程:</span><br><span class="line">输入: pose, scan_points, grid_map(来源: 1, 建图 2, 加载)</span><br><span class="line">迭代直到更新距离足够小或者超出最大迭代次数</span><br><span class="line">输出: estimate_pose</span><br></pre></td></tr></table></figure><h2 id="迭代一次流程"><a href="#迭代一次流程" class="headerlink" title="迭代一次流程"></a>迭代一次流程</h2><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br></pre></td><td class="code"><pre><span class="line"><span class="function"><span class="keyword">bool</span> <span class="title">ScanToMap::EstimateTransformationOnce</span><span class="params">(<span class="keyword">const</span> std::vector<Eigen::Vector2f> &scan_points, std::shared_ptr<GridMapUtils> map_utils, Eigen::Vector3f &estimate_in_world)</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> Eigen::Matrix3f H = Eigen::Matrix3f::<span class="built_in">Zero</span>();</span><br><span class="line"> Eigen::Vector3f dTr = Eigen::Vector3f::<span class="built_in">Zero</span>();</span><br><span class="line"></span><br><span class="line"> Eigen::Vector3f last_estimate_pose = estimate_in_world;</span><br><span class="line"> <span class="built_in">GetHessianDerivative</span>(estimate_in_world, scan_points, map_utils, H, dTr); <span class="comment">// 求解海森矩阵, 通过高斯牛顿求解</span></span><br><span class="line"></span><br><span class="line"> <span class="keyword">if</span>(<span class="built_in">H</span>(<span class="number">0</span>, <span class="number">0</span>) != <span class="number">0</span> || <span class="built_in">H</span>(<span class="number">1</span>, <span class="number">1</span>) != <span class="number">0</span>)</span><br><span class="line"> {</span><br><span class="line"> Eigen::Vector3f d_pose_in_map = H.<span class="built_in">inverse</span>() * dTr;</span><br><span class="line"> <span class="built_in">UpdateEstimatedPose</span>(map_utils, estimate_in_world, d_pose_in_map); <span class="comment">// 更新预测坐标</span></span><br><span class="line"> }</span><br><span class="line"></span><br><span class="line"> <span class="keyword">if</span>(<span class="built_in">PoseDiffSmallerThan</span>(last_estimate_pose, estimate_in_world)) <span class="keyword">return</span> <span class="literal">true</span>; <span class="comment">// 判断更新距离足够小,则退出</span></span><br><span class="line"></span><br><span class="line"> <span class="keyword">return</span> <span class="literal">false</span>;</span><br><span class="line">}</span><br></pre></td></tr></table></figure><h2 id="对于scantomap理解"><a href="#对于scantomap理解" class="headerlink" title="对于scantomap理解"></a>对于scantomap理解</h2><figure class="highlight plaintext"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br></pre></td><td class="code"><pre><span class="line">1. 通过scan_point与grid_map匹配构建最小二乘,通过高斯牛顿求解</span><br><span class="line">2. 由于求解时发现grid_map是离散的需要线性化,所以进行双线性插补,得到线性化后map,即▽map</span><br><span class="line">3. 由于匹配时间过长以及担心局部最优,进行多分辨率地图设计</span><br></pre></td></tr></table></figure><h2 id="公式推导"><a href="#公式推导" class="headerlink" title="公式推导"></a>公式推导</h2><p><a href="https://imgse.com/i/v7JwZj"><img src="https://s1.ax1x.com/2022/09/06/v7JwZj.png" alt="v7JwZj.png"></a></p><h2 id="代码体现"><a href="#代码体现" class="headerlink" title="代码体现"></a>代码体现</h2><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br><span class="line">25</span><br><span class="line">26</span><br><span class="line">27</span><br><span class="line">28</span><br><span class="line">29</span><br><span class="line">30</span><br><span class="line">31</span><br><span class="line">32</span><br><span class="line">33</span><br><span class="line">34</span><br><span class="line">35</span><br><span class="line">36</span><br><span class="line">37</span><br><span class="line">38</span><br><span class="line">39</span><br><span class="line">40</span><br><span class="line">41</span><br><span class="line">42</span><br><span class="line">43</span><br><span class="line">44</span><br><span class="line">45</span><br><span class="line">46</span><br><span class="line">47</span><br><span class="line">48</span><br><span class="line">49</span><br><span class="line">50</span><br><span class="line">51</span><br><span class="line">52</span><br><span class="line">53</span><br></pre></td><td class="code"><pre><span class="line"><span class="function"><span class="keyword">void</span> <span class="title">ScanToMap::GetHessianDerivative</span><span class="params">(<span class="keyword">const</span> Eigen::Vector3f &robot_in_world, <span class="keyword">const</span> std::vector<Eigen::Vector2f> &scan_point, </span></span></span><br><span class="line"><span class="params"><span class="function"> std::shared_ptr<GridMapUtils> map_utils, Eigen::Matrix3f &H, Eigen::Vector3f &dTr)</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> <span class="keyword">float</span> ss = <span class="built_in">sin</span>(<span class="built_in">robot_in_world</span>(<span class="number">2</span>));</span><br><span class="line"> <span class="keyword">float</span> cs = <span class="built_in">cos</span>(<span class="built_in">robot_in_world</span>(<span class="number">2</span>));</span><br><span class="line"></span><br><span class="line"> <span class="keyword">for</span>(uint i = <span class="number">0</span>; i < scan_point.<span class="built_in">size</span>(); i++)</span><br><span class="line"> {</span><br><span class="line"> Eigen::Vector2f point_in_scaled_laser = map_utils-><span class="built_in">LaserInScaledLaser</span>(scan_point[i]); <span class="comment">// 获得栅格化的laser_point</span></span><br><span class="line"></span><br><span class="line"> Eigen::Vector2f point_in_world = map_utils-><span class="built_in">LaserPointToWorld</span>(scan_point[i], robot_in_world); <span class="comment">// 求解世界坐标系下的point</span></span><br><span class="line"></span><br><span class="line"> Eigen::Vector2f point_in_map = map_utils-><span class="built_in">WorldToMapFloat</span>(point_in_world); <span class="comment">// 求解地图坐标系下的laser_point</span></span><br><span class="line"></span><br><span class="line"> Eigen::Vector3f interpolated_value = <span class="built_in">BilinearInterpolationWithDerivative</span>(point_in_map, map_utils); <span class="comment">// 双线性插补</span></span><br><span class="line"></span><br><span class="line"> <span class="keyword">float</span> func_val = <span class="number">1</span> - <span class="built_in">interpolated_value</span>(<span class="number">0</span>); <span class="comment">// 具体公式参考hector论文</span></span><br><span class="line"> <span class="built_in">dTr</span>(<span class="number">0</span>) += func_val * <span class="built_in">interpolated_value</span>(<span class="number">1</span>);</span><br><span class="line"> <span class="built_in">dTr</span>(<span class="number">1</span>) += func_val * <span class="built_in">interpolated_value</span>(<span class="number">2</span>);</span><br><span class="line"> <span class="keyword">float</span> rot_deriv = (<span class="built_in">interpolated_value</span>(<span class="number">1</span>) * (-ss * <span class="built_in">point_in_scaled_laser</span>(<span class="number">0</span>) - cs * <span class="built_in">point_in_scaled_laser</span>(<span class="number">1</span>)) +</span><br><span class="line"> <span class="built_in">interpolated_value</span>(<span class="number">2</span>) * (cs * <span class="built_in">point_in_scaled_laser</span>(<span class="number">0</span>) - ss * <span class="built_in">point_in_scaled_laser</span>(<span class="number">1</span>)));</span><br><span class="line"> <span class="built_in">dTr</span>(<span class="number">2</span>) += rot_deriv * func_val;</span><br><span class="line"></span><br><span class="line"> <span class="built_in">H</span>(<span class="number">0</span>, <span class="number">0</span>) += <span class="built_in">interpolated_value</span>(<span class="number">1</span>) * <span class="built_in">interpolated_value</span>(<span class="number">1</span>);</span><br><span class="line"> <span class="built_in">H</span>(<span class="number">1</span>, <span class="number">1</span>) += <span class="built_in">interpolated_value</span>(<span class="number">2</span>) * <span class="built_in">interpolated_value</span>(<span class="number">2</span>);</span><br><span class="line"> <span class="built_in">H</span>(<span class="number">2</span>, <span class="number">2</span>) += rot_deriv * rot_deriv;</span><br><span class="line"></span><br><span class="line"> <span class="built_in">H</span>(<span class="number">0</span>, <span class="number">1</span>) += <span class="built_in">interpolated_value</span>(<span class="number">1</span>) * <span class="built_in">interpolated_value</span>(<span class="number">2</span>);</span><br><span class="line"> <span class="built_in">H</span>(<span class="number">0</span>, <span class="number">2</span>) += <span class="built_in">interpolated_value</span>(<span class="number">1</span>) * rot_deriv;</span><br><span class="line"> <span class="built_in">H</span>(<span class="number">1</span>, <span class="number">2</span>) += <span class="built_in">interpolated_value</span>(<span class="number">2</span>) * rot_deriv;</span><br><span class="line"> }</span><br><span class="line"></span><br><span class="line"> <span class="built_in">H</span>(<span class="number">1</span>, <span class="number">0</span>) = <span class="built_in">H</span>(<span class="number">0</span>, <span class="number">1</span>);</span><br><span class="line"> <span class="built_in">H</span>(<span class="number">2</span>, <span class="number">0</span>) = <span class="built_in">H</span>(<span class="number">0</span>, <span class="number">2</span>);</span><br><span class="line"> <span class="built_in">H</span>(<span class="number">2</span>, <span class="number">1</span>) = <span class="built_in">H</span>(<span class="number">1</span>, <span class="number">2</span>);</span><br><span class="line">}</span><br><span class="line"></span><br><span class="line"><span class="function">Eigen::Vector3f <span class="title">ScanToMap::BilinearInterpolationWithDerivative</span><span class="params">(<span class="keyword">const</span> Eigen::Vector2f &point_in_map, std::shared_ptr<GridMapUtils> map_utils)</span> <span class="keyword">const</span></span></span><br><span class="line"><span class="function"></span>{ </span><br><span class="line"> <span class="keyword">float</span> factor0 = <span class="built_in">point_in_map</span>(<span class="number">0</span>) - <span class="built_in">floor</span>(<span class="built_in">point_in_map</span>(<span class="number">0</span>));</span><br><span class="line"> <span class="keyword">float</span> factor1 = <span class="built_in">point_in_map</span>(<span class="number">1</span>) - <span class="built_in">floor</span>(<span class="built_in">point_in_map</span>(<span class="number">1</span>));</span><br><span class="line"> <span class="keyword">float</span> factor0_inv = <span class="number">1</span> - factor0;</span><br><span class="line"> <span class="keyword">float</span> factor1_inv = <span class="number">1</span> - factor1;</span><br><span class="line"></span><br><span class="line"> <span class="keyword">float</span> p00 = map_utils-><span class="built_in">GetCellProb</span>(point_in_map.cast<<span class="keyword">int</span>>()); <span class="comment">// 栅格四角prob</span></span><br><span class="line"> <span class="keyword">float</span> p01 = map_utils-><span class="built_in">GetCellProb</span>(point_in_map.cast<<span class="keyword">int</span>>() + Eigen::<span class="built_in">Vector2i</span>(<span class="number">0</span>, <span class="number">1</span>));</span><br><span class="line"> <span class="keyword">float</span> p10 = map_utils-><span class="built_in">GetCellProb</span>(point_in_map.cast<<span class="keyword">int</span>>() + Eigen::<span class="built_in">Vector2i</span>(<span class="number">1</span>, <span class="number">0</span>));</span><br><span class="line"> <span class="keyword">float</span> p11 = map_utils-><span class="built_in">GetCellProb</span>(point_in_map.cast<<span class="keyword">int</span>>() + Eigen::<span class="built_in">Vector2i</span>(<span class="number">1</span>, <span class="number">1</span>));</span><br><span class="line"></span><br><span class="line"> <span class="keyword">return</span> Eigen::<span class="built_in">Vector3f</span>(((factor1*(factor0*p11 + factor0_inv*p01)) + (factor1_inv * (factor0*p10 + factor0_inv*p00))),</span><br><span class="line"> ( factor1 * ( p11 - p01 ) + factor1_inv * ( p10 - p00 ) ),</span><br><span class="line"> ( factor0 * ( p11 - p10 ) + factor0_inv * ( p01 - p00 ) ));</span><br><span class="line">}</span><br></pre></td></tr></table></figure>]]></content>
<summary type="html"><h1 id="参考网址"><a href="#参考网址" class="headerlink" title="参考网址:"></a>参考网址:</h1><p><a href="https://github.com/peitianyu/lite_slam">peitianyu/l</summary>
<category term="slam" scheme="https://peitianyu.github.io/blog/categories/slam/"/>
<category term="lite_slam" scheme="https://peitianyu.github.io/blog/categories/slam/lite-slam/"/>
<category term="lite_slam" scheme="https://peitianyu.github.io/blog/tags/lite-slam/"/>
</entry>
</feed>