opencv4学习笔记（四）单目摄像机标定与坐标解算，实现原理+标定程序+解算程序

在平面检测和简单的机器人设计中我们经常会遇到一些图像定位的问题。举个例子：我只有一个普通的单目相机，通过拍摄前方的物体，我想要获得那个物体在世界坐标中的大概位置，然后控制机器人在位置坐标下进行运动，那我需要做点什么工作呢？

首先-相机标定

相机的标定的方法有很多，就不介绍了，绝大部分方法都是20多年前就被研究透的东西。
我们直接用张正有标定法，上棋盘。
通过标定获得至少几个内参外参的有效参数：
1、内参：内参矩阵、镜头畸变系数
2、外参：旋转矩阵、平移向量
使用张正右标定法标定时，通过对特定大小的棋盘拍摄多张图像通过OpenCV计算机开源视觉库相关函数的调用可以计算出相机的内外参数。

相机标定的具体代码实现

这里我使用的是OpenCV4.5.1+Visual Studio 2019进行编译运行的。
代码看文章最后，相关内容写在代码注释里了

相机坐标系转换为机器人世界坐标系

在实际计算中，通过镜头的选用，使用无畸变的镜头可以忽略一部分因为畸变矫正导致的误差。在实际的实验测试中，只使用旋转矩阵+平移向量+内参矩阵三个参数就可以实现坐标的转换，精度误差可以控制到5mm以内（测试视角场距1m—2.5m），在不是特别高精度的场景下有较好的实际价值。
在单目相机的标定中主要是完成对摄像机坐标系、世界坐标系、图像坐标系三者之间的转换，通过相机的标定可以还原摄像头成像物体在真实世界的位置，此外相机标定还用于解决相机透视投影中造成的畸变问题。为了实现坐标转换和畸变矫正，需要求解相机的内参和外参。
相机的内参包含径向畸变系数和切向畸变。径向畸变是由镜片原因导致的光线在距离投透镜中心位置较远的位置发生偏移，使得图像的边缘呈现出弯曲的效果。切向畸变是由于摄像头在制造过程中感光元件平面与镜片平面不平行导致的，会使图像发生位置的偏移。
相机外参包含旋转矩阵R和平移向量T。通过在三维空间中，旋转可以分解为绕各自坐标轴的二维旋转，其中旋转的轴线的度量保存不变。如果依次绕x，y，z轴旋转角度ψ，φ和θ，那么总的旋转矩阵R是三个矩阵Rx(ψ)，Ry(φ)，Rz(θ)的乘积。

解算原理

核心就是基本的矩阵运算（坐标变换）
通过相机的标定我们可以得到相机的内参fx,fy,cx,cy和相机的外参旋转矩阵R和平移向量t
所可以得到几个基本坐标系之间的关系是：

其中s为比例系数，u,v为相机像素坐标，f,c为相机内参系数，r为旋转矩阵，t为平移向量，X,Y,Z为对应的世界坐标系。对上式矩阵进行变换和处理后将标定板所在平面设为原点，可以求出s。
再使用咋们一年级就滚瓜烂熟的矩阵变换对上面的矩阵变下型，提取系数，就可以得到：
其中X,Y,Z就是世界坐标系的坐标，a和b作为中间变量进行代换求解。

实现代码

这里直接上代码，把你的待求的坐标点以列表的方式输入会直接返回一个坐标值（x,y）就是实际的世界平面坐标系下的位置，要注意的是计算出来的世界坐标系是以标定的到的平移向量为原点参数进行计算的，说人话就是计算出来的坐标原点是标定板的第一个角点位置。

坐标系原点的设置

如果你还有原点的坐标需求，要自己设定原点的话，就按照你的原点相对标定原点再进行一次平移向量的矩阵相乘就可以得到自己设定位置的坐标。
也有一种简单做法，就是让标定板的第一个角点放置在屏幕画幅中心或者边角上，这样可以减少计算量，直接在输出坐标上进行坐标偏移就可以得到。
代码中的标定参数仅供参考

