kinect v2：从数据采集到点云网格化

1.深度图+RGBA图采集

kinectv2.h

#pragma once

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 


using namespace cv;
using namespace std;

class Kinectv2 
{

private:
	IKinectSensor* kinect_sensor = nullptr;

	IDepthFrameSource* depth_source = nullptr;
	IDepthFrameReader* depth_reader = nullptr;
	IDepthFrame* depth_frame = nullptr;

	IColorFrameSource* color_source = nullptr;
	IColorFrameReader* color_reader = nullptr;
	IColorFrame* color_frame = nullptr;

	static const int depth_width = 512;
	static const int depth_height = 424;
	static const int color_width = 1920;
	static const int color_height = 1080;
	//const mask
	static const int mask_height = 538, mask_width = 557;
	const Rect mask_depth = Rect(0, 33, mask_width, mask_height), mask_rgba = Rect(145, 0, mask_width, mask_height);

	string save_path = "kinect_data";
	int fps = 4;

public:
	//窗口状态：显示、保存图片、退出
	enum window_event { show, save, quit }event;
	Kinectv2(int fps_ = 4);
	~Kinectv2();
	void rgbaMapDepth();
	void shotPhoto();
	void exitCamera();

private:
	//作为子线程采集图像
	void windowEvent();
};


int main()
{
	Kinectv2 kinect(4);
	kinect.rgbaMapDepth();

}

kinectv2.cpp

#include 

Kinectv2::Kinectv2(int fps_)
{
    fps = fps_;
    //如果文件夹不存在
    if (_access(save_path.c_str(), 0) == -1)
        _mkdir(save_path.c_str());
    //system(("mkdir " + save_path).c_str());
}


Kinectv2::~Kinectv2()
{

}


void Kinectv2::rgbaMapDepth()
{
    GetDefaultKinectSensor(&kinect_sensor);
    kinect_sensor->Open();

    kinect_sensor->get_DepthFrameSource(&depth_source);
    depth_source->OpenReader(&depth_reader);

    kinect_sensor->get_ColorFrameSource(&color_source);
    color_source->OpenReader(&color_reader);

    thread::thread(bind(&Kinectv2::windowEvent, this));

    int a;
    cout << "input 1 to save image,2 to quit" << endl;
    while (1)
    {
        cin >> a;
        if (a == 1) shotPhoto();
        if (a == 2) exitCamera();

    }

    depth_reader->Release();
    depth_reader = nullptr;
    depth_source->Release();
    depth_source = nullptr;

    color_reader->Release();
    color_reader = nullptr;
    color_source->Release();
    color_source = nullptr;

    if (kinect_sensor != nullptr)
    {
        kinect_sensor->Close();
        kinect_sensor->Release();
        kinect_sensor = nullptr;
    }
}

void Kinectv2::shotPhoto()
{
    event = save;
}

void Kinectv2::exitCamera()
{
    event = quit;
}

void Kinectv2::windowEvent()
{
    namedWindow("Depth", 0);
    namedWindow("Rgba", 0);
    INT64 relative_time = 0;

    Mat img_depth = Mat(Size(depth_width, depth_height), CV_16SC1, Scalar::all(0.0));
    Mat depth_final = Mat(Size(mask_width, mask_height), CV_16SC1, Scalar::all(0));
    Mat img_color = Mat(Size(color_width, color_height), CV_8UC4, Scalar::all(0));
    Mat color_final = Mat(Size(mask_width, mask_height), CV_8UC4, Scalar::all(0));

    //保存为CV_32FC1深度图，正常保存只有8位
    vector< int> compression_params;
    compression_params.push_back(IMWRITE_TIFF_COMPRESSION);
    compression_params.push_back(0); // 无压缩png.

    fstream depth_fout(save_path + "\\depth_images.txt", fstream::app);
    fstream rgba_fout(save_path + "\\rgba_images.txt", fstream::app);
    while (event != quit)
    {
        if (color_reader->AcquireLatestFrame(&color_frame) == S_OK
            && depth_reader->AcquireLatestFrame(&depth_frame) == S_OK)
        {
            switch (event)
            {
            case show:
                //将图像帧copy到存储图像数据区
                depth_frame->CopyFrameDataToArray(depth_width * depth_height, (UINT16*)img_depth.data);
                color_frame->CopyConvertedFrameDataToArray(4 * color_width * color_height, img_color.data, ColorImageFormat_Bgra);
                cv::imshow("Depth", img_depth);
                cv::imshow("Rgba", img_color);
                break;

            case save:
                cv::resize(img_depth, depth_final, Size(img_depth.cols * 1.437, img_depth.rows * 1.437));
                cv::resize(img_color, color_final, Size(img_color.cols / 2, img_color.rows / 2));
                color_final = color_final(mask_rgba);
                depth_final = depth_final(mask_depth);

                depth_frame->get_RelativeTime(&relative_time);
                depth_final.convertTo(depth_final, CV_32FC1);
                cv::imwrite(save_path + "\\depth" + to_string(relative_time) + ".tiff", depth_final, compression_params);
                cv::imwrite(save_path + "\\RGBA" + to_string(relative_time) + ".bmp", color_final);
                depth_fout << save_path + "\\depth" + to_string(relative_time) + ".tiff" << endl;
                rgba_fout << save_path + "\\RGBA" + to_string(relative_time) + ".bmp" << endl;
                event = show;
                break;

            case quit:
                event = quit;
                break;

            default:
                break;
            }
            cv::waitKey(1000 / fps);
            depth_frame->Release();
            depth_frame = nullptr;
            color_frame->Release();
            color_frame = nullptr;
        }
    }

    depth_fout.close();
    rgba_fout.close();

    cv::destroyAllWindows();
}

