[Discussion] Mapping with Moving Obstacles

[SteveMacenski]

Hi,

I did some reading on distance function mapping and localization methods and I wondered if this has a problem with moving obstacles for feature/scan/(surface by your code?) matching.

A talk I was at at IROS explaining visually how their technique works showed that the features generated by free space are very sensitive to changes, which makes sense. So if you have something moving in free space while mapping, will that trip this up? For example people walking around while mapping.

If this is a problem, would it make sense the problem is worse in narrow passage ways than in larger open spaces?

I appreciate how clean and clear your code is, thank you for that. This was clearly built with love and it shows.

[eupedrosa]

Hi,

The distance function is used to calculate the matching error (or residual) and create the gradient that will attract the scan (or surface as I call it in the code) to the walls. Moving obstacles introduce outliers to the scan and that is a potential problem to the non linear least squares (nlls) optimization process. A single outlier can have enough energy to influence the outcome of the optimization. This is known problem of least squares.

To address this issue I have 2 mechanism:

1. Robust Fitting or (robust regression). I use the Cauchy M-Estimator (or loss function) that reduces the influence of an outlier. A point in the scan with a higher error will have its influence reduced by scaling down its residual and gradient. The Ceres Solver has a quick explanation.

2. The distance function is truncated to a maximum value. Any outlier with distance higher than this maximum will have zero impact, the gradient is null -- as a bonus, generating the distance map is faster with lower maximum values. For indoor environments I have been using 0.5 meters.

During mapping, a moving person is not a problem. It may originate a false object with the respective distance function, but (hopefully) it will be erase by new scans.

There as some situation where the scan matching will fail:

• Not enough points in the range of the distance function (mechanism 2).
• Most of the scan is an outlier.
• Bad initial estimation provided by the odometry.
• other things that I do not know (yet) ...

I appreciate how clean and clear your code is, thank you for that. This was clearly built with love and it shows.

And sometime !love 😉
Thank you.

[SteveMacenski]

I'm going to break this down a little bit to make sure I understand and ask a few follow ups. You (obviously) know more about distance function mapping techniques than me. Plus its rare to find an academic who not only writes good software but also makes it open source 🥇

The distance function is used to calculate the matching error (or residual) and create the gradient that will attract the scan (or surface as I call it in the code) to the walls.

Mhm, this sounds very different on the get-go from the talks and papers I read at IROS. So we have some mask on the map who's value at (x, y) is D(x,y) where D(a, b) is the distance between the cell and its closest occupied cell. create the gradient that's pretty clear, but how is that being used to match against the map obstacles (walls, etc)? I didn't see a very clear scan matcher so I figured it was a feature matcher based solution. The feature matching solutions are the ones I listened to that had particular issues with new objects in freespace.

I use the Cauchy M-Estimator (or loss function) that reduces the influence of an outlier

Got it, I know Ceres very well. I think the question in my last comment may help make clear to me what the measurements are going into the optimization problem and what's considered an "outlier" a continuous obstacle, a feature, etc).

The distance function is truncated to a maximum value

Ah that's nice. Computing a distance mask on the map for a large map sounds computational, but might still come out ahead than other methods, remains to be seen. So if I'm intepreting this correctly, you're only computing the distance mask for cells within 0.5m of an obstacle, and the rest are unvisited and set to 0 (null)? This could be comparable to the inflation layer.

It may originate a false object with the respective distance function, but (hopefully) it will be erase by new scans.

So dynamic obstacles in your environment being marked in the map I'm not concerned about. For the exact reasons you mention and this is a common artifact in all mapping techniques. Go over an old space again to wipe it out if you care (or use gimp later). I'm more concerned by the potential that dynamic obstacles mess with the distance mask because now there's a new object in the map at some position where the mask is now being computed or messing with the gradients from the walls. That then in turn messing with the scan matching and making the problem "blow up" as I showed in the video. I see it moving around for a rew cycles without a problem but its possible that starts to mess up on the first optimization run (not sure, would need to check) because something was inserted from a moving person in free space (or near a wall to really mess up the gradients needed).

There as some situation where the scan matching will fail:

I can say it happened so early and so consistently in the map, its not from poor odometry, that bag I've run with every 2D SLAM algorithm around and hasn't had an issue at that spot (10 seconds into the bag). They'll fail at scale later on, but not processing the first hallway. Not enough points in the range of the distance function (mechanism 2). I think I need a clarification on that point.

[eupedrosa]

but how is that being used to match against the map obstacles (walls, etc)? I didn't see a very clear scan matcher so I figured it was a feature matcher based solution.

The scan matcher is coded in match_surface_2d.cpp. For each point in the current scan it calculates the residuals (i.e. the distance to the closest object) and the respective gradient. The objective is to find the pose with the lowest sum of residuals. The nicest thing about this formulation is that I have no features (only occupied cells) and no (explicit) data association. I also make no assumptions about outliers. But I known they may exist, hence the robust fitting.
In short, the scan matcher is like ICP, where the reference scan is the map, with a few extra steps to avoid data association.

