增量式：利用深度图生成密集点云的简单代码

该程序主要包括三个文件，分别是main.cpp，source.cpp，source.h。

main.cpp:

#include <iostream>
#include "source.h"
#include <opencv2/opencv.hpp>
#include <fstream>

int main() {
    std::string folder_path = "../Depth_images/";

    // 调用载入深度图的函数，返回加载的深度图
    std::vector<cv::Mat> depth_images = load_all_depth_images(folder_path);

    // 输出加载的深度图数量
    std::cout << "总共加载了 " << depth_images.size() << " 张深度图。" << std::endl;

    // 相机内参
    float fx = 525.0;  // 焦距 x
    float fy = 525.0;  // 焦距 y
    float cx = 319.5;  // 光轴中心 x
    float cy = 239.5;  // 光轴中心 y

    // 创建一个空的总点云容器
    std::vector<cv::Point3f> total_point_cloud;

    // 增量式处理每一张深度图
    for (size_t i = 0; i < depth_images.size(); ++i) {
        std::cout << "正在处理第 " << (i + 1) << " 张深度图..." << std::endl;
        // 将当前深度图增量式转换为点云并追加到总点云中
        add_depth_to_point_cloud(depth_images[i], total_point_cloud, fx, fy, cx, cy);
    }

    // 保存总点云为PLY文件
    save_point_cloud_as_ply(total_point_cloud, "Point_Cloud_Test.ply");

    return 0;
}

source.h:

#ifndef SOURCE_H
#define SOURCE_H

#include <opencv2/opencv.hpp>
#include <vector>
#include <string>

// 声明加载深度图的函数
std::vector<cv::Mat> load_all_depth_images(const std::string& folder_path);

// 声明将深度图转换为3D点云的函数
std::vector<cv::Point3f> depth_to_point_cloud(const cv::Mat& depth_map, float fx, float fy, float cx, float cy);


//声明保存3D点云为PLY格式的函数
void save_point_cloud_as_ply(const std::vector<cv::Point3f>& point_cloud, const std::string& file_path);


//声明将单张深度图增量式转换为3D点云并追加到总点云的函数
void add_depth_to_point_cloud(const cv::Mat& depth_map, std::vector<cv::Point3f>& total_point_cloud, float fx, float fy, float cx, float cy);

#endif  // SOURCE_H

source.cpp:

#include "source.h"
#include <iostream>
#include <filesystem>
#include <fstream>
namespace fs = std::filesystem;

// 实现加载所有深度图的函数
std::vector<cv::Mat> load_all_depth_images(const std::string& folder_path) {
    std::vector<cv::Mat> depth_images;

    // 检查文件夹是否存在
    if (!fs::exists(folder_path) || !fs::is_directory(folder_path)) {
        std::cerr << "文件夹不存在或不是文件夹: " << folder_path << std::endl;
        return depth_images;  // 返回空的 vector
    }

    // 遍历文件夹中的每个文件
    for (const auto& entry : fs::directory_iterator(folder_path)) {
        const std::string file_path = entry.path().string();

        // 只处理 PNG 格式的图像
        if (entry.path().extension() == ".png") {
            // 加载深度图
            cv::Mat depth_map = cv::imread(file_path, cv::IMREAD_UNCHANGED);

            if (depth_map.empty()) {
                std::cerr << "无法加载图像: " << file_path << std::endl;
                continue;
            }

            // 将加载的深度图加入到 vector 中
            depth_images.push_back(depth_map);

            std::cout << "成功加载图像: " << file_path << std::endl;
        }
    }

    return depth_images;  // 返回包含所有深度图的 vector
}



// 实现将深度图转换为3D点云的函数
std::vector<cv::Point3f> depth_to_point_cloud(const cv::Mat& depth_map, float fx, float fy, float cx, float cy) {
    std::vector<cv::Point3f> point_cloud;

    for (int v = 0; v < depth_map.rows; ++v) {
        for (int u = 0; u < depth_map.cols; ++u) {
            float Z = depth_map.at<ushort>(v, u);  // 假设深度图是16位无符号整数

            if (Z == 0) continue;  // 跳过无效深度

            // 使用相机内参将深度图像素转换为3D点
            float X = (u - cx) * Z / fx;
            float Y = (v - cy) * Z / fy;

            point_cloud.emplace_back(X, Y, Z);
        }
    }

    return point_cloud;
}







// 实现保存3D点云为PLY格式的函数
void save_point_cloud_as_ply(const std::vector<cv::Point3f>& point_cloud, const std::string& file_path) {
    std::ofstream ofs(file_path);
    std::cout << "开始生成点云ply！ " << file_path << std::endl;
    if (!ofs) {
        std::cerr << "无法打开文件: " << file_path << std::endl;
        return;
    }

    // 写入PLY头信息
    ofs << "ply\n";
    ofs << "format ascii 1.0\n";
    ofs << "element vertex " << point_cloud.size() << "\n";
    ofs << "property float x\n";
    ofs << "property float y\n";
    ofs << "property float z\n";
    ofs << "end_header\n";

    // 写入点云数据
    for (const auto& point : point_cloud) {
        ofs << point.x << " " << point.y << " " << point.z << "\n";
    }

    ofs.close();
    std::cout << "点云已保存为: " << file_path << std::endl;
}



//实现 将单张深度图增量式转换为3D点云并追加到总点云的函数
void add_depth_to_point_cloud(const cv::Mat& depth_map, std::vector<cv::Point3f>& total_point_cloud, float fx, float fy, float cx, float cy) {
    for (int v = 0; v < depth_map.rows; ++v) {
        for (int u = 0; u < depth_map.cols; ++u) {
            float Z = depth_map.at<ushort>(v, u);  // 假设深度图是16位无符号整数

            if (Z == 0) continue;  // 跳过无效深度

            // 使用相机内参将深度图像素转换为3D点
            float X = (u - cx) * Z / fx;
            float Y = (v - cy) * Z / fy;

            total_point_cloud.emplace_back(X, Y, Z);  // 将3D点添加到总点云中
        }
    }
}