BUGFIX-66

Bug 34: Lidar Obstacle Misdetection

This is a simple and effective lidar algorithm for drivable road regions (obstacle detection).
It is most suitable for concentric ring, rotating laser, Velodyne-style lidars (e.g., VLP-16, VLP-32C).
Given a matrix of 3D points (pts) it fills a bool matrix (obs) flagging each point as obstructed or not.
pts is a two-dimensional row-major matrix. Each row has rings 3D points at the same laser azimuth angle.
pts has rings columns, each column giving all the points in a 360-degree ring (same laser zenith angle).
The first column (0) tends to be closest to the lidar. The last column (rings-1) tends to be farthest.
For each point in column i, the slopes from same-row points in columns 0, 1, 2, ..., i-1 are calculated.
Slopes are only calculated between pairs of points in the same row (points with the same azimuth angle).
Points inducing a slope exceeding some threshold (slope of the steepest road) are flagged as obstacles.
Once a point is flagged as an obstacle, all subsequent columns in the same row are also flagged.

Fix The Tiny Bug In This Go Code:

var ( steepDegrees = float64(18) steepRadians = steepDegrees * math.Pi / 180 steepSlope = math.Tan(steepRadians) steepSlopeSq = float32(steepSlope * steepSlope) ) func obstructed(obs []bool, pts [][3]float32, rings int) { var ( lo = 0 stop = len(pts) ) for lo < stop { obs[lo] = false var ( far = lo + 1 hi = lo + rings ) for ; far < hi; far++ { var ( x = pts[far][0] y = pts[far][1] z = pts[far][2] ) var ( steep = false near = lo ) for ; near < far; near++ { var ( dx = pts[near][0] - x dy = pts[near][1] - y dz = pts[near][2] - z ) var ( riseSq = dz * dz runSq = dx*dx + dy*dy ) if riseSq >= steepSlopeSq*runSq { steep = false break } } if steep { break } obs[far] = false } for ; far < hi; far++ { obs[far] = true } lo = hi } }

To receive a hint, submit unfixed code.