Trailing occupancy artifacts left by mobile agents

[idlebear]

Hi all -- finally have time to get back to this and have run into an interesting issue. Dynamic objects that pass over the same location (like cars following lanes might) tend to leave tracks that build over time -- much like snails... Here's what it looks like when running against carla. On the far left is the LaserScan input, the parallel tracks are the snail markings that build over time. The actual views are on the right.

dogm-ros-carla-.2.mp4

Initially the field is white, but seems to build these long lived blocks over time. They can eventually fade, but take a long time -- non-ideal when attempting to use the field for navigation.

Any thoughts?

[idlebear]

Another, possibly related point of interest -- this is running with all the OPENGL code replaced with a simple polar -> cartesian map. Polar scaling and grid scaling are completely decoupled. Everything is pushed to my fork, with the ROS code in a separate project. Take care.

[idlebear]

One more data point -- recompiled everything into release mode and voila, no more tracks. Odd.... I still have rotations coded in, but they are too inaccurate and messy -- they're getting tossed in favour of keeping everything in world reference/axis.

So, no crisis, working mostly as designed...

[TheCodez]

Only reading the above messages I would have assumed that it is either not enough particles and birth particles or not ideal velocity_birth, process_noise_position or process_noise_velocity parameters.

Could you please send me the binaries of both debug and release (with debug symbols) if possible. Maybe I can find the cause, because that shouldn't happen.

[idlebear]

Sure thing -- I still see the artifacts crop up, just not as common as that first example. Interestingly enough, they don't move when the car does... I'm going to check first that in removing OpenGL I didn't shoot myself in the foot.

[idlebear]

Here are debug and release libraries based on my fork with most of the @ShepelIlya changes -- had to skip the parallel resampling as that seemed to have a resource contention in paralle_ns()). If you'r going to pull the source, grab the perf-test branch. Both seem to leave artifacts that build over time when the car is still, just need to run for long enough.

libdogm.gz

(note that this version of the lib is using OpenGL)

[idlebear]

For the ROS node, use the main branch. There is a ros launch file you need to run to convert the pointcloud to a laser scan

roslaunch dogm_ros pointcloud_to_laserscan.launch input:=/carla/ego_vehicle/lidar/lidar1/point_cloud output:=/scan

and you'll need Carla, and the Carla ros bridge to get the demo working. Here are the debug/release nodes:
dogm_ros_node.gz

[idlebear]

So I had some more time to look at this and have figured out a workaround of sorts. Basically, what seems to be happening is some cells build up an occupancy mass and never correct it when the map moves. Here's what that looks like in the gazebo env using the clearpath jackal/velodyne

artifact1-left-behind-.2.mp4

You can see those occupied cells in the middle of the field never clear, even though the measurement data says to do so. This is partly related to the movement code -- specifically, if the robot doesn't move enough to trigger a motion update, then measurements are applied from a slightly wrong position (by up to a grid cell). However, if I always perform the motion update, regardless of the robot's change in status, then I get the following:

artifact1-workaround.mp4

And the reason this clears up the artifacts? Because this line was added:

https://github.com/idlebear/dynamic-occupancy-grid-map/blob/045f8ac0f6e7280544c729e5bc42186d8e95f81b/dogm/src/kernel/ego_motion_compensation.cu#L44

So, when the old cell was valid or when we measure something in the the old cell's location, then we copy. Otherwise, the new location is zeroed. The result: no artifacts.

I'm not saying this is the proper solution. It effectively obliterates our priors for the probabilistic calculations, so everything is only based on the most recent scan. And, a little more subtle, we're checking the old location on the new measurement grid -- that can't be good either.

So, if you have a suggestion of how to tone down the permanence, I'm open to suggestions. Thanks for reading.

[TheCodez]

So if I understand correctly, adding meas_cell_array.occ_mass[index] > eps fixed the issue?
This should be a lot simpler without any workarounds.
The issue was there before the whole grid cells SoA changes, right?

I’m planning on handling the ego motion compensation by implementing it using a 2d ring buffer. That way we don’t have to move the whole map and performance should get a whole lot better.

[idlebear]

The issue was there before the SoA changes.

Only moving/keeping cells that have a measured value greater than the threshold is (I think) essentially resetting the occupancy grid on every update. It solves the issue, but I'm not sure what the cost is w.r.t. the rest of the algorithm.

The 2d ring buffer sounds like a great idea -- looking forward to seeing how it changes the performance.

I'll work with the fixed code for now and post if I come up with a better solution. Thanks!

[idlebear]

