基于标定数据将3D LiDAR点云与相机图像对齐（含C++版本代码）

这段C++代码演示了如何将Velodyne激光雷达的点云数据投影到相机图像上。该过程涉及以下主要步骤:

  1. 读取并解析来自文件的标定数据，包括P2矩阵、R0_rect矩阵和Tr_velo_to_cam矩阵。这些矩阵用于将激光雷达点云从Velodyne坐标系转换到相机坐标系。
  2. 从二进制文件中读取Velodyne激光雷达点云数据，并将其存储在Eigen矩阵中。
  3. 使用标定矩阵将Velodyne点云从Velodyne坐标系转换到相机坐标系。这涉及将点云与标定矩阵相乘。
  4. 过滤转换后的点云,移除深度值(Z坐标)为负的点。
  5. 通过将X和Y坐标除以相应的Z坐标,将转换后的3D点投影到2D图像平面上。
  6. 读取并显示对应的相机图像。
  7. 在相机图像上绘制投影点，仅绘制落在图像边界内的点。

1. 工程结构

2. CMakeLists.txt

cmake_minimum_required(VERSION 3.5)
project(velo2cam)set(CMAKE_CXX_STANDARD 11)find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})# 找到并包含PCL
find_package(PCL 1.8 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})# Find Eigen
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIRS})add_executable(velo2cam main.cpp)
target_link_libraries(velo2cam ${OpenCV_LIBS} Eigen3::Eigen ${PCL_LIBRARIES})

3. main.cpp

