cilantro(2)

最新推荐文章于 2024-08-09 07:32:23 发布

人生体验

最新推荐文章于 2024-08-09 07:32:23 发布

阅读量287

点赞数

分类专栏：实习

本文链接：https://blog.csdn.net/weixin_43384504/article/details/104878901

版权

实习专栏收录该内容

58 篇文章 12 订阅

订阅专栏

还是基础的，我发现这个库，好像没有所谓的滤波模块，不知道是不是我没找到。所以，我仿照pcl 的statistic 思路，写了一个简单的验证的程序，后面想自己写一个类!

#include <cilantro/kd_tree.hpp>
#include <cilantro/io.hpp>
#include <iostream>


int main(int argc, char ** argv) {
    std::vector<Eigen::Vector3f> points;
    points.emplace_back(0, 0, 0);
    points.emplace_back(1, 0, 0);
    points.emplace_back(0, 1, 0);
    points.emplace_back(0, 0, 1);
    points.emplace_back(0, 1, 1);
    points.emplace_back(1, 0, 1);
    points.emplace_back(1, 1, 0);
    points.emplace_back(1, 1, 1);
    points.emplace_back(10,0,0);
    cilantro::KDTree3f tree(points);
    //第一个参数是点，第二个参数是k近邻的个数，第三个参数是半径，可以用来约束找到点的个数

    //找到每个点的最近5个点,这里的主要是用平均距离来筛选的，实际上还有其他的参数，后面关于滤波可以一写一个类
    std::vector<Eigen::Vector3f>::iterator i;
    for( i=points.begin();i!=points.end();){

        cilantro::Neighborhood<float> num=tree.kNNSearch(*i,5);

        float mean,sum=0;

        for (int j = 0; j < num.size(); ++j) {
            sum+=num[j].value;
        }
        mean=sum/num.size();

        if(mean>5){
            points.erase(i);
        } else{
            ++i;
        }
    }


    //打印出剩下的点
    std::cout<<"points size()"<<points.size()<< std::endl;
    for (int k = 0; k < points.size(); ++k) {
        std::cout<<points[k][0]<<"  "<<points[k][1]<<"  "<<points[k][2]<<std::endl;
    }
return 0;
}