-
Notifications
You must be signed in to change notification settings - Fork 14
/
Copy patha01454_source.html
262 lines (260 loc) · 40 KB
/
a01454_source.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
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "https://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=11"/>
<meta name="generator" content="Doxygen 1.9.3"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>gtsam: /Users/dellaert/git/github/gtsam_unstable/slam/InvDepthFactorVariant2.h Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="resize.js"></script>
<script type="text/javascript" src="navtreedata.js"></script>
<script type="text/javascript" src="navtree.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/x-mathjax-config">
MathJax.Hub.Config({
extensions: ["tex2jax.js"],
jax: ["input/TeX","output/HTML-CSS"],
});
</script>
<script type="text/javascript" async="async" src="https://cdn.jsdelivr.net/npm/mathjax@2/MathJax.js"></script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
<tbody>
<tr id="projectrow">
<td id="projectalign">
<div id="projectname">gtsam<span id="projectnumber"> 4.1.1</span>
</div>
<div id="projectbrief">gtsam</div>
</td>
</tr>
</tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.9.3 -->
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:d3d9a9a6595521f9666a5e94cc830dab83b65699&dn=expat.txt MIT */
var searchBox = new SearchBox("searchBox", "search",'Search','.html');
/* @license-end */
</script>
<script type="text/javascript" src="menudata.js"></script>
<script type="text/javascript" src="menu.js"></script>
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:d3d9a9a6595521f9666a5e94cc830dab83b65699&dn=expat.txt MIT */
$(function() {
initMenu('',true,false,'search.php','Search');
$(document).ready(function() { init_search(); });
});
/* @license-end */
</script>
<div id="main-nav"></div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
<div id="nav-tree">
<div id="nav-tree-contents">
<div id="nav-sync" class="sync"></div>
</div>
</div>
<div id="splitbar" style="-moz-user-select:none;"
class="ui-resizable-handle">
</div>
</div>
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:d3d9a9a6595521f9666a5e94cc830dab83b65699&dn=expat.txt MIT */
$(document).ready(function(){initNavTree('a01454_source.html',''); initResizable(); });
/* @license-end */
</script>
<div id="doc-content">
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
onmouseover="return searchBox.OnSearchSelectShow()"
onmouseout="return searchBox.OnSearchSelectHide()"
onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>
<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0"
name="MSearchResults" id="MSearchResults">
</iframe>
</div>
<div class="header">
<div class="headertitle"><div class="title">InvDepthFactorVariant2.h</div></div>
</div><!--header-->
<div class="contents">
<a href="a01454.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a id="l00001" name="l00001"></a><span class="lineno"> 1</span> </div>
<div class="line"><a id="l00012" name="l00012"></a><span class="lineno"> 12</span><span class="preprocessor">#pragma once</span></div>
<div class="line"><a id="l00013" name="l00013"></a><span class="lineno"> 13</span> </div>
<div class="line"><a id="l00014" name="l00014"></a><span class="lineno"> 14</span><span class="preprocessor">#include <<a class="code" href="a01022.html">gtsam/nonlinear/NonlinearFactor.h</a>></span></div>
<div class="line"><a id="l00015" name="l00015"></a><span class="lineno"> 15</span><span class="preprocessor">#include <<a class="code" href="a00314.html">gtsam/geometry/PinholeCamera.h</a>></span></div>
<div class="line"><a id="l00016" name="l00016"></a><span class="lineno"> 16</span><span class="preprocessor">#include <<a class="code" href="a00266.html">gtsam/geometry/Cal3_S2.h</a>></span></div>
<div class="line"><a id="l00017" name="l00017"></a><span class="lineno"> 17</span><span class="preprocessor">#include <<a class="code" href="a00350.html">gtsam/geometry/Pose3.h</a>></span></div>
<div class="line"><a id="l00018" name="l00018"></a><span class="lineno"> 18</span><span class="preprocessor">#include <<a class="code" href="a00380.html">gtsam/geometry/Point2.h</a>></span></div>
<div class="line"><a id="l00019" name="l00019"></a><span class="lineno"> 19</span><span class="preprocessor">#include <<a class="code" href="a00125.html">gtsam/base/numericalDerivative.h</a>></span></div>
<div class="line"><a id="l00020" name="l00020"></a><span class="lineno"> 20</span> </div>
<div class="line"><a id="l00021" name="l00021"></a><span class="lineno"> 21</span><span class="preprocessor">#include <boost/bind/bind.hpp></span></div>
<div class="line"><a id="l00022" name="l00022"></a><span class="lineno"> 22</span> </div>
<div class="line"><a id="l00023" name="l00023"></a><span class="lineno"> 23</span><span class="keyword">namespace </span><a class="code hl_namespace" href="a01596.html">gtsam</a> {</div>
<div class="line"><a id="l00024" name="l00024"></a><span class="lineno"> 24</span> </div>
<div class="line"><a id="l00028" name="l00028"></a><span class="lineno"><a class="line" href="a05280.html"> 28</a></span><span class="keyword">class </span><a class="code hl_class" href="a05280.html">InvDepthFactorVariant2</a>: <span class="keyword">public</span> <a class="code hl_class" href="a04456.html">NoiseModelFactor2</a><Pose3, Vector3> {</div>
<div class="line"><a id="l00029" name="l00029"></a><span class="lineno"> 29</span><span class="keyword">protected</span>:</div>
<div class="line"><a id="l00030" name="l00030"></a><span class="lineno"> 30</span> </div>
<div class="line"><a id="l00031" name="l00031"></a><span class="lineno"> 31</span> <span class="comment">// Keep a copy of measurement and calibration for I/O</span></div>
<div class="line"><a id="l00032" name="l00032"></a><span class="lineno"><a class="line" href="a05280.html#af936b4b128aac0b1cad29a8ad021dd96"> 32</a></span> <a class="code hl_typedef" href="a01596.html#a6ede8384dee0353a0ce5fb54ea50c21d">Point2</a> <a class="code hl_variable" href="a05280.html#af936b4b128aac0b1cad29a8ad021dd96">measured_</a>; </div>
<div class="line"><a id="l00033" name="l00033"></a><span class="lineno"><a class="line" href="a05280.html#a02c0d546827eaf8efd7cb7447b0e37b1"> 33</a></span> Cal3_S2::shared_ptr <a class="code hl_variable" href="a05280.html#a02c0d546827eaf8efd7cb7447b0e37b1">K_</a>; </div>
<div class="line"><a id="l00034" name="l00034"></a><span class="lineno"><a class="line" href="a05280.html#a1751ebcce9070692b0323a99e500b5c3"> 34</a></span> <a class="code hl_typedef" href="a01596.html#aaa8ed89fd60ea4601d9de63c4811525b">Point3</a> <a class="code hl_variable" href="a05280.html#a1751ebcce9070692b0323a99e500b5c3">referencePoint_</a>; </div>
<div class="line"><a id="l00035" name="l00035"></a><span class="lineno"> 35</span> </div>
<div class="line"><a id="l00036" name="l00036"></a><span class="lineno"> 36</span><span class="keyword">public</span>:</div>
<div class="line"><a id="l00037" name="l00037"></a><span class="lineno"> 37</span> </div>
<div class="line"><a id="l00039" name="l00039"></a><span class="lineno"><a class="line" href="a05280.html#addb66abec98197833c991df96ccdbd48"> 39</a></span> <span class="keyword">typedef</span> <a class="code hl_class" href="a04456.html">NoiseModelFactor2<Pose3, Vector3></a> <a class="code hl_typedef" href="a05280.html#addb66abec98197833c991df96ccdbd48">Base</a>;</div>
<div class="line"><a id="l00040" name="l00040"></a><span class="lineno"> 40</span> </div>
<div class="line"><a id="l00042" name="l00042"></a><span class="lineno"><a class="line" href="a05280.html#af99a8bcbe5cfe559724ec2d02df4d072"> 42</a></span> <span class="keyword">typedef</span> <a class="code hl_class" href="a05280.html">InvDepthFactorVariant2</a> <a class="code hl_typedef" href="a05280.html#af99a8bcbe5cfe559724ec2d02df4d072">This</a>;</div>
<div class="line"><a id="l00043" name="l00043"></a><span class="lineno"> 43</span> </div>
<div class="line"><a id="l00045" name="l00045"></a><span class="lineno"><a class="line" href="a05280.html#aa1de0753a1897687a3376956a56a21c9"> 45</a></span> <span class="keyword">typedef</span> boost::shared_ptr<This> <a class="code hl_typedef" href="a05280.html#aa1de0753a1897687a3376956a56a21c9">shared_ptr</a>;</div>
<div class="line"><a id="l00046" name="l00046"></a><span class="lineno"> 46</span> </div>
<div class="line"><a id="l00048" name="l00048"></a><span class="lineno"><a class="line" href="a05280.html#a2a1562da3696397fcbdfc15b5ea37029"> 48</a></span> <a class="code hl_function" href="a05280.html#a2a1562da3696397fcbdfc15b5ea37029">InvDepthFactorVariant2</a>() :</div>
<div class="line"><a id="l00049" name="l00049"></a><span class="lineno"> 49</span> <a class="code hl_variable" href="a05280.html#af936b4b128aac0b1cad29a8ad021dd96">measured_</a>(0.0, 0.0), <a class="code hl_variable" href="a05280.html#a02c0d546827eaf8efd7cb7447b0e37b1">K_</a>(new <a class="code hl_class" href="a03052.html">Cal3_S2</a>(444, 555, 666, 777, 888)) {</div>
<div class="line"><a id="l00050" name="l00050"></a><span class="lineno"> 50</span> }</div>
<div class="line"><a id="l00051" name="l00051"></a><span class="lineno"> 51</span> </div>
<div class="line"><a id="l00060" name="l00060"></a><span class="lineno"><a class="line" href="a05280.html#ae845ae5cd3dcb960941edd2e29fdd643"> 60</a></span> <a class="code hl_function" href="a05280.html#ae845ae5cd3dcb960941edd2e29fdd643">InvDepthFactorVariant2</a>(<span class="keyword">const</span> <a class="code hl_typedef" href="a01596.html#adad029f5f6ffce610428b5fe768b0df2">Key</a> poseKey, <span class="keyword">const</span> <a class="code hl_typedef" href="a01596.html#adad029f5f6ffce610428b5fe768b0df2">Key</a> landmarkKey,</div>
<div class="line"><a id="l00061" name="l00061"></a><span class="lineno"> 61</span> <span class="keyword">const</span> <a class="code hl_typedef" href="a01596.html#a6ede8384dee0353a0ce5fb54ea50c21d">Point2</a>& measured, <span class="keyword">const</span> Cal3_S2::shared_ptr& K, <span class="keyword">const</span> <a class="code hl_typedef" href="a01596.html#aaa8ed89fd60ea4601d9de63c4811525b">Point3</a> <a class="code hl_function" href="a05280.html#a99e83c3e07540b08e4138201532504ee">referencePoint</a>,</div>
<div class="line"><a id="l00062" name="l00062"></a><span class="lineno"> 62</span> <span class="keyword">const</span> <a class="code hl_typedef" href="a01596.html#ab6e5a4884342656e0837ef07008ec03f">SharedNoiseModel</a>& model) :</div>
<div class="line"><a id="l00063" name="l00063"></a><span class="lineno"> 63</span> <a class="code hl_class" href="a03544.html">Base</a>(model, poseKey, landmarkKey), <a class="code hl_variable" href="a05280.html#af936b4b128aac0b1cad29a8ad021dd96">measured_</a>(measured), <a class="code hl_variable" href="a05280.html#a02c0d546827eaf8efd7cb7447b0e37b1">K_</a>(K), <a class="code hl_variable" href="a05280.html#a1751ebcce9070692b0323a99e500b5c3">referencePoint_</a>(<a class="code hl_function" href="a05280.html#a99e83c3e07540b08e4138201532504ee">referencePoint</a>) {}</div>
<div class="line"><a id="l00064" name="l00064"></a><span class="lineno"> 64</span> </div>
<div class="line"><a id="l00066" name="l00066"></a><span class="lineno"><a class="line" href="a05280.html#a4f7c7c86906838768800ddb8e0d55556"> 66</a></span> <a class="code hl_function" href="a05280.html#a4f7c7c86906838768800ddb8e0d55556">~InvDepthFactorVariant2</a>()<span class="keyword"> override </span>{}</div>
<div class="line"><a id="l00067" name="l00067"></a><span class="lineno"> 67</span> </div>
<div class="line"><a id="l00073" name="l00073"></a><span class="lineno"><a class="line" href="a05280.html#a9bc66090d5477a029e92d339ffd9fbe6"> 73</a></span> <span class="keywordtype">void</span> <a class="code hl_function" href="a05280.html#a9bc66090d5477a029e92d339ffd9fbe6">print</a>(<span class="keyword">const</span> std::string& s = <span class="stringliteral">"InvDepthFactorVariant2"</span>,</div>
<div class="line"><a id="l00074" name="l00074"></a><span class="lineno"> 74</span> <span class="keyword">const</span> <a class="code hl_typedef" href="a01596.html#ae4b4e8e7f0d745882c6a02b507d5bffe">KeyFormatter</a>& keyFormatter = DefaultKeyFormatter)<span class="keyword"> const override </span>{</div>
<div class="line"><a id="l00075" name="l00075"></a><span class="lineno"> 75</span> <a class="code hl_function" href="a04448.html#ab45b9c35a90eca2777f32dab550d1569">Base::print</a>(s, keyFormatter);</div>
<div class="line"><a id="l00076" name="l00076"></a><span class="lineno"> 76</span> <a class="code hl_struct" href="a02444.html">traits<Point2>::Print</a>(<a class="code hl_variable" href="a05280.html#af936b4b128aac0b1cad29a8ad021dd96">measured_</a>, s + <span class="stringliteral">".z"</span>);</div>
<div class="line"><a id="l00077" name="l00077"></a><span class="lineno"> 77</span> }</div>
<div class="line"><a id="l00078" name="l00078"></a><span class="lineno"> 78</span> </div>
<div class="line"><a id="l00080" name="l00080"></a><span class="lineno"><a class="line" href="a05280.html#a399e49735e88e4900188a60bfc74d7a1"> 80</a></span> <span class="keywordtype">bool</span> <a class="code hl_function" href="a05280.html#a399e49735e88e4900188a60bfc74d7a1">equals</a>(<span class="keyword">const</span> <a class="code hl_class" href="a04440.html">NonlinearFactor</a>& p, <span class="keywordtype">double</span> tol = 1e-9)<span class="keyword"> const override </span>{</div>
<div class="line"><a id="l00081" name="l00081"></a><span class="lineno"> 81</span> <span class="keyword">const</span> <a class="code hl_class" href="a03544.html">This</a> *e = <span class="keyword">dynamic_cast<</span><span class="keyword">const </span><a class="code hl_class" href="a03544.html">This</a>*<span class="keyword">></span>(&p);</div>
<div class="line"><a id="l00082" name="l00082"></a><span class="lineno"> 82</span> <span class="keywordflow">return</span> e</div>
<div class="line"><a id="l00083" name="l00083"></a><span class="lineno"> 83</span> && <a class="code hl_function" href="a04448.html#a0cb99e64e817f707a5bd5ed2e951af2e">Base::equals</a>(p, tol)</div>
<div class="line"><a id="l00084" name="l00084"></a><span class="lineno"> 84</span> && <a class="code hl_struct" href="a02444.html">traits<Point2>::Equals</a>(this->measured_, e->measured_, tol)</div>
<div class="line"><a id="l00085" name="l00085"></a><span class="lineno"> 85</span> && this->K_->equals(*e->K_, tol)</div>
<div class="line"><a id="l00086" name="l00086"></a><span class="lineno"> 86</span> && <a class="code hl_struct" href="a02444.html">traits<Point3>::Equals</a>(this->referencePoint_, e->referencePoint_, tol);</div>
<div class="line"><a id="l00087" name="l00087"></a><span class="lineno"> 87</span> }</div>
<div class="line"><a id="l00088" name="l00088"></a><span class="lineno"> 88</span> </div>
<div class="line"><a id="l00089" name="l00089"></a><span class="lineno"> 89</span> Vector inverseDepthError(<span class="keyword">const</span> <a class="code hl_class" href="a03288.html">Pose3</a>& pose, <span class="keyword">const</span> Vector3& landmark)<span class="keyword"> const </span>{</div>
<div class="line"><a id="l00090" name="l00090"></a><span class="lineno"> 90</span> <span class="keywordflow">try</span> {</div>
<div class="line"><a id="l00091" name="l00091"></a><span class="lineno"> 91</span> <span class="comment">// Calculate the 3D coordinates of the landmark in the world frame</span></div>
<div class="line"><a id="l00092" name="l00092"></a><span class="lineno"> 92</span> <span class="keywordtype">double</span> theta = landmark(0), phi = landmark(1), rho = landmark(2);</div>
<div class="line"><a id="l00093" name="l00093"></a><span class="lineno"> 93</span> <a class="code hl_typedef" href="a01596.html#aaa8ed89fd60ea4601d9de63c4811525b">Point3</a> world_P_landmark = <a class="code hl_variable" href="a05280.html#a1751ebcce9070692b0323a99e500b5c3">referencePoint_</a> + <a class="code hl_typedef" href="a01596.html#aaa8ed89fd60ea4601d9de63c4811525b">Point3</a>(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);</div>
<div class="line"><a id="l00094" name="l00094"></a><span class="lineno"> 94</span> <span class="comment">// Project landmark into Pose2</span></div>
<div class="line"><a id="l00095" name="l00095"></a><span class="lineno"> 95</span> <a class="code hl_class" href="a03212.html">PinholeCamera<Cal3_S2></a> camera(pose, *<a class="code hl_variable" href="a05280.html#a02c0d546827eaf8efd7cb7447b0e37b1">K_</a>);</div>
<div class="line"><a id="l00096" name="l00096"></a><span class="lineno"> 96</span> <span class="keywordflow">return</span> camera.project(world_P_landmark) - <a class="code hl_variable" href="a05280.html#af936b4b128aac0b1cad29a8ad021dd96">measured_</a>;</div>
<div class="line"><a id="l00097" name="l00097"></a><span class="lineno"> 97</span> } <span class="keywordflow">catch</span>( CheiralityException& e) {</div>
<div class="line"><a id="l00098" name="l00098"></a><span class="lineno"> 98</span> std::cout << e.what()</div>
<div class="line"><a id="l00099" name="l00099"></a><span class="lineno"> 99</span> << <span class="stringliteral">": Inverse Depth Landmark ["</span> << DefaultKeyFormatter(this->key2()) << <span class="stringliteral">"]"</span></div>
<div class="line"><a id="l00100" name="l00100"></a><span class="lineno"> 100</span> << <span class="stringliteral">" moved behind camera ["</span> << DefaultKeyFormatter(this-><a class="code hl_function" href="a04456.html#a6fb96c2753a88e1e214b450cfda19e21">key1</a>()) <<<span class="stringliteral">"]"</span></div>
<div class="line"><a id="l00101" name="l00101"></a><span class="lineno"> 101</span> << std::endl;</div>
<div class="line"><a id="l00102" name="l00102"></a><span class="lineno"> 102</span> <span class="keywordflow">return</span> Vector::Ones(2) * 2.0 * <a class="code hl_variable" href="a05280.html#a02c0d546827eaf8efd7cb7447b0e37b1">K_</a>->fx();</div>
<div class="line"><a id="l00103" name="l00103"></a><span class="lineno"> 103</span> }</div>
<div class="line"><a id="l00104" name="l00104"></a><span class="lineno"> 104</span> <span class="keywordflow">return</span> (Vector(1) << 0.0).finished();</div>
<div class="line"><a id="l00105" name="l00105"></a><span class="lineno"> 105</span> }</div>
<div class="line"><a id="l00106" name="l00106"></a><span class="lineno"> 106</span> </div>
<div class="line"><a id="l00108" name="l00108"></a><span class="lineno"><a class="line" href="a05280.html#ae55671a9b4d761551279932faf94f884"> 108</a></span> Vector <a class="code hl_function" href="a05280.html#ae55671a9b4d761551279932faf94f884">evaluateError</a>(<span class="keyword">const</span> <a class="code hl_class" href="a03288.html">Pose3</a>& pose, <span class="keyword">const</span> Vector3& landmark,</div>
<div class="line"><a id="l00109" name="l00109"></a><span class="lineno"> 109</span> boost::optional<Matrix&> H1=boost::none,</div>
<div class="line"><a id="l00110" name="l00110"></a><span class="lineno"> 110</span> boost::optional<Matrix&> H2=boost::none)<span class="keyword"> const override </span>{</div>
<div class="line"><a id="l00111" name="l00111"></a><span class="lineno"> 111</span> </div>
<div class="line"><a id="l00112" name="l00112"></a><span class="lineno"> 112</span> <span class="keywordflow">if</span> (H1) {</div>
<div class="line"><a id="l00113" name="l00113"></a><span class="lineno"> 113</span> (*H1) = numericalDerivative11<Vector, Pose3>(</div>
<div class="line"><a id="l00114" name="l00114"></a><span class="lineno"> 114</span> std::bind(&InvDepthFactorVariant2::inverseDepthError, <span class="keyword">this</span>,</div>
<div class="line"><a id="l00115" name="l00115"></a><span class="lineno"> 115</span> std::placeholders::_1, landmark), pose);</div>
<div class="line"><a id="l00116" name="l00116"></a><span class="lineno"> 116</span> }</div>
<div class="line"><a id="l00117" name="l00117"></a><span class="lineno"> 117</span> <span class="keywordflow">if</span> (H2) {</div>
<div class="line"><a id="l00118" name="l00118"></a><span class="lineno"> 118</span> (*H2) = numericalDerivative11<Vector, Vector3>(</div>
<div class="line"><a id="l00119" name="l00119"></a><span class="lineno"> 119</span> std::bind(&InvDepthFactorVariant2::inverseDepthError, <span class="keyword">this</span>, pose,</div>
<div class="line"><a id="l00120" name="l00120"></a><span class="lineno"> 120</span> std::placeholders::_1), landmark);</div>
<div class="line"><a id="l00121" name="l00121"></a><span class="lineno"> 121</span> }</div>
<div class="line"><a id="l00122" name="l00122"></a><span class="lineno"> 122</span> </div>
<div class="line"><a id="l00123" name="l00123"></a><span class="lineno"> 123</span> <span class="keywordflow">return</span> inverseDepthError(pose, landmark);</div>
<div class="line"><a id="l00124" name="l00124"></a><span class="lineno"> 124</span> }</div>
<div class="line"><a id="l00125" name="l00125"></a><span class="lineno"> 125</span> </div>
<div class="line"><a id="l00127" name="l00127"></a><span class="lineno"><a class="line" href="a05280.html#a051afbabd8429b95e2f87f56e14fcc3d"> 127</a></span> <span class="keyword">const</span> <a class="code hl_typedef" href="a01596.html#a6ede8384dee0353a0ce5fb54ea50c21d">Point2</a>& <a class="code hl_function" href="a05280.html#a051afbabd8429b95e2f87f56e14fcc3d">imagePoint</a>()<span class="keyword"> const </span>{</div>
<div class="line"><a id="l00128" name="l00128"></a><span class="lineno"> 128</span> <span class="keywordflow">return</span> <a class="code hl_variable" href="a05280.html#af936b4b128aac0b1cad29a8ad021dd96">measured_</a>;</div>
<div class="line"><a id="l00129" name="l00129"></a><span class="lineno"> 129</span> }</div>
<div class="line"><a id="l00130" name="l00130"></a><span class="lineno"> 130</span> </div>
<div class="line"><a id="l00132" name="l00132"></a><span class="lineno"><a class="line" href="a05280.html#a768c00d6eec5a1241b49f8095c8cc673"> 132</a></span> <span class="keyword">const</span> Cal3_S2::shared_ptr <a class="code hl_function" href="a05280.html#a768c00d6eec5a1241b49f8095c8cc673">calibration</a>()<span class="keyword"> const </span>{</div>
<div class="line"><a id="l00133" name="l00133"></a><span class="lineno"> 133</span> <span class="keywordflow">return</span> <a class="code hl_variable" href="a05280.html#a02c0d546827eaf8efd7cb7447b0e37b1">K_</a>;</div>
<div class="line"><a id="l00134" name="l00134"></a><span class="lineno"> 134</span> }</div>
<div class="line"><a id="l00135" name="l00135"></a><span class="lineno"> 135</span> </div>
<div class="line"><a id="l00137" name="l00137"></a><span class="lineno"><a class="line" href="a05280.html#a99e83c3e07540b08e4138201532504ee"> 137</a></span> <span class="keyword">const</span> <a class="code hl_typedef" href="a01596.html#aaa8ed89fd60ea4601d9de63c4811525b">Point3</a>& <a class="code hl_function" href="a05280.html#a99e83c3e07540b08e4138201532504ee">referencePoint</a>()<span class="keyword"> const </span>{</div>
<div class="line"><a id="l00138" name="l00138"></a><span class="lineno"> 138</span> <span class="keywordflow">return</span> <a class="code hl_variable" href="a05280.html#a1751ebcce9070692b0323a99e500b5c3">referencePoint_</a>;</div>
<div class="line"><a id="l00139" name="l00139"></a><span class="lineno"> 139</span> }</div>
<div class="line"><a id="l00140" name="l00140"></a><span class="lineno"> 140</span> </div>
<div class="line"><a id="l00141" name="l00141"></a><span class="lineno"> 141</span><span class="keyword">private</span>:</div>
<div class="line"><a id="l00142" name="l00142"></a><span class="lineno"> 142</span> </div>
<div class="line"><a id="l00144" name="l00144"></a><span class="lineno"><a class="line" href="a05280.html#ac98d07dd8f7b70e16ccb9a01abf56b9c"> 144</a></span> <span class="keyword">friend</span> <span class="keyword">class </span><a class="code hl_friend" href="a05280.html#ac98d07dd8f7b70e16ccb9a01abf56b9c">boost::serialization::access</a>;</div>
<div class="line"><a id="l00145" name="l00145"></a><span class="lineno"> 145</span> <span class="keyword">template</span><<span class="keyword">class</span> ARCHIVE></div>
<div class="line"><a id="l00146" name="l00146"></a><span class="lineno"> 146</span> <span class="keywordtype">void</span> serialize(ARCHIVE & ar, <span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> <span class="comment">/*version*/</span>) {</div>
<div class="line"><a id="l00147" name="l00147"></a><span class="lineno"> 147</span> ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(<a class="code hl_class" href="a03544.html">Base</a>);</div>
<div class="line"><a id="l00148" name="l00148"></a><span class="lineno"> 148</span> ar & BOOST_SERIALIZATION_NVP(<a class="code hl_variable" href="a05280.html#af936b4b128aac0b1cad29a8ad021dd96">measured_</a>);</div>
<div class="line"><a id="l00149" name="l00149"></a><span class="lineno"> 149</span> ar & BOOST_SERIALIZATION_NVP(<a class="code hl_variable" href="a05280.html#a02c0d546827eaf8efd7cb7447b0e37b1">K_</a>);</div>
<div class="line"><a id="l00150" name="l00150"></a><span class="lineno"> 150</span> ar & BOOST_SERIALIZATION_NVP(<a class="code hl_variable" href="a05280.html#a1751ebcce9070692b0323a99e500b5c3">referencePoint_</a>);</div>
<div class="line"><a id="l00151" name="l00151"></a><span class="lineno"> 151</span> }</div>
<div class="line"><a id="l00152" name="l00152"></a><span class="lineno"> 152</span>};</div>
<div class="line"><a id="l00153" name="l00153"></a><span class="lineno"> 153</span> </div>
<div class="line"><a id="l00154" name="l00154"></a><span class="lineno"> 154</span>} <span class="comment">// \ namespace gtsam</span></div>
<div class="ttc" id="aa00125_html"><div class="ttname"><a href="a00125.html">numericalDerivative.h</a></div><div class="ttdoc">Some functions to compute numerical derivatives.</div></div>
<div class="ttc" id="aa00266_html"><div class="ttname"><a href="a00266.html">Cal3_S2.h</a></div><div class="ttdoc">The most common 5DOF 3D->2D calibration.</div></div>
<div class="ttc" id="aa00314_html"><div class="ttname"><a href="a00314.html">PinholeCamera.h</a></div><div class="ttdoc">Base class for all pinhole cameras.</div></div>
<div class="ttc" id="aa00350_html"><div class="ttname"><a href="a00350.html">Pose3.h</a></div><div class="ttdoc">3D Pose</div></div>
<div class="ttc" id="aa00380_html"><div class="ttname"><a href="a00380.html">Point2.h</a></div><div class="ttdoc">2D Point</div></div>
<div class="ttc" id="aa01022_html"><div class="ttname"><a href="a01022.html">NonlinearFactor.h</a></div><div class="ttdoc">Non-linear factor base classes.</div></div>
<div class="ttc" id="aa01596_html"><div class="ttname"><a href="a01596.html">gtsam</a></div><div class="ttdoc">Global functions in a separate testing namespace.</div><div class="ttdef"><b>Definition:</b> chartTesting.h:28</div></div>
<div class="ttc" id="aa01596_html_a6ede8384dee0353a0ce5fb54ea50c21d"><div class="ttname"><a href="a01596.html#a6ede8384dee0353a0ce5fb54ea50c21d">gtsam::Point2</a></div><div class="ttdeci">Vector2 Point2</div><div class="ttdoc">As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...</div><div class="ttdef"><b>Definition:</b> Point2.h:27</div></div>
<div class="ttc" id="aa01596_html_aaa8ed89fd60ea4601d9de63c4811525b"><div class="ttname"><a href="a01596.html#aaa8ed89fd60ea4601d9de63c4811525b">gtsam::Point3</a></div><div class="ttdeci">Vector3 Point3</div><div class="ttdoc">As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...</div><div class="ttdef"><b>Definition:</b> Point3.h:35</div></div>
<div class="ttc" id="aa01596_html_ab6e5a4884342656e0837ef07008ec03f"><div class="ttname"><a href="a01596.html#ab6e5a4884342656e0837ef07008ec03f">gtsam::SharedNoiseModel</a></div><div class="ttdeci">noiseModel::Base::shared_ptr SharedNoiseModel</div><div class="ttdoc">Note, deliberately not in noiseModel namespace.</div><div class="ttdef"><b>Definition:</b> NoiseModel.h:736</div></div>
<div class="ttc" id="aa01596_html_adad029f5f6ffce610428b5fe768b0df2"><div class="ttname"><a href="a01596.html#adad029f5f6ffce610428b5fe768b0df2">gtsam::Key</a></div><div class="ttdeci">std::uint64_t Key</div><div class="ttdoc">Integer nonlinear key type.</div><div class="ttdef"><b>Definition:</b> types.h:69</div></div>
<div class="ttc" id="aa01596_html_ae4b4e8e7f0d745882c6a02b507d5bffe"><div class="ttname"><a href="a01596.html#ae4b4e8e7f0d745882c6a02b507d5bffe">gtsam::KeyFormatter</a></div><div class="ttdeci">std::function< std::string(Key)> KeyFormatter</div><div class="ttdoc">Typedef for a function to format a key, i.e. to convert it to a string.</div><div class="ttdef"><b>Definition:</b> Key.h:35</div></div>
<div class="ttc" id="aa02444_html"><div class="ttname"><a href="a02444.html">gtsam::traits</a></div><div class="ttdoc">A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...</div><div class="ttdef"><b>Definition:</b> concepts.h:30</div></div>
<div class="ttc" id="aa03052_html"><div class="ttname"><a href="a03052.html">gtsam::Cal3_S2</a></div><div class="ttdef"><b>Definition:</b> Cal3_S2.h:34</div></div>
<div class="ttc" id="aa03212_html"><div class="ttname"><a href="a03212.html">gtsam::PinholeCamera</a></div><div class="ttdef"><b>Definition:</b> PinholeCamera.h:33</div></div>
<div class="ttc" id="aa03288_html"><div class="ttname"><a href="a03288.html">gtsam::Pose3</a></div><div class="ttdef"><b>Definition:</b> Pose3.h:37</div></div>
<div class="ttc" id="aa03544_html"><div class="ttname"><a href="a03544.html">gtsam::Factor</a></div><div class="ttdoc">This is the base class for all factor types.</div><div class="ttdef"><b>Definition:</b> Factor.h:56</div></div>
<div class="ttc" id="aa04440_html"><div class="ttname"><a href="a04440.html">gtsam::NonlinearFactor</a></div><div class="ttdoc">Nonlinear factor base class.</div><div class="ttdef"><b>Definition:</b> NonlinearFactor.h:43</div></div>
<div class="ttc" id="aa04448_html_a0cb99e64e817f707a5bd5ed2e951af2e"><div class="ttname"><a href="a04448.html#a0cb99e64e817f707a5bd5ed2e951af2e">gtsam::NoiseModelFactor::equals</a></div><div class="ttdeci">bool equals(const NonlinearFactor &f, double tol=1e-9) const override</div><div class="ttdoc">Check if two factors are equal.</div><div class="ttdef"><b>Definition:</b> NonlinearFactor.cpp:71</div></div>
<div class="ttc" id="aa04448_html_ab45b9c35a90eca2777f32dab550d1569"><div class="ttname"><a href="a04448.html#ab45b9c35a90eca2777f32dab550d1569">gtsam::NoiseModelFactor::print</a></div><div class="ttdeci">void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override</div><div class="ttdoc">Print.</div><div class="ttdef"><b>Definition:</b> NonlinearFactor.cpp:63</div></div>
<div class="ttc" id="aa04456_html"><div class="ttname"><a href="a04456.html">gtsam::NoiseModelFactor2</a></div><div class="ttdoc">A convenient base class for creating your own NoiseModelFactor with 2 variables.</div><div class="ttdef"><b>Definition:</b> NonlinearFactor.h:369</div></div>
<div class="ttc" id="aa04456_html_a6fb96c2753a88e1e214b450cfda19e21"><div class="ttname"><a href="a04456.html#a6fb96c2753a88e1e214b450cfda19e21">gtsam::NoiseModelFactor2< Pose3, Vector3 >::key1</a></div><div class="ttdeci">Key key1() const</div><div class="ttdoc">methods to retrieve both keys</div><div class="ttdef"><b>Definition:</b> NonlinearFactor.h:401</div></div>
<div class="ttc" id="aa05280_html"><div class="ttname"><a href="a05280.html">gtsam::InvDepthFactorVariant2</a></div><div class="ttdoc">Binary factor representing a visual measurement using an inverse-depth parameterization.</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:28</div></div>
<div class="ttc" id="aa05280_html_a02c0d546827eaf8efd7cb7447b0e37b1"><div class="ttname"><a href="a05280.html#a02c0d546827eaf8efd7cb7447b0e37b1">gtsam::InvDepthFactorVariant2::K_</a></div><div class="ttdeci">Cal3_S2::shared_ptr K_</div><div class="ttdoc">shared pointer to calibration object</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:33</div></div>
<div class="ttc" id="aa05280_html_a051afbabd8429b95e2f87f56e14fcc3d"><div class="ttname"><a href="a05280.html#a051afbabd8429b95e2f87f56e14fcc3d">gtsam::InvDepthFactorVariant2::imagePoint</a></div><div class="ttdeci">const Point2 & imagePoint() const</div><div class="ttdoc">return the measurement</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:127</div></div>
<div class="ttc" id="aa05280_html_a1751ebcce9070692b0323a99e500b5c3"><div class="ttname"><a href="a05280.html#a1751ebcce9070692b0323a99e500b5c3">gtsam::InvDepthFactorVariant2::referencePoint_</a></div><div class="ttdeci">Point3 referencePoint_</div><div class="ttdoc">the reference point/origin for this landmark</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:34</div></div>
<div class="ttc" id="aa05280_html_a2a1562da3696397fcbdfc15b5ea37029"><div class="ttname"><a href="a05280.html#a2a1562da3696397fcbdfc15b5ea37029">gtsam::InvDepthFactorVariant2::InvDepthFactorVariant2</a></div><div class="ttdeci">InvDepthFactorVariant2()</div><div class="ttdoc">Default constructor.</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:48</div></div>
<div class="ttc" id="aa05280_html_a399e49735e88e4900188a60bfc74d7a1"><div class="ttname"><a href="a05280.html#a399e49735e88e4900188a60bfc74d7a1">gtsam::InvDepthFactorVariant2::equals</a></div><div class="ttdeci">bool equals(const NonlinearFactor &p, double tol=1e-9) const override</div><div class="ttdoc">equals</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:80</div></div>
<div class="ttc" id="aa05280_html_a4f7c7c86906838768800ddb8e0d55556"><div class="ttname"><a href="a05280.html#a4f7c7c86906838768800ddb8e0d55556">gtsam::InvDepthFactorVariant2::~InvDepthFactorVariant2</a></div><div class="ttdeci">~InvDepthFactorVariant2() override</div><div class="ttdoc">Virtual destructor.</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:66</div></div>
<div class="ttc" id="aa05280_html_a768c00d6eec5a1241b49f8095c8cc673"><div class="ttname"><a href="a05280.html#a768c00d6eec5a1241b49f8095c8cc673">gtsam::InvDepthFactorVariant2::calibration</a></div><div class="ttdeci">const Cal3_S2::shared_ptr calibration() const</div><div class="ttdoc">return the calibration object</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:132</div></div>
<div class="ttc" id="aa05280_html_a99e83c3e07540b08e4138201532504ee"><div class="ttname"><a href="a05280.html#a99e83c3e07540b08e4138201532504ee">gtsam::InvDepthFactorVariant2::referencePoint</a></div><div class="ttdeci">const Point3 & referencePoint() const</div><div class="ttdoc">return the calibration object</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:137</div></div>
<div class="ttc" id="aa05280_html_a9bc66090d5477a029e92d339ffd9fbe6"><div class="ttname"><a href="a05280.html#a9bc66090d5477a029e92d339ffd9fbe6">gtsam::InvDepthFactorVariant2::print</a></div><div class="ttdeci">void print(const std::string &s="InvDepthFactorVariant2", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override</div><div class="ttdoc">print</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:73</div></div>
<div class="ttc" id="aa05280_html_aa1de0753a1897687a3376956a56a21c9"><div class="ttname"><a href="a05280.html#aa1de0753a1897687a3376956a56a21c9">gtsam::InvDepthFactorVariant2::shared_ptr</a></div><div class="ttdeci">boost::shared_ptr< This > shared_ptr</div><div class="ttdoc">shorthand for a smart pointer to a factor</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:45</div></div>
<div class="ttc" id="aa05280_html_ac98d07dd8f7b70e16ccb9a01abf56b9c"><div class="ttname"><a href="a05280.html#ac98d07dd8f7b70e16ccb9a01abf56b9c">gtsam::InvDepthFactorVariant2::access</a></div><div class="ttdeci">friend class boost::serialization::access</div><div class="ttdoc">Serialization function.</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:144</div></div>
<div class="ttc" id="aa05280_html_addb66abec98197833c991df96ccdbd48"><div class="ttname"><a href="a05280.html#addb66abec98197833c991df96ccdbd48">gtsam::InvDepthFactorVariant2::Base</a></div><div class="ttdeci">NoiseModelFactor2< Pose3, Vector3 > Base</div><div class="ttdoc">shorthand for base class type</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:39</div></div>
<div class="ttc" id="aa05280_html_ae55671a9b4d761551279932faf94f884"><div class="ttname"><a href="a05280.html#ae55671a9b4d761551279932faf94f884">gtsam::InvDepthFactorVariant2::evaluateError</a></div><div class="ttdeci">Vector evaluateError(const Pose3 &pose, const Vector3 &landmark, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override</div><div class="ttdoc">Evaluate error h(x)-z and optionally derivatives.</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:108</div></div>
<div class="ttc" id="aa05280_html_ae845ae5cd3dcb960941edd2e29fdd643"><div class="ttname"><a href="a05280.html#ae845ae5cd3dcb960941edd2e29fdd643">gtsam::InvDepthFactorVariant2::InvDepthFactorVariant2</a></div><div class="ttdeci">InvDepthFactorVariant2(const Key poseKey, const Key landmarkKey, const Point2 &measured, const Cal3_S2::shared_ptr &K, const Point3 referencePoint, const SharedNoiseModel &model)</div><div class="ttdoc">Constructor.</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:60</div></div>
<div class="ttc" id="aa05280_html_af936b4b128aac0b1cad29a8ad021dd96"><div class="ttname"><a href="a05280.html#af936b4b128aac0b1cad29a8ad021dd96">gtsam::InvDepthFactorVariant2::measured_</a></div><div class="ttdeci">Point2 measured_</div><div class="ttdoc">2D measurement</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:32</div></div>
<div class="ttc" id="aa05280_html_af99a8bcbe5cfe559724ec2d02df4d072"><div class="ttname"><a href="a05280.html#af99a8bcbe5cfe559724ec2d02df4d072">gtsam::InvDepthFactorVariant2::This</a></div><div class="ttdeci">InvDepthFactorVariant2 This</div><div class="ttdoc">shorthand for this class</div><div class="ttdef"><b>Definition:</b> InvDepthFactorVariant2.h:42</div></div>
</div><!-- fragment --></div><!-- contents -->
</div><!-- doc-content -->
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
<ul>
<li class="navelem"><a class="el" href="dir_27146aeee5e094ad2a49b6c39cc56ecc.html">gtsam_unstable</a></li><li class="navelem"><a class="el" href="dir_cfb0833a2d0a882859966d02dbcbbb30.html">slam</a></li><li class="navelem"><a class="el" href="a01454.html">InvDepthFactorVariant2.h</a></li>
<li class="footer">Generated on Tue Jan 25 2022 13:36:44 for gtsam by <a href="https://www.doxygen.org/index.html"><img class="footer" src="doxygen.svg" width="104" height="31" alt="doxygen"/></a> 1.9.3 </li>
</ul>
</div>
</body>
</html>