NormalDistributionsTransform segfaults with a sparse pointcloud

[Tamme]

Problem

When setting a sparse pointcloud as an input target for NormalDistributionsTransform, in the init function, voxel filtering is automatically done and with a small leaf size, this will produce empty voxel centroids and
rows 291 - 295 in voxel_grid_convariance.h do not set an input cloud for kdtree_ variable. Later in line 482, kdtree_.radiusSearch is called on an empty pointcloud that is the likley cause of the segfault.

Probable fix would be to add a check for the kdtree_ having points in NormalDistributionsTransform and also if possible make a public variable to decide if the filtering is wanted in the first place.

Callstack

libpcl_kdtree.so.1.7!void pcl::PointRepresentationpcl::PointXYZI::vectorize<std::vector<float, std::allocator > >(pcl::PointXYZI const&, std::vector<float, std::allocator >&) const (Unknown Source:0)
libpcl_kdtree.so.1.7!pcl::KdTreeFLANN<pcl::PointXYZI, flann::L2_Simple >::radiusSearch(pcl::PointXYZI const&, double, std::vector<int, std::allocator >&, std::vector<float, std::allocator >&, unsigned int) const (Unknown Source:0)
pcl::VoxelGridCovariancepcl::PointXYZI::radiusSearch(pcl::VoxelGridCovariancepcl::PointXYZI * this, const pcl::PointXYZI & point, double radius, std::vector<pcl::VoxelGridCovariancepcl::PointXYZI::Leaf const*, std::allocator<pcl::VoxelGridCovariancepcl::PointXYZI::Leaf const*> > & k_leaves, std::vector<float, std::allocator > & k_sqr_distances, unsigned int max_nn) (/usr/include/pcl-1.7/pcl/filters/voxel_grid_covariance.h:482)
pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>::computeDerivatives(pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> * this, Eigen::Matrix<double, 6, 1, 0, 6, 1> & score_gradient, Eigen::Matrix<double, 6, 6, 0, 6, 6> & hessian, pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>::PointCloudSource & trans_cloud, Eigen::Matrix<double, 6, 1, 0, 6, 1> & p, bool compute_hessian) (/usr/include/pcl-1.7/pcl/registration/impl/ndt.hpp:206)
pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>::computeTransformation(pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> * this, pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>::PointCloudSource & output, const Eigen::Matrix4f & guess) (/usr/include/pcl-1.7/pcl/registration/impl/ndt.hpp:120)
pcl::Registration<pcl::PointXYZI, pcl::PointXYZI, float>::align(pcl::Registration<pcl::PointXYZI, pcl::PointXYZI, float> * this, pcl::Registration<pcl::PointXYZI, pcl::PointXYZI, float>::PointCloudSource & output, const pcl::Registration<pcl::PointXYZI, pcl::PointXYZI, float>::Matrix4 & guess) (/usr/include/pcl-1.7/pcl/registration/impl/registration.hpp:216)
agv::LazyNDTLocalizerNode::handlePoints(agv::LazyNDTLocalizerNode * this, const pcl::PointCloudpcl::PointXYZI::ConstPtr msg) (/home/.../.../ros/src/agv_mapping/lazy_ndt_localizer/src/lazy_ndt_localizer_node.cpp:80)
boost::_mfi::mf1<void, agv::LazyNDTLocalizerNode, boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> >::operator()(const boost::_mfi::mf1<void, agv::LazyNDTLocalizerNode, boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> > * this, agv::LazyNDTLocalizerNode * p, boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> a1) (/usr/include/boost/bind/mem_fn_template.hpp:165)
boost::_bi::list2<boost::_bi::valueagv::LazyNDTLocalizerNode*, boost::arg<1> >::operator()<boost::_mfi::mf1<void, agv::LazyNDTLocalizerNode, boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> >, boost::_bi::list1<boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> const&> >(boost::_bi::list2<boost::_bi::valueagv::LazyNDTLocalizerNode*, boost::arg<1> > * this, boost::_mfi::mf1<void, agv::LazyNDTLocalizerNode, boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> > & f, boost::_bi::list1<boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> const&> & a) (/usr/include/boost/bind/bind.hpp:313)
boost::_bi::bind_t<void, boost::_mfi::mf1<void, agv::LazyNDTLocalizerNode, boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> >, boost::_bi::list2<boost::_bi::valueagv::LazyNDTLocalizerNode*, boost::arg<1> > >::operator()<boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> >(boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const>&&)(boost::_bi::bind_t<void, boost::_mfi::mf1<void, agv::LazyNDTLocalizerNode, boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> >, boost::_bi::list2<boost::_bi::valueagv::LazyNDTLocalizerNode*, boost::arg<1> > > * this, <unknown type in /home/.../.../ros/devel_debug/.private/lazy_ndt_localizer/lib/lazy_ndt_localizer/lazy_ndt_localizer_node, CU 0x19d57d, DIE 0x46ae90> a1) (/usr/include/boost/bind/bind.hpp:905)
boost::detail::function::void_function_obj_invoker1<boost::_bi::bind_t<void, boost::_mfi::mf1<void, agv::LazyNDTLocalizerNode, boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> >, boost::_bi::list2<boost::_bi::valueagv::LazyNDTLocalizerNode*, boost::arg<1> > >, void, boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> >::invoke(boost::detail::function::function_buffer & function_obj_ptr, boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> a0) (/usr/include/boost/function/function_template.hpp:159)
boost::function1<void, boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> >::operator()(const boost::function1<void, boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> > * this, boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const> a0) (/usr/include/boost/function/function_template.hpp:772)
ros::SubscriptionCallbackHelperT<boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const>, void>::call(ros::SubscriptionCallbackHelperT<boost::shared_ptr<pcl::PointCloudpcl::PointXYZI const>, void> * this, ros::SubscriptionCallbackHelperCallParams & params) (/opt/ros/kinetic/include/ros/subscription_callback_helper.h:144)
libroscpp.so!ros::SubscriptionQueue::call() (Unknown Source:0)
libroscpp.so!ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) (Unknown Source:0)
libroscpp.so!ros::CallbackQueue::callAvailable(ros::WallDuration) (Unknown Source:0)
libroscpp.so!ros::SingleThreadedSpinner::spin(ros::CallbackQueue*) (Unknown Source:0)
libroscpp.so!ros::spin() (Unknown Source:0)

