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