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

添加了charuco的修正系数，以补偿标定板的制作误差

Browse Source
This commit is contained in:
jerryzeng 2025-08-30 16:11:06 +08:00
parent a634036379
commit ea6d44c6d1
3 changed files with 140 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
camCalib/camCalib.cpp
View File

@ -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<cv::Point2f> corners;
std::vector<int> charucoIds;
if (CALIB_CHESS_BOARD == calibType)
{
detectCorners(img, cbPattern, corners);
@ -447,7 +465,6 @@ int main()
{
std::vector<int> markerIds;
std::vector<std::vector<cv::Point2f> > markerCorners;
std::vector<int> 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<cv::Point2f> 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);

151
camCalib/sourceCode/MonoLaserCalibrate.cpp
View File

@ -191,58 +191,48 @@ void detectCharucoCorners(const cv::Mat& img,
cv::Ptr<cv::aruco::CharucoBoard> ptrBoard = cv::makePtr<cv::aruco::CharucoBoard>(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<std::vector<cv::Point2f>>& imagePoints,
const std::vector<cv::Point2f>& corners,
const cv::Size& patternSize,
const float squareSize,
std::vector<std::vector<cv::Point3f>>& objectPoints
std::vector<cv::Point3f>& 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<cv::Point3f> 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<std::vector<int>>& charucoIds,
std::vector<std::vector<cv::Point2f>>& charucoCorners,
std::vector<int>& charucoIds,
std::vector<cv::Point2f>& charucoCorners,
const cv::Size& patternSize,
const float squareSize,
std::vector<std::vector<cv::Point3f>>& objectPoints)
std::vector<cv::Point3f>& objectPoints)
{
// 根据角点生成3d点
for (int i = 0, i_max = (int)charucoCorners.size(); i < i_max; i ++) {
std::vector<cv::Point2f>& a_cornerList = charucoCorners[i];
std::vector<int>& a_idList = charucoIds[i];
// 准备3D世界坐标点 (z=0)
std::vector<cv::Point3f> 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<double>& reprojectionError)
{
// 根据角点生成3d点
std::vector<std::vector<cv::Point3f>> objectPoints;
gen3DCoordinate_chessboard(
imagePoints,
patternSize,
squareSize,
objectPoints
);
for (const auto& corners : imagePoints) {
std::vector<cv::Point3f> 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<double>& reprojectionError)
{
// 根据角点生成3d点
std::vector<std::vector<cv::Point3f>> objectPoints;
gen3DCoordinate_charuco(
charucoIds,
charucoCorners,
patternSize,
squareSize,
objectPoints);
for (int i = 0, i_max = (int)charucoCorners.size(); i < i_max; i++)
{
std::vector<cv::Point3f> 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<cv::Point2f>& corners,
std::vector<cv::Point2f>& corners,
std::vector<cv::Point3f>& 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<cv::Point3f> 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<cv::Point2f>& corners,
const cv::Mat& cameraMatrix,
const cv::Mat& distCoeffs,
const cv::Size& patternSize,
const float squareSize,
cv::Vec4f& planeEquation)
{
std::vector<cv::Point3f> objectPoints;
gen3DCoordinate_chessboard(
corners,
patternSize,
squareSize,
objectPoints);
fitChessboardPlane(
corners,
objectPoints,
cameraMatrix,
distCoeffs,
planeEquation);
return;
}
/*Brief: 拟合二维码标定板角点平面*/
void fitChessboardPlane_charuco(
std::vector<int>& charucoIds,
std::vector<cv::Point2f>& corners,
const cv::Mat& cameraMatrix,
const cv::Mat& distCoeffs,
const cv::Size& patternSize,
const float squareSize,
cv::Vec4f& planeEquation)
{
std::vector<cv::Point3f> objectPoints;
gen3DCoordinate_charuco(
charucoIds,
corners,
patternSize,
squareSize,
objectPoints);
fitChessboardPlane(
corners,
objectPoints,
cameraMatrix,
distCoeffs,
planeEquation);
return;
}
/*Brief: 构造激光出射平面方程*/
void generateLaserLine(
const float baseLine,

14
camCalib/sourceCode/MonoLaserCalibrate.h
View File

@ -49,8 +49,18 @@ void monocularCalibration_charuco(
std::vector<double>& reprojectionError);
/*Brief: 拟合棋盘格角点平面*/
void fitChessboardPlane(
const std::vector<cv::Point2f>& corners,
void fitChessboardPlane_chessboard(
std::vector<cv::Point2f>& 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<int>& charucoIds,
std::vector<cv::Point2f>& corners,
const cv::Mat& cameraMatrix,
const cv::Mat& distCoeffs,
const cv::Size& patternSize,