自动驾驶点云(作业2)

最新推荐文章于 2023-10-19 17:09:06 发布

人生体验

最新推荐文章于 2023-10-19 17:09:06 发布

阅读量573

点赞数 1

分类专栏：自动驾驶点云处理

本文链接：https://blog.csdn.net/weixin_43384504/article/details/105784828

版权

自动驾驶点云处理专栏收录该内容

5 篇文章 1 订阅

订阅专栏

实在没时间好好写博客，就先暂时记录以下做的过程。

1.首先是订阅激光雷达的每一帧点云，并将其转化为pcl点云数据，并显示


#include "ros/ros.h"
#include <iostream>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>

using namespace pcl;
boost::shared_ptr<visualization::CloudViewer> viewer;	
void laserCallback(const sensor_msgs::PointCloud2& input)
{
   pcl::PointCloud<pcl::PointXYZRGBA> cloud;
   pcl::fromROSMsg(input, cloud);//需要转化一下格式
   if(! viewer->wasStopped()) viewer->showCloud(cloud.makeShared());

   //转化点云为深度图




}

//这个是最基本的CloudViewer 显示
boost::shared_ptr<visualization::CloudViewer> createViewer()
{
    boost::shared_ptr<visualization::CloudViewer> v(new visualization::CloudViewer("OpenNI viewer"));
    return(v);
}

//显示点云和转化的深度图

int main(int argc, char **argv)
{

    ros::init(argc, argv, "rang_image");
    ros::NodeHandle rs_lidar;
   
    viewer = createViewer();
    ros::Subscriber pc = rs_lidar.subscribe("/rslidar_points", 1,laserCallback);
    ros::spin();

    return 0;
}

这个cmakelists要增加 pcl_ros.

参考博客:https://blog.csdn.net/yaked/article/details/52584231

2.转化为深度图(调用pcl 的api)


#include "ros/ros.h"
#include <iostream>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/png_io.h>
#include <pcl/visualization/common/float_image_utils.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/range_image/range_image.h> 
#include <pcl/visualization/pcl_visualizer.h> 
#include <pcl/visualization/range_image_visualizer.h>  
#include <pcl/console/parse.h> //deg2rad
using namespace pcl;


pcl::visualization::PCLVisualizer viewer ("3D Viewer");
pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
int i=1;

float angularResolution = (float)(0.5f * (M_PI / 180.0f));
float noise_level = 0.0;
float min_range = 0.0f;
int border_size = 1;

Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());  
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;


