C++实现双目校准

接着昨天的继续用C++实现双目校准。该方法比昨天的MATLAB的效果更加理想。

这里首先申明一下，首先要使用OpenCV来获取校准参数。环境：win10+vs2013+OpenCV3.1.0

这里的方法也是使用直接法来实现双目校准，首先使用OpenCV获取校准参数，并将校准参数保存到txt文档，供校准使用。

/******************************/
/*        立体匹配和测距        */
/******************************/

#include <opencv2/opencv.hpp>
#include <iostream>
#include <fstream>

using namespace std;
using namespace cv;

const int imageWidth = 640;                             //摄像头的分辨率
const int imageHeight = 480;
Size imageSize = Size(imageWidth, imageHeight);

Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;
Mat rectifyImageL, rectifyImageR;

Mat mapLx, mapLy, mapRx, mapRy;     //映射表
Mat Rl, Rr, Pl, Pr, Q;              //校正旋转矩阵R，投影矩阵P 重投影矩阵Q

/*
事先标定好的相机的参数
fx 0 cx
0 fy cy
0 0  1
*/
Mat cameraMatrixL = (Mat_<double>(3, 3) << 688.30717, 0, 315.31975,
0, 683.13738, 264.40537,
0, 0, 1);
Mat distCoeffL = (Mat_<double>(5, 1) << 0.27153, -1.00396, 0.01107, 0.00364, 0.00000);

Mat cameraMatrixR = (Mat_<double>(3, 3) << 665.32773, 0, 345.54178,
0, 660.17940, 249.92143,
0, 0, 1);
Mat distCoeffR = (Mat_<double>(5, 1) << 0.22386, -0.93591, 0.00725, 0.00018, 0.00000);

Mat T = (Mat_<double>(3, 1) << -39.78384, 1.21230, -40.44029);//T平移向量
Mat rec = (Mat_<double>(3, 1) << -0.02302, -0.00735, 0.00089);//rec旋转向量
Mat R;//R 旋转矩阵

/*****主函数*****/
int main()
{
std::ofstream Fs_mapLx("path\\mapLx.txt");
std::ofstream Fs_mapLy("path\\mapLy.txt");
std::ofstream Fs_mapRx("path\\mapRx.txt");
std::ofstream Fs_mapRy("path\\mapRy.txt");

/*
立体校正
*/
Rodrigues(rec, R); //Rodrigues变换
stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
0, imageSize, &validROIL, &validROIR);

initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);
initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);
int he = mapLx.rows;
int wi = mapLx.cols;

// 保存校准数据
for (int i = 0; i < he; i++)
{
for (int j = 0; j < wi; j++)
{
Fs_mapLx << (float)mapLx.ptr<float>(i)[j] << "\t";
Fs_mapLy << (float)mapLy.ptr<float>(i)[j] << "\t";
Fs_mapRx << (float)mapRx.ptr<float>(i)[j] << "\t";
Fs_mapRy << (float)mapRy.ptr<float>(i)[j] << "\t";
}
Fs_mapLx << endl;
Fs_mapLy << endl;
Fs_mapRx << endl;
Fs_mapRy << endl;

}

Fs_mapLx.close();
Fs_mapLy.close();
Fs_mapRx.close();
Fs_mapRy.close();
// 保存校准数据

return 0;
}

其中Fs_mapLx、Fs_mapLy、Fs_mapRx、Fs_mapRy为校准数据。

下面是校准程序：

#include <iostream>
#include <fstream>
#include <vector>
#include <atlimage.h>

using namespace std;

