diff --git a/CSM_MANUAL.html b/csm_manual.html similarity index 78% rename from CSM_MANUAL.html rename to csm_manual.html index ca0e1f88fc185eca2c45423bb50dcd66a6fa9f33..c96d705f878457bbcffabd7ee34ffa6483c2efcb 100644 --- a/CSM_MANUAL.html +++ b/csm_manual.html @@ -21,7 +21,7 @@ h2 { padding: 1em; margin-left: -1em;} </style> <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’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> <p>I created this package:</p> @@ -528,4 +528,122 @@ $ log2pdf -use estimate -in in.log -out out_estimate.pdf</code></pre> <p>Create the animation:</p> <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’t work</h3> + +<p>Actually, there are a million reasons for which it shouldn’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 – if your sensor is very noisy, like an Hokuyo, it’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> — and you should! — 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("CSM_LIBRARY_DIRS: ${CSM_LIBRARY_DIRS}") + MESSAGE("CSM_LIBRARIES: ${CSM_LIBRARIES}") + MESSAGE("CSM_INCLUDE_DIRS: ${CSM_INCLUDE_DIRS}") + + INCLUDE_DIRECTORIES(${CSM_INCLUDE_DIRS}) # important! + LINK_DIRECTORIES(${CSM_LIBRARY_DIRS}) # important! +ELSE(${CSM_FOUND}) + MESSAGE(FATAL_ERROR "CSM not found. Check that the environment \ + variable PKG_CONFIG_PATH includes the path containing the file 'csm.pc'.") +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 <csm/csm_all.h></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 <csm/csm_all.h> +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><csm/algos.h></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->laser_ref</code>: pointer of a structure of type <code>laser_data</code> (described before in this document) representing the “ref”erence scan (first scan).</li> + +<li><code>params->laser_sens</code>: pointer of a structure of type <code>laser_data</code> representing the second scan.</li> + +<li><code>params->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> diff --git a/csm_manual.pdf b/csm_manual.pdf new file mode 100644 index 0000000000000000000000000000000000000000..1a1dec32bb487f1b7c835433dfdfc41c5aafe70c Binary files /dev/null and b/csm_manual.pdf differ diff --git a/docs/embedding.txt b/docs/embedding.txt index 79c036ed58d99c1685ed80cc71cfb2d14fe08feb..d33291678ab1ff06dbfe1604e252a17f6ae87bbf 100644 --- a/docs/embedding.txt +++ b/docs/embedding.txt @@ -63,3 +63,54 @@ You can download the sources for this example in the repository (directory `docs [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 + + + + + + + + diff --git a/sm/csm/algos.h b/sm/csm/algos.h index 85765c0cb4a30be9ca7369a88e286fe7cf686a2a..59a804239770e59b70e2dbc98e717c4f92d482c0 100644 --- a/sm/csm/algos.h +++ b/sm/csm/algos.h @@ -31,16 +31,12 @@ struct sm_params { /** A threshold for stopping. */ double epsilon_theta; - /** dubious parameter */ + /** Maximum distance for a correspondence to be valid */ double max_correspondence_dist; - - /** Noise in the scan */ - double sigma; - - /** Use smart tricks for finding correspondences. */ + /** Use smart tricks for finding correspondences. Only influences speed; not convergence. */ int use_corr_tricks; - /** Restart if error under threshold */ + /** Restart if error under threshold (0 or 1)*/ int restart; /** Threshold for restarting */ double restart_threshold_mean_error; @@ -50,24 +46,41 @@ struct sm_params { double restart_dtheta; - /** 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; + /* Functions concerning discarding correspondences. + THESE ARE MAGIC NUMBERS -- and they need to be tuned. */ - /** Discard correspondences based on the angles */ - int do_alpha_test; - double do_alpha_test_thresholdDeg; - - /** Percentage of correspondences to consider */ + /** Percentage of correspondences to consider: if 0.9, + always discard the top 10% of correspondences with more error */ double outliers_maxPerc; - double outliers_adaptive_order; /* 0.7 */ - double outliers_adaptive_mult; /* 2 */ + /** Parameters describing a simple adaptive algorithm for discarding. + 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; + + /* 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; /** If 1, use PlICP; if 0, use vanilla ICP. */ @@ -95,27 +108,37 @@ struct sm_params { the first estimate given the odometry. */ double laser[3]; + /** Noise in the scan */ + double sigma; + + /** mark as invalid ( = don't use ) rays outside of this interval */ double min_reading, max_reading; + /* Parameters specific to GPM (unfinished :-/ ) */ double gpm_theta_bin_size_deg; double gpm_extend_range_deg; int gpm_interval; - - + /* Parameter specific to HSM (unfinished :-/ ) */ struct hsm_params hsm; }; struct sm_result { + /** 1 if the result is valid */ int valid; + /** Scan matching result (x,y,theta) */ double x[3]; + /** Number of iterations done */ int iterations; + /** Number of valid correspondence in the end */ int nvalid; + /** Total correspondence error */ double error; + /** Fields used for covariance computation */ #ifndef RUBY gsl_matrix *cov_x_m; gsl_matrix *dx_dy1_m; @@ -127,6 +150,8 @@ struct sm_result { void sm_icp(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); + +/* Unfinished, untested :-/ */ void sm_mbcip(struct sm_params*input, struct sm_result*output);