双目校正|双目立体校正C/C++复现计算机视觉|算法|c++

【立体校正】C/C++复现

前言
立体校正原理简述
实际效果
问题求助

前言项目需要，基于C/C++复现了双目立体校正功能，仅支持双线性插值方式。
立体校正原理简述功能简述：
立体校正即把左右摄像头采集的图像中同一物点变换到同一水平线（使其在图像中的纵坐标相等），其主要目的是加速后续双目匹配速度。如下图中，红点1与红点2在真实三维世界中表示同一物点，但两个摄像机光心并不处于同一水平线，导致成像时该点纵坐标不相等，极大的增加了双目匹配时的搜索范围。

文章图片

双目校正原理简述：
通过一个（模型、方程式）获取校正后的图中每个像素点坐标与原图坐标的映射关系，再把原图中的像素值赋值给校正后图中的对应位置即可。如下图中，假设点1的坐标为（10,10），通过模型计算该点对应原图点2坐标为（35,40），那么便把点2（35,40）的像素值赋值给点1（10,10）。至于这个模型的方程表达式为什么是这个样子，都是学术大佬研究的，我选择只用用就好。（其实就是没看懂原理，哈哈！有懂的大佬可以评论区指点指点）

文章图片

双线性插值：
用周围的4个点的像素值，通过分配不同的权重表示该点的像素值。例如坐标点（35.2，40.3）的像素值会通过周围4个点：（35,40）、（36,40）、（35,41）、（36,41）来表示。其中小数点0.2,0.3会用于计算权重，具体公式如下：

i = 35
j = 40
u = 0.2
v = 0.3
f(i+u,j+v) = (1-u)(1-v)f(i,j) + (1-u)vf(i,j+1) + u(1-v)f(i+1,j) + uvf(i+1,j+1)

文章图片

【双目校正|双目立体校正C/C++复现】具体代码，opencv主要用于读图和存图，验证我的校正是否正确。

