激光雷达点云与单幅图像配准/映射变为彩色点云

这篇具有很好参考价值的文章主要介绍了激光雷达点云与单幅图像配准/映射变为彩色点云。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

激光雷达点云与单图像配准/映射变为彩色点云

如果没有对pcl库进行配置,则需要先配置pcl:可重复使用的VS-PCL1.10.1环境配置
本文提供激光雷达采集的点云与单幅二维图像之间的配准方法,目的是实现点云到图像之间的映射,同时也可以将点云转变为彩色点云。
数据准备:

  • point_cloud: 激光雷达点云数据;
  • img: 与激光雷达对应的图像;
  • camera_par: 相机内参 >> 相机坐标系到像素坐标系的变换矩阵
  • D: 相机畸变系数
  • t_word_to_cam: 激光雷达到相机位置的变换矩阵:世界坐标到相机坐标变换矩阵

关于激光雷达点云与图像之间的映射原理有网上给出了很多,但是具体转换的方法或开源的代码却较少。因此本文就以一副点云和一副图像进行介绍,并给出具体的实现过程。
这个中间具体的转换原理就不做介绍了,就给一个具体的转换思路。

转换过程:世界坐标 》相机坐标 》像素坐标 此时每个点云(x,y,z)就会对应到一个像素坐标(u,v),即对应一个颜色点(r,g,b)

具体的转换思路:
1、首先我们根据相机的畸变系数对图像去畸变,得到正确的图像。
2、然后将点云中的点 乘以 世界坐标到相机坐标变换矩阵 t_word_to_cam,目的是将世界坐标点转换为相机坐标点;
3、随后再将转换的相机坐标点乘以相机内参,目的是相机坐标点转换为平面像素坐标点
4、最后将像素坐标点对应的颜色值赋予点云中的点,表现形式为:生成了彩色点云。

数据准备

激光雷达点云与单幅图像配准/映射变为彩色点云
激光雷达点云与单幅图像配准/映射变为彩色点云

// 文件读取
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	std::string path = "2.pcd";
	std::string imgPath = "2.jpeg";
	pcl::io::loadPCDFile(path, *point_cloud);
	//PointCloudRoar(point_cloud, point_cloud_ptr);

	Mat img = imread(imgPath);
	if (img.channels() != 3) {
		cout << "RGB pics needed." << endl;
		return 0;
	}
	// 相机内参 3X3
	const Mat K = (Mat_<double>(3, 3) <<
		1.9506592935364870e+03, 0.0, 1.9770478473401959e+03,
		0.0, 1.9508117738745232e+03, 1.0786204201895550e+03,
		0.0, 0.0, 1.);
	// 相机内参 3X4
	const Mat camera_par = (Mat_<double>(3, 4) <<
		1.9506592935364870e+03, 0.0, 1.9770478473401959e+03, 0,
		0.0, 1.9508117738745232e+03, 1.0786204201895550e+03, 0,
		0.0, 0.0, 1.0, 0);
	// 相机畸变参数
	const cv::Mat D = (cv::Mat_<double>(5, 1) <<
		-0.050519061674533024, -0.007992982752507883, 0.00970045657644595, -0.004354775040194558,0.0);
	// 世界坐标到相机坐标变换矩阵
	Mat t_word_to_cam = (Mat_<double>(4, 4) <<
		2.4747462378258280e-02, -9.9955232303502073e-01, -1.6839925611563663e-02, -9.2541271346932907e-02,
		-1.3087302341509554e-02, 1.6519577885364300e-02, -9.9977861656954914e-01, 2.4302538338292576e+00,
		9.9960858363250360e-01, 2.4962312041639460e-02, -1.2672595327247342e-02, -5.0924142692133323e+00, 
		0., 0. ,0. ,1);

图像去畸变

	cv::Mat UndistortImage;
	cv::undistort(img, UndistortImage, K, D, K);

世界坐标》相机坐标》像素坐标

