帮我修改代码：#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <fstream>
#include <sstream>
#include <iostream>
#include <vector>
#include <limits>
#include <cmath>
// 辅助函数：打印Eigen矩阵
template<typename Derived>
void printEigenMatrix(const std::string& name, const Eigen::MatrixBase<Derived>& mat) {
std::cout << "\n" << name << ":\n";
std::cout << std::fixed << std::setprecision(6) << mat << std::endl;
}
// 辅助函数：打印OpenCV矩阵
void printCvMatrix(const std::string& name, const cv::Mat& mat) {
std::cout << "\n" << name << ":\n";
for (int i = 0; i < mat.rows; ++i) {
for (int j = 0; j < mat.cols; ++j) {
std::cout << std::fixed << std::setprecision(6) << mat.at<double>(i, j) << "\t";
}
std::cout << std::endl;
}
}
// 从文件读取投影矩阵（处理CONTOUR格式）
bool readProjectionMatrix(const std::string& filePath, Eigen::Matrix<double, 3, 4>& P) {
std::ifstream file(filePath);
if (!file.is_open()) {
std::cerr << "无法打开文件: " << filePath << std::endl;
return false;
}
std::string line;
std::getline(file, line); // 跳过首行"CONTOUR"
for (int i = 0; i < 3; ++i) {
if (!std::getline(file, line)) {
std::cerr << "文件格式错误: " << filePath << std::endl;
return false;
}
std::istringstream iss(line);
for (int j = 0; j < 4; ++j) {
if (!(iss >> P(i, j))) {
std::cerr << "数据解析错误: " << filePath << " (行: " << i + 2 << ")" << std::endl;
return false;
}
}
}
file.close();
// 归一化投影矩阵
double normFactor = P(2, 3);
if (std::abs(normFactor) > 1e-6) P /= normFactor;
// 输出读取的投影矩阵
printEigenMatrix("读取的投影矩阵 (" + filePath + ")", P);
return true;
}
// RQ分解
void rqDecompose(const Eigen::Matrix3d& M, Eigen::Matrix3d& R, Eigen::Matrix3d& Q) {
R = M;
Q = Eigen::Matrix3d::Identity();
// 消除R(1,0)
if (std::abs(R(1, 0)) > 1e-8) {
double c = R(0, 0) / std::hypot(R(0, 0), R(1, 0));
double s = R(1, 0) / std::hypot(R(0, 0), R(1, 0));
Eigen::Matrix3d G;
G << c, s, 0.0, -s, c, 0.0, 0.0, 0.0, 1.0;
R = G * R;
Q = Q * G.transpose();
}
// 消除R(2,0)
if (std::abs(R(2, 0)) > 1e-8) {
double c = R(0, 0) / std::hypot(R(0, 0), R(2, 0));
double s = R(2, 0) / std::hypot(R(0, 0), R(2, 0));
Eigen::Matrix3d G;
G << c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c;
R = G * R;
Q = Q * G.transpose();
}
// 消除R(2,1)
if (std::abs(R(2, 1)) > 1e-8) {
double c = R(1, 1) / std::hypot(R(1, 1), R(2, 1));
double s = R(2, 1) / std::hypot(R(1, 1), R(2, 1));
Eigen::Matrix3d G;
G << 1.0, 0.0, 0.0, 0.0, c, s, 0.0, -s, c;
R = G * R;
Q = Q * G.transpose();
}
// 强制下三角为0
for (int i = 1; i < 3; ++i)
for (int j = 0; j < i; ++j)
R(i, j) = 0.0;
}
// 分解投影矩阵（保留原始像主点）
void decomposeProjectionMatrix(const Eigen::Matrix<double, 3, 4>& P,
Eigen::Matrix3d& K, Eigen::Matrix3d& R, Eigen::Vector3d& T,
double imgWidth, double imgHeight, const std::string& camName) {
Eigen::Matrix3d M = P.block<3, 3>(0, 0);
Eigen::Matrix3d R_upper, Q;
rqDecompose(M, R_upper, Q);
// 提取内参K并修正符号
K = R_upper;
for (int i = 0; i < 3; ++i) {
if (K(i, i) < 0) {
K.row(i) *= -1;
Q.col(i) *= -1;
}
}
// 归一化K
double scale = K(2, 2);
if (std::abs(scale) > 1e-8) K /= scale;
else K(2, 2) = 1.0;
// 提取旋转矩阵R
R = Q;
Eigen::JacobiSVD<Eigen::Matrix3d> svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV);
R = svd.matrixU() * svd.matrixV().transpose();
if (R.determinant() < 0) R = -R;
// 计算平移向量T
T = K.inverse() * P.col(3);
// 输出分解得到的K、R、T
printEigenMatrix(camName + " 内参矩阵 K", K);
printEigenMatrix(camName + " 旋转矩阵 R", R);
printEigenMatrix(camName + " 平移向量 T", T);
}
// 计算核线校正单应矩阵（修正方向和边界）
void calculateRectificationHomographies(
const Eigen::Matrix3d& K1, const Eigen::Matrix3d& R1, const Eigen::Vector3d& T1,
const Eigen::Matrix3d& K2, const Eigen::Matrix3d& R2, const Eigen::Vector3d& T2,
cv::Mat& H1, cv::Mat& H2) {
// 计算相对旋转和基线
Eigen::Matrix3d R21 = R2 * R1.transpose();
Eigen::Vector3d C1 = -R1.transpose() * T1; // 左相机中心
Eigen::Vector3d C2 = -R2.transpose() * T2; // 右相机中心
Eigen::Vector3d baseline = C2 - C1; // 基线向量（左→右）
// 输出相机中心和基线向量
printEigenMatrix("左相机中心 C1", C1);
printEigenMatrix("右相机中心 C2", C2);
printEigenMatrix("基线向量 (C2 - C1)", baseline);
if (baseline.norm() < 1e-6) {
std::cerr << "警告：基线长度接近零" << std::endl;
baseline = Eigen::Vector3d(1, 0, 0); // 默认基线向右
}
// 【核心修正：确保X轴方向为水平向右】
Eigen::Vector3d tx = baseline.normalized();
// 若X分量为负（方向向左），则翻转X轴方向
if (tx.x() < 0) {
tx = -tx;
std::cout << "\n注意：X轴方向被翻转（原方向向左）" << std::endl;
}
// 输出校正后的轴方向
printEigenMatrix("校正后 X轴方向 tx", tx);
// 计算Y轴方向（向下）
Eigen::Vector3d cam1_down = -R1.col(1); // 左相机下方向
Eigen::Vector3d ty = (tx.cross(cam1_down)).normalized();
if (ty.norm() < 1e-6) {
ty = tx.cross(-Eigen::Vector3d::UnitY()).normalized();
}
printEigenMatrix("校正后 Y轴方向 ty", ty);
Eigen::Vector3d tz = tx.cross(ty); // 计算Z轴（右手系）
printEigenMatrix("校正后 Z轴方向 tz", tz);
// 构造校正旋转矩阵
Eigen::Matrix3d R_rect;
R_rect.col(0) = tx; // X轴：水平向右
R_rect.col(1) = ty; // Y轴：向下
R_rect.col(2) = tz; // Z轴：垂直于XY平面
printEigenMatrix("校正旋转矩阵 R_rect", R_rect);
// 计算单应矩阵
Eigen::Matrix3d H1_eigen = K1 * R_rect * R1 * K1.inverse();
Eigen::Matrix3d H2_eigen = K2 * R_rect * R2 * K2.inverse();
// 尺度归一化
double scale1 = H1_eigen(2, 2);
double scale2 = H2_eigen(2, 2);
if (std::abs(scale1) > 1e-8) H1_eigen /= scale1;
if (std::abs(scale2) > 1e-8) H2_eigen /= scale2;
// 输出单应矩阵（Eigen格式）
printEigenMatrix("左相机单应矩阵 H1 (Eigen)", H1_eigen);
printEigenMatrix("右相机单应矩阵 H2 (Eigen)", H2_eigen);
// 转换为OpenCV矩阵
H1 = cv::Mat(3, 3, CV_64F);
H2 = cv::Mat(3, 3, CV_64F);
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j) {
H1.at<double>(i, j) = H1_eigen(i, j);
H2.at<double>(i, j) = H2_eigen(i, j);
}
// 输出单应矩阵（OpenCV格式）
printCvMatrix("左相机单应矩阵 H1 (OpenCV)", H1);
printCvMatrix("右相机单应矩阵 H2 (OpenCV)", H2);
}
// 生成核线影像，并旋转180度使地面在下、天空在上，同时保持原始纵横比
void generateEpipolarImage(
const std::string& imgPath, const cv::Mat& H,
const std::string& savePath, bool isRightImage = false) {
cv::Mat image = cv::imread(imgPath, cv::IMREAD_COLOR);
if (image.empty()) {
std::cerr << "无法读取图像: " << imgPath << std::endl;
return;
}
int imgWidth = image.cols;
int imgHeight = image.rows;
// 计算变换后图像边界
std::vector<cv::Point2d> corners = {
{0.0, 0.0}, {(double)imgWidth, 0.0},
{0.0, (double)imgHeight}, {(double)imgWidth, (double)imgHeight}
};
std::vector<cv::Point2d> transformedCorners;
cv::perspectiveTransform(corners, transformedCorners, H);
// 计算变换后的边界
double minX = std::numeric_limits<double>::max(), maxX = -minX;
double minY = std::numeric_limits<double>::max(), maxY = -minY;
for (const auto& pt : transformedCorners) {
minX = std::min(minX, pt.x);
maxX = std::max(maxX, pt.x);
minY = std::min(minY, pt.y);
maxY = std::max(maxY, pt.y);
}
// 增加1%冗余避免截断
double paddingX = (maxX - minX) * 0.01;
double paddingY = (maxY - minY) * 0.01;
minX -= paddingX;
maxX += paddingX;
minY -= paddingY;
maxY += paddingY;
// 获取原图纵横比
double originalAspectRatio = static_cast<double>(imgWidth) / imgHeight;
// 当前边界计算出的纵横比
double currentAspectRatio = (maxX - minX) / (maxY - minY);
// 如果当前纵横比与原图不一致，则进行裁剪/扩展
if (currentAspectRatio > originalAspectRatio) {
// 宽度过大，上下扩展高度
double targetHeight = (maxX - minX) / originalAspectRatio;
double heightDiff = targetHeight - (maxY - minY);
minY -= heightDiff / 2.0;
maxY += heightDiff / 2.0;
}
else {
// 高度过大，左右扩展宽度
double targetWidth = (maxY - minY) * originalAspectRatio;
double widthDiff = targetWidth - (maxX - minX);
minX -= widthDiff / 2.0;
maxX += widthDiff / 2.0;
}
// 构造平移矩阵（将图像移至正坐标）
cv::Mat H_trans = (cv::Mat_<double>(3, 3) <<
1.0, 0.0, -minX,
0.0, 1.0, -minY,
0.0, 0.0, 1.0) * H;
// 构造180度旋转矩阵
cv::Mat H_rotate_180 = (cv::Mat_<double>(3, 3) <<
-1, 0, static_cast<double>(imgWidth),
0, -1, static_cast<double>(imgHeight),
0, 0, 1);
// 组合最终变换矩阵：先旋转再平移
cv::Mat H_final = H_trans * H_rotate_180;
// 输出组合后的单应矩阵（调试用）
printCvMatrix(
(std::string("[") + (isRightImage ? "右" : "左") + "] 最终变换矩阵 H_final").c_str(),
H_final
);
// 计算输出图像尺寸，保持原图比例
int newWidth = static_cast<int>(std::round(maxX - minX));
int newHeight = static_cast<int>(std::round(maxY - minY));
cv::Size newSize(newWidth, newHeight);
std::cout << "[" << (isRightImage ? "右" : "左") << "核线影像] 输出尺寸: "
<< newSize.width << "x" << newSize.height << std::endl;
// 执行透视变换
cv::Mat epipolar;
cv::warpPerspective(image, epipolar, H_final, newSize,
cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar(0, 0, 0));
// 绘制水平核线
cv::Mat epipolarWithLines = epipolar.clone();
for (int y = 0; y < epipolarWithLines.rows; y += 50) {
cv::line(epipolarWithLines,
cv::Point(0, y), cv::Point(epipolarWithLines.cols, y),
cv::Scalar(0, 255, 0), 1, cv::LINE_AA);
}
// 保存结果
cv::imwrite(savePath, epipolar);
cv::imwrite(savePath + "_with_lines.tif", epipolarWithLines);
std::cout << "保存核线影像: " << savePath << std::endl;
// 显示图像
std::string windowName = isRightImage ? "右核线影像" : "左核线影像";
cv::namedWindow(windowName, cv::WINDOW_NORMAL);
cv::imshow(windowName, epipolarWithLines);
}
int main() {
// 引入setprecision用于控制输出精度
std::cout << std::fixed << std::setprecision(6);
std::vector<std::pair<std::string, std::string>> stereoPair = {
{"015AR027.txt", "015AR027.tif"},
{"015AR028.txt", "015AR028.tif"}
};
std::string saveDir = "C:\\Users\\ASUS\\Desktop\\摄测课设\\相机标定\\相机标定\\results";
if (stereoPair.size() != 2) {
std::cerr << "请提供两个相机数据！" << std::endl;
return -1;
}
// 处理左相机
std::cout << "\n======= 处理左相机 =======" << std::endl;
Eigen::Matrix<double, 3, 4> P1;
cv::Mat img1 = cv::imread(stereoPair[0].second);
if (img1.empty() || !readProjectionMatrix(stereoPair[0].first, P1)) {
std::cerr << "左相机数据错误！" << std::endl;
return -1;
}
Eigen::Matrix3d K1, R1;
Eigen::Vector3d T1;
decomposeProjectionMatrix(P1, K1, R1, T1, img1.cols, img1.rows, "左相机");
// 处理右相机
std::cout << "\n======= 处理右相机 =======" << std::endl;
Eigen::Matrix<double, 3, 4> P2;
cv::Mat img2 = cv::imread(stereoPair[1].second);
if (img2.empty() || !readProjectionMatrix(stereoPair[1].first, P2)) {
std::cerr << "右相机数据错误！" << std::endl;
return -1;
}
Eigen::Matrix3d K2, R2;
Eigen::Vector3d T2;
decomposeProjectionMatrix(P2, K2, R2, T2, img2.cols, img2.rows, "右相机");
// 计算校正单应矩阵
std::cout << "\n======= 计算单应矩阵 =======" << std::endl;
cv::Mat H1, H2;
calculateRectificationHomographies(K1, R1, T1, K2, R2, T2, H1, H2);
// 生成并保存核线影像
std::cout << "\n======= 生成左核线影像 =======" << std::endl;
generateEpipolarImage(stereoPair[0].second, H1, saveDir + "/left_epipolar.tif", false);
std::cout << "\n======= 生成右核线影像 =======" << std::endl;
generateEpipolarImage(stereoPair[1].second, H2, saveDir + "/right_epipolar.tif", true);
cv::waitKey(0);
cv::destroyAllWindows();
return 0;
}
最新发布
OpenCV坐标访问解析
本文详细解析了在OpenCV中如何正确理解图像坐标系及访问图像数据的方式。介绍了坐标系的原点及其方向，并对比了不同访问方式的效果，强调了在多通道图像中访问像素值的注意事项。
扫一扫
9035
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
