FastACVNet_plus_Algorithm代码阅读笔记
最近实验室的任务推进到双目深度估计,之前我并没有接触过这方面的知识,更别说实际部署了,限于onnx模型推理速度不够快导致实际应用中帧率较低,难以提供较为鲁棒的点云信息和深度估计信息,因此必须要在原基础上实现推理加速。在Github上找到了一份 fork资源, 原资源是科科的博一学长(dlddw),同时要感谢另一位大佬将这份repo搞成了tensorrt加速版本。在此记录,若有侵权,必定删改。
1.main.cpp
a. 导入图片和onnx引擎(绝对路径,不然会出错)
char* stereo_calibration_path="/home/xyx/Downloads/StereoAlgorithms-main/FastACVNet_plus/test/StereoCalibration.yml";
char* strero_engine_path="/home/xyx/Downloads/ONNX-FastACVNet-Depth-Estimation-main/models/fast_acvnet_plus_generalization_opset16_480x640.onnx";
cv::Mat imageL=cv::imread("/home/xyx/Downloads/StereoAlgorithms-main/FastACVNet_plus/test/left0.jpg");
cv::Mat imageR=cv::imread("/home/xyx/Downloads/StereoAlgorithms-main/FastACVNet_plus/test/right0.jpg");
b. 初始化对象
//init
//void *是一种无类型指针,可以指向任何类型的数据,将这个指针赋值给一个通用的指针变量 fastacvnet,
//在处理一些特定的数据结构或对象时提供更大的灵活性
void * fastacvnet=Initialize(strero_engine_path,0,stereo_calibration_path);
c.由图像行和列生成点云,循环处理(不使用上一循环的副本图像)生成热力图和视差图
//x,y,z,r,g,b
float*pointcloud=new float[imageL.cols*imageL.rows*6];
cv::Mat disparity;
for (size_t i = 0; i < 1000;i++)
{
cv::Mat imageL1=imageL.clone();
cv::Mat imageR1=imageR.clone();
//auto start = std::chrono::system_clock::now();
//need RectifyImage
RunFastACVNet_plus_RectifyImage(fastacvnet,imageL1,imageR1,pointcloud,disparity);
//auto end = std::chrono::system_clock::now();
//std::cout<<"time:"<<(std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count())<<"ms"<<std::endl;
}
//disparity
cv::imwrite("/home/xyx/Downloads/disparity.jpg",disparity);
//heat map
cv::Mat Heap_map=heatmap(disparity);
cv::imwrite("/home/xyx/Downloads/heatmap.jpg",Heap_map);
std::cout<<"saved pics"<<std::endl;
cv::imshow("Heatmap", Heap_map);
cv::waitKey(0);
d. 保存点云信息并释放内存
//save pointcloud
std::fstream pointcloudtxt;
pointcloudtxt.open("/home/xyx/Downloads/pointcloud.txt",std::ios::out);
//循环遍历 pointcloud 数组,将点云数据以一定格式写入文本文件,每行包含了一个点的 x、y、z 以及颜色信息 r、g、b
for (size_t i = 0; i < imageL.cols*imageL.rows*6; i+=6)
{
pointcloudtxt<<pointcloud[i]<<" "<<pointcloud[i+1]<<" "<<pointcloud[i+2]<<" "
<<pointcloud[i+3]<<" "<<pointcloud[i+4]<<" "<<pointcloud[i+5]<<std::endl;
}
pointcloudtxt.close();
Release(fastacvnet);
delete []pointcloud;
pointcloud=nullptr;
return 0;
e.子函数生成热力图
cv::Mat heatmap(cv::Mat&disparity)
{
//max min
//生成单通道image_re
cv::Mat image_re = disparity.reshape(1);
double minValue, maxValue;
cv::Point minIdx, maxIdx;
//找到image_re中的最大最小值
cv::minMaxLoc(image_re, &minValue, &maxValue, &minIdx, &maxIdx);
//mean_mat:一个与disparity相同大小的矩阵
cv::Mat mean_mat(cv::Size(disparity.cols,disparity.rows), CV_32FC1, minValue);
//std_mat:用来归一化处理的矩阵,大小与disparity相同
cv::Mat std_mat(cv::Size(disparity.cols,disparity.rows), CV_32FC1, (maxValue-minValue)/255);
//归一化后的视差图
cv::Mat norm_disparity_map = (disparity - mean_mat) / std_mat;
cv::Mat heap_map,abs_map;
cv::convertScaleAbs(norm_disparity_map,abs_map,1);//abs_map是归一化后取绝对值的图像
cv::applyColorMap(abs_map,heap_map,cv::COLORMAP_JET);//将abs_map根据数值的大小映射为彩色图像
return heap_map;
}
2. FastACVNet_plus_Algorithm.cpp
a. 类定义
class FastACVNet_plus_Algorithm
{
private:
//私有成员变量,类型为 FastACVNet_plus
//是 FastACVNet_plus 类的一部分,用于实现其中的功能或算法。
FastACVNet_plus crestereo;
public:
FastACVNet_plus_Algorithm(/* args */);
~FastACVNet_plus_Algorithm();
int Initialize(char* model_path,int gpu_id,char*calibration_path);
int RunFastACVNet_plus(cv::Mat&left_image,cv::Mat&right_image,float*pointcloud,cv::Mat&disparity);
int RunFastACVNet_plus_RectifyImage(cv::Mat&left_image,cv::Mat&right_image,float*pointcloud,cv::Mat&disparity);
int Release();
void ReadObjectYml(const char* filename, CalibrationParam&Calibrationparam);
//重载成员函数
int RectifyImage(cv::Mat&Image_src,cv::Mat&rectifyImageL2,cv::Mat&rectifyImageR2);
int RectifyImage(cv::Mat&rectifyImageL2,cv::Mat&rectifyImageR2);
public:
CalibrationParam Calibrationparam;//标定类,在TRTFastACVNet_plus.h中定义
int initflag=-1;
};
//无参构造函数
FastACVNet_plus_Algorithm::FastACVNet_plus_Algorithm(/* args */)
{
}
FastACVNet_plus_Algorithm::~FastACVNet_plus_Algorithm()
{
}
b. 初始化函数
int FastACVNet_plus_Algorithm::Initialize(char* model_path,int gpu_id,char*calibration_path)
{
std::string model_path_str=model_path;
if(crestereo.file_exists(calibration_path))
{
ReadObjectYml(calibration_path,Calibrationparam);//读取标定数据,并在函数中对Calibrationparam赋值
}
else
{
std::cout<<"calibration_path does not exist!!!"<<std::endl;
}
//FastACVNet_plus类成员初始化,检查tensorrt的引擎文件是否需要生成并加载,绑定GPU内存,设置缓冲区,返回初始化flag
initflag=crestereo.Initialize(model_path_str,gpu_id,Calibrationparam);
if (initflag!=0)
{
std::cout<<"init failed!!"<<std::endl;
return -1;
}
initflag=0;
std::cout<<"init successed!"<<std::endl;
return 0;
}
c. 检验初始化情况并修正图像
int FastACVNet_plus_Algorithm::RunFastACVNet_plus_RectifyImage(cv::Mat&left_image,cv::Mat&right_image,float*pointcloud,cv::Mat&disparity)
{
if (initflag!=0)
{
std::cout<<"init failed,please check!!"<<std::endl;
return -1;
}
if (left_image.empty()||left_image.data==nullptr||right_image.empty()||right_image.data==nullptr)
{
std::cout<<"Image_src is empty!!!"<<std::endl;
return -1;
}
//修正图像
RectifyImage(left_image,right_image);
//利用tensorrt进行推理
crestereo.RunFastACVNet_plus(left_image,right_image,pointcloud,disparity);
return 0;
}
d. 释放在初始化和运行过程中所分配的 CUDA 内存以及 TensorRT 引擎等资源,以防止内存泄漏和资源浪费
int FastACVNet_plus_Algorithm::Release()
{
return crestereo.Release();
}
e. 读取标定数据
void FastACVNet_plus_Algorithm::ReadObjectYml(const char* filename, CalibrationParam&Calibrationparam)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
fs["intrinsic_left"] >> Calibrationparam.intrinsic_left;
fs["distCoeffs_left"] >> Calibrationparam.distCoeffs_left;
fs["intrinsic_right"] >> Calibrationparam.intrinsic_right;
fs["distCoeffs_right"] >> Calibrationparam.distCoeffs_right;
fs["R"] >> Calibrationparam.R;
fs["T"] >> Calibrationparam.T;
fs["R_L"] >> Calibrationparam.R_L;
fs["R_R"] >> Calibrationparam.R_R;
fs["P1"] >> Calibrationparam.P1;
fs["P2"] >> Calibrationparam.P2;
fs["Q"] >> Calibrationparam.Q;
fs.release();
return;
}
f.修正图像的两个重载函数
//区别只是在于有无左右两幅图像的信息,没有就调用前一个,前一个函数将一整张图像分割为左右两幅图像
int FastACVNet_plus_Algorithm::RectifyImage(cv::Mat&Image_src,cv::Mat&rectifyImageL2,cv::Mat&rectifyImageR2)
{
cv::Mat img_left, img_right;
//cv::Range(0,480) 指定了在垂直方向上截取图像的范围,从行索引 0 到 479(不包括 480),这代表了图像的上半部分
//cv::Range(0,640) 指定了在水平方向上截取图像的范围,从列索引 0 到 639(不包括 640),这代表了图像的左半部分
//Image_src(cv::Range(0,480),cv::Range(0,640)) 即提取了原始图像的左上部分,大小为 480x640
// .clone() 方法被调用,它创建了图像的一个副本。这个副本 img_left 包含了被裁剪部分的完整拷贝。这样做是为了
// 确保 img_left 是一个独立于原始图像的副本,而不是共享相同数据内存的视图或引用。
img_left = Image_src(cv::Range(0,480),cv::Range(0,640)).clone();
img_right= Image_src(cv::Range(0,480),cv::Range(640,1280)).clone();
//RectifyImage
cv::Size s1, s2;
s1 = img_left.size();
s2 = img_right.size();
cv::Mat mapLx, mapLy, mapRx, mapRy;
//通过initUndistortRectifyMap函数和读取到的畸变信息等来实现校正准备
cv::initUndistortRectifyMap(Calibrationparam.intrinsic_left, Calibrationparam.distCoeffs_left, Calibrationparam.R_L, Calibrationparam.P1, s1, CV_16SC2, mapLx, mapLy);
cv::initUndistortRectifyMap(Calibrationparam.intrinsic_right, Calibrationparam.distCoeffs_right, Calibrationparam.R_R, Calibrationparam.P2, s1, CV_16SC2, mapRx, mapRy);
//图像校正,通过之前得到的映射关系进行校正
cv::remap(img_left, rectifyImageL2, mapLx, mapLy, cv::INTER_LINEAR);
cv::remap(img_right, rectifyImageR2, mapRx, mapRy, cv::INTER_LINEAR);
return 0;
}
int FastACVNet_plus_Algorithm::RectifyImage(cv::Mat&rectifyImageL2,cv::Mat&rectifyImageR2)
{
//RectifyImage
cv::Size s1, s2;
s1 = rectifyImageL2.size();
s2 = rectifyImageR2.size();
cv::Mat mapLx, mapLy, mapRx, mapRy;
cv::initUndistortRectifyMap(Calibrationparam.intrinsic_left, Calibrationparam.distCoeffs_left, Calibrationparam.R_L, Calibrationparam.P1, s1, CV_16SC2, mapLx, mapLy);
cv::initUndistortRectifyMap(Calibrationparam.intrinsic_right, Calibrationparam.distCoeffs_right, Calibrationparam.R_R, Calibrationparam.P2, s1, CV_16SC2, mapRx, mapRy);
//cv::Mat rectifyImageL2, rectifyImageR2;
cv::remap(rectifyImageL2, rectifyImageL2, mapLx, mapLy, cv::INTER_LINEAR);
cv::remap(rectifyImageR2, rectifyImageR2, mapRx, mapRy, cv::INTER_LINEAR);
return 0;
}
g. 创建实例调用先前的初始化函数,最后返回实例的指针
void* Initialize(char* model_path,int gpu_id,char*calibration_path)
{
// 使用指针的原因是为了在堆上动态分配对象。这种动态分配的好处是,对象的生命周期可以控制得更加灵活,
// 可以在需要时手动释放内存,而不是像在栈上分配的对象那样在离开作用域时自动销毁
FastACVNet_plus_Algorithm*FastACVNet_plus_algorithm=new FastACVNet_plus_Algorithm();
FastACVNet_plus_algorithm->Initialize(model_path,gpu_id,calibration_path);
return FastACVNet_plus_algorithm;
}
3. TRTFastACVNet_plus.cpp
a. 初始化
int FastACVNet_plus::Initialize(std::string model_path,int gpu_id,CalibrationParam&Calibrationparam)
{
INPUT_H = 480;
INPUT_W = 640;
OUTPUT_SIZE= INPUT_H * INPUT_W;
//指定模型的节点输入和输出名称
INPUT_BLOB_NAME1 = "left_image";
INPUT_BLOB_NAME2 = "right_image";
OUTPUT_BLOB_NAME = "output";
// 更新类成员变量赋值
this->Calibrationparam=Calibrationparam;
//声明指向字符的指针
char* trtModelStream{nullptr};
// size_t 用于存储数组的索引、容器的大小、内存分配的字节数等,初始化size大小为0
size_t size{0};
std::string directory;
// const size_t last_slash_idx = model_path.rfind(".onnx");:查找字符串 model_path 中最后一个 .onnx 出现的位置
const size_t last_slash_idx=model_path.rfind(".onnx");
// 检查是否找到了onnx文件,如果找到了将截取字符串并将前缀存储在directory中
if (std::string::npos != last_slash_idx)
{
directory = model_path.substr(0, last_slash_idx);
}
std::string out_engine=directory+"_batch=1.engine";//根据之前的路径命名输出的tensorrt引擎名
bool enginemodel=file_exists(out_engine);//判断该路径下是否存在tensorrt引擎
//不存在引擎,先检验onnx_model是否真的存在(上一步并未给出onnx模型是否存在的判断),之后再生成tensorrt引擎并保存
if (!enginemodel)
{
std::cout << "Building engine, please wait for a while..." << std::endl;
bool onnx_model=file_exists(model_path);
if (!onnx_model)
{
std::cout<<"stereo.onnx does not Exist!!!Please Check!"<<std::endl;
return -1;
}
Onnx2Ttr onnx2trt;
//IHostMemory* modelStream{ nullptr };
onnx2trt.onnxToTRTModel(gLogger,model_path.c_str(),1,out_engine.c_str());
}
cudaSetDevice(gpu_id);
//读取一个tensorrt引擎文件,该文件包含了神经网络模型的序列化数据,用于后续在TensorRT中加载这个模型
std::ifstream file(out_engine, std::ios::binary);
if (file.good())
{
file.seekg(0, file.end);//文件读取指针移动到文件的末尾
size = file.tellg();//获取当前指针的位置,即可得到文件的大小并将其存储在size中
file.seekg(0, file.beg);//指针移回文件的开头
trtModelStream = new char[size];//使用new给trtModelStream分配size大小的内存空间
//"trtModelStream == nullptr" 是一个 C 风格的字符串,这个字符串在逻辑上永远为真(因为它是非空的)。
assert(trtModelStream && "trtModelStream == nullptr");//判定内存分配是否成功
file.read(trtModelStream, size);//从文件中读取 size 字节的数据,并将数据存储到 trtModelStream 所指向的内存块中。
file.close();//关闭文件流
}
else
{
return -1;
}
// init plugin
bool didInitPlugins = initLibNvInferPlugins(nullptr, "");//初始化NVIDIA推理插件
//创建推理环境和日志信息
runtime = createInferRuntime(gLogger);
assert(runtime != nullptr);
//从内存中的 trtModelStream 中反序列化得到 CUDA 引擎
engine = runtime->deserializeCudaEngine(trtModelStream, size);
assert(engine != nullptr);
// 创建CUDA引擎的执行上下文
context = engine->createExecutionContext();
assert(context != nullptr);
// 释放之前分配的 trtModelStream 内存,因为引擎已经被反序列化,不再需要原始的模型流数据。
delete[] trtModelStream;
// 检查引擎的绑定数量是否符合预期,通常是输入输出张量的数量。
assert(engine->getNbBindings() == 3);
// In order to bind the buffers, we need to know the names of the input and output tensors.
// Note that indices are guaranteed to be less than IEngine::getNbBindings()
// 获得输入输出张量的索引,在后续推断中需要
inputIndex1 = engine->getBindingIndex(INPUT_BLOB_NAME1);
inputIndex2 = engine->getBindingIndex(INPUT_BLOB_NAME2);
outputIndex = engine->getBindingIndex(OUTPUT_BLOB_NAME);
assert(inputIndex1 == 0);
assert(inputIndex2 == 1);
assert(outputIndex == 2);
// 为了执行推断,准备了一些额外的数据结构和内存,比如 PointCloud_devide、Q_device 和 flow_up
// 创建了一个 CUDA 流 stream,在 GPU 上执行的操作可以通过这个流来进行同步或异步执行
CUDA_CHECK(cudaStreamCreate(&stream));
// Create GPU buffers on device
// 分配一个大小为CRESTEREO_BATCH_SIZE * 3 * INPUT_H * INPUT_W * sizeof(float) 字节的 GPU 缓冲区,用于存储输入数据的索引
CUDA_CHECK(cudaMalloc((void**)&buffers[inputIndex1], CRESTEREO_BATCH_SIZE * 3 * INPUT_H * INPUT_W * sizeof(float)));
CUDA_CHECK(cudaMalloc((void**)&buffers[inputIndex2], CRESTEREO_BATCH_SIZE * 3 * INPUT_H * INPUT_W * sizeof(float)));
//
CUDA_CHECK(cudaMalloc((void**)&buffers[outputIndex], CRESTEREO_BATCH_SIZE * OUTPUT_SIZE * sizeof(float)));
// prepare input data cache in pinned memory
// 在主机CPU上分配针对输入数据的内存,提高传输效率
CUDA_CHECK(cudaMallocHost((void**)&img_left_host, INPUT_H * INPUT_W * 3));
CUDA_CHECK(cudaMallocHost((void**)&img_right_host, INPUT_H * INPUT_W * 3));
// prepare input data cache in device memory
// 为输入数据在 GPU 上分配了内存
CUDA_CHECK(cudaMalloc((void**)&img_left_device, INPUT_H * INPUT_W * 3));
CUDA_CHECK(cudaMalloc((void**)&img_right_device, INPUT_H * INPUT_W * 3));
// 为点云数据分配了 GPU 内存
CUDA_CHECK(cudaMalloc((void**)&PointCloud_devide, INPUT_H*INPUT_W*6*sizeof(float)));
// 为Q_device分配了 GPU 内存
CUDA_CHECK(cudaMalloc((void**)&Q_device,16*sizeof(float)));
cv::Matx44d _Q;
// 使用循环将this->Calibrationparam.Q 转换为 Calibrationparam_Q,将cv::Matx44d 类型转换为 float 数组类型,以便在 GPU 上使用
this->Calibrationparam.Q.convertTo(_Q, CV_64F);
cudaMallocManaged((void**)&Calibrationparam_Q,16*sizeof(float));
for (size_t i = 0; i <16; i++)
{
Calibrationparam_Q[i]=(float)_Q.val[i];
}
//disparity
flow_up=new float[CRESTEREO_BATCH_SIZE * OUTPUT_SIZE];
return 0;
}
b. 执行推理
int FastACVNet_plus::RunFastACVNet_plus(cv::Mat&rectifyImageL2,cv::Mat&rectifyImageR2,float*pointcloud,cv::Mat&DisparityMap)
{
assert((INPUT_H == rectifyImageL2.rows) && (INPUT_H == rectifyImageR2.rows));
assert((INPUT_W == rectifyImageL2.cols) && (INPUT_W == rectifyImageR2.cols));
auto start = std::chrono::system_clock::now();
//copy data to pinned memory
// 将左右校正后的图像数据复制到固定内存(pinned memory) img_left_host 和 img_right_host 中。这些数据将在主机(CPU)和设备(GPU)之间进行传输。
memcpy(img_left_host,rectifyImageL2.data, 3*INPUT_H*INPUT_W*sizeof(uint8_t));
memcpy(img_right_host,rectifyImageR2.data, 3*INPUT_H*INPUT_W*sizeof(uint8_t));
//copy data to device memory
// 将数据从主机复制到设备端,使用了异步的方式进行传输,可以更好地实现数据并行传输
CUDA_CHECK(cudaMemcpyAsync(img_left_device, img_left_host, 3*INPUT_H*INPUT_W*sizeof(uint8_t), cudaMemcpyHostToDevice,stream));
CUDA_CHECK(cudaMemcpyAsync(img_right_device, img_right_host, 3*INPUT_H*INPUT_W*sizeof(uint8_t), cudaMemcpyHostToDevice,stream));
// 图像数据进行预处理,将数据加载到 TensorRT 的输入缓冲区 buffers[inputIndex1] 和 buffers[inputIndex2] 中
FastACVNet_plus_preprocess(img_left_device, buffers[inputIndex1], INPUT_W, INPUT_H, stream);
FastACVNet_plus_preprocess(img_right_device, buffers[inputIndex2], INPUT_W, INPUT_H, stream);
// Run inference
//(*context).enqueue(CRESTEREO_BATCH_SIZE, (void**)buffers, stream, nullptr);
//推理!!!
(*context).enqueueV2((void**)buffers, stream, nullptr);
// 等待 CUDA 流的同步,确保了在将结果从设备端复制回主机端之前,模型执行已经完成
cudaStreamSynchronize(stream);
// 模型输出作后处理,深度图转换成点云
FastACVNet_plus_reprojectImageTo3D(img_left_device,buffers[2],PointCloud_devide,Calibrationparam_Q,INPUT_H,INPUT_W);
// 将结果从设备端复制到主机端,复制了buffers[2]到flow_up和点云数据PointCloud_devide
CUDA_CHECK(cudaMemcpy(flow_up, buffers[2], CRESTEREO_BATCH_SIZE * OUTPUT_SIZE * sizeof(float), cudaMemcpyDeviceToHost));
CUDA_CHECK(cudaMemcpy(pointcloud,PointCloud_devide,INPUT_H*INPUT_W*6*sizeof(float),cudaMemcpyDeviceToHost));
auto end = std::chrono::system_clock::now();
std::cout<<"inference time:"<<(std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count())<<"ms"<<std::endl;
cv::Mat DisparityImage(INPUT_H,INPUT_W, CV_32FC1,flow_up);
DisparityMap=DisparityImage.clone();
return 0;
}
c. 内存释放
int FastACVNet_plus::Release()
{
cudaStreamDestroy(stream);
CUDA_CHECK(cudaFreeHost(img_left_host));
CUDA_CHECK(cudaFreeHost(img_right_host));
CUDA_CHECK(cudaFree(img_left_device));
CUDA_CHECK(cudaFree(img_right_device));
CUDA_CHECK(cudaFree(buffers[inputIndex1]));
CUDA_CHECK(cudaFree(buffers[inputIndex2]));
CUDA_CHECK(cudaFree(buffers[outputIndex]));
CUDA_CHECK(cudaFree(PointCloud_devide));
CUDA_CHECK(cudaFree(Q_device));
CUDA_CHECK(cudaFree(Calibrationparam_Q));
context->destroy();
engine->destroy();
runtime->destroy();
delete []flow_up;
flow_up=NULL;
return 0;
}
4. FastACVNet_plus_preprocess.cu
a. 预处理kernel
__global__ void FastACVNet_plus_preprocess_kernel(uint8_t* src, int src_line_size, int src_width, int src_height, float* dst, int edge)
{
// blockDim.x: 表示一个线程块中的线程数量;blockIdx.x: 表示当前线程块在网格中的索引;threadIdx.x: 表示线程在其所属的线程块内的索引
// positon:确定每个线程需要处理的数据或执行的任务
int position = blockDim.x * blockIdx.x + threadIdx.x;
if (position >= edge)
return;
// 根据线性的 position 计算图像中的 (x, y) 位置
int dx = position % src_width;// 线程在图像中的水平位置(列数)
int dy = position / src_width;// 线程在图像中的垂直位置(行数)
float c0, c1, c2;
uint8_t* v = src + dy * src_line_size + dx * 3;//利用dx dy计算当前像素在数据缓冲区中的位置,乘3来处理每个像素由三个分量(BGR)组成情况
// 分别代表BGR通道的值
c0 = v[0];
c1 = v[1];
c2 = v[2];
//bgr to rgb
float t = c2;
c2 = c0;
c0 = t;
//normalization
c0 = (c0 / 255.0f - 0.485) / 0.229;
c1 = (c1 / 255.0f - 0.456) / 0.224;
c2 = (c2 / 255.0f - 0.406) / 0.225;
//rgbrgbrgb to rrrgggbbb
// 像素数据重新排列:计算图像总共有多少像素,使用三个指针把归一化后的数值存储到dst目标缓冲区中
int area = src_width * src_height;
// 分别计算RBG三个通道值的存储位置,再把之前得到的归一化数值赋值到对应位置
float* pdst_c0 = dst + dy * src_width + dx;
float* pdst_c1 = pdst_c0 + area;
float* pdst_c2 = pdst_c1 + area;
*pdst_c0 = c0;
*pdst_c1 = c1;
*pdst_c2 = c2;
}
b. 视差图转换成三维点云
__global__ void FastACVNet_plus_reprojectImageTo3D_kernel(uint8_t* left_img,float*disparity,float*pointcloud,float*Q_device,int disparity_rows,int disparity_cols)
{
// 计算线程索引
int tid=blockIdx.x*blockDim.x+threadIdx.x;
// 判断当前线程处理的像素点是否在合理范围内,超出视差图尺寸的线程将被忽略
if(tid>=disparity_cols*disparity_rows)
{
return;
}
// 计算像素在视差图中的行和列
int col=tid%disparity_cols;
int row=tid/disparity_cols;
// 定位左视图图像数据中的当前像素位置,并获取其RGB值
uint8_t* v = left_img + row * disparity_cols*3 + col * 3;
//Q*disparity
// 使用 Q_device 矩阵将视差值转换为三维点云坐标。这个操作利用了 Q_device 矩阵的特定计算方式,将视差转换为点云的X、Y、Z坐标。
float w=Q_device[14]*disparity[row*disparity_cols+col];
for (size_t i = 0; i < 3; i++)
{
// 将转换后的三维坐标存储到pointcloud数组中,此处存储XYZ值
pointcloud[(row*disparity_cols+col)*6+i]=(Q_device[i*4]*col+Q_device[i*4+1]*row+Q_device[i*4+3]*1)/w;
}
// 存储左视图RGB值
pointcloud[(row*disparity_cols+col)*6+3]=(float)v[2];
pointcloud[(row*disparity_cols+col)*6+4]=(float)v[1];
pointcloud[(row*disparity_cols+col)*6+5]=(float)v[0];
}
c. 图像预处理
// 调用了名为FastACVNet_plus_preprocess_kernel的CUDA内核函数执行实际的图像预处理操作
void FastACVNet_plus_preprocess(uint8_t* src, float* dst, int src_width, int src_height, cudaStream_t stream)
{
int jobs = src_width * src_height;//像素总数
int threads = 256;// 指定每个线程块的线程数量和总的线程块数量
int blocks = (jobs +threads-1)/threads;
// 调用内核函数,传递图像数据、宽高、目标缓冲区dst,以及得到的jobs值
FastACVNet_plus_preprocess_kernel<<<blocks,threads, 0, stream>>>(src, src_width*3, src_width, src_height, dst, jobs);
}
d. 视差图重投影
void FastACVNet_plus_reprojectImageTo3D(uint8_t* left_img,float*disparity,float*pointcloud,float*Q_device,int disparity_rows,int disparity_cols)
{
int jobs=disparity_rows*disparity_cols;
int threads=256;
int blocks=(jobs+threads-1)/threads;
FastACVNet_plus_reprojectImageTo3D_kernel<<<blocks,threads>>>(left_img,disparity,pointcloud,Q_device,disparity_rows,disparity_cols);
}