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

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