轮子——双目立体校正（C++/OpenCV）

在对双目相机进行标定之后，将在ubuntu系统中进行开发。首先要做的是编写基础程序从双目相机中实时的获取原始图像并对其进行矫正。

准备标定参数

依照《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;
}

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