maps - 充填MRPTベロダインスキャン観察

Question

私は MRPT を初めて使用し、velodyne ポイント クラウドを使用して占有グリッド マップを作成するために使用したいと考えています。

KITTI データセットは、ベロダイン点群を (x,y,z,r) 形式で提供します。ここで、r は反射率です。mrpt::obs::CObservationVelodyneScan にそのようなデータを入力しようとしていますが、insertObservation メソッドを使用しても何も起こらないようです。この観察タイプを使用するための正しい方向を教えてもらえますか?

私のコードは基本的に次のようになります。

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);

ありがとう、

フランチェスコ