From 838c37da3c176df7b2bd4eabd2d771d762a2a74e Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sat, 23 Aug 2025 21:35:12 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BB=A3=E7=A0=81=E5=90=8C=E6=AD=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- camCalib/camCalib.cpp | 308 ++++++++++++++++++++++++------------------ 1 file changed, 177 insertions(+), 131 deletions(-) diff --git a/camCalib/camCalib.cpp b/camCalib/camCalib.cpp index 301c8e0..a1fd0da 100644 --- a/camCalib/camCalib.cpp +++ b/camCalib/camCalib.cpp @@ -29,7 +29,7 @@ void sg_outputCalibK(const char* fileName, cv::Mat& fitMap) return; } -void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D) +void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D, cv::Vec4f& pe) { std::ofstream sw(fileName); @@ -50,6 +50,20 @@ void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D) temp[_c] = D.ptr(0)[_c]; sprintf_s(dataStr, 250, "%g, %g, %g, %g, %g", temp[0], temp[1], temp[2], temp[3], temp[4]); sw << dataStr << std::endl; + + //ax+by+cz+d = 0 + float a = (float)pe[0]; + float b = (float)pe[1]; + float c = (float)pe[2]; + float d = (float)pe[3]; + //将c变成-1，转成z=ax+by+c的形式，使用3个参数 + a = -a / c; + b = -b / c; + d = -d / c; + c = -1; + sprintf_s(dataStr, 250, "%g, %g, %g", a, b, d); + sw << dataStr << std::endl; + sw.close(); return; } @@ -87,6 +101,24 @@ void sg_readCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D) return; } +void saveSubpixData(char* filename, std::vector& subpixPnt) +{ + if (subpixPnt.size() < 6) + return; + + std::ofstream TXTFile(filename); + char TXTData[250]; + int headOffset = 6; + for (int i = 6, i_max = (int)subpixPnt.size(); i < i_max; i++) + { + cv::Point2f a_subPix = subpixPnt[i]; + snprintf(TXTData, sizeof(TXTData), "%.5f %.5f", a_subPix.x, a_subPix.y); + TXTFile << TXTData << std::endl; + } + TXTFile.close(); +} + + typedef struct { int nMin; //< 最小值 @@ -103,13 +135,13 @@ int main() std::cout << "Hello World!\n"; const char* calibDataPath[CALIB_TEST_GROUP] = { - "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\撕裂原理相机标定图像\\", //0 - "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\chessboard\\", //1 - "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\circlePoint\\", //2 - "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\charuCo\\", //3 + "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\撕裂原理相机标定图像\\", //0 + "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\chessboard\\", //1 + "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\circlePoint\\", //2 + "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\charuCo\\", //3 }; const SWdNLRange fileIdx[CALIB_TEST_GROUP] = { - {3,39},{1,200},{1,166},{122,141} + {3,39},{1,33},{1,33},{1,10} }; const int boardType[CALIB_TEST_GROUP] = { @@ -121,7 +153,7 @@ int main() for(int grp = 0; grp < CALIB_TEST_GROUP; grp ++) { - grp = 2; + grp = 1; int calibType = boardType[grp]; cv::Size cbPattern; float cbSquareSize; @@ -255,9 +287,7 @@ int main() std::cout << "Y Mean difference: " << meanVal_y[0] << std::endl; //生成矫正图像 - index = 3; - for (;; index++) { - + for (index = startIndex; index <= endIndex; index++) { char filename[256]; sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index); cv::Mat srcImg = cv::imread(filename); @@ -278,10 +308,6 @@ int main() cv::imwrite(filename, calibImg); } - //output K and D - char calibKDName[256]; - sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]); - sg_outputCalibKD(calibKDName, K, D); #else char calibKDName[256]; sprintf_s(calibKDName, "%scalib_param_K_D.txt", cbImagePath); @@ -326,136 +352,156 @@ int main() #endif - { - std::vector all_pts3d; + std::vector all_pts3d; + for (index = startIndex; index <= endIndex; index++) { - index = 3; - for (;; index++) { + char filename[256]; + sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index); + cv::Mat srcImg = cv::imread(filename); + if (srcImg.empty()) + break; - char filename[256]; - sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index); - cv::Mat srcImg = cv::imread(filename); - if (srcImg.empty()) - break; + cv::Mat img; + cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE); + std::vector corners; + detectCorners(img, cbPattern, corners); + if (corners.empty()) + continue; - cv::Mat img; - cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE); - std::vector corners; - detectCorners(img, cbPattern, 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::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); + cv::Vec4f pe; + fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe); - sprintf_s(filename, "%slaser_%03d.bmp", calibDataPath[grp], index); - cv::Mat srcLaserImg = cv::imread(filename); - if (srcLaserImg.empty()) - break; + sprintf_s(filename, "%slaser_%03d.bmp", calibDataPath[grp], index); + cv::Mat srcLaserImg = cv::imread(filename); + if (srcLaserImg.empty()) + break; - cv::Mat laserImg_unMask; - cv::rotate(srcLaserImg, laserImg_unMask, cv::ROTATE_90_COUNTERCLOCKWISE); - //与Mask相与，保证待处理的激光线在标定板上 - cv::Mat laserImg; - cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask); - #if 1 - sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index); - cv::imwrite(filename, laserImg); - #endif - std::vector pts2d = detectLaserLine(laserImg); - //显示亚像素点 - cv::Mat enlargeImg; - if (laserImg.channels() == 1) - laserImg.convertTo(enlargeImg, cv::COLOR_GRAY2BGR); - else - enlargeImg = laserImg.clone(); - cv::Size objSize = laserImg.size(); - objSize.width = objSize.width * 5; - cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST); + cv::Mat laserImg_unMask; + cv::rotate(srcLaserImg, laserImg_unMask, cv::ROTATE_90_COUNTERCLOCKWISE); + //与Mask相与，保证待处理的激光线在标定板上 + cv::Mat laserImg; + cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask); +#if 1 + sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index); + cv::imwrite(filename, laserImg); - for (int i = 0, i_max = pts2d.size(); i < i_max; i++) - { - cv::Point2f a_subPix = pts2d[i]; - a_subPix.x += 0.5; - int row = (int)(a_subPix.y + 0.5); - int col = (int)(a_subPix.x * 5 + 0.5); - enlargeImg.at(row, col)[0] = 0; - enlargeImg.at(row, col)[1] = 0; - enlargeImg.at(row, col)[2] = 255; - } - sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpix.png", calibDataPath[grp], index); - cv::imwrite(filename, enlargeImg); - std::vector pts3d = project2DTo3D(pts2d, pe, K, D); + cv::Mat calibImg; + remap(laserImg, + calibImg, + mapGen_x, + mapGen_y, + cv::INTER_LINEAR, + cv::BORDER_CONSTANT, + cv::Scalar(0, 0, 0)); + sprintf_s(filename, "%slaser_%03d_calib.bmp", calibDataPath[grp], index); + cv::imwrite(filename, calibImg); - all_pts3d.insert(all_pts3d.end(), pts3d.begin(), pts3d.end()); +#endif + std::vector pts2d = detectLaserLine(laserImg); + //显示亚像素点 + cv::Mat enlargeImg; + if (laserImg.channels() == 1) + laserImg.convertTo(enlargeImg, cv::COLOR_GRAY2BGR); + else + enlargeImg = laserImg.clone(); + cv::Size objSize = laserImg.size(); + objSize.width = objSize.width * 5; + cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST); + + if (pts2d.size() > 0) + { + sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpixData.txt", calibDataPath[grp], index); + saveSubpixData(filename, pts2d); } + for (int i = 0, i_max = pts2d.size(); i < i_max; i++) + { + cv::Point2f a_subPix = pts2d[i]; + int row = (int)(a_subPix.y + 0.5); + int col = (int)(a_subPix.x * 5 + 0.5); + enlargeImg.at(row, col)[0] = 0; + enlargeImg.at(row, col)[1] = 0; + enlargeImg.at(row, col)[2] = 255; + } + sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpix.png", calibDataPath[grp], index); + cv::imwrite(filename, enlargeImg); + std::vector pts3d = project2DTo3D(pts2d, pe, K, D); +#if 1 + //保存3D点 - cv::Vec4f pe = fitPlaneToPoints(all_pts3d); - std::cout << "pe: " << pe << std::endl; +#endif + all_pts3d.insert(all_pts3d.end(), pts3d.begin(), pts3d.end()); } + + cv::Vec4f pe = fitPlaneToPoints(all_pts3d); + std::cout << "pe: " << pe << std::endl; + + //output K and D + char calibKDName[256]; + sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]); + sg_outputCalibKD(calibKDName, K, D, pe); } return 0;