From a634036379c90c870cbbb32a2522b06831153c90 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Thu, 28 Aug 2025 11:03:16 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=8C=E7=BB=B4=E7=A0=81?= =?UTF-8?q?=E6=A0=87=E5=AE=9A=E6=B5=81=E7=A8=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- camCalib/camCalib.cpp | 248 +++++++++++++-------- camCalib/sourceCode/MonoLaserCalibrate.cpp | 147 +++++++++++- camCalib/sourceCode/MonoLaserCalibrate.h | 25 ++- 3 files changed, 321 insertions(+), 99 deletions(-) diff --git a/camCalib/camCalib.cpp b/camCalib/camCalib.cpp index d2af778..9d11254 100644 --- a/camCalib/camCalib.cpp +++ b/camCalib/camCalib.cpp @@ -8,7 +8,7 @@ #include "sourceCode/MonoLaserCalibrate.h" #include "sourceCode/FitMapParam.h" -#define _DO_CAMERA_CALIB 0 +#define _DO_CAMERA_CALIB 1 void sg_outputCalibK(const char* fileName, cv::Mat& fitMap) { @@ -88,7 +88,7 @@ void sg_readCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D) for (int _c = 0; _c < 3; _c++) K.ptr(line)[_c] = data[_c]; } - else + else if(line == 3) { double data[5]; sscanf_s(linedata.c_str(), "%lf, %lf, %lf, %lf, %lf", &data[0], &data[1], &data[2], &data[3], &data[4]); @@ -180,10 +180,11 @@ int main() for(int grp = 0; grp < CALIB_TEST_GROUP; grp ++) { - grp = 1; + grp = 3; int calibType = boardType[grp]; cv::Size cbPattern; float cbSquareSize; + float markSize; if (CALIB_CHESS_BOARD == calibType) { cbPattern = cv::Size(8, 11); // 10); @@ -196,13 +197,15 @@ int main() } else if (CALIB_CHARUCO == calibType) { - cbPattern = cv::Size(20, 46); // 10); - cbSquareSize = 50.0f; // 10.f; + cbPattern = cv::Size(47, 21); // 10); + cbSquareSize = 0.05f; + markSize = 0.037f; } else continue; std::vector> cbCornersList; + std::vector> cbCornersIdList; cv::Size imageSize; int startIndex = fileIdx[grp].nMin; @@ -237,6 +240,9 @@ int main() cv::imwrite(filename, color); } #endif + if (corners.empty()) + continue; + cbCornersList.push_back(corners); } else if (CALIB_CIRCLE_GRID == calibType) { @@ -251,26 +257,55 @@ int main() cv::imwrite(filename, color); } #endif - + if (corners.empty()) + continue; + cbCornersList.push_back(corners); } else if (CALIB_CHARUCO == calibType) { - } - if (corners.empty()) - continue; + std::vector markerIds; + std::vector > markerCorners; + std::vector charucoIds; + detectCharucoCorners(img, + cbPattern, + cbSquareSize, + markSize, + markerIds, + markerCorners, + charucoIds, + corners); + cv::Mat imageCopy = img.clone(); + if (markerIds.size() > 0) + { + cv::aruco::drawDetectedMarkers(imageCopy, markerCorners, markerIds); + } + if (charucoIds.size() > 0) + { + cv::aruco::drawDetectedCornersCharuco(imageCopy, corners, charucoIds, cv::Scalar(0, 255, 255)); + } + char markFilename[256]; + sprintf_s(markFilename, "%scalib_%03d_markers.bmp", calibDataPath[grp], index); + cv::imwrite(markFilename, imageCopy); + if (corners.empty()) + continue; + cbCornersList.push_back(corners); + cbCornersIdList.push_back(charucoIds); + } imageSize = cv::Size(img.cols, img.rows); - cbCornersList.push_back(corners); } cv::Mat K, D; std::vector reprojectionError; - monocularCalibration(cbCornersList, imageSize, cbPattern, cbSquareSize, K, D, reprojectionError); + if (CALIB_CHARUCO == calibType) + monocularCalibration_charuco(cbCornersIdList, cbCornersList, imageSize, cbPattern, cbSquareSize, K, D, reprojectionError); + else + monocularCalibration_chessboard(cbCornersList, imageSize, cbPattern, cbSquareSize, K, D, reprojectionError); for(int i = 0; i < reprojectionError.size(); i ++) std::cout << reprojectionError[i] << std::endl; // 输出映射类型，通常使用CV_32FC1或CV_16SC2 - cv::Mat map_x, map_y; + cv::Mat backwardMap_x, backwardMap_y; cv::Mat newCamMatrix; // 生成畸变矫正映射 #if ENABLE_FISH_EYE @@ -279,12 +314,12 @@ int main() double alpha = 0.4; // 0.4; newCamMatrix = cv::getOptimalNewCameraMatrix(K, D, imageSize, alpha, imageSize, 0); //cv::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, map_x, map_y); - initForwardRectMap(K, D, cv::Mat(), newCamMatrix, imageSize, map_x, map_y); + initForwardRectMap(K, D, cv::Mat(), newCamMatrix, imageSize, backwardMap_x, backwardMap_y); #endif // 生成系数表 - cv::Mat fitMap_x = GetFitParamMap(map_x, 1); - cv::Mat fitMap_y = GetFitParamMap(map_y, 1); + cv::Mat fitMap_x = GetFitParamMap(backwardMap_x, 1); + cv::Mat fitMap_y = GetFitParamMap(backwardMap_y, 1); //输出系数文件 char calibParamName[256]; sprintf_s(calibParamName, "%scalib_param_x.txt", calibDataPath[grp]); @@ -298,8 +333,8 @@ int main() //搜索最大和平均误差 // 计算绝对差异 cv::Mat diff_x, diff_y; - cv::absdiff(map_x, mapGen_x, diff_x); - cv::absdiff(map_y, mapGen_y, diff_y); + cv::absdiff(backwardMap_x, mapGen_x, diff_x); + cv::absdiff(backwardMap_y, mapGen_y, diff_y); // 查找最大值和最小值 double minVal_x, maxVal_x; cv::minMaxLoc(diff_x, &minVal_x, &maxVal_x); @@ -325,7 +360,7 @@ int main() cv::Mat img; cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE); cv::Mat calibImg; - remap(img, + cv::remap(img, calibImg, mapGen_x, mapGen_y, @@ -341,6 +376,14 @@ int main() sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]); cv::Mat K, D; sg_readCalibKD(calibKDName, K, D); + + //生成opencv校正表 + cv::Mat backwardMap_x, backwardMap_y; + double alpha = 0.4; // 0.4; + imageSize = cv::Size(1200, 2048); + cv::Mat newCamMatrix = cv::getOptimalNewCameraMatrix(K, D, imageSize, alpha, imageSize, 0); + cv::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, backwardMap_x, backwardMap_y); + #endif #if ENABLE_GEN_IMAGE @@ -392,72 +435,99 @@ int main() cv::Mat img; cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE); std::vector corners; - detectCorners(img, cbPattern, corners); + if (CALIB_CHESS_BOARD == calibType) + { + detectCorners(img, cbPattern, corners); + } + else if (CALIB_CIRCLE_GRID == calibType) + { + detectCirclePoints(img, cbPattern, corners); + } + else if (CALIB_CHARUCO == calibType) + { + std::vector markerIds; + std::vector > markerCorners; + std::vector charucoIds; + detectCharucoCorners(img, + cbPattern, + cbSquareSize, + markSize, + markerIds, + markerCorners, + charucoIds, + corners); + } if (corners.empty()) continue; - + // 创建棋盘格区域的掩码 - cv::Mat chessMask = cv::Mat::zeros(img.size(), CV_8UC1); - // 使用多边形近似来填充角点之间的区域 - // 棋盘格区域需要比角点区域大一圈 - std::vector contour_line[4]; - for (int i = 0; i < cbPattern.width; i++) + cv::Mat chessMask; + if (CALIB_CHARUCO == calibType) + chessMask = cv::Mat::ones(img.size(), CV_8UC1); + else { - cv::Point2f pt_c = corners[i]; - cv::Point2f pt_2 = corners[cbPattern.width + i]; - cv::Point2f pt_1; - pt_1.x = pt_c.x * 2 - pt_2.x; - pt_1.y = pt_c.y * 2 - pt_2.y; - contour_line[0].push_back(pt_1); - } - for (int i = 0; i < cbPattern.height; i++) - { - cv::Point2f pt_c = corners[i * cbPattern.width + cbPattern.width - 1]; - cv::Point2f pt_2 = corners[i * cbPattern.width + cbPattern.width - 2]; - cv::Point2f pt_1; - pt_1.x = pt_c.x * 2 - pt_2.x; - pt_1.y = pt_c.y * 2 - pt_2.y; - contour_line[1].push_back(pt_1); - } - for (int i = cbPattern.width - 1; i >= 0; i--) - { - cv::Point2f pt_c = corners[(cbPattern.height - 1) * cbPattern.width + i]; - cv::Point2f pt_2 = corners[(cbPattern.height - 2) * cbPattern.width + i]; - cv::Point2f pt_1; - pt_1.x = pt_c.x * 2 - pt_2.x; - pt_1.y = pt_c.y * 2 - pt_2.y; - contour_line[2].push_back(pt_1); - } - for (int i = cbPattern.height-1; i >= 0; i--) - { - cv::Point2f pt_c = corners[i * cbPattern.width]; - cv::Point2f pt_2 = corners[i * cbPattern.width + 1]; - cv::Point2f pt_1; - pt_1.x = pt_c.x * 2 - pt_2.x; - pt_1.y = pt_c.y * 2 - pt_2.y; - contour_line[3].push_back(pt_1); - } - std::vector contours; - //生成轮廓点 - for (int n = 0; n < 4; n++) - { - int num = contour_line[n].size(); - for (int i = 0; i < num; i++) - contours.push_back(contour_line[n][i]); - cv::Point2f pt_c = contour_line[n][num - 1]; - cv::Point2f pt_2 = contour_line[n][num - 2]; - cv::Point2f pt_1; - pt_1.x = pt_c.x * 2 - pt_2.x; - pt_1.y = pt_c.y * 2 - pt_2.y; - contours.push_back(pt_1); - } - // 使用 fillPoly 填充多边形 - cv::Scalar color(255); // 红色 - cv::fillPoly(chessMask, contours, color); + cv::Mat chessMask = cv::Mat::zeros(img.size(), CV_8UC1); + // 使用多边形近似来填充角点之间的区域 + // 棋盘格区域需要比角点区域大一圈 + std::vector contour_line[4]; + for (int i = 0; i < cbPattern.width; i++) + { + cv::Point2f pt_c = corners[i]; + cv::Point2f pt_2 = corners[cbPattern.width + i]; + cv::Point2f pt_1; + pt_1.x = pt_c.x * 2 - pt_2.x; + pt_1.y = pt_c.y * 2 - pt_2.y; + contour_line[0].push_back(pt_1); + } + for (int i = 0; i < cbPattern.height; i++) + { + cv::Point2f pt_c = corners[i * cbPattern.width + cbPattern.width - 1]; + cv::Point2f pt_2 = corners[i * cbPattern.width + cbPattern.width - 2]; + cv::Point2f pt_1; + pt_1.x = pt_c.x * 2 - pt_2.x; + pt_1.y = pt_c.y * 2 - pt_2.y; + contour_line[1].push_back(pt_1); + } + for (int i = cbPattern.width - 1; i >= 0; i--) + { + cv::Point2f pt_c = corners[(cbPattern.height - 1) * cbPattern.width + i]; + cv::Point2f pt_2 = corners[(cbPattern.height - 2) * cbPattern.width + i]; + cv::Point2f pt_1; + pt_1.x = pt_c.x * 2 - pt_2.x; + pt_1.y = pt_c.y * 2 - pt_2.y; + contour_line[2].push_back(pt_1); + } + for (int i = cbPattern.height - 1; i >= 0; i--) + { + cv::Point2f pt_c = corners[i * cbPattern.width]; + cv::Point2f pt_2 = corners[i * cbPattern.width + 1]; + cv::Point2f pt_1; + pt_1.x = pt_c.x * 2 - pt_2.x; + pt_1.y = pt_c.y * 2 - pt_2.y; + contour_line[3].push_back(pt_1); + } + std::vector contours; + //生成轮廓点 + for (int n = 0; n < 4; n++) + { + int num = contour_line[n].size(); + for (int i = 0; i < num; i++) + contours.push_back(contour_line[n][i]); + cv::Point2f pt_c = contour_line[n][num - 1]; + cv::Point2f pt_2 = contour_line[n][num - 2]; + cv::Point2f pt_1; + pt_1.x = pt_c.x * 2 - pt_2.x; + pt_1.y = pt_c.y * 2 - pt_2.y; + contours.push_back(pt_1); + } + // 使用 fillPoly 填充多边形 + cv::Scalar color(255); // 红色 + cv::fillPoly(chessMask, contours, color); #if 1 - sprintf_s(filename, "%schessMask_%03d.png", calibDataPath[grp], index); - cv::imwrite(filename, chessMask); + sprintf_s(filename, "%schessMask_%03d.png", calibDataPath[grp], index); + cv::imwrite(filename, chessMask); #endif + } cv::Vec4f pe; fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe); @@ -475,19 +545,20 @@ int main() #if 1 sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index); cv::imwrite(filename, laserImg); -/* - cv::Mat calibImg; - remap(laserImg, - calibImg, - mapGen_x, - mapGen_y, + + cv::Mat laserCalibImg; + cv::remap(laserImg, + laserCalibImg, + backwardMap_x, + backwardMap_y, cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar(0, 0, 0)); + cv::Size laserImgSize = laserCalibImg.size(); + laserImgSize.width = laserImgSize.width * 5; + cv::resize(laserCalibImg, laserCalibImg, laserImgSize, 0, 0, cv::INTER_NEAREST); sprintf_s(filename, "%slaser_%03d_calib.bmp", calibDataPath[grp], index); - cv::imwrite(filename, calibImg); -*/ - + cv::imwrite(filename, laserCalibImg); #endif std::vector pts2d = detectLaserLine(laserImg); //显示亚像素点 @@ -530,9 +601,8 @@ int main() //output K and D char calibKDPName[256]; sprintf_s(calibKDPName, "%scalib_param_K_D.txt", calibDataPath[grp]); - sg_outputCalibKD(calibKDName, K, D, pe); + sg_outputCalibKD(calibKDPName, K, D, pe); } - return 0; } diff --git a/camCalib/sourceCode/MonoLaserCalibrate.cpp b/camCalib/sourceCode/MonoLaserCalibrate.cpp index 8e513b2..64d3f11 100644 --- a/camCalib/sourceCode/MonoLaserCalibrate.cpp +++ b/camCalib/sourceCode/MonoLaserCalibrate.cpp @@ -2,6 +2,8 @@ // #include #include +#include "aruco.hpp" +#include "aruco/charuco.hpp" #include #include #include "MonoLaserCalibrate.h" @@ -158,18 +160,53 @@ void detectCirclePoints(const cv::Mat& img, return; } -/*Breif：单目相机标定函数*/ -void monocularCalibration( - const std::vector>& imagePoints, - const cv::Size& imageSize, +/*Breif：二维码标定板角点函数*/ +void detectCharucoCorners(const cv::Mat& img, + const cv::Size& patternSize, + float sqSize, + float mkSize, + std::vector& markerIds, + std::vector >& markerCorners, + std::vector& charucoIds, + std::vector& charucoCorners) +{ + cv::Mat gray; + if (img.channels() == 3) + cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY); + else + gray = img.clone(); + + cv::aruco::Dictionary dictionary(cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_1000)); + cv::aruco::CharucoBoard board = cv::aruco::CharucoBoard(patternSize, sqSize, mkSize, dictionary); + cv::aruco::DetectorParameters params = cv::aruco::DetectorParameters(); + params.cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; + + cv::Ptr ptrDictionary = cv::makePtr(dictionary); + cv::Ptr ptrParams = cv::makePtr(params); + cv::aruco::detectMarkers(gray, ptrDictionary, markerCorners, markerIds, ptrParams); + + // if at least one marker detected + if (markerIds.size() > 0) { + //cv::aruco::drawDetectedMarkers(imageCopy, markerCorners, markerIds); + cv::Ptr ptrBoard = cv::makePtr(board); + cv::aruco::interpolateCornersCharuco(markerCorners, markerIds, gray, ptrBoard, charucoCorners, charucoIds); + // if at least one charuco corner detected + if (charucoIds.size() > 0) + // 亚像素精确化 + cv::cornerSubPix(gray, charucoCorners, cv::Size(11, 11), cv::Size(-1, -1), + cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 50, 0.1)); + } + return; +} + +void gen3DCoordinate_chessboard( + const std::vector>& imagePoints, const cv::Size& patternSize, const float squareSize, - cv::Mat& cameraMatrix, - cv::Mat& distCoeffs, - std::vector& reprojectionError) + std::vector>& objectPoints + ) { // 根据角点生成3d点 - std::vector> objectPoints; for (const auto& corners : imagePoints) { // 准备3D世界坐标点 (z=0) @@ -180,7 +217,44 @@ void monocularCalibration( objectPoints.push_back(obj); } + return; +} +void gen3DCoordinate_charuco( + std::vector>& charucoIds, + std::vector>& charucoCorners, + const cv::Size& patternSize, + const float squareSize, + std::vector>& objectPoints) +{ + // 根据角点生成3d点 + for (int i = 0, i_max = (int)charucoCorners.size(); i < i_max; i ++) { + + std::vector& a_cornerList = charucoCorners[i]; + std::vector& a_idList = charucoIds[i]; + // 准备3D世界坐标点 (z=0) + std::vector obj; + for (int j = 0, j_max = (int)a_cornerList.size(); j < j_max; j++) + { + int id = a_idList[j]; + int id_row = id / (patternSize.width-1); + int id_col = id % (patternSize.width-1); + obj.emplace_back(id_col * squareSize, id_row *squareSize, 0); + } + objectPoints.push_back(obj); + } + return; +} + +/*Breif：单目相机标定函数*/ +void monocularCalibration( + const std::vector>& imagePoints, + const cv::Size& imageSize, + std::vector>& objectPoints, + cv::Mat& cameraMatrix, + cv::Mat& distCoeffs, + std::vector& reprojectionError) +{ // 执行相机标定 std::vector rvecs, tvecs; #if ENABLE_FISH_EYE @@ -236,6 +310,63 @@ void monocularCalibration( return; } +/*Breif：棋盘格单目相机标定函数*/ +void monocularCalibration_chessboard( + const std::vector>& imagePoints, + const cv::Size& imageSize, + const cv::Size& patternSize, + const float squareSize, + cv::Mat& cameraMatrix, + cv::Mat& distCoeffs, + std::vector& reprojectionError) +{ + std::vector> objectPoints; + gen3DCoordinate_chessboard( + imagePoints, + patternSize, + squareSize, + objectPoints + ); + + monocularCalibration( + imagePoints, + imageSize, + objectPoints, + cameraMatrix, + distCoeffs, + reprojectionError); + return; +} + +/*Breif：二维码标定板单目相机标定函数*/ +void monocularCalibration_charuco( + std::vector>& charucoIds, + std::vector>& charucoCorners, + const cv::Size& imageSize, + const cv::Size& patternSize, + const float squareSize, + cv::Mat& cameraMatrix, + cv::Mat& distCoeffs, + std::vector& reprojectionError) +{ + std::vector> objectPoints; + gen3DCoordinate_charuco( + charucoIds, + charucoCorners, + patternSize, + squareSize, + objectPoints); + + monocularCalibration( + charucoCorners, + imageSize, + objectPoints, + cameraMatrix, + distCoeffs, + reprojectionError); + return; +} + /*Brief: 拟合棋盘格角点平面*/ void fitChessboardPlane( const std::vector& corners, diff --git a/camCalib/sourceCode/MonoLaserCalibrate.h b/camCalib/sourceCode/MonoLaserCalibrate.h index c4b20ef..9367786 100644 --- a/camCalib/sourceCode/MonoLaserCalibrate.h +++ b/camCalib/sourceCode/MonoLaserCalibrate.h @@ -17,8 +17,18 @@ void detectCirclePoints(const cv::Mat& img, const cv::Size& patternSize, std::vector& corners); -/*BreifĿ궨*/ -void monocularCalibration( +/*Breifά궨ǵ㺯*/ +void detectCharucoCorners(const cv::Mat& img, + const cv::Size& patternSize, + float sqSize, + float mkSize, + std::vector& markerIds, + std::vector >& markerCorners, + std::vector& charucoIds, + std::vector& charucoCorners); + +/*Breif̸Ŀ궨*/ +void monocularCalibration_chessboard( const std::vector>& imagePoints, const cv::Size& imageSize, const cv::Size& patternSize, @@ -27,6 +37,17 @@ void monocularCalibration( cv::Mat& distCoeffs, std::vector& reprojectionError); +/*Breifά궨嵥Ŀ궨*/ +void monocularCalibration_charuco( + std::vector>& charucoIds, + std::vector>& charucoCorners, + const cv::Size& imageSize, + const cv::Size& patternSize, + const float squareSize, + cv::Mat& cameraMatrix, + cv::Mat& distCoeffs, + std::vector& reprojectionError); + /*Brief: ̸ǵƽ*/ void fitChessboardPlane( const std::vector& corners,