There are three levels of waypoint projection quality (best to worst): stereo projection, flat earth projection, and line following projection. In any given cycle, we use the best quality of waypoint projection for which there is sufficient information. The projection quality of a waypoint can change from one cycle from the next as range information availability changes.
We return a confidence value [0..1) along with the projected range. This is used by waypoint seeking to compute a weighted average of the projected ranges. The interpretation of this value is that a value with a confidence of 0.5 should be weighted as much as 5 estimates with confidence 0.1, etc.
We project the waypoint ray into our current field of view and query the range all along this line. If we get a range that lies sufficiently close to the waypoint ray, then we use that intersection point as the projected waypoint (we return the distance along the original ray of the intersection.) This projected waypoint may be arbitrarily far from the actual waypoint, see analysis.
The confidence of stereo projection values is generally high, but should take into consideration the basic stereo range uncertainty, which goes up as the square of distance. We might also want to take into consideration how close an intersection we got between the stereo range and the waypoint ray.
flat earth projection guesses a 3-space location for the waypoint by intersecting the designated waypoint ray with the current orientation plane of the robot. The distance along the designated ray to the intersection is returned as the result of waypoint projection. If there is no intersection between the waypoint ray and the current plane, then there is no flat earth projection, and we fall back to line following. We can use the intersection angle between the designated ray and the current place as a confidence value (confidence is greatest when the ray is normal to the plane.) The maximum confidence of flat earth projection is less than that of stereo.
Line following projection is is the mode of waypoint projection used when no range is available. We effectively assume that the waypoint is at infinity, and drive along the designated ray. This is trivially implemented by returning an infinite range and a confidence of zero.