在坐标像素点映射的过程中我们只对位于图像像素范围内的点进行映射,超出图像范围的点将不做处理。

	Mat word_h = Mat(4, 1, CV_64FC1);
	Mat p_result = Mat(3, 1, CV_64FC1);
	Mat cam_h = Mat(4, 1, CV_64FC1);
	double p_u, p_v, p_w;//pics_uv1;(u for cols, v for lines!!!)
	double c_x, c_y, c_z, c_i;//clouds_xyz、intensity;

	for (int nIndex = 0; nIndex < point_cloud->points.size(); nIndex++)
	{
		c_x = point_cloud->points[nIndex].x;
		c_y = point_cloud->points[nIndex].y;
		c_z = point_cloud->points[nIndex].z;
		word_h = (Mat_<double>(4, 1) << c_x, c_y, c_z, 1);
		// 矩阵计算 (3, 4) x (4, 4) x (4,1) = (3,1) 最后得到一个 3x1 的矩阵
		p_result = camera_par * t_word_to_cam * word_h;
		p_w = p_result.at<double>(2, 0);
		p_u = (int)((p_result.at<double>(0, 0)) / p_w);
		p_v = (int)((p_result.at<double>(1, 0)) / p_w);
		// 判断当前点映射的像素点是否在图像像素范围内,如果在就将该像素点的颜色赋予世界坐标点
		if(p_u >= 0 && p_u < cols && p_v >= 0 && p_v < rows && p_w>0){
			/*cout << "--" << p_u << "--" << cols << endl;
			cout << "***" << p_v << "--" << rows << endl;*/
			float r = UndistortImage.at<Vec3b>(p_v, p_u)[2];
			float g = UndistortImage.at<Vec3b>(p_v, p_u)[1];
			float b = UndistortImage.at<Vec3b>(p_v, p_u)[0];
			point_cloud->points[nIndex].r = r;
			point_cloud->points[nIndex].g = g;
			point_cloud->points[nIndex].b = b;
		}
		else {
			point_cloud->points[nIndex].r = 255;
			point_cloud->points[nIndex].g = 255;
			point_cloud->points[nIndex].b = 255;
		}
	}

映射结果

从结果中可见车,围栏、路边的树木,地面都进行了准确映射。
激光雷达点云与单幅图像配准/映射变为彩色点云

文件下载

本文所使用的测试点云和图像文件可从此处免费下载
资源:点云数据,二维图像文章来源地址https://www.toymoban.com/news/detail-400437.html

全部代码

#include <iostream>  
#include <boost/thread/thread.hpp>  
#include <pcl/common/common_headers.h>  
#include <pcl/common/common_headers.h>  
#include <pcl/features/normal_3d.h>  
#include <pcl/io/pcd_io.h>  
#include <pcl/io/io.h>
#include <pcl/point_types.h>  
#include <pcl/registration/icp.h>  
#include <pcl/visualization/pcl_visualizer.h>  
#include <pcl/console/parse.h>  
#include <pcl/visualization/cloud_viewer.h>//点云查看窗口头文件
#include "opencv2/highgui.hpp"
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <Eigen/Dense>

using namespace cv;
using namespace std;

