Adding min_age_outside_frustum parameter (solving the noisy edge problem)

[doisyg]

This PR adds the min_age_outside_frustum parameter. I am open to discussion and debate about it (and also about the name of the parameter which I am not super happy with).

The original goal was to solve the noisy frustum edges problem (see for instance #141). The bigger the size (resolution) of the grid the more this problem appears, even for a perfectly calibrated frustum. More precisely, considering a non moving robot, grid voxels will be marked and not cleared under the following conditions:

• The voxel sits on a frustum edge with its center outside the frustum (and hence non clearable)
• There is an obstacle point on the part of the voxel that is inside the frustum (and hence the voxel is marked occupied)

This could be solved by avoiding the marking of grid voxels whose center is not inside any frustum. However this would need an additional potentially costly IsInside test when marking. But STVL being temporal, it is possible to get the same behavior almost for free at the next TemporalClearAndGenerateCostmap step where each points are already IsInside tested: if outside we simply check their age against the new min_age_outside_frustum param. In other words, if we find a voxel too young outside every frustum, it means that it was marked without being in a frustum.

By setting min_age_outside_frustum just above the period of one cycle, we get the desired "don't mark voxels whose center is not part of a frustum" behavior.
Bonus: by increasing this parameter we get an additional behavior which could be describe as "don't allow a voxel to stay marked outside my field of view if I did not have it under my sight for at least this amount of time".

[nickvaras]

As frustrating as they may be, the word is frustum, not frustrum. 😉

[nickvaras]

I obviously love the intent behind this PR, but I haven't yet fully grasped whether it solves the noisy edge problem or just replaces it for a different one. What about defining two frustums, one slightly bigger than the other, so that a perfectly calibrated physical sensor frustum would fall right between those two, therefore allowing a stepped treatment of voxels, that is, those that are outside, those that are inside, and those considered "on the edge" or in transition, so that the decay rates can be bridged and not basically follow a step function. Even wilder would be to somehow have the decay ramp up/down as the voxel transits between this outer and inner frustum frontiers, but that's probably a nightmare to implement.

[doisyg]

frustrum

Damn! Pardon my french... Edited and commited

[doisyg]

I obviously love the intent behind this PR, but I haven't yet fully grasped whether it solves the noisy edge problem or just replaces it for a different one. What about defining two frustums, one slightly bigger than the other, so that a perfectly calibrated physical sensor frustum would fall right between those two, therefore allowing a stepped treatment of voxels, that is, those that are outside, those that are inside, and those considered "on the edge" or in transition, so that the decay rates can be bridged and not basically follow a step function. Even wilder would be to somehow have the decay ramp up/down as the voxel transits between this outer and inner frustum frontiers, but that's probably a nightmare to implement.

Just try it! You ll see that it works elegantly. I set a value of 0.11s for my setup, you can also see the effect in live with rqt_reconfigure.
What I like about it is that by setting the fov of the frustum slightly below the real fov, there is absolutely no transition volume, ie no volume where a voxel gets a fast decay without beeing in the sensor fov.

On another note, I absolutely concur on the need of defining clearing frustum with more freedom (ie decoupling the center from the sensor origin), I can think of many use cases for that. I think I will do a PR allowing it if there is interest. The questions remains about how it should be implemented: with a tf frame ? But maybe this discussion should be on a separate thread.

[SteveMacenski]

So a problem with this is that you're also checking if its < threshold when it may have actually been around for awhile and in this cycle isn't viewed in the frustum boundaries, its not restricted to newly added points. It feels like to me you should really be something when marking

spatio_temporal_voxel_layer/src/spatio_temporal_voxel_grid.cpp

Line 298 in 2692f15

void SpatioTemporalVoxelGrid::operator()(const \

but I'm not sure how that logic would work out exactly since we don't want to be iterating through the grid multiple times in an iteration. I'm not sure if there's a good way to tell in the grid if this particular point was "new" "recently" and is now within some threshold to remove.

The original goal was to solve the noisy frustum edges problem

Wouldn't a better solution just be having a filter before going into the costmap to remove the points along the edges? I've done this in the past with crop filters.

I've also found empirically I get better performance with this if I nudge up the frustum a few degrees from the actual camera frustum - probably I did that in response to this exact issue.

[SteveMacenski]

This is me approving the code as an implementation acceptable to merge, but we still need to discuss the actual idea.

[doisyg]

This is me approving the code as an implementation acceptable to merge, but we still need to discuss the actual idea.

Great, I ll prepare a graphical base for discussion in the next few days when I ll get a minute

[doisyg]

Ok, this is my proposed base for discussion.
Let's start to reason in 2D for simplicity with the following grid of voxels and with the volume covered by our sensor outlined in green (the volume/fov of the clearing frustum will be outlined in black below) :

I will used the term "cleared" instead of "having the fast decay rate of the frustum applied to" for the sake of readability.

Let's represent :

• in green a voxel which has its center in both the volume covered by the sensor and the frustum -> in other words a voxel that is properly marked and cleared.
• in red a voxel which has part of its volume inside the volume covered by the sensor but its center outside the frustum -> in other words a voxel that can be marked by the sensor but not cleared by the frustum.
• in purple a voxel which has part of its volume outside the volume covered by the sensor and its center inside the frustum -> in other word a voxel that can be cleared by the frustum without being entirely in the sensor covered area.

| |

Now let's examine the different possible configurations of the frustum relative to the volume covered by the sensor.

Without this PR

A. Matching sensor volume and frustum fovs:

Here, we have the sensor fov and frustum fov matching perfectly. The well known noisy edge issue appears clearly: the red voxels are marked but not cleared.

B. Frustum fov smaller than sensor volume

Here, with a frustum fov smaller than the sensor volume, we obviously increased the noisy edge issue: more voxels are marked without being cleared.

C. Frustum fov bigger than the sensor volume

This is @SteveMacenski proposed solution. The frustum clearing fov is larger than the volume covered by the sensor.

In order to work, the frustum fovs needs to include the center of all voxels that can be marked by the sensor. And the maximum distance between the sensor fov edges and the center of a markable voxel is the half diagonal of the voxel ( voxel_size * sqrt(2) / 2 in 2D for a square, voxel_size * sqrt(3) / 2 in 3D for a cube). So we need to increase our fov angles to include all points which are voxel_size * sqrt(3) / 2 away from the edges of the sensor covered volume (writing that, I realized that the image above doesn't represent the mess that can happen at the min_z of the sensor area when min_z is small and/or voxels are big).

With this configuration, we properly clean all marked voxels. However we have some problematic purple voxels. And the farther we are from the sensor origin, the more purple voxels there are. The problem with purple voxels is that we are over-clearing them. And depending on the sensor speed and the configured decay rate, some marked voxels will be purple for too long : they will never make it to the low decay rate area (outside the frustum). This is really hard or impossible to tune across all robot speeds and sensor orientations.

D. Offsetted frustum (not implemented)

Here, the idea, not implemented yet, is to offset the origin of the frustum from the origin of the sensor in order to enable a similar behavior as in C. but without creating more purple voxels at higher ranges.
To include all voxel centers covered by the sensor, the planes of the smallest of the fov (vertical or horizontal) of the frustum needs to be parallel and voxel_size * sqrt(3) / 2 away from the planes of the smallest of the fov of the sensor.

Same as in C., we properly clean all marked voxels and have less purple voxels but have still the same issue of over-clearing.

With this PR: Not marking voxels whom center is not inside a frustum using the min_age_outside_frustum parameter

The effect of this PR is to avoid marking a voxels whom center is not inside the frustum: essentially not marking the previously red voxels.

Previously defined configurations C. and D. don't change. However A. and B. have no more problematic red voxels:

A. Matching sensor volume and frustum fovs:

B. Frustum fov smaller than sensor volume

[doisyg]

Wouldn't a better solution just be having a filter before going into the costmap to remove the points along the edges? I've done this in the past with crop filters.

This is equivalent is terms of cost of checking if each point is inside the frustum or not, and it increases the number of place where something needs to be configured. I prefer something more unified and less error prone. Plus doing it inside STVL enables the check against multiple frustums.

[doisyg]

So a problem with this is that you're also checking if its < threshold when it may have actually been around for awhile and in this cycle isn't viewed in the frustum boundaries, its not restricted to newly added points

True, it adds a < threshold check for every point that exists outside the frustum. However I did not notice any degradation of performance.

[doisyg]

What about defining two frustums, one slightly bigger than the other, so that a perfectly calibrated physical sensor frustum would fall right between those two, therefore allowing a stepped treatment of voxels, that is, those that are outside, those that are inside, and those considered "on the edge" or in transition, so that the decay rates can be bridged and not basically follow a step function. Even wilder would be to somehow have the decay ramp up/down as the voxel transits between this outer and inner frustum frontiers, but that's probably a nightmare to implement.

It is an idea that I contemplated too, but the time a voxel will spend in this intermediate zone depends on the speed of the robot, the orientation of the sensor and which edge you are.
I believe it would impossible to get a perfect tuning. For instance, for a given speed of the robot and a given orientation of the sensor (apart aligned with a pure forward movement), the time a voxel will spend in this intermediate zone will be different for the upper edge and the lower edge.

[nickvaras]

Nice work with the graphics! By the way, there is something like the proposed D- Offset frustum built into the 3d lidar model (Padding). The same parameter could be added to the rgbd camera model. But it seems that at a base level, there's a need to determine whether a voxel lies on the edge, that is, we would have to check not just the sensor provided value but the 8 vertices of the voxel.so if reading's x is 3.002, we'd have to check for 3.000 and 3.005 (assuming a 5cm voxel). I wonder what the performance hit would be...

[nickvaras]

What would a value look like for the new parameter. Is the life counted in cycles or seconds?

[doisyg]

By the way, there is something like the proposed D- Offset frustum built into the 3d lidar model (Padding).

I ll have to check that!

What would a value look like for the new parameter. Is the life counted in cycles or seconds?

It is in second, so for me it works well with 0.11s which is just above the cycle period.

[SteveMacenski]

So this wouldn't only set the threshold for newly marked points, but also points outside of the frustum that have been accelerated before, since time_since_marking is set as the time it was marked, but is also updated by the frustum acceleration if it appeared in another iteration in the frustum.

That means that this is going to not only affect the clearing of recently marked points, but also longer-standing points that have had any level of frustum acceleration already applied. This is still overclearing.

I could make the argument that as long as the minimum age is very small, we can approximate those older points as being likely to be removed as well very soon in the next update cycle, but it does break that clear divide.

I really question if this is necessary. Going back to your problem statement: The original goal was to solve the noisy frustum edges problem . That could be much more easily resolved for this layer and all other perception tasks you have on your robots by setting up a trivial crop filter either in STVL to knock off the edges or - more correctly - add the crop filter to your filter chain attached to your depth sensor feeds. That way you just don't have those edge points even marking at all and since they're notoriously noisy and unreliable due to poor edge calibration, you really don't want to be using that information anywhere anyway. Why not just filter that out beforehand? That seems like the best overall solution to your problem both here and in any other processing pipeline you'd ever create using the realsense cameras.

[SteveMacenski]

Thank you for the really extensive explanation, that helps. See the comment above though, I think you went down a bit of a rabbit hole and missed the obvious solution to your problem. The problem with noisy edges of depth cameras is incredibly common and there are common and efficient solutions to it.

This is equivalent is terms of cost of checking if each point is inside the frustum or not, and it increases the number of place where something needs to be configured. I prefer something more unified and less error prone. Plus doing it inside STVL enables the check against multiple frustums.

That is totally not true. You have a depth image, applying a crop filter is crazy fast. You should have a light filter chain on your depth cameras as is, and if you don't, then I'm telling you that you probably should be. Your proposed approach is more error prone to overclearing voxels caught in the crossfire trying to hack around a problem with known solutions.

I'd be more than happy to merge a "padding" parameter analog to that in the 3D lidar model to this, which would also help largely solve the problem you face in a much more robust way.

You mention "overclearing" as a problem to the other solutions of offset / larger FOV / etc, but then you completely ignore analyzing the fact that your solution also overclears, and does so in nearly the exact same way.

However I did not notice any degradation of performance.

As I'm sure would also be true with a padded or crop filtered FOV and have some more mathematical completeness.

[doisyg]

I see several independent discussions around this topic so I will try to separate them for clarity.

Noisy edge issue definition

That way you just don't have those edge points even marking at all and since they're notoriously noisy and unreliable due to poor edge calibration, you really don't want to be using that information anywhere anyway.

I feel that there is a misunderstanding about what I call noisy edge issue. To me it is the fact that points can be marked in the grid while not being clearable by the frustum, and so even for an ideal and perfect sensor.
My understanding is that the issues that I have at the edges do not come from poor sensor quality/caibration (or partially at the very least ). It has to do with the quantization of the 3D space by the grid and the fact that we use continuous planes to describe a clearing frustum. Hence, for a perfectly matching sensor area/frustum fov, a grid voxel can have part of its volume inside (making it markable) and its center outside (so not clearable by the frustum), as illustrated with this figure:

Proper implementation of this PR idea

I would like to separate the discussion about the proper implementation of this PR idea from its relevance (where we disagree)

So this wouldn't only set the threshold for newly marked points, but also points outside of the frustum that have been accelerated before, since time_since_marking is set as the time it was marked, but is also updated by the frustum acceleration if it appeared in another iteration in the frustum.

That means that this is going to not only affect the clearing of recently marked points, but also longer-standing points that have had any level of frustum acceleration already applied. This is still overclearing.

I don’t understand this. The update you mention is done here right ?

spatio_temporal_voxel_layer/src/spatio_temporal_voxel_grid.cpp

Line 212 in 14e7f5e

if(!this->MarkGridPoint(pt_index, updated_mark))

So only for voxels inside the frustum. And frustum_acceleration is positive so essentially this update is making the points inside the frustum older (and this PR is about removing them only if they are too young). If on a next iteration they are outside the frustum, time_since_marking will be bigger and the time_since_marking < _min_age_outside_frustum condition will not be more fulfilled than without this update.

Anyway, the idea is to remove only points that have “just” being marked outside the frustum. In order to work properly min_age_outside_frustum has to be bigger that the time between the marking of a point and the next call to TemporalClearAndGenerateCostmap (otherwise nothing will be cleared at all), but also smaller than the time between the marking of a point and the second next call to TemporalClearAndGenerateCostmap (otherwise overclearing).

Your proposed approach is more error prone to overclearing voxels caught in the crossfire trying to hack around a problem with known solutions.

I agree that in the current state it is error prone and needs tuning and calculation of the proper value which is not ideal. Maybe there is an automatic way to determine the proper value. Or another way of checking the “just marked” condition. Or to fallback to an additional proper IsInside check at the time of the marking which will be cleaner but more CPU intensive. I will give it a bit more thoughts and tests. Ideally I would like to replace the min_age_outside_frustum parameter by a bool parameter avoid_marking_outside_frustum (default to false for legacy behavior consistency).

Filtering inside or outside STVL ?

So this is more an architectural debate.

I really question if this is necessary. Going back to your problem statement: The original goal was to solve the noisy frustum edges problem . That could be much more easily resolved for this layer and all other perception tasks you have on your robots by setting up a trivial crop filter either in STVL to knock off the edges or - more correctly - add the crop filter to your filter chain attached to your depth sensor feeds.

I already have a nice GPU optimized filtering pipeline in place (noise, footprint, down sampling) and its output is used by other perception tasks (a reactive security node for instance) without issue. What you are asking is to remove perfectly valid points outside STVL in order to compensate an internal STVL issue (the quantization of the 3D space by the grid in combination with IsInside checks based on frustum defined by continuous planes). I have a philosophical problem with this. I believe that any filtering that needs to be done for the sole goal of making STVL works properly should be handled by STVL itself. Moreover this filter would be dependent on the value of few STVL parameters and variables, at least:

• vertical_fov_angle
• horizontal_fov_angle
• The origin of each sensor (or the complete frustum definition)
• The origin of the STVL grid
• The size of the grid
  Making the implementation far from trivial.

[doisyg]

As I'm sure would also be true with a padded or crop filtered FOV and have some more mathematical completeness.

In my opinion, the most complete solution would be one which avoid marking unclearable points (independently from its implementation). Cropping removes unnecessarily valid points whereas padded FOV would create overclearing issues.

[SteveMacenski]

My understanding is that the issues that I have at the edges do not come from poor sensor quality/caibration (or partially at the very least ). It has to do with the quantization of the 3D space by the grid and the fact that we use continuous planes to describe a clearing frustum.

I believe you are in the pits of a "solution not the problem" mindset. I believe your real issue is that the edges of your depth images produced by your sensor is noisy, which snowballs into all these other issues. Sure, you could pad the FOV to help accelerate and deal with quantization of the voxel bins, which would also fix your problem, but lets think about what you're actually doing in that. You're processing known-noisy measurements and then trying to remove them as quickly as possible. Wouldn't it make more sense to just crop-filter them out and then go on your marry way? Padding or this modification would also help with this issue, but you could kill 2 birds with 1 efficient stone in crop-filtering out the edges.

I would be OK with adding a padding feature, which would also fix this issue, but what you've implemented in this PR is probably not going to be merged. It breaks math & isn't rigorous.