camCalib
version 1.0.0 : base version released
This commit is contained in:
parent
6244873e6d
commit
1127ec7b25
14
CamAlgo.sln
14
CamAlgo.sln
@ -1,7 +1,7 @@
|
||||
|
||||
Microsoft Visual Studio Solution File, Format Version 12.00
|
||||
# Visual Studio Version 16
|
||||
VisualStudioVersion = 16.0.33027.164
|
||||
# Visual Studio Version 17
|
||||
VisualStudioVersion = 17.14.37411.7 d17.14
|
||||
MinimumVisualStudioVersion = 10.0.40219.1
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "PeakSearch", "CamAlgo\CamAlgo.vcxproj", "{2B6D42C0-7146-47D2-B6AA-9BC125C4CF93}"
|
||||
EndProject
|
||||
@ -18,6 +18,8 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "camAlgoSW_test", "camAlgoSW
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "camCalib", "camCalib\camCalib.vcxproj", "{3C3A4670-25E6-4E17-9723-2897316D2437}"
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "simDataGenerator", "simDataGenerator\simDataGenerator.vcxproj", "{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}"
|
||||
EndProject
|
||||
Global
|
||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||
Debug|x64 = Debug|x64
|
||||
@ -74,6 +76,14 @@ Global
|
||||
{3C3A4670-25E6-4E17-9723-2897316D2437}.Release|x64.Build.0 = Release|x64
|
||||
{3C3A4670-25E6-4E17-9723-2897316D2437}.Release|x86.ActiveCfg = Release|Win32
|
||||
{3C3A4670-25E6-4E17-9723-2897316D2437}.Release|x86.Build.0 = Release|Win32
|
||||
{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Debug|x64.Build.0 = Debug|x64
|
||||
{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Debug|x86.Build.0 = Debug|Win32
|
||||
{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Release|x64.ActiveCfg = Release|x64
|
||||
{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Release|x64.Build.0 = Release|x64
|
||||
{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Release|x86.ActiveCfg = Release|Win32
|
||||
{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Release|x86.Build.0 = Release|Win32
|
||||
EndGlobalSection
|
||||
GlobalSection(SolutionProperties) = preSolution
|
||||
HideSolutionNode = FALSE
|
||||
|
||||
@ -10,6 +10,13 @@
|
||||
|
||||
#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);
|
||||
@ -157,6 +164,67 @@ void initForwardRectMap(const cv::Mat& K, const cv::Mat& D, const cv::Mat& R,
|
||||
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()
|
||||
{
|
||||
@ -493,7 +561,40 @@ int MonoCamLaserCalib()
|
||||
// 创建棋盘格区域的掩码
|
||||
cv::Mat chessMask;
|
||||
if (CALIB_CHARUCO == calibType)
|
||||
chessMask = cv::Mat::ones(img.size(), CV_8UC1);
|
||||
{
|
||||
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);
|
||||
@ -576,7 +677,7 @@ int MonoCamLaserCalib()
|
||||
cv::Mat laserImg;
|
||||
cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask);
|
||||
#if 1
|
||||
sprintf_s(filename, "%slaser_mask_%d.png", calibDataPath[grp], index);
|
||||
sprintf_s(filename, "%sresult/laser_mask_%d.png", calibDataPath[grp], index);
|
||||
cv::imwrite(filename, laserImg);
|
||||
|
||||
cv::Mat laserCalibImg;
|
||||
@ -587,13 +688,19 @@ int MonoCamLaserCalib()
|
||||
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);
|
||||
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
|
||||
std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg);
|
||||
bool multiPeakFlag = false; //每行一个Peak
|
||||
std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg, multiPeakFlag);
|
||||
#if 0
|
||||
//显示亚像素点
|
||||
cv::Mat enlargeImg;
|
||||
if (laserImg.channels() == 1)
|
||||
@ -603,38 +710,76 @@ int MonoCamLaserCalib()
|
||||
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)
|
||||
// 去畸变
|
||||
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++)
|
||||
{
|
||||
sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpixData.txt", calibDataPath[grp], index);
|
||||
saveSubpixData(filename, pts2d);
|
||||
cv::Point2f a_pt;
|
||||
a_pt.y = undistortedPoints[i].y;
|
||||
a_pt.x = -(a_pt.y * _b + _c) / _a;
|
||||
fittingPts2d.push_back(a_pt);
|
||||
}
|
||||
for (int i = 0, i_max = (int)pts2d.size(); i < i_max; i++)
|
||||
#else
|
||||
std::vector<cv::Point2f> fittingPts2d;
|
||||
fittingPts2d.insert(fittingPts2d.end(), undistortedPoints.begin(), undistortedPoints.end());
|
||||
#endif
|
||||
|
||||
if (fittingPts2d.size() > 0)
|
||||
{
|
||||
cv::Point2f a_subPix = pts2d[i];
|
||||
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);
|
||||
enlargeImg.at<cv::Vec3b>(row, col)[0] = 0;
|
||||
enlargeImg.at<cv::Vec3b>(row, col)[1] = 0;
|
||||
enlargeImg.at<cv::Vec3b>(row, col)[2] = 255;
|
||||
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, "%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点
|
||||
sprintf_s(filename, "%sresult/laser_enlarge_%d_raw_subpix.png", calibDataPath[grp], index);
|
||||
cv::imwrite(filename, cloneEnlageImg);
|
||||
|
||||
#endif
|
||||
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 pe = fitPlaneToPoints(all_pts3d);
|
||||
std::cout << "pe: " << pe << std::endl;
|
||||
cv::Vec4f laser_pe = fitPlaneToPoints(all_pts3d);
|
||||
std::cout << "pe: " << laser_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);
|
||||
sprintf_s(calibKDPName, "%sresult/calib_param_K_D.txt", calibDataPath[grp]);
|
||||
sg_outputCalibKD(calibKDPName, K, D, laser_pe);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -1,4 +1,5 @@
|
||||
1004.94, 0, 619.327
|
||||
0, 1004.94, 1023.07
|
||||
1006.74, 0, 620.098
|
||||
0, 1006.74, 1023.51
|
||||
0, 0, 1
|
||||
-0.0869453, 0.0919085, -1.61443e-05, -2.84163e-05, -0.0224749
|
||||
-0.0817425, 0.0844376, -2.2634e-05, 0.000762232, -0.0165931
|
||||
-2.67733, 0.0501805, 631.944
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -655,7 +655,7 @@ void getLinePeaks(double* lineData, int dataSize, int row, int scaleWin, double
|
||||
}
|
||||
|
||||
std::vector<cv::Point2f> detectLaserLine(
|
||||
const cv::Mat& inputImage)
|
||||
const cv::Mat& inputImage, bool multiPeaksPerLine)
|
||||
{
|
||||
std::vector<cv::Point2f> laserPoints;
|
||||
if (inputImage.empty())
|
||||
@ -673,8 +673,8 @@ std::vector<cv::Point2f> detectLaserLine(
|
||||
//高斯滤波
|
||||
cv::Mat img;
|
||||
gray.convertTo(img, CV_64FC1);
|
||||
cv::GaussianBlur(img, img, cv::Size(3, 3), 0.9, 0.9);
|
||||
//cv::imwrite("gauss_blur_src.bmp", img);
|
||||
cv::GaussianBlur(img, img, cv::Size(0, 0), 2.0, 2.0);
|
||||
cv::imwrite("gauss_blur_src.bmp", img);
|
||||
// 提取最大值点
|
||||
int scaleWin = 5;
|
||||
double minPkValue = 20;
|
||||
@ -684,6 +684,23 @@ std::vector<cv::Point2f> detectLaserLine(
|
||||
std::vector< cv::Point> linePeaks;
|
||||
double* lineData = img.ptr<double>(i);
|
||||
getLinePeaks(lineData, img.cols, i, scaleWin, minPkValue, linePeaks);
|
||||
if ((false == multiPeaksPerLine) && (linePeaks.size() > 1))
|
||||
{
|
||||
//每行保留一个Peak,
|
||||
cv::Point bestOne = linePeaks[0];
|
||||
double bestValue = lineData[bestOne.x];
|
||||
for (int i = 1; i < (int)linePeaks.size(); i++)
|
||||
{
|
||||
double value = lineData[linePeaks[i].x];
|
||||
if (bestValue < value)
|
||||
{
|
||||
bestValue = value;
|
||||
bestOne = linePeaks[i];
|
||||
}
|
||||
}
|
||||
linePeaks.clear();
|
||||
linePeaks.push_back(bestOne);
|
||||
}
|
||||
pkPoints.insert(pkPoints.end(), linePeaks.begin(), linePeaks.end());
|
||||
}
|
||||
//取亚像素值
|
||||
@ -701,27 +718,22 @@ std::vector<cv::Point2f> detectLaserLine(
|
||||
#endif
|
||||
/*Breif: 根据棋盘格平面和2d坐标计算3d值*/
|
||||
std::vector<cv::Point3f> project2DTo3D(
|
||||
const std::vector<cv::Point2f>& imagePoints,
|
||||
const std::vector<cv::Point2f>& undistortedPoints,
|
||||
const cv::Vec4f& planeEquation, // [a,b,c,d] for ax+by+cz+d=0
|
||||
const cv::Mat& cameraMatrix, // 相机内参K
|
||||
const cv::Mat& distCoeffs) // 畸变系数D
|
||||
{
|
||||
std::vector<cv::Point3f> objectPoints;
|
||||
if (imagePoints.empty())
|
||||
if (undistortedPoints.empty())
|
||||
return objectPoints;
|
||||
|
||||
// 1. 去畸变
|
||||
std::vector<cv::Point2f> undistortedPoints;
|
||||
cv::undistortPoints(imagePoints, undistortedPoints,
|
||||
cameraMatrix, distCoeffs, cv::noArray(), cameraMatrix);
|
||||
|
||||
// 2. 提取平面参数
|
||||
// 1. 提取平面参数
|
||||
const float a = planeEquation[0], b = planeEquation[1];
|
||||
const float c = planeEquation[2], d = planeEquation[3];
|
||||
const float denom = a * a + b * b + c * c;
|
||||
if (fabs(denom) < 1e-6f) return objectPoints;
|
||||
|
||||
// 3. 构造射线并求交平面
|
||||
// 2. 构造射线并求交平面
|
||||
const cv::Mat invK = cameraMatrix.inv();
|
||||
for (const auto& pt : undistortedPoints) {
|
||||
// 生成归一化射线方向
|
||||
|
||||
@ -69,7 +69,7 @@ void fitChessboardPlane_charuco(
|
||||
|
||||
/*Brief: 激光线检测函数*/
|
||||
std::vector<cv::Point2f> detectLaserLine(
|
||||
const cv::Mat& inputImage);
|
||||
const cv::Mat& inputImage, bool multiPeaksPerLine);
|
||||
|
||||
/*Breif: 根据棋盘格平面和2d坐标计算3d值*/
|
||||
std::vector<cv::Point3f> project2DTo3D(
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user