diff --git a/CamAlgo/CamAlgo.vcxproj b/CamAlgo/CamAlgo.vcxproj index e030b23..26ede9d 100644 --- a/CamAlgo/CamAlgo.vcxproj +++ b/CamAlgo/CamAlgo.vcxproj @@ -30,26 +30,26 @@ Application true - v142 + v143 Unicode Application false - v142 + v143 true Unicode Application true - v142 + v143 Unicode Application false - v142 + v143 true Unicode diff --git a/HessianTable/HessianTable.vcxproj b/HessianTable/HessianTable.vcxproj index bdf73ea..607af4e 100644 --- a/HessianTable/HessianTable.vcxproj +++ b/HessianTable/HessianTable.vcxproj @@ -29,26 +29,26 @@ Application true - v142 + v143 Unicode Application false - v142 + v143 true Unicode Application true - v142 + v143 Unicode Application false - v142 + v143 true Unicode diff --git a/Subpixel/Subpixel.vcxproj b/Subpixel/Subpixel.vcxproj index a592115..426b7a2 100644 --- a/Subpixel/Subpixel.vcxproj +++ b/Subpixel/Subpixel.vcxproj @@ -29,26 +29,26 @@ Application true - v142 + v143 Unicode Application false - v142 + v143 true Unicode Application true - v142 + v143 Unicode Application false - v142 + v143 true Unicode diff --git a/camAlgoSW/camAlgoSW.vcxproj b/camAlgoSW/camAlgoSW.vcxproj index 9baf6c7..6c77f9f 100644 --- a/camAlgoSW/camAlgoSW.vcxproj +++ b/camAlgoSW/camAlgoSW.vcxproj @@ -45,26 +45,26 @@ DynamicLibrary true - v142 + v143 Unicode DynamicLibrary false - v142 + v143 true Unicode DynamicLibrary true - v142 + v143 Unicode DynamicLibrary false - v142 + v143 true Unicode diff --git a/camAlgoSW_test/camAlgoSW_test.vcxproj b/camAlgoSW_test/camAlgoSW_test.vcxproj index 9c59262..cbec48f 100644 --- a/camAlgoSW_test/camAlgoSW_test.vcxproj +++ b/camAlgoSW_test/camAlgoSW_test.vcxproj @@ -29,26 +29,26 @@ Application true - v142 + v143 Unicode Application false - v142 + v143 true Unicode Application true - v142 + v143 Unicode Application false - v142 + v143 true Unicode diff --git a/camCalib/camCalib.cpp b/camCalib/camCalib.cpp index 48664e9..29e28e5 100644 --- a/camCalib/camCalib.cpp +++ b/camCalib/camCalib.cpp @@ -128,6 +128,7 @@ typedef struct #define CALIB_CHESS_BOARD 1 #define CALIB_CIRCLE_GRID 2 #define CALIB_CHARUCO 3 +#define CALIB_BALL_POLE 4 void initForwardRectMap(const cv::Mat& K, const cv::Mat& D, const cv::Mat& R, const cv::Mat& newK, const cv::Size& size, cv::Mat& mapX, cv::Mat& mapY) { @@ -156,31 +157,33 @@ void initForwardRectMap(const cv::Mat& K, const cv::Mat& D, const cv::Mat& R, return; } -#define CALIB_TEST_GROUP 4 -int main() +#define MONO_CALIB_TEST_GROUP 5 +int MonoCamLaserCalib() { std::cout << "Hello World!\n"; - const char* calibDataPath[CALIB_TEST_GROUP] = { + const char* calibDataPath[MONO_CALIB_TEST_GROUP] = { "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 + "F:/ShangGu/单目相机标定数据/TestData/", //4 }; - const SWdNLRange fileIdx[CALIB_TEST_GROUP] = { - {3,39},{1,33},{1,33},{1,10} + const SWdNLRange fileIdx[MONO_CALIB_TEST_GROUP] = { + {3,39},{6,33},{1,33},{1,10},{1,26} }; - const int boardType[CALIB_TEST_GROUP] = + const int boardType[MONO_CALIB_TEST_GROUP] = { CALIB_CHESS_BOARD, CALIB_CHESS_BOARD, CALIB_CIRCLE_GRID, - CALIB_CHARUCO + CALIB_CHARUCO, + CALIB_CHARUCO, }; - for(int grp = 0; grp < CALIB_TEST_GROUP; grp ++) + for(int grp = 4; grp <= 4; grp ++) { - grp = 3; + //grp = 1; int calibType = boardType[grp]; cv::Size cbPattern; float cbSquareSize; @@ -197,9 +200,9 @@ int main() } else if (CALIB_CHARUCO == calibType) { - cbPattern = cv::Size(47, 21); // 10); - cbSquareSize = 0.05f; - markSize = 0.037f; + cbPattern = cv::Size(9, 9); //cv::Size(47, 21); // 10); + cbSquareSize = 50.0f; + markSize = 37.0f; } else continue; @@ -213,19 +216,21 @@ int main() int index; #if _DO_CAMERA_CALIB for (index = startIndex; index <= endIndex; index++) { - char filename[256]; - sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index); - cv::Mat srcImg = cv::imread(filename); - if (srcImg.empty()) + char filenameInput[256]; + sprintf_s(filenameInput, "%sImage_%d.png", calibDataPath[grp], index); + cv::Mat img = cv::imread(filenameInput); + if (img.empty()) break; + char filename[256]; +#if 0 cv::Mat img; cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE); char rotateFilename[256]; - sprintf_s(rotateFilename, "%scalib_%03d_rotate.bmp", calibDataPath[grp], index); + sprintf_s(rotateFilename, "%sresult/calib_%03d_rotate.bmp", calibDataPath[grp], index); cv::String rFileName(rotateFilename); cv::imwrite(rFileName, img); - +#endif std::vector corners; if (CALIB_CHESS_BOARD == calibType) { @@ -253,7 +258,7 @@ int main() cv::Mat color = img.clone(); cv::drawChessboardCorners(color, cbPattern, corners, true); //cv::resize(color, color, cv::Size(), 0.5, 0.5); - sprintf_s(filename, "%scalib_%03d_center.bmp", calibDataPath[grp], index); + sprintf_s(filename, "%sresult/calib_%03d_center.bmp", calibDataPath[grp], index); cv::imwrite(filename, color); } #endif @@ -284,13 +289,14 @@ int main() cv::aruco::drawDetectedCornersCharuco(imageCopy, corners, charucoIds, cv::Scalar(0, 255, 255)); } char markFilename[256]; - sprintf_s(markFilename, "%scalib_%03d_markers.bmp", calibDataPath[grp], index); + sprintf_s(markFilename, "%sresult/calib_%03d_markers.bmp", calibDataPath[grp], index); cv::imwrite(markFilename, imageCopy); if (corners.empty()) continue; cbCornersList.push_back(corners); cbCornersIdList.push_back(charucoIds); + printf("%s... cornerSize=%d, charucoIDSize=%d\n", filenameInput, (int)corners.size(), (int)charucoIds.size()); } imageSize = cv::Size(img.cols, img.rows); } @@ -301,9 +307,14 @@ int main() monocularCalibration_charuco(cbCornersIdList, cbCornersList, imageSize, cbPattern, cbSquareSize, K, D, reprojectionError); else monocularCalibration_chessboard(cbCornersList, imageSize, cbPattern, cbSquareSize, K, D, reprojectionError); + std::cout << "K:" << std::endl; + std::cout << K << std::endl; + std::cout << "D:" << std::endl; + std::cout << D << std::endl; for(int i = 0; i < reprojectionError.size(); i ++) std::cout << reprojectionError[i] << std::endl; + // 输出映射类型，通常使用CV_32FC1或CV_16SC2 cv::Mat backwardMap_x, backwardMap_y; cv::Mat forwardMap_x, forwardMap_y; @@ -323,9 +334,9 @@ int main() cv::Mat fitMap_y = GetFitParamMap(forwardMap_y, 1); //输出系数文件 char calibParamName[256]; - sprintf_s(calibParamName, "%scalib_param_x.txt", calibDataPath[grp]); + sprintf_s(calibParamName, "%sresult/calib_param_x.txt", calibDataPath[grp]); sg_outputCalibK(calibParamName, fitMap_x); - sprintf_s(calibParamName, "%scalib_param_y.txt", calibDataPath[grp]); + sprintf_s(calibParamName, "%sresult/calib_param_y.txt", calibDataPath[grp]); sg_outputCalibK(calibParamName, fitMap_y); //比较误差 @@ -350,16 +361,17 @@ int main() std::cout << "Y Max difference: " << maxVal_y << std::endl; std::cout << "Y Mean difference: " << meanVal_y[0] << std::endl; +#if 0 //生成矫正图像 for (index = startIndex; index <= endIndex; index++) { char filename[256]; - sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index); - cv::Mat srcImg = cv::imread(filename); - if (srcImg.empty()) + sprintf_s(filename, "%sImage_%d.png", calibDataPath[grp], index); + cv::Mat img = cv::imread(filename); + if (img.empty()) break; - cv::Mat img; - cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE); + //cv::Mat img; + //cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE); cv::Mat calibImg; cv::remap(img, calibImg, @@ -368,9 +380,10 @@ int main() cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar(0, 0, 0)); - sprintf_s(filename, "%scalib_%03d_calib.bmp", calibDataPath[grp], index); + sprintf_s(filename, "%sresult/calib_%03d_calib.bmp", calibDataPath[grp], index); cv::imwrite(filename, calibImg); } +#endif #else char calibKDName[256]; @@ -428,14 +441,14 @@ int main() for (index = startIndex; index <= endIndex; index++) { char filename[256]; - sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index); - cv::Mat srcImg = cv::imread(filename); - if (srcImg.empty()) + sprintf_s(filename, "%sImage_%d.png", calibDataPath[grp], index); + cv::Mat img = cv::imread(filename); + if (img.empty()) break; - cv::Mat img; - cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE); -#if 0 + //cv::Mat img; + //cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE); +#if 1 cv::Mat charucoCalibImg; cv::remap(img, charucoCalibImg, @@ -447,7 +460,7 @@ int main() 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); + sprintf_s(filename, "%sresult/calib_%03d_calib.bmp", calibDataPath[grp], index); cv::imwrite(filename, charucoCalibImg); #endif @@ -552,6 +565,482 @@ int main() else fitChessboardPlane_chessboard(corners, K, D, cbPattern, cbSquareSize, pe); + sprintf_s(filename, "%sImage_%d_Laser.png", calibDataPath[grp], index); + cv::Mat laserImg_unMask = cv::imread(filename); + if (laserImg_unMask.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_mask_%d.png", calibDataPath[grp], index); + cv::imwrite(filename, laserImg); + + 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, laserCalibImg); +#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 = (int)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点 + +#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 calibKDPName[256]; + sprintf_s(calibKDPName, "%scalib_param_K_D.txt", calibDataPath[grp]); + sg_outputCalibKD(calibKDPName, K, D, pe); + } + return 0; +} + +#if 0 +#define STEREO_CALIB_TEST_GROUP 1 +int StereoCamCalib() +{ + std::cout << "Hello World!\n"; + + const char* calibDataPath[STEREO_CALIB_TEST_GROUP] = { + "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\撕裂原理相机标定图像\\", //0 + }; + const SWdNLRange fileIdx[STEREO_CALIB_TEST_GROUP] = { + {3,39}, + }; + const int boardType[STEREO_CALIB_TEST_GROUP] = + { + CALIB_BALL_POLE + }; + + for (int grp = 0; grp < 1; grp++) + { + grp = 1; + int calibType = boardType[grp]; + cv::Size cbPattern; + float cbSquareSize; + float markSize; + if (CALIB_CHARUCO == calibType) + { + cbPattern = cv::Size(47, 21); // 10); + cbSquareSize = 50.0f; + markSize = 37.0f; + } + else if (CALIB_CIRCLE_GRID == calibType) + { + cbPattern = cv::Size(7, 7); // 10); + cbSquareSize = 50.0f; // 10.f; + } + else if (CALIB_BALL_POLE == calibType) + { + + } + else + continue; + + std::vector> cbCornersListL; + std::vector> cbCornersIdListL; + std::vector> cbCornersListR; + std::vector> cbCornersIdListR; + cv::Size imageSize; + + int startIndex = fileIdx[grp].nMin; + int endIndex = fileIdx[grp].nMax; + int index; +#if _DO_CAMERA_CALIB + for (index = startIndex; index <= endIndex; index++) + { + char imgFileL[256]; + char imgFileR[256]; + sprintf_s(imgFileL, "%scalib_%03d_L.bmp", calibDataPath[grp], index); + sprintf_s(imgFileR, "%scalib_%03d_R.bmp", calibDataPath[grp], index); + cv::Mat srcImgL = cv::imread(imgFileL); + cv::Mat srcImgR = cv::imread(imgFileR); + if ( (true == srcImgL.empty()) || (true == srcImgR.empty())) + continue; + + std::vector cornersL; + std::vector cornersR; + if (CALIB_CIRCLE_GRID == calibType) + { + detectCirclePoints(srcImgL, cbPattern, cornersL); + detectCirclePoints(srcImgL, cbPattern, cornersR); + if ( (true == cornersL.empty()) || (true == cornersR.empty())) + continue; + +#if ENABLE_DEBUG + { + cv::Mat colorL = srcImgL.clone(); + cv::Mat colorR = srcImgR.clone(); + cv::drawChessboardCorners(colorL, cbPattern, cornersL, true); + cv::drawChessboardCorners(colorR, cbPattern, cornersR, true); + sprintf_s(imgFileL, "%sdebug_%03d_gridL.bmp", calibDataPath[grp], index); + sprintf_s(imgFileR, "%sdebug_%03d_gridR.bmp", calibDataPath[grp], index); + cv::imwrite(imgFileL, colorL); + cv::imwrite(imgFileR, colorR); + } +#endif + cbCornersListL.push_back(cornersL); + cbCornersListR.push_back(cornersR); + } + else if (CALIB_CHARUCO == calibType) + { + std::vector markerIds_L; + std::vector markerIds_R; + std::vector > markerCorners_L; + std::vector > markerCorners_R; + std::vector charucoIds_L; + std::vector charucoIds_R; + detectCharucoCorners(srcImgL, + cbPattern, + cbSquareSize, + markSize, + markerIds_L, + markerCorners_L, + charucoIds_L, + cornersL); + detectCharucoCorners(srcImgR, + cbPattern, + cbSquareSize, + markSize, + markerIds_R, + markerCorners_R, + charucoIds_R, + cornersR); + + if ((true == cornersL.empty()) || (true == cornersR.empty())) + continue; +#if ENABLE_DEBUG + { + cv::Mat colorL = srcImgL.clone(); + cv::Mat colorR = srcImgR.clone(); + cv::aruco::drawDetectedMarkers(colorL, markerCorners_L, markerIds_L); + cv::aruco::drawDetectedCornersCharuco(colorL, cornersL, charucoIds_L, cv::Scalar(0, 255, 255)); + cv::aruco::drawDetectedMarkers(colorR, markerCorners_R, markerIds_R); + cv::aruco::drawDetectedCornersCharuco(colorR, cornersR, charucoIds_R, cv::Scalar(0, 255, 255)); + sprintf_s(imgFileL, "%sdebug_%03d_charucoL.bmp", calibDataPath[grp], index); + sprintf_s(imgFileR, "%sdebug_%03d_charucoR.bmp", calibDataPath[grp], index); + cv::imwrite(imgFileL, colorL); + cv::imwrite(imgFileR, colorR); + } +#endif + + cbCornersListL.push_back(cornersL); + cbCornersListR.push_back(cornersR); + cbCornersIdListL.push_back(charucoIds_L); + cbCornersIdListR.push_back(charucoIds_R); + } + else if (CALIB_BALL_POLE == calibType) + { + + } + imageSize = cv::Size(srcImgL.cols, srcImgL.rows); + } + + cv::Mat K, D; + std::vector 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); + std::cout << "K:" << std::endl; + std::cout << K << std::endl; + std::cout << "D:" << std::endl; + std::cout << D << std::endl; + for (int i = 0; i < reprojectionError.size(); i++) + std::cout << reprojectionError[i] << std::endl; + + + // 输出映射类型，通常使用CV_32FC1或CV_16SC2 + cv::Mat backwardMap_x, backwardMap_y; + cv::Mat forwardMap_x, forwardMap_y; + cv::Mat newCamMatrix; + // 生成畸变矫正映射 +#if ENABLE_FISH_EYE + cv::fisheye::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, map1, map2); +#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, backwardMap_x, backwardMap_y); + initForwardRectMap(K, D, cv::Mat(), newCamMatrix, imageSize, forwardMap_x, forwardMap_y); +#endif + + // 生成系数表 + 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]); + sg_outputCalibK(calibParamName, fitMap_x); + sprintf_s(calibParamName, "%scalib_param_y.txt", calibDataPath[grp]); + sg_outputCalibK(calibParamName, fitMap_y); + + //比较误差 + cv::Mat mapGen_x = GetMapFromFitMap(fitMap_x, imageSize, 1); + cv::Mat mapGen_y = GetMapFromFitMap(fitMap_y, imageSize, 1); + //搜索最大和平均误差 + // 计算绝对差异 + cv::Mat diff_x, 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); + double minVal_y, maxVal_y; + cv::minMaxLoc(diff_y, &minVal_y, &maxVal_y); + // 计算平均值 + cv::Scalar meanVal_x = cv::mean(diff_x); + cv::Scalar meanVal_y = cv::mean(diff_y); + + std::cout << "X Max_difference: " << maxVal_x << std::endl; + std::cout << "X Mean difference: " << meanVal_x[0] << std::endl; + std::cout << "Y Max difference: " << maxVal_y << std::endl; + std::cout << "Y Mean difference: " << meanVal_y[0] << std::endl; + + //生成矫正图像 + for (index = startIndex; index <= endIndex; index++) { + 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); + cv::Mat calibImg; + cv::remap(img, + calibImg, + backwardMap_x, + backwardMap_y, + cv::INTER_LINEAR, + cv::BORDER_CONSTANT, + cv::Scalar(0, 0, 0)); + sprintf_s(filename, "%scalib_%03d_calib.bmp", calibDataPath[grp], index); + cv::imwrite(filename, calibImg); + } + +#else + char calibKDName[256]; + 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 + cv::Vec4f laserPE; + generateLaserLine(10.f, 5.f, laserPE); + std::cout << "generateLaserLine pe: " << laserPE << std::endl; + + index = 3; + for (;; index++) { + + char filename[256]; + sprintf_s(filename, "%s%03d.bmp", cbImagePath, index); + cv::Mat img = cv::imread(filename); + if (img.empty()) + break; + + std::vector corners; + detectCorners(img, cbPattern, corners); + if (corners.empty()) + continue; + + cv::Vec4f pe; + fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe); + + cv::Mat image = generateVirtualLaserLineImage(laserPE, pe, K, D, imageSize); + +#if ENABLE_DEBUG + cv::Mat color = image.clone(); + cv::resize(color, color, cv::Size(), 0.5, 0.5); + cv::imshow("image", color); + cv::waitKey(10); · +#endif + + sprintf_s(filename, "%s%d.bmp", laserImagePath, index); + cv::imwrite(filename, image); + } + +#endif + + std::vector all_pts3d; + for (index = startIndex; index <= endIndex; index++) { + + 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); +#if 1 + 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); + } + else if (CALIB_CIRCLE_GRID == calibType) + { + detectCirclePoints(img, cbPattern, corners); + } + else if (CALIB_CHARUCO == calibType) + { + std::vector markerIds; + std::vector > markerCorners; + detectCharucoCorners(img, + cbPattern, + cbSquareSize, + markSize, + markerIds, + markerCorners, + charucoIds, + corners); + } + if (corners.empty()) + continue; + + // 创建棋盘格区域的掩码 + cv::Mat chessMask; + if (CALIB_CHARUCO == calibType) + chessMask = cv::Mat::ones(img.size(), CV_8UC1); + else + { + 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); +#endif + } + + cv::Vec4f 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); if (srcLaserImg.empty()) @@ -625,4 +1114,10 @@ int main() } return 0; } +#endif +int main(void) +{ + MonoCamLaserCalib(); + //StereoCamCalib(); +} diff --git a/camCalib/camCalib.vcxproj b/camCalib/camCalib.vcxproj index 1783a88..109705a 100644 --- a/camCalib/camCalib.vcxproj +++ b/camCalib/camCalib.vcxproj @@ -29,26 +29,26 @@ Application true - v142 + v143 Unicode Application false - v142 + v143 true Unicode Application true - v142 + v143 Unicode Application false - v142 + v143 true Unicode @@ -151,8 +151,10 @@ + + diff --git a/camCalib/camCalib.vcxproj.filters b/camCalib/camCalib.vcxproj.filters index 57fad3b..49aa063 100644 --- a/camCalib/camCalib.vcxproj.filters +++ b/camCalib/camCalib.vcxproj.filters @@ -16,6 +16,9 @@ {5709fb07-01a0-47f3-b3c7-11842aaaa85e} + + {c1cd206d-431c-4fe6-9c47-03c9c93c8b1a} + @@ -42,6 +45,12 @@ 源文件\aruco + + 源文件\ballPole + + + 源文件 + diff --git a/camCalib/sourceCode/MonoLaserCalibrate.cpp b/camCalib/sourceCode/MonoLaserCalibrate.cpp index 1a9555e..d34df39 100644 --- a/camCalib/sourceCode/MonoLaserCalibrate.cpp +++ b/camCalib/sourceCode/MonoLaserCalibrate.cpp @@ -185,11 +185,15 @@ void detectCharucoCorners(const cv::Mat& img, cv::Ptr ptrParams = cv::makePtr(params); cv::aruco::detectMarkers(gray, ptrDictionary, markerCorners, markerIds, ptrParams); + std::vector boardAlignIds; + for (int id : markerIds) + boardAlignIds.push_back(id - 1); + // 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); + cv::aruco::interpolateCornersCharuco(markerCorners, boardAlignIds, gray, ptrBoard, charucoCorners, charucoIds); // if at least one charuco corner detected #if 1 if (charucoIds.size() > 0) @@ -223,14 +227,16 @@ void gen3DCoordinate_charuco( const float squareSize, std::vector& objectPoints) { - float ratio = 1.002; + double ratio = 0.998057; // 准备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); + double x = (double)id_col * (double)squareSize*ratio; + double y = (double)id_row * (double)squareSize; + objectPoints.emplace_back((float)x, (float)y, 0); } return; @@ -255,7 +261,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 // 重投影三维点到二维图像点 // 计算重投影误差