mirror of https://github.com/koide3/small_gicp.git
171 lines
18 KiB
HTML
171 lines
18 KiB
HTML
<!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=9"/>
|
|
<meta name="generator" content="Doxygen 1.9.1"/>
|
|
<meta name="viewport" content="width=device-width, initial-scale=1"/>
|
|
<title>small_gicp: /home/runner/work/small_gicp/small_gicp/include/small_gicp/util/lie.hpp 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="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>
|
|
<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 style="height: 56px;">
|
|
<td id="projectalign" style="padding-left: 0.5em;">
|
|
<div id="projectname">small_gicp
|
|
</div>
|
|
</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
</div>
|
|
<!-- end header part -->
|
|
<!-- Generated by Doxygen 1.9.1 -->
|
|
<script type="text/javascript">
|
|
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&dn=gpl-2.0.txt GPL-v2 */
|
|
var searchBox = new SearchBox("searchBox", "search",false,'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:cf05388f2679ee054f2beb29a391d25f4e673ac3&dn=gpl-2.0.txt GPL-v2 */
|
|
$(function() {
|
|
initMenu('',true,false,'search.php','Search');
|
|
$(document).ready(function() { init_search(); });
|
|
});
|
|
/* @license-end */</script>
|
|
<div id="main-nav"></div>
|
|
<!-- 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 id="nav-path" class="navpath">
|
|
<ul>
|
|
<li class="navelem"><a class="el" href="dir_d44c64559bbebec7f509842c48db8b23.html">include</a></li><li class="navelem"><a class="el" href="dir_8f93768a68ffc2367e7100c40b4e616a.html">small_gicp</a></li><li class="navelem"><a class="el" href="dir_65c55b74f1fe3d7e17a0effca76d06ab.html">util</a></li> </ul>
|
|
</div>
|
|
</div><!-- top -->
|
|
<div class="header">
|
|
<div class="headertitle">
|
|
<div class="title">lie.hpp</div> </div>
|
|
</div><!--header-->
|
|
<div class="contents">
|
|
<a href="lie_8hpp.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span> <span class="comment">// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide</span></div>
|
|
<div class="line"><a name="l00002"></a><span class="lineno"> 2</span> <span class="comment">// SPDX-License-Identifier: MIT</span></div>
|
|
<div class="line"><a name="l00003"></a><span class="lineno"> 3</span> <span class="preprocessor">#pragma once</span></div>
|
|
<div class="line"><a name="l00004"></a><span class="lineno"> 4</span>  </div>
|
|
<div class="line"><a name="l00005"></a><span class="lineno"> 5</span> <span class="preprocessor">#include <Eigen/Core></span></div>
|
|
<div class="line"><a name="l00006"></a><span class="lineno"> 6</span> <span class="preprocessor">#include <Eigen/Geometry></span></div>
|
|
<div class="line"><a name="l00007"></a><span class="lineno"> 7</span>  </div>
|
|
<div class="line"><a name="l00008"></a><span class="lineno"> 8</span> <span class="keyword">namespace </span><a class="code" href="namespacesmall__gicp.html">small_gicp</a> {</div>
|
|
<div class="line"><a name="l00009"></a><span class="lineno"> 9</span>  </div>
|
|
<div class="line"><a name="l00013"></a><span class="lineno"><a class="line" href="namespacesmall__gicp.html#a5d86f81a31b152060f1b1f72585fdaf5"> 13</a></span> <span class="keyword">inline</span> Eigen::Matrix3d <a class="code" href="namespacesmall__gicp.html#a5d86f81a31b152060f1b1f72585fdaf5">skew</a>(<span class="keyword">const</span> Eigen::Vector3d& x) {</div>
|
|
<div class="line"><a name="l00014"></a><span class="lineno"> 14</span>  Eigen::Matrix3d <a class="code" href="namespacesmall__gicp.html#a5d86f81a31b152060f1b1f72585fdaf5">skew</a> = Eigen::Matrix3d::Zero();</div>
|
|
<div class="line"><a name="l00015"></a><span class="lineno"> 15</span>  <a class="code" href="namespacesmall__gicp.html#a5d86f81a31b152060f1b1f72585fdaf5">skew</a>(0, 1) = -x[2];</div>
|
|
<div class="line"><a name="l00016"></a><span class="lineno"> 16</span>  <a class="code" href="namespacesmall__gicp.html#a5d86f81a31b152060f1b1f72585fdaf5">skew</a>(0, 2) = x[1];</div>
|
|
<div class="line"><a name="l00017"></a><span class="lineno"> 17</span>  <a class="code" href="namespacesmall__gicp.html#a5d86f81a31b152060f1b1f72585fdaf5">skew</a>(1, 0) = x[2];</div>
|
|
<div class="line"><a name="l00018"></a><span class="lineno"> 18</span>  <a class="code" href="namespacesmall__gicp.html#a5d86f81a31b152060f1b1f72585fdaf5">skew</a>(1, 2) = -x[0];</div>
|
|
<div class="line"><a name="l00019"></a><span class="lineno"> 19</span>  <a class="code" href="namespacesmall__gicp.html#a5d86f81a31b152060f1b1f72585fdaf5">skew</a>(2, 0) = -x[1];</div>
|
|
<div class="line"><a name="l00020"></a><span class="lineno"> 20</span>  <a class="code" href="namespacesmall__gicp.html#a5d86f81a31b152060f1b1f72585fdaf5">skew</a>(2, 1) = x[0];</div>
|
|
<div class="line"><a name="l00021"></a><span class="lineno"> 21</span>  </div>
|
|
<div class="line"><a name="l00022"></a><span class="lineno"> 22</span>  <span class="keywordflow">return</span> <a class="code" href="namespacesmall__gicp.html#a5d86f81a31b152060f1b1f72585fdaf5">skew</a>;</div>
|
|
<div class="line"><a name="l00023"></a><span class="lineno"> 23</span> }</div>
|
|
<div class="line"><a name="l00024"></a><span class="lineno"> 24</span>  </div>
|
|
<div class="line"><a name="l00025"></a><span class="lineno"> 25</span> <span class="comment">/*</span></div>
|
|
<div class="line"><a name="l00026"></a><span class="lineno"> 26</span> <span class="comment"> * SO3 expmap code taken from Sophus</span></div>
|
|
<div class="line"><a name="l00027"></a><span class="lineno"> 27</span> <span class="comment"> * https://github.com/strasdat/Sophus/blob/593db47500ea1a2de5f0e6579c86147991509c59/sophus/so3.hpp#L585</span></div>
|
|
<div class="line"><a name="l00028"></a><span class="lineno"> 28</span> <span class="comment"> *</span></div>
|
|
<div class="line"><a name="l00029"></a><span class="lineno"> 29</span> <span class="comment"> * Copyright 2011-2017 Hauke Strasdat</span></div>
|
|
<div class="line"><a name="l00030"></a><span class="lineno"> 30</span> <span class="comment"> * 2012-2017 Steven Lovegrove</span></div>
|
|
<div class="line"><a name="l00031"></a><span class="lineno"> 31</span> <span class="comment"> *</span></div>
|
|
<div class="line"><a name="l00032"></a><span class="lineno"> 32</span> <span class="comment"> * Permission is hereby granted, free of charge, to any person obtaining a copy</span></div>
|
|
<div class="line"><a name="l00033"></a><span class="lineno"> 33</span> <span class="comment"> * of this software and associated documentation files (the "Software"), to</span></div>
|
|
<div class="line"><a name="l00034"></a><span class="lineno"> 34</span> <span class="comment"> * deal in the Software without restriction, including without limitation the</span></div>
|
|
<div class="line"><a name="l00035"></a><span class="lineno"> 35</span> <span class="comment"> * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or</span></div>
|
|
<div class="line"><a name="l00036"></a><span class="lineno"> 36</span> <span class="comment"> * sell copies of the Software, and to permit persons to whom the Software is</span></div>
|
|
<div class="line"><a name="l00037"></a><span class="lineno"> 37</span> <span class="comment"> * furnished to do so, subject to the following conditions:</span></div>
|
|
<div class="line"><a name="l00038"></a><span class="lineno"> 38</span> <span class="comment"> *</span></div>
|
|
<div class="line"><a name="l00039"></a><span class="lineno"> 39</span> <span class="comment"> * The above copyright notice and this permission notice shall be included in</span></div>
|
|
<div class="line"><a name="l00040"></a><span class="lineno"> 40</span> <span class="comment"> * all copies or substantial portions of the Software.</span></div>
|
|
<div class="line"><a name="l00041"></a><span class="lineno"> 41</span> <span class="comment"> *</span></div>
|
|
<div class="line"><a name="l00042"></a><span class="lineno"> 42</span> <span class="comment"> * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR</span></div>
|
|
<div class="line"><a name="l00043"></a><span class="lineno"> 43</span> <span class="comment"> * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,</span></div>
|
|
<div class="line"><a name="l00044"></a><span class="lineno"> 44</span> <span class="comment"> * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE</span></div>
|
|
<div class="line"><a name="l00045"></a><span class="lineno"> 45</span> <span class="comment"> * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER</span></div>
|
|
<div class="line"><a name="l00046"></a><span class="lineno"> 46</span> <span class="comment"> * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING</span></div>
|
|
<div class="line"><a name="l00047"></a><span class="lineno"> 47</span> <span class="comment"> * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS</span></div>
|
|
<div class="line"><a name="l00048"></a><span class="lineno"> 48</span> <span class="comment"> * IN THE SOFTWARE.</span></div>
|
|
<div class="line"><a name="l00049"></a><span class="lineno"> 49</span> <span class="comment"> */</span></div>
|
|
<div class="line"><a name="l00050"></a><span class="lineno"> 50</span>  </div>
|
|
<div class="line"><a name="l00054"></a><span class="lineno"><a class="line" href="namespacesmall__gicp.html#a5c5f249a06b1a660d2e519a9936da3f7"> 54</a></span> <span class="keyword">inline</span> Eigen::Quaterniond <a class="code" href="namespacesmall__gicp.html#a5c5f249a06b1a660d2e519a9936da3f7">so3_exp</a>(<span class="keyword">const</span> Eigen::Vector3d& omega) {</div>
|
|
<div class="line"><a name="l00055"></a><span class="lineno"> 55</span>  <span class="keywordtype">double</span> theta_sq = omega.dot(omega);</div>
|
|
<div class="line"><a name="l00056"></a><span class="lineno"> 56</span>  </div>
|
|
<div class="line"><a name="l00057"></a><span class="lineno"> 57</span>  <span class="keywordtype">double</span> imag_factor;</div>
|
|
<div class="line"><a name="l00058"></a><span class="lineno"> 58</span>  <span class="keywordtype">double</span> real_factor;</div>
|
|
<div class="line"><a name="l00059"></a><span class="lineno"> 59</span>  <span class="keywordflow">if</span> (theta_sq < 1e-10) {</div>
|
|
<div class="line"><a name="l00060"></a><span class="lineno"> 60</span>  <span class="keywordtype">double</span> theta_quad = theta_sq * theta_sq;</div>
|
|
<div class="line"><a name="l00061"></a><span class="lineno"> 61</span>  imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad;</div>
|
|
<div class="line"><a name="l00062"></a><span class="lineno"> 62</span>  real_factor = 1.0 - 1.0 / 8.0 * theta_sq + 1.0 / 384.0 * theta_quad;</div>
|
|
<div class="line"><a name="l00063"></a><span class="lineno"> 63</span>  } <span class="keywordflow">else</span> {</div>
|
|
<div class="line"><a name="l00064"></a><span class="lineno"> 64</span>  <span class="keywordtype">double</span> theta = std::sqrt(theta_sq);</div>
|
|
<div class="line"><a name="l00065"></a><span class="lineno"> 65</span>  <span class="keywordtype">double</span> half_theta = 0.5 * theta;</div>
|
|
<div class="line"><a name="l00066"></a><span class="lineno"> 66</span>  imag_factor = std::sin(half_theta) / theta;</div>
|
|
<div class="line"><a name="l00067"></a><span class="lineno"> 67</span>  real_factor = std::cos(half_theta);</div>
|
|
<div class="line"><a name="l00068"></a><span class="lineno"> 68</span>  }</div>
|
|
<div class="line"><a name="l00069"></a><span class="lineno"> 69</span>  </div>
|
|
<div class="line"><a name="l00070"></a><span class="lineno"> 70</span>  <span class="keywordflow">return</span> Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z());</div>
|
|
<div class="line"><a name="l00071"></a><span class="lineno"> 71</span> }</div>
|
|
<div class="line"><a name="l00072"></a><span class="lineno"> 72</span>  </div>
|
|
<div class="line"><a name="l00073"></a><span class="lineno"> 73</span> <span class="comment">// Rotation-first</span></div>
|
|
<div class="line"><a name="l00077"></a><span class="lineno"><a class="line" href="namespacesmall__gicp.html#a0eae106c70deac5c20205c70c4bdaf3c"> 77</a></span> <span class="comment"></span><span class="keyword">inline</span> Eigen::Isometry3d <a class="code" href="namespacesmall__gicp.html#a0eae106c70deac5c20205c70c4bdaf3c">se3_exp</a>(<span class="keyword">const</span> Eigen::Matrix<double, 6, 1>& a) {</div>
|
|
<div class="line"><a name="l00078"></a><span class="lineno"> 78</span>  <span class="keyword">const</span> Eigen::Vector3d omega = a.head<3>();</div>
|
|
<div class="line"><a name="l00079"></a><span class="lineno"> 79</span>  </div>
|
|
<div class="line"><a name="l00080"></a><span class="lineno"> 80</span>  <span class="keyword">const</span> <span class="keywordtype">double</span> theta_sq = omega.dot(omega);</div>
|
|
<div class="line"><a name="l00081"></a><span class="lineno"> 81</span>  <span class="keyword">const</span> <span class="keywordtype">double</span> theta = std::sqrt(theta_sq);</div>
|
|
<div class="line"><a name="l00082"></a><span class="lineno"> 82</span>  </div>
|
|
<div class="line"><a name="l00083"></a><span class="lineno"> 83</span>  Eigen::Isometry3d se3 = Eigen::Isometry3d::Identity();</div>
|
|
<div class="line"><a name="l00084"></a><span class="lineno"> 84</span>  se3.linear() = <a class="code" href="namespacesmall__gicp.html#a5c5f249a06b1a660d2e519a9936da3f7">so3_exp</a>(omega).toRotationMatrix();</div>
|
|
<div class="line"><a name="l00085"></a><span class="lineno"> 85</span>  </div>
|
|
<div class="line"><a name="l00086"></a><span class="lineno"> 86</span>  <span class="keywordflow">if</span> (theta < 1e-10) {</div>
|
|
<div class="line"><a name="l00087"></a><span class="lineno"> 87</span>  se3.translation() = se3.linear() * a.tail<3>();</div>
|
|
<div class="line"><a name="l00089"></a><span class="lineno"> 89</span>  } <span class="keywordflow">else</span> {</div>
|
|
<div class="line"><a name="l00090"></a><span class="lineno"> 90</span>  <span class="keyword">const</span> Eigen::Matrix3d Omega = <a class="code" href="namespacesmall__gicp.html#a5d86f81a31b152060f1b1f72585fdaf5">skew</a>(omega);</div>
|
|
<div class="line"><a name="l00091"></a><span class="lineno"> 91</span>  <span class="keyword">const</span> Eigen::Matrix3d V = (Eigen::Matrix3d::Identity() + (1.0 - std::cos(theta)) / theta_sq * Omega + (theta - std::sin(theta)) / (theta_sq * theta) * Omega * Omega);</div>
|
|
<div class="line"><a name="l00092"></a><span class="lineno"> 92</span>  se3.translation() = V * a.tail<3>();</div>
|
|
<div class="line"><a name="l00093"></a><span class="lineno"> 93</span>  }</div>
|
|
<div class="line"><a name="l00094"></a><span class="lineno"> 94</span>  </div>
|
|
<div class="line"><a name="l00095"></a><span class="lineno"> 95</span>  <span class="keywordflow">return</span> se3;</div>
|
|
<div class="line"><a name="l00096"></a><span class="lineno"> 96</span> }</div>
|
|
<div class="line"><a name="l00097"></a><span class="lineno"> 97</span>  </div>
|
|
<div class="line"><a name="l00098"></a><span class="lineno"> 98</span> } <span class="comment">// namespace small_gicp</span></div>
|
|
<div class="ttc" id="anamespacesmall__gicp_html"><div class="ttname"><a href="namespacesmall__gicp.html">small_gicp</a></div><div class="ttdef"><b>Definition:</b> flat_container.hpp:12</div></div>
|
|
<div class="ttc" id="anamespacesmall__gicp_html_a0eae106c70deac5c20205c70c4bdaf3c"><div class="ttname"><a href="namespacesmall__gicp.html#a0eae106c70deac5c20205c70c4bdaf3c">small_gicp::se3_exp</a></div><div class="ttdeci">Eigen::Isometry3d se3_exp(const Eigen::Matrix< double, 6, 1 > &a)</div><div class="ttdoc">SE3 expmap (Rotation-first).</div><div class="ttdef"><b>Definition:</b> lie.hpp:77</div></div>
|
|
<div class="ttc" id="anamespacesmall__gicp_html_a5c5f249a06b1a660d2e519a9936da3f7"><div class="ttname"><a href="namespacesmall__gicp.html#a5c5f249a06b1a660d2e519a9936da3f7">small_gicp::so3_exp</a></div><div class="ttdeci">Eigen::Quaterniond so3_exp(const Eigen::Vector3d &omega)</div><div class="ttdoc">SO3 expmap.</div><div class="ttdef"><b>Definition:</b> lie.hpp:54</div></div>
|
|
<div class="ttc" id="anamespacesmall__gicp_html_a5d86f81a31b152060f1b1f72585fdaf5"><div class="ttname"><a href="namespacesmall__gicp.html#a5d86f81a31b152060f1b1f72585fdaf5">small_gicp::skew</a></div><div class="ttdeci">Eigen::Matrix3d skew(const Eigen::Vector3d &x)</div><div class="ttdoc">Create skew symmetric matrix.</div><div class="ttdef"><b>Definition:</b> lie.hpp:13</div></div>
|
|
</div><!-- fragment --></div><!-- contents -->
|
|
<!-- start footer part -->
|
|
<hr class="footer"/><address class="footer"><small>
|
|
Generated by <a href="https://www.doxygen.org/index.html"><img class="footer" src="doxygen.svg" width="104" height="31" alt="doxygen"/></a> 1.9.1
|
|
</small></address>
|
|
</body>
|
|
</html>
|