准备标定参数

依照《SLAM开发之双目标定（MATLAB）》标定了一波，标定结果给出如下

左相机内参 (使用前需要对齐进行转置)
stereoParams.CameraParameters1.IntrinsicMatrix

[3.794933296903811e+02,0,0;0,3.789423824318373e+02,0;1.708814624155970e+02,1.115666514433592e+02,1]

左相机径向畸变(K1, K2, K3)
stereoParams.CameraParameters1.RadialDistortion

[0.030551047027847,0.649726442995634,-3.135805398892529]

左相机切向畸变(P1, P2)
stereoParams.CameraParameters1.TangentialDistortion

[0.002296124889110,0.001247128894521]

右相机内参 (使用前需要对齐进行转置)
stereoParams.CameraParameters2.IntrinsicMatrix

[3.812723804323131e+02,0,0;0,3.807879935881336e+02,0;1.726763231981177e+02,1.228890378500635e+02,1]

右相机径向畸变(K1, K2, K3)
stereoParams.CameraParameters2.RadialDistortion

[0.051714797025223,0.301689634726206,-1.949431177752929]

右相机切向畸变(P1, P2)
stereoParams.CameraParameters2.TangentialDistortion

[0.002019080886440,-0.002338594348609]

两个摄像头的旋转参数 (使用前需要对齐进行转置)
stereoParams.RotationOfCamera2

[0.999964138274955,-2.631593168759495e-04,-0.008464804262361;3.707800649684831e-04,0.999919094244024,0.012714853060666;0.008460773378928,-0.012717535664776,0.999883332994628]

两个摄像头的平移参数
stereoParams.TranslationOfCamera2

[-59.825188219129230,-0.439775295864600,0.962473834066763]

双目立体校正输出

基于MATLAB工具箱校正得到各项参数后，将其移植到基于OpenCV的C++工程中，主要使用了以下三个函数

cv::stereoRectify(...)
cv::initUndistortRectifyMap(...)
cv::remap(...)

