[registration] Sporadic segfault in NDT after repeated align

[sadsimulation]

Describe the bug

After calling align on an NormalDistributionsTransform object, I get a segfault somewhere deep inside the call stack (see below).

Context

I'm trying to match successive point clouds using the NDT. The NDT is used in the callback function of a ROS service. Every time the callback function executes, I instantiate a NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>, set the source and target point clouds and execute the align method. This seems to work the first four or five times but then suddenly I get a segfault and the node crashes.

Expected behavior

NDT shouldn't segfault, especially because it seems to work for a while before the issue occurs.

Current Behavior

Repeated calls to NDT cause segfault somewhere deep inside the call stack.

To Reproduce

See code below, it's rather straightforward, just repeatedly call NDT.

Screenshots/Code snippets

#include "ros/ros.h"

#include "pcl/PCLPointCloud2.h"
#include "pcl/point_types.h"
#include "pcl/registration/ndt.h"
#include "pcl/registration/icp.h"

#include "pcl_conversions/pcl_conversions.h"
#include "point_types.h"

#include "tf2_eigen/tf2_eigen.h"

#include "myslam_msgs/PointCloudScanMatcher.h"

namespace myslam
{

class ScanMatcher
{
    private:
        double termination_threshold, optimization_step_size, grid_resolution;
        unsigned int max_iterations;

    public:
        ScanMatcher(double termination_threshold, unsigned int max_iterations, double optimization_step_size, double grid_resolution)
            : termination_threshold(termination_threshold), max_iterations(max_iterations), optimization_step_size(optimization_step_size), grid_resolution(grid_resolution) {}

        bool match(myslam_msgs::PointCloudScanMatcher::Request &req, myslam_msgs::PointCloudScanMatcher::Response &res)
        {
            // ROS interface
            // convert ROS PointCloud2 messages to CustomCloud (PointCloud<CustomPoint>)
            CustomCloud::Ptr reference(new CustomCloud);
            CustomCloud::Ptr measurement(new CustomCloud);
            pcl::fromROSMsg(req.reference, *reference);
            pcl::fromROSMsg(req.measurement, *measurement);
            // convert ROS Transform to Eigen::Matrix4f
            Eigen::Matrix4f initial_guess = tf2::transformToEigen(req.transform).matrix().cast<float>();
            // PCL interface
            pcl::PointCloud<pcl::PointXYZ>::Ptr reference_xyz(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::PointCloud<pcl::PointXYZ>::Ptr measurement_xyz(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::copyPointCloud(*reference, *reference_xyz);
            pcl::copyPointCloud(*measurement, *measurement_xyz);

            // // crashes
            pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> reg;
            reg.setTransformationEpsilon(termination_threshold);
            reg.setMaximumIterations(max_iterations);
            reg.setStepSize(optimization_step_size);
            reg.setResolution(grid_resolution);

            // the input cloud which will be aligned
            reg.setInputSource(reference_xyz);
            // the reference pointcloud to align to
            reg.setInputTarget(measurement_xyz);
            // output dummy cloud (unused)
            pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
            // perform alignment
            reg.align(*output_cloud, initial_guess);
            // extract transformation
            Eigen::Affine3d final_transform;
            final_transform.matrix() = reg.getFinalTransformation().cast<double>();
            // ROS interface
            // convert to ROS message
            res.transform = tf2::eigenToTransform(final_transform).transform;
            res.valid = reg.hasConverged();
            return true;
        }
};

} // namespace myslam

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pointcloud_scanmatcher_server");
    ros::NodeHandle nh;
    ros::NodeHandle nh_private("~");

    double termination_threshold;
    double optimization_step_size;
    int max_iterations;
    double grid_resolution;

    nh_private.param("termination_threshold", termination_threshold, 0.01);
    nh_private.param("optimization_step_size", optimization_step_size, 0.1);
    nh_private.param("max_iterations", max_iterations, 30);
    nh_private.param("grid_resolution", grid_resolution, 0.5);

    myslam::ScanMatcher matcher(termination_threshold, optimization_step_size, (unsigned int)max_iterations, grid_resolution);