2.相机标定、坐标变换得到点云

calibratecamera.h

#pragma once
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include 
#include 
#include 
#include 

#include 
#include 

using namespace std;
using namespace cv;
using namespace pcl;

class CalibrateCamera
{

public:
	//标定板上每行，列角点数
	Size board_size;
	//标定板上每个方块的尺寸，单位mm
	Size square_size;
	//图片尺寸
	Size img_size;

	Matx33d camera_matrix;
	Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));
	//vect can convert to matrix with Rodrigues()
	vector<Mat> R_vects, T_vects;
	Mat R, T;

private:
	vector<Point2f> getCornerFromImage(Mat img);
	//角点对应的世界坐标系点（根据标定板尺寸计算）
	vector<Point3f> makeWorldPoints();
	//坐标系变换:像素坐标系->相机坐标系 Zc*[u,v,1]-1=k(R[X,Y,Z]+T),R=E,T=0  [X,Y,Z]=k-1.[u,v,1].Zc
	vector<Point3f> mapPixelToCamera(Mat img_depth);
	//坐标系变换:像素坐标系->世界坐标系
	vector<Point3f> mapPixelToWorld(Mat img_depth);
	//保存相关参数
	void save_paras(string filename = "calib_paras.xml");
	//加载相关参数
	void load_paras(string filename = "calib_paras.xml");

public:
	CalibrateCamera(Size img = Size(557, 538), Size board = Size(8, 6), Size square = Size(20, 20));
	~CalibrateCamera();
	//use one image to calib
	void calib(Mat img);
	//use images to calib
	void calib(string file_path);
	//rgb图、depth图同时畸变矫正
	vector<Mat> rectifyImage(Mat rgb, Mat depth, bool show = true);
	Mat rectifyImage(Mat rgb,bool show = true);

	//坐标系变换，并转换为点云（可选择保存）
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr convert2cloud(Mat img_depth, Mat rgba, int min_z = 1, int max_z = 1500, bool save2pcd = false, string save_name = "");
	void showCornerInImage(Mat img);
	double calcLoss(Mat img);
	//查看depth的直方图
	void showDepthdistribute(Mat depth);

};


int main()
{
	string path1 = "kinect_data\\RGBA18019481096.bmp";
	string path2 = "kinect_data\\depth21168469678.tiff";

	Mat rgba = cv::imread(path1, 1);
	Mat depth = imread(path2, -1);


	CalibrateCamera calib(rgba.size(), Size(8, 6), Size(20, 20));

	calib.calib(rgba);
	cout << calib.camera_matrix << endl;
	//	cout << camera_matrix << endl << distCoeffs << endl << R_vects[0] << endl << T_vects[0] << endl;

	calib.showCornerInImage(rgba);

	Mat img_true = calib.rectifyImage(rgba, true);

	calib.showDepthdistribute(depth);

	String save_name = "test_cloud";
	calib.convert2cloud(depth, img_true, 300, 800, true, save_name);

}

calibratecamera.cpp

#include 


CalibrateCamera::CalibrateCamera(Size img, Size board, Size square)
{
	load_paras();
	board_size = board;
	square_size = square;
	img_size = img;
}


CalibrateCamera::~CalibrateCamera()
{
	save_paras();
}


