search

SteveMacenski / slam_toolbox

Slam Toolbox for lifelong mapping and localization in potentially massive maps with ROS
GNU Lesser General Public License v2.1
1.65k stars 520 forks source link

SLAM Toolbox Localization Mode Performance #285

Closed jjbecomespheh closed 3 years ago

jjbecomespheh commented 4 years ago

Required Info:

Steps to reproduce issue

I have mapped out the environment with Slam Toolbox and have generated the serialised pose-graph data which I used for localization later on using the localization.launch launch file with localization mode enabled.

The first problem that I have is the Set 2D Pose Estimate in Rviz (/initialpose topic) doesn't work as how AMCL would work, setting the 2D Pose Estimate doesn't always bring the robot pose to the correct position. However, at some other places, it can be easier to set to the correct initial pose.

ezgif com-video-to-gif (1)

After setting the correct initial pose, Slam Toolbox is able to localize the robot as it moves around. However, localization is not as precise as AMCL or other localization methods with slight offset here and there as the robot moves. Most critically, at times or a certain part of the map, Slam Toolbox would "snap" out of localization and causes the map visualised to be skewed. I just want to check if this localization performance is expected.

ezgif com-video-to-gif

Expected [behavior]

Decent localization performance

Actual behavior

Poor initial pose registration Poor localization performance with instance of robot snapping out of localization

Additional information

SteveMacenski commented 4 years ago

I'd recommend using AMCL if after tuning the localization mode doesn't work well for your platform. It requires tuning and accurate odometry. Its not always suitable for all applications.

This is provided as an option amongst a number of options in the ecosystem to consider. There's no requirement to use it and each solution has the environmental / system strengths, I won't say that this is an end-all-be-all solution suited for every person.

cblesing commented 4 years ago

Hi,

we are facing with a similar problem. I don't want to create an own isssue for that. We use the toolbox for large scale mapping and are really satisfied with your work. We also use the toolbox in localization mode and this works fine (see the first video, speed 4x). The problem occurs when we increase the robot speed. In the first video we have a speed about aprox 0.1m/sec. In the second video the robot moves with 1.0m/sec. With that speed we get some localization "jumps" which rips our path following alorithm. We have tried to tune some parameters i.e the scan_buffer_size and get slightly better results. Can you give us some hints which paramters we can tune in addition? Our odometry is accurate and the laserscans come in with 25Hz both front and back scan but the back scan is not used at all at this moment. Thanks! gio_slow

gio_fast

SteveMacenski commented 4 years ago

The second video looks good to me - I'm not sure your issue. That seems like pretty reasonable performance that a little more dialing in could even further improve. The little bit of going off the path looks more like a function of your controller not being able to handle the speed than a positioning issue.

cblesing commented 4 years ago

@SteveMacenski thanks for your reply. You are right that it is hard to see our localization problem in the video. Therefore we have tried to produce a situation that is even worse and we recorded another one. Again our problem is that the localization is hanging behind when the vehicle rotates. As you can see, as soon as we take a turn, the scan no longer corresponds to the real world.

Do you have a hint which parameter could reduce this behaviour?

gio_fast_bad

SteveMacenski commented 4 years ago

I don't off hand, I haven't spent a great deal of time specifically trying to optimize the localizer parameters. I spent most of my time optimizing the parameters for the SLAM part so that folks had a great out of the box experience with that.

You would try reducing the penalties on changes in orientation and/or position so that if things appear to be a bit off, you're more likely to let it correct there vs try to modify. Something else to aid could be increasing the search space (within reason) but making the scan correlation parameters more strict. That could help let you search more space if you get off a bit from odometry but require a higher burden of proof that there's a quality match.

As I mention above, really, this is a niche technique if you read it. There's no MCL backend in this to help filter out individual bad poses. For applications I built it for, that was OK because even if the map deformed a little bit, that was fine for the type of autonomy we were using. I also found that if you just had great odometry, it was a non-issue because you didn't regularly have problems of deformations. This, however, might not be suitable for all applications. Things like AMCL that have a particle filter back end are still going to be more robust to arbitrary perturbations and noise.

I'm sorry if the localization mode doesn't meet your needs. Admittedly, if I had more time, I would have liked to augment the graph with some additional filters to make it more robust to those types of changes you see, but I wasn't able to get there. I'd be absolutely more than happy to chat about contributions if you like this technique but want to add some more robustness to it for your specific needs. However, I've had to largely move onto other projects because this met the goals I had at the time and something like this I could spend years on to make incremental changes (and there's so much more to do!).

cblesing commented 4 years ago

@SteveMacenski again thanks for your detailed reply! I will try your recommendations as soon as i'm in your lab again. I know about that particle filter back end of AMCL and we used it yesterday to have some comparison. To be honest, we didn't tune any AMCL param at all (except the required like topics etc.). The results with AMCL were much worse as with the toolbox. Of course the PF backend is a powerful technique but we want to stay with the elastic pose-graph localization and tune it al little bit more. If that does not work we will have a look at some additional filters for the pose graph.

SeungRyeol commented 3 years ago

@jjbecomespheh @cblesing

Hello,

I also want to use the Localization function.

I have a simple question.

Where should I move the ".posegraph" data saved through Rviz Plugin?

I changed the file name to test.posegraph and then set the "map_file_name" parameter value to "test" in mapper_params_localization.yaml.

I tried putting it in the config file folder, launch file folder and .ros folder, but I got the following error message.

[ERROR] [1605863596.351068649]: serialization::Read: Failed to read file: Exception: input stream error
[ERROR] [1605863596.351246987]: DeserializePoseGraph: Failed to read file: test.

I changed it like this, but it is the same. How can I solve this problem?

map_file_name: /home/amr/.ros/test
SteveMacenski commented 3 years ago

@cblesing any update here? Any reason to keep this ticket open?

SteveMacenski commented 3 years ago

https://github.com/SteveMacenski/slam_toolbox/issues/334

Pushing this discussion into #334 where we're making some headway of root cause.

@cblesing @jjbecomespheh Try turning off loop closures in localization mode, that might just fix your issue immediately.

cblesing commented 3 years ago

Hey,

i have read the #334 issue and yes you are right. Turning of loop closures in localization mode sounds perfectly reasonable to me with respect to the rolling scan window. I will give it a try.

Am Mi., 3. März 2021 um 20:49 Uhr schrieb Steve Macenski < notifications@github.com>:

334 https://github.com/SteveMacenski/slam_toolbox/issues/334

Pushing this discussion into #334 https://github.com/SteveMacenski/slam_toolbox/issues/334 where we're making some headway of root cause. Try turning off loop closures in localization mode, that might just fix your issue immediately.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/SteveMacenski/slam_toolbox/issues/285#issuecomment-790009430, or unsubscribe https://github.com/notifications/unsubscribe-auth/AHTKQ2EZTUKJGYRC2OHYIDLTB2HENANCNFSM4QLP44RQ .

SteveMacenski commented 3 years ago

Awesome, please do follow back and let me know. If it does solve your issues, I'll add a note to the readme about this in localization mode + change the default parameters for localization mode to disable this.