andreacensi / csm Goto Github PK

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.

• open_room_70.* has two scans of 70 samples each (50 rays hit a wall and are valid).
• open_room_72.* has two scans of 72 samples each (51 rays hit a wall and are valid).

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 ?

open_room_tests.zip

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

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.

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.

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

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.

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.

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'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. The master version has the memory leak problem. When I run the laser_scan_matcher_node node, the memory leak was detected.

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

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 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".

As of today there isn't a release of csm for ROS Melodic.
Would it be possible to release it ?