Filling MRPT velodyne scan observation

I'm new to MRPT and I would like to use it for building an occupancy grid map using velodyne point clouds.

The KITTI dataset provide velodyne point clouds in (x,y,z,r) format, where r is the reflectance. I'm trying to fill a mrpt::obs::CObservationVelodyneScan with such data, but using insertObservation method seems to do just nothing. Can you point me in the right direction for using this observation type?

My code basically looks like this:

COccupancyGridMap2D map;

// allocate 4 MB buffer (only ~130*4*4 KB are needed)
int32_t num = 1000000;
float *data = (float*)malloc(num*sizeof(float));

// pointers
float *px = data+0;
float *py = data+1;
float *pz = data+2;
float *pr = data+3;

// load point cloud
FILE *stream;
stream = fopen (args.velodyne_filename.c_str(),"rb");
num = fread(data,sizeof(float),num,stream)/4;

obs::CObservationVelodyneScan v;
v.point_cloud.x.resize(num);
v.point_cloud.y.resize(num);
v.point_cloud.z.resize(num);
v.point_cloud.intensity.resize(num);

for (int32_t i=0; i<num; i++) 
{
    v.point_cloud.x[i] = *px;
    v.point_cloud.y[i] = *py;
    v.point_cloud.z[i] = *pz;
    v.point_cloud.intensity[i] = *pr;

    px+=4; py+=4; pz+=4; pr+=4;
}
fclose(stream);

map.likelihoodOptions.likelihoodMethod = OccupancyGridMap2D::lmRayTracing;

map.insertObservation(&v);

Thanks,

Francesco

Solution

I was these days also working on the Kitti dataset, so I just added a new function to load a kiti velodyne data file directly into MRPT (see this PR).

However, after some thinking, I noticed that Kitti raw data does not match exactly with CObservationVelodyneScan, which is aimed at storing the raw ranges for each LiDAR beam, and only optionally, a pointcloud. The Kitti velodyne data are pointclouds, actually, hence I added a new PointCloud type with XYZ+Intensity mrpt::maps::CPointsMapXYZI and added a method loadFromKittiVelodyneFile() to it. Notice this is for the mrpt master git branch, "version 1.9.9".

Now, how to insert that into a gridmap? Your idea of using a velodyne CObservation to insert it into a gridmap is one of the pending issues on our queue but, anyway, as said above, the Kitti datasets are better loaded as pointclouds.

I would recommend you converting the pointcloud into a CObservation2DRangeScan, then inserting it into the grid. That would allow you to control what part of the 3D data you really want to be reflected in the grid (i.e. what heights, etc.)

Hope it helped!