mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:41 -04:00
Add changes for 67edaf8b02
This commit is contained in:
GitHub Action
424
_modules/Mapping/rectangle_fitting/rectangle_fitting.html
Normal file
424
_modules/Mapping/rectangle_fitting/rectangle_fitting.html
Normal file
@@ -0,0 +1,424 @@
|
||||
<!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.rectangle_fitting.rectangle_fitting — 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/copybutton.css" type="text/css" />
|
||||
<link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
|
||||
<link rel="stylesheet" href="../../../_static/dark_mode_css/general.css" type="text/css" />
|
||||
<link rel="stylesheet" href="../../../_static/dark_mode_css/dark.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/clipboard.min.js"></script>
|
||||
<script src="../../../_static/copybutton.js"></script>
|
||||
<script src="../../../_static/dark_mode_js/default_dark.js"></script>
|
||||
<script src="../../../_static/dark_mode_js/theme_switcher.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.rectangle_fitting.rectangle_fitting</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.rectangle_fitting.rectangle_fitting</h1><div class="highlight"><pre>
|
||||
<span></span><span class="sd">"""</span>
|
||||
|
||||
<span class="sd">Object shape recognition with L-shape fitting</span>
|
||||
|
||||
<span class="sd">author: Atsushi Sakai (@Atsushi_twi)</span>
|
||||
|
||||
<span class="sd">Ref:</span>
|
||||
<span class="sd">- Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners -</span>
|
||||
<span class="sd">The Robotics Institute Carnegie Mellon University</span>
|
||||
<span class="sd">https://www.ri.cmu.edu/publications/</span>
|
||||
<span class="sd">efficient-l-shape-fitting-for-vehicle-detection-using-laser-scanners/</span>
|
||||
|
||||
<span class="sd">"""</span>
|
||||
|
||||
<span class="kn">import</span> <span class="nn">sys</span>
|
||||
<span class="kn">import</span> <span class="nn">pathlib</span>
|
||||
<span class="n">sys</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="nb">str</span><span class="p">(</span><span class="n">pathlib</span><span class="o">.</span><span class="n">Path</span><span class="p">(</span><span class="vm">__file__</span><span class="p">)</span><span class="o">.</span><span class="n">parent</span><span class="o">.</span><span class="n">parent</span><span class="o">.</span><span class="n">parent</span><span class="p">))</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">import</span> <span class="nn">itertools</span>
|
||||
<span class="kn">from</span> <span class="nn">enum</span> <span class="kn">import</span> <span class="n">Enum</span>
|
||||
|
||||
<span class="kn">from</span> <span class="nn">utils.angle</span> <span class="kn">import</span> <span class="n">rot_mat_2d</span>
|
||||
|
||||
<span class="kn">from</span> <span class="nn">Mapping.rectangle_fitting.simulator</span> \
|
||||
<span class="kn">import</span> <span class="nn">VehicleSimulator</span><span class="o">,</span> <span class="nn">LidarSimulator</span>
|
||||
|
||||
<span class="n">show_animation</span> <span class="o">=</span> <span class="kc">True</span>
|
||||
|
||||
|
||||
<div class="viewcode-block" id="LShapeFitting"><a class="viewcode-back" href="../../../modules/mapping/rectangle_fitting/rectangle_fitting.html#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting">[docs]</a><span class="k">class</span> <span class="nc">LShapeFitting</span><span class="p">:</span>
|
||||
<span class="w"> </span><span class="sd">"""</span>
|
||||
<span class="sd"> LShapeFitting class. You can use this class by initializing the class and</span>
|
||||
<span class="sd"> changing the parameters, and then calling the fitting method.</span>
|
||||
|
||||
<span class="sd"> """</span>
|
||||
|
||||
<div class="viewcode-block" id="LShapeFitting.Criteria"><a class="viewcode-back" href="../../../modules/mapping/rectangle_fitting/rectangle_fitting.html#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.Criteria">[docs]</a> <span class="k">class</span> <span class="nc">Criteria</span><span class="p">(</span><span class="n">Enum</span><span class="p">):</span>
|
||||
<span class="n">AREA</span> <span class="o">=</span> <span class="mi">1</span>
|
||||
<span class="n">CLOSENESS</span> <span class="o">=</span> <span class="mi">2</span>
|
||||
<span class="n">VARIANCE</span> <span class="o">=</span> <span class="mi">3</span></div>
|
||||
|
||||
<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="w"> </span><span class="sd">"""</span>
|
||||
<span class="sd"> Default parameter settings</span>
|
||||
<span class="sd"> """</span>
|
||||
<span class="c1">#: Fitting criteria parameter</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">criteria</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">Criteria</span><span class="o">.</span><span class="n">VARIANCE</span>
|
||||
<span class="c1">#: Minimum distance for closeness criteria parameter [m]</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">min_dist_of_closeness_criteria</span> <span class="o">=</span> <span class="mf">0.01</span>
|
||||
<span class="c1">#: Angle difference parameter [deg]</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">d_theta_deg_for_search</span> <span class="o">=</span> <span class="mf">1.0</span>
|
||||
<span class="c1">#: Range segmentation parameter [m]</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">R0</span> <span class="o">=</span> <span class="mf">3.0</span>
|
||||
<span class="c1">#: Range segmentation parameter [m]</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">Rd</span> <span class="o">=</span> <span class="mf">0.001</span>
|
||||
|
||||
<div class="viewcode-block" id="LShapeFitting.fitting"><a class="viewcode-back" href="../../../modules/mapping/rectangle_fitting/rectangle_fitting.html#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.fitting">[docs]</a> <span class="k">def</span> <span class="nf">fitting</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">ox</span><span class="p">,</span> <span class="n">oy</span><span class="p">):</span>
|
||||
<span class="w"> </span><span class="sd">"""</span>
|
||||
<span class="sd"> Fitting L-shape model to object points</span>
|
||||
|
||||
<span class="sd"> Parameters</span>
|
||||
<span class="sd"> ----------</span>
|
||||
<span class="sd"> ox : x positions of range points from an object</span>
|
||||
<span class="sd"> oy : y positions of range points from an object</span>
|
||||
|
||||
<span class="sd"> Returns</span>
|
||||
<span class="sd"> -------</span>
|
||||
<span class="sd"> rects: Fitting rectangles</span>
|
||||
<span class="sd"> id_sets: id sets of each cluster</span>
|
||||
|
||||
<span class="sd"> """</span>
|
||||
<span class="c1"># step1: Adaptive Range Segmentation</span>
|
||||
<span class="n">id_sets</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_adoptive_range_segmentation</span><span class="p">(</span><span class="n">ox</span><span class="p">,</span> <span class="n">oy</span><span class="p">)</span>
|
||||
|
||||
<span class="c1"># step2 Rectangle search</span>
|
||||
<span class="n">rects</span> <span class="o">=</span> <span class="p">[]</span>
|
||||
<span class="k">for</span> <span class="n">ids</span> <span class="ow">in</span> <span class="n">id_sets</span><span class="p">:</span> <span class="c1"># for each cluster</span>
|
||||
<span class="n">cx</span> <span class="o">=</span> <span class="p">[</span><span class="n">ox</span><span class="p">[</span><span class="n">i</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">ox</span><span class="p">))</span> <span class="k">if</span> <span class="n">i</span> <span class="ow">in</span> <span class="n">ids</span><span class="p">]</span>
|
||||
<span class="n">cy</span> <span class="o">=</span> <span class="p">[</span><span class="n">oy</span><span class="p">[</span><span class="n">i</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">oy</span><span class="p">))</span> <span class="k">if</span> <span class="n">i</span> <span class="ow">in</span> <span class="n">ids</span><span class="p">]</span>
|
||||
<span class="n">rects</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">_rectangle_search</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="k">return</span> <span class="n">rects</span><span class="p">,</span> <span class="n">id_sets</span></div>
|
||||
|
||||
<span class="nd">@staticmethod</span>
|
||||
<span class="k">def</span> <span class="nf">_calc_area_criterion</span><span class="p">(</span><span class="n">c1</span><span class="p">,</span> <span class="n">c2</span><span class="p">):</span>
|
||||
<span class="n">c1_max</span><span class="p">,</span> <span class="n">c1_min</span><span class="p">,</span> <span class="n">c2_max</span><span class="p">,</span> <span class="n">c2_min</span> <span class="o">=</span> <span class="n">LShapeFitting</span><span class="o">.</span><span class="n">_find_min_max</span><span class="p">(</span><span class="n">c1</span><span class="p">,</span> <span class="n">c2</span><span class="p">)</span>
|
||||
<span class="n">alpha</span> <span class="o">=</span> <span class="o">-</span><span class="p">(</span><span class="n">c1_max</span> <span class="o">-</span> <span class="n">c1_min</span><span class="p">)</span> <span class="o">*</span> <span class="p">(</span><span class="n">c2_max</span> <span class="o">-</span> <span class="n">c2_min</span><span class="p">)</span>
|
||||
<span class="k">return</span> <span class="n">alpha</span>
|
||||
|
||||
<span class="k">def</span> <span class="nf">_calc_closeness_criterion</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">c1</span><span class="p">,</span> <span class="n">c2</span><span class="p">):</span>
|
||||
<span class="n">c1_max</span><span class="p">,</span> <span class="n">c1_min</span><span class="p">,</span> <span class="n">c2_max</span><span class="p">,</span> <span class="n">c2_min</span> <span class="o">=</span> <span class="n">LShapeFitting</span><span class="o">.</span><span class="n">_find_min_max</span><span class="p">(</span><span class="n">c1</span><span class="p">,</span> <span class="n">c2</span><span class="p">)</span>
|
||||
|
||||
<span class="c1"># Vectorization</span>
|
||||
<span class="n">d1</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">minimum</span><span class="p">(</span><span class="n">c1_max</span> <span class="o">-</span> <span class="n">c1</span><span class="p">,</span> <span class="n">c1</span> <span class="o">-</span> <span class="n">c1_min</span><span class="p">)</span>
|
||||
<span class="n">d2</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">minimum</span><span class="p">(</span><span class="n">c2_max</span> <span class="o">-</span> <span class="n">c2</span><span class="p">,</span> <span class="n">c2</span> <span class="o">-</span> <span class="n">c2_min</span><span class="p">)</span>
|
||||
<span class="n">d</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">maximum</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">minimum</span><span class="p">(</span><span class="n">d1</span><span class="p">,</span> <span class="n">d2</span><span class="p">),</span> <span class="bp">self</span><span class="o">.</span><span class="n">min_dist_of_closeness_criteria</span><span class="p">)</span>
|
||||
<span class="n">beta</span> <span class="o">=</span> <span class="p">(</span><span class="mf">1.0</span> <span class="o">/</span> <span class="n">d</span><span class="p">)</span><span class="o">.</span><span class="n">sum</span><span class="p">()</span>
|
||||
|
||||
<span class="k">return</span> <span class="n">beta</span>
|
||||
|
||||
<span class="nd">@staticmethod</span>
|
||||
<span class="k">def</span> <span class="nf">_calc_variance_criterion</span><span class="p">(</span><span class="n">c1</span><span class="p">,</span> <span class="n">c2</span><span class="p">):</span>
|
||||
<span class="n">c1_max</span><span class="p">,</span> <span class="n">c1_min</span><span class="p">,</span> <span class="n">c2_max</span><span class="p">,</span> <span class="n">c2_min</span> <span class="o">=</span> <span class="n">LShapeFitting</span><span class="o">.</span><span class="n">_find_min_max</span><span class="p">(</span><span class="n">c1</span><span class="p">,</span> <span class="n">c2</span><span class="p">)</span>
|
||||
|
||||
<span class="c1"># Vectorization</span>
|
||||
<span class="n">d1</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">minimum</span><span class="p">(</span><span class="n">c1_max</span> <span class="o">-</span> <span class="n">c1</span><span class="p">,</span> <span class="n">c1</span> <span class="o">-</span> <span class="n">c1_min</span><span class="p">)</span>
|
||||
<span class="n">d2</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">minimum</span><span class="p">(</span><span class="n">c2_max</span> <span class="o">-</span> <span class="n">c2</span><span class="p">,</span> <span class="n">c2</span> <span class="o">-</span> <span class="n">c2_min</span><span class="p">)</span>
|
||||
<span class="n">e1</span> <span class="o">=</span> <span class="n">d1</span><span class="p">[</span><span class="n">d1</span> <span class="o"><</span> <span class="n">d2</span><span class="p">]</span>
|
||||
<span class="n">e2</span> <span class="o">=</span> <span class="n">d2</span><span class="p">[</span><span class="n">d1</span> <span class="o">>=</span> <span class="n">d2</span><span class="p">]</span>
|
||||
<span class="n">v1</span> <span class="o">=</span> <span class="o">-</span> <span class="n">np</span><span class="o">.</span><span class="n">var</span><span class="p">(</span><span class="n">e1</span><span class="p">)</span> <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="n">e1</span><span class="p">)</span> <span class="o">></span> <span class="mi">0</span> <span class="k">else</span> <span class="mf">0.</span>
|
||||
<span class="n">v2</span> <span class="o">=</span> <span class="o">-</span> <span class="n">np</span><span class="o">.</span><span class="n">var</span><span class="p">(</span><span class="n">e2</span><span class="p">)</span> <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="n">e2</span><span class="p">)</span> <span class="o">></span> <span class="mi">0</span> <span class="k">else</span> <span class="mf">0.</span>
|
||||
<span class="n">gamma</span> <span class="o">=</span> <span class="n">v1</span> <span class="o">+</span> <span class="n">v2</span>
|
||||
|
||||
<span class="k">return</span> <span class="n">gamma</span>
|
||||
|
||||
<span class="nd">@staticmethod</span>
|
||||
<span class="k">def</span> <span class="nf">_find_min_max</span><span class="p">(</span><span class="n">c1</span><span class="p">,</span> <span class="n">c2</span><span class="p">):</span>
|
||||
<span class="n">c1_max</span> <span class="o">=</span> <span class="nb">max</span><span class="p">(</span><span class="n">c1</span><span class="p">)</span>
|
||||
<span class="n">c2_max</span> <span class="o">=</span> <span class="nb">max</span><span class="p">(</span><span class="n">c2</span><span class="p">)</span>
|
||||
<span class="n">c1_min</span> <span class="o">=</span> <span class="nb">min</span><span class="p">(</span><span class="n">c1</span><span class="p">)</span>
|
||||
<span class="n">c2_min</span> <span class="o">=</span> <span class="nb">min</span><span class="p">(</span><span class="n">c2</span><span class="p">)</span>
|
||||
<span class="k">return</span> <span class="n">c1_max</span><span class="p">,</span> <span class="n">c1_min</span><span class="p">,</span> <span class="n">c2_max</span><span class="p">,</span> <span class="n">c2_min</span>
|
||||
|
||||
<span class="k">def</span> <span class="nf">_rectangle_search</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">xy</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</span><span class="p">,</span> <span class="n">y</span><span class="p">])</span><span class="o">.</span><span class="n">T</span>
|
||||
|
||||
<span class="n">d_theta</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">deg2rad</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">d_theta_deg_for_search</span><span class="p">)</span>
|
||||
<span class="n">min_cost</span> <span class="o">=</span> <span class="p">(</span><span class="o">-</span><span class="nb">float</span><span class="p">(</span><span class="s1">'inf'</span><span class="p">),</span> <span class="kc">None</span><span class="p">)</span>
|
||||
<span class="k">for</span> <span class="n">theta</span> <span class="ow">in</span> <span class="n">np</span><span class="o">.</span><span class="n">arange</span><span class="p">(</span><span class="mf">0.0</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">pi</span> <span class="o">/</span> <span class="mf">2.0</span> <span class="o">-</span> <span class="n">d_theta</span><span class="p">,</span> <span class="n">d_theta</span><span class="p">):</span>
|
||||
|
||||
<span class="n">c</span> <span class="o">=</span> <span class="n">xy</span> <span class="o">@</span> <span class="n">rot_mat_2d</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>
|
||||
<span class="n">c1</span> <span class="o">=</span> <span class="n">c</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]</span>
|
||||
<span class="n">c2</span> <span class="o">=</span> <span class="n">c</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]</span>
|
||||
|
||||
<span class="c1"># Select criteria</span>
|
||||
<span class="n">cost</span> <span class="o">=</span> <span class="mf">0.0</span>
|
||||
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">criteria</span> <span class="o">==</span> <span class="bp">self</span><span class="o">.</span><span class="n">Criteria</span><span class="o">.</span><span class="n">AREA</span><span class="p">:</span>
|
||||
<span class="n">cost</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_calc_area_criterion</span><span class="p">(</span><span class="n">c1</span><span class="p">,</span> <span class="n">c2</span><span class="p">)</span>
|
||||
<span class="k">elif</span> <span class="bp">self</span><span class="o">.</span><span class="n">criteria</span> <span class="o">==</span> <span class="bp">self</span><span class="o">.</span><span class="n">Criteria</span><span class="o">.</span><span class="n">CLOSENESS</span><span class="p">:</span>
|
||||
<span class="n">cost</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_calc_closeness_criterion</span><span class="p">(</span><span class="n">c1</span><span class="p">,</span> <span class="n">c2</span><span class="p">)</span>
|
||||
<span class="k">elif</span> <span class="bp">self</span><span class="o">.</span><span class="n">criteria</span> <span class="o">==</span> <span class="bp">self</span><span class="o">.</span><span class="n">Criteria</span><span class="o">.</span><span class="n">VARIANCE</span><span class="p">:</span>
|
||||
<span class="n">cost</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_calc_variance_criterion</span><span class="p">(</span><span class="n">c1</span><span class="p">,</span> <span class="n">c2</span><span class="p">)</span>
|
||||
|
||||
<span class="k">if</span> <span class="n">min_cost</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o"><</span> <span class="n">cost</span><span class="p">:</span>
|
||||
<span class="n">min_cost</span> <span class="o">=</span> <span class="p">(</span><span class="n">cost</span><span class="p">,</span> <span class="n">theta</span><span class="p">)</span>
|
||||
|
||||
<span class="c1"># calc best rectangle</span>
|
||||
<span class="n">sin_s</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">min_cost</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span>
|
||||
<span class="n">cos_s</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">min_cost</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span>
|
||||
|
||||
<span class="n">c1_s</span> <span class="o">=</span> <span class="n">xy</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">cos_s</span><span class="p">,</span> <span class="n">sin_s</span><span class="p">])</span><span class="o">.</span><span class="n">T</span>
|
||||
<span class="n">c2_s</span> <span class="o">=</span> <span class="n">xy</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="o">-</span><span class="n">sin_s</span><span class="p">,</span> <span class="n">cos_s</span><span class="p">])</span><span class="o">.</span><span class="n">T</span>
|
||||
|
||||
<span class="n">rect</span> <span class="o">=</span> <span class="n">RectangleData</span><span class="p">()</span>
|
||||
<span class="n">rect</span><span class="o">.</span><span class="n">a</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">cos_s</span>
|
||||
<span class="n">rect</span><span class="o">.</span><span class="n">b</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">sin_s</span>
|
||||
<span class="n">rect</span><span class="o">.</span><span class="n">c</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="nb">min</span><span class="p">(</span><span class="n">c1_s</span><span class="p">)</span>
|
||||
<span class="n">rect</span><span class="o">.</span><span class="n">a</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">sin_s</span>
|
||||
<span class="n">rect</span><span class="o">.</span><span class="n">b</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">cos_s</span>
|
||||
<span class="n">rect</span><span class="o">.</span><span class="n">c</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="nb">min</span><span class="p">(</span><span class="n">c2_s</span><span class="p">)</span>
|
||||
<span class="n">rect</span><span class="o">.</span><span class="n">a</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">cos_s</span>
|
||||
<span class="n">rect</span><span class="o">.</span><span class="n">b</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">sin_s</span>
|
||||
<span class="n">rect</span><span class="o">.</span><span class="n">c</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="nb">max</span><span class="p">(</span><span class="n">c1_s</span><span class="p">)</span>
|
||||
<span class="n">rect</span><span class="o">.</span><span class="n">a</span><span class="p">[</span><span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">sin_s</span>
|
||||
<span class="n">rect</span><span class="o">.</span><span class="n">b</span><span class="p">[</span><span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="n">cos_s</span>
|
||||
<span class="n">rect</span><span class="o">.</span><span class="n">c</span><span class="p">[</span><span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="nb">max</span><span class="p">(</span><span class="n">c2_s</span><span class="p">)</span>
|
||||
|
||||
<span class="k">return</span> <span class="n">rect</span>
|
||||
|
||||
<span class="k">def</span> <span class="nf">_adoptive_range_segmentation</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">ox</span><span class="p">,</span> <span class="n">oy</span><span class="p">):</span>
|
||||
|
||||
<span class="c1"># Setup initial cluster</span>
|
||||
<span class="n">segment_list</span> <span class="o">=</span> <span class="p">[]</span>
|
||||
<span class="k">for</span> <span class="n">i</span><span class="p">,</span> <span class="n">_</span> <span class="ow">in</span> <span class="nb">enumerate</span><span class="p">(</span><span class="n">ox</span><span class="p">):</span>
|
||||
<span class="n">c</span> <span class="o">=</span> <span class="nb">set</span><span class="p">()</span>
|
||||
<span class="n">r</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">R0</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">Rd</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">ox</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">oy</span><span class="p">[</span><span class="n">i</span><span class="p">]])</span>
|
||||
<span class="k">for</span> <span class="n">j</span><span class="p">,</span> <span class="n">_</span> <span class="ow">in</span> <span class="nb">enumerate</span><span class="p">(</span><span class="n">ox</span><span class="p">):</span>
|
||||
<span class="n">d</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">hypot</span><span class="p">(</span><span class="n">ox</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">-</span> <span class="n">ox</span><span class="p">[</span><span class="n">j</span><span class="p">],</span> <span class="n">oy</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">-</span> <span class="n">oy</span><span class="p">[</span><span class="n">j</span><span class="p">])</span>
|
||||
<span class="k">if</span> <span class="n">d</span> <span class="o"><=</span> <span class="n">r</span><span class="p">:</span>
|
||||
<span class="n">c</span><span class="o">.</span><span class="n">add</span><span class="p">(</span><span class="n">j</span><span class="p">)</span>
|
||||
<span class="n">segment_list</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">c</span><span class="p">)</span>
|
||||
|
||||
<span class="c1"># Merge cluster</span>
|
||||
<span class="k">while</span> <span class="kc">True</span><span class="p">:</span>
|
||||
<span class="n">no_change</span> <span class="o">=</span> <span class="kc">True</span>
|
||||
<span class="k">for</span> <span class="p">(</span><span class="n">c1</span><span class="p">,</span> <span class="n">c2</span><span class="p">)</span> <span class="ow">in</span> <span class="nb">list</span><span class="p">(</span><span class="n">itertools</span><span class="o">.</span><span class="n">permutations</span><span class="p">(</span><span class="nb">range</span><span class="p">(</span><span class="nb">len</span><span class="p">(</span><span class="n">segment_list</span><span class="p">)),</span> <span class="mi">2</span><span class="p">)):</span>
|
||||
<span class="k">if</span> <span class="n">segment_list</span><span class="p">[</span><span class="n">c1</span><span class="p">]</span> <span class="o">&</span> <span class="n">segment_list</span><span class="p">[</span><span class="n">c2</span><span class="p">]:</span>
|
||||
<span class="n">segment_list</span><span class="p">[</span><span class="n">c1</span><span class="p">]</span> <span class="o">=</span> <span class="p">(</span><span class="n">segment_list</span><span class="p">[</span><span class="n">c1</span><span class="p">]</span> <span class="o">|</span> <span class="n">segment_list</span><span class="o">.</span><span class="n">pop</span><span class="p">(</span><span class="n">c2</span><span class="p">))</span>
|
||||
<span class="n">no_change</span> <span class="o">=</span> <span class="kc">False</span>
|
||||
<span class="k">break</span>
|
||||
<span class="k">if</span> <span class="n">no_change</span><span class="p">:</span>
|
||||
<span class="k">break</span>
|
||||
|
||||
<span class="k">return</span> <span class="n">segment_list</span></div>
|
||||
|
||||
|
||||
<span class="k">class</span> <span class="nc">RectangleData</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="bp">self</span><span class="o">.</span><span class="n">a</span> <span class="o">=</span> <span class="p">[</span><span class="kc">None</span><span class="p">]</span> <span class="o">*</span> <span class="mi">4</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">b</span> <span class="o">=</span> <span class="p">[</span><span class="kc">None</span><span class="p">]</span> <span class="o">*</span> <span class="mi">4</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">c</span> <span class="o">=</span> <span class="p">[</span><span class="kc">None</span><span class="p">]</span> <span class="o">*</span> <span class="mi">4</span>
|
||||
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">rect_c_x</span> <span class="o">=</span> <span class="p">[</span><span class="kc">None</span><span class="p">]</span> <span class="o">*</span> <span class="mi">5</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">rect_c_y</span> <span class="o">=</span> <span class="p">[</span><span class="kc">None</span><span class="p">]</span> <span class="o">*</span> <span class="mi">5</span>
|
||||
|
||||
<span class="k">def</span> <span class="nf">plot</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">calc_rect_contour</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="bp">self</span><span class="o">.</span><span class="n">rect_c_x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">rect_c_y</span><span class="p">,</span> <span class="s2">"-r"</span><span class="p">)</span>
|
||||
|
||||
<span class="k">def</span> <span class="nf">calc_rect_contour</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
|
||||
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">rect_c_x</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">rect_c_y</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">calc_cross_point</span><span class="p">(</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">a</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">2</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">b</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">2</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">c</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">2</span><span class="p">])</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">rect_c_x</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">rect_c_y</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">calc_cross_point</span><span class="p">(</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">a</span><span class="p">[</span><span class="mi">1</span><span class="p">:</span><span class="mi">3</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">b</span><span class="p">[</span><span class="mi">1</span><span class="p">:</span><span class="mi">3</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">c</span><span class="p">[</span><span class="mi">1</span><span class="p">:</span><span class="mi">3</span><span class="p">])</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">rect_c_x</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">rect_c_y</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">calc_cross_point</span><span class="p">(</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">a</span><span class="p">[</span><span class="mi">2</span><span class="p">:</span><span class="mi">4</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">b</span><span class="p">[</span><span class="mi">2</span><span class="p">:</span><span class="mi">4</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">c</span><span class="p">[</span><span class="mi">2</span><span class="p">:</span><span class="mi">4</span><span class="p">])</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">rect_c_x</span><span class="p">[</span><span class="mi">3</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">rect_c_y</span><span class="p">[</span><span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">calc_cross_point</span><span class="p">(</span>
|
||||
<span class="p">[</span><span class="bp">self</span><span class="o">.</span><span class="n">a</span><span class="p">[</span><span class="mi">3</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">a</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span> <span class="p">[</span><span class="bp">self</span><span class="o">.</span><span class="n">b</span><span class="p">[</span><span class="mi">3</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">b</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span> <span class="p">[</span><span class="bp">self</span><span class="o">.</span><span class="n">c</span><span class="p">[</span><span class="mi">3</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">c</span><span class="p">[</span><span class="mi">0</span><span class="p">]])</span>
|
||||
<span class="bp">self</span><span class="o">.</span><span class="n">rect_c_x</span><span class="p">[</span><span class="mi">4</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">rect_c_y</span><span class="p">[</span><span class="mi">4</span><span class="p">]</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">rect_c_x</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">rect_c_y</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
|
||||
|
||||
<span class="nd">@staticmethod</span>
|
||||
<span class="k">def</span> <span class="nf">calc_cross_point</span><span class="p">(</span><span class="n">a</span><span class="p">,</span> <span class="n">b</span><span class="p">,</span> <span class="n">c</span><span class="p">):</span>
|
||||
<span class="n">x</span> <span class="o">=</span> <span class="p">(</span><span class="n">b</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">*</span> <span class="o">-</span><span class="n">c</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">b</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">*</span> <span class="o">-</span><span class="n">c</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o">/</span> <span class="p">(</span><span class="n">a</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">*</span> <span class="n">b</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">a</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">*</span> <span class="n">b</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
|
||||
<span class="n">y</span> <span class="o">=</span> <span class="p">(</span><span class="n">a</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">*</span> <span class="o">-</span><span class="n">c</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">a</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">*</span> <span class="o">-</span><span class="n">c</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span> <span class="o">/</span> <span class="p">(</span><span class="n">a</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">*</span> <span class="n">b</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">a</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">*</span> <span class="n">b</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
|
||||
<span class="k">return</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span>
|
||||
|
||||
|
||||
<span class="k">def</span> <span class="nf">main</span><span class="p">():</span>
|
||||
|
||||
<span class="c1"># simulation parameters</span>
|
||||
<span class="n">sim_time</span> <span class="o">=</span> <span class="mf">30.0</span> <span class="c1"># simulation time</span>
|
||||
<span class="n">dt</span> <span class="o">=</span> <span class="mf">0.2</span> <span class="c1"># time tick</span>
|
||||
|
||||
<span class="n">angle_resolution</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">deg2rad</span><span class="p">(</span><span class="mf">3.0</span><span class="p">)</span> <span class="c1"># sensor angle resolution</span>
|
||||
|
||||
<span class="n">v1</span> <span class="o">=</span> <span class="n">VehicleSimulator</span><span class="p">(</span><span class="o">-</span><span class="mf">10.0</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">deg2rad</span><span class="p">(</span><span class="mf">90.0</span><span class="p">),</span>
|
||||
<span class="mf">0.0</span><span class="p">,</span> <span class="mf">50.0</span> <span class="o">/</span> <span class="mf">3.6</span><span class="p">,</span> <span class="mf">3.0</span><span class="p">,</span> <span class="mf">5.0</span><span class="p">)</span>
|
||||
<span class="n">v2</span> <span class="o">=</span> <span class="n">VehicleSimulator</span><span class="p">(</span><span class="mf">20.0</span><span class="p">,</span> <span class="mf">10.0</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">deg2rad</span><span class="p">(</span><span class="mf">180.0</span><span class="p">),</span>
|
||||
<span class="mf">0.0</span><span class="p">,</span> <span class="mf">50.0</span> <span class="o">/</span> <span class="mf">3.6</span><span class="p">,</span> <span class="mf">4.0</span><span class="p">,</span> <span class="mf">10.0</span><span class="p">)</span>
|
||||
|
||||
<span class="n">l_shape_fitting</span> <span class="o">=</span> <span class="n">LShapeFitting</span><span class="p">()</span>
|
||||
<span class="n">lidar_sim</span> <span class="o">=</span> <span class="n">LidarSimulator</span><span class="p">()</span>
|
||||
|
||||
<span class="n">time</span> <span class="o">=</span> <span class="mf">0.0</span>
|
||||
<span class="k">while</span> <span class="n">time</span> <span class="o"><=</span> <span class="n">sim_time</span><span class="p">:</span>
|
||||
<span class="n">time</span> <span class="o">+=</span> <span class="n">dt</span>
|
||||
|
||||
<span class="n">v1</span><span class="o">.</span><span class="n">update</span><span class="p">(</span><span class="n">dt</span><span class="p">,</span> <span class="mf">0.1</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">)</span>
|
||||
<span class="n">v2</span><span class="o">.</span><span class="n">update</span><span class="p">(</span><span class="n">dt</span><span class="p">,</span> <span class="mf">0.1</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.05</span><span class="p">)</span>
|
||||
|
||||
<span class="n">ox</span><span class="p">,</span> <span class="n">oy</span> <span class="o">=</span> <span class="n">lidar_sim</span><span class="o">.</span><span class="n">get_observation_points</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="n">angle_resolution</span><span class="p">)</span>
|
||||
|
||||
<span class="n">rects</span><span class="p">,</span> <span class="n">id_sets</span> <span class="o">=</span> <span class="n">l_shape_fitting</span><span class="o">.</span><span class="n">fitting</span><span class="p">(</span><span class="n">ox</span><span class="p">,</span> <span class="n">oy</span><span class="p">)</span>
|
||||
|
||||
<span class="k">if</span> <span class="n">show_animation</span><span class="p">:</span> <span class="c1"># pragma: no cover</span>
|
||||
<span class="n">plt</span><span class="o">.</span><span class="n">cla</span><span class="p">()</span>
|
||||
<span class="c1"># for stopping simulation with the esc key.</span>
|
||||
<span class="n">plt</span><span class="o">.</span><span class="n">gcf</span><span class="p">()</span><span class="o">.</span><span class="n">canvas</span><span class="o">.</span><span class="n">mpl_connect</span><span class="p">(</span>
|
||||
<span class="s1">'key_release_event'</span><span class="p">,</span>
|
||||
<span class="k">lambda</span> <span class="n">event</span><span class="p">:</span> <span class="p">[</span><span class="n">exit</span><span class="p">(</span><span class="mi">0</span><span class="p">)</span> <span class="k">if</span> <span class="n">event</span><span class="o">.</span><span class="n">key</span> <span class="o">==</span> <span class="s1">'escape'</span> <span class="k">else</span> <span class="kc">None</span><span class="p">])</span>
|
||||
<span class="n">plt</span><span class="o">.</span><span class="n">axis</span><span class="p">(</span><span class="s2">"equal"</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="mf">0.0</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">,</span> <span class="s2">"*r"</span><span class="p">)</span>
|
||||
<span class="n">v1</span><span class="o">.</span><span class="n">plot</span><span class="p">()</span>
|
||||
<span class="n">v2</span><span class="o">.</span><span class="n">plot</span><span class="p">()</span>
|
||||
|
||||
<span class="c1"># Plot range observation</span>
|
||||
<span class="k">for</span> <span class="n">ids</span> <span class="ow">in</span> <span class="n">id_sets</span><span class="p">:</span>
|
||||
<span class="n">x</span> <span class="o">=</span> <span class="p">[</span><span class="n">ox</span><span class="p">[</span><span class="n">i</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">ox</span><span class="p">))</span> <span class="k">if</span> <span class="n">i</span> <span class="ow">in</span> <span class="n">ids</span><span class="p">]</span>
|
||||
<span class="n">y</span> <span class="o">=</span> <span class="p">[</span><span class="n">oy</span><span class="p">[</span><span class="n">i</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">ox</span><span class="p">))</span> <span class="k">if</span> <span class="n">i</span> <span class="ow">in</span> <span class="n">ids</span><span class="p">]</span>
|
||||
|
||||
<span class="k">for</span> <span class="p">(</span><span class="n">ix</span><span class="p">,</span> <span class="n">iy</span><span class="p">)</span> <span class="ow">in</span> <span class="nb">zip</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">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">([</span><span class="mf">0.0</span><span class="p">,</span> <span class="n">ix</span><span class="p">],</span> <span class="p">[</span><span class="mf">0.0</span><span class="p">,</span> <span class="n">iy</span><span class="p">],</span> <span class="s2">"-og"</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">ox</span><span class="p">[</span><span class="n">i</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">ox</span><span class="p">))</span> <span class="k">if</span> <span class="n">i</span> <span class="ow">in</span> <span class="n">ids</span><span class="p">],</span>
|
||||
<span class="p">[</span><span class="n">oy</span><span class="p">[</span><span class="n">i</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">ox</span><span class="p">))</span> <span class="k">if</span> <span class="n">i</span> <span class="ow">in</span> <span class="n">ids</span><span class="p">],</span>
|
||||
<span class="s2">"o"</span><span class="p">)</span>
|
||||
<span class="k">for</span> <span class="n">rect</span> <span class="ow">in</span> <span class="n">rects</span><span class="p">:</span>
|
||||
<span class="n">rect</span><span class="o">.</span><span class="n">plot</span><span class="p">()</span>
|
||||
|
||||
<span class="n">plt</span><span class="o">.</span><span class="n">pause</span><span class="p">(</span><span class="mf">0.1</span><span class="p">)</span>
|
||||
|
||||
<span class="nb">print</span><span class="p">(</span><span class="s2">"Done"</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="n">main</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>
|
||||
@@ -102,6 +102,7 @@
|
||||
<h1>All modules for which code is available</h1>
|
||||
<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="Mapping/rectangle_fitting/rectangle_fitting.html">Mapping.rectangle_fitting.rectangle_fitting</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>
|
||||
|
||||
@@ -5,3 +5,65 @@ This is an object shape recognition using rectangle fitting.
|
||||
|
||||
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/rectangle_fitting/animation.gif
|
||||
|
||||
This example code is based on this paper algorithm:
|
||||
|
||||
- `Efficient L\-Shape Fitting for Vehicle Detection Using Laser Scanners \- The Robotics Institute Carnegie Mellon University <https://www.ri.cmu.edu/publications/efficient-l-shape-fitting-for-vehicle-detection-using-laser-scanners>`_
|
||||
|
||||
The algorithm consists of 2 steps as below.
|
||||
|
||||
Step1: Adaptive range segmentation
|
||||
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
||||
|
||||
In the first step, all range data points are segmented into some clusters.
|
||||
|
||||
We calculate the distance between each range data and the nearest range data, and if this distance is below a certain threshold, it is judged to be in the same cluster.
|
||||
This distance threshold is determined in proportion to the distance from the sensor.
|
||||
This is taking advantage of the general model of distance sensors, which tends to have sparser data distribution as the distance from the sensor increases.
|
||||
|
||||
The threshold range is calculated by:
|
||||
|
||||
.. math:: r_{th} = R_0 + R_d * r_{origin}
|
||||
|
||||
where
|
||||
|
||||
- :math:`r_{th}`: Threashold range
|
||||
- :math:`R_0, R_d`: Constant parameters
|
||||
- :math:`r_{origin}`: Distance from the sensor for a range data.
|
||||
|
||||
Step2: Rectangle search
|
||||
~~~~~~~~~~~~~~~~~~~~~~~~~~
|
||||
|
||||
In the second step, for each cluster calculated in the previous step, rectangular fittings will be applied.
|
||||
In this rectangular fitting, each cluster's distance data is rotated at certain angle intervals.
|
||||
It is evaluated by one of the three evaluation functions below, then best angle parameter one is selected as the rectangle shape.
|
||||
|
||||
1. Rectangle Area Minimization criteria
|
||||
=========================================
|
||||
|
||||
This evaluation function calculates the area of the smallest rectangle that includes all the points, derived from the difference between the maximum and minimum values on the x-y axis for all distance data points.
|
||||
This allows for fitting a rectangle in a direction that encompasses as much of the smallest rectangular shape as possible.
|
||||
|
||||
|
||||
2. Closeness criteria
|
||||
======================
|
||||
|
||||
This evaluation function uses the distances between the top and bottom vertices on the right side of the rectangle and each point in the distance data as evaluation values.
|
||||
If there are points on the rectangle edges, this evaluation value decreases.
|
||||
|
||||
3. Variance criteria
|
||||
=======================
|
||||
|
||||
This evaluation function uses the squreed distances between the edges of the rectangle (horizontal and vertical) and each point.
|
||||
Calculating the squared error is the same as calculating the variance.
|
||||
The smaller this variance, the more it signifies that the points fit within the rectangle.
|
||||
|
||||
API
|
||||
~~~~~~
|
||||
|
||||
.. autoclass:: Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting
|
||||
:members:
|
||||
|
||||
References
|
||||
~~~~~~~~~~
|
||||
|
||||
- `Efficient L\-Shape Fitting for Vehicle Detection Using Laser Scanners \- The Robotics Institute Carnegie Mellon University <https://www.ri.cmu.edu/publications/efficient-l-shape-fitting-for-vehicle-detection-using-laser-scanners>`_
|
||||
|
||||
@@ -65,7 +65,7 @@ represent the initial uncertainty. At each time step we do:
|
||||
|
||||
The following equations and code snippets we can see how the particles
|
||||
distribution evolves in case we provide only the control :math:`(v,w)`,
|
||||
which are the linear and angular velocity repsectively.
|
||||
which are the linear and angular velocity respectively.
|
||||
|
||||
:math:`\begin{equation*} F= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{equation*}`
|
||||
|
||||
|
||||
@@ -106,8 +106,11 @@
|
||||
<div class="genindex-jumpbox">
|
||||
<a href="#A"><strong>A</strong></a>
|
||||
| <a href="#C"><strong>C</strong></a>
|
||||
| <a href="#D"><strong>D</strong></a>
|
||||
| <a href="#F"><strong>F</strong></a>
|
||||
| <a href="#I"><strong>I</strong></a>
|
||||
| <a href="#L"><strong>L</strong></a>
|
||||
| <a href="#M"><strong>M</strong></a>
|
||||
| <a href="#P"><strong>P</strong></a>
|
||||
| <a href="#R"><strong>R</strong></a>
|
||||
| <a href="#V"><strong>V</strong></a>
|
||||
@@ -141,6 +144,8 @@
|
||||
<li><a href="modules/path_planning/cubic_spline/cubic_spline.html#PathPlanning.CubicSpline.cubic_spline_planner.CubicSpline1D.calc_second_derivative">calc_second_derivative() (PathPlanning.CubicSpline.cubic_spline_planner.CubicSpline1D method)</a>
|
||||
</li>
|
||||
<li><a href="modules/path_planning/cubic_spline/cubic_spline.html#PathPlanning.CubicSpline.cubic_spline_planner.CubicSpline2D.calc_yaw">calc_yaw() (PathPlanning.CubicSpline.cubic_spline_planner.CubicSpline2D method)</a>
|
||||
</li>
|
||||
<li><a href="modules/mapping/rectangle_fitting/rectangle_fitting.html#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.criteria">criteria (Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting attribute)</a>
|
||||
</li>
|
||||
<li><a href="modules/path_planning/cubic_spline/cubic_spline.html#PathPlanning.CubicSpline.cubic_spline_planner.CubicSpline1D">CubicSpline1D (class in PathPlanning.CubicSpline.cubic_spline_planner)</a>
|
||||
</li>
|
||||
@@ -149,10 +154,22 @@
|
||||
</ul></td>
|
||||
</tr></table>
|
||||
|
||||
<h2 id="D">D</h2>
|
||||
<table style="width: 100%" class="indextable genindextable"><tr>
|
||||
<td style="width: 33%; vertical-align: top;"><ul>
|
||||
<li><a href="modules/mapping/rectangle_fitting/rectangle_fitting.html#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.d_theta_deg_for_search">d_theta_deg_for_search (Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting attribute)</a>
|
||||
</li>
|
||||
</ul></td>
|
||||
</tr></table>
|
||||
|
||||
<h2 id="F">F</h2>
|
||||
<table style="width: 100%" class="indextable genindextable"><tr>
|
||||
<td style="width: 33%; vertical-align: top;"><ul>
|
||||
<li><a href="modules/mapping/point_cloud_sampling/point_cloud_sampling.html#Mapping.point_cloud_sampling.point_cloud_sampling.farthest_point_sampling">farthest_point_sampling() (in module Mapping.point_cloud_sampling.point_cloud_sampling)</a>
|
||||
</li>
|
||||
</ul></td>
|
||||
<td style="width: 33%; vertical-align: top;"><ul>
|
||||
<li><a href="modules/mapping/rectangle_fitting/rectangle_fitting.html#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.fitting">fitting() (Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting method)</a>
|
||||
</li>
|
||||
</ul></td>
|
||||
</tr></table>
|
||||
@@ -165,6 +182,26 @@
|
||||
</ul></td>
|
||||
</tr></table>
|
||||
|
||||
<h2 id="L">L</h2>
|
||||
<table style="width: 100%" class="indextable genindextable"><tr>
|
||||
<td style="width: 33%; vertical-align: top;"><ul>
|
||||
<li><a href="modules/mapping/rectangle_fitting/rectangle_fitting.html#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting">LShapeFitting (class in Mapping.rectangle_fitting.rectangle_fitting)</a>
|
||||
</li>
|
||||
</ul></td>
|
||||
<td style="width: 33%; vertical-align: top;"><ul>
|
||||
<li><a href="modules/mapping/rectangle_fitting/rectangle_fitting.html#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.Criteria">LShapeFitting.Criteria (class in Mapping.rectangle_fitting.rectangle_fitting)</a>
|
||||
</li>
|
||||
</ul></td>
|
||||
</tr></table>
|
||||
|
||||
<h2 id="M">M</h2>
|
||||
<table style="width: 100%" class="indextable genindextable"><tr>
|
||||
<td style="width: 33%; vertical-align: top;"><ul>
|
||||
<li><a href="modules/mapping/rectangle_fitting/rectangle_fitting.html#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.min_dist_of_closeness_criteria">min_dist_of_closeness_criteria (Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting attribute)</a>
|
||||
</li>
|
||||
</ul></td>
|
||||
</tr></table>
|
||||
|
||||
<h2 id="P">P</h2>
|
||||
<table style="width: 100%" class="indextable genindextable"><tr>
|
||||
<td style="width: 33%; vertical-align: top;"><ul>
|
||||
@@ -181,8 +218,14 @@
|
||||
|
||||
<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/rectangle_fitting/rectangle_fitting.html#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.R0">R0 (Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting attribute)</a>
|
||||
</li>
|
||||
</ul></td>
|
||||
<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>
|
||||
<li><a href="modules/mapping/rectangle_fitting/rectangle_fitting.html#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.Rd">Rd (Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting attribute)</a>
|
||||
</li>
|
||||
</ul></td>
|
||||
</tr></table>
|
||||
|
||||
@@ -129,7 +129,13 @@
|
||||
</li>
|
||||
<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="rectangle_fitting/rectangle_fitting.html">Object shape recognition using rectangle fitting</a><ul>
|
||||
<li class="toctree-l2"><a class="reference internal" href="rectangle_fitting/rectangle_fitting.html#step1-adaptive-range-segmentation">Step1: Adaptive range segmentation</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="rectangle_fitting/rectangle_fitting.html#step2-rectangle-search">Step2: Rectangle search</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="rectangle_fitting/rectangle_fitting.html#api">API</a></li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="rectangle_fitting/rectangle_fitting.html#references">References</a></li>
|
||||
</ul>
|
||||
</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>
|
||||
|
||||
@@ -22,6 +22,7 @@
|
||||
<script src="../../../_static/doctools.js"></script>
|
||||
<script src="../../../_static/clipboard.min.js"></script>
|
||||
<script src="../../../_static/copybutton.js"></script>
|
||||
<script async="async" src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-mml-chtml.js"></script>
|
||||
<script src="../../../_static/dark_mode_js/default_dark.js"></script>
|
||||
<script src="../../../_static/dark_mode_js/theme_switcher.js"></script>
|
||||
<script src="../../../_static/js/theme.js"></script>
|
||||
@@ -73,7 +74,18 @@
|
||||
<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 current"><a class="current reference internal" href="#">Object shape recognition using rectangle fitting</a></li>
|
||||
<li class="toctree-l2 current"><a class="current reference internal" href="#">Object shape recognition using rectangle fitting</a><ul>
|
||||
<li class="toctree-l3"><a class="reference internal" href="#step1-adaptive-range-segmentation">Step1: Adaptive range segmentation</a></li>
|
||||
<li class="toctree-l3"><a class="reference internal" href="#step2-rectangle-search">Step2: Rectangle search</a><ul>
|
||||
<li class="toctree-l4"><a class="reference internal" href="#rectangle-area-minimization-criteria">1. Rectangle Area Minimization criteria</a></li>
|
||||
<li class="toctree-l4"><a class="reference internal" href="#closeness-criteria">2. Closeness criteria</a></li>
|
||||
<li class="toctree-l4"><a class="reference internal" href="#variance-criteria">3. Variance criteria</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li class="toctree-l3"><a class="reference internal" href="#api">API</a></li>
|
||||
<li class="toctree-l3"><a class="reference internal" href="#references">References</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li class="toctree-l2"><a class="reference internal" href="../normal_vector_estimation/normal_vector_estimation.html">Normal vector estimation</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
@@ -118,6 +130,121 @@
|
||||
<h1>Object shape recognition using rectangle fitting<a class="headerlink" href="#object-shape-recognition-using-rectangle-fitting" title="Permalink to this headline"></a></h1>
|
||||
<p>This is an object shape recognition using rectangle fitting.</p>
|
||||
<img alt="https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/rectangle_fitting/animation.gif" src="https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/rectangle_fitting/animation.gif" />
|
||||
<p>This example code is based on this paper algorithm:</p>
|
||||
<ul class="simple">
|
||||
<li><p><a class="reference external" href="https://www.ri.cmu.edu/publications/efficient-l-shape-fitting-for-vehicle-detection-using-laser-scanners">Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners - The Robotics Institute Carnegie Mellon University</a></p></li>
|
||||
</ul>
|
||||
<p>The algorithm consists of 2 steps as below.</p>
|
||||
<section id="step1-adaptive-range-segmentation">
|
||||
<h2>Step1: Adaptive range segmentation<a class="headerlink" href="#step1-adaptive-range-segmentation" title="Permalink to this headline"></a></h2>
|
||||
<p>In the first step, all range data points are segmented into some clusters.</p>
|
||||
<p>We calculate the distance between each range data and the nearest range data, and if this distance is below a certain threshold, it is judged to be in the same cluster.
|
||||
This distance threshold is determined in proportion to the distance from the sensor.
|
||||
This is taking advantage of the general model of distance sensors, which tends to have sparser data distribution as the distance from the sensor increases.</p>
|
||||
<p>The threshold range is calculated by:</p>
|
||||
<div class="math notranslate nohighlight">
|
||||
\[r_{th} = R_0 + R_d * r_{origin}\]</div>
|
||||
<p>where</p>
|
||||
<ul class="simple">
|
||||
<li><p><span class="math notranslate nohighlight">\(r_{th}\)</span>: Threashold range</p></li>
|
||||
<li><p><span class="math notranslate nohighlight">\(R_0, R_d\)</span>: Constant parameters</p></li>
|
||||
<li><p><span class="math notranslate nohighlight">\(r_{origin}\)</span>: Distance from the sensor for a range data.</p></li>
|
||||
</ul>
|
||||
</section>
|
||||
<section id="step2-rectangle-search">
|
||||
<h2>Step2: Rectangle search<a class="headerlink" href="#step2-rectangle-search" title="Permalink to this headline"></a></h2>
|
||||
<p>In the second step, for each cluster calculated in the previous step, rectangular fittings will be applied.
|
||||
In this rectangular fitting, each cluster’s distance data is rotated at certain angle intervals.
|
||||
It is evaluated by one of the three evaluation functions below, then best angle parameter one is selected as the rectangle shape.</p>
|
||||
<section id="rectangle-area-minimization-criteria">
|
||||
<h3>1. Rectangle Area Minimization criteria<a class="headerlink" href="#rectangle-area-minimization-criteria" title="Permalink to this headline"></a></h3>
|
||||
<p>This evaluation function calculates the area of the smallest rectangle that includes all the points, derived from the difference between the maximum and minimum values on the x-y axis for all distance data points.
|
||||
This allows for fitting a rectangle in a direction that encompasses as much of the smallest rectangular shape as possible.</p>
|
||||
</section>
|
||||
<section id="closeness-criteria">
|
||||
<h3>2. Closeness criteria<a class="headerlink" href="#closeness-criteria" title="Permalink to this headline"></a></h3>
|
||||
<p>This evaluation function uses the distances between the top and bottom vertices on the right side of the rectangle and each point in the distance data as evaluation values.
|
||||
If there are points on the rectangle edges, this evaluation value decreases.</p>
|
||||
</section>
|
||||
<section id="variance-criteria">
|
||||
<h3>3. Variance criteria<a class="headerlink" href="#variance-criteria" title="Permalink to this headline"></a></h3>
|
||||
<p>This evaluation function uses the squreed distances between the edges of the rectangle (horizontal and vertical) and each point.
|
||||
Calculating the squared error is the same as calculating the variance.
|
||||
The smaller this variance, the more it signifies that the points fit within the rectangle.</p>
|
||||
</section>
|
||||
</section>
|
||||
<section id="api">
|
||||
<h2>API<a class="headerlink" href="#api" title="Permalink to this headline"></a></h2>
|
||||
<dl class="py class">
|
||||
<dt class="sig sig-object py" id="Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting">
|
||||
<em class="property"><span class="pre">class</span><span class="w"> </span></em><span class="sig-prename descclassname"><span class="pre">Mapping.rectangle_fitting.rectangle_fitting.</span></span><span class="sig-name descname"><span class="pre">LShapeFitting</span></span><a class="reference internal" href="../../../_modules/Mapping/rectangle_fitting/rectangle_fitting.html#LShapeFitting"><span class="viewcode-link"><span class="pre">[source]</span></span></a><a class="headerlink" href="#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting" title="Permalink to this definition"></a></dt>
|
||||
<dd><p>LShapeFitting class. You can use this class by initializing the class and
|
||||
changing the parameters, and then calling the fitting method.</p>
|
||||
<dl class="py class">
|
||||
<dt class="sig sig-object py" id="Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.Criteria">
|
||||
<em class="property"><span class="pre">class</span><span class="w"> </span></em><span class="sig-name descname"><span class="pre">Criteria</span></span><span class="sig-paren">(</span><em class="sig-param"><span class="n"><span class="pre">value</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">names</span></span><span class="o"><span class="pre">=</span></span><span class="default_value"><span class="pre">None</span></span></em>, <em class="sig-param"><span class="o"><span class="pre">*</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">module</span></span><span class="o"><span class="pre">=</span></span><span class="default_value"><span class="pre">None</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">qualname</span></span><span class="o"><span class="pre">=</span></span><span class="default_value"><span class="pre">None</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">type</span></span><span class="o"><span class="pre">=</span></span><span class="default_value"><span class="pre">None</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">start</span></span><span class="o"><span class="pre">=</span></span><span class="default_value"><span class="pre">1</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">boundary</span></span><span class="o"><span class="pre">=</span></span><span class="default_value"><span class="pre">None</span></span></em><span class="sig-paren">)</span><a class="reference internal" href="../../../_modules/Mapping/rectangle_fitting/rectangle_fitting.html#LShapeFitting.Criteria"><span class="viewcode-link"><span class="pre">[source]</span></span></a><a class="headerlink" href="#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.Criteria" title="Permalink to this definition"></a></dt>
|
||||
<dd></dd></dl>
|
||||
|
||||
<dl class="py attribute">
|
||||
<dt class="sig sig-object py" id="Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.R0">
|
||||
<span class="sig-name descname"><span class="pre">R0</span></span><a class="headerlink" href="#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.R0" title="Permalink to this definition"></a></dt>
|
||||
<dd><p>Range segmentation parameter [m]</p>
|
||||
</dd></dl>
|
||||
|
||||
<dl class="py attribute">
|
||||
<dt class="sig sig-object py" id="Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.Rd">
|
||||
<span class="sig-name descname"><span class="pre">Rd</span></span><a class="headerlink" href="#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.Rd" title="Permalink to this definition"></a></dt>
|
||||
<dd><p>Range segmentation parameter [m]</p>
|
||||
</dd></dl>
|
||||
|
||||
<dl class="py attribute">
|
||||
<dt class="sig sig-object py" id="Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.criteria">
|
||||
<span class="sig-name descname"><span class="pre">criteria</span></span><a class="headerlink" href="#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.criteria" title="Permalink to this definition"></a></dt>
|
||||
<dd><p>Fitting criteria parameter</p>
|
||||
</dd></dl>
|
||||
|
||||
<dl class="py attribute">
|
||||
<dt class="sig sig-object py" id="Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.d_theta_deg_for_search">
|
||||
<span class="sig-name descname"><span class="pre">d_theta_deg_for_search</span></span><a class="headerlink" href="#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.d_theta_deg_for_search" title="Permalink to this definition"></a></dt>
|
||||
<dd><p>Angle difference parameter [deg]</p>
|
||||
</dd></dl>
|
||||
|
||||
<dl class="py method">
|
||||
<dt class="sig sig-object py" id="Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.fitting">
|
||||
<span class="sig-name descname"><span class="pre">fitting</span></span><span class="sig-paren">(</span><em class="sig-param"><span class="n"><span class="pre">ox</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">oy</span></span></em><span class="sig-paren">)</span><a class="reference internal" href="../../../_modules/Mapping/rectangle_fitting/rectangle_fitting.html#LShapeFitting.fitting"><span class="viewcode-link"><span class="pre">[source]</span></span></a><a class="headerlink" href="#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.fitting" title="Permalink to this definition"></a></dt>
|
||||
<dd><p>Fitting L-shape model to object points</p>
|
||||
<dl class="field-list simple">
|
||||
<dt class="field-odd">Parameters</dt>
|
||||
<dd class="field-odd"><ul class="simple">
|
||||
<li><p><strong>ox</strong> (<em>x positions of range points from an object</em>) – </p></li>
|
||||
<li><p><strong>oy</strong> (<em>y positions of range points from an object</em>) – </p></li>
|
||||
</ul>
|
||||
</dd>
|
||||
<dt class="field-even">Returns</dt>
|
||||
<dd class="field-even"><p><ul class="simple">
|
||||
<li><p><strong>rects</strong> (<em>Fitting rectangles</em>)</p></li>
|
||||
<li><p><strong>id_sets</strong> (<em>id sets of each cluster</em>)</p></li>
|
||||
</ul>
|
||||
</p>
|
||||
</dd>
|
||||
</dl>
|
||||
</dd></dl>
|
||||
|
||||
<dl class="py attribute">
|
||||
<dt class="sig sig-object py" id="Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.min_dist_of_closeness_criteria">
|
||||
<span class="sig-name descname"><span class="pre">min_dist_of_closeness_criteria</span></span><a class="headerlink" href="#Mapping.rectangle_fitting.rectangle_fitting.LShapeFitting.min_dist_of_closeness_criteria" title="Permalink to this definition"></a></dt>
|
||||
<dd><p>Minimum distance for closeness criteria parameter [m]</p>
|
||||
</dd></dl>
|
||||
|
||||
</dd></dl>
|
||||
|
||||
</section>
|
||||
<section id="references">
|
||||
<h2>References<a class="headerlink" href="#references" title="Permalink to this headline"></a></h2>
|
||||
<ul class="simple">
|
||||
<li><p><a class="reference external" href="https://www.ri.cmu.edu/publications/efficient-l-shape-fitting-for-vehicle-detection-using-laser-scanners">Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners - The Robotics Institute Carnegie Mellon University</a></p></li>
|
||||
</ul>
|
||||
</section>
|
||||
</section>
|
||||
|
||||
|
||||
|
||||
@@ -175,7 +175,7 @@ and the unlikely ones with the lowest weights die out.</p></li>
|
||||
<h2>1- Predict<a class="headerlink" href="#predict" title="Permalink to this headline"></a></h2>
|
||||
<p>The following equations and code snippets we can see how the particles
|
||||
distribution evolves in case we provide only the control <span class="math notranslate nohighlight">\((v,w)\)</span>,
|
||||
which are the linear and angular velocity repsectively.</p>
|
||||
which are the linear and angular velocity respectively.</p>
|
||||
<p><span class="math notranslate nohighlight">\(\begin{equation*} F= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{equation*}\)</span></p>
|
||||
<p><span class="math notranslate nohighlight">\(\begin{equation*} B= \begin{bmatrix} \Delta t cos(\theta) & 0\\ \Delta t sin(\theta) & 0\\ 0 & \Delta t \end{bmatrix} \end{equation*}\)</span></p>
|
||||
<p><span class="math notranslate nohighlight">\(\begin{equation*} X = FX + BU \end{equation*}\)</span></p>
|
||||
|
||||
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