From ea6d44c6d1aab53353edbc7fcc0762646dccd2ff Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sat, 30 Aug 2025 16:11:06 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86charuco=E7=9A=84?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E7=B3=BB=E6=95=B0=EF=BC=8C=E4=BB=A5=E8=A1=A5?= =?UTF-8?q?=E5=81=BF=E6=A0=87=E5=AE=9A=E6=9D=BF=E7=9A=84=E5=88=B6=E4=BD=9C?= =?UTF-8?q?=E8=AF=AF=E5=B7=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- camCalib/camCalib.cpp | 42 ++++-- camCalib/sourceCode/MonoLaserCalibrate.cpp | 151 +++++++++++++-------- camCalib/sourceCode/MonoLaserCalibrate.h | 14 +- 3 files changed, 140 insertions(+), 67 deletions(-) diff --git a/camCalib/camCalib.cpp b/camCalib/camCalib.cpp index 9d11254..48664e9 100644 --- a/camCalib/camCalib.cpp +++ b/camCalib/camCalib.cpp @@ -306,6 +306,7 @@ int main() // 输出映射类型，通常使用CV_32FC1或CV_16SC2 cv::Mat backwardMap_x, backwardMap_y; + cv::Mat forwardMap_x, forwardMap_y; cv::Mat newCamMatrix; // 生成畸变矫正映射 #if ENABLE_FISH_EYE @@ -313,13 +314,13 @@ int main() #else 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, backwardMap_x, backwardMap_y); + cv::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, backwardMap_x, backwardMap_y); + initForwardRectMap(K, D, cv::Mat(), newCamMatrix, imageSize, forwardMap_x, forwardMap_y); #endif // 生成系数表 - cv::Mat fitMap_x = GetFitParamMap(backwardMap_x, 1); - cv::Mat fitMap_y = GetFitParamMap(backwardMap_y, 1); + cv::Mat fitMap_x = GetFitParamMap(forwardMap_x, 1); + cv::Mat fitMap_y = GetFitParamMap(forwardMap_y, 1); //输出系数文件 char calibParamName[256]; sprintf_s(calibParamName, "%scalib_param_x.txt", calibDataPath[grp]); @@ -333,8 +334,8 @@ int main() //搜索最大和平均误差 // 计算绝对差异 cv::Mat diff_x, diff_y; - cv::absdiff(backwardMap_x, mapGen_x, diff_x); - cv::absdiff(backwardMap_y, mapGen_y, diff_y); + cv::absdiff(forwardMap_x, mapGen_x, diff_x); + cv::absdiff(forwardMap_y, mapGen_y, diff_y); // 查找最大值和最小值 double minVal_x, maxVal_x; cv::minMaxLoc(diff_x, &minVal_x, &maxVal_x); @@ -362,8 +363,8 @@ int main() cv::Mat calibImg; cv::remap(img, calibImg, - mapGen_x, - mapGen_y, + backwardMap_x, + backwardMap_y, cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar(0, 0, 0)); @@ -434,7 +435,24 @@ int main() cv::Mat img; cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE); +#if 0 + cv::Mat charucoCalibImg; + cv::remap(img, + charucoCalibImg, + backwardMap_x, + backwardMap_y, + cv::INTER_LINEAR, + cv::BORDER_CONSTANT, + cv::Scalar(0, 0, 0)); + cv::Size _size = charucoCalibImg.size(); + _size.width = _size.width * 5; + //cv::resize(charucoCalibImg, charucoCalibImg, _size, 0, 0, cv::INTER_NEAREST); + sprintf_s(filename, "%scalib_%03d_calib.bmp", calibDataPath[grp], index); + cv::imwrite(filename, charucoCalibImg); +#endif + std::vector corners; + std::vector charucoIds; if (CALIB_CHESS_BOARD == calibType) { detectCorners(img, cbPattern, corners); @@ -447,7 +465,6 @@ int main() { std::vector markerIds; std::vector > markerCorners; - std::vector charucoIds; detectCharucoCorners(img, cbPattern, cbSquareSize, @@ -466,7 +483,7 @@ int main() chessMask = cv::Mat::ones(img.size(), CV_8UC1); else { - cv::Mat chessMask = cv::Mat::zeros(img.size(), CV_8UC1); + chessMask = cv::Mat::zeros(img.size(), CV_8UC1); // 使用多边形近似来填充角点之间的区域 // 棋盘格区域需要比角点区域大一圈 std::vector contour_line[4]; @@ -530,7 +547,10 @@ int main() } cv::Vec4f pe; - fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe); + if (CALIB_CHARUCO == calibType) + fitChessboardPlane_charuco(charucoIds,corners, K, D, cbPattern, cbSquareSize, pe); + else + fitChessboardPlane_chessboard(corners, K, D, cbPattern, cbSquareSize, pe); sprintf_s(filename, "%slaser_%03d.bmp", calibDataPath[grp], index); cv::Mat srcLaserImg = cv::imread(filename); diff --git a/camCalib/sourceCode/MonoLaserCalibrate.cpp b/camCalib/sourceCode/MonoLaserCalibrate.cpp index 64d3f11..1a9555e 100644 --- a/camCalib/sourceCode/MonoLaserCalibrate.cpp +++ b/camCalib/sourceCode/MonoLaserCalibrate.cpp @@ -191,58 +191,48 @@ void detectCharucoCorners(const cv::Mat& img, cv::Ptr ptrBoard = cv::makePtr(board); cv::aruco::interpolateCornersCharuco(markerCorners, markerIds, gray, ptrBoard, charucoCorners, charucoIds); // if at least one charuco corner detected +#if 1 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)); +#endif } return; } void gen3DCoordinate_chessboard( - const std::vector>& imagePoints, + const std::vector& corners, const cv::Size& patternSize, const float squareSize, - std::vector>& objectPoints + std::vector& objectPoints ) { - // 根据角点生成3d点 - for (const auto& corners : imagePoints) { + // 准备3D世界坐标点 (z=0) + for (int i = 0; i < patternSize.height; ++i) + for (int j = 0; j < patternSize.width; ++j) + objectPoints.emplace_back(j * squareSize, i * squareSize, 0); - // 准备3D世界坐标点 (z=0) - std::vector obj; - for (int i = 0; i < patternSize.height; ++i) - for (int j = 0; j < patternSize.width; ++j) - obj.emplace_back(j * squareSize, i * squareSize, 0); - - objectPoints.push_back(obj); - } return; } void gen3DCoordinate_charuco( - std::vector>& charucoIds, - std::vector>& charucoCorners, + std::vector& charucoIds, + std::vector& charucoCorners, const cv::Size& patternSize, const float squareSize, - std::vector>& objectPoints) + 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); + float ratio = 1.002; + // 准备3D世界坐标点 (z=0) + for (int j = 0, j_max = (int)charucoCorners.size(); j < j_max; j++) + { + int id = charucoIds[j]; + int id_row = id / (patternSize.width-1); + int id_col = id % (patternSize.width-1); + objectPoints.emplace_back(id_col * squareSize/ ratio, id_row *squareSize, 0); } + return; } @@ -265,7 +255,7 @@ void monocularCalibration( flags |= cv::fisheye::CALIB_FIX_SKEW; cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs);// , flags, cv::TermCriteria(3, 20, 1e-6)); #else - cv::calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, cv::CALIB_FIX_ASPECT_RATIO); + cv::calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs); // , cv::CALIB_FIX_ASPECT_RATIO); #endif // 重投影三维点到二维图像点 // 计算重投影误差 @@ -320,13 +310,17 @@ void monocularCalibration_chessboard( cv::Mat& distCoeffs, std::vector& reprojectionError) { + // 根据角点生成3d点 std::vector> objectPoints; - gen3DCoordinate_chessboard( - imagePoints, - patternSize, - squareSize, - objectPoints - ); + for (const auto& corners : imagePoints) { + std::vector obj; + gen3DCoordinate_chessboard( + corners, + patternSize, + squareSize, + obj); + objectPoints.push_back(obj); + } monocularCalibration( imagePoints, @@ -349,13 +343,19 @@ void monocularCalibration_charuco( cv::Mat& distCoeffs, std::vector& reprojectionError) { + // 根据角点生成3d点 std::vector> objectPoints; - gen3DCoordinate_charuco( - charucoIds, - charucoCorners, - patternSize, - squareSize, - objectPoints); + for (int i = 0, i_max = (int)charucoCorners.size(); i < i_max; i++) + { + std::vector obj; + gen3DCoordinate_charuco( + charucoIds[i], + charucoCorners[i], + patternSize, + squareSize, + obj); + objectPoints.push_back(obj); + } monocularCalibration( charucoCorners, @@ -369,21 +369,12 @@ void monocularCalibration_charuco( /*Brief: 拟合棋盘格角点平面*/ void fitChessboardPlane( - const std::vector& corners, + std::vector& corners, + std::vector& objectPoints, const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs, - const cv::Size& patternSize, - const float squareSize, cv::Vec4f& planeEquation) { - // 1. 生成3D世界坐标点 (z=0) - std::vector objectPoints; - for (int i = 0; i < patternSize.height; ++i) { - for (int j = 0; j < patternSize.width; ++j) { - objectPoints.emplace_back(j * squareSize, i * squareSize, 0); - } - } - // 2. 解算PnP获取棋盘格位姿 cv::Mat rvec, tvec; cv::solvePnP(objectPoints, corners, cameraMatrix, distCoeffs, rvec, tvec); @@ -412,6 +403,58 @@ void fitChessboardPlane( return; } +/*Brief: 拟合棋盘格角点平面*/ +void fitChessboardPlane_chessboard( + std::vector& corners, + const cv::Mat& cameraMatrix, + const cv::Mat& distCoeffs, + const cv::Size& patternSize, + const float squareSize, + cv::Vec4f& planeEquation) +{ + std::vector objectPoints; + gen3DCoordinate_chessboard( + corners, + patternSize, + squareSize, + objectPoints); + + fitChessboardPlane( + corners, + objectPoints, + cameraMatrix, + distCoeffs, + planeEquation); + return; +} + +/*Brief: 拟合二维码标定板角点平面*/ +void fitChessboardPlane_charuco( + std::vector& charucoIds, + std::vector& corners, + const cv::Mat& cameraMatrix, + const cv::Mat& distCoeffs, + const cv::Size& patternSize, + const float squareSize, + cv::Vec4f& planeEquation) +{ + std::vector objectPoints; + gen3DCoordinate_charuco( + charucoIds, + corners, + patternSize, + squareSize, + objectPoints); + + fitChessboardPlane( + corners, + objectPoints, + cameraMatrix, + distCoeffs, + planeEquation); + return; +} + /*Brief: 构造激光出射平面方程*/ void generateLaserLine( const float baseLine, diff --git a/camCalib/sourceCode/MonoLaserCalibrate.h b/camCalib/sourceCode/MonoLaserCalibrate.h index 9367786..57b49e3 100644 --- a/camCalib/sourceCode/MonoLaserCalibrate.h +++ b/camCalib/sourceCode/MonoLaserCalibrate.h @@ -49,8 +49,18 @@ void monocularCalibration_charuco( std::vector& reprojectionError); /*Brief: ̸ǵƽ*/ -void fitChessboardPlane( - const std::vector& corners, +void fitChessboardPlane_chessboard( + std::vector& corners, + const cv::Mat& cameraMatrix, + const cv::Mat& distCoeffs, + const cv::Size& patternSize, + const float squareSize, + cv::Vec4f& planeEquation); + +/*Brief: ϶ά궨ǵƽ*/ +void fitChessboardPlane_charuco( + std::vector& charucoIds, + std::vector& corners, const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs, const cv::Size& patternSize,