点云数据与多相机图像融合实现3D场景的彩色可视化

🕗 发布于 2024-10-13 09:11 PCL

引言

在现代3D计算机视觉和机器人感知领域，点云数据和图像信息的融合正变得越来越重要。点云数据提供了精确的几何结构，而图像则包含了丰富的颜色和纹理细节。将这两种数据源结合起来，我们能够创建更加逼真和信息丰富的3D场景表示。本文将深入探讨一个结合点云数据和多相机图像的项目，旨在生成高质量的彩色3D点云，从而大幅提升场景的可视化效果。

我们的项目利用了多个开源库，包括Point Cloud Library (PCL)用于点云处理，OpenCV用于图像处理，以及Eigen用于高效的矩阵运算。项目的核心目标是将来自多个相机的2D图像信息精确地映射到3D点云上，最终生成一个色彩丰富、细节清晰的3D场景表示。

数据集的构成包括以下几个关键部分：

一个存储为PCD格式的点云文件（frame3438.pcd）
五张来自不同相机的JPEG格式图像（frame3438cam1.jpg 到 frame3438cam5.jpg）
每个相机的参数矩阵，包括：
- 内参矩阵（K），描述相机的光学特性
- 畸变系数（D），用于校正镜头畸变
- 外参矩阵（t_word_to_cam），描述相机在世界坐标系中的位置和姿态

项目的核心算法实现主要包括以下几个步骤：

点云加载：使用PCL库的io模块加载PCD文件。
图像处理：利用OpenCV加载并预处理图像。
点云投影：将3D点云中的每个点投影到2D图像平面上。
颜色映射：从图像中提取对应的颜色信息，并将其赋给点云中的点。
可视化：使用PCL的可视化模块展示最终的彩色点云。

CMakeList.txt

cmake_minimum_required(VERSION 3.28)
project(UGV_Pt2Pc)

set(CMAKE_CXX_STANDARD 11)
set(CMAKE_SUPPRESS_DEVELOPER_WARNINGS TRUE CACHE INTERNAL "")
# Find required packages
find_package(OpenCV REQUIRED)
find_package(PCL REQUIRED)
find_package(Eigen3 REQUIRED)

# Include directories
include_directories(
        ${OpenCV_INCLUDE_DIRS}
        ${PCL_INCLUDE_DIRS}
        ${EIGEN3_INCLUDE_DIRS}
)

# Add definitions and link directories for PCL
add_definitions(${PCL_DEFINITIONS})
link_directories(${PCL_LIBRARY_DIRS})

# Add executable
add_executable(UGV_Pt2Pc main.cpp)

# Link libraries
target_link_libraries(UGV_Pt2Pc
        ${OpenCV_LIBS}
        ${PCL_LIBRARIES}
)

main.cpp

#include <iostream>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_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/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)
{
    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);
}

void projectPointsToImage(pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud, const Mat& img, const Mat& K, const Mat& D, const Mat& t_word_to_cam)
{
    Mat camera_par = K * Mat::eye(3, 4, CV_64F);
    Mat UndistortImage;
    cv::undistort(img, UndistortImage, K, D, K);
    int rows = UndistortImage.rows;
    int cols = UndistortImage.cols;

    Mat word_h = Mat(4, 1, CV_64FC1);
    Mat p_result = Mat(3, 1, CV_64FC1);

    for (int nIndex = 0; nIndex < point_cloud->points.size(); nIndex++)
    {
        double c_x = point_cloud->points[nIndex].x;
        double c_y = point_cloud->points[nIndex].y;
        double 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;

        double p_w = p_result.at<double>(2, 0);
        int p_u = (int)((p_result.at<double>(0, 0)) / p_w);
        int 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){
            Vec3b color = UndistortImage.at<Vec3b>(p_v, p_u);
            point_cloud->points[nIndex].r = color[2];
            point_cloud->points[nIndex].g = color[1];
            point_cloud->points[nIndex].b = color[0];
        }
    }
}