void laserCallback(const sensor_msgs::PointCloud2& input)
{
 
   pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
   pcl::PointCloud<pcl::PointXYZ>& cloud = *point_cloud_ptr;
   pcl::fromROSMsg(input, cloud);//需要转化一下格式


   cloud.width=(uint32_t)cloud.points.size();
   cloud.height = 1;

   //****************************显示原来的点云*********************************
   viewer.setBackgroundColor (1, 1, 1);
   viewer.addCoordinateSystem(1.0f);  

   pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> point_cloud_color_handler(point_cloud_ptr, 0, 0, 0);
   viewer.addPointCloud(point_cloud_ptr, point_cloud_color_handler, "original point cloud");
   
   
/*
   pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0); 
   viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
   viewer.initCameraParameters ();
*/




   //*************************转化点云为深度图********************************

   //boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
   //pcl::RangeImage& range_image = *range_image_ptr;
   pcl::RangeImage range_image;
   range_image.createFromPointCloud (cloud,angularResolution,pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
   
   std::cout << range_image << "\n";
   range_image_widget.showRangeImage (range_image);


  //保存其中一帧深度图片

if(i==1)
{
  float* ranges = range_image.getRangesArray();
  unsigned char* rgb_image = pcl::visualization::FloatImageUtils::getVisualImage(ranges,    range_image.width, range_image.height);
  pcl::io::saveRgbPNGFile("xly-1.png", rgb_image, range_image.width, range_image.height);

  i++;
}

}


//显示点云和转化的深度图

int main(int argc, char **argv)
{

    ros::init(argc, argv, "rang_image");
    ros::NodeHandle rs_lidar;
   

    ros::Subscriber pc = rs_lidar.subscribe("/rslidar_points", 1,laserCallback);

    ros::spin();

    while (!viewer.wasStopped ())
  {
    range_image_widget.spinOnce ();
    viewer.spinOnce ();
    pcl_sleep (0.01);
  }

    return 0;
}

有点小问题，先放着。

参考博客:https://blog.csdn.net/Suo_ivy/article/details/79866941

这种才采用的是球形投影的方式，其实我们一般是结合相机的内参，做平面投影重投影回深度图比较多。球形自己写不出来点云重投影到深度图的代码。

以上是第一题，下面是第二题,转化为BEV图:

看到网上有博客:https://blog.csdn.net/learning_tortosie/article/details/88828388

这个挺不错的，我用python 试了一下，可以就先放着，后面用c++写。

#include <iostream>
#include <pcl/io/pcd_io.h>  
#include <pcl/point_types.h>  
#include <pcl/filters/conditional_removal.h>

#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
using namespace std;
using namespace pcl;
using namespace cv;

double Scale_to_255(double value,double min,double max)
{
  double ans;
  ans=((value-min)/float(max-min)*255);
  return ans;
}




int main(int argc, char *argv[])
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

    // Fill in the cloud data  
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("inputCloud0.pcd", *cloud) == -1)
    {
        PCL_ERROR("Couldn't read file chuli.pcd\n");
        return (-1);
    }

    cout<<"      cloud    "<<cloud->points.size()<<endl;
   //去除nan点

    std::vector<int> mapping;
    pcl::removeNaNFromPointCloud(*cloud, *cloud, mapping);


    int Num = cloud->points.size();
    cout<<"     cloud 0 nan   "<<cloud->points.size()<<endl;
   //传感器分辨率、以及roi区域参数
    double res=0.05;
    double side_rang[2] ={-50.0,50.0};
    double fwd_range[2] ={-20.0,20.0};
    double height_range[2] ={-5.0,5.0};


   //roi 区域的选择 ，使用pcl的函数做旋转 

   //创建条件限定的下的滤波器
    pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionAnd<pcl::PointXYZ> ()); //创建条件定义对象
   //为条件定义对象添加比较算子
    range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
    pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, fwd_range[0])));   //添加在x字段上大于-20的比较算子
    range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
    pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, fwd_range[1])));   //添加在x字段上小于20的比较算子

    range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
    pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, side_rang[0])));   //添加在y字段上大于-50的比较算子
    range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
    pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, side_rang[1])));   //添加在y字段上小于50的比较算子

   // 创建滤波器并用条件定义对象初始化
    pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
    condrem.setCondition (range_cond);               
    condrem.setInputCloud (cloud);                   //输入点云
    condrem.setKeepOrganized(false);               //设置保持点云的结构
   // 执行滤波
    condrem.filter (*cloud_filtered);  //两个条件用于建立滤波
    cout<<"      cloud_filtered    "<<cloud_filtered->points.size()<<endl;


    double *X = new double[Num] {0};
    double *Y = new double[Num] {0};
    double *Z = new double[Num] {0};
    double *V = new double[Num] {0};

    int x_max = 1 + int((side_rang[1] - side_rang[0]) / res);
    int y_max = 1 + int((fwd_range[1] - fwd_range[0]) / res);

   // 这里要申请一个图像数据

   Mat image = Mat::zeros( y_max, x_max, CV_8UC1);




    for (size_t i = 0; i < cloud_filtered->points.size(); ++i)
    {
        X[i] = (-cloud->points[i].y/res);
        Y[i] = (-cloud->points[i].x/res);
        Z[i] = (-cloud->points[i].z);
	if( Z[i]>height_range[1])
	{
		V[i]=height_range[1];
	}
	else if(Z[i]<height_range[0])
	{
		V[i]=height_range[0];
	}
	else
	{
		V[i]=cloud->points[i].z;
	}

        X[i]-= int(side_rang[0]/res-1);
        Y[i]+= int(fwd_range[1]/res);   
	V[i]=Scale_to_255(V[i],height_range[0],height_range[1]);



        image.at<uchar>(Y[i], X[i])=V[i];
    }



	//namedWindow("bev_img", CV_WINDOW_AUTOSIZE);
	//imshow("bev_img", image);

	//waitKey(0);
    return 0;
}

