在进行计算机视觉项目时，我们经常需要处理相机位姿的变换。最近，我在项目中遇到了一个看似简单但实际上颇具挑战性的问题：从 OpenCV 的 cv::Mat 格式转换到 Eigen 库的格式。这个过程中遇到了一些问题，但最终找到了一个稳健的解决方案。
问题描述： 我们有两个表示相机位姿的 4x4 变换矩阵，格式为 cv::Mat。目标是计算这两个位姿之间的变换，并提取出平移向量。
初始尝试： 最初，我们尝试直接从 cv::Mat 中提取平移向量：
Eigen::Vector3d transLast(
mLastFrameTcw.at<double>(0, 3),
mLastFrameTcw.at<double>(1, 3),
mLastFrameTcw.at<double>(2, 3)
);
Eigen::Vector3d transCurrent(
mCurrentFrameTcw.at<double>(0, 3),
mCurrentFrameTcw.at<double>(1, 3),
mCurrentFrameTcw.at<double>(2, 3)
);
TRANS_PRED = transCurrent - transLast;遇到的问题： 针对简单的工程，代码运行完全没有问题，但是放到复杂工程里面，代码就会输出莫名其妙的结果！！！！！这种方法可能会导致错误，原因如下：
- 直接访问 cv::Mat 的元素可能不安全，特别是当矩阵的存储格式不确定时。
- 这种方法忽略了旋转部分的影响，可能导致计算结果不准确。
- 在某些情况下，可能会出现索引错误或类型不匹配的问题。
改进的解决方案： 经过多次尝试和改进，我们最终采用了以下方法：
// 直接相减 cv::Mat
cv::Mat diff = mCurrentFrameTcw - mLastFrameTcw;
// 将 cv::Mat 转换为 Eigen 矩阵
Eigen::MatrixXd eigenDiff;
cv::cv2eigen(diff, eigenDiff);
// 从 eigenDiff 的最后一列提取前三个元素赋值给 TRANS_PRED
TRANS_PRED = eigenDiff.block<3,1>(0, eigenDiff.cols()-1);这个解决方案的优点：
- 使用 cv::Mat 的矩阵减法，保持了原始数据的完整性。
- 利用 OpenCV 提供的 cv2eigen 函数，安全地将 cv::Mat 转换为 Eigen 矩阵。
- 使用 Eigen 的 block 操作，精确地提取所需的平移向量。
结论： 在处理计算机视觉中的坐标变换问题时，正确地在不同库（如 OpenCV 和 Eigen）之间转换数据格式是至关重要的。通过采用矩阵减法和适当的类型转换，我们可以准确地计算相机位姿之间的变换。这个经验教训提醒我们，在处理不同库之间的数据转换时，要特别注意数据类型的一致性和操作的正确性。在future类似的问题中，我们可以借鉴这种方法，确保在不同数学库之间进行安全和准确的数据转换。
下面给出完整的代码（注意：下面的两个版本都可以用，只是区分两个中哪个更鲁棒！！！）
#include <iostream>
#include <iomanip>
#include <opencv2/core.hpp>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
void CalculateTransPred_Method1(const cv::Mat& mLastFrameTcw, const cv::Mat& mCurrentFrameTcw, Eigen::Vector3d& TRANS_PRED) {
std::cout << "Method 1: Direct extraction from cv::Mat" << std::endl;
Eigen::Vector3d transLast(
mLastFrameTcw.at<double>(0, 3),
mLastFrameTcw.at<double>(1, 3),
mLastFrameTcw.at<double>(2, 3)
);
Eigen::Vector3d transCurrent(
mCurrentFrameTcw.at<double>(0, 3),
mCurrentFrameTcw.at<double>(1, 3),
mCurrentFrameTcw.at<double>(2, 3)
);
std::cout << "transLast: " << transLast.transpose() << std::endl;
std::cout << "transCurrent: " << transCurrent.transpose() << std::endl;
TRANS_PRED = transCurrent - transLast;
std::cout << "TRANS_PRED: " << TRANS_PRED.transpose() << std::endl;
}
void CalculateTransPred_Method2(const cv::Mat& mLastFrameTcw, const cv::Mat& mCurrentFrameTcw, Eigen::Vector3d& TRANS_PRED) {
std::cout << "\nMethod 2: Using cv::Mat subtraction and Eigen conversion" << std::endl;
cv::Mat diff = mCurrentFrameTcw - mLastFrameTcw;
Eigen::MatrixXd eigenDiff;
cv::cv2eigen(diff, eigenDiff);
std::cout << "Difference (diff) in Eigen::MatrixXd format:" << std::endl;
std::cout << eigenDiff << std::endl;
TRANS_PRED = eigenDiff.block<3,1>(0, eigenDiff.cols()-1);
std::cout << "TRANS_PRED: " << TRANS_PRED.transpose() << std::endl;
}
int main() {
cv::Mat mLastFrameTcw = (cv::Mat_<double>(4, 4)
-0.1642483, 0.094168551, -0.98191381, 0.90703607,
0.0095526827, 0.99553794, 0.093877248, -0.038507219,
0.98637277, 0.0060392693, -0.164415, -3.4207926,
0, 0, 0, 1);
cv::Mat mCurrentFrameTcw = (cv::Mat_<double>(4, 4)
-0.16892175, 0.093616515, -0.98117346, 0.92409742,
0.009967736, 0.99559039, 0.093275994, -0.039425559,
0.98557907, 0.0059762667, -0.16911002, -3.4853551,
0, 0, 0, 1);
Eigen::Vector3d TRANS_PRED;
std::cout << std::fixed << std::setprecision(6);
std::cout << "mLastFrame.mTcw:" << std::endl;
std::cout << mLastFrameTcw << std::endl;
std::cout << "\nmCurrentFrame.mTcw:" << std::endl;
std::cout << mCurrentFrameTcw << std::endl;
CalculateTransPred_Method1(mLastFrameTcw, mCurrentFrameTcw, TRANS_PRED);
CalculateTransPred_Method2(mLastFrameTcw, mCurrentFrameTcw, TRANS_PRED);
return 0;
}
