来自本科毕设
1、匹配棋盘格角点
2、利用p1 p2 p3生成旋转矩阵然后分解成欧拉角
代码
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <iostream>
using namespace std;
using namespace cv;
cv::Point2d pixel2cam(const cv::Point2f& p, const cv::Mat& K) {
return cv::Point2d(
(p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
);
}
int main() {
// 定义左右相机的相机矩阵
cv::Mat T1 = (cv::Mat_<float>(3, 4) <<
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0);
/*cv::Mat T2 = (cv::Mat_<float>(3, 4) <<
0.953637062614699, 0.00658496811859602, -0.300887007035011, 4.458319257471509e+02,
-0.00250704140519836, 0.999899730598709, 0.0139371263904078, 11.501725596171031,
0.300948612807905, -0.0125366240873153, 0.953557950785106, 65.623152047724048);*/
//cv::triangulatePoints(T2, T1, pts1, pts2, points4D);//for 2
//cv::triangulatePoints(T1, T2, pts2, pts1, points4D);//for 2
//cv::triangulatePoints(T2, T1, pts2, pts1, points4D);//啥也不是
//cv::triangulatePoints(T1, T2, pts1, pts2, points4D);//啥也不是
cv::Mat T2 = (cv::Mat_<float>(3, 4) <<
0.995305136270557, -0.0144566356161613, 0.0957010522413938, -862.944841702106,
0.0152041841514424, 0.999859296099956, -0.00708666259880605, -25.7816438948644,
-0.0955851374311795, 0.00850844810537307, 0.995384894306268, 26.6371015576055);
cv::Mat K1 = (cv::Mat_<double>(3, 3) <<
13803.4389203154, 0, 635.162923580742,
0, 13804.0945005903, 629.015160590977,
0, 0, 1);
cv::Mat K2 = (cv::Mat_<double>(3, 3) <<
14138.2738088473, 0, 583.581246482159,
0, 14136.9818157979, 622.304905721788,
0, 0, 1);
int total_img = 11;//多少张图片
// 定义棋盘格内角点的行数和列数
int chessboardRows = 8; // 棋盘格行数
int chessboardCols = 11; // 棋盘格列数
string l_path = "../../../../实验数据/图片数据/原始图片/12m5/测量/l/";
string r_path = "../../../../实验数据/图片数据/原始图片/12m5/测量/r/";
vector<double> x_d, y_d,z_d;
// 读取图像
cv::Mat image1 = cv::imread(l_path+"6.bmp");
cv::Mat image2 = cv::imread(r_path + "6.bmp");
/*
// 获取帧的宽度和高度
int frameWidth = static_cast<int>(image1.cols*2);
int frameHeight = static_cast<int>(image2.rows);
// 定义视频编解码器和创建 VideoWriter 对象
int fps = 1; // 帧率
std::string outputVideo = "E:/BaiduNetdiskDownload/毕设/12m5/output_video.avi"; // 输出视频文件路径
cv::VideoWriter videoWriter(outputVideo, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), fps, cv::Size(frameWidth, frameHeight));
if (!videoWriter.isOpened()) {
std::cerr << "Error: Could not open the output video file for writing." << std::endl;
return -1;
}
*/
/*
cv::Mat image1, image2;
Mat distort1 = (cv::Mat_<double>(4, 1) << 0.359491204591065, 42.5099982392409, 0, 0);
Mat distort2 = (cv::Mat_<double>(4, 1) << 0.444058777698131, 34.8489892235698, 0, 0);
undistort(dist_img1, image1, K1, distort1);
undistort(dist_img2, image2, K2, distort2);
*/
Mat gray1, gray2;
cvtColor(image1, gray1, COLOR_BGR2GRAY);
cvtColor(image2, gray2, COLOR_BGR2GRAY);
// 用于存储检测到的角点
std::vector<cv::Point2f> corners1, corners2;
// 尝试在图像中检测棋盘格角点
bool patternFound1 = cv::findChessboardCorners(image1, cv::Size(chessboardCols, chessboardRows), corners1, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);
bool patternFound2 = cv::findChessboardCorners(image2, cv::Size(chessboardCols, chessboardRows), corners2, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);
int t1 = 0;
//for (int i = 5; i >= 1; i--)
for(int i=1;i<=total_img;i++)
{
cv::Mat image3 = cv::imread(l_path+to_string(i)+".bmp");
cv::Mat image4 = cv::imread(r_path + to_string(i) + ".bmp");
/*cv::Mat image3, image4;
undistort(dist_img3, image3, K1, distort1);
undistort(dist_img4, image4, K2, distort2);
*/
Mat gray3, gray4;
cvtColor(image3, gray3, COLOR_BGR2GRAY);
cvtColor(image4, gray4, COLOR_BGR2GRAY);
// 检查图像是否成功加载
if (image1.empty() || image2.empty() || image3.empty() || image4.empty()) {
std::cerr << "无法加载图像文件!" << std::endl;
return -1;
}
// 计时开始
double time1 = static_cast<double>(getTickCount());
// 用于存储检测到的角点
std::vector<cv::Point2f> corners3, corners4;
bool patternFound3 = cv::findChessboardCorners(image3, cv::Size(chessboardCols, chessboardRows), corners3, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);
bool patternFound4 = cv::findChessboardCorners(image4, cv::Size(chessboardCols, chessboardRows), corners4, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);
// 计时结束
time1 = ((double)getTickCount() - time1) / getTickFrequency();
t1 += time1;
// 如果找到棋盘格角点
if (patternFound1 && patternFound2 && patternFound3 && patternFound4) {
/*
// 在图像中绘制检测到的角点
cv::drawChessboardCorners(image1, cv::Size(chessboardCols, chessboardRows), corners1, patternFound1);
cv::drawChessboardCorners(image2, cv::Size(chessboardCols, chessboardRows), corners2, patternFound2);
// 在图像中绘制检测到的角点
cv::drawChessboardCorners(image3, cv::Size(chessboardCols, chessboardRows), corners3, patternFound3);
cv::drawChessboardCorners(image4, cv::Size(chessboardCols, chessboardRows), corners4, patternFound4);
*/
// 提高角点的精确度
cornerSubPix(gray1, corners1, Size(11, 11), Size(-1, -1),
TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 50, 0.01));
cornerSubPix(gray2, corners2, Size(11, 11), Size(-1, -1),
TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 50, 0.01));
cornerSubPix(gray3, corners3, Size(11, 11), Size(-1, -1),
TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 50, 0.01));
cornerSubPix(gray4, corners4, Size(11, 11), Size(-1, -1),
TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 50, 0.01));
std::vector<cv::Point2d> pts1, pts2, pts3, pts4;
for (int i = 0; i < corners1.size(); i++)
{
// 调用函数将像素坐标系下的点转换为相机坐标系下的点
pts1.push_back(pixel2cam(corners1[i], K1));
pts2.push_back(pixel2cam(corners2[i], K2));
pts3.push_back(pixel2cam(corners3[i], K1));
pts4.push_back(pixel2cam(corners4[i], K2));
}
/*
std::vector<cv::Point2f> p1;
p1.push_back(pixel2cam(corners1[1], K1));
std::vector<cv::Point2f> p2;
p2.push_back(pixel2cam(corners2[1], K2));
*/
// 通过三角测量计算三维点坐标
cv::Mat points4D1, points4D2;
cv::triangulatePoints(T1, T2, pts1, pts2, points4D1);//T2 == T21 服了
cv::triangulatePoints(T1, T2, pts3, pts4, points4D2);//T2 == T21 服了
//cv::triangulatePoints(T2, T1, pts1, pts2, points4D);//for 2
// 将齐次坐标转换为非齐次坐标
cv::Mat points3D1, points3D2;
cv::convertPointsFromHomogeneous(points4D1.t(), points3D1);
cv::convertPointsFromHomogeneous(points4D2.t(), points3D2);
/*
std::vector<cv::Point3d> px1, py1, px2, py2;
for (int i = 0; i < chessboardCols; i++)
{
px1.push_back(points3D1.at<cv::Vec3d>(i, 0));
px2.push_back(points3D2.at<cv::Vec3d>(i, 0));
}
for (int i = 0; i < chessboardRows; i++)
{
py1.push_back(points3D1.at<cv::Vec3d>(i* chessboardCols, 0));
py2.push_back(points3D2.at<cv::Vec3d>(i* chessboardCols, 0));
}
Vec6f linesx1, linesx2,linesy1, linesy2;
fitLine(px1, linesx1, DIST_L2, 0.0, 1e-2, 1e-2);
fitLine(px2, linesx2, DIST_L2, 0.0, 1e-2, 1e-2);
fitLine(py1, linesy1, DIST_L2, 0.0, 1e-2, 1e-2);
fitLine(py2, linesy2, DIST_L2, 0.0, 1e-2, 1e-2);
cv::Vec3d X1, X2, Y1, Y2;
if (linesx1[0] > 0)
{
X1[0] = linesx1[0];
X1[1] = linesx1[1];
X1[2] = linesx1[2];
}
else
{
X1[0] = -linesx1[0];
X1[1] = -linesx1[1];
X1[2] = -linesx1[2];
}
if (linesy1[1] > 0)
{
Y1[0] = linesy1[0];
Y1[1] = linesy1[1];
Y1[2] = linesy1[2];
}
else
{
Y1[0] = -linesy1[0];
Y1[1] = -linesy1[1];
Y1[2] = -linesy1[2];
}
if (linesx2[0] > 0)
{
X2[0] = linesx2[0];
X2[1] = linesx2[1];
X2[2] = linesx2[2];
}
else
{
X2[0] = -linesx2[0];
X2[1] = -linesx2[1];
X2[2] = -linesx2[2];
}
if (linesy2[1] > 0)
{
Y2[0] = linesy2[0];
Y2[1] = linesy2[1];
Y2[2] = linesy2[2];
}
else
{
Y2[0] = -linesy2[0];
Y2[1] = -linesy2[1];
Y2[2] = -linesy2[2];
}
*/
cv::Vec3d X0(1.0, 0.0, 0.0);
cv::Vec3d Y0(0.0, 1.0, 0.0);
cv::Vec3d Z0(0.0, 0.0, 1.0);
cv::Vec3d X1(points3D1.at<cv::Vec3d>(10, 0)[0] - points3D1.at<cv::Vec3d>(0, 0)[0],
points3D1.at<cv::Vec3d>(10, 0)[1] - points3D1.at<cv::Vec3d>(0, 0)[1],
points3D1.at<cv::Vec3d>(10, 0)[2] - points3D1.at<cv::Vec3d>(0, 0)[2]);
cv::Vec3d Y1(points3D1.at<cv::Vec3d>(77, 0)[0] - points3D1.at<cv::Vec3d>(0, 0)[0],
points3D1.at<cv::Vec3d>(77, 0)[1] - points3D1.at<cv::Vec3d>(0, 0)[1],
points3D1.at<cv::Vec3d>(77, 0)[2] - points3D1.at<cv::Vec3d>(0, 0)[2]);
/* cv::normalize(_X1, _X1, 1.0, 0, cv::NORM_L2);
cv::normalize(_Y1, _Y1, 1.0, 0, cv::NORM_L2);*/
cv::Vec3d Z1 = X1.cross(Y1);
//归一化
cv::normalize(X1, X1, 1.0, 0, cv::NORM_L2);
cv::normalize(Y1, Y1, 1.0, 0, cv::NORM_L2);
cv::normalize(Z1, Z1, 1.0, 0, cv::NORM_L2);
cv::Vec3d X2(points3D2.at<cv::Vec3d>(10, 0)[0] - points3D2.at<cv::Vec3d>(0, 0)[0],
points3D2.at<cv::Vec3d>(10, 0)[1] - points3D2.at<cv::Vec3d>(0, 0)[1],
points3D2.at<cv::Vec3d>(10, 0)[2] - points3D2.at<cv::Vec3d>(0, 0)[2]);
cv::Vec3d Y2(points3D2.at<cv::Vec3d>(77, 0)[0] - points3D2.at<cv::Vec3d>(0, 0)[0],
points3D2.at<cv::Vec3d>(77, 0)[1] - points3D2.at<cv::Vec3d>(0, 0)[1],
points3D2.at<cv::Vec3d>(77, 0)[2] - points3D2.at<cv::Vec3d>(0, 0)[2]);
/*cv::normalize(_X2, _X2, 1.0, 0, cv::NORM_L2);
cv::normalize(_Y2, _Y2, 1.0, 0, cv::NORM_L2);*/
cv::Vec3d Z2 = X2.cross(Y2);
//归一化
cv::normalize(X2, X2, 1.0, 0, cv::NORM_L2);
cv::normalize(Y2, Y2, 1.0, 0, cv::NORM_L2);
cv::normalize(Z2, Z2, 1.0, 0, cv::NORM_L2);
cv::Mat R12 = (cv::Mat_<double>(3, 3) <<
X2.dot(X1), Y2.dot(X1), Z2.dot(X1),
X2.dot(Y1), Y2.dot(Y1), Z2.dot(Y1),
X2.dot(Z1), Y2.dot(Z1), Z2.dot(Z1));
// 计算欧拉角
double sy = std::sqrt(R12.at<double>(0, 0) * R12.at<double>(0, 0) + R12.at<double>(1, 0) * R12.at<double>(1, 0));
bool singular = sy < 1e-6; // 如果sy接近0,矩阵是奇异的
double x, y, z;
if (!singular) {
x = std::atan2(R12.at<double>(2, 1), R12.at<double>(2, 2));
y = std::atan2(-R12.at<double>(2, 0), sy);
z = std::atan2(R12.at<double>(1, 0), R12.at<double>(0, 0));
}
else {
x = std::atan2(-R12.at<double>(1, 2), R12.at<double>(1, 1));
y = std::atan2(-R12.at<double>(2, 0), sy);
z = 0;
}
double xdeg, ydeg, zdeg;
xdeg = x * 180 / CV_PI;
ydeg = y * 180 / CV_PI;
zdeg = z * 180 / CV_PI;
x_d.push_back(xdeg);
y_d.push_back(ydeg);
z_d.push_back(zdeg);
/*
// 水平拼接图像
Mat result1;
hconcat(image3, image4, result1);
*/
/*
// 构造DMatch对象
std::vector<cv::DMatch> lrmatches;
for (size_t i = 0; i < 3; i++) {
lrmatches.push_back(cv::DMatch(i, i, 0));
}
// 转换为KeyPoint对象
std::vector<cv::KeyPoint> keypointsl, keypointsr;
keypointsl.push_back(cv::KeyPoint(corners3[0], 1.f));
keypointsl.push_back(cv::KeyPoint(corners3[10], 1.f));
keypointsl.push_back(cv::KeyPoint(corners3[77], 1.f));
keypointsr.push_back(cv::KeyPoint(corners4[0], 1.f));
keypointsr.push_back(cv::KeyPoint(corners4[10], 1.f));
keypointsr.push_back(cv::KeyPoint(corners4[77], 1.f));
// 绘制匹配结果
cv::Mat img_matches;
cv::drawMatches(image3, keypointsl, image4, keypointsr, lrmatches, img_matches);
// 使用 putText 函数写入文字
cv::putText(img_matches, "X:"+to_string(-xdeg), Point(2000, 50), FONT_HERSHEY_SIMPLEX, 2, Scalar(0, 0, 255), 3);
cv::putText(img_matches, "Y:" + to_string(-ydeg), Point(2000, 100), FONT_HERSHEY_SIMPLEX, 2, Scalar(0, 0, 255), 3);
cv::putText(img_matches, "Z:" + to_string(-zdeg), Point(2000, 150), FONT_HERSHEY_SIMPLEX, 2, Scalar(0, 0, 255), 3);
// 将当前帧写入视频文件
videoWriter.write(img_matches);
*/
/*// 水平拼接图像
Mat result2;
hconcat(image1, image2, result2);
Mat result3;
vconcat(result1, result2, result3);
*/
double dis = points3D1.at<cv::Vec3d>(0, 0)[2];
}
else {
std::cerr << "未能检测到棋盘格角点!" << std::endl;
}
}
//videoWriter.release();
return 0;
}
传感器精度0.02°(不公布)