SLAM開發之雙目圖像獲取及畸變校正輸出

在對雙目相機進行標定之後，將在ubuntu系統中進行開發。首先要做的是編寫基礎程序從雙目相機中實時的獲取原始圖像並對其進行矯正。

坑的備忘

神坑出現： 在之前的博客《SLAM開發之雙目標定（MATLAB）》中，雙目攝像頭工作在windows系統中，可以同時以1280x720分辨率打開並採集圖像。但是在ubuntu系統中，我們的小雙目攝像頭是usb2.0接口的，且當一個攝像頭工作時，系統會給它較大的USB帶寬，使得另一個攝像頭不能正常工作。
無奈，在不更換設備的條件下，將兩個攝像頭的分辨率修改爲 320x240，這樣纔可以正常工作如下圖

打開攝像頭的程序是C++編寫的，並基於ROS系統開發，給出ROS節點代碼如下：

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sstream>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

int main(int argc,char **argv)
{
    ros::init(argc,argv,"starter_node");       
    ros::NodeHandle nh;                           
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
    
    cv::namedWindow("LeftRaw",cv::WINDOW_AUTOSIZE);
    cv::VideoCapture cap_left;
    cap_left.open(1);
    cap_left.set(cv::CAP_PROP_FRAME_WIDTH,320);		// 設定左攝像頭分辨率爲320x240
    cap_left.set(cv::CAP_PROP_FRAME_HEIGHT,240);
    cv::Mat frame_left;

    cv::namedWindow("RightRaw",cv::WINDOW_AUTOSIZE);
    cv::VideoCapture cap_right;
    cap_right.open(0);
    cap_right.set(cv::CAP_PROP_FRAME_WIDTH,320);		// 設定右攝像頭分辨率爲320x240
    cap_right.set(cv::CAP_PROP_FRAME_HEIGHT,240);
    cv::Mat frame_right;

    ros::Rate loop_rate(30);                    
    while(ros::ok())                     
    {
        if( cap_left.read(frame_left))// && cap_right.read(frame_right) )
        {
            cv::imshow("LeftRaw",frame_left);
        }
        
        if( cap_right.read(frame_right))
        {
            cv::imshow("RightRaw",frame_right);
        }
        cv::waitKey(1);	// 不可缺少
        loop_rate.sleep();               
    }
    
    return 0;
}

重新標定及參數

由於分辨率改變了，依照《SLAM開發之雙目標定（MATLAB）》重新標定了一波，這次的標定結果給出如下

左相機內參 (使用前需要對齊進行轉置)
stereoParams.CameraParameters1.IntrinsicMatrix

[3.790085669382943e+02,0,0]
[0.089771599149422,3.784075446688422e+02,0]
[1.696407742620804e+02,1.115501716419414e+02,1]

左相機徑向畸變(K1, K2, K3)
stereoParams.CameraParameters1.RadialDistortion

[0.005412083574223,1.332325389427173,-6.264793798758338]

左相機切向畸變(P1, P2)
stereoParams.CameraParameters1.TangentialDistortion

[0.001877877086867,5.802366904245328e-04]

右相機內參
stereoParams.CameraParameters2.IntrinsicMatrix

[3.749698617187610e+02,0,0]
[-0.440183986709164,3.750405482867855e+02,0]
[1.634261567100395e+02,1.234245778745944e+02,1]

右相機徑向畸變(K1, K2, K3)
stereoParams.CameraParameters2.RadialDistortion

[0.053974595897912,0.395589637510881,-2.942725149883875]

右相機切向畸變(P1, P2)
stereoParams.CameraParameters2.TangentialDistortion

[0.003505590151115,-0.009821849279182]

兩個攝像頭的旋轉參數
stereoParams.RotationOfCamera2

[0.999724179381092,-1.053695181268993e-04,-0.023485188056864]
[4.759125227745664e-04,0.999875489948082,0.015772701365146]
[0.023480601952937,-0.015779527823990,0.999599753818287]

兩個攝像頭的平移參數
stereoParams.TranslationOfCamera2

[-48.809756232266146,0.192057705977703,-8.496007179190105]

畸變校正輸出

畸變校正直接用opencv提供的校正函數

cv::undistort(InputArray 	src,	//有畸變的原始圖像
OutputArray 	dst,	//經過校正的圖像
InputArray 	cameraMatrix,	//內參矩陣
InputArray 	distCoeffs,	//鏡頭畸變參數
InputArray 	newCameraMatrix = noArray() )

依據OpenCV的官方文檔：
“The function is simply a combination of cv::initUndistortRectifyMap (with unity R ) and cv::remap (with bilinear interpolation). ”，該函數是cv::initUndistortRectifyMap和cv::remap的結合，所以直接一行代碼執行畸變校正
以左攝像頭爲例，內參矩陣和鏡頭畸變參數向量按照如下格式賦值

    cv::Mat intrinsic_matrix_left = (cv::Mat_<double>(3,3)<<379.008566938294,0.0897715991494220,169.640774262080,0,378.407544668842,111.550171641941,0,0,1);
    cv::Mat dist_coeffs_left = (cv::Mat_<double>(5,1)<<0.005412083574223,1.332325389427173,0.001877877086867,5.802366904245328e-04,-6.264793798758338);

修正代碼如下
其中，frame_left爲原始圖像，frame_left_undistort爲修正後的圖像，均爲Mat類型

cv::undistort(frame_left,frame_left_undistort,intrinsic_matrix_left,dist_coeffs_left); 

最後得到的左右目同時輸出校正前後的圖像，畸變程度都很小，校正後的兩幅圖如下：