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

news/2025/2/15 13:01:44/

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

如果没有对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;}// 相机内参 3X3const 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.);// 相机内参 3X4const 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;}}

映射结果

从结果中可见车，围栏、路边的树木，地面都进行了准确映射。
在这里插入图片描述

文件下载

本文所使用的测试点云和图像文件可从此处免费下载
资源：点云数据，二维图像

全部代码

#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;}// 相机内参 3X3const 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.);// 相机内参 3X4const 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);}
}

http://www.ppmy.cn/news/153269.html

相关文章

你不可错过的光模块中的激光器类型大盘点！

你不可错过的光模块中的激光器类型大盘点！

“ laser “ 由LASER (Light Amplification by Stimulated Emission of Radiation) 受激辐射光放大器缩写演变而来&#xff0c;激光是一种相干光&#xff0c;即具有频率相同、振动方向相同、相位相同&#xff08;或者说相位相差保持恒定&#xff09;的特性&#xff0c;光通信系…
阅读更多...
【低光增强】Zero-DCE

【低光增强】Zero-DCE

文章目录 一、前言二、算法理解2.1 低光增强曲线2.2 整体框架2.3 网络结构2.4 损失函数2.4.1 空间一致性2.4.2 曝光控制2.4.3 色彩恒常2.4.4 光照平滑 2.5 Zero-DCE 三、效果测试 一、前言 Paper&#xff1a;Learning to Enhance Low-Light Image via Zero-Reference Deep Cur…
阅读更多...
蓝天准系统P750的介绍与开箱

蓝天准系统P750的介绍与开箱

准系统笔记本&#xff1a;准系统笔记本是指使用由工厂&#xff08;即ODM厂商&#xff09;采购的标准化笔记本模具&#xff0c;再通过商家或懂技术的玩家安装相兼容的配件&#xff08;如CPU&#xff0c;显卡&#xff0c;内存&#xff0c;硬盘&#xff0c;光驱&#xff0c;无线网…
阅读更多...
Nvidia Jetson AGX Orin 初体验

Nvidia Jetson AGX Orin 初体验

一、开箱 Nvidia Jetson AGX Orin是今年Nvidia推出的唯一的开发套件&#xff0c;相比Jetson Nano 472GFLOP算力、Jetson Xaiver 32TOPS(INT8)算力&#xff0c;它的算力达到了200 TOPS左右。也就是说&#xff0c;几乎相当于目前主流设备的8-10倍的算力。这就让张小白有点动心了…
阅读更多...
NVIDIA GeForce RTX 2070显卡参数

NVIDIA GeForce RTX 2070显卡参数

以下是通过CUDA例程deviceQuery得到的结果&#xff1a; CUDA Device Query (Runtime API) version (CUDART static linking) Detected 1 CUDA Capable device(s) Device 0: "NVIDIA GeForce RTX 2070" CUDA Driver Version / Runtime Version 11.6 / 11…
阅读更多...
DMA+PWM驱动彩色RGB灯

DMA+PWM驱动彩色RGB灯

前言 现在带IC的彩灯虽然有 SK6812 、WS2812 等不同信号&#xff0c;但是其控制逻辑基本是完全兼容的&#xff0c;本文所描述相关控制参数来源于 SK6812 ,原于带 IC 芯片的 RGB 可以通过串联来实现数据的传输&#xff0c;使得其在PCB布线时变得更为简单。 SK6812 时序 驱动方…
阅读更多...
英伟达RTX 4070最新测评来了！光追效果更棒，但仅限于2k游戏

英伟达RTX 4070最新测评来了！光追效果更棒，但仅限于2k游戏

萧箫 发自 凹非寺量子位 | 公众号 QbitAI 英伟达RTX 4070开卖&#xff0c;4799元的价格也是让不少人心动。 由于之前矿难一波&#xff0c;英伟达30系显卡失去了不少人的信任&#xff0c;大伙儿的目光也都放在了40系显卡上。 现在4070性价比卡终于姗姗来迟&#xff0c;它实际性能…
阅读更多...
一行代码让英特尔显卡光追性能“改进100 倍”，网友们笑了

一行代码让英特尔显卡光追性能“改进100 倍”，网友们笑了

Pine 发自 凹非寺量子位 | 公众号 QbitAI 将光线追踪性能大幅提升“100倍”&#xff0c;英特尔做到了&#xff01; 英特尔近日合并了一个光线追踪的修复程序到开源的Mesa Vulkan驱动程序中&#xff0c;这个合并请求目前已经登陆Mesa 22.2。 同时&#xff0c;修改这个程序的英特…
阅读更多...
最新文章

Copyright @ 2022~2023