【车路协同】路端局部坐标

最新推荐文章于 2021-11-24 19:30:24 发布

钱敏大好人

最新推荐文章于 2021-11-24 19:30:24 发布

阅读量198

点赞数

分类专栏： ros 文章标签：算法

本文链接：https://blog.csdn.net/qianmin12345678/article/details/116597093

版权

ros 专栏收录该内容

22 篇文章 1 订阅

订阅专栏

由于路端设备是倾斜的，存在角度问题，需要变换

1倾角计算

流程

获得一帧点云
剪裁点云，只剩下地面
RANSAC平面拟合
计算平面与水平面夹角

1获得一帧点云，保存为pcd

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/io.h>
#include <time.h>

 
int flag=1;
 
void cloudCallback(const sensor_msgs::PointCloud2 ros_cloud)
{
    //ROS_INFO("cloud is going");
    pcl::PointCloud<pcl::PointXYZ> pcl_cloud_temp;
    pcl::fromROSMsg(ros_cloud, pcl_cloud_temp);
    if(flag==1)
    {
        pcl::io::savePCDFileASCII ("./road_pcd.pcd", pcl_cloud_temp); //将点云保存到PCD文件中
        flag=0;
        std::cout<<"saved one"<<endl;
    }
}
int main(int argc, char **argv)
{ 
    ros::init(argc, argv, "pcd_get");
    ros::NodeHandle n;
    ROS_INFO("begin");
    ros::Subscriber cloud_sub = n.subscribe("/converted_velodyne_points", 50, cloudCallback);
    ros::spin();
    return 0;
}

2剪裁点云，提取地面

#include <iostream>
#include <vector>

#include <boost/shared_ptr.hpp>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>

#include<pcl/filters/passthrough.h>  //直通滤波器头文件
#include<pcl/filters/voxel_grid.h>  //体素滤波器头文件
#include<pcl/filters/statistical_outlier_removal.h>   //统计滤波器头文件
#include <pcl/filters/conditional_removal.h>    //条件滤波器头文件
#include <pcl/filters/radius_outlier_removal.h>   //半径滤波器头文件

using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
using namespace std;
int main(int argc, char *argv[])
{
    std::cout<<"cnm"<<endl;
	//读取
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("/home/qm/my_projects/pcl_projects/pcd/road_pcd.pcd", *cloud) == -1)
	{
		std::cerr << "open failed!" << std::endl;
		return -1;
	}




	//
	//直通滤波器对点云 x方向进行处理。*/
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_x_PassThrough(new pcl::PointCloud<pcl::PointXYZ>);//

	pcl::PassThrough<pcl::PointXYZ> passthrough_x;
	passthrough_x.setInputCloud(cloud);//输入点云

	passthrough_x.setFilterFieldName("x");//对z轴进行操作
	passthrough_x.setFilterLimits(-5.0, 5.0);//设置直通滤波器操作范围
	// passthrough.setFilterLimitsNegative(true);//true表示保留范围内，false表示保留范围外
	passthrough_x.filter(*cloud_x_PassThrough);//执行滤波，过滤结果保存在 cloud_after_PassThrough



	//直通滤波器对点云 y方向进行处理。*/
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_y_PassThrough(new pcl::PointCloud<pcl::PointXYZ>);//

	pcl::PassThrough<pcl::PointXYZ> passthrough_y;
	passthrough_y.setInputCloud(cloud_x_PassThrough);//输入点云

	passthrough_y.setFilterFieldName("y");//对z轴进行操作
	passthrough_y.setFilterLimits(-100, 0.0);//设置直通滤波器操作范围
	// passthrough.setFilterLimitsNegative(true);//true表示保留范围内，false表示保留范围外
	passthrough_y.filter(*cloud_y_PassThrough);//执行滤波，过滤结果保存在 cloud_after_PassThrough

	//点保存点云
	pcl::io::savePCDFileASCII("./road_ground.pcd", *cloud_y_PassThrough);//将点云保存到PCD文件中
	std::cerr << "Saved" << cloud_y_PassThrough->points.size() << "data points to test_pcd.pcd" << std::endl;








	//显示
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_y_PassThrough, "sample cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
	viewer->addCoordinateSystem(1.0);
	viewer->initCameraParameters();

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

	return 0;
}

3 RANSAC 点云平面拟合

注意看看提取的点云是不是真的一个平面
调整ransac.setDistanceThreshold(0.1);//设定阈值默认0.3
越小越精细