void CalibrateCamera::calib(Mat img)
{
	vector<Point2f> cornerpoints = getCornerFromImage(img);
	//保存所有图像上检测到的所有角点
	vector<vector<Point2f>> cornerpoints_set;
	cornerpoints_set.push_back(cornerpoints);

	vector<Point3f> worldpoints = makeWorldPoints();
	vector<vector<Point3f>> worldpoints_set;
	worldpoints_set.push_back(worldpoints);

	//最后一个参数为标定时使用的算法
	cv::calibrateCamera(worldpoints_set, cornerpoints_set, img.size(), camera_matrix, distCoeffs, R_vects, T_vects, 0);
}


void CalibrateCamera::calib(string file_path)
{
	string filename;
	Mat img_tmp;
	vector<Point2f> cornerpoints;
	vector<vector<Point2f>> cornerpoints_set;
	vector<Point3f> worldpoints = makeWorldPoints();
	vector<vector<Point3f>> worldpoints_set;

	ifstream fin(file_path);
	while (getline(fin, filename))
	{
		img_tmp = imread(filename);
		cornerpoints = getCornerFromImage(img_tmp);
		cornerpoints_set.push_back(cornerpoints);

		worldpoints_set.push_back(worldpoints);
	}
	fin.close();

	cv::calibrateCamera(worldpoints_set, cornerpoints_set, img_size, camera_matrix, distCoeffs, R_vects, T_vects, 0);
}

//Zc*[u,v,1]-1=k(R[X,Y,Z]+T),R=E,T=0  [X,Y,Z]=k-1.[u,v,1].Zc
vector<Point3f> CalibrateCamera::mapPixelToCamera(Mat img_depth)
{
	cout << "please assure calibrate camera before" << endl;
	vector<Point3f> camerapoints;

	//内参逆
	Matx33d K_1 = camera_matrix.inv();

	//中间矩阵
	Mat H = Mat(3, 1, CV_64FC1, Scalar::all(0.0));
	Mat result_point = Mat(3, 1, CV_64FC1, Scalar::all(0));
	double Zc = 0.0;
	for (int i = 0; i < img_depth.size().height; i++)
	{
		for (int j = 0; j < img_depth.size().width; j++)
		{

			Zc = img_depth.at<float>(i, j);
			H.at<double>(0, 0) = Zc * i;
			H.at<double>(1, 0) = Zc * j;
			H.at<double>(2, 0) = Zc;
			result_point = K_1 * H;

			camerapoints.push_back(Point3f(result_point));
		}
	}
	return camerapoints;

}

//R-1.C-1(Zc*[u,v,1]-1—CT)=[X,Y,Z]-1
vector<Point3f> CalibrateCamera::mapPixelToWorld(Mat img_depth)
{
	cout << "please assure calibrate camera before" << endl;
	vector<Point3f> worldpoints;
	cv::Rodrigues(R_vects[0], R);
	T = T_vects[0];
	//旋转逆.内参逆
	Mat RC_1 = R.inv() * (camera_matrix.inv());
	//内参.平移
	Mat CT = camera_matrix * T;
	//中间矩阵
	Mat H = Mat(3, 1, CV_64FC1, Scalar::all(0.0));
	Mat result_point = Mat(3, 1, CV_64FC1, Scalar::all(0));
	double Zc = 0.0;
	for (int i = 0; i < img_depth.size().height; i++)
	{
		for (int j = 0; j < img_depth.size().width; j++)
		{
			Zc = img_depth.at<float>(i, j);
			H.at<double>(0, 0) = Zc * i;
			H.at<double>(1, 0) = Zc * j;
			H.at<double>(2, 0) = Zc;
			result_point = RC_1 * (H - CT);

			worldpoints.push_back(Point3f(result_point));
		}
	}
	return worldpoints;
}

vector<Mat> CalibrateCamera::rectifyImage(Mat rgb, Mat depth, bool show)
{

	cout << "please assure calibrate camera before" << endl;
	Mat map_x(rgb.size(), CV_32F);
	Mat map_y(rgb.size(), CV_32F);

	Mat img_true, depth_true;
	//输出的X、y坐标重映射参数
	initUndistortRectifyMap(camera_matrix, distCoeffs, Mat(),
		getOptimalNewCameraMatrix(camera_matrix, distCoeffs, img_size, 1, img_size, 0),
		rgb.size(), CV_32FC1, map_x, map_y);

	cv::remap(rgb, img_true, map_x, map_y, INTER_LINEAR);
	cv::remap(depth, depth_true, map_x, map_y, INTER_LINEAR);


	if (show)
	{
		cv::imshow("rectify_img", img_true);
		cv::imshow("rectify_depth", depth_true);
		cv::waitKey(0);
	}

	vector<Mat> imganddepth;
	imganddepth.push_back(img_true);
	imganddepth.push_back(depth_true);

	return imganddepth;
}

