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

news2024/11/13 3:55:04

这段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) {

    // 得到  000007
    int sn = (argc > 1) ? stoi(argv[1]) : 396; // Default 0-7517
    string name = to_string(sn);
    name = string(6 - name.length(), '0') + name; // 6 digit zeropadding
    cout << 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，并在右下角添加1
    Matrix4d 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，并在右下角添加1
    Matrix4d 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 data
    string 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 image
    string 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.coloradmin.cn/o/1808865.html

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

相关文章

10.1 Go Goroutine

10.1 Go Goroutine

&#x1f49d;&#x1f49d;&#x1f49d;欢迎莅临我的博客&#xff0c;很高兴能够在这里和您见面&#xff01;希望您在这里可以感受到一份轻松愉快的氛围&#xff0c;不仅可以获得有趣的内容和知识&#xff0c;也可以畅所欲言、分享您的想法和见解。 推荐:「stormsha的主页」…
阅读更多...
解密Spring Boot：深入理解条件装配与条件注解

解密Spring Boot：深入理解条件装配与条件注解

文章目录 一、条件装配概述1.1 条件装配的基本原理1.2 条件装配的作用 二、常用注解2.1 ConditionalOnClass2.2 ConditionalOnBean2.3 ConditionalOnProperty2.4 ConditionalOnExpression2.5 ConditionalOnMissingBean 三、条件装配的实现原理四、实际案例 一、条件装配概述 1…
阅读更多...
SpringBoot中实现一个通用Excel导出功能

SpringBoot中实现一个通用Excel导出功能

SpringBoot中实现一个通用Excel导出功能 文章目录 SpringBoot中实现一个通用Excel导出功能这个导出功能的特色看效果代码解析1、依赖2、Excel 入参（ExcelExportRequest）3、Excel 出参（ExcelExportResponse）4、ExcelExportField5、ExcelExportUtils 工具类6、ExcelHead 头部…
阅读更多...
LeetCode1318或运算的最小翻转次数

LeetCode1318或运算的最小翻转次数

题目描述 给你三个正整数 a、b 和 c。你可以对 a 和 b 的二进制表示进行位翻转操作&#xff0c;返回能够使按位或运算 a OR b c 成立的最小翻转次数。「位翻转操作」是指将一个数的二进制表示任何单个位上的 1 变成 0 或者 0 变成 1 。 解析 这一题就按位依次比较就行了。取这…
阅读更多...
【SQLAlChemy】Query函数可传入的参数有哪些？

【SQLAlChemy】Query函数可传入的参数有哪些？

Query 函数的使用 参数种类 一般可以传递的参数有如下三种&#xff1a; 模型名。指定查找这个模型的全部属性&#xff08;对应于数据库查询中的全表查询&#xff09;。模型中的属性。可以用来指定只查询某个模型的几个属性值。使用聚合函数。 func.count():统计行的数量。fu…
阅读更多...
【机器学习】GPT-4中的机器学习如何塑造人类与AI的新对话

【机器学习】GPT-4中的机器学习如何塑造人类与AI的新对话

&#x1f680;时空传送门 &#x1f50d;引言&#x1f4d5;GPT-4概述&#x1f339;机器学习在GPT-4中的应用&#x1f686;文本生成与摘要&#x1f388;文献综述与知识图谱构建&#x1f6b2;情感分析与文本分类&#x1f680;搜索引擎优化&#x1f4b4;智能客服与虚拟助手&#x1…
阅读更多...
打造智慧校园信息系统，提升学校科技实力

打造智慧校园信息系统，提升学校科技实力

在如今数字化的时代&#xff0c;打造智慧校园信息系统已成为提升学校科技实力的关键。随着科技的迅猛发展&#xff0c;学校需要跟上时代步伐&#xff0c;利用先进技术建设一个高效、智能的信息系统&#xff0c;为学生、教师和管理人员提供更好的学习和工作环境。 智慧校园信息系…
阅读更多...
企业官网：过时了，但又没完全过时

企业官网：过时了，但又没完全过时

作为一名互联网冲浪级选手&#xff0c;我经常会看到一些有趣的产品。 这两年比较让我感兴趣的产品有「飞聊」、「即刻」及其旗下的「橙 App」等等&#xff0c;然后我就想上它们的官网看看。 虽然现在 app 是主流&#xff0c;但我非常不喜欢下载 app&#xff0c;一是麻烦&…
阅读更多...
数据结构与算法笔记：基础篇 - 二叉树基础（下）：有了如此高效的散列表，为什么还需要二叉树？

数据结构与算法笔记：基础篇 - 二叉树基础（下）：有了如此高效的散列表，为什么还需要二叉树？

概述 上篇文章&#xff0c;我们学习了树、二叉树及二叉树的遍历&#xff0c;本章来学习一种特殊的二叉树&#xff0c;二叉查找树。二叉查找树最大的特点就是&#xff0c;支持动态数据集合的快速插入、删除、查找操作。 之前说过&#xff0c;散列表也是支持这些操作的&#xf…
阅读更多...
RAG：如何从0到1搭建一个RAG应用

RAG：如何从0到1搭建一个RAG应用

通过本文你可以了解到&#xff1a; 什么是RAG&#xff1f;如何搭建一个RAG应用&#xff1f;目前开源的RAG应用有哪些&#xff1f; 大模型学习参考&#xff1a; 1.大模型学习资料整理&#xff1a;大模型学习资料整理&#xff1a;如何从0到1学习大模型&#xff0c;搭建个人或企业…
阅读更多...
67. UE5 RPG 创建法师敌人角色

67. UE5 RPG 创建法师敌人角色

我们设置的敌人类型分三种&#xff0c;分别时战士类型&#xff0c;远程射手&#xff0c;和法师类型。在前面&#xff0c;我们创建了战士和射手类型的&#xff0c;还没有法师类型的&#xff0c;在这一篇文章中&#xff0c;我们创建一个法师类型的角色。 在64. UE5 RPG 创建新的双…
阅读更多...
矩阵杯2024 Re wp 前两题

矩阵杯2024 Re wp 前两题

1. packpy UPX壳&#xff0c;但不能直接脱&#xff0c;应该是修改了头文件&#xff08;l_info) 改一下就能脱了 脱完是个elf文件 进IDA看一眼 明显的Py打包标志&#xff0c;用pyinstxtractor解包出来&#xff08;最好用对应的python3.8&#xff09; 可以得到packpy.pyc文件&a…
阅读更多...
ROS学习记录：C++节点发布自定义地图

ROS学习记录：C++节点发布自定义地图

前言 ROS栅格地图格式 在了解了ROS地图消息包的数据结构后(链接在上)&#xff0c;本文将编写一个节点&#xff0c;发布地图消息包&#xff0c;看看在RViz中显示是什么效果。 一、准备 1、为了简单起见&#xff0c;发布一个两行四列的地图 2、为了便于观测&#xff0c;只对地…
阅读更多...
textattack报错：不能导入自定义search_methods (cannot import name ‘xxx‘ from ‘xxx‘)

textattack报错：不能导入自定义search_methods (cannot import name ‘xxx‘ from ‘xxx‘)

1. 报错信息 ImportError: cannot import name AAA from textattack.search_methods (/home/666/anaconda3/envs/textattack37_env/lib/python3.7/site-packages/textattack/search_methods/__init__.py)2. 出错简述 贴一段test1.py的模块导入 #建议使用&#xff01; import…
阅读更多...
时钟影响ADC性能不仅仅是抖动

时钟影响ADC性能不仅仅是抖动

时钟影响ADC性能除了抖动&#xff0c;还有占空比。 在高速AD采样中&#xff0c;时钟占空比是非常重要的一个参数。时钟信号的上升沿控制ADC的采样&#xff0c;而下降沿控制着信号的保持&#xff0c;在一个周期内才可以完成量化输出&#xff0c;所以必须保持时钟的占空比为50%&…
阅读更多...
Typora Markdown编辑器 for Mac v1.8.10 安装

Typora Markdown编辑器 for Mac v1.8.10 安装

Mac分享吧 文章目录 效果一、准备工作二、开始安装1、双击运行软件&#xff0c;将其从左侧拖入右侧文件夹中&#xff0c;等待安装完毕2. 应用程序显示软件图标&#xff0c;表示安装成功 三、运行调试1、修改主题2、显示文档列表&#xff0c;如下图3、查看版本信息 **安装完成&…
阅读更多...
AI智能体做高考志愿填报分析

AI智能体做高考志愿填报分析

关注公众号&#xff0c;赠送AI/Python/Linux资料&#xff0c;对AI智能体有兴趣的朋友也可以添加一起交流 高考正在进行时&#xff0c;学生焦虑考试&#xff0c;家长们焦虑的则是高考志愿怎么填。毕竟一个好的学校&#xff0c;好的专业是进入社会的第一个敲门砖 你看张雪峰老师…
阅读更多...
CvT（ICCV 2021）论文与代码解读

CvT（ICCV 2021）论文与代码解读

paper&#xff1a;CvT: Introducing Convolutions to Vision Transformers official implementation&#xff1a;https://github.com/microsoft/CvT 出发点 该论文的出发点是改进Vision Transformer (ViT) 的性能和效率。传统的ViT在处理图像分类任务时虽然表现出色&#xf…
阅读更多...
xilinx的Aurora8B10B的IP仿真及上板测试（高速收发器十七）

xilinx的Aurora8B10B的IP仿真及上板测试（高速收发器十七）

前文讲解了Aurora8B10B协议原理及xilinx相关IP&#xff0c;本文讲解如何设置该IP&#xff0c;并且通过示例工程完成该IP的仿真和上板。 1、生成Aurora8B10B IP 如下图所示&#xff0c;首先在vivado的IP catalog中输入Aurora 8B10B&#xff0c;双击该IP。 图1 查找Aurora 8B10…
阅读更多...
【Python】成功解决SyntaxError: invalid syntax

【Python】成功解决SyntaxError: invalid syntax

【Python】成功解决SyntaxError: invalid syntax 下滑即可查看博客内容 &#x1f308; 欢迎莅临我的个人主页 &#x1f448;这里是我静心耕耘深度学习领域、真诚分享知识与智慧的小天地&#xff01;&#x1f387; &#x1f393; 博主简介&#xff1a;985高校的普通本硕&am…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023