496 lines
16 KiB
C++
496 lines
16 KiB
C++
// MonoLaserCalibrate.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
|
||
//
|
||
#include <iostream>
|
||
#include <opencv2/opencv.hpp>
|
||
#include <vector>
|
||
#include <math.h>
|
||
#include "MonoLaserCalibrate.h"
|
||
#include "../lineDetection_steger.h"
|
||
|
||
/*Breif：角点检测函数*/
|
||
void detectCorners(const cv::Mat& img,
|
||
const cv::Size& patternSize,
|
||
std::vector<cv::Point2f>& corners)
|
||
{
|
||
cv::Mat gray;
|
||
if (img.channels() == 3)
|
||
cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
|
||
else
|
||
gray = img.clone();
|
||
|
||
bool found = cv::findChessboardCorners(gray, patternSize, corners);
|
||
#if 1
|
||
if (found) {
|
||
// 亚像素精确化
|
||
cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1),
|
||
cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 50, 0.1));
|
||
}
|
||
#endif
|
||
return;
|
||
}
|
||
|
||
/*Breif：圆形网格函数*/
|
||
void detectCirclePoints(const cv::Mat& img,
|
||
const cv::Size& patternSize,
|
||
std::vector<cv::Point2f>& corners)
|
||
{
|
||
cv::Mat gray;
|
||
if (img.channels() == 3)
|
||
cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
|
||
else
|
||
gray = img.clone();
|
||
|
||
//需要将圆点外的背景去除，否则复杂的背景会导致检测失败
|
||
//使用连通域检测
|
||
cv::Mat grayBin;
|
||
|
||
|
||
// 检测圆形网格
|
||
bool found = cv::findCirclesGrid(gray, patternSize, corners, cv::CALIB_CB_SYMMETRIC_GRID);
|
||
return;
|
||
}
|
||
|
||
/*Breif：单目相机标定函数*/
|
||
void monocularCalibration(
|
||
const std::vector<std::vector<cv::Point2f>>& imagePoints,
|
||
const cv::Size& imageSize,
|
||
const cv::Size& patternSize,
|
||
const float squareSize,
|
||
cv::Mat& cameraMatrix,
|
||
cv::Mat& distCoeffs,
|
||
std::vector<double>& reprojectionError)
|
||
{
|
||
// 根据角点生成3d点
|
||
std::vector<std::vector<cv::Point3f>> objectPoints;
|
||
for (const auto& corners : imagePoints) {
|
||
|
||
// 准备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);
|
||
}
|
||
|
||
// 执行相机标定
|
||
std::vector<cv::Mat> rvecs, tvecs;
|
||
#if ENABLE_FISH_EYE
|
||
// 步骤十(2)：计算内参和畸变系数，设 flags 和 增加迭代终止条件 需要 图象数>=2张，这是鱼眼摄像头常规方式, 但是使用 undistortImage
|
||
int flags = 0;
|
||
flags |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
|
||
flags |= cv::fisheye::CALIB_CHECK_COND;
|
||
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);
|
||
#endif
|
||
// 重投影三维点到二维图像点
|
||
// 计算重投影误差
|
||
double totalError = 0.0;
|
||
int totalSize = 0;
|
||
for (int i = 0; i < objectPoints.size(); i++)
|
||
{
|
||
std::vector<cv::Point2f> reprojectedPoints;
|
||
#if ENABLE_FISH_EYE
|
||
{
|
||
cv::fisheye::projectPoints(objectPoints[i], reprojectedPoints, rvecs[i], tvecs[i], cameraMatrix,
|
||
distCoeffs);
|
||
}
|
||
#else
|
||
{
|
||
projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix, distCoeffs, reprojectedPoints);
|
||
}
|
||
#endif
|
||
|
||
//totalSize += objectPoints[i].size();
|
||
//double localError = 0;
|
||
double err = norm(imagePoints[i], reprojectedPoints, cv::NORM_L2);
|
||
size_t n = objectPoints[i].size();
|
||
double localError = (double)std::sqrt(err * err / n);
|
||
totalError += err * err;
|
||
totalSize += n;
|
||
#if 0
|
||
for (int j = 0; j < objectPoints[i].size(); j++)
|
||
{
|
||
totalError += cv::norm(imagePoints[i][j] - reprojectedPoints[j]);
|
||
localError += cv::norm(imagePoints[i][j] - reprojectedPoints[j]);
|
||
//totalError += sqrt(pow(imagePoints[i][j].x - reprojectedPoints[j].x,2) + pow(imagePoints[i][j].y - reprojectedPoints[j].y, 2)); // 使用L2范数计算误差
|
||
}
|
||
int localSize = (int)objectPoints[i].size();
|
||
localError = localError / (double)localSize;
|
||
#endif
|
||
reprojectionError.push_back(localError);
|
||
}
|
||
totalError = std::sqrt(totalError / totalSize);
|
||
//totalError /= totalSize; // 取平均误差
|
||
reprojectionError.push_back(totalError);
|
||
return;
|
||
}
|
||
|
||
/*Brief: 拟合棋盘格角点平面*/
|
||
void fitChessboardPlane(
|
||
const std::vector<cv::Point2f>& corners,
|
||
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);
|
||
|
||
// 3. 转换旋转向量为旋转矩阵
|
||
cv::Mat R;
|
||
cv::Rodrigues(rvec, R);
|
||
|
||
// 4. 计算平面法向量 (世界坐标系Z轴转换到相机坐标系)
|
||
cv::Mat normal_world = (cv::Mat_<double>(3, 1) << 0, 0, 1);
|
||
cv::Mat normal_camera = R * normal_world;
|
||
|
||
// 5. 获取平面上的一个点 (棋盘格原点在相机坐标系的位置)
|
||
cv::Mat origin_camera = tvec;
|
||
|
||
// 6. 构建平面方程: n·(X - P0) = 0
|
||
float a = normal_camera.at<double>(0);
|
||
float b = normal_camera.at<double>(1);
|
||
float c = normal_camera.at<double>(2);
|
||
float d = -(a * origin_camera.at<double>(0) +
|
||
b * origin_camera.at<double>(1) +
|
||
c * origin_camera.at<double>(2));
|
||
|
||
planeEquation = cv::Vec4f(a, b, c, d);
|
||
|
||
return;
|
||
}
|
||
|
||
/*Brief: 构造激光出射平面方程*/
|
||
void generateLaserLine(
|
||
const float baseLine,
|
||
const float angle,
|
||
cv::Vec4f& planeEquation)
|
||
{
|
||
// aX + bY + cZ + d = 0
|
||
const float a = cos(angle * CV_PI / 180.f);
|
||
const float b = 0.f;
|
||
const float c = sin(angle * CV_PI / 180.f);
|
||
const float d = baseLine * cos(angle * CV_PI / 180.f);
|
||
planeEquation = cv::Vec4f(a, b, c, d);
|
||
|
||
return;
|
||
}
|
||
|
||
/*Breif: 构造虚拟仿真图*/
|
||
cv::Mat generateVirtualLaserLineImage(
|
||
const cv::Vec4f& laserPlane, // 激光出射面方程 ax+by+cz+d=0
|
||
const cv::Vec4f& targetPlane, // 目标平面方程(棋盘格平面)
|
||
const cv::Mat& cameraMatrix, // 相机内参K
|
||
const cv::Mat& distCoeffs, // 畸变系数D
|
||
const cv::Size& imageSize) // 输出图像尺寸
|
||
{
|
||
|
||
// 生成直线上的3D点集（Z范围1mm~200mm）
|
||
std::vector<cv::Point3f> linePoints3D;
|
||
for (float z = 400; z <= 600.0; z += 0.05f) {
|
||
//根据激光出射面(b=0)确定x
|
||
const float x = -(laserPlane[2] * z + laserPlane[3]) / laserPlane[0];
|
||
//根据棋盘格平面确定y
|
||
const float y = -(targetPlane[0] * x + targetPlane[2] * z + targetPlane[3]) / targetPlane[1];
|
||
linePoints3D.push_back(cv::Point3f(x, y, z));
|
||
}
|
||
|
||
// 投影到图像平面
|
||
std::vector<cv::Point2f> imagePoints;
|
||
cv::projectPoints(linePoints3D,
|
||
cv::Vec3f::zeros(), cv::Vec3f::zeros(),
|
||
cameraMatrix, distCoeffs, imagePoints);
|
||
|
||
// 绘制激光线
|
||
cv::Mat laserImage = cv::Mat::zeros(imageSize, CV_8UC1);
|
||
for (size_t i = 1; i < imagePoints.size(); ++i) {
|
||
if (cv::norm(imagePoints[i] - imagePoints[i - 1]) < 50) { // 过滤异常投影
|
||
cv::line(laserImage, imagePoints[i - 1], imagePoints[i],
|
||
cv::Scalar(255), 2, cv::LINE_AA);
|
||
}
|
||
}
|
||
|
||
return laserImage;
|
||
}
|
||
|
||
/*Brief: 激光线检测函数*/
|
||
#if 0
|
||
std::vector<cv::Point2f> detectLaserLine(
|
||
const cv::Mat& inputImage)
|
||
{
|
||
std::vector<cv::Point2f> laserPoints;
|
||
if (inputImage.empty())
|
||
return laserPoints;
|
||
|
||
// 1. 颜色空间转换（处理彩色图像）
|
||
cv::Mat gray;
|
||
if (inputImage.channels() == 3) {
|
||
cv::cvtColor(inputImage, gray, cv::COLOR_BGR2GRAY);
|
||
}
|
||
else {
|
||
gray = inputImage.clone();
|
||
}
|
||
|
||
// 2. 动态阈值二值化（自动适应亮度变化）
|
||
cv::Mat binary;
|
||
double thresh = cv::threshold(gray, binary, 0, 255,
|
||
cv::THRESH_BINARY | cv::THRESH_OTSU);
|
||
|
||
// 3. 形态学降噪（可选）
|
||
cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
|
||
cv::morphologyEx(binary, binary, cv::MORPH_OPEN, kernel);
|
||
|
||
// 4. 骨架提取（细化激光线）
|
||
cv::Mat skel(binary.size(), CV_8UC1, cv::Scalar(0));
|
||
cv::Mat temp;
|
||
cv::Mat eroded;
|
||
cv::Mat element = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(3, 3));
|
||
do {
|
||
cv::erode(binary, eroded, element);
|
||
cv::dilate(eroded, temp, element);
|
||
cv::subtract(binary, temp, temp);
|
||
cv::bitwise_or(skel, temp, skel);
|
||
eroded.copyTo(binary);
|
||
} while (cv::countNonZero(binary) != 0);
|
||
|
||
// 6. 提取非零点坐标
|
||
cv::findNonZero(skel, laserPoints);
|
||
return laserPoints;
|
||
}
|
||
#else
|
||
void getLinePeaks(double* lineData, int dataSize, int row, int scaleWin, double minPkValue, std::vector<cv::Point>& linePeaks)
|
||
{
|
||
int _state = 0;
|
||
int pre_i = -1;
|
||
int sEdgePtIdx = -1;
|
||
int eEdgePtIdx = -1;
|
||
double* pre_data = NULL;
|
||
std::vector<int> pkTops;
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
double* curr_data = &lineData[i];
|
||
if (NULL == pre_data)
|
||
{
|
||
sEdgePtIdx = i;
|
||
eEdgePtIdx = i;
|
||
pre_data = curr_data;
|
||
pre_i = i;
|
||
continue;
|
||
}
|
||
|
||
eEdgePtIdx = i;
|
||
double px_diff = *curr_data - *pre_data;
|
||
switch (_state)
|
||
{
|
||
case 0: //初态
|
||
if (px_diff < 0) //下降
|
||
_state = 2;
|
||
else if (px_diff > 0) //上升
|
||
_state = 1;
|
||
break;
|
||
case 1: //上升
|
||
if (px_diff < 0) //下降
|
||
{
|
||
pkTops.push_back(pre_i);
|
||
_state = 2;
|
||
}
|
||
break;
|
||
case 2: //下降
|
||
if (px_diff > 0) // 上升
|
||
_state = 1;
|
||
break;
|
||
default:
|
||
_state = 0;
|
||
break;
|
||
}
|
||
pre_data = curr_data;
|
||
pre_i = i;
|
||
}
|
||
//最后一个
|
||
if(1 == _state)
|
||
pkTops.push_back(pre_i);
|
||
|
||
//在窗口内搜索
|
||
for (int i = 0, i_max = (int)pkTops.size(); i < i_max; i++)
|
||
{
|
||
double pkValue = lineData[pkTops[i]];
|
||
if (pkValue < minPkValue)
|
||
continue;
|
||
|
||
bool isPeak = true;
|
||
//向前搜索
|
||
for (int j = i - 1; j >= 0; j--)
|
||
{
|
||
int dist = pkTops[i] - pkTops[j];
|
||
if (dist < 0) dist = -dist;
|
||
if (dist > scaleWin) //超出尺度窗口
|
||
break;
|
||
if (lineData[pkTops[i]] < lineData[pkTops[j]])
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
//向后搜索
|
||
if (true == isPeak)
|
||
{
|
||
for (int j = i + 1; j < i_max; j++)
|
||
{
|
||
int dist = pkTops[i] - pkTops[j];
|
||
if (dist < 0) dist = -dist;
|
||
if (dist > scaleWin) //超出尺度窗口
|
||
break;
|
||
if (lineData[pkTops[i]] < lineData[pkTops[j]])
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
if (true == isPeak)
|
||
linePeaks.push_back(cv::Point(pkTops[i], row));
|
||
}
|
||
return;
|
||
}
|
||
|
||
std::vector<cv::Point2f> detectLaserLine(
|
||
const cv::Mat& inputImage)
|
||
{
|
||
std::vector<cv::Point2f> laserPoints;
|
||
if (inputImage.empty())
|
||
return laserPoints;
|
||
|
||
// 1. 颜色空间转换（处理彩色图像）
|
||
cv::Mat gray;
|
||
if (inputImage.channels() == 3) {
|
||
cv::cvtColor(inputImage, gray, cv::COLOR_BGR2GRAY);
|
||
}
|
||
else {
|
||
gray = inputImage.clone();
|
||
}
|
||
|
||
//高斯滤波
|
||
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);
|
||
// 提取最大值点
|
||
int scaleWin = 5;
|
||
double minPkValue = 100;
|
||
std::vector< cv::Point> pkPoints;
|
||
for (int i = 0; i < img.rows; i++)
|
||
{
|
||
std::vector< cv::Point> linePeaks;
|
||
double* lineData = img.ptr<double>(i);
|
||
getLinePeaks(lineData, img.cols, i, scaleWin, minPkValue, linePeaks);
|
||
pkPoints.insert(pkPoints.end(), linePeaks.begin(), linePeaks.end());
|
||
}
|
||
//取亚像素值
|
||
std::vector<cv::Point2f> posSubpix;
|
||
int gaussWin = 3; //5x5
|
||
computePointSubpix(
|
||
gray, //uchar型单通道输入图像（灰度图像）
|
||
pkPoints, //峰值像素的整数坐标。
|
||
gaussWin, //使用steger算法时指定的窗口宽度。此宽度需要与激光线的宽度基本吻合
|
||
posSubpix //计算出的亚像素坐标
|
||
);
|
||
return posSubpix;
|
||
}
|
||
|
||
#endif
|
||
/*Breif: 根据棋盘格平面和2d坐标计算3d值*/
|
||
std::vector<cv::Point3f> project2DTo3D(
|
||
const std::vector<cv::Point2f>& imagePoints,
|
||
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())
|
||
return objectPoints;
|
||
|
||
// 1. 去畸变
|
||
std::vector<cv::Point2f> undistortedPoints;
|
||
cv::undistortPoints(imagePoints, undistortedPoints,
|
||
cameraMatrix, distCoeffs, cv::noArray(), cameraMatrix);
|
||
|
||
// 2. 提取平面参数
|
||
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. 构造射线并求交平面
|
||
const cv::Mat invK = cameraMatrix.inv();
|
||
for (const auto& pt : undistortedPoints) {
|
||
// 生成归一化射线方向
|
||
cv::Mat ray = (cv::Mat_<double>(3, 1) << pt.x, pt.y, 1.0f);
|
||
ray = invK * ray;
|
||
const float rx = ray.at<double>(0), ry = ray.at<double>(1), rz = ray.at<double>(2);
|
||
|
||
// 计算射线与平面交点
|
||
const float t = -(a * rx + b * ry + c * rz + d) / (a * ray.at<double>(0) + b * ray.at<double>(1) + c * ray.at<double>(2));
|
||
objectPoints.emplace_back(
|
||
rx * t, // X = t * ray_dir_x
|
||
ry * t, // Y = t * ray_dir_y
|
||
rz * t // Z = t * ray_dir_z
|
||
);
|
||
}
|
||
return objectPoints;
|
||
}
|
||
|
||
/*Breif: 拟合平面方程*/
|
||
cv::Vec4f fitPlaneToPoints(const std::vector<cv::Point3f>& points) {
|
||
CV_Assert(points.size() >= 3); // 至少需要3个点
|
||
|
||
// 构建数据矩阵(每行为[x,y,z,1])
|
||
cv::Mat A(points.size(), 4, CV_32F);
|
||
for (size_t i = 0; i < points.size(); ++i) {
|
||
A.at<float>(i, 0) = points[i].x;
|
||
A.at<float>(i, 1) = points[i].y;
|
||
A.at<float>(i, 2) = points[i].z;
|
||
A.at<float>(i, 3) = 1.0f;
|
||
}
|
||
|
||
// SVD分解求最小奇异值对应的右奇异向量
|
||
cv::Mat svdU, svdW, svdVt;
|
||
cv::SVDecomp(A, svdW, svdU, svdVt, cv::SVD::MODIFY_A);
|
||
|
||
// 取最后一行作为平面方程系数
|
||
cv::Vec4f plane(
|
||
svdVt.at<float>(3, 0),
|
||
svdVt.at<float>(3, 1),
|
||
svdVt.at<float>(3, 2),
|
||
svdVt.at<float>(3, 3)
|
||
);
|
||
|
||
// 归一化前三个系数
|
||
float norm = sqrt(pow(plane[0], 2) + pow(plane[1], 2) + pow(plane[2], 2));
|
||
plane = plane / norm;
|
||
return plane;
|
||
}
|
||
|
||
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
|
||
// 调试程序: F5 或调试 >“开始调试”菜单
|
||
|
||
// 入门使用技巧:
|
||
// 1. 使用解决方案资源管理器窗口添加/管理文件
|
||
// 2. 使用团队资源管理器窗口连接到源代码管理
|
||
// 3. 使用输出窗口查看生成输出和其他消息
|
||
// 4. 使用错误列表窗口查看错误
|
||
// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
|
||
// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
|