andreacensi / csm Goto Github PK
View Code? Open in Web Editor NEWThe C(canonical) Scan Matcher
Home Page: http://purl.org/censi/2007/csm
License: GNU Lesser General Public License v3.0
The C(canonical) Scan Matcher
Home Page: http://purl.org/censi/2007/csm
License: GNU Lesser General Public License v3.0
I couldn't build the csm_eigen
branch using catkin build
. Indeed, the top CMakeLists.txt
doesn't include (subdirs
) the one in src
, so the build passes but it doesn't do anything at all. See:
https://github.com/AndreaCensi/csm/blob/csm_eigen/CMakeLists.txt
Therefore, I've decided to fork and catkinize it. See this compare diff:
https://github.com/AndreaCensi/csm/compare/csm_eigen...clearpathrobotics:catkinize_csm_eigen?expand=1
@csprunk @RainerKuemmerle If you are OK with a change like that, I can open a PR.
The following gsl error is not handled properly leading to the program to crash:
gsl: lu.c:262: ERROR: matrix is singular
Default GSL error handler invoked.
Unfortunately this is not catchable from outer code and must be handled directly within csm
.
The solution is something along those lines:
// TURN OFF GSL ERROR ERROR HANDLER
// (GSL JUST PRINT ERROR MESSAGE AND KILL THE PROGRAM IF FLAG IN ON)
gsl_set_error_handler_off();
int status = gsl_function (...);
if (status) { /* an error occurred */
/* status value specifies the type of error */
/* deal with it */
}
However as of now I'm not sure which gsl functions spits this error.
There is one item "ld_*" in sm/.gitignore, so all the source files in sm/apps starting with "ld_" are not tracked by git.
./sm/apps/ld_smooth.c
./sm/apps/ld_resample.c
./sm/apps/ld_alternate.c
./sm/apps/ld_cluster_curv.c
./sm/apps/ld_recover.c
./sm/apps/ld_noise.c
./sm/apps/ld_remove_doubles.c
./sm/apps/ld_fisher.c
./sm/apps/ld_stats.c
./sm/apps/ld_select.c
./sm/apps/ld_exp_tro1.c
./sm/apps/ld_correct.c
./sm/apps/ld_slip.c
./sm/apps/ld_linearize.c
./sm/apps/ld_purify.c
I suggest that we should delete the item "ld_*" in sm/.gitignore
42! the answer for all!
https://github.com/AndreaCensi/csm/blob/master/sm/csm/icp/icp_corr_tricks.c#L86
In compute_next_estimate, the matrix C contains NaN if first and second correspondences are the same. csm then fails.
Here is a patch which corrects this, but I don't know the consequences of solving it this way:
From c8defacbba5fe42f283e44430a4f801522ef31c6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ga=C3=ABl=20Ecorchard?= <[email protected]>
Date: Thu, 23 Jul 2015 11:36:06 +0200
Subject: [PATCH] Fix normal computation
---
csm/sm/csm/icp/icp_loop.c | 13 ++++++++++---
1 file changed, 10 insertions(+), 3 deletions(-)
diff --git a/csm/sm/csm/icp/icp_loop.c b/csm/sm/csm/icp/icp_loop.c
index f64d17a..1495b55 100644
--- a/csm/sm/csm/icp/icp_loop.c
+++ b/csm/sm/csm/icp/icp_loop.c
@@ -200,10 +200,17 @@ int compute_next_estimate(struct sm_params*params,
double diff[2];
diff[0] = laser_ref->points[j1].p[0]-laser_ref->points[j2].p[0];
diff[1] = laser_ref->points[j1].p[1]-laser_ref->points[j2].p[1];
- double one_on_norm = 1 / sqrt(diff[0]*diff[0]+diff[1]*diff[1]);
+ const double length_diff = sqrt(diff[0]*diff[0]+diff[1]*diff[1]);
double normal[2];
- normal[0] = +diff[1] * one_on_norm;
- normal[1] = -diff[0] * one_on_norm;
+ if (length_diff < 1e-10) {
+ normal[0] = 1.0;
+ normal[1] = 0.0;
+ }
+ else {
+ double one_on_norm = 1 / length_diff;
+ normal[0] = +diff[1] * one_on_norm;
+ normal[1] = -diff[0] * one_on_norm;
+ }
double cos_alpha = normal[0];
double sin_alpha = normal[1];
--
1.9.1
Gaël
Hi. The master version has the memory leak problem. When I run the laser_scan_matcher_node node, the memory leak was detected.
Hello,
I am using CSM on master branch to implement an online sequential scan matching. Currently I am experiencing a strange issue, where the ICP starts failing abruptly after working nicely for several hundreds of scans without any visible reason or change in input data. While working with around 500~700 correspondences all the time, it drops to only 20 correspondences. What could be a reason for such a behaviour?
sm_icp: laser_sens has 721/721; laser_ref has 721/721 rays valid
sm_icp: laser_sens has 721/721; laser_ref has 721/721 rays valid (after removing outside interval [0.000000, 100.000000])
icp: starting at q0 = (0.00 mm, 0.00 mm, 0.0000 deg)
== icp_loop: starting iteration. 0
kill_outliers_double: killed 16 correspondences
icp_outliers: maxPerc 0.900000 error_limit: fix 0.296794 adaptive 0.589670
icp_outliers: valid 20/22 (limit: 0.296794) mean error = 0.183793
icp_loop: total error: 3.675868 valid 20 mean = 0.183793
icp_loop: failed: after trimming, only 20 correspondences.
icp: ICP failed for some reason.
Thanks,
Sebastian
Hello,
First of all, I would like to thank you for your work on the PLICP CSM library, and putting it open source. I am integrating it in our software after reading High-accuracy vehicle localization for autonomous warehousing by Goran Vasiljević et al., it has been a great help !
However, I have some questions about some behavior of your library. I have joined two set of data, describing a simple "room" with two perpendicular walls.
In each case, the reference scan is 50cm from each wall, and the sensor scan is 1cm farther from one wall, with a bit of noise added. In the case with 70 samples, the algorithm succeed (it also works with less than 70 samples), but with 72 samples or more, the algorithm fails to converge. Note that if the room is closed, the algorithm has no problem converging no matter the number of samples.
The .json files contains the inputs. The .jj files have been obtain using sm2 with the default parameters.
Is this an expected behavior for this algorithm ? If yes, why ? If no, is this a bug, or an issue with the other parameters ?
I sure that the frames are set rightly. And I use the command "rosrun tf tf_echo /base_link /laser", the result can be get. While I RUN the laser_scan_matcher node, the error noted above happened.
I'd like to get csm and https://github.com/CCNYRoboticsLab/scan_tools released for ros2 (galactic, foxy, etc) based on the csm_eigen version.
I created a ros2
branch on my fork, branched off from csm_eigen
: https://github.com/malban/csm/tree/ros2 that uses ament_cmake: https://docs.ros.org/en/foxy/How-To-Guides/Ament-CMake-Documentation.html.
Once a ros2 branch is made, then the repo can be bloomed to the ros2 releases: https://docs.ros.org/en/foxy/How-To-Guides/Releasing-a-ROS-2-package-with-bloom.html
Hi,friends. I'm using the matlab code. but i find some fuction are missing. for example log_bighouse.
the mistake is as following.
gpm_test2
Undefined function 'log_bighouse' for input arguments of type 'double'.test_yasmine
Error using load
Unable to read file logs/bighouse_half.mat: No such file or directory.test_icp2
Undefined function 'ld_add_noise' for input arguments of type 'struct'.
Error in test_icp2 (line 28)
params.laser_sens = ld_add_noise(params.laser_sens, params.sigma);
I've recently built CSM, and after reading the manual I was hoping to test it with some correct .log files, as my own appear to be failing. Regarding section 3.3 of the manual, does laserazosSM3.log or an alternative exists currently?
I understand this is more of a documentation bug than implementation. Thank you.
I've encountered the following problem while using csm in ros2_laser_scan_matcher (ros2 galactic, gazebo11 simulation). The output pose is mostly perfect, as there is no noise on the lidar sensor. However, sometimes it drifts, and gives an erroneous output. I've testes multiple times, and the pose always turns bad at specific places while only rotating. Translations are harder to test in a reproducible way. I've also tested on a real robot and the same phenomenon appears.
The following parameter values are used:
max_correspondence_dist: 100.0
max_iterations: 10
orientation_neighborhood: 40
The others are left at default. While trying different parameter settings, I noticed that setting max_correspondence_dist
low, such as 0.2 as stated in the docs intensifies the phenomenon, thus the high setting, but seemingly it saturates above a certain threshold.
Also, I opened an issue here, because I think if the ros wrapper was faulty, it would be faulty everywhere, and looking through its code, it really just forwards data between ros and csm.
I've recorded two videos from the simulation to demonstrate the problem. The first one is seen from the point of view of the laser scan matcher (from the odom frame, with the odom -> base_link transform provided by the laser scan matcher), and the second from the simulator's point of view, thus it is the real transform, unknown by the laser scan matcher. In both cases, ideally the visualized laser scan should remain stationary. The red arrow shows the real position of the robot, while the black one shows the output of the laser scan matcher.
As of today there isn't a release of csm for ROS Melodic.
Would it be possible to release it ?
I get the following error when running sudo make install
:
CMake Error at sm/cmake_install.cmake:36 (FILE):
file INSTALL cannot find "/home/gael/04-build/csm/csm/sm/sm1".
Thank you for making the great tool opensourced!
ROS wrapper of CSM, laser_scan_matcher downloads source code of CSM during build process. Right now it downloads by specifying the checksum of a commit. This may make us ponder which commit we should aim, when there are newer commits happen on CSM(*1). If there's a release tag in CSM, it's much easier on ROS end to just point to it.
Would you consider tagging a release?
*1 Currently laser_scan_matcher points to the commit in 2010. Ref. CCNYRoboticsLab/scan_tools#29
Thank you again.
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google ❤️ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.