标签：cloudMat viewer C++ 实操 pcl 点云 PointCloud itr cloud

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

一、VS2013配置PCl1.80库

参考链接

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

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

三、PCA降维

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Dense>

//功能介绍：读入txt、asc->pcl::PointXYZ格式(数据集中只读取前三列)
void CreateCloudFromTxt(const std::string& file_path, pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZ>::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<pcl::PointXYZ>::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<Eigen::MatrixXd> 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<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr &normals)
{
	//pcl::PointCloud<pcl::Normal>::Ptr normalsExcess(new pcl::PointCloud<pcl::Normal>);

	for (int iPt = 0; iPt < cloud->points.size(); iPt++)
	{
		pcl::KdTreeFLANN<pcl::PointXYZ> kdTree;
		kdTree.setInputCloud(cloud);
		pcl::PointCloud<pcl::PointXYZ>::Ptr searchKPoints(new pcl::PointCloud<pcl::PointXYZ>);  //搜寻结果
		int k = 10;
		std::vector<int> kIndices(k);
		std::vector<float> 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<int> radiusIndices;
		//std::vector<float> 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<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	CreateCloudFromTxt("airplane_0008.asc", cloud);
	Eigen::MatrixXd cloudMat;
	PointConversionEigen(cloud, cloudMat);

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

	//降维
	pcl::PointCloud<pcl::PointXYZ>::Ptr projectionCloud(new pcl::PointCloud<pcl::PointXYZ>);
	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.size(); i++)
	{
		mPoint.x = projectionPointMat(i, 0);
		mPoint.y = projectionPointMat(i, 1);
		mPoint.z = projectionPointMat(i, 2);
		projectionCloud->push_back(mPoint);
	}


	/*图形显示模块*/
	boost::shared_ptr<pcl::visualization::PCLVisualizer> 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<pcl::PointXYZ> single_color(cloud, 0, 225, 0);
	//添加坐标系
	viewer->addCoordinateSystem(0.5, v1);

	viewer->addPointCloud<pcl::PointXYZ>(projectionCloud, "sample cloud", v1);
	viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud0", v2);
	viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, normals, 1, 0.01, "normals", v2);
	viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample cloud1", v3);
	viewer->addPointCloud<pcl::PointXYZ>(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<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
	//pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
	//viewer->addPointCloudNormals<pcl::PointNormal>(cloud_with_normals, 50, 0.01, "normals");
	//
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	return 0;
}

标签：cloudMat,viewer,C++,实操,pcl,点云,PointCloud,itr,cloud
来源： https://blog.csdn.net/qq_41188371/article/details/113739202

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。