//RGB colour visualisation example  
boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
	//创建3D窗口并添加点云   
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "sample cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");
	viewer->addCoordinateSystem(1.0);
	viewer->initCameraParameters();
	return (viewer);
}
int main()
{
	// 文件读取
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	std::string path = "2.pcd";
	std::string imgPath = "2.jpeg";
	pcl::io::loadPCDFile(path, *point_cloud);
	//PointCloudRoar(point_cloud, point_cloud_ptr);

	Mat img = imread(imgPath);
	if (img.channels() != 3) {
		cout << "RGB pics needed." << endl;
		return 0;
	}
	// 相机内参 3X3
	const Mat K = (Mat_<double>(3, 3) <<
		1.9506592935364870e+03, 0.0, 1.9770478473401959e+03,
		0.0, 1.9508117738745232e+03, 1.0786204201895550e+03,
		0.0, 0.0, 1.);
	// 相机内参 3X4
	const Mat camera_par = (Mat_<double>(3, 4) <<
		1.9506592935364870e+03, 0.0, 1.9770478473401959e+03, 0,
		0.0, 1.9508117738745232e+03, 1.0786204201895550e+03, 0,
		0.0, 0.0, 1.0, 0);
	// 相机畸变参数
	const cv::Mat D = (cv::Mat_<double>(5, 1) <<
		-0.050519061674533024, -0.007992982752507883, 0.00970045657644595, -0.004354775040194558,0.0);
	cv::Mat UndistortImage;
	cv::undistort(img, UndistortImage, K, D, K);
	int rows = UndistortImage.rows;
	int cols = UndistortImage.cols;

	// 世界坐标到相机坐标变换矩阵
	Mat t_word_to_cam = (Mat_<double>(4, 4) <<
		2.4747462378258280e-02, -9.9955232303502073e-01, -1.6839925611563663e-02, -9.2541271346932907e-02,
		-1.3087302341509554e-02, 1.6519577885364300e-02, -9.9977861656954914e-01, 2.4302538338292576e+00,
		9.9960858363250360e-01, 2.4962312041639460e-02, -1.2672595327247342e-02, -5.0924142692133323e+00, 
		0., 0. ,0. ,1);
	Mat word_h = Mat(4, 1, CV_64FC1);
	Mat p_result = Mat(3, 1, CV_64FC1);
	Mat cam_h = Mat(4, 1, CV_64FC1);

	double p_u, p_v, p_w;//pics_uv1;(u for cols, v for lines!!!)
	double c_x, c_y, c_z, c_i;//clouds_xyz、intensity;

	for (int nIndex = 0; nIndex < point_cloud->points.size(); nIndex++)
	{
		c_x = point_cloud->points[nIndex].x;
		c_y = point_cloud->points[nIndex].y;
		c_z = point_cloud->points[nIndex].z;
		word_h = (Mat_<double>(4, 1) << c_x, c_y, c_z, 1);
		p_result = camera_par * t_word_to_cam * word_h;

		p_w = p_result.at<double>(2, 0);
		p_u = (int)((p_result.at<double>(0, 0)) / p_w);
		p_v = (int)((p_result.at<double>(1, 0)) / p_w);
		
		if(p_u >= 0 && p_u < cols && p_v >= 0 && p_v < rows && p_w>0){
			/*cout << "--" << p_u << "--" << cols << endl;
			cout << "***" << p_v << "--" << rows << endl;*/
			float r = UndistortImage.at<Vec3b>(p_v, p_u)[2];
			float g = UndistortImage.at<Vec3b>(p_v, p_u)[1];
			float b = UndistortImage.at<Vec3b>(p_v, p_u)[0];
			point_cloud->points[nIndex].r = r;
			point_cloud->points[nIndex].g = g;
			point_cloud->points[nIndex].b = b;
		}
		else {
			point_cloud->points[nIndex].r = 255;
			point_cloud->points[nIndex].g = 255;
			point_cloud->points[nIndex].b = 255;
		}
	}
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	viewer = rgbVis(point_cloud);
	// 主循环  
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
	}
}

