欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识！
您现在的位置是： 首页

KITTI 64线激光点云投影相机图像

程序员文章站 2022-03-07 12:45:24
...

代码链接：https://gitee.com/long_xiao_wyh/KITTI_FUSION
KITTI Dataset
KITTI数据集是一个非常有名的自动驾驶场景下的计算机视觉算法评测数据集。该数据集的数据采集平台装配有2个灰度摄像机，2个彩色摄像机，4个变焦镜头,一个Velodyne 64线激光雷达，一个GPS导航系统：
1惯性导航系统（GPS / IMU）：OXTS RT 3003
1 Laserscanner：Velodyne HDL-64E
2个灰度相机，1.4百万像素：Point Grey Flea 2（FL2-14S3M-C）
2个彩色摄像机，1.4百万像素：Point Grey Flea 2（FL2-14S3C-C）
4个变焦镜头，4-8 mm：Edmund Optics NT59-917
其中激光雷达的详细参数如下：
Velodyne HDL-64E rotating 3D laser scanner

  • 10 Hz
  • 64 beams
  • 0.09 degree angular resolution
  • 2 cm distanceaccuracy
  • collecting∼1.3 million points/second
  • field of view: 360° - horizontal, 26.8°
  • vertical, range: 120 m
    激光扫描仪（面向前方）以每秒10帧的速度触发相机。摄像机安装在与地平面大致平行的位置。

KITTI官网上给出了他们数据收集平台的传感器设置：
KITTI 64线激光点云投影相机图像KITTI 64线激光点云投影相机图像
激光点云投影相机图像
代码比较简单，分为两个ROS节点，一个节点负责不断读取点云原始bin文件，并以 sensor_msgs::PointCloud2 的消息格式发布话题；另一个节点对接收到的点云消息进行处理，把点云投影到相机图像上。
KITTI 64线激光点云投影相机图像
点云原始bin文件:
KITTI 64线激光点云投影相机图像
发布节点laserhandle.cpp代码：

#include <cmath>
#include <vector>

#include <opencv/cv.h>


#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <iostream>
#include <cstdlib>
#include <string>
#include <fstream>

#include <signal.h>

ros::Publisher pubLaserCloud;


void laserCloudHandler(pcl::PointCloud<pcl::PointXYZ> laserCloudIn)
{

  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
  int cloudSize = laserCloudIn.points.size();

  sensor_msgs::PointCloud2 cornerPointsMsg;
  pcl::toROSMsg(laserCloudIn, cornerPointsMsg);

  pubLaserCloud.publish(cornerPointsMsg);
  
}

std::string getFrameStr(unsigned int frame)
{
	if(frame>9999)
		return "00000"+std::to_string(frame);
	else if(frame>999)
		return "000000"+std::to_string(frame);
	else if(frame>99)
		return "0000000"+std::to_string(frame);
	else if(frame>9)
		return "00000000"+std::to_string(frame);
	else if(frame<=9)
		return "000000000"+std::to_string(frame);
}

void my_handler(int s){
    std::cout<<"Finishing program with signal "<<s<<std::endl;
    exit(1); 
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "laserhandle");
  ros::NodeHandle nh;

  struct sigaction sigIntHandler;
  sigIntHandler.sa_handler = my_handler;
  sigemptyset(&sigIntHandler.sa_mask);
  sigIntHandler.sa_flags = 0;
  sigaction(SIGINT, &sigIntHandler, NULL);

  pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>
                                 ("/velodyne_cloud", 10);

  pcl::PointCloud<pcl::PointXYZ> cloud;

  // allocate 4 MB buffer (only ~130*4*4 KB are needed)
  int32_t num = 1000000;
  float *data = (float*)malloc(num*sizeof(float)); //equal to float *data = float[num];分配内存空间

  // pointers
  float *px = data+0;
  float *py = data+1;
  float *pz = data+2;
  float *pr = data+3;

  unsigned int currentFrame = 0;
  
  std::string file = "/home/wyh/Documents/2011_09_26/2011_09_26_drive_0009_sync/velodyne_points/data/" + getFrameStr(currentFrame) + ".bin";
  
  FILE *stream;
  stream = fopen (file.c_str(),"rb");
  //std::cout<<file<<std::endl;

  ros::Rate r(10);

  while(stream!=NULL)
  {
	  num = fread(data,sizeof(float),num,stream)/4; 

	  cloud.clear();

	  for (int32_t i=0; i<num; i++) {
	    pcl::PointXYZ point;
	    point.x = *px;
	    point.y = *py;
	    point.z = *pz;
	
	    cloud.push_back(point);
	    px+=4; py+=4; pz+=4; pr+=4;
	  }

	  laserCloudHandler(cloud);

	  //reset variables to read a new sweep
	  fclose(stream);
	  currentFrame++;
	  file = "/home/wyh/Documents/2011_09_26/2011_09_26_drive_0009_sync/velodyne_points/data/" + getFrameStr(currentFrame) + ".bin";
	  //std::cout<<file<<std::endl;
	  fflush(stream);
	  stream = fopen (file.c_str(),"rb");
	  free(data);
	  num = 1000000;
	  data = (float*)malloc(num*sizeof(float));
	  px = data+0;
	  py = data+1;
    pz = data+2;
    pr = data+3;

    r.sleep();
  }

  free(data);

  return 0;
}

