Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"' failed.

[DarrenWong]

Trying hdl_graph_slam with the hdl_graph_slam_400.launch. PCL 1.7 with ubuntu 16.04. The offical dataset is good,

But It might occur the error in the registration function in the scan-match process, but not much. https://github.com/koide3/hdl_graph_slam/blob/0228e86b7f4f9649515b0217440694bc1fdf7f20/apps/scan_matching_odometry_nodelet.cpp#L185 I have check every point does not contain NAN or Inf with codes below.

roslaunch hdl_graph_slam hdl_graph_slam_400.launch
rosbag play --clock hdl_400.bag

codes:

    auto &points = filtered->points;
    int cloud_size = points.size();
    for (size_t i = 0; i < cloud_size; ++i) {
        PointT p = points[i];
        // skip NaN and INF valued points
        if (!pcl_isfinite(p.x) ||
            !pcl_isfinite(p.y) ||
            !pcl_isfinite(p.z)) {
          std::cout<<"test"<<p<<std::endl;
          continue;
        }
    }
    registration->setInputSource(filtered);

Also, I running with ubuntu 18.04(as the readme says GICP not working well in ros-kinect) with ROS Melodi, PCL 1.8(default), the same error might happen with a custom dataset(with HDL32e). But if I change NDT_OMP method to others, for example, ICP no error occurs. But the results very bad.

My question is:

1. Does it support PCL1.8 with ROS Melodic? Because ubuntu 18.04 with PCL1.8 default support.
2. If supported, is anyone appears the same issue with the default or custom dataset?

[DarrenWong]

For the first question, I think it supported as I can finish a whole custom outdoor dataset(around 6 min's driving dataset) with ICP/GICP/NDT method with a better results.

Try to revise the registration_method in the launch file, (ubuntu18.04, ROS Melodi, PCL 1.8 with custom dataset HDL32e) frontend/backend( scan_matching_odometry_nodelet/hdl_graph_slam_nodelet)

1. NDT_OMP/NDT_OMP failed
2. NDT/NDT_OMP ok
3. GICP/NDT_OMP ok
4. ICP/NDT_OMP ok

It seems the frontend NDT_OMP's problem. I think I can keep digging if any mismatch with the hdl_400.bag and our's dataset. I Will update later.

[RonaldEnsing]

I am running into this issue as well. It also failed with NDT/NDT_OMP, although it took some more time for the failure to appear. This happens on my own bag file and with hdl_400.bag. I am running ubuntu 18.04, ROS Melodic and PCL 1.8.

[DarrenWong]

@RonaldEnsing I have a tmp solution that my bag file work after reverts a commit.

https://github.com/koide3/ndt_omp/commit/eae5ab78c05f1c4fc020160b6299d1db7f78d7ce https://github.com/koide3/ndt_omp/blob/eae5ab78c05f1c4fc020160b6299d1db7f78d7ce/include/pclomp/ndt_omp_impl.hpp#L803 change
bool interval_converged = (step_max - step_min) < 0, open_interval = true; to bool interval_converged = (step_max - step_min) > 0, open_interval = true;

[koide3]

@DarrenWong , Thanks for reporting the issue. I'll revert the update of ndt_omp that maybe introduced a bug. If possible, can you share a part of your rosbag with me so that I can reproduce the problem and dig into the bug?

[RonaldEnsing]

@DarrenWong Your modification works for me as well.

@koide3 I am using the example rosbag hdl_400.bag.

[DarrenWong]

@koide3 Sure, please refer to the the link for a 10 seconds bag file(velodyne_points only)

[koide3]

@RonaldEnsing @DarrenWong , Thanks. I'll dig into the problem on both bags.

[DarrenWong]

closed as code has reverted

[koide3]

@RonaldEnsing @DarrenWong I pushed an updated ndt_omp code to fix the segfault bug. I would appreciate if you could test if the latest NDT code works on your environments.

[DarrenWong]

@koide3 Yes the bug fix, but the performance of NDT_omp seems to degrade a lot. My bag file cannot run in realtime. The hdl_400.bag also not good enough. Did you miss some upload?

Record some timing of matching function https://github.com/koide3/hdl_graph_slam/blob/0228e86b7f4f9649515b0217440694bc1fdf7f20/apps/scan_matching_odometry_nodelet.cpp#L113 with hdl_400.bag

running with https://github.com/koide3/ndt_omp/commit/77cd5b68f749144bb55158f62bb80a066e8f16c7 latest commit ( in ms) 179.597115 90.174333 55.364579 248.044441 51.107786 238.768399 65.994021 31.756935 59.296627 176.306680 190.943328 171.338488 54.812161

revert a commit ( in ms) 25.097968 16.212199 18.206174 15.485753 16.043828 16.303549 15.542154 21.407743 25.360994 27.361463 26.565266 27.406027 25.137772

[koide3]

@DarrenWong Thanks for testing. It is expected behavior. The commit fixes a bug that prevents line search from being executed. The latest version properly performs line search and show better accuracy but can be slower.

I added a parameter to the launch file that stops line search early. Can you check if it recovers the processing speed on your environment?

https://github.com/koide3/hdl_graph_slam/blob/701a39e17e2be6d39261b5f27e0d1df993efbc99/launch/hdl_graph_slam_400.launch#L51

[DarrenWong]

@koide3 yes I think it should be better, but seems a few frame processing time might be a little higher. I also have a processing timing issue when I trying to test the performance of fast-gicp, it would be very appreciated if you can help. https://github.com/SMRT-AIST/fast_gicp/issues/17

experiment results, 23.516925 18.384633 19.514168 19.856820 19.365725 19.185885 83.883283 26.960373 23.307291 24.672768 101.783415 122.440700 19.843530 19.348690 19.923485 18.446249 22.829327 23.684086 24.022108 19.321542 18.355131 18.506491