OpenCV cv::Mat到 Eigen 的正确转换——cv2eigen

在进行计算机视觉项目时，我们经常需要处理相机位姿的变换。最近，我在项目中遇到了一个看似简单但实际上颇具挑战性的问题：从 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;

遇到的问题： 针对简单的工程，代码运行完全没有问题，但是放到复杂工程里面，代码就会输出莫名其妙的结果！！！！！这种方法可能会导致错误，原因如下：

1. 直接访问 cv::Mat 的元素可能不安全，特别是当矩阵的存储格式不确定时。
2. 这种方法忽略了旋转部分的影响，可能导致计算结果不准确。
3. 在某些情况下，可能会出现索引错误或类型不匹配的问题。

改进的解决方案： 经过多次尝试和改进，我们最终采用了以下方法：

// 直接相减 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);

这个解决方案的优点：

1. 使用 cv::Mat 的矩阵减法，保持了原始数据的完整性。
2. 利用 OpenCV 提供的 cv2eigen 函数，安全地将 cv::Mat 转换为 Eigen 矩阵。
3. 使用 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;
}