#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <opencv2/opencv.hpp>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <thread>
#include <Eigen/Dense>using namespace std;
using namespace cv;
using namespace Eigen;int main(int argc, char** argv) {// 得到  000007int sn = (argc > 1) ? stoi(argv[1]) : 396; // Default 0-7517string name = to_string(sn);name = string(6 - name.length(), '0') + name; // 6 digit zeropaddingcout << name <<endl;// 读取标定数据文件string calib_file =  "/home/fairlee/CLionProjects/velo2cam/testing/calib/" + name + ".txt";ifstream calib_fin(calib_file);vector<string> calib_lines;string line;while (getline(calib_fin, line)) {calib_lines.push_back(line);}calib_fin.close();// 初始化P2矩阵(3x4) 从标定数据的第3行解析P2矩阵Matrix<double, 3, 4> P2;istringstream P2_ss(calib_lines[2].substr(calib_lines[2].find(" ") + 1));for (int i = 0; i < P2.rows(); ++i) {for (int j = 0; j < P2.cols(); ++j) {P2_ss >> P2(i, j);}}// 设置cout的输出格式，显示高精度的浮点数cout << fixed << setprecision(12);cout << "P2矩阵:" << endl;cout << P2 << endl;// 初始化R0_rect矩阵(3x3) 从标定数据的第5行解析R0_rect矩阵Matrix3d R0_rect = Matrix3d::Identity();istringstream R0_ss(calib_lines[4].substr(calib_lines[4].find(" ") + 1));for (int i = 0; i < R0_rect.rows(); ++i) {for (int j = 0; j < R0_rect.cols(); ++j) {R0_ss >> R0_rect(i, j);}}cout << fixed << setprecision(12);// 输出R0_rect矩阵cout << "R0_rect矩阵:" << endl;cout << R0_rect << endl;// 扩展R0_rect矩阵为4x4，并在右下角添加1Matrix4d R0_rect_4x4 = Matrix4d::Identity();for (int i = 0; i < R0_rect.rows(); ++i) {for (int j = 0; j < R0_rect.cols(); ++j) {R0_rect_4x4(i, j) = R0_rect(i, j);}}R0_rect_4x4(3, 3) = 1.0;// 输出R0_rect_4x4矩阵cout << "R0_rect_4x4矩阵:" << endl;cout << R0_rect_4x4 << endl;// 初始化Tr_velo_to_cam矩阵(3x4) 从标定数据的第6行解析Tr_velo_to_cam矩阵Matrix<double, 3, 4> Tr_velo_to_cam;istringstream Tr_ss(calib_lines[5].substr(calib_lines[5].find(" ") + 1));for (int i = 0; i < Tr_velo_to_cam.rows(); ++i) {for (int j = 0; j < Tr_velo_to_cam.cols(); ++j) {Tr_ss >> Tr_velo_to_cam(i, j);}}// 设置cout的输出格式，显示高精度的浮点数cout << fixed << setprecision(12);// 输出Tr_velo_to_cam矩阵cout << "Tr_velo_to_cam矩阵:" << endl;cout << Tr_velo_to_cam << endl;// 扩展Tr_velo_to_cam矩阵为4x4，并在右下角添加1Matrix4d Tr_velo_to_cam_4x4 = Matrix4d::Identity();for (int i = 0; i < Tr_velo_to_cam.rows(); ++i) {for (int j = 0; j < Tr_velo_to_cam.cols(); ++j) {Tr_velo_to_cam_4x4(i, j) = Tr_velo_to_cam(i, j);}}Tr_velo_to_cam_4x4(3, 3) = 1.0;// 输出R0_rect_4x4矩阵cout << "Tr_velo_to_cam_4x4 矩阵:" << endl;cout << Tr_velo_to_cam_4x4  << endl;// Read point cloud datastring binary_file = "/home/fairlee/CLionProjects/velo2cam/data_object_velodyne/testing/velodyne/" + name + ".bin";ifstream binary_fin(binary_file, ios::binary);vector<float> scan;float tmp;while (binary_fin.read(reinterpret_cast<char*>(&tmp), sizeof(float))) {scan.push_back(tmp);}binary_fin.close();int num_points = scan.size() / 4;// 初始化 points 矩阵MatrixXd points(num_points, 4);int index = 0;for (int i = 0; i < num_points; i++) {if (scan[i * 4] >= 0) {points(index, 0) = scan[i * 4];points(index, 1) = scan[i * 4 + 1];points(index, 2) = scan[i * 4 + 2];points(index, 3) = 1;index++;}}cout << "Rows: " << points.rows() << ", Cols: " << points.cols() << endl;// 调整 points 矩阵的大小以仅包含有效点points.conservativeResize(index, NoChange);cout << "Rows: " << points.rows() << ", Cols: " << points.cols() << endl;//    // 创建PCL点云对象//    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);//    cloud->width = points.rows();//    cloud->height = 1;//    cloud->is_dense = false;//    cloud->points.resize(cloud->width * cloud->height);////    for (int i = 0; i < points.rows(); i++) {//        pcl::PointXYZI point;//        point.x = points(i, 0);//        point.y = points(i, 1);//        point.z = points(i, 2);//        point.intensity = points(i, 3); // 如果你有反射强度数据，可以在这里设置//        cloud->points[i] = point;//    }////    // 创建PCL可视化对象//    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));//    viewer->setBackgroundColor(0, 0, 0);////    // 使用反射强度字段设置点云颜色//    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> intensity_distribution(cloud, "intensity");//    viewer->addPointCloud<pcl::PointXYZI>(cloud, intensity_distribution, "sample cloud");////    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");//    viewer->addCoordinateSystem(1.0);//    viewer->initCameraParameters();////    // 主循环，直到可视化窗口关闭//    while (!viewer->wasStopped()) {//        viewer->spinOnce(100);//        std::this_thread::sleep_for(std::chrono::milliseconds(100));//    }MatrixXd cam = P2*(R0_rect_4x4*(Tr_velo_to_cam_4x4*points.transpose()));// 输出 cam 矩阵的大小cout << "Original cam matrix size: " << cam.rows() << " x " << cam.cols() << endl;// Step 1: 找到需要删除的列vector<int> columns_to_delete;for (int i = 0; i < cam.cols(); ++i) {if (cam(2, i) < 0) {columns_to_delete.push_back(i);}}// Step 2: 创建一个新的矩阵，包含所有未被删除的列int new_cols = cam.cols() - columns_to_delete.size();MatrixXd cam_filtered(3, new_cols);int col_index = 0;for (int i = 0; i < cam.cols(); ++i) {if (find(columns_to_delete.begin(), columns_to_delete.end(), i) == columns_to_delete.end()) {cam_filtered.col(col_index) = cam.col(i);++col_index;}}// 输出过滤后的 cam 矩阵的大小cout << "Filtered cam matrix size: " << cam_filtered.rows() << " x " << cam_filtered.cols() << endl;// 确保第三行的元素不为零，以避免除以零的错误for (int i = 0; i < cam_filtered.cols(); ++i) {if (cam_filtered(2, i) == 0) {cam_filtered(2, i) = 1e-9; // 可以使用一个非常小的值来避免除以零}}// 对前两行分别进行元素除法cam_filtered.row(0).array() /= cam_filtered.row(2).array();cam_filtered.row(1).array() /= cam_filtered.row(2).array();// 输出处理后的 cam_filtered 矩阵的前 3 行 10 列//    int num_rows_to_print = min(3, static_cast<int>(cam_filtered.rows()));//    int num_cols_to_print = min(10, static_cast<int>(cam_filtered.cols()));//    cout << "Processed cam_filtered matrix (first " << num_rows_to_print << " rows and " << num_cols_to_print << " columns):\n";//    cout << cam_filtered.block(0, 0, num_rows_to_print, num_cols_to_print) << endl;// Read and display imagestring img_file = "/home/fairlee/CLionProjects/velo2cam/data_object_image_2/testing/image_2/" + name + ".png";Mat img = imread(img_file);if (img.empty()) {cerr << "Error: Could not open or find the image." << endl;return -1;}namedWindow("Projection", WINDOW_NORMAL);resizeWindow("Projection", img.cols, img.rows);imshow("Projection", img);// 过滤并绘制投影点vector<Point2f> pts;for (int i = 0; i < cam_filtered.cols(); i++) {float u = cam_filtered(0, i);float v = cam_filtered(1, i);if (u >= 0 && u < img.cols && v >= 0 && v < img.rows) {drawMarker(img, Point(u, v), Scalar(0, 255, 0), MARKER_CROSS, 1, 2); // 使用绿色, 大小 5, 线宽 2}}imshow("Projection", img);waitKey(0);return 0;
}

 4. 数据