• stereoRectify() 的作用是为每个摄像头计算立体校正的映射矩阵。所以其运行结果并不是直接将图片进行立体矫正，而是得出进行立体矫正所需要的映射矩阵；
• initUndistortRectifyMap() 是映射变换计算函数，该函数功能是计算畸变矫正和立体校正的映射变换；
• remap(）重映射函数，就是把一幅图像中某位置的像素放置到另一个图片指定位置的过程。可以依据initUndistortRectifyMap()计算出的畸变校正映射关系对图片进行校正

给出节点的完整代码如下：

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sstream>
#include <iostream>
#include "mono_camera.h"
#include "mono_frame.h"
#include "map_point.h"
#include "map.h"
#include "config.h"
#include "visual_odometry.h"
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>

int main(int argc,char **argv)
{
    ROS_INFO("hello ROOTSLAM");
    ros::init(argc,argv,"starter_node");       
    ros::NodeHandle nh;                           
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);

    int freq;
    nh.getParam("/Mi8Cam/frequency",freq);
    ROS_INFO("FREQ:%d\n",freq);

    // 参数设定 
    // M1:左相机内参矩阵
    // M2:右相机内参矩阵
    // D1:左相机畸变参数（k1 k2 p1 p2 k3）
    // D2:右相机畸变参数（k1 k2 p1 p2 k3）
    // R:右相机相对于左相机的旋转矩阵
    // T:右相机相对于左相机的平移向量
    cv::Size image_size(320,240);
	cv::Mat M1 = (cv::Mat_<double>(3, 3) << 3.794933296903811e+02, 0, 1.708814624155970e+02, 0, 3.789423824318373e+02, 1.115666514433592e+02, 0, 0, 1);
	cv::Mat D1 = (cv::Mat_<double>(5, 1) << 0.030551047027847, 0.649726442995634, 0.002296124889110, 0.001247128894521, -3.135805398892529);
	cv::Mat M2 = (cv::Mat_<double>(3, 3) << 3.812723804323131e+02, 0, 1.726763231981177e+02, 0, 3.807879935881336e+02, 1.228890378500635e+02, 0, 0, 1);
	cv::Mat D2 = (cv::Mat_<double>(5, 1) << 0.051714797025223, 0.301689634726206, 0.002019080886440, -0.002338594348609, -1.949431177752929);
	cv::Mat R = (cv::Mat_<double>(3, 3) << 0.999964138274955, 3.707800649684831e-04, 0.008460773378928, -2.631593168759495e-04, 0.999919094244024, -0.012717535664776, -0.008464804262361, 0.012714853060666, 0.999883332994628);
	cv::Mat T = (cv::Mat_<double>(3, 1) << -59.825188219129230, -0.439775295864600, 0.962473834066763);
    // 左相机参数设定
    cv::VideoCapture cap_left;
    cap_left.open(0);
    cap_left.set(cv::CAP_PROP_FRAME_WIDTH,320);
    cap_left.set(cv::CAP_PROP_FRAME_HEIGHT,240);
    cv::Mat frame_left; 
    cv::Mat frame_left_rec;
    cv::Mat intrinsic_matrix_left = M1;
    cv::Mat dist_coeffs_left = D1;
    // 右相机参数设定
    cv::VideoCapture cap_right;
    cap_right.open(1);
    cap_right.set(cv::CAP_PROP_FRAME_WIDTH,320);
    cap_right.set(cv::CAP_PROP_FRAME_HEIGHT,240);
    cv::Mat frame_right;
    cv::Mat frame_right_rec;
    cv::Mat intrinsic_matrix_right = M2;
    cv::Mat dist_coeffs_right = D2;
    // 双目相机立体校正参数计算
    cv::Mat stereo_rotation_cam_right = R;
    cv::Mat stereo_translation_cam_right = T;
    cv::Mat _R1, _R2, _P1, _P2, _Q, _map11, _map12, _map21, _map22;
    cv::Rect validRoiL, validRoiR;   //左右相机立体修正后有效像素的区域
    cv::stereoRectify(intrinsic_matrix_left, dist_coeffs_left, intrinsic_matrix_right, dist_coeffs_right, image_size, stereo_rotation_cam_right, -stereo_rotation_cam_right*stereo_translation_cam_right,\
    _R1, _R2, _P1, _P2, _Q, cv::CALIB_ZERO_DISPARITY, -1, image_size, &validRoiL, &validRoiR);
    cv::initUndistortRectifyMap(intrinsic_matrix_left, dist_coeffs_left, _R1, _P1, image_size, CV_16SC2, _map11, _map12);
    cv::initUndistortRectifyMap(intrinsic_matrix_right, dist_coeffs_right, _R2, _P2, image_size, CV_16SC2, _map21, _map22);

    ros::Rate loop_rate(30);                    
    while(ros::ok())                     
    {
        static int cam_left_update = 0;
        static int cam_right_update = 0;
        // 更新左相机图像
        if( cap_left.read(frame_left))
        {
            cam_left_update = 1;
        }
        // 更新右相机图像
        if( cap_right.read(frame_right))
        {
            cam_right_update = 1;
        }
        // 只有当两个相机都更新时，才执行修正操作
        if((cam_left_update==1) && (cam_right_update==1))
        {
            cam_left_update = 0;    cam_right_update = 0;
            // 左右相机修正
            cv::remap(frame_left, frame_left_rec, _map11, _map12, cv::INTER_LINEAR);
            rectangle(frame_left_rec, validRoiL, cv::Scalar(0, 0, 255));  
            cv::remap(frame_right, frame_right_rec, _map21, _map22, cv::INTER_LINEAR);
            rectangle(frame_right_rec, validRoiR, cv::Scalar(0, 0, 255));  
            // 修正后的左右相机图像
            cv::Mat rec_img_pair;
            hconcat(frame_left_rec, frame_right_rec, rec_img_pair);
            for (int j = 0; j < image_size.height; j += 16)
            {
                cv::line(rec_img_pair, cv::Point(0, j), cv::Point(image_size.width * 2, j), cv::Scalar(0, 255, 0));
            }
            // 原始的左右相机图像
            cv::Mat raw_img_pair;
            hconcat(frame_left,frame_right,raw_img_pair);
            for (int j = 0; j < image_size.height; j += 16)
            {
                cv::line(raw_img_pair, cv::Point(0, j), cv::Point(image_size.width * 2, j), cv::Scalar(0, 255, 0));
            }
            // 
            cv::imshow("RectifiedPair",rec_img_pair);
            cv::imshow("RawImagePair",raw_img_pair);
        }
        else
        {
            cam_left_update = 0;    cam_right_update = 0;
        }

        cv::waitKey(1);
        loop_rate.sleep();               
    }
    
    return 0;
}

最终输出的校正结果与原始图像进行对比如下，可以看到，经过校正的左右相机获取的图像，相同的特征处于同一水平线上：

前言

