Итеративные Ближайшие Точки вылетают после прохождения сквозь отфильтрованные Облака Точек
При объединении нескольких облаков точек с модулем регистрации в библиотеке облаков точек программа завершает работу с SIGSEGV в алгоритме ICP, если я использую отфильтрованные облака точек с помощью Passthrough-Filter.
Если я использую нефильтрованные облака точек, все работает нормально.
Мне просто интересно, если кто-то испытал нечто подобное, может дать мне подсказку или даже может разоблачить ошибку, которую я сделал.
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
inline void filterPointCloud(PointCloud::Ptr& input){
pcl::PassThrough<PointT> pass;
PointCloud::Ptr output = boost::make_shared<PointCloud>();
pass.setFilterFieldName("z");
pass.setFilterLimits(1.7, 2.);
pass.setInputCloud(input);
pass.filter(*output);
input = output;
}
int main (int argc, char** argv){
FrameFeeder feeder;
feeder.loadAllFrames();
PointCloud::Ptr cloud_out(feeder.getNextFrame());
PointCloud::Ptr cloud_in;
filterPointCloud(cloud_out);
pcl::IterativeClosestPoint<PointT, PointT> icp;
bool run = true;
while(run) {
if(!feeder.isFramePipeEmpty()){
cloud_in = feeder.getNextFrame();
filterPointCloud(cloud_in);
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
icp.setTransformationEpsilon(1e-5);
std::cout<< "aligning..." << std::endl;
icp.align(*cloud_in);
std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
PointCloud::Ptr o_tmp = boost::make_shared<PointCloud>();
*o_tmp = *(cloud_in) + *(cloud_out);
cloud_out = o_tmp;
}else{
run = false;
if (pcl::io::savePCDFile<PointT> ("../results/final.pcd", *cloud_out) == -1) {
PCL_ERROR ("Couldn't write in file\n");
return (-1);
}
/*
}
}
return (0);
}