int main()
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    string path = "/home/fairlee/CLionProjects/UGVPoint2Picture/Data/frame3438.pcd";
    pcl::io::loadPCDFile(path, *point_cloud);

    vector<string> imgPaths = {
            "/home/fairlee/CLionProjects/UGVPoint2Picture/Data/frame3438cam1.jpg",
            "/home/fairlee/CLionProjects/UGVPoint2Picture/Data/frame3438cam2.jpg",
            "/home/fairlee/CLionProjects/UGVPoint2Picture/Data/frame3438cam3.jpg",
            "/home/fairlee/CLionProjects/UGVPoint2Picture/Data/frame3438cam4.jpg",
            "/home/fairlee/CLionProjects/UGVPoint2Picture/Data/frame3438cam5.jpg"
    };

    vector<Mat> K_matrices = {
            (Mat_<double>(3, 3) << 7.449480591354508e+02, 0.0, 6.239975192140465e+02, 0.0, 7.477630653570311e+02, 3.718130492325440e+02, 0.0, 0.0, 1.0),
            (Mat_<double>(3, 3) << 7.496397448476606e+02, 0.0, 6.406747754223233e+02, 0.0, 7.498672914667321e+02, 3.738034702122652e+02, 0.0, 0.0, 1.0),
            (Mat_<double>(3, 3) << 7.453318932115172e+02, 0.0, 6.353637094339318e+02, 0.0, 7.451280155876273e+02, 3.580752219734694e+02, 0.0, 0.0, 1.0),
            (Mat_<double>(3, 3) << 7.342274827447166e+02, 0.0, 6.544610847908701e+02, 0.0, 7.343520333235763e+02, 3.620389108671441e+02, 0.0, 0.0, 1.0),
            (Mat_<double>(3, 3) << 7.476554400662411e+02, 0.0, 6.788461062313610e+02, 0.0, 7.477630653570311e+02, 3.185225546149534e+02, 0.0, 0.0, 1.0)
    };

    vector<Mat> D_matrices = {
            (Mat_<double>(5, 1) << -0.012152688773890, 0.006312744290089, 0.0, 0.0, 0.0),
            (Mat_<double>(5, 1) << -0.004788988582043, 1.664519226194170e-04, 0.0, 0.0, 0.0),
            (Mat_<double>(5, 1) << -0.010742962922821, 0.004899856208959, 0.0, 0.0, 0.0),
            (Mat_<double>(5, 1) << -0.008567792387783, -0.003979577222784, 0.0, 0.0, 0.0),
            (Mat_<double>(5, 1) << -0.004199529851394, -0.006221124713178, 0.0, 0.0, 0.0)
    };

    vector<Mat> t_word_to_cam_matrices = {
            (Mat_<double>(4, 4) << -0.573083193216839, 0.819337857655761, 0.016159475995802, 0.004020654954081, -0.037263512116764, -0.006355334635993, -0.999285264769970, -5.940790067281352e-04, -0.818649549146101, -0.573275749298479, 0.034173541653635, -0.131669026218482, 0.0, 0.0, 0.0, 1.0),
            (Mat_<double>(4, 4) << 0.606118826106767, 0.795372432267145, -0.001631756232093, 0.015694798142177, 0.012875614383068, -0.011863195183812, -0.999846729831273, -0.045896562287801, -0.795269883242924, 0.606004916308009, -0.017431414667482, -0.110877525711608, 0.0, 0.0, 0.0, 1.0),
            (Mat_<double>(4, 4) << -0.945075320161107, -0.325701878485533, 0.027403021245412, 0.012344765860022, -0.014072099115867, -0.043215933241321, -0.998966645659681, -0.063750385501104, 0.326549560172502, -0.944484340508130, 0.036259002827830, -0.129189511868516, 0.0, 0.0, 0.0, 1.0),
            (Mat_<double>(4, 4) << 0.957103431035445, -0.289410872954947, -0.013941625286710, -0.010506759453144, -0.026517852059621, -0.039578790762524, -0.998864516760867, -0.028600625727241, 0.288530459089372, 0.956386358088211, -0.045555551148511, -0.153290341112109, 0.0, 0.0, 0.0, 1.0),
            (Mat_<double>(4, 4) << -0.058774247147796, -0.998268213079476, -0.002482463961589, -0.016783534106880, -0.004403582752182, 0.002746003285998, -0.999986533871781, -0.013253349162886, 0.998261587125917, -0.058762523950764, -0.004557350960968, -0.137347689399076, 0.0, 0.0, 0.0, 1.0)
    };

    for (int i = 0; i < 5; i++)
    {
        Mat img = imread(imgPaths[i]);
        if (img.empty()) {
            cout << "Failed to load image: " << imgPaths[i] << endl;
            continue;
        }
        if (img.channels() != 3) {
            cout << "RGB pics needed for image: " << imgPaths[i] << endl;
            continue;
        }
        projectPointsToImage(point_cloud, img, K_matrices[i], D_matrices[i], t_word_to_cam_matrices[i]);
    }

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
    viewer = rgbVis(point_cloud);

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
    }

    return 0;
}

结果

注：里面的数据来自MATLAB工具箱标定的数据

原文地址：https://blog.csdn.net/u014374826/article/details/142886083

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！

上一篇：基于Nodemcu的手机控制小车
下一篇：设计一个多环境配置管理系统的实用指南

点云数据与多相机图像融合实现3D场景的彩色可视化

引言

CMakeList.txt

main.cpp

结果

相关文章