ransac算法图像匹配 matlab_相机标定（张正友标定算法）解读与实战三

相机标定系列

1. 相机标定(张正友标定算法)解读与实战一

2. 相机标定(张正友标定算法)解读与实战二

前两篇文章偏重理论，介绍了针孔相机模型、镜头畸变模型和张氏标定的原理。今天主要讲解代码实现，虽然很多成熟视觉框架已经包含了相机标定，opencv 、matlab、halcon、ros， 为了更深入的结合原理，还是有必要自己码一遍。并在最后附上github 源码地址。

这里数据采用OpenCV data 中提供的left 和right标定图像，我这里用left的，共有13张，棋盘格BorderSize是 9x6，单元格大小SquareSize是 25x25。
用到的库：
OpenCV (图像库)
Eigen3 (矩阵库)
Ceres (算法优化库)

算法实现步骤：

1. 提取图像的棋盘格角点 imagePoints
2. 设置棋盘格角点对应的世界坐标点(z=0) objectPoints
3. 计算imagePoints 与 objectPoints 对应的单应性矩阵 共有13个H
4. 对H进行分解，构建Vb = 0, 计算B矩阵和内参矩阵K
5. 根据K计算，每张棋盘格的外参R和t
6. 把畸变系数也考虑进去构建最优化算法，求出最优K 和 畸变系数 k1、k2、k3、p1、p2。

1.提取图像的棋盘格角点

 // 准备数据 13张 std::vector<std::string> files = {        "../data/images/left01.jpg",        "../data/images/left02.jpg",        "../data/images/left03.jpg",        "../data/images/left04.jpg",        "../data/images/left05.jpg",        "../data/images/left06.jpg",        "../data/images/left07.jpg",        "../data/images/left08.jpg",        "../data/images/left09.jpg",        "../data/images/left11.jpg",        "../data/images/left12.jpg",        "../data/images/left13.jpg",        "../data/images/left14.jpg",    }; // 2. 提取棋盘格角点    std::vector<std::vector<:vector2d>> imagePoints;    std::vector<std::vector<:vector3d>> objectPoints;    cv::Size boardSize(9, 6); // 棋盘格大小    cv::Size2f squareSize(25., 25.); // 单元格大小， 单位mm    for(int i=0; i        cv::Mat img =  cv::imread(files[i]);        std::vector<:point2f> corners;        bool ok = cv::findChessboardCorners(img, boardSize, corners, cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);        if(ok) {            cv::Mat gray;            cv::cvtColor(img, gray, CV_BGR2GRAY);            cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.001));           cv::drawChessboardCorners(img, boardSize, cv::Mat(corners), ok);           cv::imshow("corners", img);           cv::waitKey(100);            std::vector<:vector2d> _corners;            for(auto& pt: corners){                _corners.push_back(Eigen::Vector2d(pt.x, pt.y));            }            imagePoints.push_back(_corners);        }    }

2. 设置棋盘格角点对应的世界坐标点

 //3.0 设置世界坐标    for(int i=0; i        std::vector<:vector3d> corners;        getObjectPoints(boardSize, squareSize, corners);        objectPoints.push_back(corners);    }void getObjectPoints(const cv::Size& borderSize, const cv::Size2f& squareSize, std::vector<:vector3d>& objectPoints) {    for(int r=0; r    {        for(int c=0; c            objectPoints.push_back(Eigen::Vector3d(c*squareSize.width, r*squareSize.height, 0.));        }    }}

3. 计算imagePoints 与 objectPoints 对应的单应性矩阵

之前已经写过关于单应性矩阵的求解DLP算法，具体代码中已经包含。这里直接贴出来OpenCV求解。

bool findHomographyByOpenCV(std::vector<:vector2d>& srcPoints, std::vector<:vector2d>& dstPoints, Eigen::Matrix3d& H) {    std::vector<:point2f> objectPoints, imagePoints;    for(int i=0; i        objectPoints.push_back(cv::Point2f(srcPoints[i](0), srcPoints[i](1)));        imagePoints.push_back(cv::Point2f(dstPoints[i](0), dstPoints[i](1)));    }    cv::Mat hMat = findHomography(objectPoints, imagePoints, cv::RANSAC);    cv::cv2eigen(hMat, H);}

4. 对H进行分解，构建Vb = 0, 计算B矩阵并求解K内参矩阵