大体框架如此，有点问题的，后面要修改!

#include <iostream>
#include <pcl/io/pcd_io.h>  
#include <pcl/point_types.h>  
#include <pcl/filters/conditional_removal.h>

#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
using namespace std;
using namespace pcl;
using namespace cv;

double Scale_to_255(double value,double min,double max)
{
  double ans;
  ans=((value-min)/float(max-min)*255);
  return ans;
}




int main(int argc, char *argv[])
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

    // Fill in the cloud data  
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("inputCloud0.pcd", *cloud) == -1)
    {
        PCL_ERROR("Couldn't read file chuli.pcd\n");
        return (-1);
    }

    cout<<"      cloud    "<<cloud->points.size()<<endl;
   //去除nan点

    std::vector<int> mapping;
    pcl::removeNaNFromPointCloud(*cloud, *cloud, mapping);


    int Num = cloud->points.size();
    cout<<"     cloud 0 nan   "<<cloud->points.size()<<endl;
   //传感器分辨率、以及roi区域参数
    double res=0.05;
    double side_rang[2] ={-200.0,200.0};
    double fwd_range[2] ={-200.0,200.0};
    double height_range[2] ={-5.0,5.0};


   //roi 区域的选择 ，使用pcl的函数做旋转 

   //创建条件限定的下的滤波器
    pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionAnd<pcl::PointXYZ> ()); //创建条件定义对象
   //为条件定义对象添加比较算子
    range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
    pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, fwd_range[0])));   //添加在x字段上大于-20的比较算子
    range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
    pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, fwd_range[1])));   //添加在x字段上小于20的比较算子

    range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
    pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, side_rang[0])));   //添加在y字段上大于-50的比较算子
    range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
    pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, side_rang[1])));   //添加在y字段上小于50的比较算子

   // 创建滤波器并用条件定义对象初始化
    pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
    condrem.setCondition (range_cond);               
    condrem.setInputCloud (cloud);                   //输入点云
    condrem.setKeepOrganized(false);               //设置保持点云的结构
   // 执行滤波
    condrem.filter (*cloud_filtered);  //两个条件用于建立滤波
    cout<<"      cloud_filtered    "<<cloud_filtered->points.size()<<endl;


    double *X = new double[Num] {0};
    double *Y = new double[Num] {0};
    double *Z = new double[Num] {0};
    double *V = new double[Num] {0};

    int x_max = 1 + int((side_rang[1] - side_rang[0]) / res);
    int y_max = 1 + int((fwd_range[1] - fwd_range[0]) / res);
cout<<"x_max"<<x_max<<endl;
cout<<"y_max"<<y_max<<endl;
   // 这里要申请一个图像数据

    Mat image = Mat::zeros( y_max, x_max, CV_8UC1);



	double ximg_max,ximg_min,yimg_max,yimg_min;
	ximg_max=yimg_max=0;
	ximg_min=yimg_min=10000000;

    for (size_t i = 0; i < cloud_filtered->points.size(); ++i)
    {

	
        X[i] = int(-cloud->points[i].y/res);
	Y[i] = int(-cloud->points[i].x/res);


        Z[i] = (cloud->points[i].z);

	if( Z[i]>height_range[1])
	{
		V[i]=height_range[1];
	}
	else if(Z[i]<height_range[0])
	{
		V[i]=height_range[0];
	}
	else
	{
		V[i]=cloud->points[i].z;
	}

        X[i]-= int(side_rang[0]/res-1);
        Y[i]+= int(fwd_range[1]/res);  
	//求最值
        if(X[i]>=ximg_max)
	{ximg_max=X[i];}
	if(X[i]<=ximg_min)
	{ximg_min=X[i];}
   
	if(Y[i]>=yimg_max)
	{yimg_max=Y[i];}
	if(Y[i]<yimg_min)
	{yimg_min=Y[i];} 

	V[i]=Scale_to_255(V[i],height_range[0],height_range[1]);

	if(0<=Y[i]<=y_max)
	{
		if((0<=X[i]<=x_max))
		{
	  	 image.at<uchar>(Y[i], X[i])=V[i];
		}
	}

        
    }
