#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include "gdal/gdal_priv.h"
#pragma comment(lib, "gdal_i.lib")
#include <filesystem>
#include<tuple>
#include <gdal.h>
#include <gdal_priv.h>
#include <cpl_conv.h>
#include <boost/filesystem.hpp>
#include "gdal_priv.h"
#include "cpl_conv.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
using namespace std;
using namespace cv;
// Function to read data from a txt file
and store it in a matrix
bool readDataFromFile(const string& filename, Mat& dataMat) {
ifstream file(filename);
if (!file.is_open()) {
cout << "Error: Could not open the file " << filename << endl;
return false;
}
dataMat.create(3, 4, CV_32FC1);
for (int i = 0; i < dataMat.rows; ++i) {
for (int j = 0; j < dataMat.cols; ++j) {
float value;
if (!(file >> value)) {
cout << "Error: Could not read data properly from the file " << filename << endl;
return false;
}
dataMat.at<float>(i, j) = value;
}
}
file.close();
return true;
}
// Function to convert disparity map to
depth map
Mat disparityTo
Depth(cv::Mat disImg, float baselineLength, float focalLength) {
int width = disImg.cols;
int height = disImg.rows;
std::cout << "distance:" << width << height << std::endl;
Mat
depthImage = Mat::zeros(height, width, CV_32FC1);
std::cout << "断点" << " 6 " << std::endl;
for (int row = 0; row < height; ++row)
{
// std::cout << "断点" << " 7 " << std::endl;
for (int col = 0; col < width; ++col)
{
float distance = 0;
distance = disImg.at<float>(row, col);
// std::cout << "distance:" << distance << std::endl;
if (distance != 0.0)
{
depthImage.at<float>(row, col) = (baselineLength * focalLength ) / distance;
}
else
{
continue;
}
}
}
return
depthImage; // 返回
深度图
}
std::vector<Mat> readEplimg(const std::string& folderPath)
{
GDALAllRegister();
CPLSetConfigOption("GDAL_FILENAME_IS_UTF8", "NO"); //设置支持中文路径
和文件名
std::vector<Mat> Eplimg;
if (!boost::filesystem::exists(folderPath) || !boost::filesystem::is_directory(folderPath)) {
std::cerr << "Error: " << folderPath << " is not a valid directory." << std::endl;
return Eplimg;
}
// 遍历文件夹中的每个文件
boost::filesystem::directory_iterator end_itr;
for (boost::filesystem::directory_iterator itr(folderPath); itr != end_itr; ++itr) {
if (boost::filesystem::is_regular_file(itr->status()) &&
(itr->path().extension() == ".tiff" || itr->path().extension() == ".tif")) {
std::string filePath = itr->path().string();
// 打开tiff文件
GDALDataset* poDataset = (GDALDataset*)GDALOpenShared(filePath.c_str(), GA_ReadOnly);
if (poDataset == nullptr) {
std::cerr << "Error opening
image " << filePath << std::endl;
continue;
}
// 获取图像宽度
和高度
int width = poDataset->GetRasterXSize();
int height = poDataset->GetRasterYSize();
// 获取波段数量
int numB
ands = poDataset->GetRasterCount();
// 读取图像数据
cv::Mat
image(height, width, CV_16U);
//poDataset->RasterIO(GF_Read, 0, 0, width, height,
image.data, width, height, GDT_UInt16, 0, 0);
GDALRasterB
and* poB
and = poDataset->GetRasterB
and(1);
poB
and->RasterIO(GF_Read, 0, 0, width, height,
image.data, width, height, GDT_UInt16, 0, 0);
// 关闭文件
GDALClose(poDataset);
// 将图像添加到向量中
Eplimg.push_back(
image);
}
}
return Eplimg;
}
cv::Mat readDisparity
Image(const std::string& filePath) {
GDALAllRegister();
// 打开 TIFF 文件
GDALDataset *disparityDataset = (GDALDataset *)GDALOpen(filePath.c_str(), GA_ReadOnly);
if (disparityDataset == NULL) {
std::cerr << "无法打开图像文件！" << std::endl;
return cv::Mat();
}
// 获取图像宽度
和高度
int width = disparityDataset->GetRasterXSize();
int height = disparityDataset->GetRasterYSize();
std::cout << "图像尺寸：" << width << " x " << height << std::endl;
// 获取第一个波段
// GDALRasterB
and *poB
and = disparityDataset->GetRasterB
and(1);
// 获取波段数量
int numB
ands = disparityDataset->GetRasterCount();
// 读取图像数据
cv::Mat
image(height, width, CV_32FC1);
GDALRasterB
and* poB
and = disparityDataset->GetRasterB
and(1);
GDALDataType g_type = poB
and->GetRasterDataType();
// std::cout << "断点" << width << " 1 " << height << std::endl;
// float *data = new float [width * height];
// std::cout << "断点" << width << " 2 " << height << std::endl;
// poB
and->RasterIO(GF_Read, 0, 0, width, height, data, width, height, GDT_Float64, 0, 0);
std::cout << "断点" << width << " 3 " << height << std::endl;
// 将数据转换为 OpenCV 的 cv::Mat 格式
// cv::Mat img(height, width, CV_64F, data);
disparityDataset->GetRasterB
and(1)->RasterIO(GF_Read, 0, 0, width, height,
image.data, width, height, g_type, 0, 0);
poB
and->RasterIO(GF_Read, 0, 0, width, height,
image.data, width, height, g_type, 0,0);
std::cout << "断点" << width << " 4 " << height << std::endl;
// 释放内存
// delete[] data;
GDALClose(disparityDataset);
return
image;
}
std::vector<cv::Mat> readColor
Images(const std::string& folderPath)
{
GDALAllRegister();
CPLSetConfigOption("GDAL_FILENAME_IS_UTF8", "NO"); //设置支持中文路径
和文件名
std::vector<cv::Mat> colorImg;
if (!boost::filesystem::exists(folderPath) || !boost::filesystem::is_directory(folderPath)) {
std::cerr << "Error: " << folderPath << " is not a valid directory." << std::endl;
return colorImg;
}
boost::filesystem::directory_iterator end_itr;
for (boost::filesystem::directory_iterator itr(folderPath); itr != end_itr; ++itr) {
if (boost::filesystem::is_regular_file(itr->status()) &&
(itr->path().extension() == ".tiff" || itr->path().extension() == ".tif")) {
std::string filePath = itr->path().string();
GDALDataset* poDataset = (GDALDataset*)GDALOpenShared(filePath.c_str(), GA_ReadOnly);
if (poDataset == nullptr) {
std::cerr << "Error opening
image " << filePath << std::endl;
continue;
}
int width = poDataset->GetRasterXSize();
int height = poDataset->GetRasterYSize();
int numB
ands = poDataset->GetRasterCount();
// 创建一个3通道的Mat矩阵
cv::Mat
image(height, width, CV_8UC3);
// 逐波段读取数据并存储到Mat矩阵中
for (int b = 1; b <= std::min(numB
ands, 3); ++b) { // 限制最多读取前3个波段
GDALRasterB
and* poB
and = poDataset->GetRasterB
and(b);
poB
and->RasterIO(GF_Read, 0, 0, width, height,
image.data + (b - 1), width, height, GDT_Byte, 3, width * 3);
}
// 关闭文件
GDALClose(poDataset);
// 将图像添加到向量中
cv::cvtColor(
image,
image, cv::COLOR_RGB2BGR); // 转换为BGR格式
colorImg.push_back(
image);
}
}
return colorImg;
}
void save
DepthImageWithGDAL(const cv::Mat&
depthImage, const std::string& outputPath) {
// 初始化 GDAL 库
GDALAllRegister();
// 获取
深度图像的尺寸信息
int width =
depthImage.cols;
int height =
depthImage.rows;
// 创建输出文件
GDALDriver* driver = GetGDALDriverManager()->GetDriverByName("GTiff");
if (driver == nullptr) {
std::cerr << "Error: Could not get GDAL driver." << std::endl;
return;
}
// 创建输出数据集
GDALDataset* dataset = driver->Create(outputPath.c_str(), width, height, 1, GDT_Float32, NULL);
if (dataset == nullptr) {
std::cerr << "Error: Could not create GDAL dataset." << std::endl;
return;
}
// 将
深度图像数据写入到数据集中
float* data = (float*)CPLMalloc(sizeof(float) * width * height);
for (int i = 0; i < height; ++i) {
for (int j = 0; j < width; ++j) {
data[i * width + j] =
depthImage.at<float>(i, j);
}
}
GDALRasterB
and* b
and = dataset->GetRasterB
and(1);
b
and->RasterIO(GF_Write, 0, 0, width, height, data, width, height, GDT_Float32, 0, 0);
// 释放内存
和关闭数据集
CPLFree(data);
GDALClose(dataset);
}
void
depthToCloudPoint(std::vector<Mat>color
Image,Mat
depthImage ,float focalLength_x,float focalLength_y)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::cout << "size:" <<
depthImage.rows << "x" <<
depthImage.cols << std::endl;
for (int y = 0; y <
depthImage.rows; ++y) {
for (int x = 0; x <
depthImage.cols; ++x) {
if (y >= 0 && y <
depthImage.rows && x >= 0 && x <
depthImage.cols) {
// 获取深度值
float
depth =
depthImage.at <float>(y, x);
// 如果深度值为0（无效值），跳过
if (
depth == 0.0f)
continue;
// 计算点云位置
pcl::PointXYZRGB point;
point.z =
depth;
point.x = (x - focalLength_x) *
depth / focalLength_x;
point.y = (y - focalLength_y) *
depth / focalLength_y;
std::cout << "size:" << "9" << std::endl;
//std::cout << "color:" << color
Image << std::endl;
// 获取相应位置的彩色像素值作为颜色
cv::Vec3b color = color
Image[0].at<cv::Vec3b>(y, x);
std::cout << "color:" << color << std::endl;
point.r = color[2];
point.g = color[1];
point.b = color[0];
std::cout << "color:" << color << std::endl;
// 将点添加到点云中
cloud->push_back(point);
}
}
}
pcl::io::savePLYFile("pointcloud.ply", *cloud);
pcl::io::savePCDFileBinary("point_cloud.pcd", *cloud);
pcl::io::savePLYFileBinary("point_cloud.ply", *cloud);
std::cout << "Point cloud saved to point_cloud.ply" << std::endl;
}
int main() {
std::vector<Mat> Eplimg = readEplimg("/home/tl/3rdparty/homeworks/build/EpiImg");
std::vector<Mat> color
Image = readColor
Images("/home/tl/3rdparty/homeworks/build/EpiImg");
cv::Mat disImg = readDisparity
Image("/home/tl/3rdparty/homeworks/build/017ER030_017ER031_DISP_ET0.tif");
// 检查是否成功读取
if (disImg.empty()) {
std::cerr << "图像读取失败！" << std::endl;
}
Mat dataMatleft, dataMatright;
if (!readDataFromFile("017ER030_REC.txt", dataMatleft) || !readDataFromFile("017ER031_REC.txt", dataMatright)) {
return -1;
}
Mat K1, R1, t1, K2, R2, t2;
decomposeProjectionMatrix(dataMatleft, K1, R1, t1);
decomposeProjectionMatrix(dataMatright, K2, R2, t2);
Mat B = t1 - t2;
float baseline_length = norm(B);
float focalLength_x = K1.at<float>(0, 0); // 从相机内参矩阵K1中获取水平方向的焦距
cout << "基线长度: " << baseline_length << endl;
cout << "焦距: " << focalLength_x << endl;
std::cout << "断点" << " 5 " << std::endl;
// 或者
float focalLength_y = K1.at<float>(1, 1); // 从相机内参矩阵K1中获取垂直方向的焦距
Mat
depthImage = disparityTo
Depth(disImg, baseline_length, focalLength_x); // 调用 disparityTo
Depth 函数获取
深度图
// 设置输出路径
std::string outputPath = "output_
depth_
image.tif";
// 将
深度图像保存为图片文件
save
DepthImageWithGDAL(
depthImage, outputPath);
// 调用函数进行
深度图像到点云的转换
depthToCloudPoint(color
Image,
depthImage, focalLength_x, focalLength_y);
cout << "基线长度: " << baseline_length << endl;
cout << "相机内参矩阵 K:" << endl << K1 << endl;
cout << "旋转矩阵 R:" << endl << R1 << endl;
cout << "平移向量 t:" << endl << t1 << endl;
cout << "Data matrix 1:" << endl << dataMatleft << endl;
cout << "Data matrix 2:" << endl << dataMatright << endl;
return 0;
}把这个改为只用opencv库就能实现的代码
本文深入探讨了高度图和深度图的概念及其在地形创建和三维空间中的应用。高度图作为一个二维数组，用于定义地形网格的高度，而深度图则记录了相机到场景中各点的距离，反映了景物表面的几何形状。
扫一扫
789
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
