taketwo
commented
6 years ago 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
stale[bot]
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_gridconvariance.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::PointRepresentation::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::VoxelGridCovariance::radiusSearch(pcl::VoxelGridCovariance this, const pcl::PointXYZI & point, double radius, std::vector<pcl::VoxelGridCovariance::Leaf const , std::allocator<pcl::VoxelGridCovariance::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::PointCloud::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::PointCloud const> >::operator()(const boost::_mfi::mf1<void, agv::LazyNDTLocalizerNode, boost::shared_ptr<pcl::PointCloud const> > this, agv::LazyNDTLocalizerNode p, boost::shared_ptr<pcl::PointCloud const> a1) (/usr/include/boost/bind/mem_fn_template.hpp:165)
boost::_bi::list2<boost::_bi::value<agv::LazyNDTLocalizerNode >, boost::arg<1> >::operator()<boost::_mfi::mf1<void, agv::LazyNDTLocalizerNode, boost::shared_ptr<pcl::PointCloud const> >, boost::_bi::list1<boost::shared_ptr<pcl::PointCloud const> const&> >(boost::_bi::list2<boost::_bi::value<agv::LazyNDTLocalizerNode>, boost::arg<1> > this, boost::_mfi::mf1<void, agv::LazyNDTLocalizerNode, boost::shared_ptr<pcl::PointCloud const> > & f, boost::_bi::list1<boost::shared_ptr<pcl::PointCloud 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::PointCloud const> >, boost::_bi::list2<boost::_bi::value<agv::LazyNDTLocalizerNode>, boost::arg<1> > >::operator()<boost::shared_ptr<pcl::PointCloud const> >(boost::shared_ptr<pcl::PointCloud const>&&)(boost::_bi::bind_t<void, boost::_mfi::mf1<void, agv::LazyNDTLocalizerNode, boost::shared_ptr<pcl::PointCloud const> >, boost::_bi::list2<boost::_bi::value<agv::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::PointCloud const> >, boost::_bi::list2<boost::_bi::value<agv::LazyNDTLocalizerNode >, boost::arg<1> > >, void, boost::shared_ptr<pcl::PointCloud const> >::invoke(boost::detail::function::function_buffer & function_obj_ptr, boost::shared_ptr<pcl::PointCloud const> a0) (/usr/include/boost/function/function_template.hpp:159)
boost::function1<void, boost::shared_ptr<pcl::PointCloud const> >::operator()(const boost::function1<void, boost::shared_ptr<pcl::PointCloud const> > this, boost::shared_ptr<pcl::PointCloud const> a0) (/usr/include/boost/function/function_template.hpp:772)
ros::SubscriptionCallbackHelperT<boost::shared_ptr<pcl::PointCloud const>, void>::call(ros::SubscriptionCallbackHelperT<boost::shared_ptr<pcl::PointCloud 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
Code to Reproduce
pcl::NormalDistributionsTransform<PointT, PointT> ndt; ndt.setTransformationEpsilon(transepsilon); ndt_.setStepSize(stepsize); ndt_.setResolution(ndtres); ndt_.setMaximumIterations(maxiter); ndt_.setInputSource(downsampledscan); ndt.setInputTarget(referencemap); PointCloud outputcloud; ndt.align(output_cloud, Eigen::Isometry3f::Identity().matrix());