计算.xyz点云质心

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
//#include<opencv2/opencv.hpp>
#include<opencv.hpp>
#include <pcl/common/transforms.h>
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <fstream>
#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/features/normal_3d_omp.h>//使用OMP需要添加的头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

using namespace cv;
using namespace std;

typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

// Mutex: //进程锁
boost::mutex cloud_mutex;

//用于给回调函数的结构体定义
// structure used to pass arguments to the callback function
struct callback_args
{
	PointCloudT::Ptr clicked_points_3d;
	pcl::visualization::PCLVisualizer::Ptr viewerPtr;
};

//分割字符串的方法
void split(std::string& string_input, std::vector<std::string>& string_output, std::string& delema1)
{
	std::string::size_type start = string_input.find_first_not_of(delema1, 0);//找到第一个不为delema1的下标
	std::string::size_type pose = string_input.find_first_of(delema1, start);//找到第一个delema1的下标
	while (std::string::npos != start || std::string::npos != pose) {//当即没有delema1也没有字符的时候结束
		string_output.push_back(string_input.substr(start, pose - start));
		start = string_input.find_first_not_of(delema1, pose);//更新start 从pose开始
		pose = string_input.find_first_of(delema1, start);//更新pose,从start开始再次寻找
	}
}

bool get_parameter_xyz(std::string path, pcl::PointCloud<pcl::PointXYZ>& cloud)
{
	cloud.clear();
	std::ifstream inf;
	bool flag = false;
	try
	{
		inf.open(path);
	}
	catch (const std::exception&)
	{

		return false;
	}
	std::string sline;//每一行
	std::vector<std::string>string_output;
	std::string Delema = " ";
	pcl::PointXYZ point3d;
	while (getline(inf, sline))
	{
		int i = 0;
		split(sline, string_output, Delema);
		point3d.x = stold(string_output[i]);
		point3d.y = stold(string_output[i + 1]);
		point3d.z = stold(string_output[i + 2]);
		string_output.clear();
		cloud.push_back(point3d);
	}
	inf.close();
}

//回调函数
void pp_callback(const pcl::visualization::PointPickingEvent& event, void* args)
{
	struct callback_args* data = (struct callback_args*)args;
	if (event.getPointIndex() == -1)
		return;
	PointT current_point;
	event.getPoint(current_point.x, current_point.y, current_point.z);

	//TODO
	data->clicked_points_3d->clear();//将上次选的点清空

	data->clicked_points_3d->points.push_back(current_point);//添加新选择的点
	// Draw clicked points in red:将选中点用红色标记
	pcl::visualization::PointCloudColorHandlerCustom<PointT> red(data->clicked_points_3d, 255, 0, 0);
	data->viewerPtr->removePointCloud("clicked_points");
	data->viewerPtr->addPointCloud(data->clicked_points_3d, red, "clicked_points");
	data->viewerPtr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "clicked_points");
	std::cout << current_point.x << " " << current_point.y << " " << current_point.z << std::endl;

}

void main()
{
	/*std::string filename("rabbit.pcd");*/
	std::string filename("points.xyz");
	
	//visualizer,定义可视化窗口
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("viewer"));

	//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	string path = "00003.ply";
	//string path = "D:\\Projects\\CPP\\ConsoleApplication4_ok\\x64\Debug\\points.xyz";

	get_parameter_xyz(path, *cloud);
	std::cout << "Loaded "
		<< cloud->width * cloud->height
		<< " data points from test_pcd.pcd with the following fields: "
		<< std::endl;

	pcl::PassThrough<pcl::PointXYZ> pass;
	pass.setInputCloud(cloud);
	pass.setFilterFieldName("z");
	pass.setFilterLimits(1000.0, 1140.0);
	pass.filter(*cloud_filtered);

	Eigen::Vector4f cloudCentroid;
	pcl::compute3DCentroid(*cloud_filtered, cloudCentroid);
	std::cout << cloudCentroid << std::endl;