Can I recognized if localization is failed?

[Haebeom Jung]

Thank you for sharing your work, koide.

I wonder that is there any way to get error value between align points and map??

I mean, I want to get some message if localization is fail or not.

[koide3]

This package doesn't output the alignment error between the input cloud and the map, and you may need to implement some code by yourself. I think one good way is to compute inlier_fraction between cloud and map like the RANSAC pose estimator does.

https://github.com/PointCloudLibrary/pcl/blob/3906e52ee7ae4f817aef2f9e4aa6091634f12ed5/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp#L227

[Haebeom Jung]

thank you! I will try it.

[Yonghao Yu]

Hi @zang09,

Have you made some progress. I also have the same issue. Would you mind giving some instruction on it? Thanks a lot.

[Haebeom Jung]

@yutouwd

You can get score by add code like this:

double score;
auto aligned = pose_estimator->correct(filtered, score);

after pose_estimator->predict()

[Yonghao Yu]

Hi @zang09 ,

Thanks for your kindly reply. But the pose_estimator -> correct() function only has one argument. In pose_estimator.hpp:

  /**
   * @brief correct
   * @param cloud   input cloud
   * @return cloud aligned to the globalmap
   */
  pcl::PointCloud<PointT>::Ptr correct(const pcl::PointCloud<PointT>::ConstPtr& cloud) {

[koide3]

You can consider trying pub_status branch in which some information on the scan matching result (convergence, matching_error, inlier_fraction) are published to /status topic.

https://github.com/koide3/hdl_localization/blob/pub_status/msg/ScanMatchingStatus.msg

[Haebeom Jung]

@yutouwd

In my case, I added new argument.
But I think pub_status branch that @koide3 mention is the better option!

    pcl::PointCloud<PointT>::Ptr correct(const pcl::PointCloud<PointT>::ConstPtr& cloud, double &fitnessScore) {
    .
    .
    .
    pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
    registration->setInputSource(cloud);
    registration->align(*aligned, init_guess);

    fitnessScore = registration->getFitnessScore();
    .
    .
    .
    }

[Yonghao Yu]

@koide3 @zang09

Thank you guys so much. I try the pub_status branch and it works well. By the way, how much percentage of inlier would be considered as a successful localization? Is 90% a suitable threshold?