Mat CalibrateCamera::rectifyImage(Mat rgb, bool show )
{
	cout << "please assure calibrate camera before" << endl;
	Mat map_x(rgb.size(), CV_32F);
	Mat map_y(rgb.size(), CV_32F);

	Mat img_true, depth_true;
	//输出的X、y坐标重映射参数
	initUndistortRectifyMap(camera_matrix, distCoeffs, Mat(),
		getOptimalNewCameraMatrix(camera_matrix, distCoeffs, img_size, 1, img_size, 0),
		rgb.size(), CV_32FC1, map_x, map_y);

	cv::remap(rgb, img_true, map_x, map_y, INTER_LINEAR);

	if (show)
	{
		cv::imshow("rectify_img", img_true);
		cv::waitKey(0);
	}

	return img_true;
}


pcl::PointCloud<pcl::PointXYZRGB>::Ptr CalibrateCamera::convert2cloud(Mat img_depth, Mat rgba, int min_z, int max_z, bool save2pcd, string save_name)
{
	//vector xyz= mapPixelToWorld(img_depth);
	vector<Point3f> xyz = mapPixelToCamera(img_depth);

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointXYZRGB point;

	int count = 0;
	for (int i = 0; i < rgba.rows; i++)
		for (int j = 0; j < rgba.cols; j++)
		{
			count = i * rgba.cols + j;
			if (xyz[count].z > min_z && xyz[count].z < max_z)
			{
				point.x = xyz[count].x;
				point.y = xyz[count].y;
				point.z = xyz[count].z;
				point.r = rgba.at<Vec4b>(i, j)[2];
				point.g = rgba.at<Vec4b>(i, j)[1];
				point.b = rgba.at<Vec4b>(i, j)[0];
				//cloud->points[count].a = rgba.at(i, j)[3];

				cloud->push_back(point);
			}
			//else
			//	cout << xyz[count].z << endl;
		}

	if (save2pcd)
		pcl::io::savePCDFileASCII(save_name + ".pcd", *cloud);

	//pcl::PointXYZRGB min;//用于存放三个轴的最小值
	//pcl::PointXYZRGB max;//用于存放三个轴的最大值
	//pcl::getMinMax3D(*cloud, min, max);

	//cout << "min.z = " << min.z << "\n" << endl;
	//cout << "max.z = " << max.z << "\n" << endl;

	cout << "cloud point :" << cloud->points.size() << endl;
	return cloud;
}


void CalibrateCamera::showCornerInImage(Mat img)
{
	vector<Point2f> cornerpoints = getCornerFromImage(img);
	if (img.channels() == 3)
	{
		cv::cvtColor(img, img, COLOR_RGB2GRAY);
	}
	drawChessboardCorners(img, board_size, cornerpoints, true);
	cv::imshow("current coeners", img);
	cv::waitKey(0);
}


double CalibrateCamera::calcLoss(Mat img)
{
	vector<Point2f> cornerpoints = getCornerFromImage(img);
	vector<Point3f> worldpoints = makeWorldPoints();
	calib(img);

	vector<Point2f> idealpoints;
	//使用标定得到的参数，将worldpoints投影到2d平面，计算与亚像素角点的偏差（L2损失）
	cv::projectPoints(worldpoints, R_vects[0], T_vects[0], camera_matrix, distCoeffs, idealpoints);

	double img_loss = 0.0;
	for (int j = 0; j < cornerpoints.size(); j++)
	{	//可用saturate_cast防止数据溢出
		img_loss += (double)pow(idealpoints[j].x - cornerpoints[j].x, 2)
			+ (double)pow(idealpoints[j].y - cornerpoints[j].y, 2);
	}
	img_loss = sqrt(img_loss) / (cornerpoints.size());

	return img_loss;
}


vector<Point2f> CalibrateCamera::getCornerFromImage(Mat img)
{
	//提取角点保存到corner_point_buf
	//缓存每幅图像上检测到的角点
	vector<Point2f> cornerpoints;
	cv::findChessboardCorners(img, board_size, cornerpoints);
	if (img.channels() == 3)
	{
		cv::cvtColor(img, img, COLOR_RGB2GRAY);
	}
	//获取角点的亚像素精度（两个像素间的空位继续划分）//Size(11,11)为搜索窗口的尺寸
	cv::find4QuadCornerSubpix(img, cornerpoints, Size(11, 11));
	return cornerpoints;
}


vector<Point3f> CalibrateCamera::makeWorldPoints()
{
	vector<Point3f> worldpoint_set;
	for (int i = 0; i < board_size.height; i++)
	{
		for (int j = 0; j < board_size.width; j++)
		{
			Point3f point;
			point.x = i * square_size.width;
			point.y = j * square_size.height;
			point.z = 0;
			worldpoint_set.push_back(point);
		}
	}
	return worldpoint_set;
}