Your Environment

• Operating System and version: Ubuntu 16.04
• Compiler: Clang 6
• PCL Version: 1.7.2

Code to Reproduce

pcl::NormalDistributionsTransform<PointT, PointT> ndt_;
ndt_.setTransformationEpsilon(trans_epsilon_);
ndt_.setStepSize(step_size_);
ndt_.setResolution(ndt_res_);
ndt_.setMaximumIterations(max_iter_);
ndt_.setInputSource(downsampled_scan);
ndt_.setInputTarget(reference_map_);
PointCloud output_cloud;
ndt_.align(output_cloud, Eigen::Isometry3f::Identity().matrix());

[taketwo]

Probable fix would be to add a check for the kdtree_ having points in NormalDistributionsTransform and also if possible make a public variable to decide if the filtering is wanted in the first place.

Agree that there should be a check rather than a segfault. That said, doesn't the fact that there are no voxels with sufficient points indicate that the NDT algorithm won't work? I.e. that there is not enough data to compute distributions?

[Tamme]

That would suffice, but all the functions that would enable to check it seem to be protected. Or you mean I should simulate it myself as a separate function? - That would be pointless overhead...

[taketwo]

Or you mean I should simulate it myself as a separate function?

No no. I just meant that we probably don't want to allow user to disable filtering as a workaround for the issue. Because if he experiences this issue, the data is not good anyway. (But I should admit I'm not very familiar with NDT, never used it, so I may be wrong.)

Please summarize the changes you propose (possibly with snippets), let's discuss them and integrate.

[stale]

Marking this as stale due to 30 days of inactivity. It will be closed in 7 days if no further activity occurs.