How to properly set the hyper parameters for NDT algorithm ?

[shyam-botsync]

Hi Authors! First I want to thank you all for the awesome work with PCL. Even responses to issues were very fast.

I require some guidance on how to use PCL libraries's NDT algorithm for localisation.

The problem I am trying to solve is to monitor whether the robot is localised properly to a map generated by Gmapping (2D occupancy grid.) The robot uses a laser scanner to see the environment. The reason I went with NDT is because on looking at the working concept it looked like it's computationally less intensive than ICP for alignment. Since autoware is using it as submodule for its localisation package, I concluded that it must very robust.

I am following this tutorial. However I am not able to get proper alignment as shown in below picture. Interestingly .hasConverged() calls also returns true even though it's not aligned. what is the purpose of hasConverged() ? are there any alignment metric one can get to know if its aligned ?

Coming to the main issue, how does one set NDT parameter to get proper alignment ? are there any doctrines to follow ?

My pipeline is a follows.

1. Convert map to pcl data(pcl::Pointxyz).
2. convert laserscan to pcl data(pcl::PointXYZ).
3. Downsample laserscan pcl using approximate voxel sampling.
4. apply NDT.
5. use hasConverged() call to check if aligned.

Code for reference

//approximate dynamicl voxel grid filtering
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;

//scan data downsampling
 pcl::PointCloud<pcl::PointXYZ>::Ptr scan_keypointsPtr(new pcl::PointCloud<pcl::PointXYZ>());
 pcl::PointCloud<pcl::PointXYZ>::Ptr ScanPointCloudPtr(new pcl::PointCloud<pcl::PointXYZ>(ScanPointCloud) );
 approximate_voxel_filter.setLeafSize (0.1, 0.1, 0.1);
 approximate_voxel_filter.setInputCloud (ScanPointCloudPtr);
 approximate_voxel_filter.filter (*scan_keypointsPtr);
 pcl::Indices index;
 pcl::removeNaNFromPointCloud (pcl::PointCloud<pcl::PointXYZ>(*scan_keypointsPtr), *scan_keypointsPtr, index);
//map data downsampling
//not performing filtering for target cloud as grid overlay is used instead of individual points.
pcl::PointCloud<pcl::PointXYZ>::Ptr map_keypointsPtr(new pcl::PointCloud<pcl::PointXYZ>(readmapcloud));

#ifdef visualisation
   sensor_msgs::PointCloud vis_cloud3;
   perceptionHelper::PCLtoROSmsg(*map_keypointsPtr, vis_cloud3);
   MapKeyCloud.publish(vis_cloud3);
   sensor_msgs::PointCloud vis_cloud4;
   perceptionHelper::PCLtoROSmsg(*scan_keypointsPtr, vis_cloud4);
   ScanKeyCloud.publish(vis_cloud4);
#endif

//perform NDT
 pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
  ndt.setTransformationEpsilon (0.01);
  ndt.setStepSize (5.0);
  ndt.setResolution (0.5);
  ndt.setMaximumIterations (35);
  ndt.setInputSource (scan_keypointsPtr);
  ndt.setInputTarget (map_keypointsPtr);

  Eigen::Matrix4f init_guess;
  init_guess.setIdentity();
  pcl::PointCloud<pcl::PointXYZ>::Ptr TransformedCloudPtr (new pcl::PointCloud<pcl::PointXYZ>());
  ndt.align(*TransformedCloudPtr,init_guess);

yellow-> original scan cloud, white-> map point cloud, red-> NDT transformed point cloud

[mvieth]

Please use Stackoverflow or the Discord community chat for such questions. The GitHub issues are intended for bug reports and compile errors.