void CalibrateCamera::save_paras(string filename)  //数据为NULL时会出错
{
	FileStorage fs(filename, FileStorage::WRITE);
	fs << "img_size" << img_size;
	fs << "board_size" << board_size;
	fs << "square_size" << square_size;
	fs << "camera_matrix" << camera_matrix;
	fs << "distCoeffs" << distCoeffs;
	fs << "R_vects" << R_vects;
	fs << "T_vects" << T_vects;
	fs.release();
}

void CalibrateCamera::load_paras(string filename)
{
	FileStorage fn(filename, FileStorage::READ);
	fn["img_size"] >> img_size;
	fn["board_size"] >> board_size;
	fn["square_size"] >> square_size;
	fn["camera_matrix"] >> camera_matrix;
	fn["distCoeffs"] >> distCoeffs;
	fn["R_vects"] >> R_vects;
	fn["T_vects"] >> T_vects;
	fn.release();
}



void CalibrateCamera::showDepthdistribute(Mat depth)
{
	double minp = 0.0;
	double maxp = 0.0;

	minMaxIdx(depth, &minp, &maxp);
	cout << "max depth:" << minp << " min depth:" << endl << maxp << endl;
	/// 设定bin数目
	int histSize = int(maxp);

	/// 设定取值范围
	float range[] = { 0,int(maxp) };
	const float* histRange = { range };

	Mat hist;
	/// 计算直方图:
	calcHist(&depth, 1, 0, Mat(), hist, 1, &histSize, &histRange, true, false);

	// 创建直方图画布
	int hist_w = 1000; int hist_h = 800;
	int bin_w = cvRound((double)hist_w / histSize);

	Mat histImage(hist_w, hist_h, CV_8UC3, Scalar(0, 0, 0));

	//cout << hist << endl;
	/// 在直方图画布上画出直方图

	for (int i = 1; i < histSize; i++)
	{
		line(histImage, Point(bin_w * (i - 1), hist_h - cvRound((hist.at<float>(i - 1) / maxp) * hist_h)),
			Point(bin_w * (i), hist_h - cvRound((hist.at<float>(i) / maxp) * hist_h)),
			Scalar(0, 0, 255), 2, 8, 0);
	}
	cout << hist.at<float>(0) << endl;

	/// 显示直方图
	namedWindow("calcHist Demo", 0);
	imshow("calcHist Demo", histImage);
	waitKey(0);
}

3.pcl点云处理：下采样、过滤、粗配准、精配准

clouds.h

#pragma once

#include 
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"

#include 
#include 

#include 

#include 

#include 

#include 
#include 
#include 
#include 
#include  //特征的错误对应关系去除
#include  //随机采样一致性去除

#include 
#include 

#include 
using namespace std;
using namespace cv;

class Clouds
{
public:
	Clouds();
	~Clouds();
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr loadCloud(string file);
	void savePcd(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, string save_name = "");
	void showCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud);

	vector+rgbimage=pointxyzrgb
	//pcl::PointCloud::Ptr
	//	convert2Cloud(Mat rgba, vector xyz, bool save2pcd = false, string save_name = "");

	//downsample method
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr
		voxelDownsample(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, float leafsize = 0.1f);

	//filter method
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr
		statisticFilter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, int Meank = 50, double StddevMulThresh = 1.0);

	//Coarse registration
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr
		fpfhRegistration(pcl::PointCloud<pcl::PointXYZRGB>::Ptr source, pcl::PointCloud<pcl::PointXYZRGB>::Ptr target);

	//Fine registration
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr
		icpRegistration(pcl::PointCloud<pcl::PointXYZRGB>::Ptr src, pcl::PointCloud<pcl::PointXYZRGB>::Ptr dist);

	//regist pipline:from downsample to icp
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr
		registrationPipline(pcl::PointCloud<pcl::PointXYZRGB>::Ptr src, pcl::PointCloud<pcl::PointXYZRGB>::Ptr dist);



private:
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr
		compute_fpfh_feature(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree);
};


