python二郎成長筆記（三）（matlab標定工具箱詳解，旋轉矩陣旋轉向量，matlab標定數據傳入opencv）

這一章由來：二郎之前研究matlab的雙目立體視覺，已經得到了需要的信息，可是，二郎想要對代碼進行修改使其更適宜自己的應用目的。
修改：標定不用修改……matlab內置的已經很強大了，而且沒有必要用python和c++再做一遍，因此考慮的方法爲——matlab標定後，參數導入到opencv中使用，這也是應用到python的一個原因。

一、matlab標定工具箱

matlab提供了兩種方法：1.用自帶的app進行標定（在雙目立體視覺的文章中有詳細講解）；2.用matlab標定工具箱，像下面這樣的東西

與其說它是一個工具箱，更確切地說它就是沒有被matlab集成到內部，可以看到的m文件。
用法非常簡單，下載在網上找一下，很多，或者找二郎要……吐槽一下，有的博主把這些東西掛到CSDN賣積分……感覺好不地道。
http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example.html（一篇英文教程，不想看英文的也可以接着文章往下看，二郎給大家做一遍）
1.下載的壓縮包解壓……到哪都行

這裏需要注意……點進來之後，要把這個文件夾添加到執行目錄，要不後續無法進行。
2.運行文件

3.選擇進入程序

4.將圖片目錄添加到執行目錄，且進入圖片目錄

5.以此運行，不用設置參數





點完後，去看matlab命令行窗口

上面二郎之前做過所以又記錄，其實是需要輸入棋盤格的尺寸的，也就是那個30,30

…………然後就一直持續標四個點，直至標完所有，二郎認爲大家肯定會出現在標定中小手一抖，標跑偏的現象，沒關係，後面能修改，先把四個點都標上再說。
表完之後點計算標定


你會發現我們想要求得值就出來了。然後用分析一下錯誤，可以看到我們哪張圖錯誤率較大

我們可以用一下重投影，將我們求得內外參數代入公式，算出各點，帶回到圖像，計算誤差（當然，這一步是計算機做的），重投影主要是爲了recop.corners計算提高精度。再次確定角點時，以重投影的角點爲起始角點，開始尋找角點。

做完上面一些步驟後我們的精度已經得到明顯的提高
之後就可以針對單張圖來進行操作了，比如再進行一下步驟3，這次不選所有圖，而是單獨找我們之前標錯的圖，進行重新的角點標註。在重新計算角點時，可以人爲地設置窗口大小，之前默認爲5*5，可以適當調大或者縮小。
多次進行角點檢測的方法
第二次：Recomp.corners→[9,9]
第三次：Recomp.corners→[8,8]
第四次：Recomp.corners→[7,7]
逐漸縮小角點檢測的窗口，避免產生窗口過小，造成的局部誤差。

雙目標定
當我們左右相機的單目標定完成後，將標定結果進行保存，建議保存名字爲
(Calib_Result_left.mat和Calib_Result_right.mat)
運行工具箱裏的stereo_gui.m

導入數據(Calib_Result_left.mat和Calib_Result_right.mat)，進行雙目標定。

二、旋轉矩陣和旋轉向量

從上面的介紹中可以看到matlab已經集成的標定程序和標定工具箱各有各自的優點，弄哪一個都是可以的。在matlab已經集成的標定程序使用時，我們的2相機相對於1相機的旋轉關係爲旋轉矩陣（9個參數），然而，在opencv和python中，多數程序使用的數據爲旋轉向量（3個參數）形式，這裏涉及了一個轉換定理——羅德里格斯（Rodrigues）旋轉向量與矩陣的變換

圖片截取於：https://wenku.baidu.com/view/b471efda6037ee06eff9aef8941ea76e58fa4acb.html?from=search

而具體如何實現旋轉矩陣和旋轉向量的轉換呢？
下面列出了程序
python程序——程序不是二郎寫的，來自github，方便大家看，因此黏貼過來。
https://github.com/blzq/tf_rodrigues/commit/338758887a18f0db4e4a38c949be120d7f5169e3#diff-c5ff043af7299aff712f4dc2fb35bba3

#!/usr/bin/python3
# -*- encoding: utf-8 -*-

import tensorflow as tf