#include <iostream>
#include <pcl/console/parse.h>
#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 <Eigen/Core>

using namespace std;

int main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);
	
	pcl::io::loadPCDFile<pcl::PointXYZ>("/home/qm/my_projects/pcl_projects/pcd/road_ground.pcd", *cloud);
	std::cout<<"origin points num:"<<cloud->points.size()<<endl;

	vector<int> inliers;	//用于存放合群点的vector

	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_plane(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_plane);//定义RANSAC算法模型

	ransac.setDistanceThreshold(0.1);//设定阈值 默认0.3
	ransac.computeModel();//拟合平面


	ransac.getInliers(inliers);//获取合群点
	double a, b, c, d, A, B, C, D;//a,b,c为拟合平面的单位法向量，A,B,C为重定向后的法向量
	Eigen::VectorXf coeff;
	ransac.getModelCoefficients(coeff);//获取拟合平面参数，对于平面ax+by+cz+d=0，coeff分别按顺序保存a,b,c,d
	cout << "coeff " << coeff[0] << " \t" << coeff[1] << "\t " << coeff[2] << "\t " << coeff[3] << endl;
	a = coeff[0], b = coeff[1], c = coeff[2], d = coeff[3];
	//---------------平面法向量定向，与（1，1，1）同向，并输出平面与原点的距离D----------------------
	if (a + b + c > 0) {
		A = a;
		B = b;
		C = c;
		D = abs(d);
	}
	else {
		A = -a;
		B = -b;
		C = -c;
		D = abs(d);
	}
	cout<<" ABCD:"<<endl;
	cout << "" << A << ",\t" << "" << B << ",\t" << "" << C << ",\t" << "" << D << ",\t" << endl;
	pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);


	std::cout<<"final points num:"<<final->points.size()<<endl;
	// pcl::io::savePCDFileASCII("1.11.pcd", *final);



	//同时显示
	// boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("cloud show"));
	// int v1 = 0;
	// int v2 = 1;
	// viewer->createViewPort(0, 0, 0.5, 1, v1);
	// viewer->createViewPort(0.5, 0, 1, 1, v2);
	// viewer->setBackgroundColor(0, 0, 0, v1);
	// viewer->setBackgroundColor(0.05, 0, 0, v2);
	// //----------------------------------原始点云绿色------------------------------
	// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(cloud, 0, 255, 0);
	// //--------------------------------平面拟合后的点云----------------------------
	// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> after_sac(final, 0, 0, 255);
	// viewer->setBackgroundColor(0, 0, 0);
	// viewer->addPointCloud(cloud, src_h, "source cloud", v1);
	// viewer->addPointCloud(final, after_sac, "target cloud1", v2);

	// while (!viewer->wasStopped())
	// {
	// 	viewer->spinOnce(100);
	// 	boost::this_thread::sleep(boost::posix_time::microseconds(10000));
	// }


	//显示单个
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addPointCloud<pcl::PointXYZ>(final, "sample cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
	viewer->addCoordinateSystem(1.0);
	viewer->initCameraParameters();

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

	return 0;
}

4 计算点云平面与地面夹角

Ax+By+Cz+D=0是一般方程
法向量就是（A,B,C）

#include<iostream>
#include <cmath>
using namespace std;
int main (int argc, char *argv[])
{
    // z=0
    double x1=0;
    double y1=0;
    double z1=1;


    // -0.0264245*x+0.382563*y+0.923552*z+3.34908=0 第一次
    //-0.0206086,	0.392848,	0.919372,	3.47223,	第二次
    double  x2=-0.0206086;
    double  y2=0.392848;
    double  z2=0.919372;

    // double  x2=1;
    // double  y2=0;
    // double  z2=0;

    double dz = z2 - z1;
    double dy = y2 - y1;
    double dx = x2 - x1;
    // cmath
    // arccos acos
    // 开根号 sqrt
    double t1=x1*x2+y1*y2+z1*z2;
    double l1=sqrt(x1*x1+y1*y1+z1*z1);
    double l2=sqrt(x2*x2+y2*y2+z2*z2);

    double cos_t=t1/(l1*l2);
    double angle_hd=acos(cos_t);
    double angle_jd=angle_hd * 180 / 3.1415926;
    // double angle = atan2( abs(dz), sqrt(dx * dx + dy * dy) );
    cout<<"angle"<<angle_jd<<endl;
    return 0;
}