forked from symforce-org/symforce-org.github.io
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathindex.html
740 lines (665 loc) · 106 KB
/
index.html
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
<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1.0" /><meta name="generator" content="Docutils 0.17.1: http://docutils.sourceforge.net/" />
<title>SymForce Home — symforce 0.5.0 documentation</title>
<link rel="stylesheet" type="text/css" href="_static/pygments.css" />
<link rel="stylesheet" type="text/css" href="_static/alabaster.css" />
<link rel="stylesheet" type="text/css" href="_static/css/custom.css" />
<script data-url_root="./" id="documentation_options" src="_static/documentation_options.js"></script>
<script src="_static/jquery.js"></script>
<script src="_static/underscore.js"></script>
<script src="_static/doctools.js"></script>
<script crossorigin="anonymous" integrity="sha256-Ae2Vz/4ePdIu6ZyI/5ZGsYnb+m0JlOmKPjt6XZ9JJkA=" src="https://cdnjs.cloudflare.com/ajax/libs/require.js/2.3.4/require.min.js"></script>
<link rel="shortcut icon" href="_static/favicon.ico"/>
<link rel="index" title="Index" href="genindex.html" />
<link rel="search" title="Search" href="search.html" />
<link rel="next" title="Development Guide" href="development.html" />
<link rel="stylesheet" href="_static/custom.css" type="text/css" />
<meta name="viewport" content="width=device-width, initial-scale=0.9, maximum-scale=0.9" />
</head><body>
<div class="document">
<div class="documentwrapper">
<div class="bodywrapper">
<div class="body" role="main">
<section id="symforce-home">
<h1>SymForce Home<a class="headerlink" href="#symforce-home" title="Permalink to this headline">¶</a></h1>
<!-- There's no good way to make an image that's different on light/dark mode that also links
somewhere. The closest thing is to add a #gh-light-mode-only to the target, which does this,
but is kinda confusing -->
<p><img alt="SymForce" src="docs/static/images/symforce_banner.png#gh-light-mode-only" /></p>
<!-- DARK_MODE_ONLY -->
<p><img alt="SymForce" src="docs/static/images/symforce_banner_dark.png#gh-dark-mode-only" /></p>
<!-- /DARK_MODE_ONLY -->
<p align="center">
<a href="https://symforce.org"><img alt="Documentation" src="https://img.shields.io/badge/api-docs-blue" /></a>
<a href="https://github.com/symforce-org/symforce"><img alt="Source Code" src="https://img.shields.io/badge/source-code-blue" /></a>
<a href="https://github.com/symforce-org/symforce/issues"><img alt="Issues" src="https://img.shields.io/badge/issue-tracker-blue" /></a>
<img alt="Python 3.8 | 3.9 | 3.10" src="https://img.shields.io/pypi/pyversions/symforce" />
<img alt="C++14" src="https://img.shields.io/badge/c++-14-blue" />
<a href="https://pypi.org/project/symforce/"><img alt="PyPI" src="https://img.shields.io/pypi/v/symforce" /></a>
<a href="https://github.com/symforce-org/symforce/tree/main/LICENSE"><img alt="Apache License" src="https://img.shields.io/pypi/l/symforce" /></a>
</p>
<hr class="docutils" />
<p>SymForce is a fast symbolic computation and code generation library for robotics applications like computer vision, state estimation, motion planning, and controls. It combines the development speed and flexibility of symbolic mathematics with the performance of autogenerated, highly optimized code in C++ or any target runtime language. SymForce contains three independently useful systems:</p>
<ul class="simple">
<li><p><strong>Symbolic Toolkit</strong> - builds on the SymPy API to provide rigorous geometric and camera types, lie group calculus, singularity handling, and tools to model complex problems</p></li>
<li><p><strong>Code Generator</strong> - transforms symbolic expressions into blazing-fast, branchless code with clean APIs and minimal dependencies, with a template system to target any language</p></li>
<li><p><strong>Optimization Library</strong> - a fast tangent-space optimization library based on factor graphs, with a highly optimized implementation for real-time robotics applications</p></li>
</ul>
<p>SymForce automatically computes tangent space Jacobians, eliminating the need for any bug-prone handwritten derivatives. Generated functions can be directly used as factors in our nonlinear optimizer. This workflow enables faster runtime functions, faster development time, and fewer lines of handwritten code versus alternative methods.</p>
<p>SymForce is developed and maintained by <a class="reference external" href="https://skydio.com/">Skydio</a>. It is used in production to accelerate tasks like SLAM, bundle adjustment, calibration, and sparse nonlinear MPC for autonomous robots at scale.</p>
<br/>
<img alt="SymForce" src="docs/static/images/symforce_diagram.png" width="700px"/>
<br/>
<section id="features">
<h2>Features:<a class="headerlink" href="#features" title="Permalink to this headline">¶</a></h2>
<ul class="simple">
<li><p>Symbolic implementations of geometry and camera types with Lie group operations</p></li>
<li><p>Code generation of fast native runtime code from symbolic expressions, reducing duplication and minimizing bugs</p></li>
<li><p>Novel tools to compute fast and correct tangent-space jacobians for any expression, avoiding all handwritten derivatives</p></li>
<li><p>Strategies for flattening computation and leveraging sparsity that can yield 10x speedups over standard autodiff</p></li>
<li><p>A fast tangent-space optimization library in C++ and Python based on factor graphs</p></li>
<li><p>Rapid prototyping and analysis of complex problems with symbolic math, with a seamless workflow into production use</p></li>
<li><p>Embedded-friendly C++ generation of templated Eigen code with zero dynamic memory allocation</p></li>
<li><p>Highly performant, modular, tested, and extensible code</p></li>
</ul>
</section>
<section id="read-the-paper-a-href-https-arxiv-org-abs-2204-07889-https-arxiv-org-abs-2204-07889-a">
<h2>Read the paper: <a href="https://arxiv.org/abs/2204.07889">https://arxiv.org/abs/2204.07889</a><a class="headerlink" href="#read-the-paper-a-href-https-arxiv-org-abs-2204-07889-https-arxiv-org-abs-2204-07889-a" title="Permalink to this headline">¶</a></h2>
<p>SymForce was published to <a class="reference external" href="https://roboticsconference.org/">RSS 2022</a>. Please cite it as follows:</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="nd">@inproceedings</span><span class="p">{</span><span class="n">Martiros</span><span class="o">-</span><span class="n">RSS</span><span class="o">-</span><span class="mi">22</span><span class="p">,</span>
<span class="n">author</span> <span class="o">=</span> <span class="p">{</span><span class="n">Hayk</span> <span class="n">Martiros</span> <span class="n">AND</span> <span class="n">Aaron</span> <span class="n">Miller</span> <span class="n">AND</span> <span class="n">Nathan</span> <span class="n">Bucki</span> <span class="n">AND</span> <span class="n">Bradley</span> <span class="n">Solliday</span> <span class="n">AND</span> <span class="n">Ryan</span> <span class="n">Kennedy</span> <span class="n">AND</span> <span class="n">Jack</span> <span class="n">Zhu</span> <span class="n">AND</span> <span class="n">Tung</span> <span class="n">Dang</span> <span class="n">AND</span> <span class="n">Dominic</span> <span class="n">Pattison</span> <span class="n">AND</span> <span class="n">Harrison</span> <span class="n">Zheng</span> <span class="n">AND</span> <span class="n">Teo</span> <span class="n">Tomic</span> <span class="n">AND</span> <span class="n">Peter</span> <span class="n">Henry</span> <span class="n">AND</span> <span class="n">Gareth</span> <span class="n">Cross</span> <span class="n">AND</span> <span class="n">Josiah</span> <span class="n">VanderMey</span> <span class="n">AND</span> <span class="n">Alvin</span> <span class="n">Sun</span> <span class="n">AND</span> <span class="n">Samuel</span> <span class="n">Wang</span> <span class="n">AND</span> <span class="n">Kristen</span> <span class="n">Holtz</span><span class="p">},</span>
<span class="n">title</span> <span class="o">=</span> <span class="p">{{</span><span class="n">SymForce</span><span class="p">:</span> <span class="n">Symbolic</span> <span class="n">Computation</span> <span class="ow">and</span> <span class="n">Code</span> <span class="n">Generation</span> <span class="k">for</span> <span class="n">Robotics</span><span class="p">}},</span>
<span class="n">booktitle</span> <span class="o">=</span> <span class="p">{</span><span class="n">Proceedings</span> <span class="n">of</span> <span class="n">Robotics</span><span class="p">:</span> <span class="n">Science</span> <span class="ow">and</span> <span class="n">Systems</span><span class="p">},</span>
<span class="n">year</span> <span class="o">=</span> <span class="p">{</span><span class="mi">2022</span><span class="p">},</span>
<span class="n">doi</span> <span class="o">=</span> <span class="p">{</span><span class="mf">10.15607</span><span class="o">/</span><span class="n">RSS</span><span class="mf">.2022</span><span class="o">.</span><span class="n">XVIII</span><span class="mf">.041</span><span class="p">}</span>
<span class="p">}</span>
</pre></div>
</div>
</section>
<section id="install">
<h2>Install<a class="headerlink" href="#install" title="Permalink to this headline">¶</a></h2>
<p>Install with pip:</p>
<div class="highlight-bash notranslate"><div class="highlight"><pre><span></span>pip install symforce
</pre></div>
</div>
<p>Verify the installation in Python:</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="gp">>>> </span><span class="kn">import</span> <span class="nn">symforce.symbolic</span> <span class="k">as</span> <span class="nn">sf</span>
<span class="gp">>>> </span><span class="n">sf</span><span class="o">.</span><span class="n">Rot3</span><span class="p">()</span>
</pre></div>
</div>
<p>This installs pre-compiled C++ components of SymForce on Linux and Mac using pip wheels, but does not include C++ headers. If you want to compile against C++ SymForce types (like <code class="docutils literal notranslate"><span class="pre">sym::Optimizer</span></code>), you currently need to <a href="#build-from-source">build from source</a>.</p>
</section>
<section id="tutorial">
<h2>Tutorial<a class="headerlink" href="#tutorial" title="Permalink to this headline">¶</a></h2>
<p>Let’s walk through a simple example of modeling and solving an optimization problem with SymForce. In this example a robot moves through a 2D plane and the goal is to estimate its pose at multiple time steps given noisy measurements.</p>
<p>The robot measures:</p>
<ul class="simple">
<li><p>the distance it traveled from an odometry sensor</p></li>
<li><p>relative bearing angles to known landmarks in the scene</p></li>
</ul>
<p>The robot’s heading angle is defined counter-clockwise from the x-axis, and its relative bearing measurements are defined from the robot’s forward direction:</p>
<img alt="Robot 2D Localization Figure" src="docs/static/images/robot_2d_localization/problem_setup.png" width="350px"/>
<section id="explore-the-math">
<h3>Explore the math<a class="headerlink" href="#explore-the-math" title="Permalink to this headline">¶</a></h3>
<p>Import the SymForce symbolic API, which contains the augmented SymPy API, as well as geometry and camera types:</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="kn">import</span> <span class="nn">symforce.symbolic</span> <span class="k">as</span> <span class="nn">sf</span>
</pre></div>
</div>
<p>Create a symbolic 2D pose and landmark location. Using symbolic variables lets us explore and build up the math in a pure form.</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="n">pose</span> <span class="o">=</span> <span class="n">sf</span><span class="o">.</span><span class="n">Pose2</span><span class="p">(</span>
<span class="n">t</span><span class="o">=</span><span class="n">sf</span><span class="o">.</span><span class="n">V2</span><span class="o">.</span><span class="n">symbolic</span><span class="p">(</span><span class="s2">"t"</span><span class="p">),</span>
<span class="n">R</span><span class="o">=</span><span class="n">sf</span><span class="o">.</span><span class="n">Rot2</span><span class="o">.</span><span class="n">symbolic</span><span class="p">(</span><span class="s2">"R"</span><span class="p">)</span>
<span class="p">)</span>
<span class="n">landmark</span> <span class="o">=</span> <span class="n">sf</span><span class="o">.</span><span class="n">V2</span><span class="o">.</span><span class="n">symbolic</span><span class="p">(</span><span class="s2">"L"</span><span class="p">)</span>
</pre></div>
</div>
<p>Let’s transform the landmark into the local frame of the robot. We choose to represent poses as
<code class="docutils literal notranslate"><span class="pre">world_T_body</span></code>, meaning that to take a landmark in the world frame and get its position in the body
frame, we do:</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="n">landmark_body</span> <span class="o">=</span> <span class="n">pose</span><span class="o">.</span><span class="n">inverse</span><span class="p">()</span> <span class="o">*</span> <span class="n">landmark</span>
</pre></div>
</div>
<p><img src="https://render.githubusercontent.com/render/math?math={\begin{bmatrix} R_{re} L_0 %2B R_{im} L_1 - R_{im} t_1 - R_{re} t_0 \\ -R_{im} L_0 %2B R_{re} L_1 %2B R_{im} t_0 %2B R_{re} t_1\end{bmatrix}}#gh-light-mode-only"
width="250px" /></p>
<!-- DARK_MODE_ONLY -->
<p><img src="https://render.githubusercontent.com/render/math?math={\color{white} \begin{bmatrix} R_{re} L_0 %2B R_{im} L_1 - R_{im} t_1 - R_{re} t_0 \\ -R_{im} L_0 %2B R_{re} L_1 %2B R_{im} t_0 %2B R_{re} t_1\end{bmatrix}}#gh-dark-mode-only"
width="250px" /></p>
<!-- /DARK_MODE_ONLY -->
<!-- $
\begin{bmatrix}
R_{re} L_0 + R_{im} L_1 - R_{im} t_1 - R_{re} t_0 \\
-R_{im} L_0 + R_{re} L_1 + R_{im} t_0 + R_{re} t_1
\end{bmatrix}
$ -->
<p>You can see that <code class="docutils literal notranslate"><span class="pre">sf.Rot2</span></code> is represented internally by a complex number (𝑅𝑟𝑒, 𝑅𝑖𝑚) and we can study how it rotates the landmark 𝐿.</p>
<p>For exploration purposes, let’s take the jacobian of the body-frame landmark with respect to the tangent space of the <code class="docutils literal notranslate"><span class="pre">Pose2</span></code>, parameterized as (𝜃, 𝑥, 𝑦):</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="n">landmark_body</span><span class="o">.</span><span class="n">jacobian</span><span class="p">(</span><span class="n">pose</span><span class="p">)</span>
</pre></div>
</div>
<p><img src="https://render.githubusercontent.com/render/math?math={\begin{bmatrix}-L_0 R_{im} %2B L_1 R_{re} %2B t_0 R_{im} - t_1 R_{re}, %26 -R_{re}, %26 -R_{im} \\ -L_0 R_{re} - L_1 R_{im} %2B t_0 R_{re} %2B t_1 R_{im}, %26 R_{im}, %26 -R_{re}\end{bmatrix}}#gh-light-mode-only"
width="350px" /></p>
<!-- DARK_MODE_ONLY -->
<p><img src="https://render.githubusercontent.com/render/math?math={\color{white} \begin{bmatrix}-L_0 R_{im} %2B L_1 R_{re} %2B t_0 R_{im} - t_1 R_{re}, %26 -R_{re}, %26 -R_{im} \\ -L_0 R_{re} - L_1 R_{im} %2B t_0 R_{re} %2B t_1 R_{im}, %26 R_{im}, %26 -R_{re}\end{bmatrix}}#gh-dark-mode-only"
width="350px" /></p>
<!-- /DARK_MODE_ONLY -->
<!-- $
\begin{bmatrix}
-L_0*R_{im} + L1*R_{re} + t_0*R_{im} - t_1*R_{re}, & -R_{re}, & -R_{im} \\
-L_0*R_{re} - L1*R_{im} + t_0*R_{re} + t_1*R_{im}, & R_{im}, & -R_{re}
\end{bmatrix}
$ -->
<p>Note that even though the orientation is stored as a complex number, the tangent space is a scalar angle and SymForce understands that.</p>
<p>Now compute the relative bearing angle:</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="n">sf</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">landmark_body</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">landmark_body</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
</pre></div>
</div>
<p><img src="https://render.githubusercontent.com/render/math?math={atan_2(-R_{im} L_0 %2B R_{re} L_1 %2B R_{im} t_0 %2B R_{re} t_1, R_{re} L_0 %2B R_{im} L_1 - R_{im} t_1 - R_{re} t_0)}#gh-light-mode-only"
width="500px" /></p>
<!-- DARK_MODE_ONLY -->
<p><img src="https://render.githubusercontent.com/render/math?math={\color{white} atan_2(-R_{im} L_0 %2B R_{re} L_1 %2B R_{im} t_0 %2B R_{re} t_1, R_{re} L_0 %2B R_{im} L_1 - R_{im} t_1 - R_{re} t_0)}#gh-dark-mode-only"
width="500px" /></p>
<!-- /DARK_MODE_ONLY -->
<!-- $
atan_2(-R_{im} L_0 + R_{re} L_1 + R_{im} t_0 + R_{re} t_1, R_{re} L_0 + R_{im} L_1 - R_{im} t_1 - R_{re} t_0)
$ -->
<p>One important note is that <code class="docutils literal notranslate"><span class="pre">atan2</span></code> is singular at (0, 0). In SymForce we handle this by placing a symbol ϵ (epsilon) that preserves the value of an expression in the limit of ϵ → 0, but allows evaluating at runtime with a very small nonzero value. Functions with singularities accept an <code class="docutils literal notranslate"><span class="pre">epsilon</span></code> argument:</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="n">sf</span><span class="o">.</span><span class="n">V3</span><span class="o">.</span><span class="n">symbolic</span><span class="p">(</span><span class="s2">"x"</span><span class="p">)</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">epsilon</span><span class="o">=</span><span class="n">sf</span><span class="o">.</span><span class="n">epsilon</span><span class="p">())</span>
</pre></div>
</div>
<p><img src="https://render.githubusercontent.com/render/math?math={\sqrt{x_0^2 %2B x_1^2 %2B x_2^2 %2B \epsilon}}#gh-light-mode-only"
width="135px" /></p>
<!-- DARK_MODE_ONLY -->
<p><img src="https://render.githubusercontent.com/render/math?math={\color{white} \sqrt{x_0^2 %2B x_1^2 %2B x_2^2 %2B \epsilon}}#gh-dark-mode-only"
width="135px" /></p>
<!-- /DARK_MODE_ONLY -->
<!-- $\sqrt{x_0^2 + x_1^2 + x_2^2 + \epsilon}$ -->
<p>See the <a class="reference external" href="https://symforce.org/notebooks/epsilon_tutorial.html">Epsilon Tutorial</a> in the SymForce Docs for more information.</p>
</section>
<section id="build-an-optimization-problem">
<h3>Build an optimization problem<a class="headerlink" href="#build-an-optimization-problem" title="Permalink to this headline">¶</a></h3>
<p>We will model this problem as a factor graph and solve it with nonlinear least-squares.</p>
<p>The residual function comprises of two terms - one for the bearing measurements and one for the odometry measurements. Let’s formalize the math we just defined for the bearing measurements into a symbolic residual function:</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="k">def</span> <span class="nf">bearing_residual</span><span class="p">(</span>
<span class="n">pose</span><span class="p">:</span> <span class="n">sf</span><span class="o">.</span><span class="n">Pose2</span><span class="p">,</span> <span class="n">landmark</span><span class="p">:</span> <span class="n">sf</span><span class="o">.</span><span class="n">V2</span><span class="p">,</span> <span class="n">angle</span><span class="p">:</span> <span class="n">sf</span><span class="o">.</span><span class="n">Scalar</span><span class="p">,</span> <span class="n">epsilon</span><span class="p">:</span> <span class="n">sf</span><span class="o">.</span><span class="n">Scalar</span>
<span class="p">)</span> <span class="o">-></span> <span class="n">sf</span><span class="o">.</span><span class="n">V1</span><span class="p">:</span>
<span class="n">t_body</span> <span class="o">=</span> <span class="n">pose</span><span class="o">.</span><span class="n">inverse</span><span class="p">()</span> <span class="o">*</span> <span class="n">landmark</span>
<span class="n">predicted_angle</span> <span class="o">=</span> <span class="n">sf</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">t_body</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">t_body</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">epsilon</span><span class="o">=</span><span class="n">epsilon</span><span class="p">)</span>
<span class="k">return</span> <span class="n">sf</span><span class="o">.</span><span class="n">V1</span><span class="p">(</span><span class="n">sf</span><span class="o">.</span><span class="n">wrap_angle</span><span class="p">(</span><span class="n">predicted_angle</span> <span class="o">-</span> <span class="n">angle</span><span class="p">))</span>
</pre></div>
</div>
<p>This function takes in a pose and landmark variable and returns the error between the predicted bearing angle and a measured value. Note that we call <code class="docutils literal notranslate"><span class="pre">sf.wrap_angle</span></code> on the angle difference to prevent wraparound effects.</p>
<p>The residual for distance traveled is even simpler:</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="k">def</span> <span class="nf">odometry_residual</span><span class="p">(</span>
<span class="n">pose_a</span><span class="p">:</span> <span class="n">sf</span><span class="o">.</span><span class="n">Pose2</span><span class="p">,</span> <span class="n">pose_b</span><span class="p">:</span> <span class="n">sf</span><span class="o">.</span><span class="n">Pose2</span><span class="p">,</span> <span class="n">dist</span><span class="p">:</span> <span class="n">sf</span><span class="o">.</span><span class="n">Scalar</span><span class="p">,</span> <span class="n">epsilon</span><span class="p">:</span> <span class="n">sf</span><span class="o">.</span><span class="n">Scalar</span>
<span class="p">)</span> <span class="o">-></span> <span class="n">sf</span><span class="o">.</span><span class="n">V1</span><span class="p">:</span>
<span class="k">return</span> <span class="n">sf</span><span class="o">.</span><span class="n">V1</span><span class="p">((</span><span class="n">pose_b</span><span class="o">.</span><span class="n">t</span> <span class="o">-</span> <span class="n">pose_a</span><span class="o">.</span><span class="n">t</span><span class="p">)</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">epsilon</span><span class="o">=</span><span class="n">epsilon</span><span class="p">)</span> <span class="o">-</span> <span class="n">dist</span><span class="p">)</span>
</pre></div>
</div>
<p>Now we can create <a class="reference external" href="https://symforce.org/api/symforce.opt.factor.html?highlight=factor#module-symforce.opt.factor"><code class="docutils literal notranslate"><span class="pre">Factor</span></code></a> objects from the residual functions and a set of keys. The keys are named strings for the function arguments, which will be accessed by name from a <a class="reference external" href="https://symforce.org/api/symforce.values.values.html"><code class="docutils literal notranslate"><span class="pre">Values</span></code></a> class we later instantiate with numerical quantities.</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="kn">from</span> <span class="nn">symforce.opt.factor</span> <span class="kn">import</span> <span class="n">Factor</span>
<span class="n">num_poses</span> <span class="o">=</span> <span class="mi">3</span>
<span class="n">num_landmarks</span> <span class="o">=</span> <span class="mi">3</span>
<span class="n">factors</span> <span class="o">=</span> <span class="p">[]</span>
<span class="c1"># Bearing factors</span>
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">num_poses</span><span class="p">):</span>
<span class="k">for</span> <span class="n">j</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">num_landmarks</span><span class="p">):</span>
<span class="n">factors</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">Factor</span><span class="p">(</span>
<span class="n">residual</span><span class="o">=</span><span class="n">bearing_residual</span><span class="p">,</span>
<span class="n">keys</span><span class="o">=</span><span class="p">[</span><span class="sa">f</span><span class="s2">"poses[</span><span class="si">{</span><span class="n">i</span><span class="si">}</span><span class="s2">]"</span><span class="p">,</span> <span class="sa">f</span><span class="s2">"landmarks[</span><span class="si">{</span><span class="n">j</span><span class="si">}</span><span class="s2">]"</span><span class="p">,</span> <span class="sa">f</span><span class="s2">"angles[</span><span class="si">{</span><span class="n">i</span><span class="si">}</span><span class="s2">][</span><span class="si">{</span><span class="n">j</span><span class="si">}</span><span class="s2">]"</span><span class="p">,</span> <span class="s2">"epsilon"</span><span class="p">],</span>
<span class="p">))</span>
<span class="c1"># Odometry factors</span>
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">num_poses</span> <span class="o">-</span> <span class="mi">1</span><span class="p">):</span>
<span class="n">factors</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">Factor</span><span class="p">(</span>
<span class="n">residual</span><span class="o">=</span><span class="n">odometry_residual</span><span class="p">,</span>
<span class="n">keys</span><span class="o">=</span><span class="p">[</span><span class="sa">f</span><span class="s2">"poses[</span><span class="si">{</span><span class="n">i</span><span class="si">}</span><span class="s2">]"</span><span class="p">,</span> <span class="sa">f</span><span class="s2">"poses[</span><span class="si">{</span><span class="n">i</span> <span class="o">+</span> <span class="mi">1</span><span class="si">}</span><span class="s2">]"</span><span class="p">,</span> <span class="sa">f</span><span class="s2">"distances[</span><span class="si">{</span><span class="n">i</span><span class="si">}</span><span class="s2">]"</span><span class="p">,</span> <span class="s2">"epsilon"</span><span class="p">],</span>
<span class="p">))</span>
</pre></div>
</div>
<p>Here is a visualization of the structure of this factor graph:</p>
<img alt="Robot 2D Localization Factor Graph" src="docs/static/images/robot_2d_localization/factor_graph.png" width="600px"/>
</section>
<section id="solve-the-problem">
<h3>Solve the problem<a class="headerlink" href="#solve-the-problem" title="Permalink to this headline">¶</a></h3>
<p>Our goal is to find poses of the robot that minimize the residual of this factor graph, assuming the
landmark positions in the world are known. We create an
<a class="reference external" href="https://symforce.org/api/symforce.opt.optimizer.html?highlight=optimizer#module-symforce.opt.optimizer"><code class="docutils literal notranslate"><span class="pre">Optimizer</span></code></a>
with these factors and tell it to only optimize the pose keys (the rest are held constant):</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="kn">from</span> <span class="nn">symforce.opt.optimizer</span> <span class="kn">import</span> <span class="n">Optimizer</span>
<span class="n">optimizer</span> <span class="o">=</span> <span class="n">Optimizer</span><span class="p">(</span>
<span class="n">factors</span><span class="o">=</span><span class="n">factors</span><span class="p">,</span>
<span class="n">optimized_keys</span><span class="o">=</span><span class="p">[</span><span class="sa">f</span><span class="s2">"poses[</span><span class="si">{</span><span class="n">i</span><span class="si">}</span><span class="s2">]"</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">num_poses</span><span class="p">)],</span>
<span class="c1"># So that we save more information about each iteration, to visualize later:</span>
<span class="n">debug_stats</span><span class="o">=</span><span class="kc">True</span><span class="p">,</span>
<span class="p">)</span>
</pre></div>
</div>
<p>Now we need to instantiate numerical <a class="reference external" href="https://symforce.org/api/symforce.values.values.html?highlight=values#module-symforce.values.values"><code class="docutils literal notranslate"><span class="pre">Values</span></code></a> for the problem, including an initial guess for our unknown poses (just set them to identity).</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="kn">from</span> <span class="nn">symforce.values</span> <span class="kn">import</span> <span class="n">Values</span>
<span class="n">initial_values</span> <span class="o">=</span> <span class="n">Values</span><span class="p">(</span>
<span class="n">poses</span><span class="o">=</span><span class="p">[</span><span class="n">sf</span><span class="o">.</span><span class="n">Pose2</span><span class="o">.</span><span class="n">identity</span><span class="p">()]</span> <span class="o">*</span> <span class="n">num_poses</span><span class="p">,</span>
<span class="n">landmarks</span><span class="o">=</span><span class="p">[</span><span class="n">sf</span><span class="o">.</span><span class="n">V2</span><span class="p">(</span><span class="o">-</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">),</span> <span class="n">sf</span><span class="o">.</span><span class="n">V2</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="o">-</span><span class="mi">3</span><span class="p">),</span> <span class="n">sf</span><span class="o">.</span><span class="n">V2</span><span class="p">(</span><span class="mi">5</span><span class="p">,</span> <span class="mi">2</span><span class="p">)],</span>
<span class="n">distances</span><span class="o">=</span><span class="p">[</span><span class="mf">1.7</span><span class="p">,</span> <span class="mf">1.4</span><span class="p">],</span>
<span class="n">angles</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">deg2rad</span><span class="p">([[</span><span class="mi">145</span><span class="p">,</span> <span class="mi">335</span><span class="p">,</span> <span class="mi">55</span><span class="p">],</span> <span class="p">[</span><span class="mi">185</span><span class="p">,</span> <span class="mi">310</span><span class="p">,</span> <span class="mi">70</span><span class="p">],</span> <span class="p">[</span><span class="mi">215</span><span class="p">,</span> <span class="mi">310</span><span class="p">,</span> <span class="mi">70</span><span class="p">]])</span><span class="o">.</span><span class="n">tolist</span><span class="p">(),</span>
<span class="n">epsilon</span><span class="o">=</span><span class="n">sf</span><span class="o">.</span><span class="n">numeric_epsilon</span><span class="p">,</span>
<span class="p">)</span>
</pre></div>
</div>
<p>Now run the optimization! This returns an <a class="reference external" href="https://symforce.org/api/symforce.opt.optimizer.html?highlight=optimizer#symforce.opt.optimizer.Optimizer.Result"><code class="docutils literal notranslate"><span class="pre">Optimizer.Result</span></code></a> object that contains the optimized values, error statistics, and per-iteration debug stats (if enabled).</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="n">result</span> <span class="o">=</span> <span class="n">optimizer</span><span class="o">.</span><span class="n">optimize</span><span class="p">(</span><span class="n">initial_values</span><span class="p">)</span>
</pre></div>
</div>
<p>Let’s visualize what the optimizer did. The orange circles represent the fixed landmarks, the blue
circles represent the robot, and the dotted lines represent the bearing measurements.</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="kn">from</span> <span class="nn">symforce.examples.robot_2d_localization.plotting</span> <span class="kn">import</span> <span class="n">plot_solution</span>
<span class="n">plot_solution</span><span class="p">(</span><span class="n">optimizer</span><span class="p">,</span> <span class="n">result</span><span class="p">)</span>
</pre></div>
</div>
<img alt="Robot 2D Localization Solution" src="docs/static/images/robot_2d_localization/iterations.gif" width="600px"/>
<p>All of the code for this example can also be found in <code class="docutils literal notranslate"><span class="pre">symforce/examples/robot_2d_localization</span></code>.</p>
</section>
<section id="symbolic-vs-numerical-types">
<h3>Symbolic vs Numerical Types<a class="headerlink" href="#symbolic-vs-numerical-types" title="Permalink to this headline">¶</a></h3>
<p>SymForce provides <code class="docutils literal notranslate"><span class="pre">sym</span></code> packages with runtime code for geometry and camera types that are generated from its symbolic <code class="docutils literal notranslate"><span class="pre">geo</span></code> and <code class="docutils literal notranslate"><span class="pre">cam</span></code> packages. As such, there are multiple versions of a class like <code class="docutils literal notranslate"><span class="pre">Pose3</span></code> and it can be a common source of confusion.</p>
<p>The canonical symbolic class <a class="reference external" href="https://symforce.org/api/symforce.sf.pose3.html"><code class="docutils literal notranslate"><span class="pre">sf.Pose3</span></code></a> lives in the <code class="docutils literal notranslate"><span class="pre">symforce</span></code> package:</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="n">sf</span><span class="o">.</span><span class="n">Pose3</span><span class="o">.</span><span class="n">identity</span><span class="p">()</span>
</pre></div>
</div>
<p>The autogenerated Python runtime class <a class="reference external" href="https://symforce.org/api-gen-py/sym.pose3.html?highlight=pose3#module-sym.pose3"><code class="docutils literal notranslate"><span class="pre">sym.Pose3</span></code></a> lives in the <code class="docutils literal notranslate"><span class="pre">sym</span></code> package:</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="kn">import</span> <span class="nn">sym</span>
<span class="n">sym</span><span class="o">.</span><span class="n">Pose3</span><span class="o">.</span><span class="n">identity</span><span class="p">()</span>
</pre></div>
</div>
<p>The autogenerated C++ runtime class <a class="reference external" href="https://symforce.org/api-gen-cpp/class/classsym_1_1Pose3.html"><code class="docutils literal notranslate"><span class="pre">sym::Pose3</span></code></a> lives in the <code class="docutils literal notranslate"><span class="pre">sym::</span></code> namespace:</p>
<div class="highlight-C++ notranslate"><div class="highlight"><pre><span></span><span class="n">sym</span><span class="o">::</span><span class="n">Pose3</span><span class="o"><</span><span class="kt">double</span><span class="o">>::</span><span class="n">Identity</span><span class="p">()</span><span class="w"></span>
</pre></div>
</div>
<p>The matrix type for symbolic code is <a class="reference external" href="https://symforce.org/api/symforce.sf.matrix.html?highlight=matrix#module-symforce.sf.matrix"><code class="docutils literal notranslate"><span class="pre">sf.Matrix</span></code></a>, for generated Python is <a class="reference external" href="https://numpy.org/doc/stable/reference/generated/numpy.ndarray.html"><code class="docutils literal notranslate"><span class="pre">numpy.ndarray</span></code></a>, and for C++ is <a class="reference external" href="https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html"><code class="docutils literal notranslate"><span class="pre">Eigen::Matrix</span></code></a>.</p>
<p>The symbolic classes can also handle numerical values, but will be dramatically slower than the generated classes. The symbolic classes must be used when defining functions for codegen and optimization. Generated functions always accept the runtime types.</p>
<p>The <code class="docutils literal notranslate"><span class="pre">Codegen</span></code> or <code class="docutils literal notranslate"><span class="pre">Factor</span></code> objects require symbolic types and functions to do math and generate code. However, once code is generated, numerical types should be used when invoking generated functions and in the initial values when calling the <code class="docutils literal notranslate"><span class="pre">Optimizer</span></code>.</p>
<p>As a convenience, the Python <code class="docutils literal notranslate"><span class="pre">Optimizer</span></code> class can accept symbolic types in its <code class="docutils literal notranslate"><span class="pre">Values</span></code> and convert to numerical types before invoking generated functions. This is done in the tutorial example for simplicity.</p>
</section>
<section id="generate-runtime-c-code">
<h3>Generate runtime C++ code<a class="headerlink" href="#generate-runtime-c-code" title="Permalink to this headline">¶</a></h3>
<p>Let’s look under the hood to understand how that optimization worked. For each factor, SymForce introspects the form of the symbolic function, passes through symbolic inputs to build an output expression, automatically computes tangent-space jacobians of those output expressions wrt the optimized variables, and generates fast runtime code for them.</p>
<p>The <a class="reference external" href="https://symforce.org/api/symforce.codegen.codegen.html?highlight=codegen#module-symforce.codegen.codegen"><code class="docutils literal notranslate"><span class="pre">Codegen</span></code></a> class is the central tool for generating runtime code from symbolic expressions. In this case, we pass it the bearing residual function and configure it to generate C++ code:</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="kn">from</span> <span class="nn">symforce.codegen</span> <span class="kn">import</span> <span class="n">Codegen</span><span class="p">,</span> <span class="n">CppConfig</span>
<span class="n">codegen</span> <span class="o">=</span> <span class="n">Codegen</span><span class="o">.</span><span class="n">function</span><span class="p">(</span><span class="n">bearing_residual</span><span class="p">,</span> <span class="n">config</span><span class="o">=</span><span class="n">CppConfig</span><span class="p">())</span>
</pre></div>
</div>
<p>We can then create another <code class="docutils literal notranslate"><span class="pre">Codegen</span></code> object that computes a Gauss-Newton linearization from this Codegen object. It does this by introspecting and symbolically differentiating the given arguments:</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="n">codegen_linearization</span> <span class="o">=</span> <span class="n">codegen</span><span class="o">.</span><span class="n">with_linearization</span><span class="p">(</span>
<span class="n">which_args</span><span class="o">=</span><span class="p">[</span><span class="s2">"pose"</span><span class="p">]</span>
<span class="p">)</span>
</pre></div>
</div>
<p>Generate a C++ function that computes the linearization wrt the pose argument:</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="n">metadata</span> <span class="o">=</span> <span class="n">codegen_linearization</span><span class="o">.</span><span class="n">generate_function</span><span class="p">()</span>
<span class="nb">print</span><span class="p">(</span><span class="nb">open</span><span class="p">(</span><span class="n">metadata</span><span class="o">.</span><span class="n">generated_files</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span><span class="o">.</span><span class="n">read</span><span class="p">())</span>
</pre></div>
</div>
<p>This C++ code depends only on Eigen and computes the results in a single flat function that shares all common sub-expressions:</p>
<div class="highlight-c++ notranslate"><div class="highlight"><pre><span></span><span class="cp">#pragma once</span>
<span class="cp">#include</span><span class="w"> </span><span class="cpf"><Eigen/Dense></span><span class="cp"></span>
<span class="cp">#include</span><span class="w"> </span><span class="cpf"><sym/pose2.h></span><span class="cp"></span>
<span class="k">namespace</span><span class="w"> </span><span class="nn">sym</span><span class="w"> </span><span class="p">{</span><span class="w"></span>
<span class="cm">/**</span>
<span class="cm"> * This function was autogenerated from a symbolic function. Do not modify by hand.</span>
<span class="cm"> *</span>
<span class="cm"> * Symbolic function: bearing_residual</span>
<span class="cm"> *</span>
<span class="cm"> * Args:</span>
<span class="cm"> * pose: Pose2</span>
<span class="cm"> * landmark: Matrix21</span>
<span class="cm"> * angle: Scalar</span>
<span class="cm"> * epsilon: Scalar</span>
<span class="cm"> *</span>
<span class="cm"> * Outputs:</span>
<span class="cm"> * res: Matrix11</span>
<span class="cm"> * jacobian: (1x3) jacobian of res wrt arg pose (3)</span>
<span class="cm"> * hessian: (3x3) Gauss-Newton hessian for arg pose (3)</span>
<span class="cm"> * rhs: (3x1) Gauss-Newton rhs for arg pose (3)</span>
<span class="cm"> */</span><span class="w"></span>
<span class="k">template</span><span class="w"> </span><span class="o"><</span><span class="k">typename</span><span class="w"> </span><span class="nc">Scalar</span><span class="o">></span><span class="w"></span>
<span class="kt">void</span><span class="w"> </span><span class="n">BearingFactor</span><span class="p">(</span><span class="k">const</span><span class="w"> </span><span class="n">sym</span><span class="o">::</span><span class="n">Pose2</span><span class="o"><</span><span class="n">Scalar</span><span class="o">>&</span><span class="w"> </span><span class="n">pose</span><span class="p">,</span><span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Eigen</span><span class="o">::</span><span class="n">Matrix</span><span class="o"><</span><span class="n">Scalar</span><span class="p">,</span><span class="w"> </span><span class="mi">2</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="o">>&</span><span class="w"> </span><span class="n">landmark</span><span class="p">,</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">angle</span><span class="p">,</span><span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">epsilon</span><span class="p">,</span><span class="w"></span>
<span class="w"> </span><span class="n">Eigen</span><span class="o">::</span><span class="n">Matrix</span><span class="o"><</span><span class="n">Scalar</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="o">>*</span><span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">res</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="k">nullptr</span><span class="p">,</span><span class="w"></span>
<span class="w"> </span><span class="n">Eigen</span><span class="o">::</span><span class="n">Matrix</span><span class="o"><</span><span class="n">Scalar</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="p">,</span><span class="w"> </span><span class="mi">3</span><span class="o">>*</span><span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">jacobian</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="k">nullptr</span><span class="p">,</span><span class="w"></span>
<span class="w"> </span><span class="n">Eigen</span><span class="o">::</span><span class="n">Matrix</span><span class="o"><</span><span class="n">Scalar</span><span class="p">,</span><span class="w"> </span><span class="mi">3</span><span class="p">,</span><span class="w"> </span><span class="mi">3</span><span class="o">>*</span><span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">hessian</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="k">nullptr</span><span class="p">,</span><span class="w"></span>
<span class="w"> </span><span class="n">Eigen</span><span class="o">::</span><span class="n">Matrix</span><span class="o"><</span><span class="n">Scalar</span><span class="p">,</span><span class="w"> </span><span class="mi">3</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="o">>*</span><span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">rhs</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="k">nullptr</span><span class="p">)</span><span class="w"> </span><span class="p">{</span><span class="w"></span>
<span class="w"> </span><span class="c1">// Total ops: 66</span>
<span class="w"> </span><span class="c1">// Input arrays</span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Eigen</span><span class="o">::</span><span class="n">Matrix</span><span class="o"><</span><span class="n">Scalar</span><span class="p">,</span><span class="w"> </span><span class="mi">4</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="o">>&</span><span class="w"> </span><span class="n">_pose</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">pose</span><span class="p">.</span><span class="n">Data</span><span class="p">();</span><span class="w"></span>
<span class="w"> </span><span class="c1">// Intermediate terms (24)</span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp0</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">2</span><span class="p">];</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp1</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">3</span><span class="p">];</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp2</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">landmark</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span><span class="w"> </span><span class="mi">0</span><span class="p">)</span><span class="w"> </span><span class="o">-</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">landmark</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span><span class="w"> </span><span class="mi">0</span><span class="p">);</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp3</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp0</span><span class="w"> </span><span class="o">-</span><span class="w"> </span><span class="n">_tmp1</span><span class="w"> </span><span class="o">+</span><span class="w"> </span><span class="n">_tmp2</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp4</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span><span class="w"> </span><span class="o">+</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">3</span><span class="p">];</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp5</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">landmark</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span><span class="w"> </span><span class="mi">0</span><span class="p">);</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp6</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">landmark</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span><span class="w"> </span><span class="mi">0</span><span class="p">);</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp7</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="o">-</span><span class="n">_tmp4</span><span class="w"> </span><span class="o">+</span><span class="w"> </span><span class="n">_tmp5</span><span class="w"> </span><span class="o">+</span><span class="w"> </span><span class="n">_tmp6</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp8</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp7</span><span class="w"> </span><span class="o">+</span><span class="w"> </span><span class="n">epsilon</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="p">((((</span><span class="n">_tmp7</span><span class="p">)</span><span class="w"> </span><span class="o">></span><span class="w"> </span><span class="mi">0</span><span class="p">)</span><span class="w"> </span><span class="o">-</span><span class="w"> </span><span class="p">((</span><span class="n">_tmp7</span><span class="p">)</span><span class="w"> </span><span class="o"><</span><span class="w"> </span><span class="mi">0</span><span class="p">))</span><span class="w"> </span><span class="o">+</span><span class="w"> </span><span class="n">Scalar</span><span class="p">(</span><span class="mf">0.5</span><span class="p">));</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp9</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="o">-</span><span class="n">angle</span><span class="w"> </span><span class="o">+</span><span class="w"> </span><span class="n">std</span><span class="o">::</span><span class="n">atan2</span><span class="p">(</span><span class="n">_tmp3</span><span class="p">,</span><span class="w"> </span><span class="n">_tmp8</span><span class="p">);</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp10</span><span class="w"> </span><span class="o">=</span><span class="w"></span>
<span class="w"> </span><span class="n">_tmp9</span><span class="w"> </span><span class="o">-</span><span class="w"> </span><span class="mi">2</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">Scalar</span><span class="p">(</span><span class="n">M_PI</span><span class="p">)</span><span class="w"> </span><span class="o">*</span><span class="w"></span>
<span class="w"> </span><span class="n">std</span><span class="o">::</span><span class="n">floor</span><span class="p">((</span><span class="n">Scalar</span><span class="p">(</span><span class="mi">1</span><span class="p">)</span><span class="w"> </span><span class="o">/</span><span class="w"> </span><span class="n">Scalar</span><span class="p">(</span><span class="mi">2</span><span class="p">))</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="p">(</span><span class="n">_tmp9</span><span class="w"> </span><span class="o">+</span><span class="w"> </span><span class="n">Scalar</span><span class="p">(</span><span class="n">M_PI</span><span class="p">))</span><span class="w"> </span><span class="o">/</span><span class="w"> </span><span class="n">Scalar</span><span class="p">(</span><span class="n">M_PI</span><span class="p">));</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp11</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">Scalar</span><span class="p">(</span><span class="mf">1.0</span><span class="p">)</span><span class="w"> </span><span class="o">/</span><span class="w"> </span><span class="p">(</span><span class="n">_tmp8</span><span class="p">);</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp12</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">std</span><span class="o">::</span><span class="n">pow</span><span class="p">(</span><span class="n">_tmp8</span><span class="p">,</span><span class="w"> </span><span class="n">Scalar</span><span class="p">(</span><span class="mi">2</span><span class="p">));</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp13</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp3</span><span class="w"> </span><span class="o">/</span><span class="w"> </span><span class="n">_tmp12</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp14</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp11</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="p">(</span><span class="n">_tmp4</span><span class="w"> </span><span class="o">-</span><span class="w"> </span><span class="n">_tmp5</span><span class="w"> </span><span class="o">-</span><span class="w"> </span><span class="n">_tmp6</span><span class="p">)</span><span class="w"> </span><span class="o">-</span><span class="w"> </span><span class="n">_tmp13</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="p">(</span><span class="n">_tmp0</span><span class="w"> </span><span class="o">-</span><span class="w"> </span><span class="n">_tmp1</span><span class="w"> </span><span class="o">+</span><span class="w"> </span><span class="n">_tmp2</span><span class="p">);</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp15</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp12</span><span class="w"> </span><span class="o">+</span><span class="w"> </span><span class="n">std</span><span class="o">::</span><span class="n">pow</span><span class="p">(</span><span class="n">_tmp3</span><span class="p">,</span><span class="w"> </span><span class="n">Scalar</span><span class="p">(</span><span class="mi">2</span><span class="p">));</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp16</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp12</span><span class="w"> </span><span class="o">/</span><span class="w"> </span><span class="n">_tmp15</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp17</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp14</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp16</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp18</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp13</span><span class="w"> </span><span class="o">+</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp11</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp19</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp16</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp18</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp20</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="o">-</span><span class="n">_pose</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp11</span><span class="w"> </span><span class="o">+</span><span class="w"> </span><span class="n">_pose</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp13</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp21</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp16</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp20</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp22</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">std</span><span class="o">::</span><span class="n">pow</span><span class="p">(</span><span class="n">_tmp8</span><span class="p">,</span><span class="w"> </span><span class="n">Scalar</span><span class="p">(</span><span class="mi">4</span><span class="p">))</span><span class="w"> </span><span class="o">/</span><span class="w"> </span><span class="n">std</span><span class="o">::</span><span class="n">pow</span><span class="p">(</span><span class="n">_tmp15</span><span class="p">,</span><span class="w"> </span><span class="n">Scalar</span><span class="p">(</span><span class="mi">2</span><span class="p">));</span><span class="w"></span>
<span class="w"> </span><span class="k">const</span><span class="w"> </span><span class="n">Scalar</span><span class="w"> </span><span class="n">_tmp23</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp18</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp22</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="c1">// Output terms (4)</span>
<span class="w"> </span><span class="k">if</span><span class="w"> </span><span class="p">(</span><span class="n">res</span><span class="w"> </span><span class="o">!=</span><span class="w"> </span><span class="k">nullptr</span><span class="p">)</span><span class="w"> </span><span class="p">{</span><span class="w"></span>
<span class="w"> </span><span class="n">Eigen</span><span class="o">::</span><span class="n">Matrix</span><span class="o"><</span><span class="n">Scalar</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="o">>&</span><span class="w"> </span><span class="n">_res</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="p">(</span><span class="o">*</span><span class="n">res</span><span class="p">);</span><span class="w"></span>
<span class="w"> </span><span class="n">_res</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span><span class="w"> </span><span class="mi">0</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp10</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="p">}</span><span class="w"></span>
<span class="w"> </span><span class="k">if</span><span class="w"> </span><span class="p">(</span><span class="n">jacobian</span><span class="w"> </span><span class="o">!=</span><span class="w"> </span><span class="k">nullptr</span><span class="p">)</span><span class="w"> </span><span class="p">{</span><span class="w"></span>
<span class="w"> </span><span class="n">Eigen</span><span class="o">::</span><span class="n">Matrix</span><span class="o"><</span><span class="n">Scalar</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="p">,</span><span class="w"> </span><span class="mi">3</span><span class="o">>&</span><span class="w"> </span><span class="n">_jacobian</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="p">(</span><span class="o">*</span><span class="n">jacobian</span><span class="p">);</span><span class="w"></span>
<span class="w"> </span><span class="n">_jacobian</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span><span class="w"> </span><span class="mi">0</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp17</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="n">_jacobian</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp19</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="n">_jacobian</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span><span class="w"> </span><span class="mi">2</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp21</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="p">}</span><span class="w"></span>
<span class="w"> </span><span class="k">if</span><span class="w"> </span><span class="p">(</span><span class="n">hessian</span><span class="w"> </span><span class="o">!=</span><span class="w"> </span><span class="k">nullptr</span><span class="p">)</span><span class="w"> </span><span class="p">{</span><span class="w"></span>
<span class="w"> </span><span class="n">Eigen</span><span class="o">::</span><span class="n">Matrix</span><span class="o"><</span><span class="n">Scalar</span><span class="p">,</span><span class="w"> </span><span class="mi">3</span><span class="p">,</span><span class="w"> </span><span class="mi">3</span><span class="o">>&</span><span class="w"> </span><span class="n">_hessian</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="p">(</span><span class="o">*</span><span class="n">hessian</span><span class="p">);</span><span class="w"></span>
<span class="w"> </span><span class="n">_hessian</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span><span class="w"> </span><span class="mi">0</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">std</span><span class="o">::</span><span class="n">pow</span><span class="p">(</span><span class="n">_tmp14</span><span class="p">,</span><span class="w"> </span><span class="n">Scalar</span><span class="p">(</span><span class="mi">2</span><span class="p">))</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp22</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="n">_hessian</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="mi">0</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="n">_hessian</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span><span class="w"> </span><span class="mi">2</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="mi">0</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="n">_hessian</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span><span class="w"> </span><span class="mi">0</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp14</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp23</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="n">_hessian</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">std</span><span class="o">::</span><span class="n">pow</span><span class="p">(</span><span class="n">_tmp18</span><span class="p">,</span><span class="w"> </span><span class="n">Scalar</span><span class="p">(</span><span class="mi">2</span><span class="p">))</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp22</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="n">_hessian</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span><span class="w"> </span><span class="mi">2</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="mi">0</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="n">_hessian</span><span class="p">(</span><span class="mi">2</span><span class="p">,</span><span class="w"> </span><span class="mi">0</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp14</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp20</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp22</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="n">_hessian</span><span class="p">(</span><span class="mi">2</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp20</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp23</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="n">_hessian</span><span class="p">(</span><span class="mi">2</span><span class="p">,</span><span class="w"> </span><span class="mi">2</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">std</span><span class="o">::</span><span class="n">pow</span><span class="p">(</span><span class="n">_tmp20</span><span class="p">,</span><span class="w"> </span><span class="n">Scalar</span><span class="p">(</span><span class="mi">2</span><span class="p">))</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp22</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="p">}</span><span class="w"></span>
<span class="w"> </span><span class="k">if</span><span class="w"> </span><span class="p">(</span><span class="n">rhs</span><span class="w"> </span><span class="o">!=</span><span class="w"> </span><span class="k">nullptr</span><span class="p">)</span><span class="w"> </span><span class="p">{</span><span class="w"></span>
<span class="w"> </span><span class="n">Eigen</span><span class="o">::</span><span class="n">Matrix</span><span class="o"><</span><span class="n">Scalar</span><span class="p">,</span><span class="w"> </span><span class="mi">3</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="o">>&</span><span class="w"> </span><span class="n">_rhs</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="p">(</span><span class="o">*</span><span class="n">rhs</span><span class="p">);</span><span class="w"></span>
<span class="w"> </span><span class="n">_rhs</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span><span class="w"> </span><span class="mi">0</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp10</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp17</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="n">_rhs</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span><span class="w"> </span><span class="mi">0</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp10</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp19</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="n">_rhs</span><span class="p">(</span><span class="mi">2</span><span class="p">,</span><span class="w"> </span><span class="mi">0</span><span class="p">)</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">_tmp10</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">_tmp21</span><span class="p">;</span><span class="w"></span>
<span class="w"> </span><span class="p">}</span><span class="w"></span>
<span class="p">}</span><span class="w"></span>
<span class="p">}</span><span class="w"> </span><span class="c1">// namespace sym</span>
</pre></div>
</div>
<p>SymForce can also generate runtime Python code that depends only on <code class="docutils literal notranslate"><span class="pre">numpy</span></code>.</p>
<p>The code generation system is written with pluggable <a class="reference external" href="https://palletsprojects.com/p/jinja/">jinja</a> templates to minimize the work to add new backend languages. Some of our top candidates to add are TypeScript, CUDA, and PyTorch.</p>
</section>
<section id="optimize-from-c">
<h3>Optimize from C++<a class="headerlink" href="#optimize-from-c" title="Permalink to this headline">¶</a></h3>
<p>Now that we can generate C++ functions for each residual function, we can also run the optimization purely from C++ to get Python entirely out of the loop for production use.</p>
<div class="highlight-c++ notranslate"><div class="highlight"><pre><span></span><span class="k">const</span><span class="w"> </span><span class="kt">int</span><span class="w"> </span><span class="n">num_poses</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="mi">3</span><span class="p">;</span><span class="w"></span>
<span class="k">const</span><span class="w"> </span><span class="kt">int</span><span class="w"> </span><span class="n">num_landmarks</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="mi">3</span><span class="p">;</span><span class="w"></span>
<span class="n">std</span><span class="o">::</span><span class="n">vector</span><span class="o"><</span><span class="n">sym</span><span class="o">::</span><span class="n">Factor</span><span class="o"><</span><span class="kt">double</span><span class="o">>></span><span class="w"> </span><span class="n">factors</span><span class="p">;</span><span class="w"></span>
<span class="c1">// Bearing factors</span>
<span class="k">for</span><span class="w"> </span><span class="p">(</span><span class="kt">int</span><span class="w"> </span><span class="n">i</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="mi">0</span><span class="p">;</span><span class="w"> </span><span class="n">i</span><span class="w"> </span><span class="o"><</span><span class="w"> </span><span class="n">num_poses</span><span class="p">;</span><span class="w"> </span><span class="o">++</span><span class="n">i</span><span class="p">)</span><span class="w"> </span><span class="p">{</span><span class="w"></span>
<span class="w"> </span><span class="k">for</span><span class="w"> </span><span class="p">(</span><span class="kt">int</span><span class="w"> </span><span class="n">j</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="mi">0</span><span class="p">;</span><span class="w"> </span><span class="n">j</span><span class="w"> </span><span class="o"><</span><span class="w"> </span><span class="n">num_landmarks</span><span class="p">;</span><span class="w"> </span><span class="o">++</span><span class="n">j</span><span class="p">)</span><span class="w"> </span><span class="p">{</span><span class="w"></span>
<span class="w"> </span><span class="n">factors</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="n">sym</span><span class="o">::</span><span class="n">Factor</span><span class="o"><</span><span class="kt">double</span><span class="o">>::</span><span class="n">Hessian</span><span class="p">(</span><span class="w"></span>
<span class="w"> </span><span class="n">sym</span><span class="o">::</span><span class="n">BearingFactor</span><span class="o"><</span><span class="kt">double</span><span class="o">></span><span class="p">,</span><span class="w"></span>
<span class="w"> </span><span class="p">{{</span><span class="sc">'P'</span><span class="p">,</span><span class="w"> </span><span class="n">i</span><span class="p">},</span><span class="w"> </span><span class="p">{</span><span class="sc">'L'</span><span class="p">,</span><span class="w"> </span><span class="n">j</span><span class="p">},</span><span class="w"> </span><span class="p">{</span><span class="sc">'a'</span><span class="p">,</span><span class="w"> </span><span class="n">i</span><span class="p">,</span><span class="w"> </span><span class="n">j</span><span class="p">},</span><span class="w"> </span><span class="p">{</span><span class="sc">'e'</span><span class="p">}},</span><span class="w"> </span><span class="c1">// keys</span>
<span class="w"> </span><span class="p">{{</span><span class="sc">'P'</span><span class="p">,</span><span class="w"> </span><span class="n">i</span><span class="p">}}</span><span class="w"> </span><span class="c1">// keys to optimize</span>
<span class="w"> </span><span class="p">));</span><span class="w"></span>
<span class="w"> </span><span class="p">}</span><span class="w"></span>
<span class="p">}</span><span class="w"></span>
<span class="c1">// Odometry factors</span>
<span class="k">for</span><span class="w"> </span><span class="p">(</span><span class="kt">int</span><span class="w"> </span><span class="n">i</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="mi">0</span><span class="p">;</span><span class="w"> </span><span class="n">i</span><span class="w"> </span><span class="o"><</span><span class="w"> </span><span class="n">num_poses</span><span class="w"> </span><span class="o">-</span><span class="w"> </span><span class="mi">1</span><span class="p">;</span><span class="w"> </span><span class="o">++</span><span class="n">i</span><span class="p">)</span><span class="w"> </span><span class="p">{</span><span class="w"></span>
<span class="w"> </span><span class="n">factors</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="n">sym</span><span class="o">::</span><span class="n">Factor</span><span class="o"><</span><span class="kt">double</span><span class="o">>::</span><span class="n">Hessian</span><span class="p">(</span><span class="w"></span>
<span class="w"> </span><span class="n">sym</span><span class="o">::</span><span class="n">OdometryFactor</span><span class="o"><</span><span class="kt">double</span><span class="o">></span><span class="p">,</span><span class="w"></span>
<span class="w"> </span><span class="p">{{</span><span class="sc">'P'</span><span class="p">,</span><span class="w"> </span><span class="n">i</span><span class="p">},</span><span class="w"> </span><span class="p">{</span><span class="sc">'P'</span><span class="p">,</span><span class="w"> </span><span class="n">i</span><span class="w"> </span><span class="o">+</span><span class="w"> </span><span class="mi">1</span><span class="p">},</span><span class="w"> </span><span class="p">{</span><span class="sc">'d'</span><span class="p">,</span><span class="w"> </span><span class="n">i</span><span class="p">},</span><span class="w"> </span><span class="p">{</span><span class="sc">'e'</span><span class="p">}},</span><span class="w"> </span><span class="c1">// keys</span>
<span class="w"> </span><span class="p">{{</span><span class="sc">'P'</span><span class="p">,</span><span class="w"> </span><span class="n">i</span><span class="p">},</span><span class="w"> </span><span class="p">{</span><span class="sc">'P'</span><span class="p">,</span><span class="w"> </span><span class="n">i</span><span class="w"> </span><span class="o">+</span><span class="w"> </span><span class="mi">1</span><span class="p">}}</span><span class="w"> </span><span class="c1">// keys to optimize</span>
<span class="w"> </span><span class="p">));</span><span class="w"></span>
<span class="p">}</span><span class="w"></span>
<span class="k">const</span><span class="w"> </span><span class="k">auto</span><span class="w"> </span><span class="n">params</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">sym</span><span class="o">::</span><span class="n">DefaultOptimizerParams</span><span class="p">();</span><span class="w"></span>
<span class="n">sym</span><span class="o">::</span><span class="n">Optimizer</span><span class="o"><</span><span class="kt">double</span><span class="o">></span><span class="w"> </span><span class="n">optimizer</span><span class="p">(</span><span class="w"></span>
<span class="w"> </span><span class="n">params</span><span class="p">,</span><span class="w"></span>
<span class="w"> </span><span class="n">factors</span><span class="p">,</span><span class="w"></span>
<span class="w"> </span><span class="n">sym</span><span class="o">::</span><span class="n">kDefaultEpsilon</span><span class="o"><</span><span class="kt">double</span><span class="o">></span><span class="w"></span>
<span class="p">);</span><span class="w"></span>
<span class="n">sym</span><span class="o">::</span><span class="n">Values</span><span class="o"><</span><span class="kt">double</span><span class="o">></span><span class="w"> </span><span class="n">values</span><span class="p">;</span><span class="w"></span>
<span class="k">for</span><span class="w"> </span><span class="p">(</span><span class="kt">int</span><span class="w"> </span><span class="n">i</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="mi">0</span><span class="p">;</span><span class="w"> </span><span class="n">i</span><span class="w"> </span><span class="o"><</span><span class="w"> </span><span class="n">num_poses</span><span class="p">;</span><span class="w"> </span><span class="o">++</span><span class="n">i</span><span class="p">)</span><span class="w"> </span><span class="p">{</span><span class="w"></span>
<span class="w"> </span><span class="n">values</span><span class="p">.</span><span class="n">Set</span><span class="p">({</span><span class="sc">'P'</span><span class="p">,</span><span class="w"> </span><span class="n">i</span><span class="p">},</span><span class="w"> </span><span class="n">sym</span><span class="o">::</span><span class="n">Pose2d</span><span class="o">::</span><span class="n">Identity</span><span class="p">());</span><span class="w"></span>
<span class="p">}</span><span class="w"></span>
<span class="c1">// Set additional values</span>
<span class="n">values</span><span class="p">.</span><span class="n">Set</span><span class="p">({</span><span class="sc">'L'</span><span class="p">,</span><span class="w"> </span><span class="mi">0</span><span class="p">},</span><span class="w"> </span><span class="n">Eigen</span><span class="o">::</span><span class="n">Vector2d</span><span class="p">(</span><span class="mi">-2</span><span class="p">,</span><span class="w"> </span><span class="mi">2</span><span class="p">));</span><span class="w"></span>
<span class="n">values</span><span class="p">.</span><span class="n">Set</span><span class="p">({</span><span class="sc">'L'</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="p">},</span><span class="w"> </span><span class="n">Eigen</span><span class="o">::</span><span class="n">Vector2d</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span><span class="w"> </span><span class="mi">-3</span><span class="p">));</span><span class="w"></span>
<span class="n">values</span><span class="p">.</span><span class="n">Set</span><span class="p">({</span><span class="sc">'L'</span><span class="p">,</span><span class="w"> </span><span class="mi">2</span><span class="p">},</span><span class="w"> </span><span class="n">Eigen</span><span class="o">::</span><span class="n">Vector2d</span><span class="p">(</span><span class="mi">5</span><span class="p">,</span><span class="w"> </span><span class="mi">2</span><span class="p">));</span><span class="w"></span>
<span class="n">values</span><span class="p">.</span><span class="n">Set</span><span class="p">({</span><span class="sc">'d'</span><span class="p">,</span><span class="w"> </span><span class="mi">0</span><span class="p">},</span><span class="w"> </span><span class="mf">1.7</span><span class="p">);</span><span class="w"></span>
<span class="n">values</span><span class="p">.</span><span class="n">Set</span><span class="p">({</span><span class="sc">'d'</span><span class="p">,</span><span class="w"> </span><span class="mi">1</span><span class="p">},</span><span class="w"> </span><span class="mf">1.4</span><span class="p">);</span><span class="w"></span>
<span class="k">const</span><span class="w"> </span><span class="n">std</span><span class="o">::</span><span class="n">array</span><span class="o"><</span><span class="n">std</span><span class="o">::</span><span class="n">array</span><span class="o"><</span><span class="kt">double</span><span class="p">,</span><span class="w"> </span><span class="mi">3</span><span class="o">></span><span class="p">,</span><span class="w"> </span><span class="mi">3</span><span class="o">></span><span class="w"> </span><span class="n">angles</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="p">{</span><span class="w"></span>
<span class="w"> </span><span class="p">{{</span><span class="mi">55</span><span class="p">,</span><span class="w"> </span><span class="mi">245</span><span class="p">,</span><span class="w"> </span><span class="mi">-35</span><span class="p">},</span><span class="w"> </span><span class="p">{</span><span class="mi">95</span><span class="p">,</span><span class="w"> </span><span class="mi">220</span><span class="p">,</span><span class="w"> </span><span class="mi">-20</span><span class="p">},</span><span class="w"> </span><span class="p">{</span><span class="mi">125</span><span class="p">,</span><span class="w"> </span><span class="mi">220</span><span class="p">,</span><span class="w"> </span><span class="mi">-20</span><span class="p">}}</span><span class="w"></span>
<span class="p">};</span><span class="w"></span>
<span class="k">for</span><span class="w"> </span><span class="p">(</span><span class="kt">int</span><span class="w"> </span><span class="n">i</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="mi">0</span><span class="p">;</span><span class="w"> </span><span class="n">i</span><span class="w"> </span><span class="o"><</span><span class="w"> </span><span class="n">angles</span><span class="p">.</span><span class="n">size</span><span class="p">();</span><span class="w"> </span><span class="o">++</span><span class="n">i</span><span class="p">)</span><span class="w"> </span><span class="p">{</span><span class="w"></span>
<span class="w"> </span><span class="k">for</span><span class="w"> </span><span class="p">(</span><span class="kt">int</span><span class="w"> </span><span class="n">j</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="mi">0</span><span class="p">;</span><span class="w"> </span><span class="n">j</span><span class="w"> </span><span class="o"><</span><span class="w"> </span><span class="n">angles</span><span class="p">[</span><span class="mi">0</span><span class="p">].</span><span class="n">size</span><span class="p">();</span><span class="w"> </span><span class="o">++</span><span class="n">j</span><span class="p">)</span><span class="w"> </span><span class="p">{</span><span class="w"></span>
<span class="w"> </span><span class="n">values</span><span class="p">.</span><span class="n">Set</span><span class="p">({</span><span class="sc">'a'</span><span class="p">,</span><span class="w"> </span><span class="n">i</span><span class="p">,</span><span class="w"> </span><span class="n">j</span><span class="p">},</span><span class="w"> </span><span class="n">angles</span><span class="p">[</span><span class="n">i</span><span class="p">][</span><span class="n">j</span><span class="p">]</span><span class="w"> </span><span class="o">*</span><span class="w"> </span><span class="n">M_PI</span><span class="w"> </span><span class="o">/</span><span class="w"> </span><span class="mi">180</span><span class="p">);</span><span class="w"></span>
<span class="w"> </span><span class="p">}</span><span class="w"></span>
<span class="p">}</span><span class="w"></span>
<span class="n">values</span><span class="p">.</span><span class="n">Set</span><span class="p">(</span><span class="sc">'e'</span><span class="p">,</span><span class="w"> </span><span class="n">sym</span><span class="o">::</span><span class="n">kDefaultEpsilond</span><span class="p">);</span><span class="w"></span>
<span class="c1">// Optimize!</span>
<span class="k">const</span><span class="w"> </span><span class="k">auto</span><span class="w"> </span><span class="n">stats</span><span class="w"> </span><span class="o">=</span><span class="w"> </span><span class="n">optimizer</span><span class="p">.</span><span class="n">Optimize</span><span class="p">(</span><span class="o">&</span><span class="n">values</span><span class="p">);</span><span class="w"></span>
<span class="n">std</span><span class="o">::</span><span class="n">cout</span><span class="w"> </span><span class="o"><<</span><span class="w"> </span><span class="s">"Optimized values:"</span><span class="w"> </span><span class="o"><<</span><span class="w"> </span><span class="n">values</span><span class="w"> </span><span class="o"><<</span><span class="w"> </span><span class="n">std</span><span class="o">::</span><span class="n">endl</span><span class="p">;</span><span class="w"></span>
</pre></div>
</div>
<p>This tutorial shows the central workflow in SymForce for creating symbolic expressions, generating code, and optimizing. This approach works well for a wide range of complex problems in robotics, computer vision, and applied science.</p>
<p>However, each piece may also be used independently. The optimization machinery can work with handwritten functions, and the symbolic math and code generation is useful outside of any optimization context.</p>
<!-- $
<span style="color:blue">TODO: I wanted to show `sf.V1(sm.atan2(landmark_body[1], landmark_body[0])).jacobian(pose.R)`, but you have to call `sm.simplify` to get the expression to -1, otherwise it's more complicated. All this is also showing up extraneously in the generated code. Discuss what to show.</span>
\frac{
(-\frac{
(-R_{im} L_0 + R_{re} L_1 + R_{im} t_0 + R_{re} t_1)^2
}{
(R_{re} L_0 + R_{im} L_1 - R_{im} t_1 - R_{re} t_0)^2
} + \frac{
}{
})(R_{re} L_0 + R_{im} L_1 - R_{im} t_1 - R_{re} t_0)^2
}{
(-R_{im} L_0 + R_{re} L_1 + R_{im} t_0 + R_{re} t_1)^2 +
(R_{re} L_0 + R_{im} L_1 - R_{im} t_1 - R_{re} t_0)^2
}
$ -->
<p>To learn more, visit the SymForce tutorials <a class="reference external" href="https://symforce.org/#guides">here</a>.</p>
</section>
</section>
<section id="build-from-source">
<h2>Build from Source<a class="headerlink" href="#build-from-source" title="Permalink to this headline">¶</a></h2>
<p>SymForce requires Python 3.8 or later. The build is currently tested on Linux and macOS, SymForce on Windows is untested (see <a class="reference external" href="https://github.com/symforce-org/symforce/issues/145">#145</a>). We strongly suggest creating a virtual python environment.</p>
<p>Install the <code class="docutils literal notranslate"><span class="pre">gmp</span></code> package with one of:</p>
<div class="highlight-bash notranslate"><div class="highlight"><pre><span></span>apt install libgmp-dev <span class="c1"># Ubuntu</span>
brew install gmp <span class="c1"># Mac</span>
conda install -c conda-forge gmp <span class="c1"># Conda</span>
</pre></div>
</div>
<p>SymForce contains both C++ and Python code. The C++ code is built using CMake. You can build the package either by calling pip, or by calling CMake directly. If building with <code class="docutils literal notranslate"><span class="pre">pip</span></code>, this will call CMake under the hood, and run the same CMake build for the C++ components.</p>
<p>If you encounter build issues, please file an <a class="reference external" href="https://github.com/symforce-org/symforce/issues">issue</a>.</p>
<section id="build-with-pip">
<h3>Build with pip<a class="headerlink" href="#build-with-pip" title="Permalink to this headline">¶</a></h3>
<p>The recommended way to build and install SymForce if you only plan on making Python changes is with pip. From the symforce directory:</p>
<div class="highlight-bash notranslate"><div class="highlight"><pre><span></span>pip install -e .
</pre></div>
</div>
<p>This will build the C++ components of SymForce, but you won’t be able to run <code class="docutils literal notranslate"><span class="pre">pip</span> <span class="pre">install</span> <span class="pre">-e</span> <span class="pre">.</span></code> repeatedly if you need to rebuild C++ code. If you’re changing C++ code and rebuilding, you should build with CMake directly as described <a href="#build-with-cmake">below</a>.</p>
<p><code class="docutils literal notranslate"><span class="pre">pip</span> <span class="pre">install</span> <span class="pre">.</span></code> will not install pinned versions of SymForce’s dependencies, it’ll install any compatible versions. It also won’t install all packages required to run all of the SymForce tests and build all of the targets (e.g. building the docs or running the linters). If you want all packages required for that, you should <code class="docutils literal notranslate"><span class="pre">pip</span> <span class="pre">install</span> <span class="pre">.[dev]</span></code> instead (or one of the other groups of extra requirements in our <code class="docutils literal notranslate"><span class="pre">setup.py</span></code>). If you additionally want pinned versions of our dependencies, which are the exact versions guaranteed by CI to pass all of our tests, you can install them from <code class="docutils literal notranslate"><span class="pre">pip</span> <span class="pre">install</span> <span class="pre">-r</span> <span class="pre">dev_requirements.txt</span></code>.</p>
</section>
<section id="build-with-cmake">
<h3>Build with CMake<a class="headerlink" href="#build-with-cmake" title="Permalink to this headline">¶</a></h3>
<p>If you’ll be modifying the C++ parts of SymForce, you should build with CMake directly instead - this method will not install
SymForce into your Python environment, so you’ll need to add it to your PYTHONPATH separately.</p>
<p>Install python requirements:</p>
<div class="highlight-bash notranslate"><div class="highlight"><pre><span></span>pip install -r dev_requirements.txt
</pre></div>
</div>
<p>Build SymForce (requires C++14 or later):</p>
<div class="highlight-bash notranslate"><div class="highlight"><pre><span></span>mkdir build
<span class="nb">cd</span> build
cmake ..
make -j <span class="k">$(</span>nproc<span class="k">)</span>
</pre></div>
</div>
<p>You’ll then need to add SymForce (along with <code class="docutils literal notranslate"><span class="pre">gen/python</span></code> and <code class="docutils literal notranslate"><span class="pre">third_party/skymarshal</span></code> within symforce) to your PYTHONPATH in order to use them.</p>
</section>
</section>
<section id="license">
<h2>License<a class="headerlink" href="#license" title="Permalink to this headline">¶</a></h2>
<p>SymForce is released under the <a class="reference external" href="https://spdx.org/licenses/Apache-2.0.html">Apache 2.0</a> license.</p>
<p>See the <a class="reference external" href="https://github.com/symforce-org/symforce/blob/main/LICENSE">LICENSE</a> file for more information.</p>
</section>
<section id="sponsors">
<h2>Sponsors<a class="headerlink" href="#sponsors" title="Permalink to this headline">¶</a></h2>
<p>SymForce is developed and maintained by <a class="reference external" href="https://skydio.com/">Skydio</a>. It is released as a free and open-source library for the robotics community.</p>
<a href="http://skydio.com#gh-light-mode-only">
<img alt="Skydio Logo" src="docs/static/images/skydio-logo-2.png" width="300px" />
</a>
<!-- DARK_MODE_ONLY -->
<a href="http://skydio.com#gh-dark-mode-only">
<img alt="Skydio Logo" src="docs/static/images/skydio-logo-2-white.png" width="300px" />
</a>
<!-- /DARK_MODE_ONLY -->
</section>
<section id="contributing">
<h2>Contributing<a class="headerlink" href="#contributing" title="Permalink to this headline">¶</a></h2>
<p>While SymForce already powers tens of thousands of robots at Skydio, the public library is new and we are releasing it in beta stage. This is just the beginning, and we are excited for engagement from the community. Thank you for helping us develop SymForce! The best way to get started is to file <a class="reference external" href="https://github.com/symforce-org/symforce/issues">issues</a> for bugs or desired features.</p>
<p>There are many features we’re excited to add to SymForce and would love to see contributed by the community. Most are outlined in the issues, but some major desired contributions are:</p>
<ul class="simple">
<li><p>Add more backend languages, such as TypeScript, CUDA, GLSL/HLSL, and PyTorch</p></li>
<li><p>Easily swap in approximate or architecture-specific implementations of primitive
functions, such as trig functions</p></li>
<li><p>Support for WebAssembly compilation</p></li>
<li><p>More Lie group types, in particular Sim(3)</p></li>
<li><p>Support for constraints in our optimizer</p></li>
<li><p>Integration with <a class="reference external" href="https://ispc.github.io/">ISPC</a></p></li>
</ul>
</section>
</section>
<section id="guides">
<h1>Guides<a class="headerlink" href="#guides" title="Permalink to this headline">¶</a></h1>
<dl class="simple">
<dt><a class="reference internal" href="development.html"><span class="doc">Development Guide</span></a></dt><dd><p>How to build, configure, and develop</p>
</dd>
<dt><a class="reference internal" href="tutorials/sympy_tutorial.html"><span class="doc">SymPy Tutorial</span></a></dt><dd><p>Basic introduction to SymPy</p>
</dd>
<dt><a class="reference internal" href="tutorials/geometry_tutorial.html"><span class="doc">Geometry Tutorial</span></a></dt><dd><p>Introductory guide to doing math and geometry</p>
</dd>
<dt><a class="reference internal" href="tutorials/ops_tutorial.html"><span class="doc">Ops Tutorial</span></a></dt><dd><p>Introductory guide to using Ops in symforce</p>
</dd>
<dt><a class="reference internal" href="tutorials/cameras_tutorial.html"><span class="doc">Cameras Tutorial</span></a></dt><dd><p>Introductory guide to using camera models</p>
</dd>
<dt><a class="reference internal" href="tutorials/values_tutorial.html"><span class="doc">Values Tutorial</span></a></dt><dd><p>How to structure large groups of symbols and expressions</p>
</dd>
<dt><a class="reference internal" href="tutorials/codegen_tutorial.html"><span class="doc">Codegen Tutorial</span></a></dt><dd><p>How to generate functions from symbolic expressions</p>
</dd>
<dt><a class="reference internal" href="tutorials/optimization_tutorial.html"><span class="doc">Optimization Tutorial</span></a></dt><dd><p>Basic example of using generated code to do optimization</p>
</dd>
<dt><a class="reference internal" href="tutorials/epsilon_tutorial.html"><span class="doc">Epsilon Tutorial</span></a></dt><dd><p>Guide to how Epsilon is used to prevent singularities</p>
</dd>
</dl>
<div class="toctree-wrapper compound">
</div>
<div class="toctree-wrapper compound">
</div>
<div class="toctree-wrapper compound">
</div>
<div class="toctree-wrapper compound" id="api-reference">
</div>
<div class="toctree-wrapper compound" id="genpy-api-reference">
</div>
<div class="toctree-wrapper compound" id="gencpp-api-reference">
</div>
<div class="toctree-wrapper compound" id="cpp-api-reference">
</div>
</section>
</div>
</div>
</div>
<div class="sphinxsidebar" role="navigation" aria-label="main navigation">
<div class="sphinxsidebarwrapper">
<h1 class="logo"><a href="#">symforce</a></h1>
<p class="blurb">Fast symbolic computation, code generation, and nonlinear optimization for robotics</p>
<p>
<iframe src="https://ghbtns.com/github-btn.html?user=symforce-org&repo=symforce&type=star&count=true&size=large&v=2"
allowtransparency="true" frameborder="0" scrolling="0" width="200px" height="35px"></iframe>
</p>
<h3>Navigation</h3>
<ul>
<li class="toctree-l1"><a class="reference internal" href="development.html">Development Guide</a></li>
</ul>
<p class="caption" role="heading"><span class="caption-text">Tutorials</span></p>
<ul>
<li class="toctree-l1"><a class="reference internal" href="tutorials/sympy_tutorial.html">SymPy Tutorial</a></li>
<li class="toctree-l1"><a class="reference internal" href="tutorials/geometry_tutorial.html">Geometry Tutorial</a></li>
<li class="toctree-l1"><a class="reference internal" href="tutorials/ops_tutorial.html">Ops Tutorial</a></li>
<li class="toctree-l1"><a class="reference internal" href="tutorials/cameras_tutorial.html">Cameras Tutorial</a></li>
<li class="toctree-l1"><a class="reference internal" href="tutorials/values_tutorial.html">Values Tutorial</a></li>
<li class="toctree-l1"><a class="reference internal" href="tutorials/codegen_tutorial.html">Codegen Tutorial</a></li>
<li class="toctree-l1"><a class="reference internal" href="tutorials/optimization_tutorial.html">Optimization Tutorial</a></li>
<li class="toctree-l1"><a class="reference internal" href="tutorials/epsilon_tutorial.html">Epsilon Tutorial</a></li>
</ul>
<p class="caption" role="heading"><span class="caption-text">Examples</span></p>
<ul>
<li class="toctree-l1"><a class="reference internal" href="examples/bundle_adjustment/README.html">Bundle Adjustment</a></li>
<li class="toctree-l1"><a class="reference internal" href="examples/bundle_adjustment_fixed_size/README.html">Fixed Size Bundle Adjustment</a></li>
<li class="toctree-l1"><a class="reference internal" href="examples/bundle_adjustment_in_the_large/README.html">Bundle-Adjustment-in-the-Large</a></li>
<li class="toctree-l1"><a class="reference internal" href="examples/custom_factor_generation/README.html">Custom Factor Generation</a></li>
<li class="toctree-l1"><a class="reference internal" href="examples/robot_2d_localization/README.html">Robot 2D Localization</a></li>
<li class="toctree-l1"><a class="reference internal" href="examples/robot_3d_localization/README.html">Robot 3D Localization</a></li>
</ul>
<p class="caption" role="heading"><span class="caption-text">symforce Reference</span></p>
<ul>
<li class="toctree-l1"><a class="reference internal" href="api/symforce.html">symforce package</a></li>
</ul>
<p class="caption" role="heading"><span class="caption-text">sym Python Reference</span></p>
<ul>
<li class="toctree-l1"><a class="reference internal" href="api-gen-py/sym.html">sym package</a></li>
</ul>
<p class="caption" role="heading"><span class="caption-text">sym C++ Reference</span></p>
<ul>
<li class="toctree-l1"><a class="reference internal" href="api-gen-cpp/classlist.html">Class list</a></li>
<li class="toctree-l1"><a class="reference internal" href="api-gen-cpp/filelist.html">File list</a></li>
</ul>
<p class="caption" role="heading"><span class="caption-text">opt C++ Reference</span></p>
<ul>
<li class="toctree-l1"><a class="reference internal" href="api-cpp/classlist.html">Class list</a></li>
<li class="toctree-l1"><a class="reference internal" href="api-cpp/filelist.html">File list</a></li>
</ul>
<div class="relations">
<h3>Related Topics</h3>
<ul>
<li><a href="#">Documentation overview</a><ul>
<li>Next: <a href="development.html" title="next chapter">Development Guide</a></li>
</ul></li>
</ul>
</div>
<div id="searchbox" style="display: none" role="search">
<h3 id="searchlabel">Quick search</h3>
<div class="searchformwrapper">
<form class="search" action="search.html" method="get">
<input type="text" name="q" aria-labelledby="searchlabel" autocomplete="off" autocorrect="off" autocapitalize="off" spellcheck="false"/>
<input type="submit" value="Go" />
</form>
</div>
</div>
<script>$('#searchbox').show(0);</script>
</div>
</div>
<div class="clearer"></div>
</div>
<div class="footer">
©2022, Skydio, Inc.
|
Powered by <a href="http://sphinx-doc.org/">Sphinx 4.5.0</a>
& <a href="https://github.com/bitprophet/alabaster">Alabaster 0.7.12</a>
|
<a href="_sources/index.rst.txt"
rel="nofollow">Page source</a>
</div>
</body>
</html>