    ros::ServiceServer service = nh.advertiseService("pointcloud_scanmatcher", &myslam::ScanMatcher::match, &matcher);

    ROS_INFO("Ready to match pointclouds");
    while(ros::ok()) {
        ros::spin();
    }
    return 0;
}

==173658== Invalid write of size 4
==173658==    at 0x491C969: pcl::KdTreeFLANN<pcl::PointXYZ, flann::L2_Simple<float> >::radiusSearch(pcl::PointXYZ const&, double, std::vector<int, std::allocator<int> >&, std::vector<float, std::allocator<float> >&, unsigned int) const (in /home/riy2rng/.conda/envs/mamba/envs/myenv/lib/libpcl_kdtree.so.1.12.0)
==173658==    by 0x2EF420: pcl::VoxelGridCovariance<pcl::PointXYZ>::radiusSearch(pcl::PointXYZ const&, double, std::vector<pcl::VoxelGridCovariance<pcl::PointXYZ>::Leaf const*, std::allocator<pcl::VoxelGridCovariance<pcl::PointXYZ>::Leaf const*> >&, std::vector<float, std::allocator<float> >&, unsigned int) const (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2EB074: pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::computeDerivatives(Eigen::Matrix<double, 6, 1, 0, 6, 1>&, Eigen::Matrix<double, 6, 6, 0, 6, 6>&, pcl::PointCloud<pcl::PointXYZ> const&, Eigen::Matrix<double, 6, 1, 0, 6, 1> const&, bool) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2E8B42: pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::computeTransformation(pcl::PointCloud<pcl::PointXYZ>&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2C57DA: pcl::Registration<pcl::PointXYZ, pcl::PointXYZ, float>::align(pcl::PointCloud<pcl::PointXYZ>&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2C22FC: myslam::ScanMatcher::match(myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2E05DF: boost::_mfi::mf2<bool, myslam::ScanMatcher, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>::operator()(myslam::ScanMatcher*, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&) const (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2DD571: bool boost::_bi::list3<boost::_bi::value<myslam::ScanMatcher*>, boost::arg<1>, boost::arg<2> >::operator()<bool, boost::_mfi::mf2<bool, myslam::ScanMatcher, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>, boost::_bi::rrlist2<myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&> >(boost::_bi::type<bool>, boost::_mfi::mf2<bool, myslam::ScanMatcher, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>&, boost::_bi::rrlist2<myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>&, long) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2DA6D0: bool boost::_bi::bind_t<bool, boost::_mfi::mf2<bool, myslam::ScanMatcher, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>, boost::_bi::list3<boost::_bi::value<myslam::ScanMatcher*>, boost::arg<1>, boost::arg<2> > >::operator()<myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>(myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2D73D5: boost::detail::function::function_obj_invoker2<boost::_bi::bind_t<bool, boost::_mfi::mf2<bool, myslam::ScanMatcher, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>, boost::_bi::list3<boost::_bi::value<myslam::ScanMatcher*>, boost::arg<1>, boost::arg<2> > >, bool, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>::invoke(boost::detail::function::function_buffer&, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2EC884: boost::function2<bool, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>::operator()(myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&) const (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2E9CEC: ros::ServiceSpec<myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> > >::call(boost::function<bool (myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&)> const&, ros::ServiceSpecCallParams<myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> > >&) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==  Address 0x0 is not stack'd, malloc'd or (recently) free'd
==173658== 
==173658== 
==173658== Process terminating with default action of signal 11 (SIGSEGV)
==173658==  Access not within mapped region at address 0x0
==173658==    at 0x491C969: pcl::KdTreeFLANN<pcl::PointXYZ, flann::L2_Simple<float> >::radiusSearch(pcl::PointXYZ const&, double, std::vector<int, std::allocator<int> >&, std::vector<float, std::allocator<float> >&, unsigned int) const (in /home/riy2rng/.conda/envs/mamba/envs/myenv/lib/libpcl_kdtree.so.1.12.0)
==173658==    by 0x2EF420: pcl::VoxelGridCovariance<pcl::PointXYZ>::radiusSearch(pcl::PointXYZ const&, double, std::vector<pcl::VoxelGridCovariance<pcl::PointXYZ>::Leaf const*, std::allocator<pcl::VoxelGridCovariance<pcl::PointXYZ>::Leaf const*> >&, std::vector<float, std::allocator<float> >&, unsigned int) const (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2EB074: pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::computeDerivatives(Eigen::Matrix<double, 6, 1, 0, 6, 1>&, Eigen::Matrix<double, 6, 6, 0, 6, 6>&, pcl::PointCloud<pcl::PointXYZ> const&, Eigen::Matrix<double, 6, 1, 0, 6, 1> const&, bool) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2E8B42: pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::computeTransformation(pcl::PointCloud<pcl::PointXYZ>&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2C57DA: pcl::Registration<pcl::PointXYZ, pcl::PointXYZ, float>::align(pcl::PointCloud<pcl::PointXYZ>&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2C22FC: myslam::ScanMatcher::match(myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2E05DF: boost::_mfi::mf2<bool, myslam::ScanMatcher, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>::operator()(myslam::ScanMatcher*, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&) const (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2DD571: bool boost::_bi::list3<boost::_bi::value<myslam::ScanMatcher*>, boost::arg<1>, boost::arg<2> >::operator()<bool, boost::_mfi::mf2<bool, myslam::ScanMatcher, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>, boost::_bi::rrlist2<myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&> >(boost::_bi::type<bool>, boost::_mfi::mf2<bool, myslam::ScanMatcher, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>&, boost::_bi::rrlist2<myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>&, long) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2DA6D0: bool boost::_bi::bind_t<bool, boost::_mfi::mf2<bool, myslam::ScanMatcher, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>, boost::_bi::list3<boost::_bi::value<myslam::ScanMatcher*>, boost::arg<1>, boost::arg<2> > >::operator()<myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>(myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2D73D5: boost::detail::function::function_obj_invoker2<boost::_bi::bind_t<bool, boost::_mfi::mf2<bool, myslam::ScanMatcher, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>, boost::_bi::list3<boost::_bi::value<myslam::ScanMatcher*>, boost::arg<1>, boost::arg<2> > >, bool, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>::invoke(boost::detail::function::function_buffer&, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2EC884: boost::function2<bool, myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&>::operator()(myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&) const (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==    by 0x2E9CEC: ros::ServiceSpec<myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> > >::call(boost::function<bool (myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >&, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> >&)> const&, ros::ServiceSpecCallParams<myslam_msgs::PointCloudScanMatcherRequest_<std::allocator<void> >, myslam_msgs::PointCloudScanMatcherResponse_<std::allocator<void> > >&) (in /home/riy2rng/Documents/ros/myslam_catkin_ws/devel/lib/myslam/ScanMatcher)
==173658==  If you believe this happened as a result of a stack
==173658==  overflow in your program's main thread (unlikely but
==173658==  possible), you can try to increase the size of the
==173658==  main thread stack using the --main-stacksize= flag.
==173658==  The main thread stack size used in this run was 8388608.
==173658== 
==173658== HEAP SUMMARY:
==173658==     in use at exit: 298,107 bytes in 939 blocks
==173658==   total heap usage: 25,701 allocs, 24,762 frees, 45,506,667 bytes allocated
==173658== 
==173658== LEAK SUMMARY:
==173658==    definitely lost: 0 bytes in 0 blocks
==173658==    indirectly lost: 0 bytes in 0 blocks
==173658==      possibly lost: 66,752 bytes in 12 blocks
==173658==    still reachable: 231,355 bytes in 927 blocks
==173658==         suppressed: 0 bytes in 0 blocks
==173658== Rerun with --leak-check=full to see details of leaked memory
==173658== 
==173658== For lists of detected and suppressed errors, rerun with: -s
==173658== ERROR SUMMARY: 1 errors from 1 contexts (suppressed: 0 from 0)
Segmentation fault (core dumped)

Your Environment (please complete the following information):

• OS: Ubuntu 20.04
• Compiler: GCC 9.4.0
• PCL Version 1.12.0
• ROS noetic

Possible Solution

Sorry, no idea

Additional context

There seems to be a similar bug #4944, but I'm not sure I understand their solution.

[mvieth]

My best guess right now is that sometimes, the reference or measurement cloud or the initial guess from the request cause the segfault. Can you print the req.transform and check if something looks weird, and also store the two clouds as PCD files in every match call and upload a pair that trigger a segfault?

[sadsimulation]

Hi @mvieth,
unfortunately I'm not allowed to share the point clouds themselves, however they are fairly sparse, roughly 200 to 300 points in a volume of 20x10x3 meters. Could this be the culprit?
The req.transform also seems fine and in line with what I would expect (displacement in x about 1.4 meters, almost no displacement in y and theta). Printing initial_guess also shows a Matrix of the correct structure so I don't think that's it:

initial_guess
   0.999846   0.0175365           0     1.38908
 -0.0175365    0.999846           0 -0.00778549
          0           0           1           0
          0           0           0           1

For what it's worth, when replacing pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> with pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> (and removing ndt related function calls) no segfaults occur.

[mvieth]

unfortunately I'm not allowed to share the point clouds themselves, however they are fairly sparse, roughly 200 to 300 points in a volume of 20x10x3 meters. Could this be the culprit?

Maybe, but difficult to say. If you can't share the point clouds, that means that I can't do any debugging. I can only make suggestions how you can debug. You can try building PCL yourself with debug symbols (e.g. RelWithDebInfo), so we hopefully get more information from valgrind. Most importantly, which line the invalid write happens exactly.

[sadsimulation]

Okay I did some more investigations on this. Unfortunately I didn't get a debug build of PCL running inside the conda environment yet, however the problem seems definitely related to the NDT parameter grid_resolution.
Using to example point clouds measurement.pcd and reference.pcd and the following script

#!/bin/bash
mkdir -p what && cd what
for res in $(seq 0 0.1 1); do
    cp ../measurement.pcd a.pcd
    cp ../reference.pcd b.pcd
    echo "Grid resolution: $res"
    pcl_ndt3d -r $res a.pcd b.pcd
    rm a.pcd b.pcd
done

Leads to the following output:

Grid resolution: 0.0
a.pcd width: 267 height: 1
saving first model to a.pcd
b.pcd width: 311 height: 1
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
saving result to b.pcd
Grid resolution: 0.1
a.pcd width: 267 height: 1
saving first model to a.pcd
b.pcd width: 311 height: 1
./test.sh: line 3: 91786 Segmentation fault      (core dumped) pcl_ndt3d -r $res a.pcd b.pcd
Grid resolution: 0.2
a.pcd width: 267 height: 1
saving first model to a.pcd
b.pcd width: 311 height: 1
./test.sh: line 3: 91795 Segmentation fault      (core dumped) pcl_ndt3d -r $res a.pcd b.pcd
Grid resolution: 0.3
a.pcd width: 267 height: 1
saving first model to a.pcd
b.pcd width: 311 height: 1
./test.sh: line 3: 91800 Segmentation fault      (core dumped) pcl_ndt3d -r $res a.pcd b.pcd
Grid resolution: 0.4
a.pcd width: 267 height: 1
saving first model to a.pcd
b.pcd width: 311 height: 1
./test.sh: line 3: 91806 Segmentation fault      (core dumped) pcl_ndt3d -r $res a.pcd b.pcd
Grid resolution: 0.5
a.pcd width: 267 height: 1
saving first model to a.pcd
b.pcd width: 311 height: 1
./test.sh: line 3: 91811 Segmentation fault      (core dumped) pcl_ndt3d -r $res a.pcd b.pcd
Grid resolution: 0.6
a.pcd width: 267 height: 1
saving first model to a.pcd
b.pcd width: 311 height: 1
   0.999346  -0.0337172   -0.013092  -0.0881481
  0.0337013    0.999431 -0.00143146  -0.0185708
  0.0131328 0.000989308    0.999913  -0.0239604
          0           0           0           1
saving result to b.pcd
Grid resolution: 0.7
a.pcd width: 267 height: 1
saving first model to a.pcd
b.pcd width: 311 height: 1
   0.999973  0.00662339   0.0031249   0.0378989
-0.00664188     0.99996  0.00594562   0.0917992
-0.00308539 -0.00596622    0.999977  0.00689903
          0           0           0           1
saving result to b.pcd
Grid resolution: 0.8
a.pcd width: 267 height: 1
saving first model to a.pcd
b.pcd width: 311 height: 1
   0.999929  -0.0101831  0.00621703   0.0535366
  0.0101155     0.99989   0.0108079  -0.0757622
-0.00632641  -0.0107443    0.999922    0.033686
          0           0           0           1
saving result to b.pcd
Grid resolution: 0.9
a.pcd width: 267 height: 1
saving first model to a.pcd
b.pcd width: 311 height: 1
    0.999993  -0.00382087  0.000148251    0.0994551
  0.00382077     0.999993  0.000636155    0.0055386
-0.000150681 -0.000635584            1  -0.00793616
           0            0            0            1
saving result to b.pcd
Grid resolution: 1.0
a.pcd width: 267 height: 1
saving first model to a.pcd
b.pcd width: 311 height: 1
    0.999996   0.00277143  0.000791606 -0.000678271
 -0.00277238     0.999995   0.00120338   0.00865453
-0.000788267  -0.00120557     0.999999   0.00323504
           0            0            0            1
saving result to b.pcd

As you can see, as soon as the NDT grid resolution becomes bigger the segfaults disappear.
For one of the failing test cases the valgrind output is the same as before

a.pcd width: 267 height: 1
saving first model to a.pcd
b.pcd width: 311 height: 1
==92202== Invalid write of size 4
==92202==    at 0x53AD969: pcl::KdTreeFLANN<pcl::PointXYZ, flann::L2_Simple<float> >::radiusSearch(pcl::PointXYZ const&, double, std::vector<int, std::allocator<int> >&, std::vector<float, std::allocator<float> >&, unsigned int) const (in /home/riy2rng/.conda/envs/mamba/envs/feck/lib/libpcl_kdtree.so.1.12.0)
==92202==    by 0x11F6D3: pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::computeDerivatives(Eigen::Matrix<double, 6, 1, 0, 6, 1>&, Eigen::Matrix<double, 6, 6, 0, 6, 6>&, pcl::PointCloud<pcl::PointXYZ> const&, Eigen::Matrix<double, 6, 1, 0, 6, 1> const&, bool) (in /home/riy2rng/.conda/envs/mamba/envs/feck/bin/pcl_ndt3d)
==92202==    by 0x123475: pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::computeTransformation(pcl::PointCloud<pcl::PointXYZ>&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&) (in /home/riy2rng/.conda/envs/mamba/envs/feck/bin/pcl_ndt3d)
==92202==    by 0x112659: main (in /home/riy2rng/.conda/envs/mamba/envs/feck/bin/pcl_ndt3d)
==92202==  Address 0x0 is not stack'd, malloc'd or (recently) free'd
==92202== 
==92202== 
==92202== Process terminating with default action of signal 11 (SIGSEGV)
==92202==  Access not within mapped region at address 0x0
==92202==    at 0x53AD969: pcl::KdTreeFLANN<pcl::PointXYZ, flann::L2_Simple<float> >::radiusSearch(pcl::PointXYZ const&, double, std::vector<int, std::allocator<int> >&, std::vector<float, std::allocator<float> >&, unsigned int) const (in /home/riy2rng/.conda/envs/mamba/envs/feck/lib/libpcl_kdtree.so.1.12.0)
==92202==    by 0x11F6D3: pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::computeDerivatives(Eigen::Matrix<double, 6, 1, 0, 6, 1>&, Eigen::Matrix<double, 6, 6, 0, 6, 6>&, pcl::PointCloud<pcl::PointXYZ> const&, Eigen::Matrix<double, 6, 1, 0, 6, 1> const&, bool) (in /home/riy2rng/.conda/envs/mamba/envs/feck/bin/pcl_ndt3d)
==92202==    by 0x123475: pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::computeTransformation(pcl::PointCloud<pcl::PointXYZ>&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&) (in /home/riy2rng/.conda/envs/mamba/envs/feck/bin/pcl_ndt3d)
==92202==    by 0x112659: main (in /home/riy2rng/.conda/envs/mamba/envs/feck/bin/pcl_ndt3d)
==92202==  If you believe this happened as a result of a stack
==92202==  overflow in your program's main thread (unlikely but
==92202==  possible), you can try to increase the size of the
==92202==  main thread stack using the --main-stacksize= flag.
==92202==  The main thread stack size used in this run was 8388608.
==92202== 
==92202== HEAP SUMMARY:
==92202==     in use at exit: 285,541 bytes in 1,765 blocks
==92202==   total heap usage: 6,823 allocs, 5,058 frees, 680,549 bytes allocated
==92202== 
==92202== LEAK SUMMARY:
==92202==    definitely lost: 0 bytes in 0 blocks
==92202==    indirectly lost: 0 bytes in 0 blocks
==92202==      possibly lost: 0 bytes in 0 blocks
==92202==    still reachable: 285,541 bytes in 1,765 blocks
==92202==                       of which reachable via heuristic:
==92202==                         newarray           : 16,392 bytes in 1 blocks
==92202==         suppressed: 0 bytes in 0 blocks
==92202== Rerun with --leak-check=full to see details of leaked memory
==92202== 
==92202== For lists of detected and suppressed errors, rerun with: -s
==92202== ERROR SUMMARY: 1 errors from 1 contexts (suppressed: 0 from 0)
Segmentation fault (core dumped)

[mvieth]

@sadsimulation Ok, then I am quite sure this is what happens: The VoxelGridCovariance used by NDT has a parameter that says how many points a voxel must contain to be considered valid (if a voxel contains fewer points, the voxel and the points are not taken into account). By default this parameter is 6. If the grid resolution is very small, no voxel will contain more than the required number of points and no voxel will be considered valid. Then the search fails. This effect is amplified by the fact that your cloud is quite sparse. As you already found out, you can increase the grid resolution (make voxels bigger) until no more segfaults happen. Alternatively, if you compile PCL yourself, you can try decreasing the min_points_per_voxel_ in VoxelGridCovariance from 6 to 5, 4, or 3. I hope this helps.

[sadsimulation]

@mvieth Ok, that sounds like what I expected as well.
Is there a way of pre-filtering the point cloud to avoid this behavior? Maybe using the aforementioned VoxelGrid?
Having a ROS node segfault because the point cloud wasn't dense enough isn't exactly graceful degradation.

[mvieth]

@mvieth Ok, that sounds like what I expected as well. Is there a way of pre-filtering the point cloud to avoid this behavior? Maybe using the aforementioned VoxelGrid?

Unfortunately no. The problem is that the cloud contains too few points (is too sparse), so filtering it (removing more points) will only make it worse. And I also don't see a way of adding new points to your cloud that have useful information, if your cloud doesn't contain those points/that information already. If the two solutions I suggested in my last comment are not desirable for you, maybe NDT isn't the right tool in your situation and some ICP variant might work better.

Having a ROS node segfault because the point cloud wasn't dense enough isn't exactly graceful degradation.

I agree that NDT and VoxelGridCovariance should handle this better, I am working on a fix that avoids the segfault and informs the user what is happening.

[sadsimulation]

Unfortunately no. The problem is that the cloud contains too few points (is too sparse), so filtering it (removing more points) will only make it worse.

Yeah I just tried it and realized VoxelGrid downsamples the data as well. I think something that could help is using a voxel grid to group the points but without downsampling the cloud. That would make it possible to at least remove areas of the cloud that aren't dense enough.

[mvieth]

I think something that could help is using a voxel grid to group the points but without downsampling the cloud. That would make it possible to at least remove areas of the cloud that aren't dense enough.

I don't understand how that would help? What do you mean with "remove areas of the cloud"? How would that change the cloud(s) you pass to NDT?