# ==================================================
#           将像素坐标转换为基平面世界坐标
#			import numpy as np
# ==================================================
def camera2CalibrationPlate(u: int, v: int) -> list:
    Zw = 0
    R = np.mat(  # 旋转矩阵
        [[-0.00763091786546044, 0.9990390541494044, -0.04315944134014643],
         [0.6759219860210318, -0.02665424902680036, -0.7364910181544654],
         [-0.7369336726639069, -0.03479251777437586, -0.6750690652081499]])
    T = np.mat(  # 平移向量
        [[-371.4581236000316],
         [-285.1813404415687],
         [1201.711351715502]])
    I = np.mat(  # 内参矩阵
        [[651.407130825223, 0, 475.8215062004672],
         [0, 894.8875019426742, 515.0402504930715],
         [0, 0, 1]]
    )
    imagePoint = np.mat([[u],
                         [v],
                         [1]])

    leftSideMat = R.I * I.I * imagePoint
    rightSideMat = R.I * T
    s = Zw + rightSideMat[2] / leftSideMat[2]
    s = float(s[0])
    wcPoint = R.I * (s * I.I * imagePoint - T)
    mappoint = [float(wcPoint[1]), float(wcPoint[0])]
    return mappoint

求解相机参数

运行程序后会在Vsion_makeCheckerboard.cpp的218行指定路径下读取所有.jpg后缀图片。（无效图片会有相应警告，请删除后再次运行程序）
运行成功结束后相关的内外参数据会在对应工程的输出文件路径中保存为caliberation_result.txt文件。
Vsion_makeCheckerboard.cpp的204行留了矫正图像并保存的程序，主函数中调用注释。
具体的这份代码可以完全重写为python程序，因为项目需要我只写了python的坐标解算，其它部分代码有兴趣的道友可以自行尝试。
Vsion_makeCheckerboard.cpp

#include
#include
#include

#include "vision.h"

using namespace cv;
using namespace std;


/*
@param File_Directory 为文件夹目录
@param FileType 为需要查找的文件类型
@param FilesName 为存放文件名的容器
*/
void getFilesName(string& File_Directory, string& FileType, vector& FilesName)
{
    string buffer = File_Directory + "\\*" + FileType;

    _finddata_t c_file;   // 存放文件名的结构体

    intptr_t hFile;
    hFile = _findfirst(buffer.c_str(), &c_file);   //找第一个文件命

    if (hFile == -1L)   // 检查文件夹目录下存在需要查找的文件
        std::cout << "No %s files in current directory!\n" << FileType << std::endl;
    //printf("No %s files in current directory!\n", FileType);
    else
    {
        string fullFilePath;
        do
        {
            fullFilePath.clear();

            //名字
            fullFilePath = File_Directory + "\\" + c_file.name;

            FilesName.push_back(fullFilePath);

        } while (_findnext(hFile, &c_file) == 0);  //如果找到下个文件的名字成功的话就返回0,否则返回-1  
        _findclose(hFile);
    }
}

