1269 lines
47 KiB
C++
1269 lines
47 KiB
C++
// camCalib.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
|
||
//
|
||
|
||
#include <iostream>
|
||
#include <fstream>
|
||
#include <opencv2/opencv.hpp>
|
||
#include <vector>
|
||
#include "sourceCode/MonoLaserCalibrate.h"
|
||
#include "sourceCode/FitMapParam.h"
|
||
|
||
#define _DO_CAMERA_CALIB 1
|
||
|
||
//version 1.0.0 : base version released
|
||
std::string m_strVersion = "camCalib 1.0.0";
|
||
const char* wd_camCalibVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
void sg_outputCalibK(const char* fileName, cv::Mat& fitMap)
|
||
{
|
||
std::ofstream sw(fileName);
|
||
|
||
for (int i = 0; i < fitMap.rows; i++)
|
||
{
|
||
// 存储拟合结果
|
||
double K[FittingOrder + 1];
|
||
for (int _c = 0; _c <= FittingOrder; _c++)
|
||
K[_c] = fitMap.ptr<float>(i)[_c];
|
||
char dataStr[250];
|
||
//sprintf_s(dataStr, 250, "%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf", K[0], K[1], K[2], K[3], K[4], K[5], K[6], K[7]);
|
||
sprintf_s(dataStr, 250, "%g, %g, %g, %g, %g, %g, %g, %g", K[0], K[1], K[2], K[3], K[4], K[5], K[6], K[7]);
|
||
sw << dataStr << std::endl;
|
||
}
|
||
sw.close();
|
||
return;
|
||
}
|
||
|
||
void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D, cv::Vec4f& pe)
|
||
{
|
||
std::ofstream sw(fileName);
|
||
|
||
char dataStr[250];
|
||
for (int i = 0; i < K.rows; i++)
|
||
{
|
||
// 存储旋转矩阵
|
||
double temp[3];
|
||
for (int _c = 0; _c < 3; _c++)
|
||
temp[_c] = K.ptr<double>(i)[_c];
|
||
|
||
//sprintf_s(dataStr, 250, "%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf", K[0], K[1], K[2], K[3], K[4], K[5], K[6], K[7]);
|
||
sprintf_s(dataStr, 250, "%g, %g, %g", temp[0], temp[1], temp[2]);
|
||
sw << dataStr << std::endl;
|
||
}
|
||
double temp[5];
|
||
for (int _c = 0; _c < 5; _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]);
|
||
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;
|
||
}
|
||
|
||
void sg_readCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D)
|
||
{
|
||
K = cv::Mat::zeros(3, 3, CV_64FC1);
|
||
D = cv::Mat::zeros(1, 5, CV_64FC1);
|
||
std::ifstream inputFile(fileName);
|
||
std::string linedata;
|
||
|
||
if (inputFile.is_open() == false)
|
||
return;
|
||
|
||
int line = 0;
|
||
while (getline(inputFile, linedata))
|
||
{
|
||
if (line < 3)
|
||
{
|
||
double data[3];
|
||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &data[0], &data[1], &data[2]);
|
||
for (int _c = 0; _c < 3; _c++)
|
||
K.ptr<double>(line)[_c] = data[_c];
|
||
}
|
||
else if(line == 3)
|
||
{
|
||
double data[5];
|
||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf, %lf, %lf", &data[0], &data[1], &data[2], &data[3], &data[4]);
|
||
for (int _c = 0; _c < 5; _c++)
|
||
D.ptr<double>(0)[_c] = data[_c];
|
||
}
|
||
line++;
|
||
}
|
||
inputFile.close();
|
||
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
|
||
{
|
||
int nMin; //< 最小值
|
||
int nMax; //< 最大值
|
||
} SWdNLRange;
|
||
|
||
#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) {
|
||
|
||
std::vector<cv::Point2f> srcPts;
|
||
for (int r = 0; r < size.height; r++) {
|
||
for (int c = 0; c < size.width; c++) {
|
||
srcPts.push_back(cv::Point2f(c, r));
|
||
}
|
||
}
|
||
|
||
std::vector<cv::Point2f> dstPts;
|
||
cv::undistortPoints(srcPts, dstPts, K, D, R, newK);
|
||
|
||
mapX = cv::Mat::zeros(size.height, size.width, CV_32FC1);
|
||
mapY = cv::Mat::zeros(size.height, size.width, CV_32FC1);
|
||
int idx = 0;
|
||
for (int r = 0; r < size.height; r++) {
|
||
for (int c = 0; c < size.width; c++) {
|
||
mapX.ptr<float>(r)[c] = dstPts[idx].x;
|
||
mapY.ptr<float>(r)[c] = dstPts[idx].y;
|
||
idx++;
|
||
}
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
void lineFitting(std::vector< cv::Point2f>& inliers, double* _k, double* _b)
|
||
{
|
||
//最小二乘拟合直线参数
|
||
double xx_sum = 0;
|
||
double x_sum = 0;
|
||
double y_sum = 0;
|
||
double xy_sum = 0;
|
||
int num = 0;
|
||
for (int i = 0; i < inliers.size(); i++)
|
||
{
|
||
x_sum += inliers[i].x; //x的累加和
|
||
y_sum += inliers[i].y; //y的累加和
|
||
xx_sum += inliers[i].x * inliers[i].x; //x的平方累加和
|
||
xy_sum += inliers[i].x * inliers[i].y; //x，y的累加和
|
||
num++;
|
||
}
|
||
*_k = (num * xy_sum - x_sum * y_sum) / (num * xx_sum - x_sum * x_sum); //根据公式求解k
|
||
*_b = (-x_sum * xy_sum + xx_sum * y_sum) / (num * xx_sum - x_sum * x_sum);//根据公式求解b
|
||
}
|
||
//拟合成通用直线方程ax+by+c=0，包括垂直
|
||
void lineFitting_abc(std::vector< cv::Point2f>& inliers, double* _a, double* _b, double* _c)
|
||
{
|
||
//判断是否为垂直
|
||
int dataSize = (int)inliers.size();
|
||
if (dataSize < 2)
|
||
return;
|
||
|
||
double deltaX = abs(inliers[0].x - inliers[dataSize - 1].x);
|
||
double deltaY = abs(inliers[0].y - inliers[dataSize - 1].y);
|
||
std::vector< cv::Point2f> fittingData;
|
||
if (deltaX < deltaY)
|
||
{
|
||
//x=ky+b 拟合
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
cv::Point2f a_fitPt;
|
||
a_fitPt.x = inliers[i].y;
|
||
a_fitPt.y = inliers[i].x;
|
||
fittingData.push_back(a_fitPt);
|
||
}
|
||
double k = 0, b = 0;
|
||
lineFitting(fittingData, &k, &b);
|
||
//ax+by+c
|
||
*_a = 1.0;
|
||
*_b = -k;
|
||
*_c = -b;
|
||
}
|
||
else
|
||
{
|
||
//y = kx+b拟合
|
||
double k = 0, b = 0;
|
||
lineFitting(inliers, &k, &b);
|
||
//ax+by+c
|
||
*_a = k;
|
||
*_b = -1;
|
||
*_c = b;
|
||
}
|
||
return;
|
||
}
|
||
|
||
|
||
#define MONO_CALIB_TEST_GROUP 5
|
||
int MonoCamLaserCalib()
|
||
{
|
||
std::cout << "Hello World!\n";
|
||
|
||
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[MONO_CALIB_TEST_GROUP] = {
|
||
{3,39},{6,33},{1,33},{1,10},{1,26}
|
||
};
|
||
const int boardType[MONO_CALIB_TEST_GROUP] =
|
||
{
|
||
CALIB_CHESS_BOARD,
|
||
CALIB_CHESS_BOARD,
|
||
CALIB_CIRCLE_GRID,
|
||
CALIB_CHARUCO,
|
||
CALIB_CHARUCO,
|
||
};
|
||
|
||
for(int grp = 4; grp <= 4; grp ++)
|
||
{
|
||
//grp = 1;
|
||
int calibType = boardType[grp];
|
||
cv::Size cbPattern;
|
||
float cbSquareSize;
|
||
float markSize;
|
||
if (CALIB_CHESS_BOARD == calibType)
|
||
{
|
||
cbPattern = cv::Size(8, 11); // 10);
|
||
cbSquareSize = 40.0f; // 10.f;
|
||
}
|
||
else if (CALIB_CIRCLE_GRID == calibType)
|
||
{
|
||
cbPattern = cv::Size(7, 7); // 10);
|
||
cbSquareSize = 50.0f; // 10.f;
|
||
}
|
||
else if (CALIB_CHARUCO == calibType)
|
||
{
|
||
cbPattern = cv::Size(9, 9); //cv::Size(47, 21); // 10);
|
||
cbSquareSize = 50.0f;
|
||
markSize = 37.0f;
|
||
}
|
||
else
|
||
continue;
|
||
|
||
std::vector<std::vector<cv::Point2f>> cbCornersList;
|
||
std::vector<std::vector<int>> cbCornersIdList;
|
||
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 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, "%sresult/calib_%03d_rotate.bmp", calibDataPath[grp], index);
|
||
cv::String rFileName(rotateFilename);
|
||
cv::imwrite(rFileName, img);
|
||
#endif
|
||
std::vector<cv::Point2f> corners;
|
||
if (CALIB_CHESS_BOARD == calibType)
|
||
{
|
||
detectCorners(img, cbPattern, corners);
|
||
#if ENABLE_DEBUG
|
||
if (corners.size() > 0)
|
||
{
|
||
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_corner.bmp", calibDataPath[grp], index);
|
||
cv::imwrite(filename, color);
|
||
}
|
||
#endif
|
||
if (corners.empty())
|
||
continue;
|
||
cbCornersList.push_back(corners);
|
||
}
|
||
else if (CALIB_CIRCLE_GRID == calibType)
|
||
{
|
||
detectCirclePoints(img, cbPattern, corners);
|
||
#if ENABLE_DEBUG
|
||
if (corners.size() > 0)
|
||
{
|
||
cv::Mat color = img.clone();
|
||
cv::drawChessboardCorners(color, cbPattern, corners, true);
|
||
//cv::resize(color, color, cv::Size(), 0.5, 0.5);
|
||
sprintf_s(filename, "%sresult/calib_%03d_center.bmp", calibDataPath[grp], index);
|
||
cv::imwrite(filename, color);
|
||
}
|
||
#endif
|
||
if (corners.empty())
|
||
continue;
|
||
cbCornersList.push_back(corners);
|
||
}
|
||
else if (CALIB_CHARUCO == calibType)
|
||
{
|
||
std::vector<int> markerIds;
|
||
std::vector<std::vector<cv::Point2f> > markerCorners;
|
||
std::vector<int> charucoIds;
|
||
detectCharucoCorners(img,
|
||
cbPattern,
|
||
cbSquareSize,
|
||
markSize,
|
||
markerIds,
|
||
markerCorners,
|
||
charucoIds,
|
||
corners);
|
||
cv::Mat imageCopy = img.clone();
|
||
if (markerIds.size() > 0)
|
||
{
|
||
cv::aruco::drawDetectedMarkers(imageCopy, markerCorners, markerIds);
|
||
}
|
||
if (charucoIds.size() > 0)
|
||
{
|
||
cv::aruco::drawDetectedCornersCharuco(imageCopy, corners, charucoIds, cv::Scalar(0, 255, 255));
|
||
}
|
||
char markFilename[256];
|
||
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);
|
||
}
|
||
|
||
cv::Mat K, D;
|
||
std::vector<double> 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, "%sresult/calib_param_x.txt", calibDataPath[grp]);
|
||
sg_outputCalibK(calibParamName, fitMap_x);
|
||
sprintf_s(calibParamName, "%sresult/calib_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;
|
||
|
||
#if 0
|
||
//生成矫正图像
|
||
for (index = startIndex; index <= endIndex; index++) {
|
||
char filename[256];
|
||
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 calibImg;
|
||
cv::remap(img,
|
||
calibImg,
|
||
backwardMap_x,
|
||
backwardMap_y,
|
||
cv::INTER_LINEAR,
|
||
cv::BORDER_CONSTANT,
|
||
cv::Scalar(0, 0, 0));
|
||
sprintf_s(filename, "%sresult/calib_%03d_calib.bmp", calibDataPath[grp], index);
|
||
cv::imwrite(filename, calibImg);
|
||
}
|
||
#endif
|
||
|
||
#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<cv::Point2f> 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<cv::Point3f> all_pts3d;
|
||
for (index = startIndex; index <= endIndex; index++) {
|
||
|
||
char filename[256];
|
||
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 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, "%sresult/calib_%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);
|
||
}
|
||
else if (CALIB_CIRCLE_GRID == calibType)
|
||
{
|
||
detectCirclePoints(img, cbPattern, corners);
|
||
}
|
||
else if (CALIB_CHARUCO == calibType)
|
||
{
|
||
std::vector<int> markerIds;
|
||
std::vector<std::vector<cv::Point2f> > markerCorners;
|
||
detectCharucoCorners(img,
|
||
cbPattern,
|
||
cbSquareSize,
|
||
markSize,
|
||
markerIds,
|
||
markerCorners,
|
||
charucoIds,
|
||
corners);
|
||
}
|
||
if (corners.empty())
|
||
continue;
|
||
|
||
// 创建棋盘格区域的掩码
|
||
cv::Mat chessMask;
|
||
if (CALIB_CHARUCO == calibType)
|
||
{
|
||
chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
|
||
float roi_L = corners[0].x;
|
||
float roi_R = roi_L;
|
||
float roi_T = corners[0].y;
|
||
float roi_B = roi_T;
|
||
for (int i = 1; i < (int)corners.size(); i++)
|
||
{
|
||
roi_L = roi_L > corners[i].x ? corners[i].x : roi_L;
|
||
roi_R = roi_R < corners[i].x ? corners[i].x : roi_R;
|
||
roi_T = roi_T > corners[i].y ? corners[i].y : roi_T;
|
||
roi_B = roi_B < corners[i].y ? corners[i].y : roi_B;
|
||
}
|
||
int left = (int)roi_L;
|
||
if (left < 0)
|
||
left = 0;
|
||
int right = (int)roi_R;
|
||
if (right >= chessMask.cols)
|
||
right = chessMask.cols - 1;
|
||
int top = (int)roi_T - 100;
|
||
if (top < 0)
|
||
top = 0;
|
||
int btm = (int)roi_B + 100;
|
||
if (btm >= chessMask.rows)
|
||
btm = chessMask.rows - 1;
|
||
|
||
for (int y = top; y <= btm; y++)
|
||
{
|
||
for (int x = left; x <= right; x++)
|
||
{
|
||
chessMask.at<uchar>(y, x) = 0xff;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
|
||
// 使用多边形近似来填充角点之间的区域
|
||
// 棋盘格区域需要比角点区域大一圈
|
||
std::vector<cv::Point2f> 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<cv::Point> 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, "%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, "%sresult/laser_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));
|
||
sprintf_s(filename, "%sresult/laser_%d_calib.bmp", calibDataPath[grp], index);
|
||
cv::imwrite(filename, laserCalibImg);
|
||
|
||
cv::Mat enlargeLaserImg = laserCalibImg.clone();
|
||
cv::Size laserImgSize = enlargeLaserImg.size();
|
||
laserImgSize.width = laserImgSize.width * 5;
|
||
cv::resize(enlargeLaserImg, enlargeLaserImg, laserImgSize, 0, 0, cv::INTER_NEAREST);
|
||
sprintf_s(filename, "%sresult/laser_%d_enlarge_calib.bmp", calibDataPath[grp], index);
|
||
cv::imwrite(filename, enlargeLaserImg);
|
||
#endif
|
||
bool multiPeakFlag = false; //每行一个Peak
|
||
std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg, multiPeakFlag);
|
||
#if 0
|
||
//显示亚像素点
|
||
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);
|
||
#endif
|
||
|
||
// 去畸变
|
||
if (pts2d.size() == 0)
|
||
continue;
|
||
std::vector<cv::Point2f> undistortedPoints;
|
||
cv::undistortPoints(pts2d, undistortedPoints,
|
||
K, D, cv::noArray(), newCamMatrix);
|
||
|
||
#if 1
|
||
//直线拟合
|
||
double _a, _b, _c;
|
||
lineFitting_abc(undistortedPoints, &_a, &_b, &_c);
|
||
|
||
std::vector<cv::Point2f> fittingPts2d;
|
||
for (int i = 0; i < (int)undistortedPoints.size(); i++)
|
||
{
|
||
cv::Point2f a_pt;
|
||
a_pt.y = undistortedPoints[i].y;
|
||
a_pt.x = -(a_pt.y * _b + _c) / _a;
|
||
fittingPts2d.push_back(a_pt);
|
||
}
|
||
#else
|
||
std::vector<cv::Point2f> fittingPts2d;
|
||
fittingPts2d.insert(fittingPts2d.end(), undistortedPoints.begin(), undistortedPoints.end());
|
||
#endif
|
||
|
||
if (fittingPts2d.size() > 0)
|
||
{
|
||
sprintf_s(filename, "%sresult/laser_enlarge_%d_subpixData.txt", calibDataPath[grp], index);
|
||
saveSubpixData(filename, fittingPts2d);
|
||
}
|
||
|
||
cv::Mat cloneEnlageImg = enlargeLaserImg.clone();
|
||
for (int i = 0, i_max = (int)undistortedPoints.size(); i < i_max; i++)
|
||
{
|
||
cv::Point2f a_subPix = undistortedPoints[i];
|
||
int row = (int)(a_subPix.y + 0.5);
|
||
int col = (int)(a_subPix.x * 5 + 0.5);
|
||
cloneEnlageImg.at<cv::Vec3b>(row, col)[0] = 0;
|
||
cloneEnlageImg.at<cv::Vec3b>(row, col)[1] = 0;
|
||
cloneEnlageImg.at<cv::Vec3b>(row, col)[2] = 255;
|
||
}
|
||
sprintf_s(filename, "%sresult/laser_enlarge_%d_raw_subpix.png", calibDataPath[grp], index);
|
||
cv::imwrite(filename, cloneEnlageImg);
|
||
|
||
for (int i = 0, i_max = (int)fittingPts2d.size(); i < i_max; i++)
|
||
{
|
||
cv::Point2f a_subPix = fittingPts2d[i];
|
||
int row = (int)(a_subPix.y + 0.5);
|
||
int col = (int)(a_subPix.x * 5 + 0.5);
|
||
enlargeLaserImg.at<cv::Vec3b>(row, col)[0] = 0;
|
||
enlargeLaserImg.at<cv::Vec3b>(row, col)[1] = 0;
|
||
enlargeLaserImg.at<cv::Vec3b>(row, col)[2] = 255;
|
||
}
|
||
sprintf_s(filename, "%sresult/laser_enlarge_%d_subpix.png", calibDataPath[grp], index);
|
||
cv::imwrite(filename, enlargeLaserImg);
|
||
|
||
std::vector<cv::Point3f> pts3d = project2DTo3D(fittingPts2d, pe, K, D);
|
||
all_pts3d.insert(all_pts3d.end(), pts3d.begin(), pts3d.end());
|
||
printf(" %sImage_%d_Laser.png ... done\n", calibDataPath[grp], index);
|
||
}
|
||
|
||
cv::Vec4f laser_pe = fitPlaneToPoints(all_pts3d);
|
||
std::cout << "pe: " << laser_pe << std::endl;
|
||
|
||
//output K and D
|
||
char calibKDPName[256];
|
||
sprintf_s(calibKDPName, "%sresult/calib_param_K_D.txt", calibDataPath[grp]);
|
||
sg_outputCalibKD(calibKDPName, K, D, laser_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<std::vector<cv::Point2f>> cbCornersListL;
|
||
std::vector<std::vector<int>> cbCornersIdListL;
|
||
std::vector<std::vector<cv::Point2f>> cbCornersListR;
|
||
std::vector<std::vector<int>> 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<cv::Point2f> cornersL;
|
||
std::vector<cv::Point2f> 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<int> markerIds_L;
|
||
std::vector<int> markerIds_R;
|
||
std::vector<std::vector<cv::Point2f> > markerCorners_L;
|
||
std::vector<std::vector<cv::Point2f> > markerCorners_R;
|
||
std::vector<int> charucoIds_L;
|
||
std::vector<int> 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<double> 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<cv::Point2f> 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<cv::Point3f> 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<cv::Point2f> corners;
|
||
std::vector<int> 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<int> markerIds;
|
||
std::vector<std::vector<cv::Point2f> > 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<cv::Point2f> 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<cv::Point> 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())
|
||
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);
|
||
|
||
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<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 = (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<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点
|
||
|
||
#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;
|
||
}
|
||
#endif
|
||
|
||
int main(void)
|
||
{
|
||
MonoCamLaserCalib();
|
||
//StereoCamCalib();
|
||
}
|