#include "pclFilter.h"

//点群を間引く
void pclFilter::voxelGridFilter(pcl::PointCloud<PointType>::Ptr cloud)
{
	//フィルター前の点群の数を表示する
	//pcl::console::print_info("before point clouds : %d\n", cloud->size());

	//フィルターする範囲
	//kinect Fusionはkinectのカメラ座標系で記録されるのでメートル単位
	//0.01の場合は1cm単位でフィルターする
	float leaf = 0.01f;

	pcl::VoxelGrid<PointType> grid;

	//フィルターする範囲を設定
	grid.setLeafSize(leaf, leaf, leaf);

	//フィルターする点群を設定
	grid.setInputCloud(cloud);

	//フィルターする（新しい点群に保存する）
	pcl::PointCloud<PointType>::Ptr cloud_filtered(new pcl::PointCloud<PointType>);
	grid.filter(*cloud_filtered);

	//点群を戻す
	pcl::copyPointCloud(*cloud_filtered, *cloud);

	//フィルター後のタングン数を表示する
	//pcl::console::print_info("after point clouds ; %d\n", cloud->size());
}

//平面を検出する
void pclFilter::segmentation(pcl::PointCloud<PointType>::Ptr& cloud, pcl::ModelCoefficients::Ptr coefficients, pcl::PointIndices::Ptr inliers)
{

	//segmentationオブジェクトを作る
	pcl::SACSegmentation<PointType> seg;
	seg.setOptimizeCoefficients(true);
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setDistanceThreshold(0.05);

	seg.setInputCloud(cloud);
	seg.segment(*inliers, *coefficients);
}

//3D点群から指定した点群を切り出す
void pclFilter::extractIndicies(pcl::PointCloud<PointType>::Ptr cloud, pcl::PointIndices::Ptr inliers)
{
	pcl::PointCloud<PointType>::Ptr tmp(new pcl::PointCloud<PointType>);
	pcl::copyPointCloud(*cloud, *tmp);

	//フィルタリング
	pcl::ExtractIndices<pcl::PointXYZRGB> extract;
	extract.setInputCloud(tmp);
	extract.setIndices(inliers);

	//trueにすると平面を除去、falseにすると平面以外を除去
	extract.setNegative(false);
	extract.filter(*cloud);
}