-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathindex.html
More file actions
1025 lines (795 loc) · 124 KB
/
index.html
File metadata and controls
1025 lines (795 loc) · 124 KB
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
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
<!DOCTYPE html>
<html lang="zh-CN">
<head>
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1, maximum-scale=2">
<meta name="theme-color" content="#222">
<meta name="generator" content="Hexo 7.3.0">
<link rel="apple-touch-icon" sizes="180x180" href="/images/apple-touch-icon.png">
<link rel="icon" type="image/png" sizes="32x32" href="/images/web-app-manifest-192x192.png">
<link rel="icon" type="image/png" sizes="16x16" href="/images/favicon-96x96.png">
<link rel="mask-icon" href="/images/favicon.svg" color="#222">
<link rel="stylesheet" href="/css/main.css">
<link rel="stylesheet" href="/lib/font-awesome/css/all.min.css">
<script id="hexo-configurations">
var NexT = window.NexT || {};
var CONFIG = {"hostname":"zihchen7.github.io","root":"/","scheme":"Pisces","version":"7.8.0","exturl":false,"sidebar":{"position":"left","display":"post","padding":18,"offset":12,"onmobile":false},"copycode":{"enable":true,"show_result":true,"style":"max"},"back2top":{"enable":true,"sidebar":false,"scrollpercent":false},"bookmark":{"enable":false,"color":"#222","save":"auto"},"fancybox":false,"mediumzoom":false,"lazyload":false,"pangu":true,"comments":{"style":"tabs","active":null,"storage":true,"lazyload":false,"nav":null},"algolia":{"hits":{"per_page":10},"labels":{"input_placeholder":"Search for Posts","hits_empty":"We didn't find any results for the search: ${query}","hits_stats":"${hits} results found in ${time} ms"}},"localsearch":{"enable":false,"trigger":"auto","top_n_per_article":1,"unescape":false,"preload":false},"motion":{"enable":true,"async":false,"transition":{"post_block":"fadeIn","post_header":"slideDownIn","post_body":"slideDownIn","coll_header":"slideLeftIn","sidebar":"slideUpIn"}}};
</script>
<meta name="description" content="一个爱好机器人的小学生。email: ZihChen7@163.com">
<meta property="og:type" content="website">
<meta property="og:title" content="A Blog">
<meta property="og:url" content="https://zihchen7.github.io/index.html">
<meta property="og:site_name" content="A Blog">
<meta property="og:description" content="一个爱好机器人的小学生。email: ZihChen7@163.com">
<meta property="og:locale" content="zh_CN">
<meta property="article:author" content="ZihChen7">
<meta property="article:tag" content="C++, Robot control and planning.">
<meta name="twitter:card" content="summary">
<link rel="canonical" href="https://zihchen7.github.io/">
<script id="page-configurations">
// https://hexo.io/docs/variables.html
CONFIG.page = {
sidebar: "",
isHome : true,
isPost : false,
lang : 'zh-CN'
};
</script>
<title>A Blog</title>
<noscript>
<style>
.use-motion .brand,
.use-motion .menu-item,
.sidebar-inner,
.use-motion .post-block,
.use-motion .pagination,
.use-motion .comments,
.use-motion .post-header,
.use-motion .post-body,
.use-motion .collection-header { opacity: initial; }
.use-motion .site-title,
.use-motion .site-subtitle {
opacity: initial;
top: initial;
}
.use-motion .logo-line-before i { left: initial; }
.use-motion .logo-line-after i { right: initial; }
</style>
</noscript>
<!-- hexo injector head_end start -->
<link rel="stylesheet" href="https://cdn.jsdelivr.net/npm/katex@0.12.0/dist/katex.min.css">
<link rel="stylesheet" href="https://cdn.jsdelivr.net/npm/hexo-math@4.0.0/dist/style.css">
<!-- hexo injector head_end end --></head>
<body itemscope itemtype="http://schema.org/WebPage">
<div class="container use-motion">
<div class="headband"></div>
<header class="header" itemscope itemtype="http://schema.org/WPHeader">
<div class="header-inner"><div class="site-brand-container">
<div class="site-nav-toggle">
<div class="toggle" aria-label="切换导航栏">
<span class="toggle-line toggle-line-first"></span>
<span class="toggle-line toggle-line-middle"></span>
<span class="toggle-line toggle-line-last"></span>
</div>
</div>
<div class="site-meta">
<a href="/" class="brand" rel="start">
<span class="logo-line-before"><i></i></span>
<h1 class="site-title">A Blog</h1>
<span class="logo-line-after"><i></i></span>
</a>
<p class="site-subtitle" itemprop="description">or A notebook</p>
</div>
<div class="site-nav-right">
<div class="toggle popup-trigger">
</div>
</div>
</div>
<nav class="site-nav">
<ul id="menu" class="main-menu menu">
<li class="menu-item menu-item-home">
<a href="/" rel="section"><i class="fa fa-home fa-fw"></i>首页</a>
</li>
<li class="menu-item menu-item-archives">
<a href="/archives/" rel="section"><i class="fa fa-archive fa-fw"></i>归档</a>
</li>
</ul>
</nav>
</div>
</header>
<div class="back-to-top">
<i class="fa fa-arrow-up"></i>
<span>0%</span>
</div>
<div class="reading-progress-bar"></div>
<a href="https://github.com/ZihChen7/ZihChen7.github.io" class="github-corner" title="GitHub" aria-label="GitHub" rel="noopener" target="_blank"><svg width="80" height="80" viewBox="0 0 250 250" aria-hidden="true"><path d="M0,0 L115,115 L130,115 L142,142 L250,250 L250,0 Z"></path><path d="M128.3,109.0 C113.8,99.7 119.0,89.6 119.0,89.6 C122.0,82.7 120.5,78.6 120.5,78.6 C119.2,72.0 123.4,76.3 123.4,76.3 C127.3,80.9 125.5,87.3 125.5,87.3 C122.9,97.6 130.6,101.9 134.4,103.2" fill="currentColor" style="transform-origin: 130px 106px;" class="octo-arm"></path><path d="M115.0,115.0 C114.9,115.1 118.7,116.5 119.8,115.4 L133.7,101.6 C136.9,99.2 139.9,98.4 142.2,98.6 C133.8,88.0 127.5,74.4 143.8,58.0 C148.5,53.4 154.0,51.2 159.7,51.0 C160.3,49.4 163.2,43.6 171.4,40.1 C171.4,40.1 176.1,42.5 178.8,56.2 C183.1,58.6 187.2,61.8 190.9,65.4 C194.5,69.0 197.7,73.2 200.1,77.6 C213.8,80.2 216.3,84.9 216.3,84.9 C212.7,93.1 206.9,96.0 205.4,96.6 C205.1,102.4 203.0,107.8 198.3,112.5 C181.9,128.9 168.3,122.5 157.7,114.1 C157.9,116.9 156.7,120.9 152.7,124.9 L141.0,136.5 C139.8,137.7 141.6,141.9 141.8,141.8 Z" fill="currentColor" class="octo-body"></path></svg></a>
<main class="main">
<div class="main-inner">
<div class="content-wrap">
<div class="content index posts-expand">
<article itemscope itemtype="http://schema.org/Article" class="post-block" lang="zh-CN">
<link itemprop="mainEntityOfPage" href="https://zihchen7.github.io/2025/06/23/25-6-23/">
<span hidden itemprop="author" itemscope itemtype="http://schema.org/Person">
<meta itemprop="image" content="/images/web-app-manifest-192x192.png">
<meta itemprop="name" content="ZihChen7">
<meta itemprop="description" content="一个爱好机器人的小学生。email: ZihChen7@163.com ">
</span>
<span hidden itemprop="publisher" itemscope itemtype="http://schema.org/Organization">
<meta itemprop="name" content="A Blog">
</span>
<header class="post-header">
<h2 class="post-title" itemprop="name headline">
<a href="/2025/06/23/25-6-23/" class="post-title-link" itemprop="url">Eigen Manual</a>
</h2>
<div class="post-meta">
<span class="post-meta-item">
<span class="post-meta-item-icon">
<i class="far fa-calendar"></i>
</span>
<span class="post-meta-item-text">发表于</span>
<time title="创建时间：2025-06-23 20:56:37" itemprop="dateCreated datePublished" datetime="2025-06-23T20:56:37+08:00">2025-06-23</time>
</span>
<span class="post-meta-item">
<span class="post-meta-item-icon">
<i class="far fa-calendar-check"></i>
</span>
<span class="post-meta-item-text">更新于</span>
<time title="修改时间：2025-06-25 20:22:44" itemprop="dateModified" datetime="2025-06-25T20:22:44+08:00">2025-06-25</time>
</span>
</div>
</header>
<div class="post-body" itemprop="articleBody">
<h1 id="前言"><a href="#前言" class="headerlink" title="前言"></a>前言</h1><p> 记录Eigen库的使用方法。</p>
<h1 id="头文件"><a href="#头文件" class="headerlink" title="头文件"></a>头文件</h1><p> 如Eigen官方教程中所示，头文件分为以下几种：</p>
<p><img src="https://github.com/ZihChen7/picx-images-hosting/raw/master/20250623/Eigen.3yepptv29i.webp" alt="Eigen"></p>
<p> 其中，较为常用的分别为</p>
<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></pre></td><td class="code"><pre><span class="line"><span class="comment">// 基础矩阵运算</span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string"><Eigen/Core></span> <span class="comment">// 必须包含</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string"><Eigen/Dense></span> <span class="comment">// 稠密矩阵扩展（推荐）</span></span></span><br><span class="line"></span><br><span class="line"><span class="comment">// 线性系统求解</span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string"><Eigen/LU></span> <span class="comment">// LU分解</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string"><Eigen/Cholesky></span> <span class="comment">// Cholesky分解</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string"><Eigen/QR></span> <span class="comment">// QR分解</span></span></span><br><span class="line"></span><br><span class="line"><span class="comment">// 稀疏矩阵</span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string"><Eigen/Sparse></span> <span class="comment">// 稀疏矩阵核心</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string"><Eigen/SparseLU></span> <span class="comment">// 稀疏LU求解器</span></span></span><br><span class="line"></span><br><span class="line"><span class="comment">// 几何变换</span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string"><Eigen/Geometry></span> <span class="comment">// 旋转、平移等</span></span></span><br></pre></td></tr></table></figure>
<h1 id="初始化"><a href="#初始化" class="headerlink" title="初始化"></a>初始化</h1><h2 id="静态初始化"><a href="#静态初始化" class="headerlink" title="静态初始化"></a><strong>静态初始化</strong></h2><p> 创建已知维度的矩阵，比如Matrix3d、Vector4f。Matrix2Xd （行数固定） MatrixX2d （列数固定） </p>
<ol>
<li><p>逗号初始化</p>
<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></pre></td><td class="code"><pre><span class="line">Eigen::Matrix3d mat;</span><br><span class="line">mat << <span class="number">1</span>, <span class="number">2</span>, <span class="number">3</span>,</span><br><span class="line"> <span class="number">4</span>, <span class="number">5</span>, <span class="number">6</span>,</span><br><span class="line"> <span class="number">7</span>, <span class="number">8</span>, <span class="number">9</span>;</span><br><span class="line"></span><br><span class="line">Eigen::Vector4d vec;</span><br><span class="line">vec << <span class="number">1</span>, <span class="number">2</span>, <span class="number">3</span>, <span class="number">4</span>;</span><br></pre></td></tr></table></figure>
</li>
<li><p>构造函数初始化</p>
<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></pre></td><td class="code"><pre><span class="line"><span class="function">Eigen::Matrix2d <span class="title">mat</span><span class="params">(<span class="number">1.0</span>, <span class="number">2.0</span>, <span class="comment">// 按行填充</span></span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="number">3.0</span>, <span class="number">4.0</span>)</span></span>;</span><br><span class="line"><span class="function">Eigen::Vector3d <span class="title">vec</span><span class="params">(<span class="number">1.0</span>, <span class="number">2.0</span>, <span class="number">3.0</span>)</span></span>;</span><br></pre></td></tr></table></figure>
</li>
</ol>
<h2 id="动态初始化"><a href="#动态初始化" class="headerlink" title="动态初始化"></a>动态初始化</h2><p> 创建可调节维度的矩阵，比如MatrixXd、VectorXf。</p>
<ol>
<li><p>指定大小后赋值</p>
<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></pre></td><td class="code"><pre><span class="line"><span class="function">Eigen::MatrixXd <span class="title">mat</span><span class="params">(<span class="number">2</span>, <span class="number">3</span>)</span></span>; <span class="comment">// 2行3列，未初始化</span></span><br><span class="line">mat << <span class="number">1</span>, <span class="number">2</span>, <span class="number">3</span>,</span><br><span class="line"> <span class="number">4</span>, <span class="number">5</span>, <span class="number">6</span>;</span><br><span class="line"></span><br><span class="line"><span class="built_in">mat</span>(<span class="number">0</span>, <span class="number">0</span>) = <span class="number">1.0</span>; <span class="comment">// 第1行第1列</span></span><br><span class="line"><span class="built_in">mat</span>(<span class="number">1</span>, <span class="number">1</span>) = <span class="number">4.0</span>; <span class="comment">// 第2行第2列</span></span><br></pre></td></tr></table></figure>
</li>
<li><p>使用函数进行特殊赋值</p>
<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></pre></td><td class="code"><pre><span class="line">Eigen::MatrixXd mat = Eigen::MatrixXd::<span class="built_in">Zero</span>(<span class="number">3</span>, <span class="number">3</span>); <span class="comment">// 全0矩阵</span></span><br><span class="line">Eigen::VectorXf vec = Eigen::VectorXf::<span class="built_in">Ones</span>(<span class="number">5</span>); <span class="comment">// 全1向量</span></span><br><span class="line">Eigen::Matrix3f I = Eigen::Matrix3f::<span class="built_in">Identity</span>(); <span class="comment">// 单位矩阵</span></span><br></pre></td></tr></table></figure>
</li>
</ol>
<h2 id="特殊矩阵赋值"><a href="#特殊矩阵赋值" class="headerlink" title="特殊矩阵赋值"></a><strong>特殊矩阵赋值</strong></h2><ol>
<li><p>常量矩阵</p>
<figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line">Eigen::Matrix4f mat = Eigen::Matrix4f::<span class="built_in">Constant</span>(<span class="number">3.14</span>); <span class="comment">// 所有元素为3.14</span></span><br></pre></td></tr></table></figure>
</li>
<li><p>随机矩阵</p>
<figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line">Eigen::MatrixXd rand_mat = Eigen::MatrixXd::<span class="built_in">Random</span>(<span class="number">3</span>, <span class="number">3</span>); <span class="comment">// 范围[-1, 1]</span></span><br></pre></td></tr></table></figure>
</li>
<li><p>线性序列矩阵</p>
<figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line">Eigen::VectorXd lin_vec = Eigen::VectorXd::<span class="built_in">LinSpaced</span>(<span class="number">5</span>, <span class="number">0</span>, <span class="number">10</span>); <span class="comment">// 0到10的5等分</span></span><br></pre></td></tr></table></figure>
</li>
</ol>
<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></pre></td><td class="code"><pre><span class="line"><span class="type">double</span> data[] = {<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="function">Eigen::Map<Eigen::VectorXd> <span class="title">vec</span><span class="params">(data, <span class="number">4</span>)</span></span>; <span class="comment">// 直接映射数组</span></span><br><span class="line"><span class="function">Eigen::Map<Eigen::MatrixXd> <span class="title">mat</span><span class="params">(data, <span class="number">2</span>, <span class="number">2</span>)</span></span>; <span class="comment">// 2x2矩阵</span></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><ol>
<li><p>取固定大小的块 matrix.block<Rows, Cols>(startRow, startCol)</p>
<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></pre></td><td class="code"><pre><span class="line">Eigen::Matrix3d mat;</span><br><span class="line">mat << <span class="number">1</span>, <span class="number">2</span>, <span class="number">3</span>,</span><br><span class="line"> <span class="number">4</span>, <span class="number">5</span>, <span class="number">6</span>,</span><br><span class="line"> <span class="number">7</span>, <span class="number">8</span>, <span class="number">9</span>;</span><br><span class="line"></span><br><span class="line"><span class="comment">// 提取左上角 2x2 子块</span></span><br><span class="line">Eigen::Block<Eigen::Matrix3d, <span class="number">2</span>, <span class="number">2</span>> block = mat.<span class="built_in">block</span><<span class="number">2</span>, <span class="number">2</span>>(<span class="number">0</span>, <span class="number">0</span>);</span><br><span class="line">cout << <span class="string">"2x2 Block:\n"</span> << block << endl; <span class="comment">// 输出 [1, 2; 4, 5]</span></span><br></pre></td></tr></table></figure>
</li>
<li><p>提取动态大小的块 matrix.block(startRow, startCol, rows, cols)</p>
<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></pre></td><td class="code"><pre><span class="line"><span class="comment">// 提取右下角 2x2 子块（动态大小）</span></span><br><span class="line">Eigen::MatrixXd dynamic_block = mat.<span class="built_in">block</span>(<span class="number">1</span>, <span class="number">1</span>, <span class="number">2</span>, <span class="number">2</span>);</span><br><span class="line">cout << <span class="string">"Dynamic Block:\n"</span> << dynamic_block << endl; <span class="comment">// 输出 [5, 6; 8, 9]</span></span><br></pre></td></tr></table></figure>
</li>
<li><p>特殊位置的块</p>
</li>
</ol>
<div class="table-container">
<table>
<thead>
<tr>
<th style="text-align:center">操作</th>
<th style="text-align:center">语法</th>
<th style="text-align:center">示例</th>
</tr>
</thead>
<tbody>
<tr>
<td style="text-align:center">前n行</td>
<td style="text-align:center">matrix.topRows(n)</td>
<td style="text-align:center">mat.topRows(2) → 前2行</td>
</tr>
<tr>
<td style="text-align:center">后n行</td>
<td style="text-align:center">matrix.bottomRows(n)</td>
<td style="text-align:center">mat.bottomRows(1) → 最后1行</td>
</tr>
<tr>
<td style="text-align:center">左n行</td>
<td style="text-align:center">matrix.leftCols(n)</td>
<td style="text-align:center">mat.leftCols(2) → 左2列</td>
</tr>
<tr>
<td style="text-align:center">右n行</td>
<td style="text-align:center">matrix.rightCols(n)</td>
<td style="text-align:center">mat.rightCols(1) → 右1列</td>
</tr>
<tr>
<td style="text-align:center">任意行</td>
<td style="text-align:center">matrix.row(i)</td>
<td style="text-align:center">mat.row(0) → 第1行</td>
</tr>
<tr>
<td style="text-align:center">任意列</td>
<td style="text-align:center">matrix.col(j)</td>
<td style="text-align:center">mat.col(1) → 第2列</td>
</tr>
<tr>
<td style="text-align:center">主对角线</td>
<td style="text-align:center">matrix.diagonal()</td>
<td style="text-align:center">mat.diagonal() → [1, 5, 9]</td>
</tr>
</tbody>
</table>
</div>
<h1 id="特殊函数"><a href="#特殊函数" class="headerlink" title="特殊函数"></a>特殊函数</h1><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></pre></td><td class="code"><pre><span class="line">MatrixXcf a = MatrixXcf::<span class="built_in">Random</span>(<span class="number">2</span>,<span class="number">2</span>);</span><br><span class="line">cout << <span class="string">"Here is the matrix a^T\n"</span> << a.<span class="built_in">transpose</span>() << endl;</span><br><span class="line"><span class="comment">// a = a.transpose(); // 禁止</span></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></pre></td><td class="code"><pre><span class="line">MatrixXcf a = MatrixXcf::<span class="built_in">Random</span>(<span class="number">2</span>,<span class="number">2</span>)；</span><br><span class="line">cout << <span class="string">"Here is the conjugate of a\n"</span> << a.<span class="built_in">conjugate</span>() << endl;</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></pre></td><td class="code"><pre><span class="line">MatrixXcf a = MatrixXcf::<span class="built_in">Random</span>(<span class="number">2</span>,<span class="number">2</span>);</span><br><span class="line">cout << <span class="string">"Here is the matrix a^*\n"</span> << a.<span class="built_in">adjoint</span>() << endl;</span><br></pre></td></tr></table></figure>
<h2 id="点积"><a href="#点积" class="headerlink" title="点积"></a>点积</h2><p><strong>两向量维度必须相同</strong>，输出标量</p>
<figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line"><span class="type">double</span> dot_product = a.<span class="built_in">dot</span>(b);</span><br></pre></td></tr></table></figure>
<h2 id="差积"><a href="#差积" class="headerlink" title="差积"></a>差积</h2><p>叉积结果是一个新向量 c，其方向垂直于原始 a 和 b 所在的平面。<strong>仅三维向量</strong></p>
<figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line"><span class="type">double</span> angle = <span class="built_in">acos</span>(a.<span class="built_in">dot</span>(b) / (a.<span class="built_in">norm</span>() * b.<span class="built_in">norm</span>())); <span class="comment">// 弧度制</span></span><br></pre></td></tr></table></figure>
<h2 id="归约操作"><a href="#归约操作" class="headerlink" title="归约操作"></a><strong>归约操作</strong></h2><ol>
<li><p>所有元素求和</p>
<figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line"><span class="type">double</span> sum = mat.<span class="built_in">sum</span>()；</span><br></pre></td></tr></table></figure>
</li>
<li><p>所有元素求积</p>
<figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line"><span class="type">double</span> prod = mat.<span class="built_in">prod</span>()；</span><br></pre></td></tr></table></figure>
</li>
<li><p>所有元素的平均值</p>
<figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line"><span class="type">double</span> mean = mat.<span class="built_in">mean</span>()；</span><br></pre></td></tr></table></figure>
</li>
<li><p>最小元素值</p>
<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></pre></td><td class="code"><pre><span class="line"><span class="type">int</span> row, col;</span><br><span class="line"><span class="type">double</span> min_val = mat.<span class="built_in">minCoeff</span>(&row, &col); <span class="comment">// 获取最小值及其位置</span></span><br><span class="line">cout << <span class="string">"Min: "</span> << min_val << <span class="string">" at ("</span> << row << <span class="string">","</span> << col << <span class="string">")"</span> << endl;</span><br></pre></td></tr></table></figure>
</li>
<li><p>最大元素值</p>
<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></pre></td><td class="code"><pre><span class="line"><span class="type">int</span> row, col;</span><br><span class="line"><span class="type">double</span> max_val = mat.<span class="built_in">maxCoeff</span>(&row, &col); <span class="comment">// 获取最大值及其位置</span></span><br><span class="line">cout << <span class="string">"Max: "</span> << max_val << <span class="string">" at ("</span> << row << <span class="string">","</span> << col << <span class="string">")"</span> << endl;</span><br></pre></td></tr></table></figure>
</li>
<li><p>矩阵的迹（主对角线元素之和）</p>
<figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line"><span class="type">double</span> trace = mat.<span class="built_in">trace</span>()；</span><br></pre></td></tr></table></figure>
</li>
<li><p>范数</p>
<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></pre></td><td class="code"><pre><span class="line"><span class="function">Eigen::MatrixXd <span class="title">m</span><span class="params">(<span class="number">2</span>, <span class="number">2</span>)</span></span>;</span><br><span class="line">m << <span class="number">1</span>, <span class="number">2</span>,</span><br><span class="line"> <span class="number">3</span>, <span class="number">4</span>;</span><br><span class="line"><span class="comment">// Frobenius 范数（类似于向量的 L2 范数）</span></span><br><span class="line"><span class="type">double</span> frob_norm = m.<span class="built_in">norm</span>(); <span class="comment">// sqrt(1² + 2² + 3² + 4²) = sqrt(30) ≈ 5.47723</span></span><br><span class="line"><span class="comment">// 平方范数</span></span><br><span class="line"><span class="type">double</span> frob_norm = <span class="built_in">squaredNorm</span>(); <span class="comment">// 等于上面的平方</span></span><br><span class="line"> </span><br><span class="line"><span class="comment">// L1 范数（最大绝对列和）</span></span><br><span class="line"><span class="type">double</span> l1_norm = m.<span class="built_in">colwise</span>().<span class="built_in">lpNorm</span><<span class="number">1</span>>().<span class="built_in">maxCoeff</span>(); <span class="comment">// max(1+3, 2+4) = 6</span></span><br><span class="line"></span><br><span class="line"><span class="comment">// 无穷范数（最大绝对行和）</span></span><br><span class="line"><span class="type">double</span> inf_norm = m.<span class="built_in">rowwise</span>().<span class="built_in">lpNorm</span><<span class="number">1</span>>().<span class="built_in">maxCoeff</span>(); <span class="comment">// max(1+2, 3+4) = 7</span></span><br><span class="line"></span><br><span class="line"><span class="comment">// 谱范数（最大奇异值，即 L2 范数）</span></span><br><span class="line"><span class="type">double</span> l2_norm = m.<span class="built_in">lpNorm</span><<span class="number">2</span>>();</span><br></pre></td></tr></table></figure>
</li>
</ol>
</div>
<footer class="post-footer">
<div class="post-eof"></div>
</footer>
</article>
<article itemscope itemtype="http://schema.org/Article" class="post-block" lang="zh-CN">
<link itemprop="mainEntityOfPage" href="https://zihchen7.github.io/2025/06/22/25-6-22/">
<span hidden itemprop="author" itemscope itemtype="http://schema.org/Person">
<meta itemprop="image" content="/images/web-app-manifest-192x192.png">
<meta itemprop="name" content="ZihChen7">
<meta itemprop="description" content="一个爱好机器人的小学生。email: ZihChen7@163.com ">
</span>
<span hidden itemprop="publisher" itemscope itemtype="http://schema.org/Organization">
<meta itemprop="name" content="A Blog">
</span>
<header class="post-header">
<h2 class="post-title" itemprop="name headline">
<a href="/2025/06/22/25-6-22/" class="post-title-link" itemprop="url">Optimization in Robot (Exercise 1)</a>
</h2>
<div class="post-meta">
<span class="post-meta-item">
<span class="post-meta-item-icon">
<i class="far fa-calendar"></i>
</span>
<span class="post-meta-item-text">发表于</span>
<time title="创建时间：2025-06-22 10:27:18" itemprop="dateCreated datePublished" datetime="2025-06-22T10:27:18+08:00">2025-06-22</time>
</span>
<span class="post-meta-item">
<span class="post-meta-item-icon">
<i class="far fa-calendar-check"></i>
</span>
<span class="post-meta-item-text">更新于</span>
<time title="修改时间：2025-06-26 20:13:05" itemprop="dateModified" datetime="2025-06-26T20:13:05+08:00">2025-06-26</time>
</span>
</div>
</header>
<div class="post-body" itemprop="articleBody">
<h1 id="前言"><a href="#前言" class="headerlink" title="前言"></a>前言</h1><p>课程的第一个作业是输入起始点和终止点，通过求解优化问题来生成路径平滑。与常规的搜索算法，采样算法的原理完全不同，却也能得到一条比较好的路径。</p>
<hr>
<h1 id="主节点"><a href="#主节点" class="headerlink" title="主节点"></a>主节点</h1><p> 主要用来初始化节点和接收话题消息。当存储 ‘/move_base_simple/goal’ 消息的容器满2个时，则进入处理：</p>
<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></pre></td><td class="code"><pre><span class="line"><span class="keyword">if</span> (startGoal.<span class="built_in">size</span>() == <span class="number">2</span>)</span><br><span class="line"> {</span><br><span class="line"> <span class="comment">// 设置点的数量</span></span><br><span class="line"> <span class="type">const</span> <span class="type">int</span> N = (startGoal.<span class="built_in">back</span>() - startGoal.<span class="built_in">front</span>()).<span class="built_in">norm</span>() / config.pieceLength;</span><br><span class="line"> <span class="comment">// 初始化存储内点的矩阵，不包括起点和终点</span></span><br><span class="line"> <span class="function">Eigen::Matrix2Xd <span class="title">innerPoints</span><span class="params">(<span class="number">2</span>, N - <span class="number">1</span>)</span></span>;</span><br><span class="line"> <span class="comment">// 根据线性插值填充内点</span></span><br><span class="line"> <span class="keyword">for</span> (<span class="type">int</span> i = <span class="number">0</span>; i < N - <span class="number">1</span>; ++i)</span><br><span class="line"> {</span><br><span class="line"> innerPoints.<span class="built_in">col</span>(i) = (startGoal.<span class="built_in">back</span>() - startGoal.<span class="built_in">front</span>()) * (i + <span class="number">1.0</span>) / N + startGoal.<span class="built_in">front</span>();</span><br><span class="line"> }</span><br><span class="line"> <span class="comment">// 创建路径平滑器</span></span><br><span class="line"> path_smoother::PathSmoother pathSmoother;</span><br><span class="line"> <span class="comment">// 配置路径平滑器，输入起点、终点、内点数量、障碍物和惩罚权重</span></span><br><span class="line"> pathSmoother.<span class="built_in">setup</span>(startGoal.<span class="built_in">front</span>(), startGoal.<span class="built_in">back</span>(), N, config.circleObs, config.penaltyWeight);</span><br><span class="line"> <span class="comment">// 创建曲线</span></span><br><span class="line"> CubicCurve curve;</span><br><span class="line"> <span class="comment">// 使用路径平滑器优化曲线</span></span><br><span class="line"> <span class="keyword">if</span> (std::<span class="built_in">isinf</span>(pathSmoother.<span class="built_in">optimize</span>(curve, innerPoints, config.relCostTol)))</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"> <span class="keyword">if</span> (curve.<span class="built_in">getPieceNum</span>() > <span class="number">0</span>)</span><br><span class="line"> {</span><br><span class="line"> visualizer.<span class="built_in">visualize</span>(curve);</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><h2 id="结构体"><a href="#结构体" class="headerlink" title="结构体"></a>结构体</h2><p> 这个结构体是实现优化过程的部分，通过设置代价函数，并实现优化，结构如下：</p>
<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></pre></td><td class="code"><pre><span class="line"><span class="keyword">class</span> <span class="title class_">PathSmoother</span></span><br><span class="line"> {</span><br><span class="line"> <span class="keyword">private</span>:</span><br><span class="line"> cubic_spline::CubicSpline cubSpline;</span><br><span class="line"></span><br><span class="line"> <span class="type">int</span> pieceN;</span><br><span class="line"> Eigen::Matrix3Xd diskObstacles;</span><br><span class="line"> <span class="type">double</span> penaltyWeight;</span><br><span class="line"> Eigen::Vector2d headP;</span><br><span class="line"> Eigen::Vector2d tailP;</span><br><span class="line"> Eigen::Matrix2Xd points;</span><br><span class="line"> Eigen::Matrix2Xd gradByPoints;</span><br><span class="line"> <span class="comment">// L-BFGS 求解器</span></span><br><span class="line"> lbfgs::<span class="type">lbfgs_parameter_t</span> lbfgs_params;</span><br><span class="line"></span><br><span class="line"> <span class="keyword">private</span>:</span><br><span class="line"> <span class="function"><span class="type">static</span> <span class="keyword">inline</span> <span class="type">double</span> <span class="title">costFunction</span><span class="params">(<span class="type">void</span> *ptr,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> Eigen::VectorXd &x,</span></span></span><br><span class="line"><span class="params"><span class="function"> Eigen::VectorXd &g)</span>；</span></span><br><span class="line"><span class="function"></span></span><br><span class="line"><span class="function"> <span class="keyword">public</span>:</span></span><br><span class="line"><span class="function"> inline bool setup(const Eigen::Vector2d &initialP,</span></span><br><span class="line"><span class="function"> const Eigen::Vector2d &terminalP,</span></span><br><span class="line"><span class="function"> const int &pieceNum,</span></span><br><span class="line"><span class="function"> const Eigen::Matrix3Xd &diskObs,</span></span><br><span class="line"><span class="function"> const double penaWeight)；</span></span><br><span class="line"><span class="function"> </span></span><br><span class="line"><span class="function"> inline double optimize(CubicCurve &curve,</span></span><br><span class="line"><span class="function"> const Eigen::Matrix2Xd &iniInPs,</span></span><br><span class="line"><span class="function"> const double &relCostTol)；</span></span><br><span class="line"><span class="function"> };</span></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><p> 首先，作业完成的代价函数。</p>
<p> 能量代价将在<strong>三次样条插值</strong>部分介绍。在这里先介绍障碍物代价，工程中的障碍物只由圆形组成，在diskObstacles分别存储圆心x，圆心y，半径信息。代价函数表示为：</p>
<script type="math/tex; mode=display">
P = 1000 \sum_{i=1}^{N-1} \sum_{j=1}^{M} \left( r_j - \sqrt{(x_i - a_j)^2 + (y_i - b_j)^2} \right)</script><p> 梯度也比较好求，分别为</p>
<script type="math/tex; mode=display">
\frac{dP}{dx_i} = -1000 \sum_{j=1}^M \frac{x_i - a_j}{\sqrt{(x_i - a_j)^2 + (y_i - b_j)^2}} \\
\frac{dP}{dy_i} = -1000 \sum_{j=1}^M \frac{y_i - b_j}{\sqrt{(x_i - a_j)^2 + (y_i - b_j)^2}}</script><p> 具体实现如下：<br><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="type">static</span> <span class="keyword">inline</span> <span class="type">double</span> <span class="title">costFunction</span><span class="params">(<span class="type">void</span> *ptr,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> Eigen::VectorXd &x,</span></span></span><br><span class="line"><span class="params"><span class="function"> Eigen::VectorXd &g)</span></span></span><br><span class="line"><span class="function"> </span>{</span><br><span class="line"> <span class="comment">//设置指针</span></span><br><span class="line"> <span class="keyword">auto</span> smooth_ptr = <span class="built_in">reinterpret_cast</span>< PathSmoother*>( ptr) ;</span><br><span class="line"></span><br><span class="line"> <span class="comment">/* 初始化 */</span></span><br><span class="line"> <span class="type">int</span> nums = smooth_ptr->pieceN<span class="number">-1</span>;</span><br><span class="line"> <span class="type">double</span> energy_cost = <span class="number">0</span> , obstacle_cost = <span class="number">0</span>;</span><br><span class="line"> <span class="comment">// 初始化总梯度、能量梯度、障碍梯度</span></span><br><span class="line"> Eigen::Matrix2Xd grad = Eigen::Matrix2Xd::<span class="built_in">Zero</span>(<span class="number">2</span>, nums);</span><br><span class="line"> Eigen::Matrix2Xd energy_grad = Eigen::Matrix2Xd::<span class="built_in">Zero</span>(<span class="number">2</span>, nums);</span><br><span class="line"> Eigen::Matrix2Xd obstacle_grad = Eigen::Matrix2Xd::<span class="built_in">Zero</span>(<span class="number">2</span>, nums);</span><br><span class="line"></span><br><span class="line"> <span class="comment">// 将 VectorXd 的 x 转化为 Matrix2Xd</span></span><br><span class="line"> Eigen::Matrix2Xd inPs = Eigen::Matrix2Xd::<span class="built_in">Zero</span>(<span class="number">2</span>, nums);</span><br><span class="line"> inPs.<span class="built_in">row</span>(<span class="number">0</span>) = x.<span class="built_in">head</span>(nums);</span><br><span class="line"> inPs.<span class="built_in">row</span>(<span class="number">1</span>) = x.<span class="built_in">tail</span>(nums);</span><br><span class="line"></span><br><span class="line"> <span class="comment">/* 能耗代价 */</span></span><br><span class="line"> <span class="comment">// 输入内点</span></span><br><span class="line"> smooth_ptr->cubSpline.<span class="built_in">setInnerPoints</span>(inPs);</span><br><span class="line"> <span class="comment">// 获取能耗的代价和梯度</span></span><br><span class="line"> smooth_ptr->cubSpline.<span class="built_in">getStretchEnergy</span>(energy_cost);</span><br><span class="line"> smooth_ptr->cubSpline.<span class="built_in">getGrad</span>(energy_grad);</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="keyword">for</span>(<span class="type">int</span> i = <span class="number">0</span> ; i < nums ; ++i)</span><br><span class="line"> <span class="keyword">for</span>(<span class="type">int</span> j = <span class="number">0</span> ; j < smooth_ptr->diskObstacles.<span class="built_in">cols</span>() ;++j )</span><br><span class="line"> {</span><br><span class="line"> Eigen::Vector2d diff = inPs.<span class="built_in">col</span>(i) - smooth_ptr->diskObstacles.<span class="built_in">col</span>(j).<span class="built_in">head</span>(<span class="number">2</span>);</span><br><span class="line"> <span class="type">double</span> dis = diff.<span class="built_in">norm</span>();</span><br><span class="line"> <span class="type">double</span> delta = smooth_ptr-><span class="built_in">diskObstacles</span>(<span class="number">2</span>,j) -dis ;</span><br><span class="line"> <span class="keyword">if</span>(delta > <span class="number">0</span>)</span><br><span class="line"> {</span><br><span class="line"> obstacle_cost+= smooth_ptr->penaltyWeight * delta;</span><br><span class="line"> obstacle_grad.<span class="built_in">col</span>(i) += - smooth_ptr->penaltyWeight * diff /dis ;</span><br><span class="line"> }</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="type">double</span> cost = obstacle_cost+energy_cost;</span><br><span class="line"> grad = obstacle_grad + energy_grad;</span><br><span class="line"></span><br><span class="line"> <span class="comment">// 将 Matrix2Xd 的 x 转化为 VectorXd</span></span><br><span class="line"> g.<span class="built_in">setZero</span>();</span><br><span class="line"> g.<span class="built_in">head</span>(nums) = grad.<span class="built_in">row</span>(<span class="number">0</span>).<span class="built_in">transpose</span>();</span><br><span class="line"> g.<span class="built_in">tail</span>(nums) = grad.<span class="built_in">row</span>(<span class="number">1</span>).<span class="built_in">transpose</span>();</span><br><span class="line"></span><br><span class="line"> <span class="keyword">return</span> cost;</span><br><span class="line"> }</span><br></pre></td></tr></table></figure></p>
<h2 id="优化调用"><a href="#优化调用" class="headerlink" title="优化调用"></a>优化调用</h2><p> 我注意到这个函数是static 函数，这个函数的用法很奇怪，等会和optimize() 函数一起说，</p>
<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">inline</span> <span class="type">double</span> <span class="title">optimize</span><span class="params">(CubicCurve &curve,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> Eigen::Matrix2Xd &iniInPs,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> <span class="type">double</span> &relCostTol)</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> <span class="type">double</span> minCost = <span class="number">0</span>;</span><br><span class="line"> <span class="comment">// lbfgs_optimize 需要输入 VectorXd 的数据,先创建一个2 * (n-1) 的vector</span></span><br><span class="line"> Eigen::VectorXd x = Eigen::VectorXd::<span class="built_in">Map</span>(iniInPs.<span class="built_in">data</span>(),<span class="number">2</span>*(pieceN<span class="number">-1</span>));</span><br><span class="line"> <span class="comment">// L-GFGS优化的函数接口</span></span><br><span class="line"> <span class="type">int</span> status = <span class="built_in">lbfgs_optimize</span>(iniInPs, minCost, & costFunction,<span class="literal">nullptr</span>,<span class="keyword">this</span>, lbfgs_params);</span><br><span class="line"></span><br><span class="line"> <span class="keyword">if</span>(status >= <span class="number">0</span> )</span><br><span class="line"> {</span><br><span class="line"> cubSpline.<span class="built_in">getCurve</span>();</span><br><span class="line"> }</span><br><span class="line"> <span class="keyword">else</span> </span><br><span class="line"> std::cout << <span class="string">"Generation failed!"</span> << std::endl;</span><br><span class="line"> <span class="keyword">return</span> minCost;</span><br><span class="line">}</span><br></pre></td></tr></table></figure>
<p> lbfgs_optimize 函数在optimize中，传入的是this，是PathSmoother<em> 的类型的。在传入lbfgs_optimize函数中，变为了void</em> ；在lbfgs_optimize函数中创建回调结构体，结构体中设置代价函数的地址，随后在调用proc_evaluate时，又传入instance的地址，即PathSmoother<em> 的this指针。在costFunction函数中又将的void</em> ptr还原成PathSmoother*。</p>
<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></pre></td><td class="code"><pre><span class="line"><span class="function"><span class="keyword">inline</span> <span class="type">int</span> <span class="title">lbfgs_optimize</span><span class="params">(Eigen::VectorXd &x,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">double</span> &f,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">lbfgs_evaluate_t</span> proc_evaluate,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">lbfgs_progress_t</span> proc_progress,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">void</span> *instance,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> <span class="type">lbfgs_parameter_t</span> &param)</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> ...</span><br><span class="line"> <span class="comment">/* Construct a callback data. */</span></span><br><span class="line"> <span class="type">callback_data_t</span> cd;</span><br><span class="line"> cd.instance = instance;</span><br><span class="line"> cd.proc_evaluate = proc_evaluate;</span><br><span class="line"> cd.proc_progress = proc_progress;</span><br><span class="line"></span><br><span class="line"> <span class="comment">/* Evaluate the function value and its gradient. */</span></span><br><span class="line"> fx = cd.<span class="built_in">proc_evaluate</span>(cd.instance, x, g);</span><br><span class="line"> ...</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">typedef</span> <span class="title">double</span> <span class="params">(*<span class="type">lbfgs_evaluate_t</span>)</span><span class="params">(<span class="type">void</span> *instance,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> Eigen::VectorXd &x,</span></span></span><br><span class="line"><span class="params"><span class="function"> Eigen::VectorXd &g)</span></span>;</span><br></pre></td></tr></table></figure>
<p> 实现void <em> 和 PathSmoother </em> 的来回转换本人还不是特别理解。</p>
<h1 id="三次样条插值"><a href="#三次样条插值" class="headerlink" title="三次样条插值"></a>三次样条插值</h1><h2 id="特性"><a href="#特性" class="headerlink" title="特性"></a>特性</h2><p> 三次样条由于n+1点构成（y0,y1, … , yn）。第i段的曲线表达为：</p>
<script type="math/tex; mode=display">
Y_i(t) = a_i + b_i t + c_i t^2 + d_i t^3 ，t \in [0, 1]</script><p> 在段与段的交接处，除了曲线连续外，要保证一阶二阶导数也连续，所以有</p>
<script type="math/tex; mode=display">
a_i = y_i \\
b_i = D_i \\
c_i = 3(y_{i+1} - y_i) - 2D_i - D_{i+1} \\
d_i = 2(y_i - y_{i+1}) + D_i + D_{i+1}</script><p> 整合起来，得到最终的系统为：</p>
<script type="math/tex; mode=display">
\begin{bmatrix}
4 & 1 & & & & \\
1 & 4 & 1 & & & \\
& 1 & 4 & 1 & & \\
& & \ddots & \ddots & \ddots & \\
& & & 1 & 4 & 1 \\
& & & & 1 & 4 \\
\end{bmatrix}
\begin{bmatrix}
D_1 \\
D_2 \\
D_3 \\
D_4 \\
\vdots \\
D_{n-2} \\
D_{n-1}
\end{bmatrix}
=
\begin{bmatrix}
3(y_2 - y_0) \\
3(y_3 - y_1) \\
3(y_4 - y_2) \\
3(y_5 - y_3)\\
\vdots \\
3(y_{n-1} - y_{n-3}) \\
3(y_n - y_{n-2})
\end{bmatrix}</script><p> 其中D0 = Dn =0对于曲线的能量函数，表达为</p>
<script type="math/tex; mode=display">
\mathrm{Energy}(x_{1}, x_{2}, \dots, x_{n-1}) = \sum_{i=0}^{n-1} \int_{0}^{1} \| p_{i}^{(2)}(s) \|^{2} \, ds</script><p> 对函数进行求导后平方，再积分得到最终的能量代价为</p>
<script type="math/tex; mode=display">
\sum_{i=0}^{n-1} f_i = \sum_{i=0}^{n-1} \left( 12 d_i^\top d_i + 12 c_i^\top d_i + 4 c_i^\top c_i \right)</script><p> 一阶梯度则为</p>
<script type="math/tex; mode=display">
\frac{df_i}{dx} = 24 \left( \frac{d d_i}{dx} \right)^\top d_i
+ 12 \left( \frac{d c_i}{dx} \right)^\top d_i
+ 12 \left( \frac{d d_i}{dx} \right)^\top c_i
+ 8 \left( \frac{d c_i}{dx} \right)^\top c_i</script><p> 一阶梯度需要分别求d(di)/dx 和 d(ci)/dx，为：</p>
<script type="math/tex; mode=display">
\begin{aligned}
\frac{d d_i}{d x} &= 2 \frac{d(x_i - x_{i+1})}{d x} + \frac{d D_i}{d x} + \frac{d D_{i+1}}{d x}, \\
\frac{d c_i}{d x} &= 3 \frac{d(x_{i+1} - x_i)}{d x} - 2 \frac{d D_i}{d x} - \frac{d D_{i+1}}{d x}.
\end{aligned}</script><p> 梯度的第一项为</p>
<script type="math/tex; mode=display">
\frac{d}{dx}
\begin{pmatrix}
x_0 - x_1 \\
\vdots \\
x_{i-1} - x_i \\
\vdots \\
x_{n-1} - x_n
\end{pmatrix}_{n \times 1}
=
\begin{pmatrix}
-1 & & & & \\
1 & -1 & & & \\
& 1 & -1 & & \\
& & \ddots & \ddots & \\
& & & 1 & -1 \\
& & & & 1
\end{pmatrix}_{n \times (n-1)}</script><p> 后面Di的导数可以从上面最终的系统的方程倒推。令过程变量为：</p>
<script type="math/tex; mode=display">
A_{(n-1) \times (n-1)} =
\begin{pmatrix}
4 & 1 & & & \\
1 & 4 & 1 & & \\
& 1 & 4 & 1 & \\
& & \ddots & \ddots & \ddots \\
& & & 1 & 4 & 1 \\
& & & & 1 & 4
\end{pmatrix} \\</script><script type="math/tex; mode=display">
B_{(n-1) \times 1} = 3
\begin{bmatrix}
x_2 - x_0 \\
x_3 - x_1 \\
x_4 - x_2 \\
\vdots \\
x_n - x_{n-2}
\end{bmatrix}</script><script type="math/tex; mode=display">
D_{(n-1) \times 1} =
\begin{bmatrix}
D_1 \\
D_2 \\
\vdots \\
D_{n-1}
\end{bmatrix}.</script><p> 那么D = A^(-1) <em> B，那么d(Di)/dx = A^(-1) </em> d(B)/dx。</p>
<script type="math/tex; mode=display">
\frac{dB}{dx} =
\begin{pmatrix}
0 & 3 & & & & \\
-3 & 0 & 3 & & & \\
& -3 & 0 & 3 & & \\
& & \ddots & \ddots & \ddots & \\
& & & -3 & 0 & 3 \\
& & & & -3 & 0
\end{pmatrix}_{(n-1) \times (n-1)}</script><p> 最终回带，就求得了能量函数的一阶梯度。具体可参照：</p>
<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">https://mathworld.wolfram.com/CubicSpline.html</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><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></pre></td><td class="code"><pre><span class="line"> <span class="keyword">class</span> <span class="title class_">CubicSpline</span></span><br><span class="line"> {</span><br><span class="line"> <span class="keyword">public</span>:</span><br><span class="line"> <span class="built_in">CubicSpline</span>() = <span class="keyword">default</span>;</span><br><span class="line"> ~<span class="built_in">CubicSpline</span>() { A.<span class="built_in">destroy</span>(); }</span><br><span class="line"></span><br><span class="line"> <span class="keyword">private</span>:</span><br><span class="line"> <span class="type">int</span> N;</span><br><span class="line"> Eigen::Vector2d headP;</span><br><span class="line"> Eigen::Vector2d tailP;</span><br><span class="line"> <span class="comment">//带状矩阵 </span></span><br><span class="line"> BandedSystem A;</span><br><span class="line"> Eigen::MatrixX2d b;</span><br><span class="line"> <span class="comment">// 存储系数矩阵(a,b,c,d)</span></span><br><span class="line"> Eigen::Matrix2Xd coeffs;</span><br><span class="line"> <span class="comment">// 存储过程梯度</span></span><br><span class="line"> Eigen::MatrixXd partial_c,partial_d;</span><br><span class="line"></span><br><span class="line"> <span class="keyword">public</span>:</span><br><span class="line"> <span class="comment">// 初始化系统</span></span><br><span class="line"> <span class="function"><span class="keyword">inline</span> <span class="type">void</span> <span class="title">setConditions</span><span class="params">(<span class="type">const</span> Eigen::Vector2d &headPos,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> Eigen::Vector2d &tailPos,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> <span class="type">int</span> &pieceNum)</span></span>;</span><br><span class="line"><span class="comment">// 输入内点，获取系数矩阵</span></span><br><span class="line"> <span class="function"><span class="keyword">inline</span> <span class="type">void</span> <span class="title">setInnerPoints</span><span class="params">(<span class="type">const</span> Eigen::Ref<<span class="type">const</span> Eigen::Matrix2Xd> &inPs)</span></span>;</span><br><span class="line"><span class="comment">// 根据系数矩阵得到曲线</span></span><br><span class="line"> <span class="function"><span class="keyword">inline</span> <span class="type">void</span> <span class="title">getCurve</span><span class="params">(CubicCurve &curve)</span> <span class="type">const</span></span>;</span><br><span class="line"><span class="comment">// 能量代价</span></span><br><span class="line"> <span class="function"><span class="keyword">inline</span> <span class="type">void</span> <span class="title">getStretchEnergy</span><span class="params">(<span class="type">double</span> &energy)</span> <span class="type">const</span></span>;</span><br><span class="line"><span class="comment">// 获取系数矩阵</span></span><br><span class="line"> <span class="function"><span class="keyword">inline</span> <span class="type">const</span> Eigen::MatrixX2d &<span class="title">getCoeffs</span><span class="params">(<span class="type">void</span>)</span> <span class="type">const</span></span>;</span><br><span class="line"><span class="comment">// 梯度</span></span><br><span class="line"> <span class="function"><span class="keyword">inline</span> <span class="type">void</span> <span class="title">getGrad</span><span class="params">(Eigen::Ref<Eigen::Matrix2Xd> gradByPoints)</span> <span class="type">const</span></span>;</span><br><span class="line"> <span class="comment">// 计算梯度的过程梯度</span></span><br><span class="line"> <span class="function"><span class="keyword">inline</span> <span class="type">void</span> <span class="title">getPartial</span><span class="params">()</span></span>;</span><br><span class="line"> };</span><br></pre></td></tr></table></figure>
<h2 id="计算系数矩阵"><a href="#计算系数矩阵" class="headerlink" title="计算系数矩阵"></a>计算系数矩阵</h2><p> 在初始化系统后，输入内点，就可以计算系数了。计算的思路是利用LU分解计算 A*D=b，计算过程变量D后，回带进ai，bi，ci，di的公式中，就可以得到系数矩阵。</p>
<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">inline</span> <span class="type">void</span> <span class="title">setInnerPoints</span><span class="params">(<span class="type">const</span> Eigen::Ref<<span class="type">const</span> Eigen::Matrix2Xd> &inPs)</span></span></span><br><span class="line"><span class="function"> </span>{</span><br><span class="line"> b.<span class="built_in">setZero</span>();</span><br><span class="line"> <span class="comment">/* 先计算 D = A^-1 * b ,使用LU分解计算 A*D=b D为变量 */</span></span><br><span class="line"> <span class="comment">// 填充b矩阵，inPs 内 设置的是除了起点终点的中间点，所以b的开头结尾需要单独处理</span></span><br><span class="line"> b.<span class="built_in">row</span>(<span class="number">0</span>) = <span class="number">3</span> * (inPs.<span class="built_in">col</span>(<span class="number">1</span>).<span class="built_in">transpose</span>()- headP.<span class="built_in">transpose</span>() );</span><br><span class="line"> b.<span class="built_in">row</span>(N<span class="number">-2</span>) = <span class="number">3</span> * (tailP.<span class="built_in">transpose</span>()-inPs.<span class="built_in">col</span>(N<span class="number">-3</span>).<span class="built_in">transpose</span>() );</span><br><span class="line"> <span class="keyword">for</span>(<span class="type">int</span> i = <span class="number">1</span>; i < N<span class="number">-2</span> ;++i)</span><br><span class="line"> {</span><br><span class="line"> b.<span class="built_in">row</span>(i) = <span class="number">3</span> * (inPs.<span class="built_in">col</span>(i<span class="number">+1</span>).<span class="built_in">transpose</span>()-inPs.<span class="built_in">col</span>(i<span class="number">-1</span>).<span class="built_in">transpose</span>() );</span><br><span class="line"> }</span><br><span class="line"> </span><br><span class="line"> <span class="comment">// 填充A矩阵</span></span><br><span class="line"> A.<span class="built_in">reset</span>(); </span><br><span class="line"> <span class="built_in">A</span>(<span class="number">0</span>,<span class="number">0</span>)=<span class="number">4</span>,<span class="built_in">A</span>(<span class="number">0</span>,<span class="number">1</span>)=<span class="number">1</span>;</span><br><span class="line"> <span class="built_in">A</span>(N<span class="number">-2</span>,N<span class="number">-2</span>)=<span class="number">4</span>,<span class="built_in">A</span>(N<span class="number">-2</span>,N<span class="number">-3</span>)=<span class="number">1</span>;</span><br><span class="line"> <span class="keyword">for</span>(<span class="type">int</span> i = <span class="number">1</span>; i < N<span class="number">-2</span> ;++i)</span><br><span class="line"> {</span><br><span class="line"> <span class="built_in">A</span>(i,i) = <span class="number">4</span>, <span class="built_in">A</span>(i,i<span class="number">-1</span>) = <span class="built_in">A</span>(i,i<span class="number">+1</span>) = <span class="number">1</span>; </span><br><span class="line"> }</span><br><span class="line"></span><br><span class="line"> <span class="comment">// LU分解后求解，存入b</span></span><br><span class="line"> A.<span class="built_in">factorizeLU</span>();</span><br><span class="line"> A.<span class="built_in">solve</span>(b);</span><br><span class="line"></span><br><span class="line"> <span class="comment">// 初始化coeffs</span></span><br><span class="line"> coeffs.<span class="built_in">resize</span>(<span class="number">2</span>,<span class="number">4</span>*N);</span><br><span class="line"> coeffs.<span class="built_in">setZero</span>();</span><br><span class="line"></span><br><span class="line"> coeffs.<span class="built_in">col</span>(<span class="number">0</span>) = headP;</span><br><span class="line"> coeffs.<span class="built_in">col</span>(<span class="number">1</span>) = Eigen::Vector2d::<span class="built_in">Zero</span>();</span><br><span class="line"> coeffs.<span class="built_in">col</span>(<span class="number">2</span>) = <span class="number">3</span>*(inPs.<span class="built_in">col</span>(<span class="number">0</span>) - headP) -b.<span class="built_in">row</span>(<span class="number">0</span>).<span class="built_in">transpose</span>();</span><br><span class="line"> coeffs.<span class="built_in">col</span>(<span class="number">3</span>) = <span class="number">2</span>*(headP - inPs.<span class="built_in">col</span>(<span class="number">0</span>)) +b.<span class="built_in">row</span>(<span class="number">0</span>).<span class="built_in">transpose</span>();</span><br><span class="line"></span><br><span class="line"> coeffs.<span class="built_in">col</span>(<span class="number">4</span>*N<span class="number">-4</span>) = inPs.<span class="built_in">col</span>(N<span class="number">-2</span>);</span><br><span class="line"> coeffs.<span class="built_in">col</span>(<span class="number">4</span>*N<span class="number">-3</span>) = b.<span class="built_in">row</span>(N<span class="number">-2</span>).<span class="built_in">transpose</span>();</span><br><span class="line"> coeffs.<span class="built_in">col</span>(<span class="number">4</span>*N<span class="number">-2</span>) = <span class="number">3</span>*(tailP - inPs.<span class="built_in">col</span>(N<span class="number">-2</span>)) <span class="number">-2</span> * b.<span class="built_in">row</span>(N<span class="number">-2</span>).<span class="built_in">transpose</span>();</span><br><span class="line"> coeffs.<span class="built_in">col</span>(<span class="number">4</span>*N<span class="number">-1</span>) = <span class="number">2</span>*(inPs.<span class="built_in">col</span>(N<span class="number">-2</span>) - tailP) +b.<span class="built_in">row</span>(N<span class="number">-2</span>).<span class="built_in">transpose</span>();</span><br><span class="line"></span><br><span class="line"> <span class="keyword">for</span>(<span class="type">int</span> i = <span class="number">1</span> ; i<N<span class="number">-1</span>;++i)</span><br><span class="line"> {</span><br><span class="line"> coeffs.<span class="built_in">col</span>(<span class="number">4</span>*i) = inPs.<span class="built_in">col</span>(i<span class="number">-1</span>);</span><br><span class="line"> coeffs.<span class="built_in">col</span>(<span class="number">4</span>*i<span class="number">+1</span>) = b.<span class="built_in">row</span>(i<span class="number">-1</span>).<span class="built_in">transpose</span>();</span><br><span class="line"> coeffs.<span class="built_in">col</span>(<span class="number">4</span>*i<span class="number">+2</span>) = <span class="number">3</span>*(inPs.<span class="built_in">col</span>(i) - inPs.<span class="built_in">col</span>(i<span class="number">-1</span>)) <span class="number">-2</span> * b.<span class="built_in">row</span>(i<span class="number">-1</span>).<span class="built_in">transpose</span>()-b.<span class="built_in">row</span>(i).<span class="built_in">transpose</span>() ;</span><br><span class="line"> coeffs.<span class="built_in">col</span>(<span class="number">4</span>*i<span class="number">+3</span>) = <span class="number">2</span>*(inPs.<span class="built_in">col</span>(i<span class="number">-1</span>) - inPs.<span class="built_in">col</span>(i)) +b.<span class="built_in">row</span>(i<span class="number">-1</span>).<span class="built_in">transpose</span>()+ b.<span class="built_in">row</span>(i).<span class="built_in">transpose</span>() ;</span><br><span class="line"> }</span><br><span class="line"></span><br><span class="line"> <span class="comment">// 将系数转置后存入b</span></span><br><span class="line"> b.<span class="built_in">resize</span>(<span class="number">4</span>*N,<span class="number">2</span>);</span><br><span class="line"> b = coeffs.<span class="built_in">transpose</span>();</span><br><span class="line"> </span><br><span class="line"> <span class="keyword">return</span>;</span><br><span class="line"> }</span><br></pre></td></tr></table></figure>
<h2 id="计算梯度"><a href="#计算梯度" class="headerlink" title="计算梯度"></a>计算梯度</h2><p> 最终的梯度比较难计算的是d(ci)/dx和d(di)/dx，其中的过程梯度比较多，所以先用一个求解过程梯度函数进行预处理。</p>
<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></pre></td><td class="code"><pre><span class="line"><span class="function"><span class="keyword">inline</span> <span class="type">void</span> <span class="title">getPartial</span><span class="params">()</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> <span class="comment">/* 首先求解 partial_D */</span></span><br><span class="line"> <span class="comment">// 填充A矩阵</span></span><br><span class="line"> Eigen::MatrixXd A_tmp=Eigen::MatrixXd::<span class="built_in">Zero</span>(N<span class="number">-1</span>,N<span class="number">-1</span>); </span><br><span class="line"> <span class="built_in">A_tmp</span>(<span class="number">0</span>,<span class="number">0</span>)=<span class="number">4</span>,<span class="built_in">A_tmp</span>(<span class="number">0</span>,<span class="number">1</span>)=<span class="number">1</span>;</span><br><span class="line"> <span class="built_in">A_tmp</span>(N<span class="number">-2</span>,N<span class="number">-2</span>)=<span class="number">4</span>,<span class="built_in">A_tmp</span>(N<span class="number">-2</span>,N<span class="number">-3</span>)=<span class="number">1</span>;</span><br><span class="line"> <span class="keyword">for</span>(<span class="type">int</span> i = <span class="number">1</span>; i < N<span class="number">-2</span> ;++i)</span><br><span class="line"> {</span><br><span class="line"> <span class="built_in">A_tmp</span>(i,i) = <span class="number">4</span>, <span class="built_in">A_tmp</span>(i,i<span class="number">-1</span>) = <span class="built_in">A_tmp</span>(i,i<span class="number">+1</span>) = <span class="number">1</span>; </span><br><span class="line"> }</span><br><span class="line"> <span class="comment">//填充partial_b矩阵</span></span><br><span class="line"> Eigen::MatrixXd partial_b = Eigen::MatrixXd::<span class="built_in">Zero</span>(N<span class="number">-1</span>,N<span class="number">-1</span>);</span><br><span class="line"> <span class="built_in">partial_b</span>(<span class="number">0</span>,<span class="number">0</span>)=<span class="number">0</span>,<span class="built_in">partial_b</span>(<span class="number">0</span>,<span class="number">1</span>)=<span class="number">3</span>;</span><br><span class="line"> <span class="built_in">partial_b</span>(N<span class="number">-2</span>,N<span class="number">-2</span>)=<span class="number">0</span>,<span class="built_in">partial_b</span>(N<span class="number">-2</span>,N<span class="number">-3</span>)=<span class="number">-3</span>;</span><br><span class="line"> <span class="keyword">for</span>(<span class="type">int</span> i = <span class="number">1</span>; i < N<span class="number">-2</span> ;++i)</span><br><span class="line"> {</span><br><span class="line"> <span class="built_in">partial_b</span>(i,i) = <span class="number">0</span>, <span class="built_in">partial_b</span>(i,i<span class="number">-1</span>) = <span class="number">-3</span>, <span class="built_in">partial_b</span>(i,i<span class="number">+1</span>) = <span class="number">3</span>; </span><br><span class="line"> }</span><br><span class="line"> <span class="comment">// partial_D = A^-1 * partial_b </span></span><br><span class="line"> <span class="comment">// 个人觉得也能够用代价函数里LU分解的方式求partial_D</span></span><br><span class="line"> Eigen::MatrixXd partial_D = A_tmp.<span class="built_in">inverse</span>() * partial_b;</span><br><span class="line"> </span><br><span class="line"> <span class="comment">/* 其次求解partial_delta*/</span></span><br><span class="line"> Eigen::MatrixXd partial_delta = Eigen::MatrixXd::<span class="built_in">Zero</span>(N,N<span class="number">-1</span>);</span><br><span class="line"> <span class="comment">//填充partial_delta矩阵</span></span><br><span class="line"> <span class="built_in">partial_delta</span>(<span class="number">0</span>,<span class="number">0</span>) = <span class="number">-1</span>;</span><br><span class="line"> <span class="built_in">partial_delta</span>(N<span class="number">-1</span>,N<span class="number">-2</span>) = <span class="number">1</span>;</span><br><span class="line"> <span class="keyword">for</span>(<span class="type">int</span> i = <span class="number">1</span>; i < N<span class="number">-1</span> ;++i)</span><br><span class="line"> {</span><br><span class="line"> <span class="built_in">partial_delta</span>(i,i) =<span class="number">-1</span>, <span class="built_in">partial_delta</span>(i,i<span class="number">-1</span>) = <span class="number">1</span>; </span><br><span class="line"> }</span><br><span class="line"> </span><br><span class="line"> <span class="comment">/*求解最终的partial_c，partial_d*/</span></span><br><span class="line"> partial_c.<span class="built_in">resize</span>(N,N<span class="number">-1</span>);</span><br><span class="line"> partial_c.<span class="built_in">setZero</span>();</span><br><span class="line"> partial_d.<span class="built_in">resize</span>(N,N<span class="number">-1</span>);</span><br><span class="line"> partial_d.<span class="built_in">setZero</span>(); </span><br><span class="line"> <span class="comment">// 填写最终的矩阵系数</span></span><br><span class="line"> Eigen::MatrixXd Q_c = Eigen::MatrixXd::<span class="built_in">Zero</span>(N,N<span class="number">-1</span>); </span><br><span class="line"> Eigen::MatrixXd Q_d = Eigen::MatrixXd::<span class="built_in">Zero</span>(N,N<span class="number">-1</span>); </span><br><span class="line"> <span class="comment">//因为D0=Dn = 0 。而 partial_D 是从D1~Dn-1的 </span></span><br><span class="line"> <span class="built_in">Q_c</span>(<span class="number">0</span>,<span class="number">0</span>) = <span class="number">-1</span>,<span class="built_in">Q_c</span>(N<span class="number">-1</span>,N<span class="number">-2</span>)=<span class="number">-2</span>;</span><br><span class="line"> <span class="built_in">Q_d</span>(<span class="number">0</span>,<span class="number">0</span>) = <span class="number">1</span>,<span class="built_in">Q_d</span>(N<span class="number">-1</span>,N<span class="number">-2</span>) = <span class="number">1</span>;</span><br><span class="line"> <span class="keyword">for</span>(<span class="type">int</span> i = <span class="number">1</span> ;i< N<span class="number">-1</span>;++i)</span><br><span class="line"> {</span><br><span class="line"> <span class="built_in">Q_c</span>(i,i) = <span class="number">-1</span>,<span class="built_in">Q_c</span>(i,i<span class="number">-1</span>)=<span class="number">-2</span>;</span><br><span class="line"> <span class="built_in">Q_d</span>(i,i) = <span class="number">1</span>,<span class="built_in">Q_d</span>(i,i<span class="number">-1</span>)=<span class="number">1</span>;</span><br><span class="line"> }</span><br><span class="line"> partial_c = <span class="number">-3</span> * partial_delta + Q_c * partial_D;</span><br><span class="line"> partial_d = <span class="number">2</span> * partial_delta + Q_d * partial_D;</span><br><span class="line">}</span><br></pre></td></tr></table></figure>
<p> 最终得到梯度：</p>
<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></pre></td><td class="code"><pre><span class="line"><span class="function"><span class="keyword">inline</span> <span class="type">void</span> <span class="title">getGrad</span><span class="params">(Eigen::Ref<Eigen::Matrix2Xd> gradByPoints)</span> <span class="type">const</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> gradByPoints.<span class="built_in">setZero</span>();</span><br><span class="line"> <span class="keyword">for</span>(<span class="type">int</span> i = <span class="number">0</span>;i<N;++i)</span><br><span class="line"> {</span><br><span class="line"> Eigen::Vector2d ci = coeffs.<span class="built_in">col</span>(<span class="number">4</span>*i<span class="number">+2</span>);</span><br><span class="line"> Eigen::Vector2d di = coeffs.<span class="built_in">col</span>(<span class="number">4</span>*i<span class="number">+3</span>);</span><br><span class="line"> gradByPoints += (<span class="number">24</span>*di + <span class="number">12</span>*ci)*partial_d.<span class="built_in">row</span>(i)+</span><br><span class="line"> (<span class="number">12</span>*di + <span class="number">8</span>*ci)*partial_c.<span class="built_in">row</span>(i);</span><br><span class="line"> }</span><br><span class="line">}</span><br><span class="line"></span><br></pre></td></tr></table></figure>
<h1 id="L-BFGS"><a href="#L-BFGS" class="headerlink" title="L-BFGS"></a>L-BFGS</h1><h2 id="线搜索"><a href="#线搜索" class="headerlink" title="线搜索"></a>线搜索</h2><p> 通过weak wolfe原则和二分法结合，寻找步长。唯一还是不太理解的是二分法的选择，不满足<strong>充分下降条件</strong>时stp 缩小，而不满足<strong>曲线条件</strong>时stp 增大。</p>
<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></pre></td><td class="code"><pre><span class="line"><span class="function"><span class="keyword">inline</span> <span class="type">int</span> <span class="title">line_search_lewisoverton</span><span class="params">(Eigen::VectorXd &x,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">double</span> &f,</span></span></span><br><span class="line"><span class="params"><span class="function"> Eigen::VectorXd &g,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">double</span> &stp,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> Eigen::VectorXd &s,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> Eigen::VectorXd &xp,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> Eigen::VectorXd &gp,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> <span class="type">double</span> stpmin,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> <span class="type">double</span> stpmax,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> <span class="type">callback_data_t</span> &cd,</span></span></span><br><span class="line"><span class="params"><span class="function"> <span class="type">const</span> <span class="type">lbfgs_parameter_t</span> &param)</span></span></span><br><span class="line"><span class="function"></span>{</span><br><span class="line"> <span class="type">int</span> iter = <span class="number">0</span> ;</span><br><span class="line"> <span class="type">double</span> upper = stpmax, low = stpmin;</span><br><span class="line"> <span class="type">double</span> fx,fxp=f;</span><br><span class="line"> <span class="keyword">while</span>(<span class="literal">true</span>)</span><br><span class="line"> {</span><br><span class="line"> iter++;</span><br><span class="line"> <span class="keyword">if</span>(iter>param.max_linesearch)</span><br><span class="line"> <span class="keyword">break</span>;</span><br><span class="line"></span><br><span class="line"> <span class="comment">// 更新x,f(x)</span></span><br><span class="line"> x = xp + stp * s;</span><br><span class="line"> fx = cd.<span class="built_in">proc_evaluate</span>(cd.instance,x,g);</span><br><span class="line"></span><br><span class="line"> <span class="comment">/* weak wolfe条件 */</span></span><br><span class="line"> <span class="comment">// 如果 fx 下降不够（即 fxp - fx 太小，不满足不等式），说明 stp 太大</span></span><br><span class="line"> <span class="keyword">if</span>( fxp - fx < - stp * param.f_dec_coeff * gp.<span class="built_in">dot</span>(s))</span><br><span class="line"> {</span><br><span class="line"> upper = stp;</span><br><span class="line"> }</span><br><span class="line"> <span class="comment">// 如果新梯度 g.dot(s) 不够平缓（不满足不等式），说明 stp 太小</span></span><br><span class="line"> <span class="keyword">else</span> <span class="keyword">if</span>(g.<span class="built_in">dot</span>(s) < param.s_curv_coeff * gp.<span class="built_in">dot</span>(s))</span><br><span class="line"> {</span><br><span class="line"> low = stp;</span><br><span class="line"> }</span><br><span class="line"> <span class="comment">// 满足 Wolfe 条件</span></span><br><span class="line"> <span class="keyword">else</span></span><br><span class="line"> {</span><br><span class="line"> f = fx;</span><br><span class="line"> <span class="keyword">return</span> iter;</span><br><span class="line"> }</span><br><span class="line"></span><br><span class="line"> <span class="keyword">if</span> (upper < stpmax)</span><br><span class="line"> {</span><br><span class="line"> stp = <span class="number">0.5</span> * (low + upper);</span><br><span class="line"> }</span><br><span class="line"> <span class="keyword">else</span></span><br><span class="line"> {</span><br><span class="line"> stp = <span class="number">2</span> * low;</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> LBFGSERR_MAXIMUMLINESEARCH;</span><br><span class="line">}</span><br></pre></td></tr></table></figure>
<h2 id="优化"><a href="#优化" class="headerlink" title="优化"></a>优化</h2><p> 这一部分是项目自带的。</p>
<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></pre></td><td class="code"><pre><span class="line">输入:</span><br><span class="line"> - 初始点 x0</span><br><span class="line"> - 目标函数 f(x)</span><br><span class="line"> - 梯度计算函数 grad_f(x)</span><br><span class="line"> - 参数: </span><br><span class="line"> - 内存大小 m (存储的向量对数量)</span><br><span class="line"> - 最大迭代次数 max_iter</span><br><span class="line"> - 梯度容差 g_epsilon</span><br><span class="line"> - 线搜索参数 (Wolfe 条件系数)</span><br><span class="line"></span><br><span class="line">输出:</span><br><span class="line"> - 最优解 x</span><br><span class="line"> - 最优值 f(x)</span><br><span class="line"></span><br><span class="line">算法步骤:</span><br><span class="line"><span class="number">1.</span> 初始化:</span><br><span class="line"> - x = x0</span><br><span class="line"> - g = grad_f(x)</span><br><span class="line"> - d = -g <span class="comment"># 初始搜索方向 (负梯度)</span></span><br><span class="line"> - k = <span class="number">0</span> <span class="comment"># 迭代计数</span></span><br><span class="line"> - 初始化历史队列 S = [], Y = [] <span class="comment"># 存储 s_k = x_{k+1} - x_k, y_k = g_{k+1} - g_k</span></span><br><span class="line"></span><br><span class="line"><span class="number">2.</span> 检查初始点是否为驻点:</span><br><span class="line"> <span class="keyword">if</span> ||g||_inf < g_epsilon:</span><br><span class="line"> <span class="keyword">return</span> x, f(x)</span><br><span class="line"></span><br><span class="line"><span class="number">3.</span> 主循环 (<span class="keyword">while</span> k < max_iter):</span><br><span class="line"> a. 线搜索 (满足 Wolfe 条件):</span><br><span class="line"> - α = line_search(x, f, g, d) <span class="comment"># 找到步长 α</span></span><br><span class="line"> - x_new = x + α * d</span><br><span class="line"> - g_new = grad_f(x_new)</span><br><span class="line"></span><br><span class="line"> b. 检查收敛:</span><br><span class="line"> <span class="keyword">if</span> ||g_new||_inf < g_epsilon:</span><br><span class="line"> <span class="keyword">return</span> x_new, f(x_new)</span><br><span class="line"></span><br><span class="line"> c. 更新历史向量对:</span><br><span class="line"> - s_k = x_new - x</span><br><span class="line"> - y_k = g_new - g</span><br><span class="line"> - 将 (s_k, y_k) 加入队列 S, Y</span><br><span class="line"> - <span class="keyword">if</span> <span class="built_in">len</span>(S) > m: <span class="comment"># 限制内存</span></span><br><span class="line"> 移除 S, Y 中最旧的向量对</span><br><span class="line"></span><br><span class="line"> d. 计算新的搜索方向 d (L-BFGS 双循环递归):</span><br><span class="line"> - q = -g_new</span><br><span class="line"> - <span class="keyword">for</span> i = <span class="built_in">len</span>(S)-<span class="number">1</span> downto <span class="number">0</span>:</span><br><span class="line"> ρ_i = <span class="number">1</span> / (y_i^T s_i)</span><br><span class="line"> α_i = ρ_i * s_i^T q</span><br><span class="line"> q = q - α_i * y_i</span><br><span class="line"> - r = q * (s_{last}^T y_{last} / y_{last}^T y_{last}) <span class="comment"># 缩放</span></span><br><span class="line"> - <span class="keyword">for</span> i = <span class="number">0</span> to <span class="built_in">len</span>(S)-<span class="number">1</span>:</span><br><span class="line"> β_i = ρ_i * y_i^T r</span><br><span class="line"> r = r + (α_i - β_i) * s_i</span><br><span class="line"> - d = r <span class="comment"># 新的搜索方向</span></span><br><span class="line"></span><br><span class="line"> e. 更新变量:</span><br><span class="line"> - x = x_new</span><br><span class="line"> - g = g_new</span><br><span class="line"> - k = k + <span class="number">1</span></span><br><span class="line"></span><br><span class="line"><span class="number">4.</span> 返回结果:</span><br><span class="line"> <span class="keyword">return</span> x, f(x)</span><br></pre></td></tr></table></figure>
<h1 id="效果"><a href="#效果" class="headerlink" title="效果"></a>效果</h1><p><img src="https://github.com/ZihChen7/picx-images-hosting/raw/master/20250626/Peek-2025-06-26-19-58.5j4gthm1kg.gif" alt="Peek-2025-06-26-19-58" style="zoom: 25%;" /></p>
<p><img src="https://github.com/ZihChen7/picx-images-hosting/raw/master/20250626/Peek-2025-06-26-20-02.4cl5kvxu1w.gif" alt="Peek-2025-06-26-20-02" style="zoom:25%;" /></p>
<p><img src="https://github.com/ZihChen7/picx-images-hosting/raw/master/20250626/Peek-2025-06-26-20-03.4cl5kvy5an.gif" alt="Peek-2025-06-26-20-03" style="zoom:25%;" /></p>
<p> 尽管确实能生成路径，但是容易生成弯绕，曲折的路径。</p>
</div>
<footer class="post-footer">
<div class="post-eof"></div>
</footer>
</article>
<article itemscope itemtype="http://schema.org/Article" class="post-block" lang="zh-CN">
<link itemprop="mainEntityOfPage" href="https://zihchen7.github.io/2025/06/18/25-6-18/">
<span hidden itemprop="author" itemscope itemtype="http://schema.org/Person">
<meta itemprop="image" content="/images/web-app-manifest-192x192.png">
<meta itemprop="name" content="ZihChen7">
<meta itemprop="description" content="一个爱好机器人的小学生。email: ZihChen7@163.com ">
</span>
<span hidden itemprop="publisher" itemscope itemtype="http://schema.org/Organization">
<meta itemprop="name" content="A Blog">
</span>
<header class="post-header">
<h2 class="post-title" itemprop="name headline">
<a href="/2025/06/18/25-6-18/" class="post-title-link" itemprop="url">Optimization in Robot (1)</a>
</h2>
<div class="post-meta">
<span class="post-meta-item">
<span class="post-meta-item-icon">
<i class="far fa-calendar"></i>
</span>
<span class="post-meta-item-text">发表于</span>
<time title="创建时间：2025-06-18 19:58:53" itemprop="dateCreated datePublished" datetime="2025-06-18T19:58:53+08:00">2025-06-18</time>
</span>
<span class="post-meta-item">
<span class="post-meta-item-icon">
<i class="far fa-calendar-check"></i>
</span>
<span class="post-meta-item-text">更新于</span>
<time title="修改时间：2025-06-22 10:30:46" itemprop="dateModified" datetime="2025-06-22T10:30:46+08:00">2025-06-22</time>
</span>
</div>
</header>
<div class="post-body" itemprop="articleBody">
<h1 id="前言"><a href="#前言" class="headerlink" title="前言"></a>前言</h1><p>最近在学习<strong>深蓝学院</strong>的汪博讲的《机器人学中的数值优化》，本文用来记录与总结。在课程第一章汪博对<strong>凸函数</strong> 的性质花了很长时间介绍，本文略过。 <strong>水平有限，如有错误之处，敬请指正！</strong></p>
<hr>
<h1 id="第一章-无约束优化问题"><a href="#第一章-无约束优化问题" class="headerlink" title="第一章 无约束优化问题"></a>第一章 无约束优化问题</h1><p> 无约束优化问题的定义为：</p>
<script type="math/tex; mode=display">
\min_{x \in \mathbb{R}^n} f(x)</script><p> 其中，<em>x</em> 属于 <em>n</em> 维实数空间的变量。关于无约束的优化问题，课程中主要介绍了几种常见数值优化方法。本文对一些求导概念性属于进行介绍。</p>
<div class="table-container">
<table>
<thead>
<tr>
<th style="text-align:center">名称</th>
<th style="text-align:center">符号</th>
<th style="text-align:center">维度</th>
<th style="text-align:center"><em>X</em> 维度</th>
<th style="text-align:center"><em>Y</em> 维度</th>
</tr>
</thead>
<tbody>
<tr>
<td style="text-align:center">导数</td>
<td style="text-align:center">f’(x)</td>
<td style="text-align:center">1</td>
<td style="text-align:center">1</td>
<td style="text-align:center">1</td>
</tr>
<tr>
<td style="text-align:center">梯度</td>
<td style="text-align:center">▽f(x)</td>
<td style="text-align:center">n × 1</td>
<td style="text-align:center">n</td>
<td style="text-align:center">1</td>
</tr>
<tr>
<td style="text-align:center">海森</td>
<td style="text-align:center">Hessian</td>
<td style="text-align:center">n × n</td>
<td style="text-align:center">n</td>
<td style="text-align:center">1</td>
</tr>
<tr>
<td style="text-align:center">雅可比</td>
<td style="text-align:center">Jacobian</td>
<td style="text-align:center">m × n</td>
<td style="text-align:center">n</td>
<td style="text-align:center">m</td>
</tr>
</tbody>
</table>
</div>
<p> 其中，梯度和海森分别是多输入单输出函数的一阶和二阶导数。</p>
<h2 id="梯度下降法"><a href="#梯度下降法" class="headerlink" title="梯度下降法"></a>梯度下降法</h2><p> 梯度下降法顾名思义，利用的是梯度信息。具体的迭代表达式为：</p>
<script type="math/tex; mode=display">
x_{k+1} = x_k - \alpha \nabla f(x_k)</script><p> 其中，<em>α</em> 为步长。</p>
<p> 为什么需要步长这一概念？-▽f(x)是搜索的方向，如果走多了，会出现“曲折、振荡”的现象。<strong>线搜索</strong> 专门用于计算步长 <em>α</em> ，避免手动设定固定步长导致的收敛失败。</p>
<p> 常见的确定步长方法有 ：</p>
<ol>
<li><em>α</em> = c ，为常数，这就特备容易引发上面说的 振荡。</li>
<li><em>α</em> = c / k ，步长随着迭代次数增加逐渐降低</li>
<li>精确线搜索</li>
<li>非精确线搜索</li>
</ol>
<p> 下面对几个线搜索相关的方法具体介绍 。</p>
<hr>
<h2 id="【补充】-线搜索"><a href="#【补充】-线搜索" class="headerlink" title="【补充】 线搜索"></a>【补充】 线搜索</h2><h3 id="精确线搜索"><a href="#精确线搜索" class="headerlink" title="精确线搜索"></a>精确线搜索</h3><p> 假设第 <em>k</em> 步的最优步长为 $ α_k $，那么则构建出<strong>子优化</strong>问题：</p>
<script type="math/tex; mode=display">
\alpha_k^* = \arg\min_{\alpha_k} f \left( \boldsymbol{x}_k + \alpha_k \, \boldsymbol{d}_k \right) \\
d_k = -\nabla f(x_k)</script><p> 但是此方法复杂度太高。</p>
<h3 id="Armijo准则"><a href="#Armijo准则" class="headerlink" title="Armijo准则"></a>Armijo准则</h3><script type="math/tex; mode=display">
f(x_k) - f(x_k + \alpha d_k) \geq -c \cdot \alpha \nabla f(x_k)^T d_k</script><p> 一般 <em>c</em> 取(0,1)。公式咋得来的？请看：</p>
<script type="math/tex; mode=display">
f(x_k + \alpha d_k) \approx f(x_k) + \alpha \nabla f(x_k)^T d_k + o(\alpha^2)\\
f(x_k) - f(x_k + \alpha d_k) \approx -\alpha \nabla f(x_k)^T d_k - o(\alpha^2)</script><p> 总的来说就是 <strong>实际下降量 ≥ 预期下降的缩小值</strong>。<strong>保证每次迭代的函数值下降是充分的，而非随意或微小的</strong>。所以，满足Armijo准则的条件又叫做<strong>充分下降条件</strong>。</p>
<h3 id="弱Wolfe准则"><a href="#弱Wolfe准则" class="headerlink" title="弱Wolfe准则"></a>弱Wolfe准则</h3><p> 弱Wolfe准则满足2个条件，分别为上述的Armijo条件和曲率条件。曲率条件表达式为：</p>
<script type="math/tex; mode=display">
\nabla f (x_k + \alpha d_k)^\top d_k ≥ c_2 \nabla f(x_k)^\top d_k</script><p> 如下图，公式的左半边是<em>Φ</em>(α)函数的导数，物理意义是说，曲率条件的物理意义是<em>Φ</em>(α)函数在α的斜率大于等于c2倍的<em>Φ</em>(0)的斜率。只限制方向导数的<strong>下界</strong> ，允许步长较大。</p>
<p><img src="https://github.com/ZihChen7/picx-images-hosting/raw/master/20250620/weak_wolfe.lvzr4ld17.webp" alt="weak_wolfe" style="zoom:75%;" /></p>
<h3 id="强Wolfe准则"><a href="#强Wolfe准则" class="headerlink" title="强Wolfe准则"></a><strong>强Wolfe准则</strong></h3><p> 强Wolfe准则和弱Wolfe准则的区别在于前者曲率条件多了个绝对值，也就是</p>
<script type="math/tex; mode=display">
\|\nabla f(x + \alpha d)^T d_k\| \leq c_3 \|\nabla f(x)^T d_k\|</script><p><img src="https://github.com/ZihChen7/picx-images-hosting/raw/master/20250620/strong_wolfe.png.32i861soeb.webp" alt="strong_wolfe" style="zoom:80%;" /></p>
<ul>
<li><p>为什么<strong>弱Wolfe准则</strong> 和 <strong>强Wolfe准则</strong> 的曲率条件一个是<strong>≥</strong> 而一个是 <strong>≤</strong> ？</p>
<p> 弱Wolfe 允许斜率过正， 而 强Wolfe准则 避免斜率过度正或过度负，从而防止步长过大（如跳过极小点）或过小（收敛缓慢）。</p>
</li>
</ul>
<hr>
<h2 id="牛顿法"><a href="#牛顿法" class="headerlink" title="牛顿法"></a>牛顿法</h2><p> 梯度下降法是使用一阶梯度信息进行更新的，牛顿法则用到了<strong>二阶的Hessian矩阵</strong>。怎么得来的呢？首先，将f(x)进泰勒展开到二次项。</p>
<script type="math/tex; mode=display">
f(x) \approx f(x_k) + \nabla f(x_k)^T (x -x_k) + \frac{1}{2} (x - x_k)^T \nabla^2 f(x_k) (x - x_k)</script><p> 求极值，即求f’(x) = 0 （因为默认是光滑凸函数），对上面公式求导得到</p>
<script type="math/tex; mode=display">
\nabla f(x_k) + \nabla^2 f(x_k) (x - x_k) = 0 \\
\mathbf x_{k+1} = x_k - [\nabla^2 f(x_k)]^{-1} \nabla f(x_k)</script><p> 这就得到了递推公式！ ，一般称x_k后面的为牛顿步。 牛顿法相对于梯度下降法有更快的收敛速度，但是稳定性较差。因为需要求解Hessian的逆，所以呀需要 Hessian <strong>非奇异且正定</strong>（非奇异—->求逆 ， 正定—->牛顿步为下降方向），而且这一步很耗时。所以出现了一些解决方案：</p>
<ol>
<li><p>使用修正矩阵M 去近似 Hessian</p>
<script type="math/tex; mode=display">
M = \nabla^2 f(x) + \lambda I, \quad \epsilon = \min\left(1, \|\nabla f(x)\|, \ldots\right) / 10</script></li>
<li><p>引入线性方程组，求解d代替求逆</p>
</li>
</ol>
<script type="math/tex; mode=display">
\nabla^2 f(x_k) d = -\nabla f(x_k)</script><p> 1）若 <strong>Hessian 矩阵半正定 </strong>， 使用<strong>Cholesky 分解</strong></p>
<script type="math/tex; mode=display">
\nabla^2 f(x_k) = LL^T</script><p> 其中 L 是下三角矩阵。 </p>
<p> 2）若<strong>Hessian 矩阵不定</strong>，使用<strong>Bunch-Kaufman 分解</strong></p>
<script type="math/tex; mode=display">
\nabla^2 f(x_k) = LDL^T</script><p> 其中 D 是 <strong>块对角矩阵</strong> （稀疏矩阵）。可以很容易地去修改这些小块矩阵的特征值，把他们改成正数。</p>
<h2 id="拟牛顿法"><a href="#拟牛顿法" class="headerlink" title="拟牛顿法"></a>拟牛顿法</h2><h3 id="BFGS"><a href="#BFGS" class="headerlink" title="BFGS"></a>BFGS</h3><p> 总的来说，牛顿法还是存在不小的问题的，其一，不保证一定是下降方向；其二，二阶Hessian求解还是比较难的，所以出现了拟牛顿法，<strong>核心思想</strong>是使用一阶梯度信息去近似二阶信息。同样是求解<strong>B</strong>矩阵近似Hessian 。</p>
<p> 对梯度▽f(x) 泰勒展开得到</p>
<script type="math/tex; mode=display">
\nabla f(x_k + p) = \nabla f(x_k) + \nabla^2 f(x_k) p + \mathcal{O}(\|p\|^2)</script><p> 然后，由于x_k + p =x_k+1，带入得到 </p>
<script type="math/tex; mode=display">
\nabla f(x_{k+1}) - \nabla f(x_k) = \nabla^2 f(x_k) (x_{k+1} - x_{k+1})</script><p> 整理得到约束条件，一般叫做 <strong>牛顿条件</strong>：</p>
<script type="math/tex; mode=display">
\Delta g^{k} = M^{k+1} \Delta x^{k} （梯度变化）\\
\Delta x^{k} = B^{k+1} \Delta g^{k} （变量变化）\\</script><p> <strong>M</strong>和<strong>B</strong>互为逆矩阵，一般用第二个<strong>B</strong> 矩阵的公式。公式里变量具体含义为：</p>
<script type="math/tex; mode=display">
\begin{aligned}
B^{k+1} = \left[ \nabla^{2} f(x^{k}) \right]^{-1}, \Delta x^{k} = x^{k+1} - x^{k}, \Delta g^{k} = \nabla f(x^{k+1}) - \nabla f(x^{k})
\end{aligned}</script><p> 那么接下来去求<strong>B</strong>。 核心思想是：尽量保留已有信息，同时满足<strong>牛顿条件</strong>。</p>
<script type="math/tex; mode=display">
\min_{B} \| H^{1/2}(B - B^k)H^{1/2} \|^2 \\
\text{s.t.} \quad B = B^T, \quad \Delta x = B \Delta g</script><p> 通过一系列复杂的计算（没看懂），最终得到BFGS算法的迭代公式，3 2 1 上公式！</p>
<p><img src="https://github.com/ZihChen7/picx-images-hosting/raw/master/20250621/BFGS.8hgqpsdc7i.webp" alt="BFGS" style="zoom:67%;" /></p>
<p> 这是一个迭代更新的过程，每一轮迭代优化步都需要对<strong>B</strong>矩阵进行更新，从而用历史的一阶梯度信息，对二阶信息进行迭代估计。</p>
<p> 但这个方法的缺点越比较明显：</p>
<ul>
<li>存储和计算开销较大，去近似的Hessian矩阵，需要n × n 的空间，当变量维数很大，内存和计算代价高</li>
<li>无法保证B^k+1 正定，不能保证目标函数下降，所以一般与<strong>线搜索</strong>连用</li>
</ul>
<h3 id="L-BFGS"><a href="#L-BFGS" class="headerlink" title="L-BFGS"></a>L-BFGS</h3><p> 核心思路：使用有限个的历史梯度变化，近似Hessian ，直接上伪代码！</p>
<p><img src="https://github.com/ZihChen7/picx-images-hosting/raw/master/20250621/L-BFGS.6f0y1sbwgi.webp" alt="L-BFGS"></p>
<h2 id="共轭梯度法"><a href="#共轭梯度法" class="headerlink" title="共轭梯度法"></a>共轭梯度法</h2><p> 共轭梯度法是专门用来求解线性方程的。共轭梯度法在每步迭代中选择一组 与A共轭的方向。主啵累了，放公式：</p>
<p><img src="https://github.com/ZihChen7/picx-images-hosting/raw/master/20250621/Newton-CG.1ovp3dcnst.webp" alt="Newton-CG" style="zoom:67%;" /></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">https://zhuanlan.zhihu.com/p/670856639</span><br><span class="line">https://www.shenlanxueyuan.com/course/745</span><br></pre></td></tr></table></figure>
</div>
<footer class="post-footer">
<div class="post-eof"></div>
</footer>
</article>
<article itemscope itemtype="http://schema.org/Article" class="post-block" lang="zh-CN">
<link itemprop="mainEntityOfPage" href="https://zihchen7.github.io/2025/06/18/hello-world/">
<span hidden itemprop="author" itemscope itemtype="http://schema.org/Person">
<meta itemprop="image" content="/images/web-app-manifest-192x192.png">
<meta itemprop="name" content="ZihChen7">
<meta itemprop="description" content="一个爱好机器人的小学生。email: ZihChen7@163.com ">
</span>
<span hidden itemprop="publisher" itemscope itemtype="http://schema.org/Organization">
<meta itemprop="name" content="A Blog">
</span>
<header class="post-header">
<h2 class="post-title" itemprop="name headline">
<a href="/2025/06/18/hello-world/" class="post-title-link" itemprop="url">Hello 超超</a>
</h2>
<div class="post-meta">
<span class="post-meta-item">
<span class="post-meta-item-icon">
<i class="far fa-calendar"></i>
</span>
<span class="post-meta-item-text">发表于</span>
<time title="创建时间：2025-06-18 15:17:03" itemprop="dateCreated datePublished" datetime="2025-06-18T15:17:03+08:00">2025-06-18</time>
</span>
<span class="post-meta-item">
<span class="post-meta-item-icon">
<i class="far fa-calendar-check"></i>
</span>
<span class="post-meta-item-text">更新于</span>
<time title="修改时间：2025-06-20 22:33:19" itemprop="dateModified" datetime="2025-06-20T22:33:19+08:00">2025-06-20</time>
</span>
</div>
</header>
<div class="post-body" itemprop="articleBody">
<h2 id="Quick-Start"><a href="#Quick-Start" class="headerlink" title="Quick Start"></a>Quick Start</h2><h3 id="Create-a-new-post"><a href="#Create-a-new-post" class="headerlink" title="Create a new post"></a>Create a new post</h3><figure class="highlight bash"><table><tr><td class="gutter"><pre><span class="line">1</span><br></pre></td><td class="code"><pre><span class="line">$ hexo new <span class="string">"My New Post"</span></span><br></pre></td></tr></table></figure>
</div>
<footer class="post-footer">
<div class="post-eof"></div>
</footer>
</article>
</div>
<script>
window.addEventListener('tabs:register', () => {
let { activeClass } = CONFIG.comments;
if (CONFIG.comments.storage) {
activeClass = localStorage.getItem('comments_active') || activeClass;
}
if (activeClass) {
let activeTab = document.querySelector(`a[href="#comment-${activeClass}"]`);
if (activeTab) {
activeTab.click();
}
}
});
if (CONFIG.comments.storage) {
window.addEventListener('tabs:click', event => {
if (!event.target.matches('.tabs-comment .tab-content .tab-pane')) return;
let commentClass = event.target.classList[1];
localStorage.setItem('comments_active', commentClass);
});
}
</script>
</div>
<div class="toggle sidebar-toggle">
<span class="toggle-line toggle-line-first"></span>
<span class="toggle-line toggle-line-middle"></span>
<span class="toggle-line toggle-line-last"></span>
</div>
<aside class="sidebar">
<div class="sidebar-inner">
<ul class="sidebar-nav motion-element">
<li class="sidebar-nav-toc">
文章目录
</li>
<li class="sidebar-nav-overview">
站点概览
</li>
</ul>
<!--noindex-->
<div class="post-toc-wrap sidebar-panel">
</div>
<!--/noindex-->
<div class="site-overview-wrap sidebar-panel">
<div class="site-author motion-element" itemprop="author" itemscope itemtype="http://schema.org/Person">
<img class="site-author-image" itemprop="image" alt="ZihChen7"
src="/images/web-app-manifest-192x192.png">
<p class="site-author-name" itemprop="name">ZihChen7</p>
<div class="site-description" itemprop="description">一个爱好机器人的小学生。email: ZihChen7@163.com </div>
</div>
<div class="site-state-wrap motion-element">
<nav class="site-state">
<div class="site-state-item site-state-posts">
<a href="/archives/">
<span class="site-state-item-count">4</span>
<span class="site-state-item-name">日志</span>
</a>
</div>
<div class="site-state-item site-state-tags">
<span class="site-state-item-count">4</span>
<span class="site-state-item-name">标签</span>
</div>
</nav>
</div>
</div>
</div>
</aside>
<div id="sidebar-dimmer"></div>
</div>
</main>
<footer class="footer">
<div class="footer-inner">
<div class="copyright">
©
<span itemprop="copyrightYear">2025</span>
<span class="with-love">
<i class="fa fa-heart"></i>
</span>
<span class="author" itemprop="copyrightHolder">ZihChen7</span>
</div>
<div class="powered-by">由 <a href="https://hexo.io/" class="theme-link" rel="noopener" target="_blank">Hexo</a> & <a href="https://pisces.theme-next.org/" class="theme-link" rel="noopener" target="_blank">NexT.Pisces</a> 强力驱动
</div>
</div>
</footer>
</div>
<script src="/lib/anime.min.js"></script>
<script src="//cdn.jsdelivr.net/npm/pangu@4/dist/browser/pangu.min.js"></script>
<script src="/lib/velocity/velocity.min.js"></script>
<script src="/lib/velocity/velocity.ui.min.js"></script>
<script src="/js/utils.js"></script>
<script src="/js/motion.js"></script>
<script src="/js/schemes/pisces.js"></script>
<script src="/js/next-boot.js"></script>
<script>
if (typeof MathJax === 'undefined') {
window.MathJax = {
loader: {
load: ['[tex]/mhchem'],
source: {
'[tex]/amsCd': '[tex]/amscd',
'[tex]/AMScd': '[tex]/amscd'
}
},
tex: {
inlineMath: {'[+]': [['$', '$']]},
packages: {'[+]': ['mhchem']},
tags: 'ams'
},
options: {
renderActions: {
findScript: [10, doc => {
document.querySelectorAll('script[type^="math/tex"]').forEach(node => {
const display = !!node.type.match(/; *mode=display/);
const math = new doc.options.MathItem(node.textContent, doc.inputJax[0], display);
const text = document.createTextNode('');
node.parentNode.replaceChild(text, node);
math.start = {node: text, delim: '', n: 0};
math.end = {node: text, delim: '', n: 0};
doc.math.push(math);
});
}, '', false],
insertedScript: [200, () => {
document.querySelectorAll('mjx-container').forEach(node => {
let target = node.parentNode;
if (target.nodeName.toLowerCase() === 'li') {
target.parentNode.classList.add('has-jax');