void m_calibration(vector& FilesName, Size board_size, Size square_size, Mat& cameraMatrix, Mat& distCoeffs, vector& rvecsMat, vector& tvecsMat)
{
    ofstream fout("caliberation_result.txt");                       // 保存标定结果的文件 

    cout << "开始提取角点………………" << endl;
    int image_count = 0;                                            // 图像数量 
    Size image_size;                                                // 图像的尺寸 

    vector image_points;                                   // 缓存每幅图像上检测到的角点
    vector> image_points_seq;                       // 保存检测到的所有角点

    for (int i = 0; i < FilesName.size(); i++)
    {
        image_count++;

        // 用于观察检验输出
        cout << "image_count = " << image_count << endl;
        Mat imageInput = imread(FilesName[i]);
        if (image_count == 1)  //读入第一张图片时获取图像宽高信息
        {
            image_size.width = imageInput.cols;
            image_size.height = imageInput.rows;
            cout << "image_size.width = " << image_size.width << endl;
            cout << "image_size.height = " << image_size.height << endl;
        }

        /* 提取角点 */
        bool ok = findChessboardCorners(imageInput, board_size, image_points, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);
        if (0 == ok)
        {
            cout << "第" << image_count << "张照片" << FilesName[i] << "提取角点失败，请删除后，重新标定！" << endl; //找不到角点
            imshow("失败照片", imageInput);
            waitKey(0);
        }
        else
        {
            Mat view_gray;
            cout << "imageInput.channels()=" << imageInput.channels() << endl;
            cvtColor(imageInput, view_gray, COLOR_BGR2GRAY);

            /* 亚像素精确化 */
            //find4QuadCornerSubpix(view_gray, image_points, Size(5, 5)); //对粗提取的角点进行精确化
            cv::cornerSubPix(view_gray, image_points, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 20, 0.01));

            image_points_seq.push_back(image_points);  //保存亚像素角点

            /* 在图像上显示角点位置 */
            drawChessboardCorners(view_gray, board_size, image_points, true);

            imshow("Camera Calibration", view_gray);//显示图片
            waitKey(100);//暂停0.1S     
        }
    }
    cout << "角点提取完成！！！" << endl;


    /*棋盘三维信息*/
    vector> object_points_seq;                     // 保存标定板上角点的三维坐标

    for (int t = 0; t < image_count; t++)
    {
        vector object_points;
        for (int i = 0; i < board_size.height; i++)
        {
            for (int j = 0; j < board_size.width; j++)
            {
                Point3f realPoint;
                /* 假设标定板放在世界坐标系中z=0的平面上 */
                realPoint.x = i * square_size.width;
                realPoint.y = j * square_size.height;
                realPoint.z = 0;
                object_points.push_back(realPoint);
            }
        }
        object_points_seq.push_back(object_points);
    }

    /* 运行标定函数 */
    double err_first = calibrateCamera(object_points_seq, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, CALIB_FIX_K3);
    fout << "重投影误差1：" << err_first << "像素" << endl << endl;
    cout << "标定完成！！！" << endl;


    cout << "开始评价标定结果………………";
    double total_err = 0.0;            // 所有图像的平均误差的总和 
    double err = 0.0;                  // 每幅图像的平均误差
    double totalErr = 0.0;
    double totalPoints = 0.0;
    vector image_points_pro;     // 保存重新计算得到的投影点

    for (int i = 0; i < image_count; i++)
    {

        projectPoints(object_points_seq[i], rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points_pro);   //通过得到的摄像机内外参数，对角点的空间三维坐标进行重新投影计算

        err = norm(Mat(image_points_seq[i]), Mat(image_points_pro), NORM_L2);

        totalErr += err * err;
        totalPoints += object_points_seq[i].size();

        err /= object_points_seq[i].size();
        //fout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
        total_err += err;
    }
    fout << "重投影误差2：" << sqrt(totalErr / totalPoints) << "像素" << endl << endl;
    fout << "重投影误差3：" << total_err / image_count << "像素" << endl << endl;


    //保存定标结果    
    cout << "开始保存定标结果………………" << endl;
    Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 保存每幅图像的旋转矩阵 */
    fout << "相机内参数矩阵：" << endl;
    fout << cameraMatrix << endl << endl;
    fout << "畸变系数：\n";
    fout << distCoeffs << endl << endl << endl;
    for (int i = 0; i < image_count; i++)
    {
        fout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
        fout << rvecsMat[i] << endl;

        /* 将旋转向量转换为相对应的旋转矩阵 */
        Rodrigues(rvecsMat[i], rotation_matrix);
        fout << "第" << i + 1 << "幅图像的旋转矩阵：" << endl;
        fout << rotation_matrix << endl;
        fout << "第" << i + 1 << "幅图像的平移向量：" << endl;
        fout << tvecsMat[i] << endl << endl;
    }
    cout << "定标结果完成保存！！！" << endl;
    fout << endl;
}

//void m_undistort(vector& FilesName, Size image_size, Mat& cameraMatrix, Mat& distCoeffs)
void m_undistort(vector& FilesName, Mat& cameraMatrix, Mat& distCoeffs)
{

    // Mat mapx = Mat(image_size, CV_32FC1);   //X 坐标重映射参数
   //  Mat mapy = Mat(image_size, CV_32FC1);   //Y 坐标重映射参数
    Mat R = Mat::eye(3, 3, CV_32F);
    cout << "保存矫正图像" << endl;
    string imageFileName;                  //校正后图像的保存路径
    stringstream StrStm;
    string temp;

    for (int i = 0; i < FilesName.size(); i++)
    {
        Mat imageSource = imread(FilesName[i]);

        Mat newimage = imageSource.clone();

        //方法一：使用initUndistortRectifyMap和remap两个函数配合实现
        //initUndistortRectifyMap(cameraMatrix,distCoeffs,R, Mat(),image_size,CV_32FC1,mapx,mapy);
        //  remap(imageSource,newimage,mapx, mapy, INTER_LINEAR);

        //方法二：不需要转换矩阵的方式，使用undistort函数实现
        undistort(imageSource, newimage, cameraMatrix, distCoeffs);

        StrStm << i + 1;
        StrStm >> temp;
        imageFileName = "E:\\1\\2\\...........你自己的文件路径" + temp + "_d.jpg";
        imwrite(imageFileName, newimage);

        StrStm.clear();
        imageFileName.clear();
    }
    std::cout << "保存结束" << endl;
}