//#include "calibratecamera.h"
//
//int man()
//{
//	string path1 = "kinect_data\\RGBA18019481096.bmp";
//	string path2 = "kinect_data\\RGBA20101803232.bmp";
//	string path3 = "kinect_data\\depth20101803232.tiff";
//	string path4 = "kinect_data\\RGBA20049473438.bmp";
//	string path5 = "kinect_data\\depth20049473438.tiff";
//	//calib
//	Mat img = cv::imread(path1, 1);
//	Mat rgba_src = imread(path2, -1);
//	Mat depth_src = imread(path3, -1);
//	Mat rgba_dist = imread(path4, -1);
//	Mat depth_dist = imread(path5, -1);
//
//	CalibrateCamera calib(img.size(), Size(8, 6), Size(20, 20));
//	calib.calib(img);
//
//	vector Mats_src = calib.rectifyImage(rgba_src, depth_src, false);
//	vector Mats_tar = calib.rectifyImage(rgba_dist, depth_dist, false);
//
//	pcl::PointCloud::Ptr cloud_src(new pcl::PointCloud);
//	pcl::PointCloud::Ptr cloud_tar(new pcl::PointCloud);
//
//	calib.showDepthdistribute(depth_src);
//	calib.showDepthdistribute(depth_dist);
//
//	//cloud_src = calib.convert2cloud(Mats_src[1], Mats_src[0], 300, 800,false);
//	//cloud_tar = calib.convert2cloud(Mats_tar[1], Mats_tar[0], 300, 800, false);
//	cloud_src = calib.convert2cloud(depth_src, rgba_src, 300, 800, false);
//	cloud_tar = calib.convert2cloud(depth_dist, rgba_dist, 300, 800, false);
//
//	Clouds clouds;
//	//cout << cloud_src->points.size() << endl;
//
//	filter
//	//pcl::PointCloud::Ptr cloud_filtered1, cloud_filtered2, cloud_filtered3;
//	//cloud_filtered1 = clouds.voxelDownsample(cloud_src, 2);
//	//cloud_filtered2 = clouds.statisticFilter(cloud_filtered1);
//	//cloud_src = clouds.statisticFilter(cloud_filtered2);
//	clouds.showCloud(cloud_src);
//
//	//cloud_filtered1 = clouds.voxelDownsample(cloud_tar, 2);
//	//cloud_filtered2 = clouds.statisticFilter(cloud_filtered1);
//	//cloud_tar = clouds.statisticFilter(cloud_filtered2);
//	clouds.showCloud(cloud_tar);
//
//	//pcl::PointCloud::Ptr cloud_fpfh;
//	//cloud_fpfh = clouds.fpfhRegistration(cloud_src,cloud_tar);
//
//	//pcl::PointCloud::Ptr cloud_final;
//	//cloud_final = clouds.icpRegistration(cloud_fpfh,cloud_tar);
//
//	//pcl::PointCloud::Ptr cloud(new pcl::PointCloud());
//
//	combine t
//	//*cloud = *cloud_final + *cloud_tar;
//	pcl::PointCloud::Ptr cloud(new pcl::PointCloud());
//
//	cloud = clouds.registrationPipline(cloud_src, cloud_tar);
//
//	clouds.showCloud(cloud);
//
//	clouds.savePcd(cloud, "icp.pcd");
//
//	return 0;
//};

clouds.cpp

#pragma once
#include 


Clouds::Clouds()
{

}

Clouds::~Clouds()
{

}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr Clouds::loadCloud(string file)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::io::loadPCDFile<pcl::PointXYZRGB>(file, *cloud);

    return cloud;

}

void Clouds::savePcd(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, string save_name)
{
    pcl::io::savePCDFileASCII(save_name + ".pcd", *cloud);
    cout << "save pcd success" << endl;
}

void Clouds::showCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
    pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");

    viewer.showCloud(cloud);
    while (!viewer.wasStopped())
    {

    }

}


///
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Clouds::voxelDownsample(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, float leafsize)
{
    std::cout << "Cloud voxelDownsample filtering... " << std::endl;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>());

    pcl::VoxelGrid<pcl::PointXYZRGB> sor;
    sor.setInputCloud(cloud);
    sor.setLeafSize(leafsize, leafsize, leafsize);
    sor.filter(*cloud_filtered);

    std::cout << "Cloud after voxelDownsample filtering: " << cloud_filtered->points.size() << std::endl;

    return cloud_filtered;

}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr Clouds::statisticFilter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr
    cloud, int Meank, double StddevMulThresh)
{
    std::cout << "Cloud statistic filtering... " << std::endl;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>());
    // Create the filtering object
    pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
    sor.setInputCloud(cloud);
    sor.setMeanK(Meank);
    sor.setStddevMulThresh(StddevMulThresh);
    sor.filter(*cloud_filtered);

    //获取离群点云
    //sor.setNegative(true);
    //sor.filter(*cloud_filtered);

    std::cout << "Cloud after statistic filtering: " << cloud_filtered->points.size() << std::endl;
    return cloud_filtered;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr
