cv坐标转pcl点云坐标并显示

void PrintToPcl(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud3D, pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPoint) {					//打印单张图
	pcl::visualization::PCLVisualizer::Ptr viewer3d(new pcl::visualization::PCLVisualizer("拟合结果3d"));
	viewer3d->addPointCloud<pcl::PointXYZ>(cloud3D, "cloud");													//添加原始点云
	viewer3d->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "cloud");	//颜色
	viewer3d->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");	//点的大小

	viewer3d->addPointCloud<pcl::PointXYZ>(cloudPoint, "circle");													//添加原始点云
	viewer3d->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 1, "circle");	//颜色
	viewer3d->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "circle");	//点的大小


	while (!viewer3d->wasStopped())
	{
		viewer3d->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}

上文说明
vector<cv::Point3f> VisionToRbt_Lst_;
....上面把三维坐标装进了VisionToRbt_Lst_
::Ptr是pcl::PointCloud<pcl::PointXYZ>的智能指针的别名
俩个::Ptr分别装的是红色的轨迹，蓝色的点坐标
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud3D(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPoint(new pcl::PointCloud<pcl::PointXYZ>);
	for (int i = 0; i < VisionToRbt_Lst_.size(); ++i) {
		cloud3D->push_back(pcl::PointXYZ(VisionToRbt_Lst_.at(i).x, VisionToRbt_Lst_.at(i).y, VisionToRbt_Lst_.at(i).z));

	}
	cloudPoint->push_back(pcl::PointXYZ(theHighest_point.x, theHighest_point.y, theHighest_point.z));
	cloudPoint->push_back(pcl::PointXYZ(theHighest_point.x, theHighest_point.y, theHighest_point.z - radius_Main));
	PrintToPcl(cloud3D, cloudPoint);				//调用了输出函数

效果如图：