这篇文章是之前【要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 欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）