int main()
{
CString path_L = "校准图像\\L.jpg";
CString path_R = "校准图像\\R.jpg";
ifstream fin_mapLx("path\\mapLx.txt"); //获取校准数据
ifstream fin_mapLy("path\\mapLy.txt"); //获取校准数据
ifstream fin_mapRx("path\\mapRx.txt"); //获取校准数据
ifstream fin_mapRy("path\\mapRy.txt"); //获取校准数据

CImage m_Image_L;
m_Image_L.Load(path_L);

CImage m_Image_R;
m_Image_R.Load(path_R);

int nimgWidth = m_Image_L.GetWidth();
int nimgheigt = m_Image_L.GetHeight();
vector<vector <int>> img_L(nimgheigt, vector<int>(nimgWidth));          // 校准前图像矩阵
vector<vector <int>> img_R(nimgheigt, vector<int>(nimgWidth));          // 校准前图像矩阵
vector<vector <int>> img_L1(nimgheigt, vector<int>(nimgWidth));         // 校准后图像矩阵
vector<vector <int>> img_R1(nimgheigt, vector<int>(nimgWidth));         // 校准后图像矩阵
vector<vector <float>> mapLx(nimgheigt, vector<float>(nimgWidth));          // 校准数据
vector<vector <float>> mapLy(nimgheigt, vector<float>(nimgWidth));          // 校准数据
vector<vector <float>> mapRx(nimgheigt, vector<float>(nimgWidth));          // 校准数据
vector<vector <float>> mapRy(nimgheigt, vector<float>(nimgWidth));          // 校准数据

byte* pRealData_L = (byte*)m_Image_L.GetBits();
int pit_L = m_Image_L.GetPitch();
int bitCount_L = m_Image_L.GetBPP() / 8;

byte* pRealData_R = (byte*)m_Image_R.GetBits();
int pit_R = m_Image_R.GetPitch();
int bitCount_R = m_Image_R.GetBPP() / 8;

for (int y = 0; y<nimgheigt; y++)
{
for (int x = 0; x<nimgWidth; x++)
{
img_L[y][x] = (int)(((int)(int)(*(pRealData_L + pit_L*y + x*bitCount_L)) * 19595 + (int)(int)(*(pRealData_L + pit_L*y + x*bitCount_L + 1)) * 38469 +
(int)(int)(*(pRealData_L + pit_L*y + x*bitCount_L + 2)) * 7472)) >> 16;         // 将RGB转灰度

img_R[y][x] = (int)(((int)(int)(*(pRealData_R + pit_R*y + x*bitCount_R)) * 19595 + (int)(int)(*(pRealData_R + pit_R*y + x*bitCount_R + 1)) * 38469 +
(int)(int)(*(pRealData_R + pit_R*y + x*bitCount_R + 2)) * 7472)) >> 16;         // 将RGB转灰度

fin_mapLx >> mapLx[y][x];
fin_mapLy >> mapLy[y][x];
fin_mapRx >> mapRx[y][x];
fin_mapRy >> mapRy[y][x];

img_L1[y][x] = 0.0;
img_R1[y][x] = 0.0;

}
}
fin_mapLx.clear();
fin_mapLy.clear();
fin_mapRx.clear();
fin_mapRy.clear();

// 将校准后的数据保存，在MATLAB中进行显示
ofstream Fs_img_L("path\\img_L.txt");
ofstream Fs_img_R("path\\img_R.txt");
for (int i = 0; i < nimgheigt; i++)
{
for (int j = 0; j < nimgWidth; j++)
{
img_L1[i][j] = img_L[mapLy[i][j]][mapLx[i][j]];             // 校准公式
img_R1[i][j] = img_R[mapRy[i][j]][mapRx[i][j]];
Fs_img_L << img_L1[i][j] << "\t";
Fs_img_R << img_R1[i][j] << "\t";
}
Fs_img_L << endl;
Fs_img_R << endl;

}

Fs_img_L.clear();
Fs_img_R.clear();
}

这里图像显示交给MATLAB来实现

MATLAB代码：

clc;clear;

L = importdata('path\img_L.txt');
R = importdata('path\img_R.txt');
[m,n] = size(L);
for i=1:m
for j=1:n
if mod(i,20)==0
L(i,j) =255;
R(i,j) =255;
end
end
end
figure;imshow(L,[]);
figure;imshow(R,[]);

或者直接用MATLAB来实现OpenCV获取的数据实现图像校准

clc;clear;

mapLx = uint16(importdata('path\mapLx.txt'));
mapLy = uint16(importdata('path \mapLy.txt'));
mapRx = uint16(importdata('path \mapRx.txt'));
mapRy = uint16(importdata('path \mapRy.txt'));

L = rgb2gray(imread('原始图像\L.jpg'));
R = rgb2gray(imread('原始图像\R.jpg'));

[m,n] = size(L);
L1 = zeros(m,n);
R1 = zeros(m,n);

for i=1:m
for j=1:n
if mod(i,20)==0
L1(i,j) =255;
R1(i,j) =255;
else
L1(i,j) =L(mapLy(i,j),mapLx(i,j));
R1(i,j) =R(mapRy(i,j),mapRx(i,j));
end
end
end
figure;imshow(L1,[]);
figure;imshow(R1,[]);

到这里，双目校准就告一段落，以后有时间了，再来完成图像立体匹配工作。争取明天早起。。。。。。