三维点云学习（Ⅰ）- C++实操（PCA降维，升维、模型点云法向量）

三维点云学习（Ⅰ）- C++实操

一、VS2013配置PCl1.80库

参考链接

二、本过程采用数据集为modelnet40

为40种物体的三维点云数据集
链接：https://pan.baidu.com/s/1LX9xeiXJ0t-Fne8BCGSjlQ
提取码：es14

三、PCA降维、模型点云法向量

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

//功能介绍：读入txt、asc->pcl::PointXYZ格式(数据集中只读取前三列)
void CreateCloudFromTxt(const std::string& file_path, pcl::PointCloud::Ptr cloud)
{
	std::ifstream file(file_path.c_str());//c_str()：生成一个const char*指针，指向以空字符终止的数组。
	std::string line;
	pcl::PointXYZ point;

	while (getline(file, line))           //用到x，y,z
	{
		std::stringstream ss(line);
		ss >> point.x;
		ss >> point.y;
		ss >> point.z;
		cloud->push_back(point);
	}
	file.close();
}

//功能：点云转换为矩阵
void PointConversionEigen(pcl::PointCloud::Ptr cloud, Eigen::MatrixXd &cloudMat)
{
	cloudMat.resize(cloud->points.size(), 3);    //定义点云行，3列
	for (int itr = 0; itr < cloud->points.size(); itr++)
	{
		cloudMat(itr, 0) = cloud->points[itr].x;
		cloudMat(itr, 1) = cloud->points[itr].y;
		cloudMat(itr, 2) = cloud->points[itr].z;
	}
}

//功能：实现PCA功能
void PCAFunctions(pcl::PointCloud::Ptr cloud, pcl::Normal &pointNormal, pcl::Normal &pointNormal2)
{
	Eigen::MatrixXd cloudMat;
	cloudMat.resize(cloud->points.size(), 3);    //定义点云行，3列
	for (int itr = 0; itr < cloud->points.size(); itr++)
	{
		cloudMat(itr, 0) = cloud->points[itr].x;
		cloudMat(itr, 1) = cloud->points[itr].y;
		cloudMat(itr, 2) = cloud->points[itr].z;
	}

	Eigen::RowVector3d meanMat = cloudMat.colwise().mean();
	cloudMat.rowwise() -= meanMat;

	//求协方差矩阵
	Eigen::MatrixXd covMat;
	if (cloudMat.rows() == 1)
		covMat = (cloudMat.adjoint()*cloudMat) / double(cloudMat.rows());
	else
		covMat = (cloudMat.adjoint()*cloudMat) / double(cloudMat.rows() - 1);

	Eigen::JacobiSVD svd(covMat, Eigen::ComputeFullU | Eigen::ComputeFullV);
	Eigen::Matrix3d V = svd.matrixV();
	Eigen::Matrix3d U = svd.matrixU();
	Eigen::Matrix3d S = U.inverse() * covMat * V.transpose().inverse();

	pointNormal2.normal_x = V(3);
	pointNormal2.normal_y = V(4);
	pointNormal2.normal_z = V(5);

	pointNormal.normal_x = V(6);
	pointNormal.normal_y = V(7);
	pointNormal.normal_z = V(8);


}

//功能：输入点云计算法相
void PointCloudNormal(pcl::PointCloud::Ptr cloud, pcl::PointCloud::Ptr &normals)
{
	//pcl::PointCloud::Ptr normalsExcess(new pcl::PointCloud);

	for (int iPt = 0; iPt < cloud->points.size(); iPt++)
	{
		pcl::KdTreeFLANN kdTree;
		kdTree.setInputCloud(cloud);
		pcl::PointCloud::Ptr searchKPoints(new pcl::PointCloud);  //搜寻结果
		int k = 10;
		std::vector kIndices(k);
		std::vector kSqrDistances(k);
		if (kdTree.nearestKSearch(cloud->points[iPt], k, kIndices, kSqrDistances) > 0)
		{
			pcl::PointXYZ kPoint;
			for (int i = 0; i < k; i++)
			{
				kPoint.x = cloud->points[kIndices[i]].x;
				kPoint.y = cloud->points[kIndices[i]].y;
				kPoint.z = cloud->points[kIndices[i]].z;
				searchKPoints->push_back(kPoint);
			}
		}
		// neighbors within radius search
		//float radius = 1.0;
		//std::vector radiusIndices;
		//std::vector radiusSqrDistance;
		//if (kdTree.radiusSearch(cloud->points[iPt], radius, radiusIndices, radiusSqrDistance) > 0)
		//{
		//		//do something
		//}

		pcl::Normal pointNormal, pointNormal2;
		PCAFunctions(searchKPoints, pointNormal, pointNormal2);
		normals->push_back(pointNormal);
	}

	//normals = normalsExcess;
}



