mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:41 -04:00
Add changes for cb08c39a93
This commit is contained in:
GitHub Action
BIN
_images/normal_vector_calc.png
Normal file
BIN
_images/normal_vector_calc.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 94 KiB |
BIN
_images/ransac_normal_vector_estimation.png
Normal file
BIN
_images/ransac_normal_vector_estimation.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 276 KiB |
@@ -0,0 +1,303 @@
|
||||
<!DOCTYPE html>
|
||||
<html class="writer-html5" lang="en" >
|
||||
<head>
|
||||
<meta charset="utf-8" />
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0" />
|
||||
<title>Mapping.normal_vector_estimation.normal_vector_estimation — PythonRobotics documentation</title>
|
||||
<link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
|
||||
<link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
|
||||
<link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
|
||||
<link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
|
||||
<!--[if lt IE 9]>
|
||||
<script src="../../../_static/js/html5shiv.min.js"></script>
|
||||
<![endif]-->
|
||||
|
||||
<script data-url_root="../../../" id="documentation_options" src="../../../_static/documentation_options.js"></script>
|
||||
<script src="../../../_static/jquery.js"></script>
|
||||
<script src="../../../_static/underscore.js"></script>
|
||||
<script src="../../../_static/doctools.js"></script>
|
||||
<script src="../../../_static/js/theme.js"></script>
|
||||
<link rel="index" title="Index" href="../../../genindex.html" />
|
||||
<link rel="search" title="Search" href="../../../search.html" />
|
||||
</head>
|
||||
|
||||
<body class="wy-body-for-nav">
|
||||
<div class="wy-grid-for-nav">
|
||||
<nav data-toggle="wy-nav-shift" class="wy-nav-side">
|
||||
<div class="wy-side-scroll">
|
||||
<div class="wy-side-nav-search" >
|
||||
|
||||
<a href="../../../index.html" class="icon icon-home"> PythonRobotics
|
||||
<img src="../../../_static/icon.png" class="logo" alt="Logo"/>
|
||||
</a>
|
||||
<div role="search">
|
||||
<form id="rtd-search-form" class="wy-form" action="../../../search.html" method="get">
|
||||
<input type="text" name="q" placeholder="Search docs" />
|
||||
<input type="hidden" name="check_keywords" value="yes" />
|
||||
<input type="hidden" name="area" value="default" />
|
||||
</form>
|
||||
</div>
|
||||
<script async src="https://pagead2.googlesyndication.com/pagead/js/adsbygoogle.js?client=ca-pub-9612347954373886"
|
||||
crossorigin="anonymous"></script>
|
||||
<!-- PythonRoboticsDoc -->
|
||||
<ins class="adsbygoogle"
|
||||
style="display:block"
|
||||
data-ad-client="ca-pub-9612347954373886"
|
||||
data-ad-slot="1579532132"
|
||||
data-ad-format="auto"
|
||||
data-full-width-responsive="true"></ins>
|
||||
<script>
|
||||
(adsbygoogle = window.adsbygoogle || []).push({});
|
||||
</script>
|
||||
|
||||
</div><div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="Navigation menu">
|
||||
<p class="caption" role="heading"><span class="caption-text">Contents</span></p>
|
||||
<ul>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../getting_started.html">Getting Started</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../modules/introduction.html">Introduction</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../modules/localization/localization.html">Localization</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../modules/mapping/mapping.html">Mapping</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../modules/slam/slam.html">SLAM</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../modules/path_planning/path_planning.html">Path Planning</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../modules/path_tracking/path_tracking.html">Path Tracking</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../modules/arm_navigation/arm_navigation.html">Arm Navigation</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../modules/aerial_navigation/aerial_navigation.html">Aerial Navigation</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../modules/bipedal/bipedal.html">Bipedal</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../modules/control/control.html">Control</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../modules/utils/utils.html">Utilities</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../modules/appendix/appendix.html">Appendix</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../how_to_contribute.html">How To Contribute</a></li>
|
||||
</ul>
|
||||
|
||||
</div>
|
||||
</div>
|
||||
</nav>
|
||||
|
||||
<section data-toggle="wy-nav-shift" class="wy-nav-content-wrap"><nav class="wy-nav-top" aria-label="Mobile navigation menu" >
|
||||
<i data-toggle="wy-nav-top" class="fa fa-bars"></i>
|
||||
<a href="../../../index.html">PythonRobotics</a>
|
||||
</nav>
|
||||
|
||||
<div class="wy-nav-content">
|
||||
<div class="rst-content">
|
||||
<div role="navigation" aria-label="Page navigation">
|
||||
<ul class="wy-breadcrumbs">
|
||||
<li><a href="../../../index.html" class="icon icon-home"></a> »</li>
|
||||
<li><a href="../../index.html">Module code</a> »</li>
|
||||
<li>Mapping.normal_vector_estimation.normal_vector_estimation</li>
|
||||
<li class="wy-breadcrumbs-aside">
|
||||
</li>
|
||||
</ul>
|
||||
<hr/>
|
||||
</div>
|
||||
<div role="main" class="document" itemscope="itemscope" itemtype="http://schema.org/Article">
|
||||
<div itemprop="articleBody">
|
||||
|
||||
<h1>Source code for Mapping.normal_vector_estimation.normal_vector_estimation</h1><div class="highlight"><pre>
|
||||
<span></span><span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
|
||||
<span class="kn">from</span> <span class="nn">matplotlib</span> <span class="kn">import</span> <span class="n">pyplot</span> <span class="k">as</span> <span class="n">plt</span>
|
||||
|
||||
<span class="kn">from</span> <span class="nn">utils.plot</span> <span class="kn">import</span> <span class="n">plot_3d_vector_arrow</span><span class="p">,</span> <span class="n">plot_triangle</span><span class="p">,</span> <span class="n">set_equal_3d_axis</span>
|
||||
|
||||
<span class="n">show_animation</span> <span class="o">=</span> <span class="kc">True</span>
|
||||
|
||||
|
||||
<div class="viewcode-block" id="calc_normal_vector"><a class="viewcode-back" href="../../../modules/mapping/normal_vector_estimation/normal_vector_estimation.html#Mapping.normal_vector_estimation.normal_vector_estimation.calc_normal_vector">[docs]</a><span class="k">def</span> <span class="nf">calc_normal_vector</span><span class="p">(</span><span class="n">p1</span><span class="p">,</span> <span class="n">p2</span><span class="p">,</span> <span class="n">p3</span><span class="p">):</span>
|
||||
<span class="w"> </span><span class="sd">"""Calculate normal vector of triangle</span>
|
||||
|
||||
<span class="sd"> Parameters</span>
|
||||
<span class="sd"> ----------</span>
|
||||
<span class="sd"> p1 : np.array</span>
|
||||
<span class="sd"> 3D point</span>
|
||||
<span class="sd"> p2 : np.array</span>
|
||||
<span class="sd"> 3D point</span>
|
||||
<span class="sd"> p3 : np.array</span>
|
||||
<span class="sd"> 3D point</span>
|
||||
|
||||
<span class="sd"> Returns</span>
|
||||
<span class="sd"> -------</span>
|
||||
<span class="sd"> normal_vector : np.array</span>
|
||||
<span class="sd"> normal vector (3,)</span>
|
||||
|
||||
<span class="sd"> """</span>
|
||||
<span class="c1"># calculate two vectors of triangle</span>
|
||||
<span class="n">v1</span> <span class="o">=</span> <span class="n">p2</span> <span class="o">-</span> <span class="n">p1</span>
|
||||
<span class="n">v2</span> <span class="o">=</span> <span class="n">p3</span> <span class="o">-</span> <span class="n">p1</span>
|
||||
|
||||
<span class="c1"># calculate normal vector</span>
|
||||
<span class="n">normal_vector</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">v1</span><span class="p">,</span> <span class="n">v2</span><span class="p">)</span>
|
||||
|
||||
<span class="c1"># normalize vector</span>
|
||||
<span class="n">normal_vector</span> <span class="o">=</span> <span class="n">normal_vector</span> <span class="o">/</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">normal_vector</span><span class="p">)</span>
|
||||
|
||||
<span class="k">return</span> <span class="n">normal_vector</span></div>
|
||||
|
||||
|
||||
<span class="k">def</span> <span class="nf">sample_3d_points_from_a_plane</span><span class="p">(</span><span class="n">num_samples</span><span class="p">,</span> <span class="n">normal</span><span class="p">):</span>
|
||||
<span class="n">points_2d</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">normal</span><span class="p">(</span><span class="n">size</span><span class="o">=</span><span class="p">(</span><span class="n">num_samples</span><span class="p">,</span> <span class="mi">2</span><span class="p">))</span> <span class="c1"># 2D points on a plane</span>
|
||||
<span class="n">d</span> <span class="o">=</span> <span class="mi">0</span>
|
||||
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="nb">len</span><span class="p">(</span><span class="n">points_2d</span><span class="p">)):</span>
|
||||
<span class="n">point_3d</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">points_2d</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="mi">0</span><span class="p">)</span>
|
||||
<span class="n">d</span> <span class="o">+=</span> <span class="n">normal</span> <span class="o">@</span> <span class="n">point_3d</span>
|
||||
<span class="n">d</span> <span class="o">/=</span> <span class="nb">len</span><span class="p">(</span><span class="n">points_2d</span><span class="p">)</span>
|
||||
|
||||
<span class="n">points_3d</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="nb">len</span><span class="p">(</span><span class="n">points_2d</span><span class="p">),</span> <span class="mi">3</span><span class="p">))</span>
|
||||
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="nb">len</span><span class="p">(</span><span class="n">points_2d</span><span class="p">)):</span>
|
||||
<span class="n">point_2d</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">points_2d</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="mi">0</span><span class="p">)</span>
|
||||
<span class="n">projection_length</span> <span class="o">=</span> <span class="p">(</span><span class="n">d</span> <span class="o">-</span> <span class="n">normal</span> <span class="o">@</span> <span class="n">point_2d</span><span class="p">)</span> <span class="o">/</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">normal</span><span class="p">)</span>
|
||||
<span class="n">points_3d</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">point_2d</span> <span class="o">+</span> <span class="n">projection_length</span> <span class="o">*</span> <span class="n">normal</span>
|
||||
|
||||
<span class="k">return</span> <span class="n">points_3d</span>
|
||||
|
||||
|
||||
<span class="k">def</span> <span class="nf">distance_to_plane</span><span class="p">(</span><span class="n">point</span><span class="p">,</span> <span class="n">normal</span><span class="p">,</span> <span class="n">origin</span><span class="p">):</span>
|
||||
<span class="n">dot_product</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">normal</span><span class="p">,</span> <span class="n">point</span><span class="p">)</span> <span class="o">-</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">normal</span><span class="p">,</span> <span class="n">origin</span><span class="p">)</span>
|
||||
<span class="k">if</span> <span class="n">np</span><span class="o">.</span><span class="n">isclose</span><span class="p">(</span><span class="n">dot_product</span><span class="p">,</span> <span class="mi">0</span><span class="p">):</span>
|
||||
<span class="k">return</span> <span class="mf">0.0</span>
|
||||
<span class="k">else</span><span class="p">:</span>
|
||||
<span class="n">distance</span> <span class="o">=</span> <span class="nb">abs</span><span class="p">(</span><span class="n">dot_product</span><span class="p">)</span> <span class="o">/</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">normal</span><span class="p">)</span>
|
||||
<span class="k">return</span> <span class="n">distance</span>
|
||||
|
||||
|
||||
<div class="viewcode-block" id="ransac_normal_vector_estimation"><a class="viewcode-back" href="../../../modules/mapping/normal_vector_estimation/normal_vector_estimation.html#Mapping.normal_vector_estimation.normal_vector_estimation.ransac_normal_vector_estimation">[docs]</a><span class="k">def</span> <span class="nf">ransac_normal_vector_estimation</span><span class="p">(</span><span class="n">points_3d</span><span class="p">,</span> <span class="n">inlier_radio_th</span><span class="o">=</span><span class="mf">0.7</span><span class="p">,</span>
|
||||
<span class="n">inlier_dist</span><span class="o">=</span><span class="mf">0.1</span><span class="p">,</span> <span class="n">p</span><span class="o">=</span><span class="mf">0.99</span><span class="p">):</span>
|
||||
<span class="w"> </span><span class="sd">"""</span>
|
||||
<span class="sd"> RANSAC based normal vector estimation</span>
|
||||
|
||||
<span class="sd"> Parameters</span>
|
||||
<span class="sd"> ----------</span>
|
||||
<span class="sd"> points_3d : np.array</span>
|
||||
<span class="sd"> 3D points (N, 3)</span>
|
||||
<span class="sd"> inlier_radio_th : float</span>
|
||||
<span class="sd"> Inlier ratio threshold. If inlier ratio is larger than this value,</span>
|
||||
<span class="sd"> the iteration is stopped. Default is 0.7.</span>
|
||||
<span class="sd"> inlier_dist : float</span>
|
||||
<span class="sd"> Inlier distance threshold. If distance between points and estimated</span>
|
||||
<span class="sd"> plane is smaller than this value, the point is inlier. Default is 0.1.</span>
|
||||
<span class="sd"> p : float</span>
|
||||
<span class="sd"> Probability that at least one of the sets of random samples does not</span>
|
||||
<span class="sd"> include an outlier. If this probability is near 1, the iteration</span>
|
||||
<span class="sd"> number is large. Default is 0.99.</span>
|
||||
|
||||
<span class="sd"> Returns</span>
|
||||
<span class="sd"> -------</span>
|
||||
<span class="sd"> center_vector : np.array</span>
|
||||
<span class="sd"> Center of estimated plane. (3,)</span>
|
||||
<span class="sd"> normal_vector : np.array</span>
|
||||
<span class="sd"> Normal vector of estimated plane. (3,)</span>
|
||||
|
||||
<span class="sd"> """</span>
|
||||
<span class="n">center</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">mean</span><span class="p">(</span><span class="n">points_3d</span><span class="p">,</span> <span class="n">axis</span><span class="o">=</span><span class="mi">0</span><span class="p">)</span>
|
||||
|
||||
<span class="n">max_iter</span> <span class="o">=</span> <span class="nb">int</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">floor</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">log</span><span class="p">(</span><span class="mf">1.0</span><span class="o">-</span><span class="n">p</span><span class="p">)</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">log</span><span class="p">(</span><span class="mf">1.0</span><span class="o">-</span><span class="p">(</span><span class="mf">1.0</span><span class="o">-</span><span class="n">inlier_radio_th</span><span class="p">)</span><span class="o">**</span><span class="mi">3</span><span class="p">)))</span>
|
||||
|
||||
<span class="k">for</span> <span class="n">ite</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">max_iter</span><span class="p">):</span>
|
||||
<span class="c1"># Random sampling</span>
|
||||
<span class="n">sampled_ids</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">choice</span><span class="p">(</span><span class="n">points_3d</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">size</span><span class="o">=</span><span class="mi">3</span><span class="p">,</span>
|
||||
<span class="n">replace</span><span class="o">=</span><span class="kc">False</span><span class="p">)</span>
|
||||
<span class="n">sampled_points</span> <span class="o">=</span> <span class="n">points_3d</span><span class="p">[</span><span class="n">sampled_ids</span><span class="p">,</span> <span class="p">:]</span>
|
||||
<span class="n">p1</span> <span class="o">=</span> <span class="n">sampled_points</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="p">:]</span>
|
||||
<span class="n">p2</span> <span class="o">=</span> <span class="n">sampled_points</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="p">:]</span>
|
||||
<span class="n">p3</span> <span class="o">=</span> <span class="n">sampled_points</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="p">:]</span>
|
||||
<span class="n">normal_vector</span> <span class="o">=</span> <span class="n">calc_normal_vector</span><span class="p">(</span><span class="n">p1</span><span class="p">,</span> <span class="n">p2</span><span class="p">,</span> <span class="n">p3</span><span class="p">)</span>
|
||||
|
||||
<span class="c1"># calc inlier ratio</span>
|
||||
<span class="n">n_inliner</span> <span class="o">=</span> <span class="mi">0</span>
|
||||
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">points_3d</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]):</span>
|
||||
<span class="n">p</span> <span class="o">=</span> <span class="n">points_3d</span><span class="p">[</span><span class="n">i</span><span class="p">,</span> <span class="p">:]</span>
|
||||
<span class="k">if</span> <span class="n">distance_to_plane</span><span class="p">(</span><span class="n">p</span><span class="p">,</span> <span class="n">normal_vector</span><span class="p">,</span> <span class="n">center</span><span class="p">)</span> <span class="o"><=</span> <span class="n">inlier_dist</span><span class="p">:</span>
|
||||
<span class="n">n_inliner</span> <span class="o">+=</span> <span class="mi">1</span>
|
||||
<span class="n">inlier_ratio</span> <span class="o">=</span> <span class="n">n_inliner</span> <span class="o">/</span> <span class="n">points_3d</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
|
||||
<span class="nb">print</span><span class="p">(</span><span class="sa">f</span><span class="s2">"Iter:</span><span class="si">{</span><span class="n">ite</span><span class="si">}</span><span class="s2">, </span><span class="si">{</span><span class="n">inlier_ratio</span><span class="si">=}</span><span class="s2">"</span><span class="p">)</span>
|
||||
<span class="k">if</span> <span class="n">inlier_ratio</span> <span class="o">></span> <span class="n">inlier_radio_th</span><span class="p">:</span>
|
||||
<span class="k">return</span> <span class="n">center</span><span class="p">,</span> <span class="n">normal_vector</span>
|
||||
|
||||
<span class="k">return</span> <span class="n">center</span><span class="p">,</span> <span class="kc">None</span></div>
|
||||
|
||||
|
||||
<span class="k">def</span> <span class="nf">main1</span><span class="p">():</span>
|
||||
<span class="n">p1</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mf">0.0</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">,</span> <span class="mf">1.0</span><span class="p">])</span>
|
||||
<span class="n">p2</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mf">1.0</span><span class="p">,</span> <span class="mf">1.0</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">])</span>
|
||||
<span class="n">p3</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mf">0.0</span><span class="p">,</span> <span class="mf">1.0</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">])</span>
|
||||
|
||||
<span class="n">center</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">mean</span><span class="p">([</span><span class="n">p1</span><span class="p">,</span> <span class="n">p2</span><span class="p">,</span> <span class="n">p3</span><span class="p">],</span> <span class="n">axis</span><span class="o">=</span><span class="mi">0</span><span class="p">)</span>
|
||||
<span class="n">normal_vector</span> <span class="o">=</span> <span class="n">calc_normal_vector</span><span class="p">(</span><span class="n">p1</span><span class="p">,</span> <span class="n">p2</span><span class="p">,</span> <span class="n">p3</span><span class="p">)</span>
|
||||
<span class="nb">print</span><span class="p">(</span><span class="sa">f</span><span class="s2">"</span><span class="si">{</span><span class="n">center</span><span class="si">=}</span><span class="s2">"</span><span class="p">)</span>
|
||||
<span class="nb">print</span><span class="p">(</span><span class="sa">f</span><span class="s2">"</span><span class="si">{</span><span class="n">normal_vector</span><span class="si">=}</span><span class="s2">"</span><span class="p">)</span>
|
||||
|
||||
<span class="k">if</span> <span class="n">show_animation</span><span class="p">:</span>
|
||||
<span class="n">fig</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">figure</span><span class="p">()</span>
|
||||
<span class="n">ax</span> <span class="o">=</span> <span class="n">fig</span><span class="o">.</span><span class="n">add_subplot</span><span class="p">(</span><span class="n">projection</span><span class="o">=</span><span class="s1">'3d'</span><span class="p">)</span>
|
||||
<span class="n">set_equal_3d_axis</span><span class="p">(</span><span class="n">ax</span><span class="p">,</span> <span class="p">[</span><span class="mf">0.0</span><span class="p">,</span> <span class="mf">2.5</span><span class="p">],</span> <span class="p">[</span><span class="mf">0.0</span><span class="p">,</span> <span class="mf">2.5</span><span class="p">],</span> <span class="p">[</span><span class="mf">0.0</span><span class="p">,</span> <span class="mf">3.0</span><span class="p">])</span>
|
||||
<span class="n">plot_triangle</span><span class="p">(</span><span class="n">p1</span><span class="p">,</span> <span class="n">p2</span><span class="p">,</span> <span class="n">p3</span><span class="p">,</span> <span class="n">ax</span><span class="p">)</span>
|
||||
<span class="n">ax</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">center</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">center</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">center</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="s2">"ro"</span><span class="p">)</span>
|
||||
<span class="n">plot_3d_vector_arrow</span><span class="p">(</span><span class="n">ax</span><span class="p">,</span> <span class="n">center</span><span class="p">,</span> <span class="n">center</span> <span class="o">+</span> <span class="n">normal_vector</span><span class="p">)</span>
|
||||
<span class="n">plt</span><span class="o">.</span><span class="n">show</span><span class="p">()</span>
|
||||
|
||||
|
||||
<span class="k">def</span> <span class="nf">main2</span><span class="p">(</span><span class="n">rng</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
|
||||
<span class="n">true_normal</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>
|
||||
<span class="n">true_normal</span> <span class="o">=</span> <span class="n">true_normal</span> <span class="o">/</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">true_normal</span><span class="p">)</span>
|
||||
<span class="n">num_samples</span> <span class="o">=</span> <span class="mi">100</span>
|
||||
<span class="n">noise_scale</span> <span class="o">=</span> <span class="mf">0.1</span>
|
||||
|
||||
<span class="n">points_3d</span> <span class="o">=</span> <span class="n">sample_3d_points_from_a_plane</span><span class="p">(</span><span class="n">num_samples</span><span class="p">,</span> <span class="n">true_normal</span><span class="p">)</span>
|
||||
<span class="c1"># add random noise</span>
|
||||
<span class="n">points_3d</span> <span class="o">+=</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">normal</span><span class="p">(</span><span class="n">size</span><span class="o">=</span><span class="n">points_3d</span><span class="o">.</span><span class="n">shape</span><span class="p">,</span> <span class="n">scale</span><span class="o">=</span><span class="n">noise_scale</span><span class="p">)</span>
|
||||
|
||||
<span class="nb">print</span><span class="p">(</span><span class="sa">f</span><span class="s2">"</span><span class="si">{</span><span class="n">points_3d</span><span class="o">.</span><span class="n">shape</span><span class="si">=}</span><span class="s2">"</span><span class="p">)</span>
|
||||
|
||||
<span class="n">center</span><span class="p">,</span> <span class="n">estimated_normal</span> <span class="o">=</span> <span class="n">ransac_normal_vector_estimation</span><span class="p">(</span>
|
||||
<span class="n">points_3d</span><span class="p">,</span> <span class="n">inlier_dist</span><span class="o">=</span><span class="n">noise_scale</span><span class="p">)</span>
|
||||
|
||||
<span class="k">if</span> <span class="n">estimated_normal</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
|
||||
<span class="nb">print</span><span class="p">(</span><span class="s2">"Failed to estimate normal vector"</span><span class="p">)</span>
|
||||
<span class="k">return</span>
|
||||
|
||||
<span class="nb">print</span><span class="p">(</span><span class="sa">f</span><span class="s2">"</span><span class="si">{</span><span class="n">true_normal</span><span class="si">=}</span><span class="s2">"</span><span class="p">)</span>
|
||||
<span class="nb">print</span><span class="p">(</span><span class="sa">f</span><span class="s2">"</span><span class="si">{</span><span class="n">estimated_normal</span><span class="si">=}</span><span class="s2">"</span><span class="p">)</span>
|
||||
|
||||
<span class="k">if</span> <span class="n">show_animation</span><span class="p">:</span>
|
||||
<span class="n">fig</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">figure</span><span class="p">()</span>
|
||||
<span class="n">ax</span> <span class="o">=</span> <span class="n">fig</span><span class="o">.</span><span class="n">add_subplot</span><span class="p">(</span><span class="n">projection</span><span class="o">=</span><span class="s1">'3d'</span><span class="p">)</span>
|
||||
<span class="n">ax</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">points_3d</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">points_3d</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">points_3d</span><span class="p">[:,</span> <span class="mi">2</span><span class="p">],</span> <span class="s2">".r"</span><span class="p">)</span>
|
||||
<span class="n">plot_3d_vector_arrow</span><span class="p">(</span><span class="n">ax</span><span class="p">,</span> <span class="n">center</span><span class="p">,</span> <span class="n">center</span> <span class="o">+</span> <span class="n">true_normal</span><span class="p">)</span>
|
||||
<span class="n">plot_3d_vector_arrow</span><span class="p">(</span><span class="n">ax</span><span class="p">,</span> <span class="n">center</span><span class="p">,</span> <span class="n">center</span> <span class="o">+</span> <span class="n">estimated_normal</span><span class="p">)</span>
|
||||
<span class="n">set_equal_3d_axis</span><span class="p">(</span><span class="n">ax</span><span class="p">,</span> <span class="p">[</span><span class="o">-</span><span class="mf">3.0</span><span class="p">,</span> <span class="mf">3.0</span><span class="p">],</span> <span class="p">[</span><span class="o">-</span><span class="mf">3.0</span><span class="p">,</span> <span class="mf">3.0</span><span class="p">],</span> <span class="p">[</span><span class="o">-</span><span class="mf">3.0</span><span class="p">,</span> <span class="mf">3.0</span><span class="p">])</span>
|
||||
<span class="n">plt</span><span class="o">.</span><span class="n">title</span><span class="p">(</span><span class="s2">"RANSAC based Normal vector estimation"</span><span class="p">)</span>
|
||||
<span class="n">plt</span><span class="o">.</span><span class="n">show</span><span class="p">()</span>
|
||||
|
||||
|
||||
<span class="k">if</span> <span class="vm">__name__</span> <span class="o">==</span> <span class="s1">'__main__'</span><span class="p">:</span>
|
||||
<span class="c1"># main1()</span>
|
||||
<span class="n">main2</span><span class="p">()</span>
|
||||
</pre></div>
|
||||
|
||||
</div>
|
||||
</div>
|
||||
<footer>
|
||||
|
||||
<hr/>
|
||||
|
||||
<div role="contentinfo">
|
||||
<p>© Copyright 2018-2021, Atsushi Sakai.</p>
|
||||
</div>
|
||||
|
||||
Built with <a href="https://www.sphinx-doc.org/">Sphinx</a> using a
|
||||
<a href="https://github.com/readthedocs/sphinx_rtd_theme">theme</a>
|
||||
provided by <a href="https://readthedocs.org">Read the Docs</a>.
|
||||
|
||||
|
||||
</footer>
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
</div>
|
||||
<script>
|
||||
jQuery(function () {
|
||||
SphinxRtdTheme.Navigation.enable(true);
|
||||
});
|
||||
</script>
|
||||
|
||||
</body>
|
||||
</html>
|
||||
@@ -93,7 +93,8 @@
|
||||
<div itemprop="articleBody">
|
||||
|
||||
<h1>All modules for which code is available</h1>
|
||||
<ul><li><a href="Mapping/point_cloud_sampling/point_cloud_sampling.html">Mapping.point_cloud_sampling.point_cloud_sampling</a></li>
|
||||
<ul><li><a href="Mapping/normal_vector_estimation/normal_vector_estimation.html">Mapping.normal_vector_estimation.normal_vector_estimation</a></li>
|
||||
<li><a href="Mapping/point_cloud_sampling/point_cloud_sampling.html">Mapping.point_cloud_sampling.point_cloud_sampling</a></li>
|
||||
<li><a href="PathPlanning/BSplinePath/bspline_path.html">PathPlanning.BSplinePath.bspline_path</a></li>
|
||||
<li><a href="PathPlanning/CubicSpline/cubic_spline_planner.html">PathPlanning.CubicSpline.cubic_spline_planner</a></li>
|
||||
<li><a href="PathPlanning/DubinsPath/dubins_path_planner.html">PathPlanning.DubinsPath.dubins_path_planner</a></li>
|
||||
|
||||
@@ -100,6 +100,10 @@
|
||||
<span class="kn">import</span> <span class="nn">math</span>
|
||||
<span class="kn">import</span> <span class="nn">matplotlib.pyplot</span> <span class="k">as</span> <span class="nn">plt</span>
|
||||
<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
|
||||
<span class="kn">from</span> <span class="nn">mpl_toolkits.mplot3d</span> <span class="kn">import</span> <span class="n">art3d</span>
|
||||
<span class="kn">from</span> <span class="nn">matplotlib.patches</span> <span class="kn">import</span> <span class="n">FancyArrowPatch</span>
|
||||
<span class="kn">from</span> <span class="nn">mpl_toolkits.mplot3d.proj3d</span> <span class="kn">import</span> <span class="n">proj_transform</span>
|
||||
<span class="kn">from</span> <span class="nn">mpl_toolkits.mplot3d</span> <span class="kn">import</span> <span class="n">Axes3D</span>
|
||||
|
||||
|
||||
<span class="k">def</span> <span class="nf">plot_arrow</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">yaw</span><span class="p">,</span> <span class="n">arrow_length</span><span class="o">=</span><span class="mf">1.0</span><span class="p">,</span>
|
||||
@@ -180,6 +184,81 @@
|
||||
<span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">cx</span><span class="p">,</span> <span class="n">cy</span><span class="p">,</span> <span class="n">c</span><span class="p">,</span> <span class="n">label</span><span class="o">=</span><span class="n">label</span><span class="p">)</span>
|
||||
<span class="k">for</span> <span class="n">ix</span><span class="p">,</span> <span class="n">iy</span><span class="p">,</span> <span class="n">icx</span><span class="p">,</span> <span class="n">icy</span> <span class="ow">in</span> <span class="nb">zip</span><span class="p">(</span><span class="n">x_list</span><span class="p">,</span> <span class="n">y_list</span><span class="p">,</span> <span class="n">cx</span><span class="p">,</span> <span class="n">cy</span><span class="p">):</span>
|
||||
<span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">([</span><span class="n">ix</span><span class="p">,</span> <span class="n">icx</span><span class="p">],</span> <span class="p">[</span><span class="n">iy</span><span class="p">,</span> <span class="n">icy</span><span class="p">],</span> <span class="n">c</span><span class="p">)</span></div>
|
||||
|
||||
|
||||
<span class="k">class</span> <span class="nc">Arrow3D</span><span class="p">(</span><span class="n">FancyArrowPatch</span><span class="p">):</span>
|
||||
|
||||
<span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">,</span> <span class="n">dx</span><span class="p">,</span> <span class="n">dy</span><span class="p">,</span> <span class="n">dz</span><span class="p">,</span> <span class="o">*</span><span class="n">args</span><span class="p">,</span> <span class="o">**</span><span class="n">kwargs</span><span class="p">):</span>
|
||||
<span class="nb">super</span><span class="p">()</span><span class="o">.</span><span class="fm">__init__</span><span class="p">((</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">),</span> <span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">),</span> <span class="o">*</span><span class="n">args</span><span class="p">,</span> <span class="o">**</span><span class="n">kwargs</span><span class="p">)</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">_xyz</span> <span class="o">=</span> <span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">)</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">_dxdydz</span> <span class="o">=</span> <span class="p">(</span><span class="n">dx</span><span class="p">,</span> <span class="n">dy</span><span class="p">,</span> <span class="n">dz</span><span class="p">)</span>
|
||||
|
||||
<span class="k">def</span> <span class="nf">draw</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">renderer</span><span class="p">):</span>
|
||||
<span class="n">x1</span><span class="p">,</span> <span class="n">y1</span><span class="p">,</span> <span class="n">z1</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_xyz</span>
|
||||
<span class="n">dx</span><span class="p">,</span> <span class="n">dy</span><span class="p">,</span> <span class="n">dz</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_dxdydz</span>
|
||||
<span class="n">x2</span><span class="p">,</span> <span class="n">y2</span><span class="p">,</span> <span class="n">z2</span> <span class="o">=</span> <span class="p">(</span><span class="n">x1</span> <span class="o">+</span> <span class="n">dx</span><span class="p">,</span> <span class="n">y1</span> <span class="o">+</span> <span class="n">dy</span><span class="p">,</span> <span class="n">z1</span> <span class="o">+</span> <span class="n">dz</span><span class="p">)</span>
|
||||
|
||||
<span class="n">xs</span><span class="p">,</span> <span class="n">ys</span><span class="p">,</span> <span class="n">zs</span> <span class="o">=</span> <span class="n">proj_transform</span><span class="p">((</span><span class="n">x1</span><span class="p">,</span> <span class="n">x2</span><span class="p">),</span> <span class="p">(</span><span class="n">y1</span><span class="p">,</span> <span class="n">y2</span><span class="p">),</span> <span class="p">(</span><span class="n">z1</span><span class="p">,</span> <span class="n">z2</span><span class="p">),</span> <span class="bp">self</span><span class="o">.</span><span class="n">axes</span><span class="o">.</span><span class="n">M</span><span class="p">)</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">set_positions</span><span class="p">((</span><span class="n">xs</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">ys</span><span class="p">[</span><span class="mi">0</span><span class="p">]),</span> <span class="p">(</span><span class="n">xs</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">ys</span><span class="p">[</span><span class="mi">1</span><span class="p">]))</span>
|
||||
<span class="nb">super</span><span class="p">()</span><span class="o">.</span><span class="n">draw</span><span class="p">(</span><span class="n">renderer</span><span class="p">)</span>
|
||||
|
||||
<span class="k">def</span> <span class="nf">do_3d_projection</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">renderer</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
|
||||
<span class="n">x1</span><span class="p">,</span> <span class="n">y1</span><span class="p">,</span> <span class="n">z1</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_xyz</span>
|
||||
<span class="n">dx</span><span class="p">,</span> <span class="n">dy</span><span class="p">,</span> <span class="n">dz</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_dxdydz</span>
|
||||
<span class="n">x2</span><span class="p">,</span> <span class="n">y2</span><span class="p">,</span> <span class="n">z2</span> <span class="o">=</span> <span class="p">(</span><span class="n">x1</span> <span class="o">+</span> <span class="n">dx</span><span class="p">,</span> <span class="n">y1</span> <span class="o">+</span> <span class="n">dy</span><span class="p">,</span> <span class="n">z1</span> <span class="o">+</span> <span class="n">dz</span><span class="p">)</span>
|
||||
|
||||
<span class="n">xs</span><span class="p">,</span> <span class="n">ys</span><span class="p">,</span> <span class="n">zs</span> <span class="o">=</span> <span class="n">proj_transform</span><span class="p">((</span><span class="n">x1</span><span class="p">,</span> <span class="n">x2</span><span class="p">),</span> <span class="p">(</span><span class="n">y1</span><span class="p">,</span> <span class="n">y2</span><span class="p">),</span> <span class="p">(</span><span class="n">z1</span><span class="p">,</span> <span class="n">z2</span><span class="p">),</span> <span class="bp">self</span><span class="o">.</span><span class="n">axes</span><span class="o">.</span><span class="n">M</span><span class="p">)</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">set_positions</span><span class="p">((</span><span class="n">xs</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">ys</span><span class="p">[</span><span class="mi">0</span><span class="p">]),</span> <span class="p">(</span><span class="n">xs</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">ys</span><span class="p">[</span><span class="mi">1</span><span class="p">]))</span>
|
||||
|
||||
<span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">min</span><span class="p">(</span><span class="n">zs</span><span class="p">)</span>
|
||||
|
||||
|
||||
<span class="k">def</span> <span class="nf">_arrow3D</span><span class="p">(</span><span class="n">ax</span><span class="p">,</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">,</span> <span class="n">dx</span><span class="p">,</span> <span class="n">dy</span><span class="p">,</span> <span class="n">dz</span><span class="p">,</span> <span class="o">*</span><span class="n">args</span><span class="p">,</span> <span class="o">**</span><span class="n">kwargs</span><span class="p">):</span>
|
||||
<span class="w"> </span><span class="sd">'''Add an 3d arrow to an `Axes3D` instance.'''</span>
|
||||
<span class="n">arrow</span> <span class="o">=</span> <span class="n">Arrow3D</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">,</span> <span class="n">dx</span><span class="p">,</span> <span class="n">dy</span><span class="p">,</span> <span class="n">dz</span><span class="p">,</span> <span class="o">*</span><span class="n">args</span><span class="p">,</span> <span class="o">**</span><span class="n">kwargs</span><span class="p">)</span>
|
||||
<span class="n">ax</span><span class="o">.</span><span class="n">add_artist</span><span class="p">(</span><span class="n">arrow</span><span class="p">)</span>
|
||||
|
||||
|
||||
<span class="k">def</span> <span class="nf">plot_3d_vector_arrow</span><span class="p">(</span><span class="n">ax</span><span class="p">,</span> <span class="n">p1</span><span class="p">,</span> <span class="n">p2</span><span class="p">):</span>
|
||||
<span class="nb">setattr</span><span class="p">(</span><span class="n">Axes3D</span><span class="p">,</span> <span class="s1">'arrow3D'</span><span class="p">,</span> <span class="n">_arrow3D</span><span class="p">)</span>
|
||||
<span class="n">ax</span><span class="o">.</span><span class="n">arrow3D</span><span class="p">(</span><span class="n">p1</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">p1</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">p1</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span>
|
||||
<span class="n">p2</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span><span class="o">-</span><span class="n">p1</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">p2</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span><span class="o">-</span><span class="n">p1</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">p2</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span><span class="o">-</span><span class="n">p1</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span>
|
||||
<span class="n">mutation_scale</span><span class="o">=</span><span class="mi">20</span><span class="p">,</span>
|
||||
<span class="n">arrowstyle</span><span class="o">=</span><span class="s2">"-|>"</span><span class="p">,</span>
|
||||
<span class="p">)</span>
|
||||
|
||||
|
||||
|
||||
<span class="k">def</span> <span class="nf">plot_triangle</span><span class="p">(</span><span class="n">p1</span><span class="p">,</span> <span class="n">p2</span><span class="p">,</span> <span class="n">p3</span><span class="p">,</span> <span class="n">ax</span><span class="p">):</span>
|
||||
<span class="n">ax</span><span class="o">.</span><span class="n">add_collection3d</span><span class="p">(</span><span class="n">art3d</span><span class="o">.</span><span class="n">Poly3DCollection</span><span class="p">([[</span><span class="n">p1</span><span class="p">,</span> <span class="n">p2</span><span class="p">,</span> <span class="n">p3</span><span class="p">]],</span> <span class="n">color</span><span class="o">=</span><span class="s1">'b'</span><span class="p">))</span>
|
||||
|
||||
|
||||
<span class="k">def</span> <span class="nf">set_equal_3d_axis</span><span class="p">(</span><span class="n">ax</span><span class="p">,</span> <span class="n">x_lims</span><span class="p">,</span> <span class="n">y_lims</span><span class="p">,</span> <span class="n">z_lims</span><span class="p">):</span>
|
||||
<span class="w"> </span><span class="sd">"""Helper function to set equal axis</span>
|
||||
|
||||
<span class="sd"> Args:</span>
|
||||
<span class="sd"> ax (Axes3DSubplot): matplotlib 3D axis, created by</span>
|
||||
<span class="sd"> `ax = fig.add_subplot(projection='3d')`</span>
|
||||
<span class="sd"> x_lims (np.array): array containing min and max value of x</span>
|
||||
<span class="sd"> y_lims (np.array): array containing min and max value of y</span>
|
||||
<span class="sd"> z_lims (np.array): array containing min and max value of z</span>
|
||||
<span class="sd"> """</span>
|
||||
<span class="n">x_lims</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">asarray</span><span class="p">(</span><span class="n">x_lims</span><span class="p">)</span>
|
||||
<span class="n">y_lims</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">asarray</span><span class="p">(</span><span class="n">y_lims</span><span class="p">)</span>
|
||||
<span class="n">z_lims</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">asarray</span><span class="p">(</span><span class="n">z_lims</span><span class="p">)</span>
|
||||
<span class="c1"># compute max required range</span>
|
||||
<span class="n">max_range</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="n">x_lims</span><span class="o">.</span><span class="n">max</span><span class="p">()</span> <span class="o">-</span> <span class="n">x_lims</span><span class="o">.</span><span class="n">min</span><span class="p">(),</span>
|
||||
<span class="n">y_lims</span><span class="o">.</span><span class="n">max</span><span class="p">()</span> <span class="o">-</span> <span class="n">y_lims</span><span class="o">.</span><span class="n">min</span><span class="p">(),</span>
|
||||
<span class="n">z_lims</span><span class="o">.</span><span class="n">max</span><span class="p">()</span> <span class="o">-</span> <span class="n">z_lims</span><span class="o">.</span><span class="n">min</span><span class="p">()])</span><span class="o">.</span><span class="n">max</span><span class="p">()</span> <span class="o">/</span> <span class="mf">2.0</span>
|
||||
<span class="c1"># compute mid-point along each axis</span>
|
||||
<span class="n">mid_x</span> <span class="o">=</span> <span class="p">(</span><span class="n">x_lims</span><span class="o">.</span><span class="n">max</span><span class="p">()</span> <span class="o">+</span> <span class="n">x_lims</span><span class="o">.</span><span class="n">min</span><span class="p">())</span> <span class="o">*</span> <span class="mf">0.5</span>
|
||||
<span class="n">mid_y</span> <span class="o">=</span> <span class="p">(</span><span class="n">y_lims</span><span class="o">.</span><span class="n">max</span><span class="p">()</span> <span class="o">+</span> <span class="n">y_lims</span><span class="o">.</span><span class="n">min</span><span class="p">())</span> <span class="o">*</span> <span class="mf">0.5</span>
|
||||
<span class="n">mid_z</span> <span class="o">=</span> <span class="p">(</span><span class="n">z_lims</span><span class="o">.</span><span class="n">max</span><span class="p">()</span> <span class="o">+</span> <span class="n">z_lims</span><span class="o">.</span><span class="n">min</span><span class="p">())</span> <span class="o">*</span> <span class="mf">0.5</span>
|
||||
|
||||
<span class="c1"># set limits to axis</span>
|
||||
<span class="n">ax</span><span class="o">.</span><span class="n">set_xlim</span><span class="p">(</span><span class="n">mid_x</span> <span class="o">-</span> <span class="n">max_range</span><span class="p">,</span> <span class="n">mid_x</span> <span class="o">+</span> <span class="n">max_range</span><span class="p">)</span>
|
||||
<span class="n">ax</span><span class="o">.</span><span class="n">set_ylim</span><span class="p">(</span><span class="n">mid_y</span> <span class="o">-</span> <span class="n">max_range</span><span class="p">,</span> <span class="n">mid_y</span> <span class="o">+</span> <span class="n">max_range</span><span class="p">)</span>
|
||||
<span class="n">ax</span><span class="o">.</span><span class="n">set_zlim</span><span class="p">(</span><span class="n">mid_z</span> <span class="o">-</span> <span class="n">max_range</span><span class="p">,</span> <span class="n">mid_z</span> <span class="o">+</span> <span class="n">max_range</span><span class="p">)</span>
|
||||
</pre></div>
|
||||
|
||||
</div>
|
||||
|
||||
@@ -13,4 +13,4 @@ Mapping
|
||||
k_means_object_clustering/k_means_object_clustering
|
||||
circle_fitting/circle_fitting
|
||||
rectangle_fitting/rectangle_fitting
|
||||
|
||||
normal_vector_estimation/normal_vector_estimation
|
||||
|
||||
@@ -0,0 +1,74 @@
|
||||
Normal vector estimation
|
||||
-------------------------
|
||||
|
||||
|
||||
Normal vector calculation of a 3D triangle
|
||||
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
||||
|
||||
A 3D point is as a vector:
|
||||
|
||||
.. math:: p = [x, y, z]
|
||||
|
||||
When there are 3 points in 3D space, :math:`p_1, p_2, p_3`,
|
||||
|
||||
we can calculate a normal vector n of a 3D triangle which is consisted of the points.
|
||||
|
||||
.. math:: n = \frac{v1 \times v2}{|v1 \times v2|}
|
||||
|
||||
where
|
||||
|
||||
.. math:: v1 = p2 - p1
|
||||
|
||||
.. math:: v2 = p3 - p1
|
||||
|
||||
This is an example of normal vector calculation:
|
||||
|
||||
.. figure:: normal_vector_calc.png
|
||||
|
||||
API
|
||||
=====
|
||||
|
||||
.. autofunction:: Mapping.normal_vector_estimation.normal_vector_estimation.calc_normal_vector
|
||||
|
||||
Normal vector estimation with RANdam SAmpling Consensus(RANSAC)
|
||||
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
||||
|
||||
Consider the problem of estimating the normal vector of a plane based on a
|
||||
set of N 3D points where a plane can be observed.
|
||||
|
||||
There is a way that uses all point cloud data to estimate a plane and
|
||||
a normal vector using the `least-squares method <https://stackoverflow.com/a/44315221/8387766>`_
|
||||
|
||||
However, this method is vulnerable to noise of the point cloud.
|
||||
|
||||
In this document, we will use a method that uses
|
||||
`RANdam SAmpling Consensus(RANSAC) <https://en.wikipedia.org/wiki/Random_sample_consensus>`_
|
||||
to estimate a plane and a normal vector.
|
||||
|
||||
RANSAC is a robust estimation methods for data set with outliers.
|
||||
|
||||
|
||||
|
||||
This RANSAC based normal vector estimation method is as follows:
|
||||
|
||||
#. Select 3 points randomly from the point cloud.
|
||||
|
||||
#. Calculate a normal vector of a plane which is consists of the sampled 3 points.
|
||||
|
||||
#. Calculate the distance between the calculated plane and the all point cloud.
|
||||
|
||||
#. If the distance is less than a threshold, the point is considered to be an inlier.
|
||||
|
||||
#. Repeat the above steps until the inlier ratio is greater than a threshold.
|
||||
|
||||
|
||||
|
||||
This is an example of RANSAC based normal vector estimation:
|
||||
|
||||
.. figure:: ransac_normal_vector_estimation.png
|
||||
|
||||
API
|
||||
=====
|
||||
|
||||
.. autofunction:: Mapping.normal_vector_estimation.normal_vector_estimation.ransac_normal_vector_estimation
|
||||
|
||||
@@ -102,6 +102,7 @@
|
||||
| <a href="#F"><strong>F</strong></a>
|
||||
| <a href="#I"><strong>I</strong></a>
|
||||
| <a href="#P"><strong>P</strong></a>
|
||||
| <a href="#R"><strong>R</strong></a>
|
||||
| <a href="#V"><strong>V</strong></a>
|
||||
|
||||
</div>
|
||||
@@ -119,6 +120,8 @@
|
||||
<li><a href="modules/path_planning/cubic_spline/cubic_spline.html#PathPlanning.CubicSpline.cubic_spline_planner.CubicSpline2D.calc_curvature">calc_curvature() (PathPlanning.CubicSpline.cubic_spline_planner.CubicSpline2D method)</a>
|
||||
</li>
|
||||
<li><a href="modules/path_planning/cubic_spline/cubic_spline.html#PathPlanning.CubicSpline.cubic_spline_planner.CubicSpline1D.calc_first_derivative">calc_first_derivative() (PathPlanning.CubicSpline.cubic_spline_planner.CubicSpline1D method)</a>
|
||||
</li>
|
||||
<li><a href="modules/mapping/normal_vector_estimation/normal_vector_estimation.html#Mapping.normal_vector_estimation.normal_vector_estimation.calc_normal_vector">calc_normal_vector() (in module Mapping.normal_vector_estimation.normal_vector_estimation)</a>
|
||||
</li>
|
||||
<li><a href="modules/path_planning/cubic_spline/cubic_spline.html#PathPlanning.CubicSpline.cubic_spline_planner.CubicSpline1D.calc_position">calc_position() (PathPlanning.CubicSpline.cubic_spline_planner.CubicSpline1D method)</a>
|
||||
|
||||
@@ -169,6 +172,14 @@
|
||||
</ul></td>
|
||||
</tr></table>
|
||||
|
||||
<h2 id="R">R</h2>
|
||||
<table style="width: 100%" class="indextable genindextable"><tr>
|
||||
<td style="width: 33%; vertical-align: top;"><ul>
|
||||
<li><a href="modules/mapping/normal_vector_estimation/normal_vector_estimation.html#Mapping.normal_vector_estimation.normal_vector_estimation.ransac_normal_vector_estimation">ransac_normal_vector_estimation() (in module Mapping.normal_vector_estimation.normal_vector_estimation)</a>
|
||||
</li>
|
||||
</ul></td>
|
||||
</tr></table>
|
||||
|
||||
<h2 id="V">V</h2>
|
||||
<table style="width: 100%" class="indextable genindextable"><tr>
|
||||
<td style="width: 33%; vertical-align: top;"><ul>
|
||||
|
||||
@@ -145,6 +145,7 @@ algorithms</a> (<a class="reference external" href="https://github.com/AtsushiSa
|
||||
<li class="toctree-l2"><a class="reference internal" href="modules/mapping/k_means_object_clustering/k_means_object_clustering.html">k-means object clustering</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="modules/mapping/circle_fitting/circle_fitting.html">Object shape recognition using circle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="modules/mapping/rectangle_fitting/rectangle_fitting.html">Object shape recognition using rectangle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="modules/mapping/normal_vector_estimation/normal_vector_estimation.html">Normal vector estimation</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="modules/slam/slam.html">SLAM</a><ul>
|
||||
|
||||
@@ -67,6 +67,7 @@
|
||||
<li class="toctree-l2"><a class="reference internal" href="../k_means_object_clustering/k_means_object_clustering.html">k-means object clustering</a></li>
|
||||
<li class="toctree-l2 current"><a class="current reference internal" href="#">Object shape recognition using circle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../rectangle_fitting/rectangle_fitting.html">Object shape recognition using rectangle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../normal_vector_estimation/normal_vector_estimation.html">Normal vector estimation</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../slam/slam.html">SLAM</a></li>
|
||||
|
||||
@@ -67,6 +67,7 @@
|
||||
<li class="toctree-l2"><a class="reference internal" href="../k_means_object_clustering/k_means_object_clustering.html">k-means object clustering</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../circle_fitting/circle_fitting.html">Object shape recognition using circle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../rectangle_fitting/rectangle_fitting.html">Object shape recognition using rectangle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../normal_vector_estimation/normal_vector_estimation.html">Normal vector estimation</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../slam/slam.html">SLAM</a></li>
|
||||
|
||||
@@ -67,6 +67,7 @@
|
||||
<li class="toctree-l2 current"><a class="current reference internal" href="#">k-means object clustering</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../circle_fitting/circle_fitting.html">Object shape recognition using circle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../rectangle_fitting/rectangle_fitting.html">Object shape recognition using rectangle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../normal_vector_estimation/normal_vector_estimation.html">Normal vector estimation</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../slam/slam.html">SLAM</a></li>
|
||||
|
||||
@@ -67,6 +67,7 @@
|
||||
<li class="toctree-l2"><a class="reference internal" href="../k_means_object_clustering/k_means_object_clustering.html">k-means object clustering</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../circle_fitting/circle_fitting.html">Object shape recognition using circle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../rectangle_fitting/rectangle_fitting.html">Object shape recognition using rectangle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../normal_vector_estimation/normal_vector_estimation.html">Normal vector estimation</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../slam/slam.html">SLAM</a></li>
|
||||
|
||||
@@ -67,6 +67,7 @@
|
||||
<li class="toctree-l2"><a class="reference internal" href="k_means_object_clustering/k_means_object_clustering.html">k-means object clustering</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="circle_fitting/circle_fitting.html">Object shape recognition using circle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="rectangle_fitting/rectangle_fitting.html">Object shape recognition using rectangle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="normal_vector_estimation/normal_vector_estimation.html">Normal vector estimation</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../slam/slam.html">SLAM</a></li>
|
||||
@@ -122,6 +123,11 @@
|
||||
<li class="toctree-l1"><a class="reference internal" href="k_means_object_clustering/k_means_object_clustering.html">k-means object clustering</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="circle_fitting/circle_fitting.html">Object shape recognition using circle fitting</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="rectangle_fitting/rectangle_fitting.html">Object shape recognition using rectangle fitting</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="normal_vector_estimation/normal_vector_estimation.html">Normal vector estimation</a><ul>
|
||||
<li class="toctree-l2"><a class="reference internal" href="normal_vector_estimation/normal_vector_estimation.html#normal-vector-calculation-of-a-3d-triangle">Normal vector calculation of a 3D triangle</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="normal_vector_estimation/normal_vector_estimation.html#normal-vector-estimation-with-randam-sampling-consensus-ransac">Normal vector estimation with RANdam SAmpling Consensus(RANSAC)</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
</ul>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
@@ -0,0 +1,253 @@
|
||||
<!DOCTYPE html>
|
||||
<html class="writer-html5" lang="en" >
|
||||
<head>
|
||||
<meta charset="utf-8" /><meta name="generator" content="Docutils 0.17.1: http://docutils.sourceforge.net/" />
|
||||
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0" />
|
||||
<title>Normal vector estimation — PythonRobotics documentation</title>
|
||||
<link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
|
||||
<link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
|
||||
<link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
|
||||
<link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
|
||||
<!--[if lt IE 9]>
|
||||
<script src="../../../_static/js/html5shiv.min.js"></script>
|
||||
<![endif]-->
|
||||
|
||||
<script data-url_root="../../../" id="documentation_options" src="../../../_static/documentation_options.js"></script>
|
||||
<script src="../../../_static/jquery.js"></script>
|
||||
<script src="../../../_static/underscore.js"></script>
|
||||
<script src="../../../_static/doctools.js"></script>
|
||||
<script async="async" src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-mml-chtml.js"></script>
|
||||
<script src="../../../_static/js/theme.js"></script>
|
||||
<link rel="index" title="Index" href="../../../genindex.html" />
|
||||
<link rel="search" title="Search" href="../../../search.html" />
|
||||
<link rel="next" title="SLAM" href="../../slam/slam.html" />
|
||||
<link rel="prev" title="Object shape recognition using rectangle fitting" href="../rectangle_fitting/rectangle_fitting.html" />
|
||||
</head>
|
||||
|
||||
<body class="wy-body-for-nav">
|
||||
<div class="wy-grid-for-nav">
|
||||
<nav data-toggle="wy-nav-shift" class="wy-nav-side">
|
||||
<div class="wy-side-scroll">
|
||||
<div class="wy-side-nav-search" >
|
||||
|
||||
<a href="../../../index.html" class="icon icon-home"> PythonRobotics
|
||||
<img src="../../../_static/icon.png" class="logo" alt="Logo"/>
|
||||
</a>
|
||||
<div role="search">
|
||||
<form id="rtd-search-form" class="wy-form" action="../../../search.html" method="get">
|
||||
<input type="text" name="q" placeholder="Search docs" />
|
||||
<input type="hidden" name="check_keywords" value="yes" />
|
||||
<input type="hidden" name="area" value="default" />
|
||||
</form>
|
||||
</div>
|
||||
<script async src="https://pagead2.googlesyndication.com/pagead/js/adsbygoogle.js?client=ca-pub-9612347954373886"
|
||||
crossorigin="anonymous"></script>
|
||||
<!-- PythonRoboticsDoc -->
|
||||
<ins class="adsbygoogle"
|
||||
style="display:block"
|
||||
data-ad-client="ca-pub-9612347954373886"
|
||||
data-ad-slot="1579532132"
|
||||
data-ad-format="auto"
|
||||
data-full-width-responsive="true"></ins>
|
||||
<script>
|
||||
(adsbygoogle = window.adsbygoogle || []).push({});
|
||||
</script>
|
||||
|
||||
</div><div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="Navigation menu">
|
||||
<p class="caption" role="heading"><span class="caption-text">Contents</span></p>
|
||||
<ul class="current">
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../getting_started.html">Getting Started</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../introduction.html">Introduction</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../localization/localization.html">Localization</a></li>
|
||||
<li class="toctree-l1 current"><a class="reference internal" href="../mapping.html">Mapping</a><ul class="current">
|
||||
<li class="toctree-l2"><a class="reference internal" href="../gaussian_grid_map/gaussian_grid_map.html">Gaussian grid map</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../ray_casting_grid_map/ray_casting_grid_map.html">Ray casting grid map</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial.html">Lidar to grid map</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../point_cloud_sampling/point_cloud_sampling.html">Point cloud Sampling</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../k_means_object_clustering/k_means_object_clustering.html">k-means object clustering</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../circle_fitting/circle_fitting.html">Object shape recognition using circle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../rectangle_fitting/rectangle_fitting.html">Object shape recognition using rectangle fitting</a></li>
|
||||
<li class="toctree-l2 current"><a class="current reference internal" href="#">Normal vector estimation</a><ul>
|
||||
<li class="toctree-l3"><a class="reference internal" href="#normal-vector-calculation-of-a-3d-triangle">Normal vector calculation of a 3D triangle</a><ul>
|
||||
<li class="toctree-l4"><a class="reference internal" href="#api">API</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li class="toctree-l3"><a class="reference internal" href="#normal-vector-estimation-with-randam-sampling-consensus-ransac">Normal vector estimation with RANdam SAmpling Consensus(RANSAC)</a><ul>
|
||||
<li class="toctree-l4"><a class="reference internal" href="#id1">API</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
</ul>
|
||||
</li>
|
||||
</ul>
|
||||
</li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../slam/slam.html">SLAM</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../path_planning/path_planning.html">Path Planning</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../path_tracking/path_tracking.html">Path Tracking</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../arm_navigation/arm_navigation.html">Arm Navigation</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../aerial_navigation/aerial_navigation.html">Aerial Navigation</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../bipedal/bipedal.html">Bipedal</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../control/control.html">Control</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../utils/utils.html">Utilities</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../appendix/appendix.html">Appendix</a></li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../../how_to_contribute.html">How To Contribute</a></li>
|
||||
</ul>
|
||||
|
||||
</div>
|
||||
</div>
|
||||
</nav>
|
||||
|
||||
<section data-toggle="wy-nav-shift" class="wy-nav-content-wrap"><nav class="wy-nav-top" aria-label="Mobile navigation menu" >
|
||||
<i data-toggle="wy-nav-top" class="fa fa-bars"></i>
|
||||
<a href="../../../index.html">PythonRobotics</a>
|
||||
</nav>
|
||||
|
||||
<div class="wy-nav-content">
|
||||
<div class="rst-content">
|
||||
<div role="navigation" aria-label="Page navigation">
|
||||
<ul class="wy-breadcrumbs">
|
||||
<li><a href="../../../index.html" class="icon icon-home"></a> »</li>
|
||||
<li><a href="../mapping.html">Mapping</a> »</li>
|
||||
<li>Normal vector estimation</li>
|
||||
<li class="wy-breadcrumbs-aside">
|
||||
<a href="https://github.com/AtsushiSakai/PythonRobotics/blob/master/docs/modules/mapping/normal_vector_estimation/normal_vector_estimation_main.rst" class="fa fa-github"> Edit on GitHub</a>
|
||||
</li>
|
||||
</ul>
|
||||
<hr/>
|
||||
</div>
|
||||
<div role="main" class="document" itemscope="itemscope" itemtype="http://schema.org/Article">
|
||||
<div itemprop="articleBody">
|
||||
|
||||
<section id="normal-vector-estimation">
|
||||
<h1>Normal vector estimation<a class="headerlink" href="#normal-vector-estimation" title="Permalink to this headline"></a></h1>
|
||||
<section id="normal-vector-calculation-of-a-3d-triangle">
|
||||
<h2>Normal vector calculation of a 3D triangle<a class="headerlink" href="#normal-vector-calculation-of-a-3d-triangle" title="Permalink to this headline"></a></h2>
|
||||
<p>A 3D point is as a vector:</p>
|
||||
<div class="math notranslate nohighlight">
|
||||
\[p = [x, y, z]\]</div>
|
||||
<p>When there are 3 points in 3D space, <span class="math notranslate nohighlight">\(p_1, p_2, p_3\)</span>,</p>
|
||||
<p>we can calculate a normal vector n of a 3D triangle which is consisted of the points.</p>
|
||||
<div class="math notranslate nohighlight">
|
||||
\[n = \frac{v1 \times v2}{|v1 \times v2|}\]</div>
|
||||
<p>where</p>
|
||||
<div class="math notranslate nohighlight">
|
||||
\[v1 = p2 - p1\]</div>
|
||||
<div class="math notranslate nohighlight">
|
||||
\[v2 = p3 - p1\]</div>
|
||||
<p>This is an example of normal vector calculation:</p>
|
||||
<figure class="align-default">
|
||||
<img alt="../../../_images/normal_vector_calc.png" src="../../../_images/normal_vector_calc.png" />
|
||||
</figure>
|
||||
<section id="api">
|
||||
<h3>API<a class="headerlink" href="#api" title="Permalink to this headline"></a></h3>
|
||||
<dl class="py function">
|
||||
<dt class="sig sig-object py" id="Mapping.normal_vector_estimation.normal_vector_estimation.calc_normal_vector">
|
||||
<span class="sig-prename descclassname"><span class="pre">Mapping.normal_vector_estimation.normal_vector_estimation.</span></span><span class="sig-name descname"><span class="pre">calc_normal_vector</span></span><span class="sig-paren">(</span><em class="sig-param"><span class="n"><span class="pre">p1</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">p2</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">p3</span></span></em><span class="sig-paren">)</span><a class="reference internal" href="../../../_modules/Mapping/normal_vector_estimation/normal_vector_estimation.html#calc_normal_vector"><span class="viewcode-link"><span class="pre">[source]</span></span></a><a class="headerlink" href="#Mapping.normal_vector_estimation.normal_vector_estimation.calc_normal_vector" title="Permalink to this definition"></a></dt>
|
||||
<dd><p>Calculate normal vector of triangle</p>
|
||||
<dl class="field-list simple">
|
||||
<dt class="field-odd">Parameters</dt>
|
||||
<dd class="field-odd"><ul class="simple">
|
||||
<li><p><strong>p1</strong> (<em>np.array</em>) – 3D point</p></li>
|
||||
<li><p><strong>p2</strong> (<em>np.array</em>) – 3D point</p></li>
|
||||
<li><p><strong>p3</strong> (<em>np.array</em>) – 3D point</p></li>
|
||||
</ul>
|
||||
</dd>
|
||||
<dt class="field-even">Returns</dt>
|
||||
<dd class="field-even"><p><strong>normal_vector</strong> – normal vector (3,)</p>
|
||||
</dd>
|
||||
<dt class="field-odd">Return type</dt>
|
||||
<dd class="field-odd"><p>np.array</p>
|
||||
</dd>
|
||||
</dl>
|
||||
</dd></dl>
|
||||
|
||||
</section>
|
||||
</section>
|
||||
<section id="normal-vector-estimation-with-randam-sampling-consensus-ransac">
|
||||
<h2>Normal vector estimation with RANdam SAmpling Consensus(RANSAC)<a class="headerlink" href="#normal-vector-estimation-with-randam-sampling-consensus-ransac" title="Permalink to this headline"></a></h2>
|
||||
<p>Consider the problem of estimating the normal vector of a plane based on a
|
||||
set of N 3D points where a plane can be observed.</p>
|
||||
<p>There is a way that uses all point cloud data to estimate a plane and
|
||||
a normal vector using the <a class="reference external" href="https://stackoverflow.com/a/44315221/8387766">least-squares method</a></p>
|
||||
<p>However, this method is vulnerable to noise of the point cloud.</p>
|
||||
<p>In this document, we will use a method that uses
|
||||
<a class="reference external" href="https://en.wikipedia.org/wiki/Random_sample_consensus">RANdam SAmpling Consensus(RANSAC)</a>
|
||||
to estimate a plane and a normal vector.</p>
|
||||
<p>RANSAC is a robust estimation methods for data set with outliers.</p>
|
||||
<p>This RANSAC based normal vector estimation method is as follows:</p>
|
||||
<ol class="arabic simple">
|
||||
<li><p>Select 3 points randomly from the point cloud.</p></li>
|
||||
<li><p>Calculate a normal vector of a plane which is consists of the sampled 3 points.</p></li>
|
||||
<li><p>Calculate the distance between the calculated plane and the all point cloud.</p></li>
|
||||
<li><p>If the distance is less than a threshold, the point is considered to be an inlier.</p></li>
|
||||
<li><p>Repeat the above steps until the inlier ratio is greater than a threshold.</p></li>
|
||||
</ol>
|
||||
<p>This is an example of RANSAC based normal vector estimation:</p>
|
||||
<figure class="align-default">
|
||||
<img alt="../../../_images/ransac_normal_vector_estimation.png" src="../../../_images/ransac_normal_vector_estimation.png" />
|
||||
</figure>
|
||||
<section id="id1">
|
||||
<h3>API<a class="headerlink" href="#id1" title="Permalink to this headline"></a></h3>
|
||||
<dl class="py function">
|
||||
<dt class="sig sig-object py" id="Mapping.normal_vector_estimation.normal_vector_estimation.ransac_normal_vector_estimation">
|
||||
<span class="sig-prename descclassname"><span class="pre">Mapping.normal_vector_estimation.normal_vector_estimation.</span></span><span class="sig-name descname"><span class="pre">ransac_normal_vector_estimation</span></span><span class="sig-paren">(</span><em class="sig-param"><span class="n"><span class="pre">points_3d</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">inlier_radio_th</span></span><span class="o"><span class="pre">=</span></span><span class="default_value"><span class="pre">0.7</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">inlier_dist</span></span><span class="o"><span class="pre">=</span></span><span class="default_value"><span class="pre">0.1</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">p</span></span><span class="o"><span class="pre">=</span></span><span class="default_value"><span class="pre">0.99</span></span></em><span class="sig-paren">)</span><a class="reference internal" href="../../../_modules/Mapping/normal_vector_estimation/normal_vector_estimation.html#ransac_normal_vector_estimation"><span class="viewcode-link"><span class="pre">[source]</span></span></a><a class="headerlink" href="#Mapping.normal_vector_estimation.normal_vector_estimation.ransac_normal_vector_estimation" title="Permalink to this definition"></a></dt>
|
||||
<dd><p>RANSAC based normal vector estimation</p>
|
||||
<dl class="field-list simple">
|
||||
<dt class="field-odd">Parameters</dt>
|
||||
<dd class="field-odd"><ul class="simple">
|
||||
<li><p><strong>points_3d</strong> (<em>np.array</em>) – 3D points (N, 3)</p></li>
|
||||
<li><p><strong>inlier_radio_th</strong> (<em>float</em>) – Inlier ratio threshold. If inlier ratio is larger than this value,
|
||||
the iteration is stopped. Default is 0.7.</p></li>
|
||||
<li><p><strong>inlier_dist</strong> (<em>float</em>) – Inlier distance threshold. If distance between points and estimated
|
||||
plane is smaller than this value, the point is inlier. Default is 0.1.</p></li>
|
||||
<li><p><strong>p</strong> (<em>float</em>) – Probability that at least one of the sets of random samples does not
|
||||
include an outlier. If this probability is near 1, the iteration
|
||||
number is large. Default is 0.99.</p></li>
|
||||
</ul>
|
||||
</dd>
|
||||
<dt class="field-even">Returns</dt>
|
||||
<dd class="field-even"><p><ul class="simple">
|
||||
<li><p><strong>center_vector</strong> (<em>np.array</em>) – Center of estimated plane. (3,)</p></li>
|
||||
<li><p><strong>normal_vector</strong> (<em>np.array</em>) – Normal vector of estimated plane. (3,)</p></li>
|
||||
</ul>
|
||||
</p>
|
||||
</dd>
|
||||
</dl>
|
||||
</dd></dl>
|
||||
|
||||
</section>
|
||||
</section>
|
||||
</section>
|
||||
|
||||
|
||||
</div>
|
||||
</div>
|
||||
<footer><div class="rst-footer-buttons" role="navigation" aria-label="Footer">
|
||||
<a href="../rectangle_fitting/rectangle_fitting.html" class="btn btn-neutral float-left" title="Object shape recognition using rectangle fitting" accesskey="p" rel="prev"><span class="fa fa-arrow-circle-left" aria-hidden="true"></span> Previous</a>
|
||||
<a href="../../slam/slam.html" class="btn btn-neutral float-right" title="SLAM" accesskey="n" rel="next">Next <span class="fa fa-arrow-circle-right" aria-hidden="true"></span></a>
|
||||
</div>
|
||||
|
||||
<hr/>
|
||||
|
||||
<div role="contentinfo">
|
||||
<p>© Copyright 2018-2021, Atsushi Sakai.</p>
|
||||
</div>
|
||||
|
||||
Built with <a href="https://www.sphinx-doc.org/">Sphinx</a> using a
|
||||
<a href="https://github.com/readthedocs/sphinx_rtd_theme">theme</a>
|
||||
provided by <a href="https://readthedocs.org">Read the Docs</a>.
|
||||
|
||||
|
||||
</footer>
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
</div>
|
||||
<script>
|
||||
jQuery(function () {
|
||||
SphinxRtdTheme.Navigation.enable(true);
|
||||
});
|
||||
</script>
|
||||
|
||||
</body>
|
||||
</html>
|
||||
@@ -81,6 +81,7 @@
|
||||
<li class="toctree-l2"><a class="reference internal" href="../k_means_object_clustering/k_means_object_clustering.html">k-means object clustering</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../circle_fitting/circle_fitting.html">Object shape recognition using circle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../rectangle_fitting/rectangle_fitting.html">Object shape recognition using rectangle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../normal_vector_estimation/normal_vector_estimation.html">Normal vector estimation</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../slam/slam.html">SLAM</a></li>
|
||||
|
||||
@@ -67,6 +67,7 @@
|
||||
<li class="toctree-l2"><a class="reference internal" href="../k_means_object_clustering/k_means_object_clustering.html">k-means object clustering</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../circle_fitting/circle_fitting.html">Object shape recognition using circle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../rectangle_fitting/rectangle_fitting.html">Object shape recognition using rectangle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../normal_vector_estimation/normal_vector_estimation.html">Normal vector estimation</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../slam/slam.html">SLAM</a></li>
|
||||
|
||||
@@ -20,7 +20,7 @@
|
||||
<script src="../../../_static/js/theme.js"></script>
|
||||
<link rel="index" title="Index" href="../../../genindex.html" />
|
||||
<link rel="search" title="Search" href="../../../search.html" />
|
||||
<link rel="next" title="SLAM" href="../../slam/slam.html" />
|
||||
<link rel="next" title="Normal vector estimation" href="../normal_vector_estimation/normal_vector_estimation.html" />
|
||||
<link rel="prev" title="Object shape recognition using circle fitting" href="../circle_fitting/circle_fitting.html" />
|
||||
</head>
|
||||
|
||||
@@ -67,6 +67,7 @@
|
||||
<li class="toctree-l2"><a class="reference internal" href="../k_means_object_clustering/k_means_object_clustering.html">k-means object clustering</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../circle_fitting/circle_fitting.html">Object shape recognition using circle fitting</a></li>
|
||||
<li class="toctree-l2 current"><a class="current reference internal" href="#">Object shape recognition using rectangle fitting</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../normal_vector_estimation/normal_vector_estimation.html">Normal vector estimation</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li class="toctree-l1"><a class="reference internal" href="../../slam/slam.html">SLAM</a></li>
|
||||
@@ -117,7 +118,7 @@
|
||||
</div>
|
||||
<footer><div class="rst-footer-buttons" role="navigation" aria-label="Footer">
|
||||
<a href="../circle_fitting/circle_fitting.html" class="btn btn-neutral float-left" title="Object shape recognition using circle fitting" accesskey="p" rel="prev"><span class="fa fa-arrow-circle-left" aria-hidden="true"></span> Previous</a>
|
||||
<a href="../../slam/slam.html" class="btn btn-neutral float-right" title="SLAM" accesskey="n" rel="next">Next <span class="fa fa-arrow-circle-right" aria-hidden="true"></span></a>
|
||||
<a href="../normal_vector_estimation/normal_vector_estimation.html" class="btn btn-neutral float-right" title="Normal vector estimation" accesskey="n" rel="next">Next <span class="fa fa-arrow-circle-right" aria-hidden="true"></span></a>
|
||||
</div>
|
||||
|
||||
<hr/>
|
||||
|
||||
@@ -21,7 +21,7 @@
|
||||
<link rel="index" title="Index" href="../../genindex.html" />
|
||||
<link rel="search" title="Search" href="../../search.html" />
|
||||
<link rel="next" title="Iterative Closest Point (ICP) Matching" href="iterative_closest_point_matching/iterative_closest_point_matching.html" />
|
||||
<link rel="prev" title="Object shape recognition using rectangle fitting" href="../mapping/rectangle_fitting/rectangle_fitting.html" />
|
||||
<link rel="prev" title="Normal vector estimation" href="../mapping/normal_vector_estimation/normal_vector_estimation.html" />
|
||||
</head>
|
||||
|
||||
<body class="wy-body-for-nav">
|
||||
@@ -152,7 +152,7 @@
|
||||
</div>
|
||||
</div>
|
||||
<footer><div class="rst-footer-buttons" role="navigation" aria-label="Footer">
|
||||
<a href="../mapping/rectangle_fitting/rectangle_fitting.html" class="btn btn-neutral float-left" title="Object shape recognition using rectangle fitting" accesskey="p" rel="prev"><span class="fa fa-arrow-circle-left" aria-hidden="true"></span> Previous</a>
|
||||
<a href="../mapping/normal_vector_estimation/normal_vector_estimation.html" class="btn btn-neutral float-left" title="Normal vector estimation" accesskey="p" rel="prev"><span class="fa fa-arrow-circle-left" aria-hidden="true"></span> Previous</a>
|
||||
<a href="iterative_closest_point_matching/iterative_closest_point_matching.html" class="btn btn-neutral float-right" title="Iterative Closest Point (ICP) Matching" accesskey="n" rel="next">Next <span class="fa fa-arrow-circle-right" aria-hidden="true"></span></a>
|
||||
</div>
|
||||
|
||||
|
||||
BIN
objects.inv
BIN
objects.inv
Binary file not shown.
File diff suppressed because one or more lines are too long
Reference in New Issue
Block a user