So I understand part of what's going on with the artifacts now -- it appears to be a result of how masses are updated when there are no particles in a cell. New particles are only born where there is an existing particle weight, so new weights/occupancies aren't properly updated. This is a result of the check for -1 here:

dynamic-occupancy-grid-map/dogm/src/kernel/mass_update.cu

Lines 72 to 97 in 839b137

	if (start_idx != -1)
	{
	float m_occ_pred = subtract(weight_array_accum, start_idx, end_idx);
	if (m_occ_pred > 1.0f)
	{
	normalize_weights(particle_array, weight_array, start_idx, end_idx, m_occ_pred);
	m_occ_pred = 1.0f;
	}
	float m_free_pred = predict_free_mass(grid_cell_array[i], m_occ_pred);
	float2 masses_up = update_masses(m_occ_pred, m_free_pred, meas_cell_array[i]);
	float rho_b = separate_newborn_part(m_occ_pred, masses_up.x, p_B);
	float rho_p = masses_up.x - rho_b;
	born_masses_array[i] = rho_b;
	store_values(rho_b, rho_p, masses_up.y, masses_up.x, m_occ_pred, grid_cell_array, i);
	}
	else
	{
	float m_occ = grid_cell_array[i].occ_mass;
	float m_free = predict_free_mass(grid_cell_array[i], m_occ);
	float2 masses_up = update_masses(m_occ, m_free, meas_cell_array[i]);
	born_masses_array[i] = 0.0f;
	store_values(0.0f, masses_up.x, masses_up.y, masses_up.x, 0.0f, grid_cell_array, i);
	}

Any cell that doesn't have a particle, has no predicted weight leading to some interesting effects. The most notable being that no particles will ever be born there, no matter what the measurement information is. the born_masses_array[j] is set to 0. Note that if a newly measured occupancy mass is near one with particles, then the motion of the particles can cause the born masses to migrate. If the code is changed to

        float  m_occ_pred = 0.0f;
        if (start_idx != -1) {
            m_occ_pred = subtract(weight_array_accum, start_idx, end_idx);
        }

        float m_free_pred = predict_free_mass(grid_cell_array.free_mass[i], m_occ_pred);
        float2 masses_up = update_masses(m_occ_pred, m_free_pred, meas_cell_array, i);
        float rho_b = separate_newborn_part(m_occ_pred, masses_up.x, p_B);
        float rho_p = masses_up.x - rho_b;
        born_masses_array[i] = rho_b;

        store_values(rho_b, rho_p, masses_up.y, masses_up.x, m_occ_pred, grid_cell_array, i);

the behaviour is better. At least new particles are born where new measurements show up. I haven't tried to work through all of the consequences of this change (hence, not a pull request), but wanted see if you had a better fix. If this isn't clear, I can post some video to demo the issue.

I'm out of time for now, but will return to this later.

[TheCodez]

Thank you for bringing this up. This sounds a lot more reasonable than the current code.

At least new particles are born where new measurements show up

Exactly what's supposed to happen. The previous behaviour looks like a really critical bug. I tested it with the demo and I don't see any issues.

If this isn't clear, I can post some video to demo the issue.

I would be happy to see a video of how this improves your demo scene once you find some more time :)

[idlebear]

Ok, a simple demo...

The jackal is in the standard arena and it is going to move back and forth. I've added some visualization windows to illustrate what's going on. The upper two are the weights from the measurement grid. The bottom two are the born visualization (from @ShepelIlya -- nice work that) and a normalized count of the particles active in the grid. Notice that when that hidden wall becomes visible, no particles are ever born there:

artifact-demo-pre-fixed.mp4

And here's the post change version:

artifact-demo-fixed.mp4

I'm not sure if we should be initializing the unknown mass to 0 (empty) or 0.5 (unknown?). Let me know what you think.

[ShepelIlya]

That`s really critical bug, thanks @idlebear for fix and demo. I think in a more general case it is better to update the mass as an unknown, cause number of required iterations for updating unknown to occupied is less than in the case with free mass, and latency for detecting obstacles is critical for our tasks. I will test your code on my robot in real conditions within a week.

[idlebear]

That's what I thought initially too; but as written, using 0.5 causes the empty spaces to spawn particles making the occupancy really noisy. Zero works far better.

[TheCodez]

I don’t think setting it to 0.5 is the intended solution here, as we are dealing with dempster-shafer masses not probabilities.

I think what could help is decreasing the free space discount factor (alpha) in predict_free_mass which is currently set to 0.9. That way freespace vanishes a lot quicker, making more room for occupancy mass. I have to add a tunable parameter for that.