C/C++实现双目矫正(不使用OpenCV内部函数)及矫正源码解析
C/CPP实现双目矫正(不使用OpenCV)及矫正源码解析
这篇文章是之前【要matlab标定数据做双目相机矫正OpenCV C++】的补充，再加上了双目矫正的原理及代码注释。更新中……
本文所需数据为matlab标定的双目摄像头外参及内参，具体使用stereoCameraCalibrator 步骤可看之前那篇文章。后续将会实现CPP版本的双目标定，又挖了个坑。
补充了，在最下面。
再补充一个棋盘格标定板（可白嫖
找paper搭配 Sci-Hub 食用更佳 (๑•̀ㅂ•́)و✧
Sci-Hub 实时更新 : https://tool.yovisun.com/scihub/
公益科研通文献求助：https://www.ablesci.com/
立体匹配算法排行榜：KITTI 立体匹配算法排行榜，Middlebury 立体匹配算法排行榜
熟悉立体匹配的同学对这幅图一定不陌生，计算双目深度就是按照上图中的相似三角形进行求解。
视 差 d i s p a r i t y = X R − X T , 视差~~~disparity = X_R - X_T, 视差 disparity=XR−XT,
深 度 z / B a s e l i n e = ( z − f ) / ( B a s e l i n e − d i s p a r i t y ) ， 深度~~~z / Baseline = (z - f) /( Baseline - disparity)， 深度 z/Baseline=(z−f)/(Baseline−disparity)，
得 到 得到 得到
z = f ∗ B a s e l i n e / d i s p a r i t y ~~~~~~~~~~~z = f*Baseline/disparity z=f∗Baseline/disparity, 也即视差与深度成反比。
重投影矩阵Q
1 / − Q [ 14 ] b a s e l i n e ( m m ) 根 据 标 定 的 相 机 外 参 计 算 。 如 果 只 需 要 相 对 深 度 取 1 即 可 1 / -Q[14] ~~~baseline (mm) ~~~~~~~ 根据标定的相机外参计算。如果只需要相对深度取1即可 1/−Q[14] baseline(mm) 根据标定的相机外参计算。如果只需要相对深度取1即可
Q [ 11 ] f o c a l l e n g t h ( p x ) 由 标 定 内 参 得 到 Q[11] ~~~~ ~~~~~~~ focal~length (px) ~~~ 由标定内参得到 Q[11] focal length(px) 由标定内参得到
以上的计算建立在双目相机的光轴完全平行，但这是不可能的，为此需要进行标定和矫正。
矫正主函数：
void StereoRectify(double *left_intrinsic, double *left_distort, double *right_intrinsic, double *right_distort,
double *rotation_mat, double *translation_vector, double *left_rota, double *right_rota, double *left_proj, double *right_proj, double *Q)
旋转矩阵求逆：
void mat33Inverse(double *PR, double *PRI) {
/* 3*3 matrix inverse */
double temp;
temp = PR[0] * PR[4] * PR[8] + PR[1] * PR[5] * PR[6] + PR[2] * PR[3] * PR[7] -
PR[2] * PR[4] * PR[6] - PR[1] * PR[3] * PR[8] - PR[0] * PR[5] * PR[7];
PRI[0] = (PR[4] * PR[8] - PR[5] * PR[7]) / temp;
PRI[1] = -(PR[1] * PR[8] - PR[2] * PR[7]) / temp;
PRI[2] = (PR[1] * PR[5] - PR[2] * PR[4]) / temp;
PRI[3] = -(PR[3] * PR[8] - PR[5] * PR[6]) / temp;
PRI[4] = (PR[0] * PR[8] - PR[2] * PR[6]) / temp;
PRI[5] = -(PR[0] * PR[5] - PR[2] * PR[3]) / temp;
PRI[6] = (PR[3] * PR[7] - PR[4] * PR[6]) / temp;
PRI[7] = -(PR[0] * PR[7] - PR[1] * PR[6]) / temp;
PRI[8] = (PR[0] * PR[4] - PR[1] * PR[3]) / temp;
}
// 3*3 -> 3*1 rodrigues Transform
rodriguesTransform(inv_rotation_mat, u, v, w);
罗格里斯变换使用到奇异值分解，使用的《Numerical Recipes in C》一书中的代码 svdcmp.c
void rodriguesTransform(double *inv_rotation_mat, double **u, double **v, double w[])
{ /*
* 使用 ** 来声明，然后用dmatrix来申请空间
* 这样在函数引用矩阵时，就不用声明矩阵大小了
*/
//double **a;
int i, j, k;
double t;
double t1[MN+1], t2[MN+1];
/* 矩阵均以M,N中的最大值来申请空间，避免越界 */
//a = dmatrix(1, MN, 1, MN); // inv_rotation_mat
for (i = 1; i <= M; i++) {
for (j = 1; j <= N; j++) {
//u[i][j] = a[i][j];
u[i][j] = inv_rotation_mat[i * 3 + j - 4];
}
}
svdcmp(u, MN, MN, w, v);
/* Sort the singular values in descending order */
for (i = 1; i <= N; i++) {
for (j = i + 1; j <= N; j++) {
if (w[i] < w[j]) { /* 对特异值排序 */
t = w[i];
w[i] = w[j];
w[j] = t;
/* 同时也要把矩阵U,V的列位置交换 */
/* 矩阵U */
for (k = 1; k <= M; k++) {
t1[k] = u[k][i];
}
for (k = 1; k <= M; k++) {
u[k][i] = u[k][j];
}
for (k = 1; k <= M; k++) {
u[k][j] = t1[k];
}
/* 矩阵V */
for (k = 1; k <= N; k++) {
t2[k] = v[k][i];
}
for (k = 1; k <= N; k++) {
v[k][i] = v[k][j];
}
for (k = 1; k <= N; k++) {
v[k][j] = t2[k];
}
}
}
}
}
// u * v'
R[0] = u[1][1] * v[1][1] + u[1][2] * v[1][2] + u[1][3] * v[1][3];
R[1] = u[1][1] * v[2][1] + u[1][2] * v[2][2] + u[1][3] * v[2][3];
R[2] = u[1][1] * v[3][1] + u[1][2] * v[3][2] + u[1][3] * v[3][3];
R[3] = u[2][1] * v[1][1] + u[2][2] * v[1][2] + u[2][3] * v[1][3];
R[4] = u[2][1] * v[2][1] + u[2][2] * v[2][2] + u[2][3] * v[2][3];
R[5] = u[2][1] * v[3][1] + u[2][2] * v[3][2] + u[2][3] * v[3][3];
R[6] = u[3][1] * v[1][1] + u[3][2] * v[1][2] + u[3][3] * v[1][3];
R[7] = u[3][1] * v[2][1] + u[3][2] * v[2][2] + u[3][3] * v[2][3];
R[8] = u[3][1] * v[3][1] + u[3][2] * v[3][2] + u[3][3] * v[3][3];
double r[3];
r[0] = R[7] - R[5];
r[1] = R[2] - R[6];
r[2] = R[3] - R[1];
float s = sqrt((r[0] * r[0] + r[1] * r[1] + r[2] * r[2]) * 0.25);
float c = (R[0] + R[4] + R[8] - 1) * 0.5;
c = c > 1. ? 1. : c < -1. ? -1. : c; // 1 -1
float theta = acos(c);
double rr[3]; // 1*3 一行三列
if (s < 1.0e-5)
{
float t;
if (c > 0)
{
double rr[3] = { 0, 0, 0 };
}
else
{
t = (R[0] + 1) * 0.5;
rr[0] = sqrt(MAX(t, float(0.)));
t = (R[4] + 1) * 0.5;
rr[1] = sqrt(MAX(t, float(0.))) * (R[1] < 0 ? -1. : 1.);
t = (R[8] + 1) * 0.5;
rr[2] = sqrt(MAX(t, float(0.))) * (R[2] < 0 ? -1. : 1.);
if (fabs(rr[0]) < fabs(rr[1]) && fabs(rr[0]) < fabs(rr[2]) && (R[5] > 0) != (rr[1] * rr[2] > 0))
rr[2] = -rr[2];
double rrsqrt = sqrt(rr[0] * rr[0] + rr[1] * rr[1] + rr[2] * rr[2]);
theta /= rrsqrt;
rr[0] = theta * rr[0];
rr[1] = theta * rr[1];
rr[2] = theta * rr[2];
}
}
else
{
float vth = 1 / (2 * s);
vth *= theta;
rr[0] = vth * r[0];
rr[1] = vth * r[1];
rr[2] = vth * r[2];
}
double eul[3];
eul[0] = rr[0] * (-0.5);
eul[1] = rr[1] * (-0.5);
eul[2] = rr[2] * (-0.5);
std::string GetExePath()
{
char szFilePath[MAX_PATH + 1] = { 0 };
GetModuleFileNameA(NULL, szFilePath, MAX_PATH);
/*
strrchr:函数功能：查找一个字符c在另一个字符串str中末次出现的位置（也就是从str的右侧开始查找字符c首次出现的位置），
并返回这个位置的地址。如果未能找到指定字符，那么函数将返回NULL。
使用这个地址返回从最后一个字符c到str末尾的字符串。
*/
(strrchr(szFilePath, '\\'))[0] = 0; // 删除文件名，只获得路径字串//
std::string path = szFilePath;
int pos = path.rfind("\\", path.length());//从后往前寻找第一个/出现的位置
path = path.substr(0, pos);//剪切[0,pos]
return path;
}
void getYamlParamters(std::string file_extrinsics, std::string file_intrisics)
{
cv::FileStorage fs_read(file_intrisics, cv::FileStorage::READ);
if (fs_read.isOpened()) {
NDouble cameraMatrixL[3 * 3], cameraMatrixR[3 * 3], distCoeffL[5], distCoeffR[5];
fs_read["cameraMatrixL"] >> cameraMatrixL;
fs_read["cameraDistcoeffL"] >> distCoeffL;
fs_read["cameraMatrixR"] >> cameraMatrixR;
fs_read["cameraDistcoeffR"] >> distCoeffR;
fs_read.release();
}
fs_read.open(file_extrinsics, cv::FileStorage::READ);
if (fs_read.isOpened()) {
NDouble Rotation_mat[3 * 3], Translation_vector[3];
fs_read["R"] >> Rotation_mat;
fs_read["T"] >> Translation_vector;
fs_read.release();
}
}
FileStorage fs("intrisics.yml", FileStorage::WRITE);
//time_t rawtime;
//time(&rawtime);
/*struct tm tm = *localtime(&(time_t) { time (NULL) });
char str[26];
asctime_s(str, sizeof str, &tm);
fs << "Time" << str;*/
if (fs.isOpened())
{
fs << "cameraMatrixL" << cameraMatrix << "cameraDistcoeffL" << distCoeffs << "cameraMatrixR" << cameraMatrixR << "cameraDistcoeffR" << distCoeffsR;
fs.release();
cout << "cameraMatrixL=:" << cameraMatrix << endl << "cameraDistcoeffL=:" << distCoeffs << endl << "cameraMatrixR=:" << cameraMatrixR << endl << "cameraDistcoeffR=:" << distCoeffsR << endl;
}
else
{
cout << "Error: can not save the intrinsics!!!!" << endl;
}
fs.open("extrinsics.yml", FileStorage::WRITE);
if (fs.isOpened())
{
fs << "R" << R << "T" << T << "E" << E << "F" << F << "Rl" << Rl << "Rr" << Rr << "Pl" << Pl << "Pr" << Pr << "Q" << Q;
cout << "R=" << R << endl << "T=" << T << endl << "E=" << E << endl << "F=" << F << endl << "Rl=" << Rl << endl << "Rr" << Rr << endl << "Pl" << Pl << endl << "Pr" << Pr << endl << "Q" << Q << endl;
fs.release();
}
else
{
cout << "Error: can not save the extrinsic parameters\n";
}
#include