项目需要，基于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)

具体代码，opencv主要用于读图和存图，验证我的校正是否正确。

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

using 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_<double>(3, 3) << 5.3340331777463712e+02, 0., 3.4251343227755160e+02, 0.,
		5.3343402398745684e+02, 2.3475353096292952e+02, 0., 0., 1.);
	Mat distCoeffL = (Mat_<double>(5, 1) << -2.8214652038759541e-01, 4.0840748605552028e-02,
		1.2058218262263004e-03, -1.2307876204068898e-04,
		1.1409651538056684e-01);

	Mat API_Rl = (Mat_<double>(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_<double>(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<uchar>(i)[j] = (uchar)((1 - u)*(1 - v)*srcImg.ptr<uchar>(int(y))[int(x)] + (1 - u)*v*srcImg.ptr<uchar>(int(y + 1))[int(x)]
					+ u*(1 - v)*srcImg.ptr<uchar>(int(y))[int(x + 1)] + u*v*srcImg.ptr<uchar>(int(y + 1))[int(x + 1)]);
				//cout << (int)(dstImg.ptr<uchar>(i)[j]) << endl;
			}
			else
			{
				dstImg.ptr<uchar>(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 = 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<uchar>(i)[j] = (((InvX*srcImg.ptr<uchar>(y)[x] + PartX*srcImg.ptr<uchar>(y)[x+1])*InvY +
					(InvX*srcImg.ptr<uchar>(y+1)[x] + PartX*srcImg.ptr<uchar>(y + 1)[x + 1])*PartY) >> 22);
				//cout << (int)(dstImg.ptr<uchar>(i)[j]) << endl;
			}
			else
			{
				dstImg.ptr<uchar>(i)[j] = 0;
			}

		}
	}
}

实际效果

可以看出，校正后的实际效果是一致的！

问题求助

1.按理说，定点运算应该快于浮点运算，但实际使用时我发现两者的执行效率几乎相等（PC端、ARM开发板端都验证过），有懂的大佬，可以指教下吗？
2.自己写的代码，debug模式下，执行速度是opencvAPI的一倍，有大佬能指点下加速吗？

在本篇文章中，将根据上一篇文章得到的两个摄像机的相对位置进行图像的立体校正

1. 引言
   当两个像平面式完全行对准的，计算立体视差是最简单的。但是两台摄像机几乎不可能有准确的共面和行对准成像平面，完美的对准结构在真实的立体系统中几乎不存在。因此，立体校正的目的是对两台摄像机的图像平面重投影，使得它们精确落在同一个平面上，而且图像的行完全地对准到前向平行的结构上。

   要保证两个摄像机的图像在校正之后是对准的，使得后续的立体匹配更可靠，计算更可行。因为只在图像的一行上面搜索另一图像的匹配点能够可靠性和算法效率。让每个图像平面都落在一个公共成像面上并水平对准的结果是极点都位于无穷远。即一幅图像上的投影中心成像与另一个像平面平行。但是由于可选择的前向平行平面个数是有限的，因此需要更多的约束，包括视图重叠最大化和畸变最小化。

   对准后的两个图像平面后的结果有八项，左右摄像机各四项（畸变向量、旋转矩阵、校正后的摄像机矩阵、未校正后的摄像机矩阵）