Clouds::fpfhRegistration(pcl::PointCloud<pcl::PointXYZRGB>::Ptr source, pcl::PointCloud<pcl::PointXYZRGB>::Ptr target)
{

    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());

    pcl::PointCloud<pcl::FPFHSignature33>::Ptr source_fpfh = compute_fpfh_feature(source, tree);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr  target_fpfh = compute_fpfh_feature(target, tree);

    float leaf = 2;
    // Perform alignment
    pcl::console::print_highlight("Starting alignment...\n");
    //对齐(占用了大部分运行时间)
    pcl::SampleConsensusPrerejective<pcl::PointXYZRGB, pcl::PointXYZRGB, pcl::FPFHSignature33> align;
    align.setInputSource(source);
    align.setSourceFeatures(source_fpfh);
    align.setInputTarget(target);
    align.setTargetFeatures(target_fpfh);
    align.setMaximumIterations(50000); // Number of RANSAC iterations
    align.setNumberOfSamples(3); // Number of points to sample for generating/prerejecting a pose
    align.setCorrespondenceRandomness(5); // Number of nearest features to use
    align.setSimilarityThreshold(0.9f); // Polygonal edge length similarity threshold
    align.setMaxCorrespondenceDistance(2.5f * leaf); // Inlier threshold
    align.setInlierFraction(0.25f); // Required inlier fraction for accepting a pose hypothesis

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_aligned(new pcl::PointCloud<pcl::PointXYZRGB>());
    align.align(*object_aligned);

    cout << "fpfh refgist point: " << object_aligned->points.size() << endl;
    return object_aligned;

}


pcl::PointCloud<pcl::PointXYZRGB>::Ptr Clouds::icpRegistration(pcl::PointCloud<pcl::PointXYZRGB>::Ptr source, pcl::PointCloud<pcl::PointXYZRGB>::Ptr target)
{

    pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
    icp.setInputSource(source);
    icp.setInputTarget(target);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr final(new pcl::PointCloud<pcl::PointXYZRGB>());
    icp.align(*final);

    std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
    std::cout << icp.getFinalTransformation() << std::endl;
    cout << "icp refgist point: " << final->points.size() << endl;

    return final;
}



pcl::PointCloud<pcl::PointXYZRGB>::Ptr
Clouds::registrationPipline(pcl::PointCloud<pcl::PointXYZRGB>::Ptr source, pcl::PointCloud<pcl::PointXYZRGB>::Ptr target)
{
    Clouds clouds;
    cout << "source size:" << source->points.size() << endl;
    cout << "target size:" << target->points.size() << endl;
    //filter
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered;
    cloud_filtered = clouds.voxelDownsample(source, 2);
    cloud_filtered = clouds.statisticFilter(cloud_filtered);
    source = clouds.statisticFilter(cloud_filtered);
    //clouds.showCloud(cloud_src);

    cloud_filtered = clouds.voxelDownsample(target, 2);
    cloud_filtered = clouds.statisticFilter(cloud_filtered);
    target = clouds.statisticFilter(cloud_filtered);
    //clouds.showCloud(cloud_tar);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_current;
    source_current = clouds.fpfhRegistration(source, target);
    source_current = clouds.icpRegistration(source_current, target);

    //combine 
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
    *cloud = *source_current + *target;

    return cloud;
}


///private
pcl::PointCloud<pcl::FPFHSignature33>::Ptr Clouds::compute_fpfh_feature(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree)
{
    //法向量
    pcl::PointCloud<pcl::Normal>::Ptr point_normal(new pcl::PointCloud<pcl::Normal>());
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> est_normal;
    est_normal.setInputCloud(cloud);
    est_normal.setSearchMethod(tree);
    est_normal.setKSearch(10);
    est_normal.compute(*point_normal);

    //fpfh 估计
    pcl::FPFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33> est_fpfh;

    est_fpfh.setInputCloud(cloud);
    est_fpfh.setInputNormals(point_normal);
    est_fpfh.setSearchMethod(tree);
    est_fpfh.setKSearch(10);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_feature(new pcl::PointCloud<pcl::FPFHSignature33>());
    est_fpfh.compute(*fpfh_feature);

    return fpfh_feature;
}

4.网格化

polygonmesh.h

#pragma once

#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 

class PolyMesh
{
public:
	PolyMesh();
	~PolyMesh();

	pcl::PolygonMesh::Ptr loadVTK(std::string path);
	pcl::PolygonMesh::Ptr loadPLY(std::string path);
	void saveVTK(std::string path, pcl::PolygonMesh::Ptr triangles, bool binary = false);
	void savePLY(std::string path, pcl::PolygonMesh::Ptr triangles, bool binary = false);

