mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-12 12:59:51 +08:00
Preview for PR #1791 b8b218f764
This commit is contained in:
github-actions[bot]
@@ -143,97 +143,125 @@ $(document).ready(function(){initNavTree('ImuMotionPredictor_8h_source.html','')
|
||||
<div class="line"><a id="l00035" name="l00035"></a><span class="lineno"> 35</span><span class="preprocessor">#include <Eigen/Geometry></span></div>
|
||||
<div class="line"><a id="l00036" name="l00036"></a><span class="lineno"> 36</span> </div>
|
||||
<div class="line"><a id="l00037" name="l00037"></a><span class="lineno"> 37</span><span class="preprocessor">#include <map></span></div>
|
||||
<div class="line"><a id="l00038" name="l00038"></a><span class="lineno"> 38</span> </div>
|
||||
<div class="line"><a id="l00039" name="l00039"></a><span class="lineno"> 39</span><span class="keyword">namespace </span><a class="code hl_namespace" href="namespacertabmap.html">rtabmap</a> {</div>
|
||||
<div class="line"><a id="l00040" name="l00040"></a><span class="lineno"> 40</span> </div>
|
||||
<div class="foldopen" id="foldopen00065" data-start="{" data-end="};">
|
||||
<div class="line"><a id="l00065" name="l00065"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html"> 65</a></span><span class="keyword">class </span>RTABMAP_CORE_EXPORT <a class="code hl_class" href="classrtabmap_1_1ImuMotionPredictor.html">ImuMotionPredictor</a></div>
|
||||
<div class="line"><a id="l00066" name="l00066"></a><span class="lineno"> 66</span>{</div>
|
||||
<div class="line"><a id="l00067" name="l00067"></a><span class="lineno"> 67</span><span class="keyword">public</span>:</div>
|
||||
<div class="line"><a id="l00089" name="l00089"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a765b9d6ae74d6fdcfbcc55e9d28b3feb"> 89</a></span> <span class="keyword">explicit</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a765b9d6ae74d6fdcfbcc55e9d28b3feb">ImuMotionPredictor</a>(</div>
|
||||
<div class="line"><a id="l00090" name="l00090"></a><span class="lineno"> 90</span> <span class="keywordtype">double</span> maxPoseInterval = 1.0,</div>
|
||||
<div class="line"><a id="l00091" name="l00091"></a><span class="lineno"> 91</span> <span class="keywordtype">double</span> velocityWindow = 0.5,</div>
|
||||
<div class="line"><a id="l00092" name="l00092"></a><span class="lineno"> 92</span> <span class="keywordtype">double</span> gravity = 9.80665);</div>
|
||||
<div class="line"><a id="l00093" name="l00093"></a><span class="lineno"> 93</span> </div>
|
||||
<div class="line"><a id="l00094" name="l00094"></a><span class="lineno"> 94</span> <span class="keywordtype">double</span> maxPoseInterval()<span class="keyword"> const </span>{<span class="keywordflow">return</span> maxPoseInterval_;}</div>
|
||||
<div class="line"><a id="l00095" name="l00095"></a><span class="lineno"> 95</span> <span class="keywordtype">double</span> velocityWindow()<span class="keyword"> const </span>{<span class="keywordflow">return</span> velocityWindow_;}</div>
|
||||
<div class="line"><a id="l00096" name="l00096"></a><span class="lineno"> 96</span> <span class="keywordtype">double</span> gravity()<span class="keyword"> const </span>{<span class="keywordflow">return</span> gravity_;}</div>
|
||||
<div class="line"><a id="l00097" name="l00097"></a><span class="lineno"> 97</span> </div>
|
||||
<div class="line"><a id="l00108" name="l00108"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a3359e2caff4d42fbcd4137b39aa2ae04"> 108</a></span> <span class="keywordtype">void</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a3359e2caff4d42fbcd4137b39aa2ae04">addImu</a>(<span class="keywordtype">double</span> stamp, <span class="keyword">const</span> <a class="code hl_class" href="classrtabmap_1_1IMU.html">IMU</a> & imu);</div>
|
||||
<div class="line"><a id="l00109" name="l00109"></a><span class="lineno"> 109</span> </div>
|
||||
<div class="line"><a id="l00116" name="l00116"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#abb4349e5902040e5440f425e0bc39c47"> 116</a></span> <span class="keywordtype">void</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#abb4349e5902040e5440f425e0bc39c47">addPose</a>(<span class="keywordtype">double</span> stamp, <span class="keyword">const</span> <a class="code hl_class" href="classrtabmap_1_1Transform.html">rtabmap::Transform</a> & pose);</div>
|
||||
<div class="line"><a id="l00117" name="l00117"></a><span class="lineno"> 117</span> </div>
|
||||
<div class="line"><a id="l00119" name="l00119"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a2b8b4cd814ae31ef46899f41992bb1a4"> 119</a></span> <span class="keywordtype">void</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a2b8b4cd814ae31ef46899f41992bb1a4">reset</a>();</div>
|
||||
<div class="line"><a id="l00120" name="l00120"></a><span class="lineno"> 120</span> </div>
|
||||
<div class="line"><a id="l00129" name="l00129"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#ac9bd37d201be18aaecfbfdbdbaec6ff0"> 129</a></span> <a class="code hl_class" href="classrtabmap_1_1Transform.html">rtabmap::Transform</a> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#ac9bd37d201be18aaecfbfdbdbaec6ff0">predict</a>(<span class="keywordtype">double</span> stamp) <span class="keyword">const</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="classrtabmap_1_1ImuMotionPredictor.html#a2c30a766cc8f251b78ed490cb2a275eb"> 132</a></span> <span class="keywordtype">bool</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a2c30a766cc8f251b78ed490cb2a275eb">hasPose</a>() <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00134" name="l00134"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a4ddd3b124501b8446869a38b1ab1ff95"> 134</a></span> <span class="keywordtype">double</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a4ddd3b124501b8446869a38b1ab1ff95">lastPoseStamp</a>() <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00136" name="l00136"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a18e326faaa67a3602eaf3b4df7a906ff"> 136</a></span> Eigen::Vector3d <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a18e326faaa67a3602eaf3b4df7a906ff">velocity</a>() <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00138" name="l00138"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a39dc460db30550b24646b312b3329245"> 138</a></span> <span class="keywordtype">size_t</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a39dc460db30550b24646b312b3329245">samples</a>() <span class="keyword">const</span>;</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><span class="keyword">private</span>:</div>
|
||||
<div class="line"><a id="l00141" name="l00141"></a><span class="lineno"> 141</span> <span class="comment">// orientation: of the base frame in the IMU's world frame; acceleration: of the base in</span></div>
|
||||
<div class="line"><a id="l00142" name="l00142"></a><span class="lineno"> 142</span> <span class="comment">// that frame, gravity removed</span></div>
|
||||
<div class="line"><a id="l00143" name="l00143"></a><span class="lineno"> 143</span> <span class="keywordtype">void</span> addSample(<span class="keywordtype">double</span> stamp, <span class="keyword">const</span> Eigen::Quaterniond & orientation, <span class="keyword">const</span> Eigen::Vector3d & acceleration);</div>
|
||||
<div class="line"><a id="l00144" name="l00144"></a><span class="lineno"> 144</span> </div>
|
||||
<div class="line"><a id="l00145" name="l00145"></a><span class="lineno"> 145</span> <span class="keyword">struct </span>Sample</div>
|
||||
<div class="line"><a id="l00146" name="l00146"></a><span class="lineno"> 146</span> {</div>
|
||||
<div class="line"><a id="l00147" name="l00147"></a><span class="lineno"> 147</span> Eigen::Quaterniond orientation;</div>
|
||||
<div class="line"><a id="l00148" name="l00148"></a><span class="lineno"> 148</span> Eigen::Vector3d acceleration;</div>
|
||||
<div class="line"><a id="l00149" name="l00149"></a><span class="lineno"> 149</span> };</div>
|
||||
<div class="line"><a id="l00150" name="l00150"></a><span class="lineno"> 150</span> Eigen::Quaterniond orientationAt(<span class="keywordtype">double</span> stamp) <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00151" name="l00151"></a><span class="lineno"> 151</span> Eigen::Vector3d accelerationAt(<span class="keywordtype">double</span> stamp) <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00152" name="l00152"></a><span class="lineno"> 152</span> <span class="keywordtype">void</span> integrate(<span class="keywordtype">double</span> from, <span class="keywordtype">double</span> to, <span class="keyword">const</span> Eigen::Quaterniond & rotation,</div>
|
||||
<div class="line"><a id="l00153" name="l00153"></a><span class="lineno"> 153</span> Eigen::Vector3d & velocity, Eigen::Vector3d & position) <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00154" name="l00154"></a><span class="lineno"> 154</span> <span class="keywordtype">void</span> updateIntegration() <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00155" name="l00155"></a><span class="lineno"> 155</span> </div>
|
||||
<div class="line"><a id="l00156" name="l00156"></a><span class="lineno"> 156</span> <span class="comment">// The acceleration integrated from the last pose up to a sample's stamp, in the</span></div>
|
||||
<div class="line"><a id="l00157" name="l00157"></a><span class="lineno"> 157</span> <span class="comment">// odometry frame: predicting a stamp then only integrates from the sample before it.</span></div>
|
||||
<div class="line"><a id="l00158" name="l00158"></a><span class="lineno"> 158</span> <span class="keyword">struct </span>Integrated</div>
|
||||
<div class="line"><a id="l00159" name="l00159"></a><span class="lineno"> 159</span> {</div>
|
||||
<div class="line"><a id="l00160" name="l00160"></a><span class="lineno"> 160</span> Eigen::Vector3d acceleration; <span class="comment">// at that stamp</span></div>
|
||||
<div class="line"><a id="l00161" name="l00161"></a><span class="lineno"> 161</span> Eigen::Vector3d velocity; <span class="comment">// change since the last pose</span></div>
|
||||
<div class="line"><a id="l00162" name="l00162"></a><span class="lineno"> 162</span> Eigen::Vector3d position; <span class="comment">// change since the last pose (without its velocity)</span></div>
|
||||
<div class="line"><a id="l00163" name="l00163"></a><span class="lineno"> 163</span> };</div>
|
||||
<div class="line"><a id="l00164" name="l00164"></a><span class="lineno"> 164</span> </div>
|
||||
<div class="line"><a id="l00165" name="l00165"></a><span class="lineno"> 165</span><span class="keyword">private</span>:</div>
|
||||
<div class="line"><a id="l00166" name="l00166"></a><span class="lineno"> 166</span> <span class="keywordtype">double</span> maxPoseInterval_;</div>
|
||||
<div class="line"><a id="l00167" name="l00167"></a><span class="lineno"> 167</span> <span class="keywordtype">double</span> velocityWindow_;</div>
|
||||
<div class="line"><a id="l00168" name="l00168"></a><span class="lineno"> 168</span> <span class="keywordtype">double</span> gravity_;</div>
|
||||
<div class="line"><a id="l00169" name="l00169"></a><span class="lineno"> 169</span> std::map<double, Sample> samples_;</div>
|
||||
<div class="line"><a id="l00170" name="l00170"></a><span class="lineno"> 170</span> <span class="comment">// Recent odometry poses, to estimate the velocity from (see velocityWindow)</span></div>
|
||||
<div class="line"><a id="l00171" name="l00171"></a><span class="lineno"> 171</span> std::map<double, rtabmap::Transform> poses_;</div>
|
||||
<div class="line"><a id="l00172" name="l00172"></a><span class="lineno"> 172</span> </div>
|
||||
<div class="line"><a id="l00173" name="l00173"></a><span class="lineno"> 173</span> <span class="comment">// The last odometry pose and the state predictions start from.</span></div>
|
||||
<div class="line"><a id="l00174" name="l00174"></a><span class="lineno"> 174</span> <span class="keywordtype">double</span> poseStamp_;</div>
|
||||
<div class="line"><a id="l00175" name="l00175"></a><span class="lineno"> 175</span> <a class="code hl_class" href="classrtabmap_1_1Transform.html">rtabmap::Transform</a> pose_;</div>
|
||||
<div class="line"><a id="l00176" name="l00176"></a><span class="lineno"> 176</span> Eigen::Vector3d velocity_;</div>
|
||||
<div class="line"><a id="l00177" name="l00177"></a><span class="lineno"> 177</span> <span class="comment">// Rotation from the IMU's world frame to the odometry frame, at the last pose: the two</span></div>
|
||||
<div class="line"><a id="l00178" name="l00178"></a><span class="lineno"> 178</span> <span class="comment">// are both gravity aligned, but their yaw differ.</span></div>
|
||||
<div class="line"><a id="l00179" name="l00179"></a><span class="lineno"> 179</span> Eigen::Quaterniond worldToOdom_;</div>
|
||||
<div class="line"><a id="l00180" name="l00180"></a><span class="lineno"> 180</span> </div>
|
||||
<div class="line"><a id="l00181" name="l00181"></a><span class="lineno"> 181</span> <span class="comment">// Built lazily by predict(), from the last pose to the newest sample; cleared when the</span></div>
|
||||
<div class="line"><a id="l00182" name="l00182"></a><span class="lineno"> 182</span> <span class="comment">// pose or the samples it was built from change.</span></div>
|
||||
<div class="line"><a id="l00183" name="l00183"></a><span class="lineno"> 183</span> <span class="keyword">mutable</span> std::map<double, Integrated> integrated_;</div>
|
||||
<div class="line"><a id="l00184" name="l00184"></a><span class="lineno"> 184</span> <span class="comment">// Whether the acceleration at the last pose's stamp (the first entry) is final: it is</span></div>
|
||||
<div class="line"><a id="l00185" name="l00185"></a><span class="lineno"> 185</span> <span class="comment">// held constant from the newest sample until a sample after that stamp is received.</span></div>
|
||||
<div class="line"><a id="l00186" name="l00186"></a><span class="lineno"> 186</span> <span class="keyword">mutable</span> <span class="keywordtype">bool</span> integratedStartIsFinal_;</div>
|
||||
<div class="line"><a id="l00187" name="l00187"></a><span class="lineno"> 187</span>};</div>
|
||||
<div class="line"><a id="l00038" name="l00038"></a><span class="lineno"> 38</span><span class="preprocessor">#include <vector></span></div>
|
||||
<div class="line"><a id="l00039" name="l00039"></a><span class="lineno"> 39</span> </div>
|
||||
<div class="line"><a id="l00040" name="l00040"></a><span class="lineno"> 40</span><span class="keyword">namespace </span><a class="code hl_namespace" href="namespacertabmap.html">rtabmap</a> {</div>
|
||||
<div class="line"><a id="l00041" name="l00041"></a><span class="lineno"> 41</span> </div>
|
||||
<div class="foldopen" id="foldopen00077" data-start="{" data-end="};">
|
||||
<div class="line"><a id="l00077" name="l00077"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html"> 77</a></span><span class="keyword">class </span>RTABMAP_CORE_EXPORT <a class="code hl_class" href="classrtabmap_1_1ImuMotionPredictor.html">ImuMotionPredictor</a></div>
|
||||
<div class="line"><a id="l00078" name="l00078"></a><span class="lineno"> 78</span>{</div>
|
||||
<div class="line"><a id="l00079" name="l00079"></a><span class="lineno"> 79</span><span class="keyword">public</span>:</div>
|
||||
<div class="line"><a id="l00103" name="l00103"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a765b9d6ae74d6fdcfbcc55e9d28b3feb"> 103</a></span> <span class="keyword">explicit</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a765b9d6ae74d6fdcfbcc55e9d28b3feb">ImuMotionPredictor</a>(</div>
|
||||
<div class="line"><a id="l00104" name="l00104"></a><span class="lineno"> 104</span> <span class="keywordtype">double</span> maxPoseInterval = 1.0,</div>
|
||||
<div class="line"><a id="l00105" name="l00105"></a><span class="lineno"> 105</span> <span class="keywordtype">double</span> velocityWindow = 0.5,</div>
|
||||
<div class="line"><a id="l00106" name="l00106"></a><span class="lineno"> 106</span> <span class="keywordtype">double</span> gravity = 9.80665);</div>
|
||||
<div class="line"><a id="l00107" name="l00107"></a><span class="lineno"> 107</span> </div>
|
||||
<div class="line"><a id="l00108" name="l00108"></a><span class="lineno"> 108</span> <span class="keywordtype">double</span> maxPoseInterval()<span class="keyword"> const </span>{<span class="keywordflow">return</span> maxPoseInterval_;}</div>
|
||||
<div class="line"><a id="l00109" name="l00109"></a><span class="lineno"> 109</span> <span class="keywordtype">double</span> velocityWindow()<span class="keyword"> const </span>{<span class="keywordflow">return</span> velocityWindow_;}</div>
|
||||
<div class="line"><a id="l00111" name="l00111"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a53a73e2ebf60ff671828732594adc7c4"> 111</a></span> <span class="keywordtype">double</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a53a73e2ebf60ff671828732594adc7c4">gravity</a>()<span class="keyword"> const </span>{<span class="keywordflow">return</span> gravity_;}</div>
|
||||
<div class="line"><a id="l00113" name="l00113"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#aa84008cbd4087273e8db74d91d89ec10"> 113</a></span> <span class="keywordtype">bool</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#aa84008cbd4087273e8db74d91d89ec10">isGravityEstimated</a>()<span class="keyword"> const </span>{<span class="keywordflow">return</span> gravityEstimated_;}</div>
|
||||
<div class="line"><a id="l00115" name="l00115"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a91c329f153da4c479509e406f6eb5112"> 115</a></span> <span class="keywordtype">size_t</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a91c329f153da4c479509e406f6eb5112">gravityWindows</a>()<span class="keyword"> const </span>{<span class="keywordflow">return</span> gravityWindowsCount_;}</div>
|
||||
<div class="line"><a id="l00116" name="l00116"></a><span class="lineno"> 116</span> </div>
|
||||
<div class="line"><a id="l00127" name="l00127"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a3359e2caff4d42fbcd4137b39aa2ae04"> 127</a></span> <span class="keywordtype">void</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a3359e2caff4d42fbcd4137b39aa2ae04">addImu</a>(<span class="keywordtype">double</span> stamp, <span class="keyword">const</span> <a class="code hl_class" href="classrtabmap_1_1IMU.html">IMU</a> & imu);</div>
|
||||
<div class="line"><a id="l00128" name="l00128"></a><span class="lineno"> 128</span> </div>
|
||||
<div class="line"><a id="l00135" name="l00135"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#abb4349e5902040e5440f425e0bc39c47"> 135</a></span> <span class="keywordtype">void</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#abb4349e5902040e5440f425e0bc39c47">addPose</a>(<span class="keywordtype">double</span> stamp, <span class="keyword">const</span> <a class="code hl_class" href="classrtabmap_1_1Transform.html">rtabmap::Transform</a> & pose);</div>
|
||||
<div class="line"><a id="l00136" name="l00136"></a><span class="lineno"> 136</span> </div>
|
||||
<div class="line"><a id="l00139" name="l00139"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a2b8b4cd814ae31ef46899f41992bb1a4"> 139</a></span> <span class="keywordtype">void</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a2b8b4cd814ae31ef46899f41992bb1a4">reset</a>();</div>
|
||||
<div class="line"><a id="l00140" name="l00140"></a><span class="lineno"> 140</span> </div>
|
||||
<div class="line"><a id="l00149" name="l00149"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#ac9bd37d201be18aaecfbfdbdbaec6ff0"> 149</a></span> <a class="code hl_class" href="classrtabmap_1_1Transform.html">rtabmap::Transform</a> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#ac9bd37d201be18aaecfbfdbdbaec6ff0">predict</a>(<span class="keywordtype">double</span> stamp) <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00150" name="l00150"></a><span class="lineno"> 150</span> </div>
|
||||
<div class="line"><a id="l00152" name="l00152"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a2c30a766cc8f251b78ed490cb2a275eb"> 152</a></span> <span class="keywordtype">bool</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a2c30a766cc8f251b78ed490cb2a275eb">hasPose</a>() <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00154" name="l00154"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a4ddd3b124501b8446869a38b1ab1ff95"> 154</a></span> <span class="keywordtype">double</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a4ddd3b124501b8446869a38b1ab1ff95">lastPoseStamp</a>() <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00156" name="l00156"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a18e326faaa67a3602eaf3b4df7a906ff"> 156</a></span> Eigen::Vector3d <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a18e326faaa67a3602eaf3b4df7a906ff">velocity</a>() <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00158" name="l00158"></a><span class="lineno"><a class="line" href="classrtabmap_1_1ImuMotionPredictor.html#a39dc460db30550b24646b312b3329245"> 158</a></span> <span class="keywordtype">size_t</span> <a class="code hl_function" href="classrtabmap_1_1ImuMotionPredictor.html#a39dc460db30550b24646b312b3329245">samples</a>() <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00159" name="l00159"></a><span class="lineno"> 159</span> </div>
|
||||
<div class="line"><a id="l00160" name="l00160"></a><span class="lineno"> 160</span><span class="keyword">private</span>:</div>
|
||||
<div class="line"><a id="l00161" name="l00161"></a><span class="lineno"> 161</span> <span class="comment">// orientation: of the base frame in the IMU's world frame; acceleration: of the base in</span></div>
|
||||
<div class="line"><a id="l00162" name="l00162"></a><span class="lineno"> 162</span> <span class="comment">// that frame, gravity removed</span></div>
|
||||
<div class="line"><a id="l00163" name="l00163"></a><span class="lineno"> 163</span> <span class="comment">// specificForce: in that frame too, including the reaction to gravity (see</span></div>
|
||||
<div class="line"><a id="l00164" name="l00164"></a><span class="lineno"> 164</span> <span class="comment">// hasAcceleration)</span></div>
|
||||
<div class="line"><a id="l00165" name="l00165"></a><span class="lineno"> 165</span> <span class="keywordtype">void</span> addSample(<span class="keywordtype">double</span> stamp, <span class="keyword">const</span> Eigen::Quaterniond & orientation,</div>
|
||||
<div class="line"><a id="l00166" name="l00166"></a><span class="lineno"> 166</span> <span class="keyword">const</span> Eigen::Vector3d & specificForce, <span class="keywordtype">bool</span> hasAcceleration);</div>
|
||||
<div class="line"><a id="l00167" name="l00167"></a><span class="lineno"> 167</span> <span class="comment">// Gravity estimation, from the IMU's own specific force and angular velocity norm</span></div>
|
||||
<div class="line"><a id="l00168" name="l00168"></a><span class="lineno"> 168</span> <span class="keywordtype">void</span> estimateGravity(<span class="keywordtype">double</span> stamp, <span class="keyword">const</span> Eigen::Vector3d & specificForce, <span class="keywordtype">double</span> angularVelocity);</div>
|
||||
<div class="line"><a id="l00169" name="l00169"></a><span class="lineno"> 169</span> </div>
|
||||
<div class="line"><a id="l00170" name="l00170"></a><span class="lineno"> 170</span> <span class="keyword">struct </span>Sample</div>
|
||||
<div class="line"><a id="l00171" name="l00171"></a><span class="lineno"> 171</span> {</div>
|
||||
<div class="line"><a id="l00172" name="l00172"></a><span class="lineno"> 172</span> Eigen::Quaterniond orientation;</div>
|
||||
<div class="line"><a id="l00173" name="l00173"></a><span class="lineno"> 173</span> <span class="comment">// Kept with gravity, which is removed when used: the estimate can change</span></div>
|
||||
<div class="line"><a id="l00174" name="l00174"></a><span class="lineno"> 174</span> Eigen::Vector3d specificForce;</div>
|
||||
<div class="line"><a id="l00175" name="l00175"></a><span class="lineno"> 175</span> <span class="keywordtype">bool</span> hasAcceleration;</div>
|
||||
<div class="line"><a id="l00176" name="l00176"></a><span class="lineno"> 176</span> };</div>
|
||||
<div class="line"><a id="l00177" name="l00177"></a><span class="lineno"> 177</span> <span class="comment">// Acceleration of a sample, gravity removed (null without acceleration)</span></div>
|
||||
<div class="line"><a id="l00178" name="l00178"></a><span class="lineno"> 178</span> Eigen::Vector3d accelerationOf(<span class="keyword">const</span> Sample & sample) <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00179" name="l00179"></a><span class="lineno"> 179</span> Eigen::Quaterniond orientationAt(<span class="keywordtype">double</span> stamp) <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00180" name="l00180"></a><span class="lineno"> 180</span> Eigen::Vector3d accelerationAt(<span class="keywordtype">double</span> stamp) <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00181" name="l00181"></a><span class="lineno"> 181</span> <span class="keywordtype">void</span> integrate(<span class="keywordtype">double</span> from, <span class="keywordtype">double</span> to, <span class="keyword">const</span> Eigen::Quaterniond & rotation,</div>
|
||||
<div class="line"><a id="l00182" name="l00182"></a><span class="lineno"> 182</span> Eigen::Vector3d & velocity, Eigen::Vector3d & position) <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00183" name="l00183"></a><span class="lineno"> 183</span> <span class="keywordtype">void</span> updateIntegration() <span class="keyword">const</span>;</div>
|
||||
<div class="line"><a id="l00184" name="l00184"></a><span class="lineno"> 184</span> </div>
|
||||
<div class="line"><a id="l00185" name="l00185"></a><span class="lineno"> 185</span> <span class="comment">// The acceleration integrated from the last pose up to a sample's stamp, in the</span></div>
|
||||
<div class="line"><a id="l00186" name="l00186"></a><span class="lineno"> 186</span> <span class="comment">// odometry frame: predicting a stamp then only integrates from the sample before it.</span></div>
|
||||
<div class="line"><a id="l00187" name="l00187"></a><span class="lineno"> 187</span> <span class="keyword">struct </span>Integrated</div>
|
||||
<div class="line"><a id="l00188" name="l00188"></a><span class="lineno"> 188</span> {</div>
|
||||
<div class="line"><a id="l00189" name="l00189"></a><span class="lineno"> 189</span> Eigen::Vector3d acceleration; <span class="comment">// at that stamp</span></div>
|
||||
<div class="line"><a id="l00190" name="l00190"></a><span class="lineno"> 190</span> Eigen::Vector3d velocity; <span class="comment">// change since the last pose</span></div>
|
||||
<div class="line"><a id="l00191" name="l00191"></a><span class="lineno"> 191</span> Eigen::Vector3d position; <span class="comment">// change since the last pose (without its velocity)</span></div>
|
||||
<div class="line"><a id="l00192" name="l00192"></a><span class="lineno"> 192</span> };</div>
|
||||
<div class="line"><a id="l00193" name="l00193"></a><span class="lineno"> 193</span> </div>
|
||||
<div class="line"><a id="l00194" name="l00194"></a><span class="lineno"> 194</span><span class="keyword">private</span>:</div>
|
||||
<div class="line"><a id="l00195" name="l00195"></a><span class="lineno"> 195</span> <span class="keywordtype">double</span> maxPoseInterval_;</div>
|
||||
<div class="line"><a id="l00196" name="l00196"></a><span class="lineno"> 196</span> <span class="keywordtype">double</span> velocityWindow_;</div>
|
||||
<div class="line"><a id="l00197" name="l00197"></a><span class="lineno"> 197</span> <span class="keywordtype">double</span> gravity_;</div>
|
||||
<div class="line"><a id="l00198" name="l00198"></a><span class="lineno"> 198</span> <span class="keywordtype">bool</span> gravityEstimated_;</div>
|
||||
<div class="line"><a id="l00199" name="l00199"></a><span class="lineno"> 199</span> std::map<double, Sample> samples_;</div>
|
||||
<div class="line"><a id="l00200" name="l00200"></a><span class="lineno"> 200</span> </div>
|
||||
<div class="line"><a id="l00201" name="l00201"></a><span class="lineno"> 201</span> <span class="comment">// Gravity estimation: the samples of the current window, and the gravity measured over</span></div>
|
||||
<div class="line"><a id="l00202" name="l00202"></a><span class="lineno"> 202</span> <span class="comment">// the last quiet windows</span></div>
|
||||
<div class="line"><a id="l00203" name="l00203"></a><span class="lineno"> 203</span> <span class="keyword">struct </span>StillSample</div>
|
||||
<div class="line"><a id="l00204" name="l00204"></a><span class="lineno"> 204</span> {</div>
|
||||
<div class="line"><a id="l00205" name="l00205"></a><span class="lineno"> 205</span> <span class="keywordtype">double</span> stamp;</div>
|
||||
<div class="line"><a id="l00206" name="l00206"></a><span class="lineno"> 206</span> Eigen::Vector3d specificForce; <span class="comment">// in the IMU frame</span></div>
|
||||
<div class="line"><a id="l00207" name="l00207"></a><span class="lineno"> 207</span> <span class="keywordtype">double</span> angularVelocity; <span class="comment">// norm</span></div>
|
||||
<div class="line"><a id="l00208" name="l00208"></a><span class="lineno"> 208</span> };</div>
|
||||
<div class="line"><a id="l00209" name="l00209"></a><span class="lineno"> 209</span> std::vector<StillSample> stillWindow_;</div>
|
||||
<div class="line"><a id="l00210" name="l00210"></a><span class="lineno"> 210</span> std::vector<double> gravityWindows_;</div>
|
||||
<div class="line"><a id="l00211" name="l00211"></a><span class="lineno"> 211</span> <span class="keywordtype">size_t</span> gravityWindowsCount_;</div>
|
||||
<div class="line"><a id="l00212" name="l00212"></a><span class="lineno"> 212</span> <span class="comment">// Recent odometry poses, to estimate the velocity from (see velocityWindow)</span></div>
|
||||
<div class="line"><a id="l00213" name="l00213"></a><span class="lineno"> 213</span> std::map<double, rtabmap::Transform> poses_;</div>
|
||||
<div class="line"><a id="l00214" name="l00214"></a><span class="lineno"> 214</span> </div>
|
||||
<div class="line"><a id="l00215" name="l00215"></a><span class="lineno"> 215</span> <span class="comment">// The last odometry pose and the state predictions start from.</span></div>
|
||||
<div class="line"><a id="l00216" name="l00216"></a><span class="lineno"> 216</span> <span class="keywordtype">double</span> poseStamp_;</div>
|
||||
<div class="line"><a id="l00217" name="l00217"></a><span class="lineno"> 217</span> <a class="code hl_class" href="classrtabmap_1_1Transform.html">rtabmap::Transform</a> pose_;</div>
|
||||
<div class="line"><a id="l00218" name="l00218"></a><span class="lineno"> 218</span> Eigen::Vector3d velocity_;</div>
|
||||
<div class="line"><a id="l00219" name="l00219"></a><span class="lineno"> 219</span> <span class="comment">// Rotation from the IMU's world frame to the odometry frame, at the last pose: the two</span></div>
|
||||
<div class="line"><a id="l00220" name="l00220"></a><span class="lineno"> 220</span> <span class="comment">// are both gravity aligned, but their yaw differ.</span></div>
|
||||
<div class="line"><a id="l00221" name="l00221"></a><span class="lineno"> 221</span> Eigen::Quaterniond worldToOdom_;</div>
|
||||
<div class="line"><a id="l00222" name="l00222"></a><span class="lineno"> 222</span> </div>
|
||||
<div class="line"><a id="l00223" name="l00223"></a><span class="lineno"> 223</span> <span class="comment">// Built lazily by predict(), from the last pose to the newest sample; cleared when the</span></div>
|
||||
<div class="line"><a id="l00224" name="l00224"></a><span class="lineno"> 224</span> <span class="comment">// pose or the samples it was built from change.</span></div>
|
||||
<div class="line"><a id="l00225" name="l00225"></a><span class="lineno"> 225</span> <span class="keyword">mutable</span> std::map<double, Integrated> integrated_;</div>
|
||||
<div class="line"><a id="l00226" name="l00226"></a><span class="lineno"> 226</span> <span class="comment">// Whether the acceleration at the last pose's stamp (the first entry) is final: it is</span></div>
|
||||
<div class="line"><a id="l00227" name="l00227"></a><span class="lineno"> 227</span> <span class="comment">// held constant from the newest sample until a sample after that stamp is received.</span></div>
|
||||
<div class="line"><a id="l00228" name="l00228"></a><span class="lineno"> 228</span> <span class="keyword">mutable</span> <span class="keywordtype">bool</span> integratedStartIsFinal_;</div>
|
||||
<div class="line"><a id="l00229" name="l00229"></a><span class="lineno"> 229</span>};</div>
|
||||
</div>
|
||||
<div class="line"><a id="l00188" name="l00188"></a><span class="lineno"> 188</span> </div>
|
||||
<div class="line"><a id="l00189" name="l00189"></a><span class="lineno"> 189</span>}</div>
|
||||
<div class="line"><a id="l00190" name="l00190"></a><span class="lineno"> 190</span> </div>
|
||||
<div class="line"><a id="l00191" name="l00191"></a><span class="lineno"> 191</span><span class="preprocessor">#endif </span><span class="comment">/* RTABMAP_CORE_IMUMOTIONPREDICTOR_H_ */</span><span class="preprocessor"></span></div>
|
||||
<div class="line"><a id="l00230" name="l00230"></a><span class="lineno"> 230</span> </div>
|
||||
<div class="line"><a id="l00231" name="l00231"></a><span class="lineno"> 231</span>}</div>
|
||||
<div class="line"><a id="l00232" name="l00232"></a><span class="lineno"> 232</span> </div>
|
||||
<div class="line"><a id="l00233" name="l00233"></a><span class="lineno"> 233</span><span class="preprocessor">#endif </span><span class="comment">/* RTABMAP_CORE_IMUMOTIONPREDICTOR_H_ */</span><span class="preprocessor"></span></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1IMU_html"><div class="ttname"><a href="classrtabmap_1_1IMU.html">rtabmap::IMU</a></div><div class="ttdoc">Inertial measurement sample (ROS sensor_msgs/Imu-like fields).</div><div class="ttdef"><b>Definition</b> <a href="IMU_8h_source.html#l00056">IMU.h:57</a></div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html">rtabmap::ImuMotionPredictor</a></div><div class="ttdoc">Predicts the pose of the base frame from the last odometry pose and the IMU.</div><div class="ttdef"><b>Definition</b> <a href="ImuMotionPredictor_8h_source.html#l00065">ImuMotionPredictor.h:66</a></div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html">rtabmap::ImuMotionPredictor</a></div><div class="ttdoc">Predicts the pose of the base frame from the last odometry pose and the IMU.</div><div class="ttdef"><b>Definition</b> <a href="ImuMotionPredictor_8h_source.html#l00077">ImuMotionPredictor.h:78</a></div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html_a18e326faaa67a3602eaf3b4df7a906ff"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html#a18e326faaa67a3602eaf3b4df7a906ff">rtabmap::ImuMotionPredictor::velocity</a></div><div class="ttdeci">Eigen::Vector3d velocity() const</div><div class="ttdoc">Velocity (m/s) of the base in the odometry frame at the last pose.</div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html_a2b8b4cd814ae31ef46899f41992bb1a4"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html#a2b8b4cd814ae31ef46899f41992bb1a4">rtabmap::ImuMotionPredictor::reset</a></div><div class="ttdeci">void reset()</div><div class="ttdoc">Forgets the poses and the IMU samples.</div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html_a2b8b4cd814ae31ef46899f41992bb1a4"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html#a2b8b4cd814ae31ef46899f41992bb1a4">rtabmap::ImuMotionPredictor::reset</a></div><div class="ttdeci">void reset()</div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html_a2c30a766cc8f251b78ed490cb2a275eb"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html#a2c30a766cc8f251b78ed490cb2a275eb">rtabmap::ImuMotionPredictor::hasPose</a></div><div class="ttdeci">bool hasPose() const</div><div class="ttdoc">Whether there is a pose to predict from (none yet, or the last one was null).</div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html_a3359e2caff4d42fbcd4137b39aa2ae04"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html#a3359e2caff4d42fbcd4137b39aa2ae04">rtabmap::ImuMotionPredictor::addImu</a></div><div class="ttdeci">void addImu(double stamp, const IMU &imu)</div><div class="ttdoc">Adds an IMU measurement.</div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html_a39dc460db30550b24646b312b3329245"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html#a39dc460db30550b24646b312b3329245">rtabmap::ImuMotionPredictor::samples</a></div><div class="ttdeci">size_t samples() const</div><div class="ttdoc">Number of IMU samples kept.</div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html_a4ddd3b124501b8446869a38b1ab1ff95"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html#a4ddd3b124501b8446869a38b1ab1ff95">rtabmap::ImuMotionPredictor::lastPoseStamp</a></div><div class="ttdeci">double lastPoseStamp() const</div><div class="ttdoc">Stamp of the last pose added, 0 if there is none.</div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html_a53a73e2ebf60ff671828732594adc7c4"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html#a53a73e2ebf60ff671828732594adc7c4">rtabmap::ImuMotionPredictor::gravity</a></div><div class="ttdeci">double gravity() const</div><div class="ttdoc">Gravity (m/s^2) removed from the specific force: the one given, or the estimate.</div><div class="ttdef"><b>Definition</b> <a href="ImuMotionPredictor_8h_source.html#l00111">ImuMotionPredictor.h:111</a></div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html_a765b9d6ae74d6fdcfbcc55e9d28b3feb"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html#a765b9d6ae74d6fdcfbcc55e9d28b3feb">rtabmap::ImuMotionPredictor::ImuMotionPredictor</a></div><div class="ttdeci">ImuMotionPredictor(double maxPoseInterval=1.0, double velocityWindow=0.5, double gravity=9.80665)</div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html_a91c329f153da4c479509e406f6eb5112"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html#a91c329f153da4c479509e406f6eb5112">rtabmap::ImuMotionPredictor::gravityWindows</a></div><div class="ttdeci">size_t gravityWindows() const</div><div class="ttdoc">Number of quiet IMU windows the gravity estimate is from (0: not estimated yet)</div><div class="ttdef"><b>Definition</b> <a href="ImuMotionPredictor_8h_source.html#l00115">ImuMotionPredictor.h:115</a></div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html_aa84008cbd4087273e8db74d91d89ec10"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html#aa84008cbd4087273e8db74d91d89ec10">rtabmap::ImuMotionPredictor::isGravityEstimated</a></div><div class="ttdeci">bool isGravityEstimated() const</div><div class="ttdoc">Whether the gravity is estimated from the IMU (see the constructor)</div><div class="ttdef"><b>Definition</b> <a href="ImuMotionPredictor_8h_source.html#l00113">ImuMotionPredictor.h:113</a></div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html_abb4349e5902040e5440f425e0bc39c47"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html#abb4349e5902040e5440f425e0bc39c47">rtabmap::ImuMotionPredictor::addPose</a></div><div class="ttdeci">void addPose(double stamp, const rtabmap::Transform &pose)</div><div class="ttdoc">Adds an odometry pose, from which the next poses are predicted.</div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1ImuMotionPredictor_html_ac9bd37d201be18aaecfbfdbdbaec6ff0"><div class="ttname"><a href="classrtabmap_1_1ImuMotionPredictor.html#ac9bd37d201be18aaecfbfdbdbaec6ff0">rtabmap::ImuMotionPredictor::predict</a></div><div class="ttdeci">rtabmap::Transform predict(double stamp) const</div><div class="ttdoc">Predicts the pose of the base frame in the odometry frame.</div></div>
|
||||
<div class="ttc" id="aclassrtabmap_1_1Transform_html"><div class="ttname"><a href="classrtabmap_1_1Transform.html">rtabmap::Transform</a></div><div class="ttdoc">Represents a 3D rigid body transformation (rotation + translation).</div><div class="ttdef"><b>Definition</b> <a href="Transform_8h_source.html#l00052">Transform.h:53</a></div></div>
|
||||
|
||||
Reference in New Issue
Block a user