void main()
{
  
    string File_Directory1 = "E:\\1\\2\\.........你的文件夹路径";   //文件夹目录1
    string FileType = ".jpg";    // 需要查找的文件类型

    vectorFilesName1;    //存放文件名的容器

    getFilesName(File_Directory1, FileType, FilesName1);   // 标定所用图像文件的路径


    Size board_size = Size(9, 6);                          // 标定板上每行、列的角点数 
    Size square_size = Size(40, 40);                       // 实际测量得到的标定板上每个棋盘格的物理尺寸，单位mm

    Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));        // 摄像机内参数矩阵
    Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));          // 摄像机的5个畸变系数：k1,k2,p1,p2,k3
    vector rvecsMat;                                          // 存放所有图像的旋转向量，每一副图像的旋转向量为一个mat
    vector tvecsMat;                                          // 存放所有图像的平移向量，每一副图像的平移向量为一个mat

    m_calibration(FilesName1, board_size, square_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat);

    //m_undistort(FilesName1, cameraMatrix, distCoeffs);
    /*这段注释掉的程序是C++版本进行坐标解算的
    cv::Mat image = cv::imread("E:\\CV\\Project\\MonocularSearch\\image\\bord_new\\bord\\56.jpg", 1);
    
    cv::imshow("test", image);

    double u = 330;
    double v = 430;
    cv::circle(image, cv::Point(330, 447), 2, (255, 255, 255));
    cv::Point3d p;
    int i = camera2CalibrationPlate(u, v, p);
    u = 310;
    v = 430;
    i = camera2CalibrationPlate(u, v, p);*/

    return  ;
}

vision.cpp

//
// Created by czh on 18-10-16.
//

#include "vision.h"


using namespace std;
using namespace cv;

const char* findName(const char* ch) {
    const char* name = strrchr(ch, '/');
    return ++name;
}

cv::Mat Vision::read(std::string file_path, int flags) {
    printf("#Vision read\n");
    cv::Mat img;
    img = cv::imread(file_path, flags);
    if (img.data == NULL) {
        printf("\tError:vision read\n");
    }
    else {
        dispConfig(img);
    }
    return img;
}

void Vision::dispConfig(cv::Mat img) {
    printf("\tpixel:%d*%d, channels:%d\n", img.size().width, img.size().height, img.channels());
}

/**
* @palam : 生成矩阵
* cv::Mat std_cb = Vision::makeCheckerboard(1280, 1280, 8, 8, 0, (char*)"./std_cb.png");
    cv::imshow("board", std_cb);
*/
cv::Mat Vision::makeCheckerboard(int bkgWidth, int bkgHeight, int sqXnum, int sqYnum, int thickNum, char* savePath) {
    if (sqYnum == 0) {
        sqYnum = sqXnum;
    }
    if (savePath == NULL) {
        char* defaultPath = (char*)"./maths.png";
        savePath = defaultPath;
    }
    int checkboardX = 0;//棋盘x坐标
    int checkboardY = 0;//棋盘y坐标
    int xLen = bkgWidth / sqXnum;//x方格长度
    int yLen = bkgHeight / sqYnum;//y方格长度
    cv::Mat img(bkgHeight + thickNum * 2, bkgWidth + thickNum * 2, CV_8UC4, cv::Scalar(0, 255, 255, 255));
    for (int i = 0; i < img.rows; i++) {

        for (int j = 0; j < img.cols; j++) {

            if (i < thickNum || i >= thickNum + bkgHeight || j < thickNum || j >= thickNum + bkgWidth) {
                img.at>(i, j) = cv::Scalar(0, 0, 0, 255);
                continue;
            }
            checkboardX = j - thickNum;
            checkboardY = i - thickNum;
            if (checkboardY / yLen % 2 == 0) {
                if ((checkboardX) / xLen % 2 == 0) {
                    img.at>(i, j) = cv::Scalar(255, 255, 255, 255);
                }
                else {
                    img.at>(i, j) = cv::Scalar(0, 0, 0, 255);
                }
            }
            else {
                if ((checkboardX) / xLen % 2 != 0) {
                    img.at>(i, j) = cv::Scalar(255, 255, 255, 255);
                }
                else {
                    img.at>(i, j) = cv::Scalar(0, 0, 0, 255);
                }
            }
        }
    }
    imwrite(savePath, img);    //保存生成的图片
    printf("#makeCheckerboard %d*%d\n", bkgWidth + thickNum, bkgHeight + thickNum);
    return img;
}