000396.txt

P0: 7.070912000000e+02 0.000000000000e+00 6.018873000000e+02 0.000000000000e+00 0.000000000000e+00 7.070912000000e+02 1.831104000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00
P1: 7.070912000000e+02 0.000000000000e+00 6.018873000000e+02 -3.798145000000e+02 0.000000000000e+00 7.070912000000e+02 1.831104000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00
P2: 7.070912000000e+02 0.000000000000e+00 6.018873000000e+02 4.688783000000e+01 0.000000000000e+00 7.070912000000e+02 1.831104000000e+02 1.178601000000e-01 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 6.203223000000e-03
P3: 7.070912000000e+02 0.000000000000e+00 6.018873000000e+02 -3.334597000000e+02 0.000000000000e+00 7.070912000000e+02 1.831104000000e+02 1.930130000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 3.318498000000e-03
R0_rect: 9.999239000000e-01 9.837760000000e-03 -7.445048000000e-03 -9.869795000000e-03 9.999421000000e-01 -4.278459000000e-03 7.402527000000e-03 4.351614000000e-03 9.999631000000e-01
Tr_velo_to_cam: 7.533745000000e-03 -9.999714000000e-01 -6.166020000000e-04 -4.069766000000e-03 1.480249000000e-02 7.280733000000e-04 -9.998902000000e-01 -7.631618000000e-02 9.998621000000e-01 7.523790000000e-03 1.480755000000e-02 -2.717806000000e-01
Tr_imu_to_velo: 9.999976000000e-01 7.553071000000e-04 -2.035826000000e-03 -8.086759000000e-01 -7.854027000000e-04 9.998898000000e-01 -1.482298000000e-02 3.195559000000e-01 2.024406000000e-03 1.482454000000e-02 9.998881000000e-01 -7.997231000000e-01

000396.png

000396.bin

        这里采用的是KITTI 05数据集第000396帧点云数据（bin 数据无法上传，如有需要自行下载）。

5. 结果

        通过将Velodyne点云投影到相机图像上，我们可以将3D激光雷达数据与2D图像数据对齐。这对于许多应用非常有用，例如自动驾驶汽车中的目标检测和跟踪。该代码提供了一个基本的实现，演示了如何使用Eigen库和OpenCV库来执行这一任务。

致谢

GitHub - azureology/kitti-velo2cam: lidar to camera projection of KITTIlidar to camera projection of KITTI. Contribute to azureology/kitti-velo2cam development by creating an account on GitHub.icon-default.png?t=N7T8https://github.com/azureology/kitti-velo2cam

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mzph.cn/news/851155.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

找素数第二、三种方法

找素数第二、三种方法