def rodrigues_batch(rvecs):
    """ 
    Convert a batch of axis-angle rotations in rotation vector form shaped
    (batch, 3) to a batch of rotation matrices shaped (batch, 3, 3).
    See
    https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation
    https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
    --- second article is wrong?
    """
    batch_size = tf.shape(rvecs)[0]

    thetas = tf.norm(rvecs, axis=1, keepdims=True)
    u = rvecs / thetas

    # Each K is the cross product matrix of unit axis vectors
    # pyformat: disable
    zero = tf.zeros([batch_size])  # for broadcasting
    Ks_1 = tf.stack([  zero   , -u[:, 2],  u[:, 1] ], axis=1)  # row 1
    Ks_2 = tf.stack([  u[:, 2],  zero   , -u[:, 0] ], axis=1)  # row 2
    Ks_3 = tf.stack([ -u[:, 1],  u[:, 0],  zero    ], axis=1)  # row 3
    # pyformat: enable
    Ks = tf.stack([Ks_1, Ks_2, Ks_3], axis=2)                  # stack rows

    Rs = tf.eye(3, batch_shape=[batch_size]) + \
         tf.sin(thetas)[..., tf.newaxis] * Ks + \
         (1 - tf.cos(thetas)[..., tf.newaxis]) * tf.matmul(Ks, Ks)

    print(Rs.shape)
    return Rs

# For testing only
if __name__ == '__main__':
    import numpy as np
    import cv2
    rvecs = np.random.randn(4, 3).astype(np.float32)

    rvecs_tf = tf.constant(rvecs)
    Rs = rodrigues_batch(rvecs_tf)
    with tf.Session() as sess:
        print("TensorFlow: ")
        print(sess.run(Rs))

    print("\nOpenCV: ")
    for rvec in rvecs:
        mat, _ = cv2.Rodrigues(rvec)
        print(mat.T)
        print()

opencv的程序——來自網上，具體出處不明（輸入旋轉向量則轉化爲旋轉矩陣，輸入旋轉矩陣，則轉化爲旋轉向量）

#include <stdio.h>
#include <cv.h>

void main()
{
    int i;
    double r_vec[3]={-2.100418,-2.167796,0.273330};
    double R_matrix[9];
    CvMat pr_vec;
    CvMat pR_matrix;

    cvInitMatHeader(&pr_vec,1,3,CV_64FC1,r_vec,CV_AUTOSTEP);
    cvInitMatHeader(&pR_matrix,3,3,CV_64FC1,R_matrix,CV_AUTOSTEP);
    cvRodrigues2(&pr_vec, &pR_matrix,0);

    for(i=0; i<9; i++)
    {
        printf("%f\n",R_matrix[i]);
    }
}

同樣，可以去opencv官網：https://docs.opencv.org/3.4.0/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac

Mat rvec1, tvec1;
solvePnP(objectPoints, corners1, cameraMatrix, distCoeffs, rvec1, tvec1);#求旋轉矩陣
Mat rvec2, tvec2;
solvePnP(objectPoints, corners2, cameraMatrix, distCoeffs, rvec2, tvec2);
Mat R1, R2;
Rodrigues(rvec1, R1);#實現旋轉矩陣向旋轉向量的轉換
Rodrigues(rvec2, R2);

三、opencv所需標定結果格式（matlab）

Radial：徑向畸變，光學透鏡的特性使得成像存在着徑向畸變（k1，k2，k3），k3可以有，也可以爲0。
Tangen：切向畸變，裝配方面的誤差，傳感器與光學鏡頭不在同一水平線，可由兩個參數P1，P2確定。
這裏提一下，畸變參數的排列（K1,K2,P1,P2,K3）（用在opencv中）

Mat cameraMatrixL = (Mat_<double>(3, 3) << 904.0589, 0, 656.6037,
    0, 869.2423, 308.9319,
    0, 0, 1);
//對應matlab裏的左相機標定矩陣

Mat distCoeffL = (Mat_<double>(5, 1) << 0.3370, -0.1561, -0.0245, 4.2596e-4, 0.0000);
//對應Matlab所得左i相機畸變參數
        
Mat cameraMatrixR = (Mat_<double>(3, 3) << 903.1359, 0, 659.2396,
    0, 870.4047, 321.2040,
    0, 0, 1);
//對應matlab裏的右相機標定矩陣

Mat distCoeffR = (Mat_<double>(5, 1) << 0.3338, -0.0631, -0.0105, -0.0022, 0.00000);
//對應Matlab所得右相機畸變參數

Mat T = (Mat_<double>(3, 1) << -118.9937, 2.5308, -1.8361);//T平移向量
                                                    //對應Matlab所得T參數
Mat rec = (Mat_<double>(3, 1) << XXXXX, XXXXXX,XXXXX);//rec旋轉向量，對應matlab om參數，這裏需要把旋轉矩陣變爲旋轉向量
Mat R;//R 旋轉矩陣

下面opencv的代碼可以參照博文：https://blog.csdn.net/xiao__run/article/details/78900652