Ask Your Question
0

How to feed Point Cloud from a file into a grid_map

asked 2019-06-05 14:28:16 -0600

RayROS gravatar image

updated 2019-06-05 14:30:54 -0600

Hello, I started working on a project that turns 3d point clouds from a file into grid_map. After successfully having set all my project in CMake I was trying to prepare a small example. I am already able to publish a grid_map from this tutorial as it is possible to see from the print screen below:

grid_map

Also from this source it seems to be possible to feed a grid_map with some point clouds that I have in a file on my Desktop. However I am finding this process a little bit difficult mostly because I am still not confident with grid_map as I started working with it recently. Any insight on how to solve this problem will be welcome.

Below I am putting the code I am using to try to feed grid_map with a point cloud file I have on my Desktop:

#include <ros/ros.h>
#include <grid_map_ros/grid_map_ros.hpp>
#include <Eigen/Eigen>
#include <grid_map_msgs/GridMap.h>
#include <string>
#include <cstring>
#include <cmath>
#include <iostream>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

using namespace grid_map;

class Point_CLoud_Reader
{
public:
    Point_CLoud_Reader();
    void pointCloudCallback(const sensor_msgs::PointCloudConstPtr& msgIn);
    void readPCloud();
    void writeToGridMap();
    pcl::PointCloud<pcl::PointXYZ> cloud;

private:
    ros::NodeHandle _node;
    ros::Publisher pCloudPub;
    ros::Subscriber pCloudSub;
    std::string _pointTopic;
};


Point_CLoud_Reader::Point_CLoud_Reader()
{
    _node.param<std::string>("pointcloud_topic", _pointTopic, "detections");
    ROS_INFO("%s subscribing to topic %s ", ros::this_node::getName().c_str(), _pointTopic.c_str());
    pCloudPub = _node.advertise<sensor_msgs::PointCloud>("/point_cloud", 100, &Point_CLoud_Reader::pointCloudCallback, this);
}

void Point_CLoud_Reader::pointCloudCallback(const sensor_msgs::PointCloudConstPtr &msgIn)
{
    sensor_msgs::PointCloud msgPointCloud = *msgIn;
    //msgPointCloud.points = cloud; // <-- Error Here
    pCloudPub.publish(msgPointCloud);
}

void Point_CLoud_Reader::readPCloud()
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if(pcl::io::loadPCDFile<pcl::PointXYZ> ("/home/to/Desktop/wigglesbank_20cm.pcd", *cloud) == -1) // load point cloud file
    {
        PCL_ERROR("Could not read the file /home/to/Desktop/wigglesbank_20cm.pcd \n");
        return;
    }
    std::cout<<"Loaded"<<cloud->width * cloud->height
             <<"data points from /home/to/Desktop/wigglesbank_20cm.pcd with the following fields: "
             <<std::endl;

    for(size_t i = 0; i < cloud->points.size(); ++i)
        std::cout << "    " << cloud->points[i].x
                  << " "    << cloud->points[i].y
                  << " "    << cloud->points[i].z << std::endl;
}

void Point_CLoud_Reader::writeToGridMap()
{
    // .....
}


int main(int argc, char** argv)
{
        // initialize node and publisher
    ros::init(argc, argv, "grid_map_test");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
    ros::Publisher pCloudPub= nh.advertise<sensor_msgs::PointCloud>("point_cloud", 1, true);
    pcl::PointCloud<pcl::PointXYZ> cloud;

    // create grid map
    GridMap map({"elevation"});
    map.setFrameId("map");
    map.setGeometry(Length(1.2, 2.0), 0.03);
    ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
             map.getLength().x(), map.getLength().y(),
             map.getSize()(0), map.getSize()(1));

    // work with grid-map in a loop
    ros::Rate rate(30.0);
    while (nh.ok()) {
        // add data to grid-map and point cloud from file just read
        ros::Time time = ros::Time::now();
        for(GridMapIterator it(map); !it.isPastEnd(); ++it)
            for(int i = 0; i < cloud.points.size(); i++)
            {
                Point_CLoud_Reader readCloud;
                readCloud.readPCloud();   // <-- Not reading point clouds
                Position ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2019-06-05 18:22:35 -0600

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.

edit flag offensive delete link more

Comments

Thanks PeteBlackerThe3rd for taking the time to read my question. And thanks for providing a good theory background that is useful especially because I could not find an exhaustive explanation specifically for this on ROS official documentation other than just code.

The difficulty I have is that because of lack of practical example that explains how to feed grid_map with existing points clouds I still am not able to build it from a practical point of view. Is there any chance you can either provide a running example on how to do that or point to some specific examples? or maybe editing the small example code I posted?

RayROS gravatar imageRayROS ( 2019-06-05 19:34:22 -0600 )edit

It's quite a large block of code for this, I can't write the whole thing for you!

I've explained all the steps you need to do, I recommend building this up in small pieces, maybe start by adding a simple horizontal rectangle to the map, then a single point, then build up to the whole point-cloud.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-06-06 05:01:45 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-06-05 14:28:16 -0600

Seen: 67 times

Last updated: Jun 05