文章目录 第一种 &#xff1a;使用标签第二种&#xff1a;本质是方法的分装 第一种 &#xff1a;使用标签 没有使用信号量。break和continue作用范围只是最近的循环&#xff0c;无法控制外部循环。 此时使用标签 对外部循环进行操作。 package com.zhang; /* 找素数 第二种方…
阅读更多...
MySQL—多表查询—外连接

MySQL—多表查询—外连接

一、引言 学到内连接&#xff0c;它是查询的数据两张表交集的部分。而接下来看看外连接。 外连接查询语法&#xff1a;&#xff08;分为两种&#xff09; 1、左外连接 语法结构&#xff1a; 表1 LEFT [OUTER] JOIN 表2 ON 条件 ...; ( ... left out join on ...) 注意&#x…
阅读更多...
三、安全工程练习题（CISSP）

三、安全工程练习题（CISSP）

1.三、安全工程练习题（CISSP）
阅读更多...
WordPress 高级缓存插件 W3 Total Cache Pro 详细配置教程

WordPress 高级缓存插件 W3 Total Cache Pro 详细配置教程

说起来有关 WordPress 缓存插件明月已经发表过不少文章了，但有关 W3 Total Cache Pro 这个 WordPress 高级缓存插件除了早期【网站缓存插件 W3 Total Cache，适合自己的才是最好的！】一文后就很少再提及了，最近因为明月另一个网站【玉满斋】因为某些性能上的需要准备更换缓存…
阅读更多...
java —— 线程（一）

java —— 线程（一）

一、进程与线程 一个进程可以包含一个以上的线程&#xff0c;CPU 时间片切换的基本单位是线程。 二、创建线程 &#xff08;一&#xff09;继承 Thread 类 public class Task extends Thread{Override //重写run方法public void run(){System.out.pr…
阅读更多...
当前 Python 版本中所有保留字keyword.kwlist

当前 Python 版本中所有保留字keyword.kwlist

【小白从小学Python、C、Java】 【考研初试复试毕业设计】 【Python基础AI数据分析】 当前 Python 版本中 所有保留字 keyword.kwlist [太阳]选择题 根据给定的Python代码&#xff0c;哪个选项是正确的&#xff1f; import keyword print("【执行】keyword.kwlist"…
阅读更多...
shell编程（四）—— 运算符

shell编程（四）—— 运算符

和其他编程语言一样&#xff0c;bash也有多种类型的运算符&#xff0c;本篇对bash的相关运算符做简单介绍。 一、运算符 1.1 算术运算符 常见的算术运算符&#xff0c;如加&#xff08;&#xff09;、减&#xff08;-&#xff09;、乘&#xff08;*&#xff09;、除&#xf…
阅读更多...
【网络架构】Nginx

【网络架构】Nginx

目录 一、I/O模型 1.1 Linux 的 I/O 1.2 零拷贝技术 1.3 网络IO模型 1.3.1 阻塞型 I/O 模型&#xff08;blocking IO&#xff09;​编辑 1.3.2非阻塞型 I/O 模型 (nonblocking IO)​编辑 1.3.3 多路复用 I/O 型 ( I/O multiplexing )​编辑 1.3.4 信号驱动式 I/O 模型 …
阅读更多...
Python 学习flask创建项目

Python 学习flask创建项目

1、使用pycharm创建flask项目 2、运行访问地址 3、可以看到访问地址内容 4、可以增加路由&#xff0c;尝试访问获取参数
阅读更多...
智能电网与微电网：引领电力未来的创新力量

智能电网与微电网：引领电力未来的创新力量

随着能源需求持续增长和环保压力日益加大&#xff0c;电力行业正面临前所未有的挑战与机遇。在这一背景下&#xff0c;智能电网和微电网作为新兴的技术应用方向&#xff0c;以其独特的优势和潜力&#xff0c;正逐步成为推动电力领域可持续发展的关键力量。 智能电网&#xff0…
阅读更多...
【车载开发系列】MCU选型

【车载开发系列】MCU选型

【车载开发系列】MCU选型 【车载开发系列】MCU选型 【车载开发系列】MCU选型一. 重要概念二. MCU选型的风险风险1风险2 三. MCU选型要点四. MCU选型维度五. MCU 选型需要考虑的因素1&#xff09;ROM/RAM2&#xff09;速度/主频3&#xff09;分析外设需求4&#xff09;工作电压(…
阅读更多...
设计模式- 责任链模式（行为型）

