Using Grid/MaxObstacleHeight with OctoMap is not working as expected with ray tracing

[matlabbe]

Refer to this discussion: introlab/rtabmap_ros#1194 (comment)

The issue is that points over the obstacle threshold won't be used for ray tracing, so some dynamic obstacles even smaller than max obstacle height may still remain in the OctoMap after it is gone. We should handle Grid/MinGroundHeight and Grid/MaxObstacleHeight differently if both Grid/3D and Grid/RayTracing are enabled. We should not filter the cloud like this:

rtabmap/corelib/include/rtabmap/core/impl/LocalMapMaker.hpp

Lines 111 to 117 in 538bc68

	// filter ground/obstacles zone
	if(minGroundHeight_ != 0.0f || maxObstacleHeight_ != 0.0f)
	{
	indices = util3d::passThrough(cloud, indices, "z",
	minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
	maxObstacleHeight_>0.0f?maxObstacleHeight_:std::numeric_limits<int>::max());
	UDEBUG("indices after max obstacles height filtering = %d", (int)indices->size());

Instead, forward Grid/MinGroundHeight and Grid/MaxObstacleHeight to OctoMap and treat them like Grid/RangeMax parameter, so that ray tracing can clear obstacles for which the background is seen outside the obstacle volume.

rtabmap/corelib/src/global_map/OctoMap.cpp

Lines 530 to 541 in 538bc68

	if(rangeMaxSqrd > 0.0f)
	{
	octomap::point3d v(pt.x - cellSize - sensorOrigin.x(), pt.y - cellSize - sensorOrigin.y(), pt.z - cellSize - sensorOrigin.z());
	if(v.norm_sq() > rangeMaxSqrd)
	{
	// compute new point to max range
	v.normalize();
	v*=rangeMax_;
	point = sensorOrigin + v;
	ignoreOccupiedCell=true;
	}
	}