Итеративные Ближайшие Точки вылетают после прохождения сквозь отфильтрованные Облака Точек

При объединении нескольких облаков точек с модулем регистрации в библиотеке облаков точек программа завершает работу с 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);
}

0 ответов

Другие вопросы по тегам