设计模式- 责任链模式（行为型）

责任链模式 责任链模式是一种行为模式&#xff0c;它为请求创建一个接收者对象的链&#xff0c;解耦了请求的发送者和接收者。责任链模式将多个处理器串联起来形成一条处理请求的链。 图解 角色 抽象处理者&#xff1a; 一个处理请求的接口&#xff0c;可以通过设置返回值的方…
阅读更多...
codesys【CAN总线】

codesys【CAN总线】

1下载设备描述文件&#xff1a; 必须下载设备描述文件&#xff0c;要不然编程软件无法正确组态。 根据实际设备【品牌】去官网搜索下载。 以 DMA882-CAN 为例 CAN的设备描述文件是【.eds】的扩展名 安装设备描述文件。 2添加CAN总线&#xff1a; 1添加【CAN总线】&#xff1a…
阅读更多...
同盾中文点选验证码识别方法

同盾中文点选验证码识别方法

中文验证码一直是识别的难题&#xff0c;首先他分类的种类很多&#xff0c;常见中文都有3500个&#xff0c;而且一般中文验证码都会有变形&#xff0c;导致每一个文字都需要大量训练样本。假设每一个汉字样本需要100个&#xff0c;100350035万个样本&#xff0c;所以标记的样本…
阅读更多...
excel拖拽怎么使单元格序号不递增

excel拖拽怎么使单元格序号不递增

拖拽下来不仅不递增&#xff0c;而且右下角没有倒三角可以设置改变&#xff0c;&#xff08;即没有下图这个&#xff09; 则&#xff0c;可以采用以下方法 excel数值拖拽不递增还有一个更快更快捷的方法&#xff0c;这就运用到了excel快捷键&#xff0c;我们把鼠标放到单元格的…
阅读更多...
IP分片的隐患，以及TCP分片

IP分片的隐患，以及TCP分片

好的&#xff0c;我们来用一个生活中的例子更详细地解释 MTU、MSS&#xff0c;以及 IP 和 TCP 分片。 MTU 和 MSS 的概念 MTU&#xff08;Maximum Transmission Unit&#xff0c;最大传输单元&#xff09;&#xff1a; 假设你搬家&#xff0c;需要用卡车搬运家具。 卡车的最…
阅读更多...
Hadoop 2.0：主流开源云架构（一）

Hadoop 2.0：主流开源云架构（一）

目录 一、引例&#xff08;一&#xff09;问题概述&#xff08;二&#xff09;常规解决方案&#xff08;三&#xff09;分布式下的解决方案&#xff08;四&#xff09;小结 自从云计算的概念被提出&#xff0c;不断地有IT厂商推出自己的云计算平台&#xff0c;但它们都是商业性…
阅读更多...
Suno小技巧大揭秘，不会这些技巧别说你懂AI音乐

Suno小技巧大揭秘，不会这些技巧别说你懂AI音乐

Suno是一个强大的AI音乐生成工具&#xff0c;它不仅可以帮你快速生成音乐&#xff0c;还能精确地根据你的需求进行调整。无论你是音乐小白还是专业音乐人&#xff0c;这篇文章将揭示一些鲜为人知的Suno技巧&#xff0c;帮助你最大化利用这个工具的潜力。 技巧一&#xff1a;精准…
阅读更多...
解读下/etc/network/interfaces配置文件

解读下/etc/network/interfaces配置文件

/etc/network/interfaces 是一个常见的网络配置文件&#xff0c;通常在 Debian 及其衍生版本的 Linux 发行版中使用。该文件用于配置网络接口和网络连接参数&#xff0c;允许用户手动设置网络连接的属性&#xff0c;包括 IP 地址、子网掩码、网关、DNS 服务器等。 以下是一个可…
阅读更多...
基于JSP的班级同学录网站

基于JSP的班级同学录网站

你好呀&#xff0c;我是计算机学长猫哥&#xff01;如果有相关需求&#xff0c;文末可以找到我的联系方式。 开发语言&#xff1a;Java 数据库&#xff1a;MySQL 技术&#xff1a;JSP技术 工具&#xff1a;IDEA/Eclipse、Navicat、Maven 系统展示 首页 个人中心 同学录管…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023