Reduce computation cost of ScanShadowsFilter

[at-wat]

ScanShadowsFilter required a lot of CPU power mainly due to atan2. This commit reduces computation cost of the filter.

• Remove atan2 and directly compare tangent values
• Add a test to check geometric calculation

[at-wat]

Here is a rough evaluation result of the CPU usage on processing five 2-D LIDARs' data (x6 replay of a bag) on Xeon E5-2620.

branch	CPU usage
indigo-devel	70%
optimize-shadows-filter	20%

[jonbinney]

Thanks @at-wat ! I'll find some time to review this.

[jonbinney]

Before reviewing this, I had to spend some time understanding what the filter does. Since I suck at trig, I made the following diagram:

That digram shows how "getAngleWithViewpoint()" computes the perpendicular angle. Here p1 and p2 are two points within the window that are being compared.

[jonbinney]

@at-wat I'm still trying to fully understand the case where min_angle is greater than 90 degrees. From the diagram I made, I think that for two points p1 and p2 in the scan, if getAngleWithViewpoint(r1, r2, included_angle) is x, then getAngleWithViewpoint(r2, r1, -included_angle) will be 180 - x - included_angle. So if included_angle were always positive, then a min_angle of greater than 90 degrees truly would make no sense, because either the perpendicular angle for p1 vs p2 would fail the test, or p2 vs p1 would fail the test.

Does that make sense? Now, actually included_angle can be negative in this case. So I don't fully understand the implications of min_angle being greater than 90 degrees.

[jonbinney]

I wrote a quick python implementation of getanglewithviewpoint to play around with it:

import numpy as np

def getAngleWithViewpoint(r1, r2, included_angle):
    return np.arctan2(r2*np.sin(included_angle), r1-r2*np.cos(included_angle))

parameters = [
        (1.0, 1.0, 0.1),
        (1.0, 1.0, -0.1)]
for r1, r2, included_angle in parameters:
    print r1, r2, included_angle, ':', getAngleWithViewpoint(r1, r2, included_angle)

[at-wat]

Thank you for your follow up.

So if included_angle were always positive, then a min_angle of greater than 90 degrees truly would make no sense, because either the perpendicular angle for p1 vs p2 would fail the test, or p2 vs p1 would fail the test.

I agree that the all points fails the test. However, only farther points, excluding self, are deleted. https://github.com/ros-perception/laser_filters/blob/5e76d3283dad78fc5d538986bc1789b74fb78b6a/include/laser_filters/scan_shadows_filter.h#L134 So, this filter with ~negative~ min_angle > 90 (edited) behaves like convex corner points detection. I don't think this usage makes sense, anyway. (green points are filtered results with min_angle = 91)

Does that make sense? Now, actually included_angle can be negative in this case. So I don't fully understand the implications of min_angle being greater than 90 degrees.

Perpendicular angle with negative included_angle is equivalant to -getAngleWithViewpoint(..., fabs(included_angle)). In the original code, the perpendicular angle is tested with abs(), so we don't have to specially care about negative included_angle. https://github.com/ros-perception/laser_filters/blob/5e76d3283dad78fc5d538986bc1789b74fb78b6a/include/laser_filters/scan_shadows_filter.h#L129 (Here, this abs() should be fabs(), BTW.)

[at-wat]

@jonbinney ping If any other comments, I'll address them.

[jonbinney]

@at-wat sorry for the delay. I've been weighing the advantages of this PR with the danger of possibly changing the behavior of a system that works. Overall, I think this is an improvement. Merging.

[at-wat]

Thanks!