465 lines
41 KiB
HTML
465 lines
41 KiB
HTML
|
||
|
||
<!DOCTYPE html>
|
||
<!--[if IE 8]><html class="no-js lt-ie9" lang="en" > <![endif]-->
|
||
<!--[if gt IE 8]><!--> <html class="no-js" lang="en" > <!--<![endif]-->
|
||
<head>
|
||
<meta charset="utf-8">
|
||
|
||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||
|
||
<title>Detecting people and their poses using PointCloud Library — Point Cloud Library 1.12.0 documentation</title>
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
<script type="text/javascript" src="_static/js/modernizr.min.js"></script>
|
||
|
||
|
||
<script type="text/javascript" id="documentation_options" data-url_root="./" src="_static/documentation_options.js"></script>
|
||
<script type="text/javascript" src="_static/jquery.js"></script>
|
||
<script type="text/javascript" src="_static/underscore.js"></script>
|
||
<script type="text/javascript" src="_static/doctools.js"></script>
|
||
<script type="text/javascript" src="_static/language_data.js"></script>
|
||
|
||
<script type="text/javascript" src="_static/js/theme.js"></script>
|
||
|
||
|
||
|
||
|
||
<link rel="stylesheet" href="_static/css/theme.css" type="text/css" />
|
||
<link rel="stylesheet" href="_static/pygments.css" type="text/css" />
|
||
<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"> Point Cloud Library
|
||
|
||
|
||
|
||
</a>
|
||
|
||
|
||
|
||
|
||
<div class="version">
|
||
1.12.0
|
||
</div>
|
||
|
||
|
||
|
||
|
||
<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>
|
||
|
||
|
||
</div>
|
||
|
||
<div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="main navigation">
|
||
|
||
|
||
|
||
|
||
|
||
|
||
<!-- Local TOC -->
|
||
<div class="local-toc"><ul>
|
||
<li><a class="reference internal" href="#">Detecting people and their poses using PointCloud Library</a></li>
|
||
<li><a class="reference internal" href="#the-code">The code</a></li>
|
||
<li><a class="reference internal" href="#the-explanation">The explanation</a></li>
|
||
<li><a class="reference internal" href="#compiling-and-running-the-program">Compiling and running the program</a></li>
|
||
</ul>
|
||
</div>
|
||
|
||
|
||
</div>
|
||
</div>
|
||
</nav>
|
||
|
||
<section data-toggle="wy-nav-shift" class="wy-nav-content-wrap">
|
||
|
||
|
||
<nav class="wy-nav-top" aria-label="top navigation">
|
||
|
||
<i data-toggle="wy-nav-top" class="fa fa-bars"></i>
|
||
<a href="index.html">Point Cloud Library</a>
|
||
|
||
</nav>
|
||
|
||
|
||
<div class="wy-nav-content">
|
||
|
||
<div class="rst-content">
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
<div role="navigation" aria-label="breadcrumbs navigation">
|
||
|
||
<ul class="wy-breadcrumbs">
|
||
|
||
<li><a href="index.html">Docs</a> »</li>
|
||
|
||
<li>Detecting people and their poses using PointCloud Library</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">
|
||
|
||
<div class="section" id="detecting-people-and-their-poses-using-pointcloud-library">
|
||
<span id="gpu-people"></span><h1>Detecting people and their poses using PointCloud Library</h1>
|
||
<p>In this tutorial we will learn how detect a person and its pose in a pointcloud.
|
||
This is based on work from Koen Buys, Cedric Cagniart, Anatoly Bashkeev and Caroline Pantofaru, this
|
||
has been presented on ICRA2012 and IROS2012 and an official reference for a journal paper is in progress. A coarse outline of how it works can be seen in the following video.</p>
|
||
<blockquote>
|
||
<div><iframe width="560" height="315" src="https://www.youtube.com/embed/Wd4OM8wOO1E?rel=0" frameborder="0" allowfullscreen></iframe></div></blockquote>
|
||
<p>This shows how to detect people with an Primesense device, the full version
|
||
working on oni and pcd files can be found in the git master.
|
||
The code assumes an organised and projectable pointcloud, and should work with other
|
||
sensors then the Primesense device.</p>
|
||
<blockquote>
|
||
<div><a class="reference internal image-reference" href="_images/ss26_1.jpg"><img alt="_images/ss26_1.jpg" src="_images/ss26_1.jpg" style="width: 400pt; height: 372pt;" /></a>
|
||
<a class="reference internal image-reference" href="_images/ss26_2.jpg"><img alt="_images/ss26_2.jpg" src="_images/ss26_2.jpg" style="width: 400pt; height: 372pt;" /></a>
|
||
</div></blockquote>
|
||
<p>In order to run the code you’ll need a decent Nvidia GPU with Fermi or Kepler architecture, have a look
|
||
at the GPU installation tutorial to get up and running with your GPU installation.</p>
|
||
</div>
|
||
<div class="section" id="the-code">
|
||
<h1>The code</h1>
|
||
<p>The full version of this code can be found in PCL gpu/people/tools,
|
||
the following is a reduced version for the tutorial.
|
||
This version can be found in doc/tutorials/content/sources/gpu/people_detect.</p>
|
||
</div>
|
||
<div class="section" id="the-explanation">
|
||
<h1>The explanation</h1>
|
||
<p>Now, let’s break down the code piece by piece. Starting from the main routine.</p>
|
||
<div class="highlight-cpp notranslate"><div class="highlight"><pre><span></span><span class="kt">int</span> <span class="nf">main</span><span class="p">(</span><span class="kt">int</span> <span class="n">argc</span><span class="p">,</span> <span class="kt">char</span><span class="o">**</span> <span class="n">argv</span><span class="p">)</span>
|
||
<span class="p">{</span>
|
||
<span class="c1">// selecting GPU and prining info</span>
|
||
<span class="kt">int</span> <span class="n">device</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span>
|
||
<span class="n">pc</span><span class="o">::</span><span class="n">parse_argument</span> <span class="p">(</span><span class="n">argc</span><span class="p">,</span> <span class="n">argv</span><span class="p">,</span> <span class="s">"-gpu"</span><span class="p">,</span> <span class="n">device</span><span class="p">);</span>
|
||
<span class="n">pcl</span><span class="o">::</span><span class="n">gpu</span><span class="o">::</span><span class="n">setDevice</span> <span class="p">(</span><span class="n">device</span><span class="p">);</span>
|
||
<span class="n">pcl</span><span class="o">::</span><span class="n">gpu</span><span class="o">::</span><span class="n">printShortCudaDeviceInfo</span> <span class="p">(</span><span class="n">device</span><span class="p">);</span>
|
||
|
||
<span class="c1">// selecting data source</span>
|
||
<span class="n">pcl</span><span class="o">::</span><span class="n">Grabber</span><span class="o">::</span><span class="n">Ptr</span> <span class="n">capture</span> <span class="p">(</span><span class="k">new</span> <span class="n">pcl</span><span class="o">::</span><span class="n">OpenNIGrabber</span><span class="p">());</span>
|
||
|
||
<span class="c1">//selecting tree files</span>
|
||
<span class="n">std</span><span class="o">::</span><span class="n">vector</span><span class="o"><</span><span class="n">std</span><span class="o">::</span><span class="n">string</span><span class="o">></span> <span class="n">tree_files</span><span class="p">;</span>
|
||
<span class="n">tree_files</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="s">"Data/forest1/tree_20.txt"</span><span class="p">);</span>
|
||
<span class="n">tree_files</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="s">"Data/forest2/tree_20.txt"</span><span class="p">);</span>
|
||
<span class="n">tree_files</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="s">"Data/forest3/tree_20.txt"</span><span class="p">);</span>
|
||
<span class="n">tree_files</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="s">"Data/forest4/tree_20.txt"</span><span class="p">);</span>
|
||
|
||
<span class="n">pc</span><span class="o">::</span><span class="n">parse_argument</span> <span class="p">(</span><span class="n">argc</span><span class="p">,</span> <span class="n">argv</span><span class="p">,</span> <span class="s">"-tree0"</span><span class="p">,</span> <span class="n">tree_files</span><span class="p">[</span><span class="mi">0</span><span class="p">]);</span>
|
||
<span class="n">pc</span><span class="o">::</span><span class="n">parse_argument</span> <span class="p">(</span><span class="n">argc</span><span class="p">,</span> <span class="n">argv</span><span class="p">,</span> <span class="s">"-tree1"</span><span class="p">,</span> <span class="n">tree_files</span><span class="p">[</span><span class="mi">1</span><span class="p">]);</span>
|
||
<span class="n">pc</span><span class="o">::</span><span class="n">parse_argument</span> <span class="p">(</span><span class="n">argc</span><span class="p">,</span> <span class="n">argv</span><span class="p">,</span> <span class="s">"-tree2"</span><span class="p">,</span> <span class="n">tree_files</span><span class="p">[</span><span class="mi">2</span><span class="p">]);</span>
|
||
<span class="n">pc</span><span class="o">::</span><span class="n">parse_argument</span> <span class="p">(</span><span class="n">argc</span><span class="p">,</span> <span class="n">argv</span><span class="p">,</span> <span class="s">"-tree3"</span><span class="p">,</span> <span class="n">tree_files</span><span class="p">[</span><span class="mi">3</span><span class="p">]);</span>
|
||
|
||
<span class="kt">int</span> <span class="n">num_trees</span> <span class="o">=</span> <span class="p">(</span><span class="kt">int</span><span class="p">)</span><span class="n">tree_files</span><span class="p">.</span><span class="n">size</span><span class="p">();</span>
|
||
<span class="n">pc</span><span class="o">::</span><span class="n">parse_argument</span> <span class="p">(</span><span class="n">argc</span><span class="p">,</span> <span class="n">argv</span><span class="p">,</span> <span class="s">"-numTrees"</span><span class="p">,</span> <span class="n">num_trees</span><span class="p">);</span>
|
||
|
||
<span class="n">tree_files</span><span class="p">.</span><span class="n">resize</span><span class="p">(</span><span class="n">num_trees</span><span class="p">);</span>
|
||
<span class="k">if</span> <span class="p">(</span><span class="n">num_trees</span> <span class="o">==</span> <span class="mi">0</span> <span class="o">||</span> <span class="n">num_trees</span> <span class="o">></span> <span class="mi">4</span><span class="p">)</span>
|
||
<span class="k">return</span> <span class="n">std</span><span class="o">::</span><span class="n">cout</span> <span class="o"><<</span> <span class="s">"Invalid number of trees"</span> <span class="o"><<</span> <span class="n">std</span><span class="o">::</span><span class="n">endl</span><span class="p">,</span> <span class="o">-</span><span class="mi">1</span><span class="p">;</span>
|
||
|
||
<span class="k">try</span>
|
||
<span class="p">{</span>
|
||
<span class="c1">// loading trees</span>
|
||
<span class="k">typedef</span> <span class="n">pcl</span><span class="o">::</span><span class="n">gpu</span><span class="o">::</span><span class="n">people</span><span class="o">::</span><span class="n">RDFBodyPartsDetector</span> <span class="n">RDFBodyPartsDetector</span><span class="p">;</span>
|
||
<span class="n">RDFBodyPartsDetector</span><span class="o">::</span><span class="n">Ptr</span> <span class="n">rdf</span><span class="p">(</span><span class="k">new</span> <span class="n">RDFBodyPartsDetector</span><span class="p">(</span><span class="n">tree_files</span><span class="p">));</span>
|
||
<span class="n">PCL_INFO</span><span class="p">(</span><span class="s">"Loaded files into rdf"</span><span class="p">);</span>
|
||
|
||
<span class="c1">// Create the app</span>
|
||
<span class="n">PeoplePCDApp</span> <span class="n">app</span><span class="p">(</span><span class="o">*</span><span class="n">capture</span><span class="p">);</span>
|
||
<span class="n">app</span><span class="p">.</span><span class="n">people_detector_</span><span class="p">.</span><span class="n">rdf_detector_</span> <span class="o">=</span> <span class="n">rdf</span><span class="p">;</span>
|
||
|
||
<span class="c1">// executing</span>
|
||
<span class="n">app</span><span class="p">.</span><span class="n">startMainLoop</span> <span class="p">();</span>
|
||
<span class="p">}</span>
|
||
<span class="k">catch</span> <span class="p">(</span><span class="k">const</span> <span class="n">pcl</span><span class="o">::</span><span class="n">PCLException</span><span class="o">&</span> <span class="n">e</span><span class="p">)</span> <span class="p">{</span> <span class="n">std</span><span class="o">::</span><span class="n">cout</span> <span class="o"><<</span> <span class="s">"PCLException: "</span> <span class="o"><<</span> <span class="n">e</span><span class="p">.</span><span class="n">detailedMessage</span><span class="p">()</span> <span class="o"><<</span> <span class="n">std</span><span class="o">::</span><span class="n">endl</span><span class="p">;</span> <span class="p">}</span>
|
||
<span class="k">catch</span> <span class="p">(</span><span class="k">const</span> <span class="n">std</span><span class="o">::</span><span class="n">runtime_error</span><span class="o">&</span> <span class="n">e</span><span class="p">)</span> <span class="p">{</span> <span class="n">std</span><span class="o">::</span><span class="n">cout</span> <span class="o"><<</span> <span class="n">e</span><span class="p">.</span><span class="n">what</span><span class="p">()</span> <span class="o"><<</span> <span class="n">std</span><span class="o">::</span><span class="n">endl</span><span class="p">;</span> <span class="p">}</span>
|
||
<span class="k">catch</span> <span class="p">(</span><span class="k">const</span> <span class="n">std</span><span class="o">::</span><span class="n">bad_alloc</span><span class="o">&</span> <span class="cm">/*e*/</span><span class="p">)</span> <span class="p">{</span> <span class="n">std</span><span class="o">::</span><span class="n">cout</span> <span class="o"><<</span> <span class="s">"Bad alloc"</span> <span class="o"><<</span> <span class="n">std</span><span class="o">::</span><span class="n">endl</span><span class="p">;</span> <span class="p">}</span>
|
||
<span class="k">catch</span> <span class="p">(</span><span class="k">const</span> <span class="n">std</span><span class="o">::</span><span class="n">exception</span><span class="o">&</span> <span class="cm">/*e*/</span><span class="p">)</span> <span class="p">{</span> <span class="n">std</span><span class="o">::</span><span class="n">cout</span> <span class="o"><<</span> <span class="s">"Exception"</span> <span class="o"><<</span> <span class="n">std</span><span class="o">::</span><span class="n">endl</span><span class="p">;</span> <span class="p">}</span>
|
||
|
||
<span class="k">return</span> <span class="mi">0</span><span class="p">;</span>
|
||
<span class="p">}</span>
|
||
</pre></div>
|
||
</div>
|
||
<p>First the GPU device is set, by default this is the first GPU found in the bus, but if you have multiple GPU’s in
|
||
your system, this allows you to select a specific one.
|
||
Then a OpenNI Capture is made, see the OpenNI Grabber tutorial for more info on this. (TODO add link)</p>
|
||
<div class="highlight-cpp notranslate"><div class="highlight"><pre><span></span> <span class="n">std</span><span class="o">::</span><span class="n">vector</span><span class="o"><</span><span class="n">std</span><span class="o">::</span><span class="n">string</span><span class="o">></span> <span class="n">tree_files</span><span class="p">;</span>
|
||
<span class="n">tree_files</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="s">"Data/forest1/tree_20.txt"</span><span class="p">);</span>
|
||
<span class="n">tree_files</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="s">"Data/forest2/tree_20.txt"</span><span class="p">);</span>
|
||
<span class="n">tree_files</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="s">"Data/forest3/tree_20.txt"</span><span class="p">);</span>
|
||
<span class="n">tree_files</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="s">"Data/forest4/tree_20.txt"</span><span class="p">);</span>
|
||
|
||
<span class="n">pc</span><span class="o">::</span><span class="n">parse_argument</span> <span class="p">(</span><span class="n">argc</span><span class="p">,</span> <span class="n">argv</span><span class="p">,</span> <span class="s">"-tree0"</span><span class="p">,</span> <span class="n">tree_files</span><span class="p">[</span><span class="mi">0</span><span class="p">]);</span>
|
||
<span class="n">pc</span><span class="o">::</span><span class="n">parse_argument</span> <span class="p">(</span><span class="n">argc</span><span class="p">,</span> <span class="n">argv</span><span class="p">,</span> <span class="s">"-tree1"</span><span class="p">,</span> <span class="n">tree_files</span><span class="p">[</span><span class="mi">1</span><span class="p">]);</span>
|
||
<span class="n">pc</span><span class="o">::</span><span class="n">parse_argument</span> <span class="p">(</span><span class="n">argc</span><span class="p">,</span> <span class="n">argv</span><span class="p">,</span> <span class="s">"-tree2"</span><span class="p">,</span> <span class="n">tree_files</span><span class="p">[</span><span class="mi">2</span><span class="p">]);</span>
|
||
<span class="n">pc</span><span class="o">::</span><span class="n">parse_argument</span> <span class="p">(</span><span class="n">argc</span><span class="p">,</span> <span class="n">argv</span><span class="p">,</span> <span class="s">"-tree3"</span><span class="p">,</span> <span class="n">tree_files</span><span class="p">[</span><span class="mi">3</span><span class="p">]);</span>
|
||
</pre></div>
|
||
</div>
|
||
<p>The implementation is based on a similar approach as Shotton et al. and thus needs off-line learned random
|
||
decision forests for labeling. The current implementation allows up to 4 decision trees to be loaded into
|
||
the forest. This is done by giving it the names of the text files to load.</p>
|
||
<div class="highlight-cpp notranslate"><div class="highlight"><pre><span></span> <span class="kt">int</span> <span class="n">num_trees</span> <span class="o">=</span> <span class="p">(</span><span class="kt">int</span><span class="p">)</span><span class="n">tree_files</span><span class="p">.</span><span class="n">size</span><span class="p">();</span>
|
||
<span class="n">pc</span><span class="o">::</span><span class="n">parse_argument</span> <span class="p">(</span><span class="n">argc</span><span class="p">,</span> <span class="n">argv</span><span class="p">,</span> <span class="s">"-numTrees"</span><span class="p">,</span> <span class="n">num_trees</span><span class="p">);</span>
|
||
</pre></div>
|
||
</div>
|
||
<p>An additional parameter allows you to configure the number of trees to be loaded.</p>
|
||
<div class="highlight-cpp notranslate"><div class="highlight"><pre><span></span> <span class="k">typedef</span> <span class="n">pcl</span><span class="o">::</span><span class="n">gpu</span><span class="o">::</span><span class="n">people</span><span class="o">::</span><span class="n">RDFBodyPartsDetector</span> <span class="n">RDFBodyPartsDetector</span><span class="p">;</span>
|
||
<span class="n">RDFBodyPartsDetector</span><span class="o">::</span><span class="n">Ptr</span> <span class="n">rdf</span><span class="p">(</span><span class="k">new</span> <span class="n">RDFBodyPartsDetector</span><span class="p">(</span><span class="n">tree_files</span><span class="p">));</span>
|
||
<span class="n">PCL_INFO</span><span class="p">(</span><span class="s">"Loaded files into rdf"</span><span class="p">);</span>
|
||
</pre></div>
|
||
</div>
|
||
<p>Then the RDF object is created, loading the trees upon creation.</p>
|
||
<div class="highlight-cpp notranslate"><div class="highlight"><pre><span></span> <span class="c1">// Create the app</span>
|
||
<span class="n">PeoplePCDApp</span> <span class="nf">app</span><span class="p">(</span><span class="o">*</span><span class="n">capture</span><span class="p">);</span>
|
||
<span class="n">app</span><span class="p">.</span><span class="n">people_detector_</span><span class="p">.</span><span class="n">rdf_detector_</span> <span class="o">=</span> <span class="n">rdf</span><span class="p">;</span>
|
||
|
||
<span class="c1">// executing</span>
|
||
<span class="n">app</span><span class="p">.</span><span class="n">startMainLoop</span> <span class="p">();</span>
|
||
</pre></div>
|
||
</div>
|
||
<p>Now we create the application object, give it the pointer to the RDF object and start the loop.
|
||
Now we’ll have a look at the main loop.</p>
|
||
<div class="highlight-cpp notranslate"><div class="highlight"><pre><span></span> <span class="kt">void</span>
|
||
<span class="nf">startMainLoop</span> <span class="p">()</span>
|
||
<span class="p">{</span>
|
||
<span class="n">cloud_cb_</span> <span class="o">=</span> <span class="nb">false</span><span class="p">;</span>
|
||
|
||
<span class="n">PCDGrabberBase</span><span class="o">*</span> <span class="n">ispcd</span> <span class="o">=</span> <span class="k">dynamic_cast</span><span class="o"><</span><span class="n">pcl</span><span class="o">::</span><span class="n">PCDGrabberBase</span><span class="o">*></span><span class="p">(</span><span class="o">&</span><span class="n">capture_</span><span class="p">);</span>
|
||
<span class="k">if</span> <span class="p">(</span><span class="n">ispcd</span><span class="p">)</span>
|
||
<span class="n">cloud_cb_</span><span class="o">=</span> <span class="nb">true</span><span class="p">;</span>
|
||
|
||
<span class="k">typedef</span> <span class="n">openni_wrapper</span><span class="o">::</span><span class="n">DepthImage</span><span class="o">::</span><span class="n">Ptr</span> <span class="n">DepthImagePtr</span><span class="p">;</span>
|
||
<span class="k">typedef</span> <span class="n">openni_wrapper</span><span class="o">::</span><span class="n">Image</span><span class="o">::</span><span class="n">Ptr</span> <span class="n">ImagePtr</span><span class="p">;</span>
|
||
|
||
<span class="n">std</span><span class="o">::</span><span class="n">function</span><span class="o"><</span><span class="kt">void</span> <span class="p">(</span><span class="k">const</span> <span class="n">PointCloud</span><span class="o"><</span><span class="n">PointXYZRGBA</span><span class="o">>::</span><span class="n">ConstPtr</span><span class="o">&</span><span class="p">)</span><span class="o">></span> <span class="n">func1</span> <span class="o">=</span> <span class="p">[</span><span class="k">this</span><span class="p">]</span> <span class="p">(</span><span class="k">const</span> <span class="n">PointCloud</span><span class="o"><</span><span class="n">PointXYZRGBA</span><span class="o">>::</span><span class="n">ConstPtr</span><span class="o">&</span> <span class="n">cloud</span><span class="p">)</span> <span class="p">{</span> <span class="n">source_cb1</span> <span class="p">(</span><span class="n">cloud</span><span class="p">);</span> <span class="p">};</span>
|
||
<span class="n">std</span><span class="o">::</span><span class="n">function</span><span class="o"><</span><span class="kt">void</span> <span class="p">(</span><span class="k">const</span> <span class="n">ImagePtr</span><span class="o">&</span><span class="p">,</span> <span class="k">const</span> <span class="n">DepthImagePtr</span><span class="o">&</span><span class="p">,</span> <span class="kt">float</span><span class="p">)</span><span class="o">></span> <span class="n">func2</span> <span class="o">=</span> <span class="p">[</span><span class="k">this</span><span class="p">]</span> <span class="p">(</span><span class="k">const</span> <span class="n">ImagePtr</span><span class="o">&</span> <span class="n">img</span><span class="p">,</span> <span class="k">const</span> <span class="n">DepthImagePtr</span><span class="o">&</span> <span class="n">depth</span><span class="p">,</span> <span class="kt">float</span> <span class="n">constant</span><span class="p">)</span>
|
||
<span class="p">{</span>
|
||
<span class="n">source_cb2</span> <span class="p">(</span><span class="n">img</span><span class="p">,</span> <span class="n">depth</span><span class="p">,</span> <span class="n">constant</span><span class="p">);</span>
|
||
<span class="p">};</span>
|
||
<span class="n">boost</span><span class="o">::</span><span class="n">signals2</span><span class="o">::</span><span class="n">connection</span> <span class="n">c</span> <span class="o">=</span> <span class="n">cloud_cb_</span> <span class="o">?</span> <span class="n">capture_</span><span class="p">.</span><span class="n">registerCallback</span> <span class="p">(</span><span class="n">func1</span><span class="p">)</span> <span class="o">:</span> <span class="n">capture_</span><span class="p">.</span><span class="n">registerCallback</span> <span class="p">(</span><span class="n">func2</span><span class="p">);</span>
|
||
|
||
<span class="p">{</span>
|
||
<span class="n">std</span><span class="o">::</span><span class="n">unique_lock</span><span class="o"><</span><span class="n">std</span><span class="o">::</span><span class="n">mutex</span><span class="o">></span> <span class="n">lock</span><span class="p">(</span><span class="n">data_ready_mutex_</span><span class="p">);</span>
|
||
|
||
<span class="k">try</span>
|
||
<span class="p">{</span>
|
||
<span class="n">capture_</span><span class="p">.</span><span class="n">start</span> <span class="p">();</span>
|
||
<span class="k">while</span> <span class="p">(</span><span class="o">!</span><span class="n">exit_</span> <span class="o">&&</span> <span class="o">!</span><span class="n">final_view_</span><span class="p">.</span><span class="n">wasStopped</span><span class="p">())</span>
|
||
<span class="p">{</span>
|
||
<span class="kt">bool</span> <span class="n">has_data</span> <span class="o">=</span> <span class="p">(</span><span class="n">data_ready_cond_</span><span class="p">.</span><span class="n">wait_for</span><span class="p">(</span><span class="n">lock</span><span class="p">,</span> <span class="mi">100</span><span class="n">ms</span><span class="p">)</span> <span class="o">==</span> <span class="n">std</span><span class="o">::</span><span class="n">cv_status</span><span class="o">::</span><span class="n">no_timeout</span><span class="p">);</span>
|
||
<span class="k">if</span><span class="p">(</span><span class="n">has_data</span><span class="p">)</span>
|
||
<span class="p">{</span>
|
||
<span class="n">SampledScopeTime</span> <span class="n">fps</span><span class="p">(</span><span class="n">time_ms_</span><span class="p">);</span>
|
||
|
||
<span class="k">if</span> <span class="p">(</span><span class="n">cloud_cb_</span><span class="p">)</span>
|
||
<span class="n">process_return_</span> <span class="o">=</span> <span class="n">people_detector_</span><span class="p">.</span><span class="n">process</span><span class="p">(</span><span class="n">cloud_host_</span><span class="p">.</span><span class="n">makeShared</span><span class="p">());</span>
|
||
<span class="k">else</span>
|
||
<span class="n">process_return_</span> <span class="o">=</span> <span class="n">people_detector_</span><span class="p">.</span><span class="n">process</span><span class="p">(</span><span class="n">depth_device_</span><span class="p">,</span> <span class="n">image_device_</span><span class="p">);</span>
|
||
|
||
<span class="o">++</span><span class="n">counter_</span><span class="p">;</span>
|
||
<span class="p">}</span>
|
||
|
||
<span class="k">if</span><span class="p">(</span><span class="n">has_data</span> <span class="o">&&</span> <span class="p">(</span><span class="n">process_return_</span> <span class="o">==</span> <span class="mi">2</span><span class="p">))</span>
|
||
<span class="n">visualizeAndWrite</span><span class="p">();</span>
|
||
<span class="p">}</span>
|
||
<span class="n">final_view_</span><span class="p">.</span><span class="n">spinOnce</span> <span class="p">(</span><span class="mi">3</span><span class="p">);</span>
|
||
<span class="p">}</span>
|
||
<span class="k">catch</span> <span class="p">(</span><span class="k">const</span> <span class="n">std</span><span class="o">::</span><span class="n">bad_alloc</span><span class="o">&</span> <span class="cm">/*e*/</span><span class="p">)</span> <span class="p">{</span> <span class="n">std</span><span class="o">::</span><span class="n">cout</span> <span class="o"><<</span> <span class="s">"Bad alloc"</span> <span class="o"><<</span> <span class="n">std</span><span class="o">::</span><span class="n">endl</span><span class="p">;</span> <span class="p">}</span>
|
||
<span class="k">catch</span> <span class="p">(</span><span class="k">const</span> <span class="n">std</span><span class="o">::</span><span class="n">exception</span><span class="o">&</span> <span class="cm">/*e*/</span><span class="p">)</span> <span class="p">{</span> <span class="n">std</span><span class="o">::</span><span class="n">cout</span> <span class="o"><<</span> <span class="s">"Exception"</span> <span class="o"><<</span> <span class="n">std</span><span class="o">::</span><span class="n">endl</span><span class="p">;</span> <span class="p">}</span>
|
||
|
||
<span class="n">capture_</span><span class="p">.</span><span class="n">stop</span> <span class="p">();</span>
|
||
<span class="p">}</span>
|
||
<span class="n">c</span><span class="p">.</span><span class="n">disconnect</span><span class="p">();</span>
|
||
<span class="p">}</span>
|
||
</pre></div>
|
||
</div>
|
||
<p>This routine first connects a callback routine to the grabber and waits for valid data to arrive.
|
||
Each time the data arrives it will call the process function of the people detector, this is a
|
||
fully encapsulated method and will call the complete pipeline.
|
||
Once the pipeline completed processing, the results can be fetched as public structs or methods from the
|
||
people detector object. Have a look at doc.pointclouds.org for more documentation on the
|
||
available structs and methods.
|
||
The visualizeAndWrite method will illustrate one of the available methods of the people detector object:</p>
|
||
<div class="highlight-cpp notranslate"><div class="highlight"><pre><span></span> <span class="kt">void</span>
|
||
<span class="nf">visualizeAndWrite</span><span class="p">(</span><span class="kt">bool</span> <span class="n">write</span> <span class="o">=</span> <span class="nb">false</span><span class="p">)</span>
|
||
<span class="p">{</span>
|
||
<span class="k">const</span> <span class="n">PeopleDetector</span><span class="o">::</span><span class="n">Labels</span><span class="o">&</span> <span class="n">labels</span> <span class="o">=</span> <span class="n">people_detector_</span><span class="p">.</span><span class="n">rdf_detector_</span><span class="o">-></span><span class="n">getLabels</span><span class="p">();</span>
|
||
<span class="n">people</span><span class="o">::</span><span class="n">colorizeLabels</span><span class="p">(</span><span class="n">color_map_</span><span class="p">,</span> <span class="n">labels</span><span class="p">,</span> <span class="n">cmap_device_</span><span class="p">);</span>
|
||
|
||
<span class="kt">int</span> <span class="n">c</span><span class="p">;</span>
|
||
<span class="n">cmap_host_</span><span class="p">.</span><span class="n">width</span> <span class="o">=</span> <span class="n">cmap_device_</span><span class="p">.</span><span class="n">cols</span><span class="p">();</span>
|
||
<span class="n">cmap_host_</span><span class="p">.</span><span class="n">height</span> <span class="o">=</span> <span class="n">cmap_device_</span><span class="p">.</span><span class="n">rows</span><span class="p">();</span>
|
||
<span class="n">cmap_host_</span><span class="p">.</span><span class="n">resize</span><span class="p">(</span><span class="n">cmap_host_</span><span class="p">.</span><span class="n">width</span> <span class="o">*</span> <span class="n">cmap_host_</span><span class="p">.</span><span class="n">height</span><span class="p">);</span>
|
||
<span class="n">cmap_device_</span><span class="p">.</span><span class="n">download</span><span class="p">(</span><span class="n">cmap_host_</span><span class="p">.</span><span class="n">points</span><span class="p">,</span> <span class="n">c</span><span class="p">);</span>
|
||
|
||
<span class="n">final_view_</span><span class="p">.</span><span class="n">showRGBImage</span><span class="o"><</span><span class="n">pcl</span><span class="o">::</span><span class="n">RGB</span><span class="o">></span><span class="p">(</span><span class="n">cmap_host_</span><span class="p">);</span>
|
||
<span class="n">final_view_</span><span class="p">.</span><span class="n">spinOnce</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="nb">true</span><span class="p">);</span>
|
||
|
||
<span class="k">if</span> <span class="p">(</span><span class="n">cloud_cb_</span><span class="p">)</span>
|
||
<span class="p">{</span>
|
||
<span class="n">depth_host_</span><span class="p">.</span><span class="n">width</span> <span class="o">=</span> <span class="n">people_detector_</span><span class="p">.</span><span class="n">depth_device1_</span><span class="p">.</span><span class="n">cols</span><span class="p">();</span>
|
||
<span class="n">depth_host_</span><span class="p">.</span><span class="n">height</span> <span class="o">=</span> <span class="n">people_detector_</span><span class="p">.</span><span class="n">depth_device1_</span><span class="p">.</span><span class="n">rows</span><span class="p">();</span>
|
||
<span class="n">depth_host_</span><span class="p">.</span><span class="n">resize</span><span class="p">(</span><span class="n">depth_host_</span><span class="p">.</span><span class="n">width</span> <span class="o">*</span> <span class="n">depth_host_</span><span class="p">.</span><span class="n">height</span><span class="p">);</span>
|
||
<span class="n">people_detector_</span><span class="p">.</span><span class="n">depth_device1_</span><span class="p">.</span><span class="n">download</span><span class="p">(</span><span class="n">depth_host_</span><span class="p">.</span><span class="n">points</span><span class="p">,</span> <span class="n">c</span><span class="p">);</span>
|
||
<span class="p">}</span>
|
||
|
||
<span class="n">depth_view_</span><span class="p">.</span><span class="n">showShortImage</span><span class="p">(</span><span class="o">&</span><span class="n">depth_host_</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">depth_host_</span><span class="p">.</span><span class="n">width</span><span class="p">,</span> <span class="n">depth_host_</span><span class="p">.</span><span class="n">height</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">5000</span><span class="p">,</span> <span class="nb">true</span><span class="p">);</span>
|
||
<span class="n">depth_view_</span><span class="p">.</span><span class="n">spinOnce</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="nb">true</span><span class="p">);</span>
|
||
|
||
<span class="k">if</span> <span class="p">(</span><span class="n">write</span><span class="p">)</span>
|
||
<span class="p">{</span>
|
||
<span class="k">if</span> <span class="p">(</span><span class="n">cloud_cb_</span><span class="p">)</span>
|
||
<span class="n">savePNGFile</span><span class="p">(</span><span class="n">make_name</span><span class="p">(</span><span class="n">counter_</span><span class="p">,</span> <span class="s">"ii"</span><span class="p">),</span> <span class="n">cloud_host_</span><span class="p">);</span>
|
||
<span class="k">else</span>
|
||
<span class="n">savePNGFile</span><span class="p">(</span><span class="n">make_name</span><span class="p">(</span><span class="n">counter_</span><span class="p">,</span> <span class="s">"ii"</span><span class="p">),</span> <span class="n">rgba_host_</span><span class="p">);</span>
|
||
<span class="n">savePNGFile</span><span class="p">(</span><span class="n">make_name</span><span class="p">(</span><span class="n">counter_</span><span class="p">,</span> <span class="s">"c2"</span><span class="p">),</span> <span class="n">cmap_host_</span><span class="p">);</span>
|
||
<span class="n">savePNGFile</span><span class="p">(</span><span class="n">make_name</span><span class="p">(</span><span class="n">counter_</span><span class="p">,</span> <span class="s">"s2"</span><span class="p">),</span> <span class="n">labels</span><span class="p">);</span>
|
||
<span class="n">savePNGFile</span><span class="p">(</span><span class="n">make_name</span><span class="p">(</span><span class="n">counter_</span><span class="p">,</span> <span class="s">"d1"</span><span class="p">),</span> <span class="n">people_detector_</span><span class="p">.</span><span class="n">depth_device1_</span><span class="p">);</span>
|
||
<span class="n">savePNGFile</span><span class="p">(</span><span class="n">make_name</span><span class="p">(</span><span class="n">counter_</span><span class="p">,</span> <span class="s">"d2"</span><span class="p">),</span> <span class="n">people_detector_</span><span class="p">.</span><span class="n">depth_device2_</span><span class="p">);</span>
|
||
<span class="p">}</span>
|
||
<span class="p">}</span>
|
||
</pre></div>
|
||
</div>
|
||
<p>Line 143 calls the RDF getLabels method which returns the labels on the device, these however
|
||
are a discrete enum of the labels and are visually hard to recognize, so these are converted to
|
||
colors that illustrate each body part in line 144.
|
||
At this point the results are still stored in the device memory and need to be copied to the CPU
|
||
host memory, this is done in line 150. Afterwards the images are shown and stored to disk.</p>
|
||
</div>
|
||
<div class="section" id="compiling-and-running-the-program">
|
||
<h1>Compiling and running the program</h1>
|
||
<p>Add the following lines to your CMakeLists.txt file:</p>
|
||
<div class="highlight-cmake notranslate"><table class="highlighttable"><tr><td class="linenos"><div class="linenodiv"><pre> 1
|
||
2
|
||
3
|
||
4
|
||
5
|
||
6
|
||
7
|
||
8
|
||
9
|
||
10
|
||
11
|
||
12
|
||
13
|
||
14
|
||
15
|
||
16
|
||
17
|
||
18</pre></div></td><td class="code"><div class="highlight"><pre><span></span><span class="nb">cmake_minimum_required</span><span class="p">(</span><span class="s">VERSION</span> <span class="s">3.5</span> <span class="s">FATAL_ERROR</span><span class="p">)</span>
|
||
|
||
<span class="nb">project</span><span class="p">(</span><span class="s">people_detect</span><span class="p">)</span>
|
||
|
||
<span class="nb">find_package</span><span class="p">(</span><span class="s">PCL</span> <span class="s">1.7</span> <span class="s">REQUIRED</span><span class="p">)</span>
|
||
|
||
<span class="nb">include_directories</span><span class="p">(</span><span class="o">${</span><span class="nv">PCL_INCLUDE_DIRS</span><span class="o">}</span><span class="p">)</span>
|
||
<span class="nb">link_directories</span><span class="p">(</span><span class="o">${</span><span class="nv">PCL_LIBRARY_DIRS</span><span class="o">}</span><span class="p">)</span>
|
||
<span class="nb">add_definitions</span><span class="p">(</span><span class="o">${</span><span class="nv">PCL_DEFINITIONS</span><span class="o">}</span><span class="p">)</span>
|
||
|
||
<span class="c">#Searching CUDA</span>
|
||
<span class="nb">find_package</span><span class="p">(</span><span class="s">CUDA</span><span class="p">)</span>
|
||
|
||
<span class="c">#Include the FindCUDA script</span>
|
||
<span class="nb">include</span><span class="p">(</span><span class="s">FindCUDA</span><span class="p">)</span>
|
||
|
||
<span class="nb">cuda_add_executable</span> <span class="p">(</span><span class="s">people_detect</span> <span class="s">src/people_detect.cpp</span><span class="p">)</span>
|
||
<span class="nb">target_link_libraries</span> <span class="p">(</span><span class="s">people_detect</span> <span class="o">${</span><span class="nv">PCL_LIBRARIES</span><span class="o">}</span><span class="p">)</span>
|
||
</pre></div>
|
||
</td></tr></table></div>
|
||
<dl class="docutils">
|
||
<dt>After you have made the executable, you can run it. Simply do:</dt>
|
||
<dd>$ ./people_detect</dd>
|
||
</dl>
|
||
</div>
|
||
|
||
|
||
</div>
|
||
|
||
</div>
|
||
<footer>
|
||
|
||
|
||
<hr/>
|
||
|
||
<div role="contentinfo">
|
||
<p>
|
||
© Copyright
|
||
|
||
</p>
|
||
</div>
|
||
Built with <a href="http://sphinx-doc.org/">Sphinx</a> using a <a href="https://github.com/rtfd/sphinx_rtd_theme">theme</a> provided by <a href="https://readthedocs.org">Read the Docs</a>.
|
||
|
||
</footer>
|
||
|
||
</div>
|
||
</div>
|
||
|
||
</section>
|
||
|
||
</div>
|
||
|
||
|
||
|
||
<script type="text/javascript">
|
||
jQuery(function () {
|
||
SphinxRtdTheme.Navigation.enable(true);
|
||
});
|
||
</script>
|
||
|
||
|
||
|
||
|
||
|
||
|
||
</body>
|
||
</html> |