	pcl::PolygonMesh::Ptr GreedyTriang(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double max_edge = 2);
	void showMesh(pcl::PolygonMesh::Ptr triangles);

private:


};

//
//
//int main(int argc, char** argv)
//{
//	 Load input file into a PointCloud with an appropriate type
//	//pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
//	//pcl::PCLPointCloud2 cloud_blob;
//	//pcl::io::loadPCDFile("icp.pcd", cloud_blob);
//	//pcl::fromPCLPointCloud2(cloud_blob, *cloud);
//	//cout << cloud->points.size();
//	* the data should be available in cloud
//
//
//	PolyMesh pmesh;
//	pcl::PolygonMesh::Ptr triangles(new pcl::PolygonMesh); //不new容易this为null异常
//	//triangles = pmesh.GreedyTriang(cloud, 3);
//	//pmesh.savePLY("a.ply", triangles);
//
//	pcl::PolygonMesh::Ptr triangles2(new pcl::PolygonMesh);
//	triangles2 = pmesh.loadPLY("a.ply");
//
//	std::cout << triangles2->polygons.size() << std::endl;
//	std::cout << triangles2->polygons.at(10000) << endl;
//	std::cout << triangles2->polygons.at(10000).vertices[0] << endl;
//
//	//pmesh.showMesh(triangles2);
//
//	// Finish
//	return (0);
//}

polygonmesh.cpp

#include 

PolyMesh::PolyMesh()
{

}
PolyMesh::~PolyMesh()
{

}

pcl::PolygonMesh::Ptr PolyMesh::loadVTK(std::string path)
{
	pcl::PolygonMesh::Ptr trangles(new pcl::PolygonMesh);
	pcl::io::loadPolygonFileVTK(path, *trangles);

	return trangles;

}
pcl::PolygonMesh::Ptr PolyMesh::loadPLY(std::string path)
{
	pcl::PolygonMesh::Ptr trangles(new pcl::PolygonMesh);
	pcl::io::loadPolygonFilePLY(path, *trangles);

	return trangles;
}
void PolyMesh::saveVTK(std::string path, pcl::PolygonMesh::Ptr triangles, bool binary)
{
	if (binary)
		pcl::io::savePolygonFileVTK(path, *triangles);
	else
		pcl::io::saveVTKFile(path, *triangles);
}
void PolyMesh::savePLY(std::string path, pcl::PolygonMesh::Ptr triangles, bool binary)
{
	if (binary)
		pcl::io::savePolygonFilePLY(path, *triangles);
	else
		pcl::io::savePLYFile(path, *triangles);

}

pcl::PolygonMesh::Ptr PolyMesh::GreedyTriang(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double max_edge)
{
	// Normal estimation*
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(cloud);
	n.setInputCloud(cloud);
	n.setSearchMethod(tree);
	n.setKSearch(20);
	n.compute(*normals);
	//* normals should not contain the point normals + surface curvatures

	// Concatenate the XYZ and normal fields*
	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
	pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
	//* cloud_with_normals = cloud + normals

	// Create search tree*
	pcl::search::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::search::KdTree<pcl::PointNormal>);
	tree2->setInputCloud(cloud_with_normals);

	// Initialize objects
	pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
	pcl::PolygonMesh::Ptr triangles(new pcl::PolygonMesh);

	// Set the maximum distance between connected points (maximum edge length)  0.025
	gp3.setSearchRadius(max_edge);

	// Set typical values for the parameters
	gp3.setMu(2.5);
	gp3.setMaximumNearestNeighbors(100);
	gp3.setMaximumSurfaceAngle(M_PI / 4); // 45 degrees
	gp3.setMinimumAngle(M_PI / 18); // 10 degrees
	gp3.setMaximumAngle(2 * M_PI / 3); // 120 degrees
	gp3.setNormalConsistency(false);

	// Get result
	gp3.setInputCloud(cloud_with_normals);
	gp3.setSearchMethod(tree2);
	gp3.reconstruct(*triangles);

	cout << "after greedy  triangles:" << triangles->polygons.size() << endl;
	// Additional vertex information
	std::vector<int> parts = gp3.getPartIDs();
	std::vector<int> states = gp3.getPointStates();

	return triangles;
}

void PolyMesh::showMesh(pcl::PolygonMesh::Ptr triangles)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);

	viewer->addPolygonMesh(*triangles, "triangles");

	//    viewer->addPolylineFromPolygonMesh(triangles);   //添加此行代码只显示三角化后的网格

	viewer->addCoordinateSystem(1.0);
	viewer->initCameraParameters();
	while (!viewer->wasStopped()) {
		viewer->spinOnce(100);
		//boost::this_thread::sleep(boost::posix_time::microseconds(100000));

	}

}