int main()
{
	//加载点云模型
	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
	CreateCloudFromTxt("airplane_0008.asc", cloud);
	Eigen::MatrixXd cloudMat;
	PointConversionEigen(cloud, cloudMat);

	//计算所有点云的法向量
	pcl::PointCloud::Ptr normals(new pcl::PointCloud);
	PointCloudNormal(cloud, normals);

	//降维
	pcl::PointCloud::Ptr projectionCloud(new pcl::PointCloud);
	pcl::Normal pointCloudNormal, pointCloudNormal2;     //降维所用法向量
	//Eigen::RowVector3d projectionMat; //降维所用法向量
	Eigen::MatrixXd projectionPointMat;
	PCAFunctions(cloud, pointCloudNormal, pointCloudNormal2);
	//projectionMat << pointCloudNormal.normal_x, pointCloudNormal.normal_y, pointCloudNormal.normal_z;
	//Eigen::Matrix3d diagonalMat(projectionMat.asDiagonal()); //构造对角矩阵
	//projectionPointMat = (diagonalMat*cloudMat.transpose()).transpose();

	Eigen::Matrix3d projectionMat;
	projectionMat << pointCloudNormal.normal_x, pointCloudNormal.normal_y, pointCloudNormal.normal_z,
		             pointCloudNormal2.normal_x, pointCloudNormal2.normal_y, pointCloudNormal2.normal_z,
		             0, 0, 0;
	projectionPointMat = (projectionMat*cloudMat.transpose()).transpose();
	pcl::PointXYZ mPoint;
	for (int i = 0; i < projectionPointMat.rows(); i++)
	{
		mPoint.x = projectionPointMat(i, 0);
		mPoint.y = projectionPointMat(i, 1);
		mPoint.z = projectionPointMat(i, 2);
		projectionCloud->push_back(mPoint);
	}


	/*图形显示模块*/
	boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer("3D viewer"));
	//viewer->initCameraParameters();

	int v1(0), v2(1), v3(2), v4(3);
	viewer->createViewPort(0.0, 0.0, 0.5, 0.5, v1);
	viewer->createViewPort(0.5, 0.0, 1.0, 0.5, v2);
	viewer->createViewPort(0.0, 0.5, 0.5, 1.0, v3);
	viewer->createViewPort(0.5, 0.5, 1.0, 1.0, v4);

	//设置背景颜色
	viewer->setBackgroundColor(5, 55, 10, v1);
	//设置点云颜色
	pcl::visualization::PointCloudColorHandlerCustom single_color(cloud, 0, 225, 0);
	//添加坐标系
	viewer->addCoordinateSystem(0.5, v1);

	viewer->addPointCloud(projectionCloud, "sample cloud", v1);
	viewer->addPointCloud(cloud, "sample cloud0", v2);
	viewer->addPointCloudNormals(cloud, normals, 1, 0.01, "normals", v2);
	viewer->addPointCloud(cloud, single_color, "sample cloud1", v3);
	viewer->addPointCloud(projectionCloud, "sample cloud3", v4);

	//设置点云大小
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud3", 4);


	//添加点云法向量的另一种方式；
	//解决来源http://www.pcl-users.org/How-to-add-PointNormal-on-PCLVisualizer-td4037041.html
	//pcl::PointCloud::Ptr cloud_with_normals(new pcl::PointCloud);
	//pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
	//viewer->addPointCloudNormals(cloud_with_normals, 50, 0.01, "normals");
	//
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	return 0;
}