629 lines
23 KiB
C++
629 lines
23 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
|
||
|
||
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
|
||
|
||
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;
|
||
}
|
||
|
||
#define CALIB_TEST_GROUP 4
|
||
int main()
|
||
{
|
||
std::cout << "Hello World!\n";
|
||
|
||
const char* calibDataPath[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
|
||
};
|
||
const SWdNLRange fileIdx[CALIB_TEST_GROUP] = {
|
||
{3,39},{1,33},{1,33},{1,10}
|
||
};
|
||
const int boardType[CALIB_TEST_GROUP] =
|
||
{
|
||
CALIB_CHESS_BOARD,
|
||
CALIB_CHESS_BOARD,
|
||
CALIB_CIRCLE_GRID,
|
||
CALIB_CHARUCO
|
||
};
|
||
|
||
for(int grp = 0; grp < CALIB_TEST_GROUP; grp ++)
|
||
{
|
||
grp = 3;
|
||
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(47, 21); // 10);
|
||
cbSquareSize = 0.05f;
|
||
markSize = 0.037f;
|
||
}
|
||
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 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);
|
||
char rotateFilename[256];
|
||
sprintf_s(rotateFilename, "%scalib_%03d_rotate.bmp", calibDataPath[grp], index);
|
||
cv::String rFileName(rotateFilename);
|
||
cv::imwrite(rFileName, img);
|
||
|
||
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, "%scalib_%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, "%scalib_%03d_markers.bmp", calibDataPath[grp], index);
|
||
cv::imwrite(markFilename, imageCopy);
|
||
|
||
if (corners.empty())
|
||
continue;
|
||
cbCornersList.push_back(corners);
|
||
cbCornersIdList.push_back(charucoIds);
|
||
}
|
||
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);
|
||
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 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);
|
||
}
|
||
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;
|
||
}
|
||
|