//输出最值
cout<<"x min"<<ximg_min<<endl;
cout<<"y min"<<yimg_min<<endl;
cout<<"x max"<<ximg_max<<endl;
cout<<"y max"<<yimg_max<<endl;
	namedWindow("bev_img", CV_WINDOW_AUTOSIZE);
	imshow("bev_img", image);
 	imwrite("t.jpg",image);
	waitKey(0);
    return 0;
}

第三题是有关特征的，提取第一帧点云作为例子写，这里先用pcl直接写一下看看:

简单的做了一下。做完了!但是不太清楚有些特征是干啥用的。

// pcl
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/common/impl/centroid.hpp>
#include <pcl/io/pcd_io.h>  
#include <pcl/common/eigen.h>
#include <math.h>
#include <pcl/filters/filter.h> //去除 nan 点

using namespace pcl;
using namespace std;


void computeEigenValuesAndEigenVectors1(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_cloud, int neibor_num=20)
{

    std::vector<int> neighbor_index;
    std::vector<float> pointNKNSquaredDistance;
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(input_cloud);
    std::vector<float> neighbor_square_distance;
    ofstream zos("xly.txt");
for(size_t i = 0; i < input_cloud->size(); i++)
 {	
    if ( kdtree.nearestKSearch (input_cloud->points[i], neibor_num, neighbor_index,pointNKNSquaredDistance) > 0 )
    {

		Eigen::Matrix<double, 4, 1> centroid;
		pcl::compute3DCentroid(*input_cloud, neighbor_index, centroid);  // 计算质心
		Eigen::Matrix<double, 3, 3> convariance_matrix;  // 协方差矩阵

       		 pcl::computeCovarianceMatrix(*input_cloud, neighbor_index, centroid, convariance_matrix);
		//特征值和特征向量
		Eigen::Matrix3d eigenVectors;
		Eigen::Vector3d eigenValues;
		pcl::eigen33(convariance_matrix, eigenVectors, eigenValues);


		// 求前两个较大的特征值
		Eigen::Vector3d::Index maxRow, maxCol, minRow, minCol;
		eigenValues.maxCoeff(&maxRow, &maxCol);
		eigenValues.minCoeff(&minRow, &minCol);
		// l1 > l2 > l3
		const double& l1 = eigenValues[maxRow];
		const double& l2 = eigenValues[3 - maxRow - minRow]; // 这个是巧算，基于 0 + 1 + 2 = 3
		const double& l3 = eigenValues[minRow];

		double e1=1;
		double e2=l2/(l1+l2);
		double e3=l3/(l1+l2+l3);

	       //  计算相应的特征

		double lr=(l1-l2)/l1;
		double pr=(l2-l3)/l1;
		double sr=l3/l1;
		double Or=3*exp(log(e1*e2*e3)/3);
		double Er=-(e1*log10(e1)+e2*log10(e2)+e2*log10(e2));
		double Cr=3*l3/(l1+l2+l3);

    	
   	 	// txt
       	  	zos << lr << " " <<pr << " " <<sr << Or << " " <<Er << " " <<Cr <<  endl;
   	 
   	

     }
  }
    cout << "trans has done!!!" << endl;
}

int main(int argc, char *argv[])
{

     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    // Fill in the cloud data  
     if (pcl::io::loadPCDFile<pcl::PointXYZ>("inputCloud0.pcd", *cloud) == -1)
     {
         PCL_ERROR("Couldn't read file chuli.pcd\n");
         return (-1);
     }

    //去除 nan 点
     std::vector<int> mapping;
     pcl::removeNaNFromPointCloud(*cloud, *cloud, mapping);
   //遍历所有的点并且计算每一个点的特征
     computeEigenValuesAndEigenVectors1(cloud,20);

 return 0;
}

利用pcl 的api 做起来很简单。

人生体验

关注

1
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
自动驾驶点云(作业2)

实在没时间好好写博客，就先暂时记录以下做的过程。1.首先是订阅激光雷达的每一帧点云，并将其转化为pcl点云数据，并显示#include "ros/ros.h"#include <iostream>#include <sensor_msgs/PointCloud2.h>#include <sensor_msgs/Image.h>#include...
复制链接

扫一扫