赞
踩
相机标定系列
前两篇文章偏重理论,介绍了针孔相机模型、镜头畸变模型和张氏标定的原理。今天主要讲解代码实现,虽然很多成熟视觉框架已经包含了相机标定,opencv 、matlab、halcon、ros, 为了更深入的结合原理,还是有必要自己码一遍。
这里数据采用OpenCV data 中提供的left 和right标定版图像,我这里用left的,共有13张,棋盘格BorderSize是 9x6,单元格大小SquareSize是 25x25。
用到的库:
OpenCV (图像库)
Eigen3 (矩阵库)
Ceres (算法优化库)
算法实现步骤:
// 准备数据 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<Eigen::Vector2d>> imagePoints; std::vector<std::vector<Eigen::Vector3d>> objectPoints; cv::Size boardSize(9, 6); // 棋盘格大小 cv::Size2f squareSize(25., 25.); // 单元格大小, 单位mm for(int i=0; i<files.size(); ++i) { cv::Mat img = cv::imread(files[i]); std::vector<cv::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<Eigen::Vector2d> _corners; for(auto& pt: corners){ _corners.push_back(Eigen::Vector2d(pt.x, pt.y)); } imagePoints.push_back(_corners); } }
//3.0 设置世界坐标
for(int i=0; i<imagePoints.size(); ++i){
std::vector<Eigen::Vector3d> corners;
getObjectPoints(boardSize, squareSize, corners);
objectPoints.push_back(corners);
}
void getObjectPoints(const cv::Size& borderSize, const cv::Size2f& squareSize, std::vector<Eigen::Vector3d>& objectPoints) {
for(int r=0; r<borderSize.height; ++r)
{
for(int c=0; c<borderSize.width; ++c) {
objectPoints.push_back(Eigen::Vector3d(c*squareSize.width, r*squareSize.height, 0.));
}
}
}
之前已经写过关于单应性矩阵的求解DLP算法,具体代码中已经包含。这里直接贴出来OpenCV求解
bool findHomographyByOpenCV(std::vector<Eigen::Vector2d>& srcPoints, std::vector<Eigen::Vector2d>& dstPoints, Eigen::Matrix3d& H) {
std::vector<cv::Point2f> objectPoints, imagePoints;
for(int i=0; i<srcPoints.size(); ++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);
}
/** * 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<Matrix3d>& homos) { int n = homos.size(); // Vb = 0 MatrixXd V(2*n, 6); for(int i=0; i<n; ++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<MatrixXd> 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; }
/** * * 计算相机外参初始值 */ void solveInitCameraExtrinsic(std::vector<Matrix3d>& homos, Matrix3d& K, std::vector<Matrix3d>& RList, std::vector<Vector3d>& tList) { int n = homos.size(); Matrix3d kInv = K.inverse(); for (int i=0; i<n; ++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); } }
求出最优K 和 畸变系数 k1、k2、k3、p1、p2以及外参。
上面计算出的K,和13个外参 R和t作为最优化的初始值。
优化框架采用ceres优化框架,首先要编写优化结构体 PROJECT_COST:
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<n; ++i) { for(int j=0; j<imagePoints[i].size(); ++j) { // 优化参数2->输出的残差数,表示x和y // 9 表示 内参4个 畸变系数5个 // 3 外参,用旋转向量表示,输入需要把旋转矩阵转为旋转向量,再输入 // 3 外参 平移向量 ceres::CostFunction* costFunction=new ceres::AutoDiffCostFunction<PROJECT_COST, 2, 9, 3, 3>( 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_<double>(3, 3) << k[0], 0.0, k[2], 0, k[1], k[3], 0, 0, 1); //distCoeffs = (cv::Mat_<double>(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.234593 0 projection error = 0.16992 1 projection error = 0.846329 2 projection error = 0.159117 3 projection error = 0.176626 4 projection error = 0.141207 5 projection error = 0.162312 6 projection error = 0.18801 7 projection error = 0.214098 8 projection error = 0.22217 9 projection error = 0.153192 10 projection error = 0.177543 11 projection error = 0.28586 12 projection error = 0.15332
核心的代码和流程就这多,还有根据畸变系数对图像校正都在代码里,我把github地址也贴出来。
里面也包含鱼眼镜头的标定。代码结构如下:
附源码地址:
CameraCalibration
https://github.com/zhaitianyong/CameraCalibration
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。