MLS upsample in PCL

mls upsample.cpp

#include <iostream>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/ModelCoefficients.h>
#include <cmath>
#include <fstream>
#include <vector>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/surface/mls.h>
#include <pcl/surface/impl/mls.hpp>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/impl/sac_segmentation.hpp>
#include <algorithm>

typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
typedef pcl::PointXYZI XYZI;


/**
 * mls上采样
 * @param input 输入点云
 * @param radius 每个点云上采样时扩张的半径 单位是m
 * @param step_size 在radius中增加的点的距离 单位是m
 * @param search_radius kdtree的搜索半径
 * @return 上采样之后的点云
 */
PointCloudPtr
mls_upsample(PointCloudPtr &input, float radius = 0.09, float step_size = 0.03, float search_radius = 0.3) {
    // resample
    pcl::search::KdTree<XYZI>::Ptr tree(new pcl::search::KdTree<XYZI>);

    PointCloudPtr mls_points(new PointCloud);

    pcl::MovingLeastSquares<XYZI, XYZI> mls;

    mls.setComputeNormals(true);
    mls.setInputCloud(input);
    mls.setPolynomialOrder(3);
    mls.setSearchMethod(tree);
    mls.setSearchRadius(search_radius);
    mls.setUpsamplingMethod(pcl::MovingLeastSquares<XYZI, XYZI>::SAMPLE_LOCAL_PLANE);
    mls.setUpsamplingRadius(radius);
    mls.setUpsamplingStepSize(step_size);
    mls.process(*mls_points);
    return mls_points;
}