Revision history [back]

This is certainly possible, I've built code to do this in a few different ways. The first thing you need to decide upon is exactly how you want to convert the point-cloud data into the grid_map. Digital elevation maps are very different to point clouds and a lot of information will get thrown away in the conversion.

Since it's likely many points from the point-cloud will fall within a since cell of the grid_map, you need to decide how to handle this. You also need to decide how to deal with grid_map cell which do not contain any points. We've found using three grid_map channels (minimum, maximum and elevation range) as well as the basic elevation is quite useful. You can then set the elevation channel to either the minimum or maximum depending on your application.

That's the theory though. Firstly you'll want to add your layers using the add method as below:

myGridMap.add("minimum");
myGridMap.add("maximum");
myGridMap.add("range");

Then you'll want to loop through each point in the cloud and add it to the grid_map. For this you'll use the atPosition method. This method takes a layer name string and a 2D position vector, this vector is in units of metres and uses the centre of the map as the origin. What is not immediately obvious is that atPosition method returns a reference to a float, so you can use it to update the map as well as get its value.

So you'll want to extract just the X and Y elements from a point in your point-cloud, check it's within the bounds of the grid_map. Then get the value of the grid_map at that location, if it's a NaN then the map is empty at that point so you would just update the elevation, min and max to the height of that point, and its range to zero. If it's not Nan then you'd have to update the min, max and range values appropriately.

Eigen::Vector2d point2D(point[0], point[1]);

// Need to check point is within grid_map here!

if (!std::isfinite(myGridMap.atPosition("elevation", point2D))
{
   myGridMap.atPosition("elevation", point2D) = point[2];
   myGridMap.atPosition("minimum", point2D) = point[2];
   myGridMap.atPosition("maxmimum", point2D) = point[2];
   myGridMap.atPosition("range", point2D) = 0;
}
else
{
    if (point[2] < myGridMap.atPosition("minimum", point2D))
    {
        myGridMap.atPosition("minimum", point2D) = point[2];
        myGridMap.atPosition("elevation", point2D) = point[2];
    }
    if (point[2] > myGridMap.atPosition("maximum", point2D))
        myGridMap.atPosition("maximum", point2D) = point[2];

    myGridMap.atPosition("range", point2D) = myGridMap.atPosition("maximum", point2D) - myGridMap.atPosition("minimum", point2D);
}

There are many ways of accessing and modifying the data within a grid_map, this is just one of them but hopefully it gives you an idea how to get started.