Skip to content
Snippets Groups Projects
Commit c23ea8cc authored by Andrea Censi's avatar Andrea Censi
Browse files

More docs

parent f29fb7c0
No related branches found
No related tags found
No related merge requests found
...@@ -21,7 +21,7 @@ h2 { ...@@ -21,7 +21,7 @@ h2 {
padding: 1em; margin-left: -1em;} padding: 1em; margin-left: -1em;}
</style> </style>
<h1 id='the_canonical_scan_matcher'>The C(anonical) Scan Matcher</h1> <h1 id='the_canonical_scan_matcher'>The C(anonical) Scan Matcher</h1>
<div class='maruku_toc'><ul style='list-style: none;'><li><span class='maruku_section_number'>1. </span><a href='#introduction'>Introduction</a></li><li><span class='maruku_section_number'>2. </span><a href='#content_of_this_package'>Content of this package</a><ul style='list-style: none;'><li><span class='maruku_section_number'>2.1. </span><a href='#stable_things_c_scan_matching_library'>Stable things: C scan matching library</a></li><li><span class='maruku_section_number'>2.2. </span><a href='#stable_things_applications'>Stable things: applications</a></li><li><span class='maruku_section_number'>2.3. </span><a href='#unstable_things_scripts'>Unstable things: scripts</a></li><li><span class='maruku_section_number'>2.4. </span><a href='#unstable_things_ruby_and_matlab_implementations'>Unstable things: Ruby and Matlab implementations</a></li></ul></li><li><span class='maruku_section_number'>3. </span><a href='#installation'>Installation</a><ul style='list-style: none;'><li><span class='maruku_section_number'>3.1. </span><a href='#required_software_dependencies'>Required software dependencies</a></li><li><span class='maruku_section_number'>3.2. </span><a href='#compiling'>Compiling</a></li><li><span class='maruku_section_number'>3.3. </span><a href='#getting_started'>Getting started</a></li><li><span class='maruku_section_number'>3.4. </span><a href='#installing_ruby_libraries_and_wrapper_optional'>Installing Ruby libraries and wrapper (optional)</a></li></ul></li><li><span class='maruku_section_number'>4. </span><a href='#the__data_structure'>The <code>laser_data</code> data structure</a></li><li><span class='maruku_section_number'>5. </span><a href='#input_and_output_formats'>Input and output formats</a><ul style='list-style: none;'><li><span class='maruku_section_number'>5.1. </span><a href='#the_json_log_format'>The JSON log format</a></li><li><span class='maruku_section_number'>5.2. </span><a href='#the_carmen_log_format'>The Carmen log format</a><ul style='list-style: none;'><li><span class='maruku_section_number'>5.2.1. </span><a href='#regarding_the_timestamp'>Regarding the timestamp</a></li></ul></li></ul></li><li><span class='maruku_section_number'>6. </span><a href='#examples'>Examples</a><ul style='list-style: none;'><li><span class='maruku_section_number'>6.1. </span><a href='#simple_scan_matching'>Simple scan matching</a></li><li><span class='maruku_section_number'>6.2. </span><a href='#creating_a_pdf'>Creating a PDF</a></li><li><span class='maruku_section_number'>6.3. </span><a href='#examining_one_particular_matching_video'>Examining one particular matching (video)</a></li></ul></li></ul></div> <div class='maruku_toc'><ul style='list-style: none;'><li><span class='maruku_section_number'>1. </span><a href='#introduction'>Introduction</a></li><li><span class='maruku_section_number'>2. </span><a href='#content_of_this_package'>Content of this package</a><ul style='list-style: none;'><li><span class='maruku_section_number'>2.1. </span><a href='#stable_things_c_scan_matching_library'>Stable things: C scan matching library</a></li><li><span class='maruku_section_number'>2.2. </span><a href='#stable_things_applications'>Stable things: applications</a></li><li><span class='maruku_section_number'>2.3. </span><a href='#unstable_things_scripts'>Unstable things: scripts</a></li><li><span class='maruku_section_number'>2.4. </span><a href='#unstable_things_ruby_and_matlab_implementations'>Unstable things: Ruby and Matlab implementations</a></li></ul></li><li><span class='maruku_section_number'>3. </span><a href='#installation'>Installation</a><ul style='list-style: none;'><li><span class='maruku_section_number'>3.1. </span><a href='#required_software_dependencies'>Required software dependencies</a></li><li><span class='maruku_section_number'>3.2. </span><a href='#compiling'>Compiling</a></li><li><span class='maruku_section_number'>3.3. </span><a href='#getting_started'>Getting started</a></li><li><span class='maruku_section_number'>3.4. </span><a href='#installing_ruby_libraries_and_wrapper_optional'>Installing Ruby libraries and wrapper (optional)</a></li></ul></li><li><span class='maruku_section_number'>4. </span><a href='#the__data_structure'>The <code>laser_data</code> data structure</a></li><li><span class='maruku_section_number'>5. </span><a href='#input_and_output_formats'>Input and output formats</a><ul style='list-style: none;'><li><span class='maruku_section_number'>5.1. </span><a href='#the_json_log_format'>The JSON log format</a></li><li><span class='maruku_section_number'>5.2. </span><a href='#the_carmen_log_format'>The Carmen log format</a><ul style='list-style: none;'><li><span class='maruku_section_number'>5.2.1. </span><a href='#regarding_the_timestamp'>Regarding the timestamp</a></li></ul></li></ul></li><li><span class='maruku_section_number'>6. </span><a href='#examples'>Examples</a><ul style='list-style: none;'><li><span class='maruku_section_number'>6.1. </span><a href='#simple_scan_matching'>Simple scan matching</a></li><li><span class='maruku_section_number'>6.2. </span><a href='#creating_a_pdf'>Creating a PDF</a></li><li><span class='maruku_section_number'>6.3. </span><a href='#examining_one_particular_matching_video'>Examining one particular matching (video)</a></li><li><span class='maruku_section_number'>6.4. </span><a href='#help_icp_doesnt_work'>Help! ICP doesn&#8217;t work</a></li></ul></li><li><span class='maruku_section_number'>7. </span><a href='#embedding_csm_in_your_programs'>Embedding CSM in your programs</a><ul style='list-style: none;'><li><span class='maruku_section_number'>7.1. </span><a href='#linking_to_csm'>Linking to CSM</a></li><li><span class='maruku_section_number'>7.2. </span><a href='#accessing_csm_functions_from_your_applications'>Accessing CSM functions from your applications</a></li><li><span class='maruku_section_number'>7.3. </span><a href='#orienting_oneself_in_the_source_code'>Orienting oneself in the source code</a></li></ul></li></ul></div>
<h2 id='introduction'><span class='maruku_section_number'>1. </span>Introduction</h2> <h2 id='introduction'><span class='maruku_section_number'>1. </span>Introduction</h2>
<p>I created this package:</p> <p>I created this package:</p>
...@@ -528,4 +528,122 @@ $ log2pdf -use estimate -in in.log -out out_estimate.pdf</code></pre> ...@@ -528,4 +528,122 @@ $ log2pdf -use estimate -in in.log -out out_estimate.pdf</code></pre>
<p>Create the animation:</p> <p>Create the animation:</p>
<pre><code>$ sm_animate -in matching13.txt</code></pre> <pre><code>$ sm_animate -in matching13.txt</code></pre>
<h3 id='help_icp_doesnt_work'><span class='maruku_section_number'>6.4. </span>Help! ICP doesn&#8217;t work</h3>
<p>Actually, there are a million reasons for which it shouldn&#8217;t work. If it gives strange results, try the following:</p>
<ol>
<li>
<p>Plot the data! Plot the input and plot the output using <code>log2pdf</code>.</p>
</li>
<li>
<p>Plot the animation! Use the procedure above and inspect the resulting videos.</p>
</li>
<li>
<p>Double-check the parameters you are using. Note that there are some like <code>max_correspondence_dist</code> which depend on the scale of your data. A value of 2m might work for a big robot making large movements, but not for a little Khepera.</p>
</li>
<li>
<p>Smooth your data &#8211; if your sensor is very noisy, like an Hokuyo, it&#8217;s worth to do simple low-pass filtering. Especially for PLICP which uses the orientation information.</p>
</li>
</ol>
<h2 id='embedding_csm_in_your_programs'><span class='maruku_section_number'>7. </span>Embedding CSM in your programs</h2>
<h3 id='linking_to_csm'><span class='maruku_section_number'>7.1. </span>Linking to CSM</h3>
<p>When CSM is installed, a <a href='http://pkg-config.freedesktop.org/wiki/'>pkgconfig</a> <code>csm.pc</code> file is installed as well. This makes it easy to link to CSM.</p>
<p>For example, on my system, after installing CSM, I can run <code>pkgconfig</code> to get the C preprocessors and linker flags.</p>
<p>This is what I get on my system (on yours, paths will be different, of course).</p>
<pre><code> $ pkg-config --cflags csm
-I/sw/include -I/Users/andrea/svn/cds/csm/deploy/include/cairo
-I/Users/andrea/svn/cds/csm/deploy/include
$ pkg-config --libs csm
-L/sw/lib -L/Users/andrea/svn/cds/csm/deploy/lib
-lcsm-static -lgsl -lgslcblas -lm</code></pre>
<p>If you use GNU Make, a basic Makefile for your program linking to CSM would be something like:</p>
<pre><code>CSM_FLAGS=`pkg-config --libs --cflags csm`
myprogram: myprogram.c
gcc $(CSM_FLAGS) -o myprogram myprogram.c</code></pre>
<p>You can download the sources for this example in the repository (directory <code>docs/example-linking-make</code>).</p>
<p>If you use <a href='http://www.cmake.org/'>CMake</a> &#8212; and you should! &#8212; it is reccomended that you use something like the following in your <code>CMakeLists.txt</code>.</p>
<pre><code>cmake_minimum_required(VERSION 2.4)
project(myproject)
# Require we have pkgconfig installed
find_package(PkgConfig REQUIRED)
# Tell pkgconfig to look for CSM
pkg_check_modules(CSM REQUIRED csm)
IF(${CSM_FOUND})
MESSAGE(&quot;CSM_LIBRARY_DIRS: ${CSM_LIBRARY_DIRS}&quot;)
MESSAGE(&quot;CSM_LIBRARIES: ${CSM_LIBRARIES}&quot;)
MESSAGE(&quot;CSM_INCLUDE_DIRS: ${CSM_INCLUDE_DIRS}&quot;)
INCLUDE_DIRECTORIES(${CSM_INCLUDE_DIRS}) # important!
LINK_DIRECTORIES(${CSM_LIBRARY_DIRS}) # important!
ELSE(${CSM_FOUND})
MESSAGE(FATAL_ERROR &quot;CSM not found. Check that the environment \
variable PKG_CONFIG_PATH includes the path containing the file &#39;csm.pc&#39;.&quot;)
ENDIF(${CSM_FOUND})
add_executable(myprogram myprogram.c)
target_link_libraries(myprogram ${CSM_LIBRARIES}) # important! </code></pre>
<p>You can download the sources for this example in the repository (directory <code>docs/example-linking-cmake</code>).</p>
<h3 id='accessing_csm_functions_from_your_applications'><span class='maruku_section_number'>7.2. </span>Accessing CSM functions from your applications</h3>
<p>All functions that you would be interested in using are accessible by including one header:</p>
<pre><code>#include &lt;csm/csm_all.h&gt;</code></pre>
<p>If you are linking from C++, as opposed to C, all functions are enclosed in the <code>CSM</code> namespace. Therefore, you need something like the following.</p>
<pre><code>#include &lt;csm/csm_all.h&gt;
using namespace CSM;</code></pre>
<h3 id='orienting_oneself_in_the_source_code'><span class='maruku_section_number'>7.3. </span>Orienting oneself in the source code</h3>
<p>The main function to call is the following:</p>
<pre><code>void sm_icp(struct sm_params*params, struct sm_result*result);</code></pre>
<p>This implements matching between two laser scans. All the applications discussed above (<code>sm1</code>, <code>sm2</code>, etc.) are essentially wrapper of <code>sm_icp</code>: they fill in the <code>params</code> structure, and read from the <code>result</code> structure.</p>
<p>The <code>sm_params</code> structure is described in the <code>&lt;csm/algos.h&gt;</code> header file. It contains parameters for both ICP and other algorithms (like HSM; however, only (PL)ICP is considered stable in CSM)</p>
<p>Note that many of the parameters greatly influence the behavior of PLICP, so it is worth reading them all. If you run <code>sm2 -help</code> you will see the default values, which are reasonable as a starting point.</p>
<p>We now briefly discuss the main parameters.</p>
<ul>
<li><code>params-&gt;laser_ref</code>: pointer of a structure of type <code>laser_data</code> (described before in this document) representing the &#8220;ref&#8221;erence scan (first scan).</li>
<li><code>params-&gt;laser_sens</code>: pointer of a structure of type <code>laser_data</code> representing the second scan.</li>
<li><code>params-&gt;first_guess</code>: first guess (x,y,theta).</li>
</ul>
<p>Parameters that influence stopping:</p>
<ul>
<li><code>max_iterations</code>: maximum number of iterations</li>
<li><code>epsilon_xy</code>, <code>epsilon_theta</code>: stop if change below these thresholds</li>
</ul>
</body></html> </body></html>
File added
...@@ -63,3 +63,54 @@ You can download the sources for this example in the repository (directory `docs ...@@ -63,3 +63,54 @@ You can download the sources for this example in the repository (directory `docs
[CMake]: http://www.cmake.org/ [CMake]: http://www.cmake.org/
### Accessing CSM functions from your applications ###
All functions that you would be interested in using are accessible by including one header:
#include <csm/csm_all.h>
If you are linking from C++, as opposed to C, all functions are enclosed
in the `CSM` namespace. Therefore, you need something like the following.
#include <csm/csm_all.h>
using namespace CSM;
### Orienting oneself in the source code ###
The main function to call is the following:
void sm_icp(struct sm_params*params, struct sm_result*result);
This implements matching between two laser scans.
All the applications discussed above (``sm1``, ``sm2``, etc.) are essentially
wrapper of ``sm_icp``: they fill in the ``params`` structure, and read from the ``result`` structure.
The ``sm_params`` structure is described in the ``<csm/algos.h>`` header file.
It contains parameters for both ICP and other algorithms (like HSM; however, only (PL)ICP is considered stable in CSM)
Note that many of the parameters greatly influence the behavior of PLICP, so it
is worth reading them all. If you run ``sm2 -help`` you will see the default
values, which are reasonable as a starting point.
We now briefly discuss the main parameters.
* ``params->laser_ref``: pointer of a structure of type ``laser_data`` (described before in this document) representing
the "ref"erence scan (first scan).
* ``params->laser_sens``: pointer of a structure of type ``laser_data`` representing
the second scan.
* ``params->first_guess``: first guess (x,y,theta).
Parameters that influence stopping:
* ``max_iterations``: maximum number of iterations
* ``epsilon_xy``, ``epsilon_theta``: stop if change below these thresholds
...@@ -31,16 +31,12 @@ struct sm_params { ...@@ -31,16 +31,12 @@ struct sm_params {
/** A threshold for stopping. */ /** A threshold for stopping. */
double epsilon_theta; double epsilon_theta;
/** dubious parameter */ /** Maximum distance for a correspondence to be valid */
double max_correspondence_dist; double max_correspondence_dist;
/** Use smart tricks for finding correspondences. Only influences speed; not convergence. */
/** Noise in the scan */
double sigma;
/** Use smart tricks for finding correspondences. */
int use_corr_tricks; int use_corr_tricks;
/** Restart if error under threshold */ /** Restart if error under threshold (0 or 1)*/
int restart; int restart;
/** Threshold for restarting */ /** Threshold for restarting */
double restart_threshold_mean_error; double restart_threshold_mean_error;
...@@ -50,24 +46,41 @@ struct sm_params { ...@@ -50,24 +46,41 @@ struct sm_params {
double restart_dtheta; double restart_dtheta;
/** For now, a very simple max-distance clustering algorithm is used */ /* Functions concerning discarding correspondences.
double clustering_threshold; THESE ARE MAGIC NUMBERS -- and they need to be tuned. */
/** Number of neighbour rays used to estimate the orientation.*/
int orientation_neighbourhood;
/** Discard correspondences based on the angles */ /** Percentage of correspondences to consider: if 0.9,
int do_alpha_test; always discard the top 10% of correspondences with more error */
double do_alpha_test_thresholdDeg;
/** Percentage of correspondences to consider */
double outliers_maxPerc; double outliers_maxPerc;
double outliers_adaptive_order; /* 0.7 */ /** Parameters describing a simple adaptive algorithm for discarding.
double outliers_adaptive_mult; /* 2 */ 1) Order the errors.
2) Choose the percentile according to outliers_adaptive_order.
(if it is 0.7, get the 70% percentile)
3) Define an adaptive threshold multiplying outliers_adaptive_mult
with the value of the error at the chosen percentile.
4) Discard correspondences over the threshold.
This is useful to be conservative; yet remove the biggest errors.
*/
double outliers_adaptive_order; /* 0.7 */
double outliers_adaptive_mult; /* 2 */
/** Do not allow two different correspondences to share a point */
int outliers_remove_doubles; int outliers_remove_doubles;
/* Functions that compute and use point orientation for defining matches. */
/** For now, a very simple max-distance clustering algorithm is used */
double clustering_threshold;
/** Number of neighbour rays used to estimate the orientation.*/
int orientation_neighbourhood;
/** Discard correspondences based on the angles */
int do_alpha_test;
double do_alpha_test_thresholdDeg;
int do_visibility_test; int do_visibility_test;
/** If 1, use PlICP; if 0, use vanilla ICP. */ /** If 1, use PlICP; if 0, use vanilla ICP. */
...@@ -95,27 +108,37 @@ struct sm_params { ...@@ -95,27 +108,37 @@ struct sm_params {
the first estimate given the odometry. */ the first estimate given the odometry. */
double laser[3]; double laser[3];
/** Noise in the scan */
double sigma;
/** mark as invalid ( = don't use ) rays outside of this interval */ /** mark as invalid ( = don't use ) rays outside of this interval */
double min_reading, max_reading; double min_reading, max_reading;
/* Parameters specific to GPM (unfinished :-/ ) */
double gpm_theta_bin_size_deg; double gpm_theta_bin_size_deg;
double gpm_extend_range_deg; double gpm_extend_range_deg;
int gpm_interval; int gpm_interval;
/* Parameter specific to HSM (unfinished :-/ ) */
struct hsm_params hsm; struct hsm_params hsm;
}; };
struct sm_result { struct sm_result {
/** 1 if the result is valid */
int valid; int valid;
/** Scan matching result (x,y,theta) */
double x[3]; double x[3];
/** Number of iterations done */
int iterations; int iterations;
/** Number of valid correspondence in the end */
int nvalid; int nvalid;
/** Total correspondence error */
double error; double error;
/** Fields used for covariance computation */
#ifndef RUBY #ifndef RUBY
gsl_matrix *cov_x_m; gsl_matrix *cov_x_m;
gsl_matrix *dx_dy1_m; gsl_matrix *dx_dy1_m;
...@@ -127,6 +150,8 @@ struct sm_result { ...@@ -127,6 +150,8 @@ struct sm_result {
void sm_icp(struct sm_params*input, struct sm_result*output); void sm_icp(struct sm_params*input, struct sm_result*output);
void sm_gpm(struct sm_params*input, struct sm_result*output); void sm_gpm(struct sm_params*input, struct sm_result*output);
void sm_hsm(struct sm_params*input, struct sm_result*output); void sm_hsm(struct sm_params*input, struct sm_result*output);
/* Unfinished, untested :-/ */
void sm_mbcip(struct sm_params*input, struct sm_result*output); void sm_mbcip(struct sm_params*input, struct sm_result*output);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment