|
在一段关于两个摄像头拍摄的图片互相映射代码中,调试时发现出现问题如下:
0x00358B53 处有未经处理的异常(在 kinect_registration.exe 中): 0xC0000005: 读取位置 0x00000000 时发生访问冲突。
代码如下:
单步调试时在标红处出现的异常。(调试时,txt中的文档中数值在变量 fx_rgb、fy_rgb中都有值并且正确,但同时也显示parm获取文本错误)
#include <opencv2\core\core.hpp>
#include <opencv2\highgui\highgui.hpp>
#include <fstream>
#include <iostream>
#include <sstream>
#include <Eigen/Core>
#include <Eigen/LU>
#include <thread>
using namespace cv;
using namespace std;
struct KinectParm
{
float fx_rgb;
float fy_rgb;
float cx_rgb;
float cy_rgb;
float fx_ir;
float fy_ir;
float cx_ir;
float cy_ir;
Eigen::Matrix3f R_ir2rgb;
Eigen::Vector3f T_ir2rgb;
};
bool loadParm(KinectParm* kinectParm)
{
// 加载参数
ifstream parm("D:\\study\\Graduation\\code\\kinect_registration\\kinect_registration\\registration.txt");
string stringLine;
if (parm.is_open())
{
// rgb相机参数:fx,fy,cx,cy
getline(parm, stringLine);
stringstream lin(stringLine);
string s1, s2, s3, s4, s5, s6, s7, s8, s9;
lin >> s1 >> s2 >> s3 >> s4;
kinectParm->fx_rgb = atof(s1.c_str());
kinectParm->fy_rgb = atof(s2.c_str());
kinectParm->cx_rgb = atof(s3.c_str());
kinectParm->cy_rgb = atof(s4.c_str());
stringLine.clear();
// ir相机参数:fx,fy,cx,cy
getline(parm, stringLine);
stringstream lin2(stringLine);
lin2 << stringLine;
lin2 >> s1 >> s2 >> s3 >> s4;
kinectParm->fx_ir = atof(s1.c_str());
kinectParm->fy_ir = atof(s2.c_str());
kinectParm->cx_ir = atof(s3.c_str());
kinectParm->cy_ir = atof(s4.c_str());
stringLine.clear();
// R_ir2rgb
getline(parm, stringLine);
stringstream lin3(stringLine);
lin3 << stringLine;
lin3 >> s1 >> s2 >> s3 >> s4 >> s5 >> s6 >> s7 >> s8 >> s9;
kinectParm->R_ir2rgb <<
atof(s1.c_str()), atof(s2.c_str()), atof(s3.c_str()),
atof(s4.c_str()), atof(s5.c_str()), atof(s6.c_str()),
atof(s7.c_str()), atof(s8.c_str()), atof(s9.c_str());
stringLine.clear();
// T_ir2rgb
getline(parm, stringLine);
stringstream lin4(stringLine);
lin4 << stringLine;
lin4 >> s1 >> s2 >> s3;
kinectParm->T_ir2rgb << atof(s1.c_str()), atof(s2.c_str()), atof(s3.c_str());
}
else
{
cout << "parm.txt not right!!!";
return false;
}
cout << "******************************************" << endl;
cout << "fx_rgb: " << kinectParm->fx_rgb << endl;
cout << "fy_rgb: " << kinectParm->fy_rgb << endl;
cout << "cx_rgb: " << kinectParm->cx_rgb << endl;
cout << "cy_rgb: " << kinectParm->cy_rgb << endl;
cout << "******************************************" << endl;
cout << "fx_ir: " << kinectParm->fx_ir << endl;
cout << "fy_ir: " << kinectParm->fy_ir << endl;
cout << "cx_ir: " << kinectParm->cx_ir << endl;
cout << "cy_ir: " << kinectParm->cy_ir << endl;
cout << "******************************************" << endl;
cout << "R_ir2rgb:" << endl << kinectParm->R_ir2rgb << endl;
cout << "******************************************" << endl;
cout << "T_ir2rgb:" << endl << kinectParm->T_ir2rgb << endl;
cout << "******************************************" << endl;
return true;
}
int main()
{
// 1. 读取参数
KinectParm *parm = new KinectParm();
if (!loadParm(parm))
return 0;
// 2. 载入rgb图片和深度图并显示
Mat bgr(1080, 1920, CV_8UC4);
bgr = imread("Color.jpg");
Mat depth(424, 512, CV_16UC1);
depth = imread("Depth.png", IMREAD_ANYDEPTH); // 图片读入后的格式不一定和定义时候的一样,比如这里读入后的格式就是8UC3
//Mat depth2rgb = imread("Depth2rgb.jpg");
// 3. 显示
thread th = std::thread([&]{
while (true)
{
imshow("原始彩色图", bgr);
waitKey(1);
imshow("原始深度图", depth);
waitKey(1);
//imshow("原始投影图", depth2rgb);
//waitKey(1);
}
});
// 4. 变换
// 4.1 计算各个矩阵
#pragma region 非齐次
Eigen::Matrix3f K_ir; // ir内参矩阵
K_ir <<
parm->fx_ir, 0, parm->cx_ir,
0, parm->fy_ir, parm->cy_ir,
0, 0, 1;
Eigen::Matrix3f K_rgb; // rgb内参矩阵
K_rgb <<
parm->fx_rgb, 0, parm->cx_rgb,
0, parm->fy_rgb, parm->cy_rgb,
0, 0, 1;
Eigen::Matrix3f R;
Eigen::Vector3f T;
R = K_rgb*parm->R_ir2rgb*K_ir.inverse();
T = K_rgb*parm->T_ir2rgb;
cout << "K_rgb:\n" << K_rgb << endl;
cout << "K_ir:\n" << K_ir << endl;
cout << "R:\n" << R << endl;
cout << "T:\n" << T << endl;
cout << depth.type() << endl;
// 4.2 计算投影
Mat result(424, 512, CV_8UC3);
int i = 0;
for (int row = 0; row < 424; row++)
{
for (int col = 0; col < 512; col++)
{
unsigned short* p = (unsigned short*)depth.data;
unsigned short depthValue = p[row * 512 + col];
//cout << "depthValue " << depthValue << endl;
if (depthValue != -std::numeric_limits<unsigned short>::infinity() && depthValue != -std::numeric_limits<unsigned short>::infinity() && depthValue != 0 && depthValue != 65535)
{
// 投影到彩色图上的坐标
Eigen::Vector3f uv_depth(col, row, 1.0f); // !!!p_ir
Eigen::Vector3f uv_color = depthValue / 1000.f*R*uv_depth + T / 1000; // !!!Z_rgb*p_rgb=R*Z_ir*p_ir+T; (除以1000,是为了从毫米变米)
int X = static_cast<int>(uv_color[0] / uv_color[2]); // !!!Z_rgb*p_rgb -> p_rgb
int Y = static_cast<int>(uv_color[1] / uv_color[2]); // !!!Z_rgb*p_rgb -> p_rgb
//cout << "X: " << X << " Y: " << Y << endl;
if ((X >= 0 && X < 1920) && (Y >= 0 && Y < 1080))
{
//cout << "X: " << X << " Y: " << Y << endl;
result.data[i * 3] = bgr.data[3 * (Y * 1920 + X)];
result.data[i * 3 + 1] = bgr.data[3 * (Y * 1920 + X) + 1];
result.data[i * 3 + 2] = bgr.data[3 * (Y * 1920 + X) + 2];
}
else
{
result.data[i * 3] = 0;
result.data[i * 3 + 1] = 0;
result.data[i * 3 + 2] = 0;
}
}
else
{
result.data[i * 3] = 0;
result.data[i * 3 + 1] = 0;
result.data[i * 3 + 2] = 0;
}
i++;
}
}
imwrite("registrationResult.png", result);
thread th2 = std::thread([&]{
while (true)
{
imshow("结果图", result);
waitKey(1);
}
});
th.join();
th2.join();
#pragma endregion
system("pause");
return 0;
}
|
|