/** *  Vij=[hi1hj1  hi1hj2+hi2hj1  hi2hj2  hi3hj1+hi1hj3  hi3hj2+hi2hj3  hi3hj3] * @param H * @param i * @param j * @return */VectorXd getVector(const Matrix3d& H, int i, int j){    i -= 1;    j -= 1;    VectorXd v(6);    v << H(0, i)*H(0, j), H(0, i)*H(1, j) + H(1, i)*H(0, j), H(1, i)*H(1, j), H(2, i)*H(0, j) + H(0, i)*H(2, j), H(2, i)*H(1, j) + H(1, i)*H(2, j), H(2, i)*H(2, j);    return v;}/** * * 计算相机内参初始值, 求解Vb =0; 并计算K矩阵 * */Matrix3d solveInitCameraIntrinsic(std::vector& homos){    int n = homos.size();    // Vb = 0    MatrixXd V(2*n, 6);    for(int i=0; i    {        VectorXd v1 = getVector(homos[i], 1, 2);        VectorXd v11 = getVector(homos[i], 1, 1);        VectorXd v22 = getVector(homos[i], 2, 2);        VectorXd v2 = v11 - v22;        for(int j=0; j<6; ++j)        {            V(2*i, j) = v1(j);            V(2*i+1, j) = v2(j);        }    }    //SVD 分解    JacobiSVD svdSolver (V, ComputeThinV);     MatrixXd v = svdSolver.matrixV();    MatrixXd b = v.rightCols(1);    std::cout <<"b = " << b << std::endl;    // 求解内参 fx fy c uo v0    double B11 = b(0), B12 = b(1), B22 = b(2), B13 = b(3), B23 = b(4), B33 = b(5);    double v0 = (B12*B13-B11*B23) / (B11*B22-B12*B12);    double s = B33-(B13*B13+v0*(B12*B13-B11*B23)) / B11;    double fx = sqrt(s/B11);    double fy = sqrt(s*B11 / (B11*B22-B12*B12));    double c = -B12*fx*fx*fy/s;    double u0 = c*v0/fx - B13*fx*fx/s;    Matrix3d K;    K << fx, c, u0,    0, fy, v0,    0, 0, 1;    return K;}

5. 根据K计算，每张棋盘格的外参R和t

/** * * 计算相机外参初始值 */void solveInitCameraExtrinsic(std::vector& homos, Matrix3d& K, std::vector& RList, std::vector& tList){    int n = homos.size();    Matrix3d kInv = K.inverse();    for (int i=0; i    {        Vector3d r0, r1, r2;        r0 = kInv*homos[i].col(0);        r1 = kInv*homos[i].col(1);        double s0 = sqrt(r0.dot(r0));        double s1 = sqrt(r1.dot(r1));        r0.array().col(0) /= s0;        r1.array().col(0) /= s1;        r2 = r0.cross(r1);        Vector3d t = kInv*homos[i].col(2) / s0;        Matrix3d R;        R.array().col(0) = r0;        R.array().col(1) = r1;        R.array().col(2) = r2;        std::cout <<"R " << R << std::endl;        std::cout <<"t " << t.transpose() << std::endl;        RList.push_back(R);        tList.push_back(t);    }}

6. 构建最优化目标函数

struct PROJECT_COST {    Eigen::Vector3d objPt;    Eigen::Vector2d imgPt;    PROJECT_COST(Eigen::Vector3d& objPt, Eigen::Vector2d& imgPt):objPt(objPt), imgPt(imgPt)    {}    template<typename T>    bool operator()(        const T *const k,        const T *const r,        const T *const t,        T* residuals)const{        T pos3d[3] = {T(objPt(0)), T(objPt(1)), T(objPt(2))};        T pos3d_proj[3];        // 旋转        ceres::AngleAxisRotatePoint(r, pos3d, pos3d_proj);        // 平移        pos3d_proj[0] += t[0];        pos3d_proj[1] += t[1];        pos3d_proj[2] += t[2];        T xp = pos3d_proj[0] / pos3d_proj[2];        T yp = pos3d_proj[1] / pos3d_proj[2];        const T& fx = k[0];        const T& fy = k[1];        const T& cx = k[2];        const T& cy = k[3];        const T& k1 = k[4];        const T& k2 = k[5];        const T& k3 = k[6];        const T& p1 = k[7];        const T& p2 = k[8];                T r_2 = xp*xp + yp*yp;                /*         // 径向畸变        T xdis = xp*(T(1.) + k1*r_2 + k2*r_2*r_2 + k3*r_2*r_2*r_2);        T ydis = yp*(T(1.) + k1*r_2 + k2*r_2*r_2 + k3*r_2*r_2*r_2);        // 切向畸变        xdis = xdis + T(2.)*p1*xp*yp + p2*(r_2 + T(2.)*xp*xp);        ydis = ydis + p1*(r_2 + T(2.)*yp*yp) + T(2.)*p2*xp*yp;        */                // 径向畸变和切向畸变        T xdis = xp*(T(1.) + k1*r_2 + k2*r_2*r_2 + k3*r_2*r_2*r_2) + T(2.)*p1*xp*yp + p2*(r_2 + T(2.)*xp*xp);        T ydis = yp*(T(1.) + k1*r_2 + k2*r_2*r_2 + k3*r_2*r_2*r_2) + p1*(r_2 + T(2.)*yp*yp) + T(2.)*p2*xp*yp;                // 像素距离        T u = fx*xdis + cx;        T v = fy*ydis + cy;                residuals[0] = u - T(imgPt[0]);        residuals[1] = v - T(imgPt[1]);                return true;    }};

具体使用

 // 优化算法    {        //        ceres::Problem problem;        double k[9] = {K(0,0), K(1,1), K(0,2), K(1,2), 0., 0., 0., 0., 0.};  // 设置内参初值        for(int i=0; i            for(int j=0; j            // 优化参数2->输出的残差数，表示x和y            // 9 表示 内参4个 畸变系数5个            // 3 外参，用旋转向量表示，输入需要把旋转矩阵转为旋转向量，再输入            // 3 外参 平移向量                ceres::CostFunction* costFunction=new ceres::AutoDiffCostFunction2,                     new PROJECT_COST(objectPoints[i][j], imagePoints[i][j]));                problem.AddResidualBlock(costFunction,                                         nullptr,                                         k,                                         rList[i].data(),                                         tList[i].data()                                        );            }        }        std::cout << " solve Options ..." << std::endl;        ceres::Solver::Options options;        options.minimizer_progress_to_stdout = true;        //options.linear_solver_type = ceres::DENSE_SCHUR;        //options.trust_region_strategy_type = ceres::TrustRegionStrategyType::LEVENBERG_MARQUARDT;        //options.preconditioner_type = ceres::JACOBI;        //options.sparse_linear_algebra_library_type = ceres::EIGEN_SPARSE;        ceres::Solver::Summary summary;        ceres::Solve(options, &problem, &summary);        std::cout << summary.BriefReport() << std::endl;        if (!summary.IsSolutionUsable())        {            std::cout << "Bundle Adjustment failed." << std::endl;        }        else        {            //summary.num_            // Display statistics about the minimization            std::cout << std::endl                      << "Bundle Adjustment statistics (approximated RMSE):\n"                      << " #views: " << n << "\n"                      << " #residuals: " << summary.num_residuals << "\n"                      << " #num_parameters: " << summary.num_parameters << "\n"                      << " #num_parameter_blocks: " << summary.num_parameter_blocks << "\n"                      << " Initial RMSE: " << std::sqrt(summary.initial_cost / summary.num_residuals) << "\n"                      << " Final RMSE: " << std::sqrt(summary.final_cost / summary.num_residuals) << "\n"                      << " Time (s): " << summary.total_time_in_seconds << "\n"                      << std::endl;            for(auto& a: k) std::cout << a << " " ;            //cv::Mat cameraMatrix, distCoeffs;            //cameraMatrix = (cv::Mat_(3, 3) << k[0], 0.0, k[2], 0, k[1], k[3], 0, 0, 1);            //distCoeffs = (cv::Mat_(1, 5) << k[4], k[5], k[7], k[8], k[6]);      // 输出优化结果            Eigen::Matrix3d cameraMatrix_;            cameraMatrix_ << k[0], 0.0, k[2], 0, k[1], k[3], 0, 0, 1;            Eigen::VectorXd  distCoeffs_(5);            distCoeffs_ << k[4], k[5], k[7], k[8], k[6];                        // 评价投影误差            std::vector<double> reprojErrs;            double totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, rList, tList, cameraMatrix_, distCoeffs_, reprojErrs);            std::cout << " avg re projection error = " << totalAvgErr << std::endl;            for (size_t i = 0; i < reprojErrs.size(); i++)            {                std::cout << i << " projection error = " << reprojErrs[i] << std::endl;            }            // Mat            cv::eigen2cv(cameraMatrix_, cameraMatrix);            cv::eigen2cv(distCoeffs_, distCoeffs);        }    }

最后的结果

536.073 536.016 342.371 235.536 -0.265091 -0.0467182 0.252215 0.00183296 -0.000314464  avg re projection error = 0.2345930 projection error = 0.169921 projection error = 0.8463292 projection error = 0.1591173 projection error = 0.1766264 projection error = 0.1412075 projection error = 0.1623126 projection error = 0.188017 projection error = 0.2140988 projection error = 0.222179 projection error = 0.15319210 projection error = 0.17754311 projection error = 0.2858612 projection error = 0.15332

核心的代码和流程就这多，还有根据畸变系数对图像校正都在代码里，我把github地址也贴出来。里面也包含鱼眼镜头的标定。代码结构如下：

附源码地址：
CameraCalibrationhttps://github.com/zhaitianyong/CameraCalibration