2. 计算校正项的两种算法

   1. Hartley算法（使用基本矩阵来生成非标定立体视觉）非标定的立体校正
      1. Hartley算法目的是找到将极点映射到无穷远处的单应矩阵，同时使两幅立体图像之间的计算误差最小化，实现这种算法要通过匹配两幅图像之间的对应点实现，通过这种方法，就可以绕过计算两个摄像机的摄像机内参数，因为这样的内参数信息隐含在匹配点之中
      2. Hartley算法的优缺点
         1) 优点：通过观察场景中的点简便地进行线粒体标定
         2) 缺点：场景图像的比例未知
         
         立体重建具有非唯一性，如果物体的大小未知，那么不同大小物体可以看起来相同，这取决于它们与摄像机之间的距离，如果摄像机参数未知，那么不同投影可以看起来相同—例如，在焦距和主点不同的情况下
      3. 算法描述（假设已知基本矩阵）
         1. 使用基本矩阵，通过关系
            
            计算左右两个极点
         2. 先求第一个单应矩阵H_r ，他将右极点映射到无穷远(1，0，0)^T处的二维齐次点。由于一个单应矩阵有7个约束，使用其中的三个进行无穷远处的映射，剩下的四个自由度进行选择矩阵H_r ，这四个自由度大都容易导致混乱，因为H_r 的大多数选择将导致高度扭曲的图像。为了找到一个较好的H_r ，我们在图像中选择一点产生扭曲的可能性最小，仅允许刚性旋转和平移。对于这一点的一个合理的选择是图像原点，进一步假设极点e_r = (k,0,1)^T落在x轴上，而下述矩阵就会实现将这样的一个点映射到无穷远。
            
         3. 在有图像上选择一个感兴趣的点（这里选择原点），计算点到图像原点的平移矩阵T和将极点指向(e_r )^T = (k，0，1)的旋转矩阵R，则需要的单应性矩阵就是H_r = GRT。
         4. 接下来就是搜索匹配的单应矩阵H_l ，他将左极点发送到无穷远，并保证两幅图像的行对准。通过步骤2中的三个约束可以容易地将座机带你转到无穷远。为了能够行对准，依靠地依据就是行对准使两幅图像地所有匹配点据距离和最小。也就是说，搜寻地H_l ，使得左右匹配点地总的视差最小，也就是
            
            最小，则这两个单应性矩阵就定义了立体校正。
   2. Bouguet算法（使用两台摄像机的旋转和平移参数）标定立体校正
      1. 算法概览
         给定立体图像间地旋转矩阵和平移矩阵（R，T），Bouguet的算法目的是使两图像中的每一幅重投影次数最小化，同时使得观测面积最大化。为了使图像重投影畸变最小，将右摄像机图像平面旋转到左摄像机图像平面的旋转矩阵R被分离成两部分，称左右摄像机的两个合成旋转矩阵r₁和r₂。每个摄像机都旋转一半，这样地旋转可以让摄像机共面但是行不对准。为了计算将左摄像机极点变换到无穷远并使极线水平对准地R_rect。创建一个由极点e_l方向开始的旋转矩阵。
      2. 算法描述
         让主点（c_x，c_y）作为左图像的原点，极点的方向就是两台摄像机投影中心之间的平移向量方向：
         
         下一个向量e₂必须与e₁正交，因此最好的选择就是选择与主光线正交的方向（通常沿着图像平面）。这可以通过计算e₁和主光线方向的叉积来得到，然后将他归一化到单位向量：
         
         这样就可以求出第三个向量e₃，它可以通过e₁和e₂的叉积得到：
         
         此时，将左摄像机的极点转换到无穷远处的矩阵如下：
         
         这个矩阵将左图像绕着投影中心旋转，使得极线变成水平，并且极点在无穷远处。两台摄像机的行对准通过下述公式实现：
         
         计算校正后的左右摄像机矩阵M_{rect_l}和M_{rect_r}，但是与投影矩阵P_l和P_r一起返回：
         
         和
         
         其中α_l 和α_r 是像素畸变比例，在现代计算机中几乎等于0。投影矩阵将齐次坐标中的三维点转换成齐次坐标系下的二维点：
         
         则屏幕坐标为(x/w，y/w)。如果给定屏幕坐标和摄像机内参数矩阵，二维点同样可以重投影到三维中，重投影矩阵如下：
         
         上述公式中，除c_x’外的所有参数都是来自左图像，c_x’是主点在右图像上的x坐标。如果主光线在无穷远处相交，那么c_x = c_x’，并且游侠的项为0。给定一个二维其次点和其关联的视差d，可以将此点投影到三维中：
         
         因此三维坐标就是(X/W，Y/W，Z/W)，在这里d还是一个未知数，在下节将会进行介绍

3. 由上面三篇文章就可以得到如下效果：
   

参考文献：Adrian Kaehler，Gary Bradski，Learning Opencv3[M]，清华大学出版社，2018.7

下面的是笔者的微信公众号，欢迎关注，会持续更新c++、python、tensorflow、机器学习、深度学习、计算机视觉等系列文章，公众号中内含300+本pdf电子书籍，肯定有你需要的一本，关注公众号即可领取哦。

如果你对JAVA方面感兴趣，可以关注下面JAVAERS公众号，陪你一起学习，一起成长，一起分享JAVA路上的诗和远方。在公众号里面都是JAVA这个世界的朋友，公众号每天会有技术类文章，面经干货，也有进阶架构的电子书籍，如Spring实战、SpringBoot实战、高性能MySQL、深入理解JVM、RabbitMQ实战、Redis设计与实现等等一些高质量书籍，关注公众号即可领取哦。