So if I'm intepreting this correctly, you're only computing the distance mask for cells within 0.5m of an obstacle, and the rest are unvisited and set to 0 (null)? This could be comparable to the inflation layer.

The unvisited are assumed to be at maximum distance (e.g. 0.5m). It can be compared to the reverse inflation layer.

I can say it happened so early and so consistently in the map, its not from poor odometry, that bag I've run with every 2D SLAM algorithm around and hasn't had an issue at that spot (10 seconds into the bag). They'll fail at scale later on, but not processing the first hallway. Not enough points in the range of the distance function (mechanism 2). I think I need a clarification on that point.

Scan points that fall in the flat plane of the distance function will not be attracted, it is as if they do not exist. I see this happening in datasets with wide open areas. And the behavior of failing the scan matching when something passes through is a possible symptom. Note that this is just my educated guess.
Can you try increasing the maximum value of the distance map and the maximum range of the scan?

[SteveMacenski]

The nicest thing about this formulation is that I have no features (only occupied cells) and no (explicit) data association. I

So if you're just in a long fairly homogenous aisle, really its just taking the word of god of the odometry for the lengthwise shift and only fitting the widthwise latteral offsets? There are many cases in industrial applications where the laser doesn't reach to the end of the aisle-way to have a second ppt. surface to have both lengthwise and widthwise fitting around. Is the gradient method sensitive enough to match small changes in this kind of situation so unique, but small changes, in the aisle shelving gets matched against? This is something a formal scan matcher would handle quite well after using the odometry as an initial guess but the "attraction" method I wouldn't think would handle it.

I'm starting to understand why this method is so much lower processing power than other alternatives. Very useful for unique spaces or areas with high visibility of surfaces in multiple principle axes. Removing the scan matcher and formulating your optimization problem in this way is a fast alternative.

Scan points that fall in the flat plane of the distance function will not be attracted, it is as if they do not exist.

But what about points that are close to the walls to non-wall new obstacles that are moving?

[eupedrosa]

So if you're just in a long fairly homogenous aisle, really its just taking the word of god of the odometry for the lengthwise shift and only fitting the widthwise latteral offsets?

In theory yes. In practice, there is always? something in the environment that provide information for the remaining principal axis. You provide a good example for this:

There are many cases in industrial applications where the laser doesn't reach to the end of the aisle-way to have a second ppt. surface to have both lengthwise and widthwise fitting around. Is the gradient method sensitive enough to match small changes in this kind of situation so unique, but small changes, in the aisle shelving gets matched against?

The shelf should provide enough changes for the gradient method to work. I did some testing with this in mind. I set the laser scan max range to a low value (e.g. 5m) and run it in a dataset composed of corridors.

This is something a formal scan matcher would handle quite well after using the odometry as an initial guess but the "attraction" method I wouldn't think would handle it.

This was my hypothesis when I started working on it, could an "attraction" method work?

But what about points that are close to the walls to non-wall new obstacles that are moving?

Not a problem. Unless the scan detects more moving obstacles than walls. But I guess this is true for most solution. (I hope to have finally answer your question)

[SteveMacenski]

That makes sense, I'm still suspicious of the small-changes in aisles but I need to play around with some robots now, no more use in talking about it.

Thanks for the explanations, that makes sense. I'm not sure about what these issues are then and I think I need to get a robot out with this running to figure out the issues. Its hard with a dataset to really get a feel for things

[msadowski]

@eupedrosa I think the question I have might be fitting this issue well, so I decided to describe it here. If you'd like me to create another one please let me know!

I'm running the iris_lama ROS node with default settings (I have only changed names of frames to match my setup).

Let's say that in my setup I have a box marked in green:

I noticed that if I move the box it takes the map quite some time to update the change in the environment. Here is the real time gif (I've left the laser scans to show the box being moved):

By the end of the gif you can see that the old box is starting being wiped out from the map and the new position is not appearing on the map (in fact the box didn't appear in the new position at all). Is there any parameter that would allow to change this behaviour?

[eupedrosa]

Thank you for your comments @msadowski.

Would you be kind to reproduce those gifs with a top down orthogonal view? It helps with the troubleshooting.

in fact the box didn't appear in the new position at all

It should, unless the number of occupied hits are much higher than the free hits.

Is there any parameter that would allow to change this behaviour?

Yes and no. Yes there is a parameter that changes the behaviour, but right now it is hard coded here:

iris_lama/src/sdm/frequency_occupancy_map.cpp

Line 38 in 07808d8

static double occ_thresh = 0.25;

I will change this in the future.

edit: I did not notice that the box was not appearing in the new position.

[jcmonteiro]

Let me jump in here. This might be a long shot, but have a look if the computer running the algorithm is running low on memory or CPU — most likely CPU since RViz was still able to show the data properly.