jerryzeng/camAlgo
2
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

代码同步

Browse Source
This commit is contained in:
jerryzeng 2025-08-23 21:35:12 +08:00
parent 2ff81bd90f
commit 838c37da3c
1 changed files with 177 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

308
camCalib/camCalib.cpp
View File

@ -29,7 +29,7 @@ void sg_outputCalibK(const char* fileName, cv::Mat& fitMap)
return; 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); std::ofstream sw(fileName);
@ -50,6 +50,20 @@ void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D)
temp[_c] = D.ptr<double>(0)[_c]; temp[_c] = D.ptr<double>(0)[_c];
sprintf_s(dataStr, 250, "%g, %g, %g, %g, %g", temp[0], temp[1], temp[2], temp[3], temp[4]); sprintf_s(dataStr, 250, "%g, %g, %g, %g, %g", temp[0], temp[1], temp[2], temp[3], temp[4]);
sw << dataStr << std::endl; 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(); sw.close();
return; return;
} }
@ -87,6 +101,24 @@ void sg_readCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D)
return; return;
} }
void saveSubpixData(char* filename, std::vector<cv::Point2f>& 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 typedef struct
{ {
int nMin; //< 最小值 int nMin; //< 最小值
@ -103,13 +135,13 @@ int main()
std::cout << "Hello World!\n"; std::cout << "Hello World!\n";
const char* calibDataPath[CALIB_TEST_GROUP] = { const char* calibDataPath[CALIB_TEST_GROUP] = {
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\撕裂原理相机标定图像\\", //0 "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\撕裂原理相机标定图像\\", //0
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\chessboard\\", //1 "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\chessboard\\", //1
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\circlePoint\\", //2 "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\circlePoint\\", //2
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\charuCo\\", //3 "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\charuCo\\", //3
}; };
const SWdNLRange fileIdx[CALIB_TEST_GROUP] = { 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] = const int boardType[CALIB_TEST_GROUP] =
{ {
@ -121,7 +153,7 @@ int main()
for(int grp = 0; grp < CALIB_TEST_GROUP; grp ++) for(int grp = 0; grp < CALIB_TEST_GROUP; grp ++)
{ {
grp = 2; grp = 1;
int calibType = boardType[grp]; int calibType = boardType[grp];
cv::Size cbPattern; cv::Size cbPattern;
float cbSquareSize; float cbSquareSize;
@ -255,9 +287,7 @@ int main()
std::cout << "Y Mean difference: " << meanVal_y[0] << std::endl; std::cout << "Y Mean difference: " << meanVal_y[0] << std::endl;
//生成矫正图像 //生成矫正图像
index = 3; for (index = startIndex; index <= endIndex; index++) {
for (;; index++) {
char filename[256]; char filename[256];
sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index); sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index);
cv::Mat srcImg = cv::imread(filename); cv::Mat srcImg = cv::imread(filename);
@ -278,10 +308,6 @@ int main()
cv::imwrite(filename, calibImg); 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 #else
char calibKDName[256]; char calibKDName[256];
sprintf_s(calibKDName, "%scalib_param_K_D.txt", cbImagePath); sprintf_s(calibKDName, "%scalib_param_K_D.txt", cbImagePath);
@ -326,136 +352,156 @@ int main()
#endif #endif
{ std::vector<cv::Point3f> all_pts3d;
std::vector<cv::Point3f> all_pts3d; for (index = startIndex; index <= endIndex; index++) {
index = 3; char filename[256];
for (;; index++) { sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index);
cv::Mat srcImg = cv::imread(filename);
if (srcImg.empty())
break;
char filename[256]; cv::Mat img;
sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index); cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
cv::Mat srcImg = cv::imread(filename); std::vector<cv::Point2f> corners;
if (srcImg.empty()) detectCorners(img, cbPattern, corners);
break; if (corners.empty())
continue;
cv::Mat img; // 创建棋盘格区域的掩码
cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE); cv::Mat chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
std::vector<cv::Point2f> corners; // 使用多边形近似来填充角点之间的区域
detectCorners(img, cbPattern, corners); // 棋盘格区域需要比角点区域大一圈
if (corners.empty()) std::vector<cv::Point2f> contour_line[4];
continue; for (int i = 0; i < cbPattern.width; i++)
{
// 创建棋盘格区域的掩码 cv::Point2f pt_c = corners[i];
cv::Mat chessMask = cv::Mat::zeros(img.size(), CV_8UC1); cv::Point2f pt_2 = corners[cbPattern.width + i];
// 使用多边形近似来填充角点之间的区域 cv::Point2f pt_1;
// 棋盘格区域需要比角点区域大一圈 pt_1.x = pt_c.x * 2 - pt_2.x;
std::vector<cv::Point2f> contour_line[4]; pt_1.y = pt_c.y * 2 - pt_2.y;
for (int i = 0; i < cbPattern.width; i++) contour_line[0].push_back(pt_1);
{ }
cv::Point2f pt_c = corners[i]; for (int i = 0; i < cbPattern.height; i++)
cv::Point2f pt_2 = corners[cbPattern.width + i]; {
cv::Point2f pt_1; cv::Point2f pt_c = corners[i * cbPattern.width + cbPattern.width - 1];
pt_1.x = pt_c.x * 2 - pt_2.x; cv::Point2f pt_2 = corners[i * cbPattern.width + cbPattern.width - 2];
pt_1.y = pt_c.y * 2 - pt_2.y; cv::Point2f pt_1;
contour_line[0].push_back(pt_1); pt_1.x = pt_c.x * 2 - pt_2.x;
} pt_1.y = pt_c.y * 2 - pt_2.y;
for (int i = 0; i < cbPattern.height; i++) contour_line[1].push_back(pt_1);
{ }
cv::Point2f pt_c = corners[i * cbPattern.width + cbPattern.width - 1]; for (int i = cbPattern.width - 1; i >= 0; i--)
cv::Point2f pt_2 = corners[i * cbPattern.width + cbPattern.width - 2]; {
cv::Point2f pt_1; cv::Point2f pt_c = corners[(cbPattern.height - 1) * cbPattern.width + i];
pt_1.x = pt_c.x * 2 - pt_2.x; cv::Point2f pt_2 = corners[(cbPattern.height - 2) * cbPattern.width + i];
pt_1.y = pt_c.y * 2 - pt_2.y; cv::Point2f pt_1;
contour_line[1].push_back(pt_1); pt_1.x = pt_c.x * 2 - pt_2.x;
} pt_1.y = pt_c.y * 2 - pt_2.y;
for (int i = cbPattern.width - 1; i >= 0; i--) contour_line[2].push_back(pt_1);
{ }
cv::Point2f pt_c = corners[(cbPattern.height - 1) * cbPattern.width + i]; for (int i = cbPattern.height-1; i >= 0; i--)
cv::Point2f pt_2 = corners[(cbPattern.height - 2) * cbPattern.width + i]; {
cv::Point2f pt_1; cv::Point2f pt_c = corners[i * cbPattern.width];
pt_1.x = pt_c.x * 2 - pt_2.x; cv::Point2f pt_2 = corners[i * cbPattern.width + 1];
pt_1.y = pt_c.y * 2 - pt_2.y; cv::Point2f pt_1;
contour_line[2].push_back(pt_1); pt_1.x = pt_c.x * 2 - pt_2.x;
} pt_1.y = pt_c.y * 2 - pt_2.y;
for (int i = cbPattern.height-1; i >= 0; i--) contour_line[3].push_back(pt_1);
{ }
cv::Point2f pt_c = corners[i * cbPattern.width]; std::vector<cv::Point> contours;
cv::Point2f pt_2 = corners[i * cbPattern.width + 1]; //生成轮廓点
cv::Point2f pt_1; for (int n = 0; n < 4; n++)
pt_1.x = pt_c.x * 2 - pt_2.x; {
pt_1.y = pt_c.y * 2 - pt_2.y; int num = contour_line[n].size();
contour_line[3].push_back(pt_1); for (int i = 0; i < num; i++)
} contours.push_back(contour_line[n][i]);
std::vector<cv::Point> contours; cv::Point2f pt_c = contour_line[n][num - 1];
//生成轮廓点 cv::Point2f pt_2 = contour_line[n][num - 2];
for (int n = 0; n < 4; n++) cv::Point2f pt_1;
{ pt_1.x = pt_c.x * 2 - pt_2.x;
int num = contour_line[n].size(); pt_1.y = pt_c.y * 2 - pt_2.y;
for (int i = 0; i < num; i++) contours.push_back(pt_1);
contours.push_back(contour_line[n][i]); }
cv::Point2f pt_c = contour_line[n][num - 1]; // 使用 fillPoly 填充多边形
cv::Point2f pt_2 = contour_line[n][num - 2]; cv::Scalar color(255); // 红色
cv::Point2f pt_1; cv::fillPoly(chessMask, contours, color);
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 #if 1
sprintf_s(filename, "%schessMask_%03d.png", calibDataPath[grp], index); sprintf_s(filename, "%schessMask_%03d.png", calibDataPath[grp], index);
cv::imwrite(filename, chessMask); cv::imwrite(filename, chessMask);
#endif #endif
cv::Vec4f pe; cv::Vec4f pe;
fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe); fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe);
sprintf_s(filename, "%slaser_%03d.bmp", calibDataPath[grp], index); sprintf_s(filename, "%slaser_%03d.bmp", calibDataPath[grp], index);
cv::Mat srcLaserImg = cv::imread(filename); cv::Mat srcLaserImg = cv::imread(filename);
if (srcLaserImg.empty()) if (srcLaserImg.empty())
break; break;
cv::Mat laserImg_unMask; cv::Mat laserImg_unMask;
cv::rotate(srcLaserImg, laserImg_unMask, cv::ROTATE_90_COUNTERCLOCKWISE); cv::rotate(srcLaserImg, laserImg_unMask, cv::ROTATE_90_COUNTERCLOCKWISE);
//与Mask相与，保证待处理的激光线在标定板上 //与Mask相与，保证待处理的激光线在标定板上
cv::Mat laserImg; cv::Mat laserImg;
cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask); cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask);
#if 1 #if 1
sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index); sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index);
cv::imwrite(filename, laserImg); cv::imwrite(filename, laserImg);
#endif
std::vector<cv::Point2f> 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);
for (int i = 0, i_max = pts2d.size(); i < i_max; i++) cv::Mat calibImg;
{ remap(laserImg,
cv::Point2f a_subPix = pts2d[i]; calibImg,
a_subPix.x += 0.5; mapGen_x,
int row = (int)(a_subPix.y + 0.5); mapGen_y,
int col = (int)(a_subPix.x * 5 + 0.5); cv::INTER_LINEAR,
enlargeImg.at<cv::Vec3b>(row, col)[0] = 0; cv::BORDER_CONSTANT,
enlargeImg.at<cv::Vec3b>(row, col)[1] = 0; cv::Scalar(0, 0, 0));
enlargeImg.at<cv::Vec3b>(row, col)[2] = 255; sprintf_s(filename, "%slaser_%03d_calib.bmp", calibDataPath[grp], index);
} cv::imwrite(filename, calibImg);
sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpix.png", calibDataPath[grp], index);
cv::imwrite(filename, enlargeImg);
std::vector<cv::Point3f> pts3d = project2DTo3D(pts2d, pe, K, D);
all_pts3d.insert(all_pts3d.end(), pts3d.begin(), pts3d.end()); #endif
std::vector<cv::Point2f> 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<cv::Vec3b>(row, col)[0] = 0;
enlargeImg.at<cv::Vec3b>(row, col)[1] = 0;
enlargeImg.at<cv::Vec3b>(row, col)[2] = 255;
}
sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpix.png", calibDataPath[grp], index);
cv::imwrite(filename, enlargeImg);
std::vector<cv::Point3f> pts3d = project2DTo3D(pts2d, pe, K, D);
#if 1
//保存3D点
cv::Vec4f pe = fitPlaneToPoints(all_pts3d); #endif
std::cout << "pe: " << pe << std::endl; 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; return 0;