cilantro(实现PCL的统计滤波)

代码如下:

#include <cilantro/kd_tree.hpp>
#include <cilantro/io.hpp>
#include <iostream>
#include <cilantro/visualizer.hpp>
#include <cilantro/point_cloud.hpp>
#include <cilantro/common_renderables.hpp>

int main(int argc, char ** argv) {
    if (argc < 2) {
        std::cout << "Please provide path to PLY file." << std::endl;
        return 0;
    }
    cilantro::PointCloud3f cloud(argv[1]);
    //创建窗口
    cilantro::Visualizer viz1("Visualizer (window 1)","disp");
    std::vector<float> scalars(cloud.size());
    for (size_t i = 0; i < cloud.size(); i++) {
        scalars[i] = cloud.points.col(i).norm();
    }
    viz1.addObject<cilantro::PointCloudRenderable>("pcd", cloud, cilantro::RenderingProperties().setColormapType(cilantro::ColormapType::JET).setLineDensityFraction(0.2f).setUseLighting(false))
            ->setPointValues(scalars);
    viz1.addObject<cilantro::CoordinateFrameRenderable>("axis", Eigen::Matrix4f::Identity(), 0.4f, cilantro::RenderingProperties().setLineWidth(5.0f));
    viz1.addObject<cilantro::TextRenderable>("text", "Coordinate Frame", 0, 0, 0, cilantro::RenderingProperties().setFontSize(20.0f).setPointColor(1.0f,1.0f,0.0f).setTextAnchorPoint(0.5f,-1.0f));

    cilantro::KDTree3f tree(cloud.points);

    //找到每个点的最近5个点,这里的主要是用平均距离来筛选的，实际上还有其他的参数，后面关于滤波可以一写一个类

    std::vector<size_t> ind;
    for (size_t i = 0; i < cloud.size(); i++) {
        cilantro::Neighborhood<float> kd_num=tree.kNNSearch(cloud.points.col(i),100);
        float mean,sum=0;
//        std::cout<<kd_num.size()<<std::endl;
        for (int j = 0; j < kd_num.size(); ++j) {
            sum+=kd_num[j].value;
        }
        mean=sum/kd_num.size();

        if(mean<=0.001) {
//            std::cout<<i<<std::endl;
            ind.push_back(i);
//            cloud.points.col(i)==Eigen::Vector3f(10.0, 10.0, 10.0);
        }

    }
    cilantro::PointCloud3f pc(cloud, ind);


    cilantro::Visualizer View("Visualizer (window 2)","disp");
    std::vector<float> scalars2(pc.size());
    for (size_t i = 0; i < pc.size(); i++) {
        scalars[i] = pc.points.col(i).norm();
    }
    View.addObject<cilantro::PointCloudRenderable>("pcd2", pc, cilantro::RenderingProperties().setColormapType(cilantro::ColormapType::JET).setLineDensityFraction(0.2f).setUseLighting(false))
            ->setPointValues(scalars2);
    View.addObject<cilantro::CoordinateFrameRenderable>("axis2", Eigen::Matrix4f::Identity(), 0.4f, cilantro::RenderingProperties().setLineWidth(5.0f));
    View.addObject<cilantro::TextRenderable>("text2", "Coordinate Frame2", 0, 0, 0, cilantro::RenderingProperties().setFontSize(20.0f).setPointColor(1.0f,1.0f,0.0f).setTextAnchorPoint(0.5f,-1.0f));

    while (!viz1.wasStopped() && !View.wasStopped()) {
        viz1.spinOnce();
        View.spinOnce();
    }
    return 0;
}

CMakeLists.txt

CMAKE_MINIMUM_REQUIRED(VERSION 3.5)
PROJECT(test)
FIND_PACKAGE(Eigen3 REQUIRED)
FIND_PACKAGE(cilantro REQUIRED)
find_package(Pangolin REQUIRED)
INCLUDE_DIRECTORIES(${Pangolin_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS} ${cilantro_INCLUDE_DIRS})
ADD_EXECUTABLE(kd kd_test.cpp)
TARGET_LINK_LIBRARIES(kd ${Pangolin_LIBRARIES} ${cilantro_LIBRARIES} ${EIGEN_LIBRARIES})

效果图:

可以看到有滤波的效果，呵呵