Livox_color点云着色——源码阅读

源码阅读以及理解
单帧着色原理

//首先需要三个矩阵：extrinsicT外参矩阵，由于坐标系的变换；intrisicT相机内参矩阵，用于三维到二维的映射；ditortion相机畸变系数，用于校准图片
//然后需要ROS订阅三个话题：FAST_LIO2里程计，获取当前世界坐标，后面转换到雷达坐标系；订阅相机话题获取图片；订阅雷达话题，获取点云数据

//Twlidar 里程计的坐标，x_w激光雷达的坐标，联合起来作一次转换
Eigen::Vector3d x_w(raw_pcl_ptr->points[i].x, raw_pcl_ptr->points[i].y, raw_pcl_ptr->points[i].z);
Eigen::Vector3d x_lidar =  Twlidar.inverse()  *  x_w   ;

//利用OpenCV库进行了一次典型的坐标变换过程，把一个三维点从激光雷达坐标系映射到图像平面
X.at<double>(0, 0) = x_lidar.x();
X.at<double>(1, 0) = x_lidar.y();
X.at<double>(2, 0) = x_lidar.z();
X.at<double>(3, 0) = 1;		
//雷达坐标转换到相机坐标，相机坐标投影到像素坐标	
Y = intrisicMat * extrinsicMat_RT * X; 

做完准备就把平面像素直接赋值给对应的坐标

cv::Point pt;

pt.x = Y.at<double>(0, 0) / Y.at<double>(2, 0);
pt.y = Y.at<double>(1, 0) / Y.at<double>(2, 0);
if (pt.x >= 0 && pt.x < W && pt.y >= 0 && pt.y < H && x_lidar.x() > 0)  //&& raw_pcl_ptr->points[i].x>0去掉图像后方的点云
	{
		PointType p;
		p.x = raw_pcl_ptr->points[i].x;
		p.y = raw_pcl_ptr->points[i].y;
		p.z = raw_pcl_ptr->points[i].z;
		//点云颜色由图像上对应点确定
		p.b = image_color[pt.y][pt.x][0];
		p.g = image_color[pt.y][pt.x][1];
		p.r = image_color[pt.y][pt.x][2];
		p.intensity = raw_pcl_ptr->points[i].intensity; //继承之前点云的intensity
		fusion_pcl_ptr->points.push_back(p);
	}