/** @brief 相机标定求标定板所在平面的三维坐标
*	@param 	u	像素坐标U
*	@param 	u	像素坐标V
*	@param 	p	所求的三维坐标
*/
int camera2CalibrationPlate(double& u, double& v, cv::Point3d& p)
{
    double Zw = 0;
    double s;
    Mat_R = (Mat_(3, 3) <<      //外参旋转矩阵
        -0.02293182308418196, 0.4912945241097036, -0.8706915768915278,
        0.9994614476731797, -0.009181699406977994, -0.03150414275982888,
        -0.02347224115987984, -0.8709451113450373, -0.4908193831941774);
    Mat_T = (Mat_(3, 1) <<       //外参平移矩阵
        -104.7131542534011,
        -63.76964834607463,
        2118.538077815101);
    cv::Mat imagePoint = cv::Mat::ones(3, 1, CV_64F);    //像素矩阵   
    imagePoint.at(0, 0) = u;
    imagePoint.at (1, 0) = v;
 //   std::cout << imagePoint << "imagePoint" << std::endl;
    Mat_I = (Mat_(3, 3) <<       //相机内参
        2999.721229466124, 0, 480.3508235539542,
        0, 1266.192221576314, 485.7521984650587,
        0, 0, 1);
    cv::Mat leftSideMat = cv::Mat::ones(3, 1, CV_64F);
    cv::Mat rightSideMat = cv::Mat::ones(3, 1, CV_64F);
    /*
    * 旋转矩阵的逆×内参的逆×像素坐标×比例系数 = 世界坐标+旋转矩阵的逆×平移向量   ------------ 1
    * leftSideMat = 旋转矩阵的逆×内参的逆×像素坐标                                 ------------ 2
    * rightSideMat = 旋转矩阵的逆×平移向量                                          ------------ 3
    *       leftSideMat × 比例系数S = 世界坐标 + rightSideMat                       ------------ 4
    *       世界坐标为（0,0,0）  S =  rightSideMat与leftSideMat的商                  ------------ 5
    * 1式移项并提取公因式之后有：
    *       世界坐标 = 旋转矩阵的逆×（内参的逆×像素坐标×比例系数 - 平移向量）     ------------ 6
    */
    leftSideMat = R.inv() * I.inv() * imagePoint;   //------------ 2
    rightSideMat = R.inv() * T;         //------------ 3
    s = Zw + rightSideMat.at(2, 0) / leftSideMat.at(2, 0); //求取比例系数时默认棋盘的第一个角点是坐标原点，世界坐标为（0,0,0）------------ 4
    std::cout << "s=" << s << std::endl;;
    //std::cout << "leftsidemat is :" << leftSideMat << std::endl;
    //std::cout << "rightSideMat is :" << rightSideMat << std::endl;

    //计算世界坐标
    Mat wcPoint; //= Mat::ones(3, 1, CV_32F);
    wcPoint = R.inv() * (s * I.inv() * imagePoint - T);     //------------ 6

   // std::cout << "wcPoint is :" << wcPoint << std::endl;
 //   std::cout << "imgepoint=" << imagePoint << std::endl;
    cv::Point3d worldPoint(wcPoint.at(0, 0), wcPoint.at(1, 0), wcPoint.at(2, 0));
    p = worldPoint;
    std::cout << "caliworldPoint" << worldPoint << std::endl;
    return 0;
}

vision.h

#ifndef OPENGL_PRO_VISION_H
#define OPENGL_PRO_VISION_H

#include "opencv2/opencv.hpp"
#include 
#include 
#include 

#include "string.h"
#include 
#include  
#include 
#include 
#include  

class Vision {
public:
    static cv::Mat read(std::string file_path, int flags = cv::IMREAD_ANYCOLOR | cv::IMREAD_ANYDEPTH);

    static cv::Mat write(std::string file_path, int flags = cv::IMREAD_ANYCOLOR | cv::IMREAD_ANYDEPTH);

    static void dispConfig(cv::Mat img);

    static cv::Mat makeCheckerboard(int bkgWidth, int bkgHeight, int sqXnum, int sqYnum = 0, int borderThickness = 0, char* savePath = NULL);
private:

};

int camera2CalibrationPlate(double& u, double& v, cv::Point3d& p);

#endif //OPENGL_PRO_VISION_H

Code and life
如果生命是一册事先装帧编好页码的空白书，放不下的过往就好比是撰写某几页时笔力太重磨痕渗透，无法磨灭了。抉择的当下就好比是写到某章的结尾，该翻页留下目之所及的空白。时光，有时便会重叠到一个人身上，披着墨痕，却无一字。