接收节点lidtocamp.cpp代码

#include <Eigen/Core>

#include <cmath>
#include <vector>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <iostream>
#include <cstdlib>
#include <string>


using namespace cv;
using namespace std;

unsigned int currentFrame = 0;

//3x3 rectifying rotation to make image planes co-planar, R_rect_0X:3x3
Eigen::Matrix<double,4,4> R_rect_02;

//3x4 projection matrix after rectification, P_rect_02:3x4						
Eigen::Matrix<double,3,4>  P_rect_02;

//Transform from velo to cam0, T:4x4
Eigen::Matrix<double,4,4> extrinsicT;

Mat	image ;

string getFrameStr(unsigned int frame)
{
	if(frame>9999)
		return "00000"+to_string(frame);
	else if(frame>999)
		return "000000"+to_string(frame);
	else if(frame>99)
		return "0000000"+to_string(frame);
	else if(frame>9)
		return "00000000"+to_string(frame);
	else if(frame<=9)
		return "000000000"+to_string(frame);
}


//把激光点云坐标投影到相机二维平面
Eigen::Vector3d transformProject(const Eigen::Vector4d& P_lidar)
{	Eigen::Vector3d z_P_uv = P_rect_02*R_rect_02*extrinsicT*P_lidar;
	return Eigen::Vector3d( int( z_P_uv[0]/z_P_uv[2] + 0.5 ) , int( z_P_uv[1]/z_P_uv[2] + 0.5 ), 1 );
}


void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &PointCloudMsg)
{	
	std::string image_name = "/home/wyh/Documents/2011_09_26/2011_09_26_drive_0009_sync/image_02/data/" + getFrameStr(currentFrame) + ".png";
	cout << "currentFrame:"<< currentFrame << endl;
	image = imread(image_name);//原图
	Mat image_show = image.clone();

	//将点云转为pcl格式
	pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
	pcl::fromROSMsg(*PointCloudMsg, laserCloudIn);

	int pointcloudsize = laserCloudIn.size(); //点云个数
	cout<<"received pointcloud feature size per sweep = "<<pointcloudsize<<endl;

	vector<Point2f> point; 

	int count = pointcloudsize;

	for(int i = 0; i < pointcloudsize; i++){
		
		Point2f cv_point;
		Eigen::Vector4d P_lidar(laserCloudIn.points[i].x,
										laserCloudIn.points[i].y,
											laserCloudIn.points[i].z,
												1);
		
		Eigen::Vector3d P_uv = transformProject(P_lidar);
		
		//去除不在图像上的投影点,并把点转为cv::Point2f类型
		if(P_uv[0] >= 0 && P_uv[1] >= 0 && P_uv[0]<=1242 && P_uv[1]<=375){
					
			cv_point.x = P_uv[0];
			cv_point.y = P_uv[1];
			circle(image_show,cv_point,1,Scalar(0,255,0));
			point.push_back(cv_point);
	
		}		
		else{
			count--;
		}			
	}
	int num = point.size();
	
	cout<<"pointcloudsize after verify = "<<num<<endl<<endl;
	

	imshow("fusion" , image_show);
	waitKey(10);

	currentFrame++;	
}

int main( int argc, char** argv)
{	
	ros::init(argc, argv, "lidtocam");   
	ros::NodeHandle nh;

	R_rect_02 <<  9.999758e-01, -5.267463e-03, -4.552439e-03, 0,
				5.251945e-03, 9.999804e-01, -3.413835e-03, 0,
				 4.570332e-03, 3.389843e-03, 9.999838e-01, 0,
				 0  ,0  ,  0,  1;

	P_rect_02 << 7.215377e+02, 0.000000e+00, 6.095593e+02, 4.485728e+01,
				 0.000000e+00, 7.215377e+02, 1.728540e+02, 2.163791e-01,
				  0.000000e+00, 0.000000e+00, 1.000000e+00, 2.745884e-03;

	extrinsicT << 7.533745e-03, -9.999714e-01, -6.166020e-04, -4.069766e-03,
			1.480249e-02, 7.280733e-04, -9.998902e-01, -7.631618e-02,
			9.998621e-01, 7.523790e-03, 1.480755e-02, -2.717806e-01,
			0 ,	0,	0,	1;
	
	ros::Subscriber pointCloudSub = nh.subscribe("/velodyne_cloud", 10 ,pointCloudCallback );
	ros::spin();
	
	return 0;
}

程序运行时的终端信息：
KITTI 64线激光点云投影相机图像
可以看到，激光雷达一个周期的点云数量大概是100k，由于64线激光雷达是横向360°、纵向26.8°视野，而相机横向是90°、纵向35°视野，所以投影在图像上的点数量接近点云总数的1/4。
运行环境
Ubuntu 16.04 + ROS Kinetic + Opencv 3.3.1 + Eigen 3.2.92 + PCL 1.7.2
数据集使用的是KITTI raw data中的数据，比如代码里使用的是这个，记得用去畸变后的数据，代码里对应的目录以及标定的变换矩阵需要更改，如果用的是09_26这一天的数据，只需要更改两个代码中的目录即可：
KITTI 64线激光点云投影相机图像

相关标签： 传感器融合

上一篇： greenev —— Python 异步网络服务框架

下一篇： 本地项目上传到GitHub

推荐阅读