#include #includeusing namespace cv; using namespace std; void GetMatrix(float *R, float *P, float *PRI); void GetMap(float *K, float *D, float *PRI, float *mapx, float *mapy, Size size); void Imgremap(Mat srcImg, Mat &dstImg, float *mapx, float *mapy); void Imgremap_new(Mat srcImg, Mat &dstImg, float *mapx, float *mapy); int main() { /* opencv API实现双目校正 */ Mat API_img = imread("left01.jpg", 0); Size imgsz(640, 480); /* 相机参数通过单目、双目标定即可获得 */ Mat cameraMatrixL = (Mat_(3, 3) << 5.3340331777463712e+02, 0., 3.4251343227755160e+02, 0., 5.3343402398745684e+02, 2.3475353096292952e+02, 0., 0., 1.); Mat distCoeffL = (Mat_(5, 1) << -2.8214652038759541e-01, 4.0840748605552028e-02, 1.2058218262263004e-03, -1.2307876204068898e-04, 1.1409651538056684e-01); Mat API_Rl = (Mat_(3, 3) << 9.9995545010198061e-01, -7.5529503409555278e-03, 5.6613384011591078e-03, 7.5729426565523507e-03, 9.9996513545382970e-01, -3.5182973615490811e-03, -5.6345674959085556e-03, 3.5610136128317303e-03, 9.9997778516884228e-01); Mat API_Pl = (Mat_(3, 4) << 5.3500952177482827e+02, 0., 3.3665814208984375e+02, 0., 0., 5.3500952177482827e+02, 2.4442177581787109e+02, 0., 0., 0., 1., 0.); Mat API_maplx, API_maply; initUndistortRectifyMap(cameraMatrixL, distCoeffL, API_Rl, API_Pl, imgsz, CV_32FC1, API_maplx, API_maply); clock_t start_API, end_API; Mat API_unimg; start_API = clock(); remap(API_img, API_unimg, API_maplx, API_maply, INTER_LINEAR); end_API = clock(); cout << "API run time:" << (double)(end_API - start_API) << endl; imwrite("left01_API.jpg", API_unimg); /* 复现代码实现双目校正 */ /* 左相机参数 */ float Kl[3][3] = { { 5.3340331777463712e+02, 0., 3.4251343227755160e+02 }, { 0.,5.3343402398745684e+02, 2.3475353096292952e+02 }, { 0., 0., 1. } }; float Dl[5] = { -2.8214652038759541e-01, 4.0840748605552028e-02, 1.2058218262263004e-03, -1.2307876204068898e-04, 1.1409651538056684e-01 }; float Rl[3][3] = { { 9.9995545010198061e-01, -7.5529503409555278e-03,5.6613384011591078e-03 }, { 7.5729426565523507e-03,9.9996513545382970e-01, -3.5182973615490811e-03 }, { -5.6345674959085556e-03, 3.5610136128317303e-03,9.9997778516884228e-01 } }; float Pl[3][3] = { { 5.3500952177482827e+02, 0., 3.3665814208984375e+02 }, { 0., 5.3500952177482827e+02, 2.4442177581787109e+02 }, { 0., 0., 1. } }; /* 右相机参数 */ float Kr[3][3] = { { 5.3699964365956180e+02, 0., 3.2744774682047540e+02 }, { 0., 5.3658501956219982e+02, 2.4990007115682096e+02 }, { 0., 0., 1. } }; float Dr[5] = { -2.9611763213840986e-01, 1.3891105660442912e-01, -5.0433529470851200e-04, 9.4658617944131683e-05, -4.9061152399050519e-02 }; float Rr[3][3] = { { 9.9993738772164931e-01, -1.1094841523320539e-02, 1.4577818686240377e-03 }, { 1.1089611831016350e-02, 9.9993221439532998e-01, 3.5478336896012114e-03 }, { -1.4970457045358340e-03, -3.5314453165933707e-03, 9.9999264384701070e-01 } }; float Pr[3][3] = { { 5.3500952177482827e+02, 0., 3.3665814208984375e+02 }, { 0., 5.3500952177482827e+02, 2.4442177581787109e+02 }, { 0., 0., 1. } }; /* 求逆矩阵 */ float PRIl[3][3]; float PRIr[3][3]; memset(PRIl, 0, sizeof(PRIl)); memset(PRIr, 0, sizeof(PRIr)); GetMatrix((float*)Rl, (float*)Pl, (float*)PRIl); GetMatrix((float*)Rr, (float*)Pr, (float*)PRIr); /* 获取映射表 */ Size ImgSize(640, 480); float *maplx = new float[ImgSize.width * ImgSize.height]; float *maply = new float[ImgSize.width * ImgSize.height]; float *maprx = new float[ImgSize.width * ImgSize.height]; float *mapry = new float[ImgSize.width * ImgSize.height]; GetMap((float*)Kl, (float*)Dl, (float*)PRIl, maplx, maply, ImgSize); GetMap((float*)Kr, (float*)Dr, (float*)PRIr, maprx, mapry, ImgSize); /* 映射校正 */ clock_t start_MY, end_MY; Mat imgl = imread("left01.jpg", 0); Mat imgr = imread("right01.jpg", 0); Mat unimgl(imgl.rows, imgl.cols, CV_8UC1); Mat unimgr(imgl.rows, imgl.cols, CV_8UC1); start_MY = clock(); Imgremap_new(imgl, unimgl, maplx, maply); Imgremap_new(imgr, unimgr, maprx, mapry); end_MY = clock(); cout << "MY run time:" << (double)(end_MY - start_MY) << endl; imwrite("left01_MY.jpg", unimgl); imwrite("right01_MY.jpg", unimgr); delete[] maplx; delete[] maply; delete[] maprx; delete[] mapry; system("pause"); return 0; }/* * brief 获取矩阵PR的逆矩阵 * param R 输入，旋转矩阵指针，双目标定获得 * param P 输入，映射矩阵指针，双目标定获得 * param PRI 输出，逆矩阵指针*/ void GetMatrix(float *R, float *P, float *PRI) { float temp; float PR[3][3]; memset(PR, 0, sizeof(PR)); /* 矩阵P*R */ for (int i = 0; i < 3; i++) { for (int k = 0; k < 3; k++) { temp = P[i * 3 + k]; for (int j = 0; j < 3; j++) { PR[i][j] += temp * R[k * 3 + j]; } } } /* 3X3矩阵求逆 */ temp = PR[0][0] * PR[1][1] * PR[2][2] + PR[0][1] * PR[1][2] * PR[2][0] + PR[0][2] * PR[1][0] * PR[2][1] - PR[0][2] * PR[1][1] * PR[2][0] - PR[0][1] * PR[1][0] * PR[2][2] - PR[0][0] * PR[1][2] * PR[2][1]; PRI[0] = (PR[1][1] * PR[2][2] - PR[1][2] * PR[2][1]) / temp; PRI[1] = -(PR[0][1] * PR[2][2] - PR[0][2] * PR[2][1]) / temp; PRI[2] = (PR[0][1] * PR[1][2] - PR[0][2] * PR[1][1]) / temp; PRI[3] = -(PR[1][0] * PR[2][2] - PR[1][2] * PR[2][0]) / temp; PRI[4] = (PR[0][0] * PR[2][2] - PR[0][2] * PR[2][0]) / temp; PRI[5] = -(PR[0][0] * PR[1][2] - PR[0][2] * PR[1][0]) / temp; PRI[6] = (PR[1][0] * PR[2][1] - PR[1][1] * PR[2][0]) / temp; PRI[7] = -(PR[0][0] * PR[2][1] - PR[0][1] * PR[2][0]) / temp; PRI[8] = (PR[0][0] * PR[1][1] - PR[0][1] * PR[1][0]) / temp; return; }/* * brief 获取映射表 * param K输入，相机内参矩阵指针，单目标定获得 * param D输入，相机畸变矩阵指针，单目标定获得 * param PRI输入，P*R的逆矩阵 * param mapx 输出，x坐标映射表 * param mapy 输出，y坐标映射表 * param size 输入，图像分辨率*/ void GetMap(float *K, float *D, float *PRI, float *mapx, float *mapy, Size size) { float fx = K[0]; float fy = K[4]; float u0 = K[2]; float v0 = K[5]; float k1 = D[0]; float k2 = D[1]; float p1 = D[2]; float p2 = D[3]; float k3 = D[4]; /* 学术大佬研究的模型：去除畸变+双目校正 */ for (int i = 0; i < size.height; i++) { float _x = i*PRI[1] + PRI[2]; float _y = i*PRI[4] + PRI[5]; float _w = i*PRI[7] + PRI[8]; for (int j = 0; j < size.width; j++, _x += PRI[0] , _y += PRI[3], _w += PRI[6]) { float w = 1. / _w; float x = _x * w; float y = _y * w; float x2 = x * x; float y2 = y * y; float r2 = x2 + y2; float _2xy = 2 * x * y; float kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2; float u = fx*(x*kr + p1*_2xy + p2*(r2 + 2 * x2)) + u0; float v = fy*(y*kr + p1*(r2 + 2 * y2) + p2*_2xy) + v0; mapx[i*size.width + j] = u; mapy[i*size.width + j] = v; } } }/* * brief 执行校正过程 * param srcImg 输入，原图数据 * param dstImg 输入，校正后图像数据 * param mapx 输入，x坐标映射表 * param mapy 输入，y坐标映射表*/ void Imgremap(Mat srcImg, Mat &dstImg, float *mapx, float *mapy) { int x, y; float u, v; /* 纯浮点运算，执行映射+插值过程 */ for (int i = 0; i < srcImg.rows; i++) { for (int j = 0; j < srcImg.cols; j++) { x = (int)mapx[i*srcImg.cols + j]; y = (int)mapy[i*srcImg.cols + j]; if (x > 1 && x < (srcImg.cols-1) && y > 1 && y < (srcImg.rows-1)) { u = mapx[i*srcImg.cols + j] - x; v = mapy[i*srcImg.cols + j] - y; dstImg.ptr(i)[j] = (uchar)((1 - u)*(1 - v)*srcImg.ptr(int(y))[int(x)] + (1 - u)*v*srcImg.ptr(int(y + 1))[int(x)] + u*(1 - v)*srcImg.ptr(int(y))[int(x + 1)] + u*v*srcImg.ptr(int(y + 1))[int(x + 1)]); //cout << (int)(dstImg.ptr(i)[j]) << endl; } else { dstImg.ptr(i)[j] = 0; }} } }/* * brief 执行校正过程 * param srcImg 输入，原图数据 * param dstImg 输入，校正后图像数据 * param mapx 输入，x坐标映射表 * param mapy 输入，y坐标映射表*/ void Imgremap_new(Mat srcImg, Mat &dstImg, float *mapx, float *mapy) { /* 浮点转定点运算，执行映射+插值过程 */ for (int i = 0; i < srcImg.rows; ++i) { for (int j = 0; j < srcImg.cols; ++j) { int pdata = https://www.it610.com/article/i*srcImg.cols + j; int x = (int)mapx[pdata]; int y = (int)mapy[pdata]; short PartX = (mapx[pdata] - x) * 2048; short PartY = (mapy[pdata] - y) * 2048; short InvX = 2048 - PartX; short InvY = 2048 - PartY; if (x> 1 && x < (srcImg.cols - 1) && y > 1 && y < (srcImg.rows - 1)) { dstImg.ptr(i)[j] = (((InvX*srcImg.ptr(y)[x] + PartX*srcImg.ptr(y)[x+1])*InvY + (InvX*srcImg.ptr(y+1)[x] + PartX*srcImg.ptr(y + 1)[x + 1])*PartY) >> 22); //cout << (int)(dstImg.ptr(i)[j]) << endl; } else { dstImg.ptr(i)[j] = 0; }} } }