到了这里,关于激光雷达点云与单幅图像配准/映射变为彩色点云的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • Fusion_PointClouds - 多激光雷达点云数据融合

    fusion_pointclouds 主要目的为Ubuntu环境下无人车多激光雷达标定之后, 将多个激光雷达点云话题/坐标系 通过PCL (Point Cloud Library)融合为 一个ros点云话题,以便于后期点云地面分割与地面处理等等。 1.1 应用场景 图1:为了保证激光雷达的360°环境覆盖,我们需要用到多传感器的拼

    2024年02月03日
    浏览(45)
  • 基于ROS的自动驾驶 激光雷达点云物体检测 项目实战

    前言: 基于Apollo的preception与Autoware的lidar_apollo_cnn_seg_detect模块,并详细记录ROS系统上进行实时检测全部流程和踩坑,文章最后附上rosbag和rosbag的制作方法。参考文章:https://adamshan.blog.csdn.net/article/details/106157761?spm=1001.2014.3001.5502感谢大佬的杰作。 检测效果视频 环境 RTX 2060(

    2024年02月08日
    浏览(41)
  • Apollo官方课程算法解读笔记——激光雷达感知模块、基于PointPillars的激光雷达点云检测算法、PointPillars模型的部署和优化模型的部署和优化

    感知模块检测效果: 左边为摄像头拍摄图像,激光雷达感知不依赖左边CAMERA,而是点云数据对应的效果图(黄色上方数字为Tracking ID) 主车红灯时的激光点云检测效果图 车道线给CAMERA提供一个标定参考,使得camera检测出来的障碍物从2维转化为3维的信息,因为此标定的参考,

    2024年02月14日
    浏览(40)
  • 如何实现基于图像与激光雷达的 3d 场景重建?

    智影S100是一款基于图像和激光点云融合建模技术的 高精度轻巧手持SLAM三维激光扫描仪。 设备机身小巧、手持轻便,可快速采集点云数据;支持实时解算、实时预览点云成果,大幅提高内外业工作效率;同时支持一键生成实景三维Mesh模型,实现城市建筑、堆体、室内空间等

    2024年02月21日
    浏览(51)
  • 【激光SLAM】激光的前端配准算法

    在激光SLAM中,前端配准(Frontend Registration)是实现定位和地图构建的关键步骤之一。它的作用是将当前帧的激光扫描数据与先前帧(或参考帧)的激光扫描数据进行配准,以获取它们之间的 相对位姿变换 。 通过前端配准,激光SLAM系统可以实现对机器人在环境中的位置和姿

    2024年02月19日
    浏览(38)
  • 点云配准(三) 传统点云配准算法概述

             图像配准是图像处理研究领域中的一个典型问题和技术难点,其目的在于比较或融合针对同一对象在不同条件下获取的图像,例如图像会来自不同的采集设备,取自不同的时间,不同的拍摄视角等等,有时也需要用到针对不同对象的图像配准问题。具体地说,对

    2024年02月02日
    浏览(48)
  • 点云配准——经典配准算法及配准效果对比

    目录 点云配准基础知识 什么是点云配准? 点云配准的步骤 粗配准 精配准  点云配准的经典算法 ICP算法 NDT算法 3DSC算法 PFH FPFH 完全配准效果对比         点云配准技术即是通过寻找不同视角下不同点云之间的映射关系,利用一定的算法将同一目标场景的不同点云转换到

    2024年02月02日
    浏览(39)
  • 3D点云处理:点云粗配准(Fast PPF)

    文章目录: 3D视觉个人入门学习路线

    2024年02月10日
    浏览(113)
  • 点云配准--对称式ICP

    对称式ICP 针对于局部平面不完美的情况,提出了一种对称式ICP目标函数,相较于传统的ICP方法,增大了收敛域,提高了收敛速度。论文理论说明不甚清楚,实验较少,但代码开源。 对称目标函数 在icp中对于一对对应点p,q:在点到法线的度量中: ( p − q ) ⋅ n q (3) (p-q) cd

    2024年02月06日
    浏览(54)
  • 多视图点云配准算法综述

    作者:杨佳琪,张世坤,范世超等 转载自:华中科技大学学报(自然科学版) 编辑:东岸因为@一点人工一点智能 原文:​​多视图点云配准算法综述​​ 摘要: 以多视图点云配准为研究对象,对近二十余年的多视图点云配准相关研究工作进行了全面的分类归纳及总结。首先

    2024年02月05日
    浏览(44)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包