各种cv::mat,Eigen等类型之间的转换和求逆

转自：http://blog.csdn.net/u012706484/article/details/78775360
cv::Mat cConverter::toCvMatInverse(const cv::Mat &Tcw)
    {
        cv::Mat Rcw = Tcw.rowRange(0,3).colRange(0,3);
        cv::Mat tcw = Tcw.rowRange(0,3).col(3);
        cv::Mat Rwc = Rcw.t();
        cv::Mat twc = -Rwc*tcw;

        cv::Mat Twc = cv::Mat::eye(4,4,Tcw.type());
        Rwc.copyTo(Twc.rowRange(0,3).colRange(0,3));
        twc.copyTo(Twc.rowRange(0,3).col(3));

        return Twc.clone();
    }

        cv::Mat cConverter::toCvMat(const Eigen::Matrix<double,3,1> &m)
    {
          cv::Mat cvMat(3,1,CV_32F);
          for(int i=0;i<3;i++)
            cvMat.at<float>(i)=m(i);
          return cvMat.clone();
    }

        cv::Mat cConverter::Matrix3dtoCvMat(const Eigen::Matrix3d &m)
    {
        cv::Mat cvMat(3,3,CV_32F);
        for(int i=0;i<3;i++)
        for(int j=0; j<3; j++)
        cvMat.at<float>(i,j)=m(i,j);

        return cvMat.clone();
    }

    cv::Mat cConverter::Matx33dtoCvMat(const Eigen::Matrix3d &m)
    {
        cv::Mat cvMat(3,3,CV_32F);
        for(int i=0;i<3;i++)
        for(int j=0; j<3; j++)
            cvMat.at<float>(i,j)=m(i,j);

        return cvMat.clone();
    }

        cv::Mat cConverter::Matx44dtoCvMat(const Eigen::Matrix<double,4,4> &m)
    {
        cv::Mat cvMat(4,4,CV_32F);
        for(int i=0;i<4;i++)
        for(int j=0; j<4; j++)
            cvMat.at<float>(i,j)=m(i,j);

        return cvMat.clone();
    }

        Eigen::Matrix<double,3,1> cConverter::toVector3d(const cv::Mat &cvVector)
    {
        Eigen::Matrix<double,3,1> v;
        v << cvVector.at<float>(0), cvVector.at<float>(1), cvVector.at<float>(2);

        return v;
      }

      Eigen::Matrix<double,3,3> cConverter::toMatrix3d(const cv::Mat &cvMat3)
      {
          Eigen::Matrix<double,3,3> M;

          M << cvMat3.at<float>(0,0), cvMat3.at<float>(0,1), cvMat3.at<float>(0,2),
          cvMat3.at<float>(1,0), cvMat3.at<float>(1,1), cvMat3.at<float>(1,2),
          cvMat3.at<float>(2,0), cvMat3.at<float>(2,1), cvMat3.at<float>(2,2);

          return M;
      }
        //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    cv::Matx44d cConverter::invMat(const cv::Matx44d& M)
    {
        cv::Matx33d R = M.get_minor<3, 3>(0, 0);
        R = R.t();
        cv::Vec3d t(M(0, 3), M(1, 3), M(2, 3));
        t = -R * t;
        cv::Matx44d out(
            R(0, 0), R(0, 1), R(0, 2), t(0),
            R(1, 0), R(1, 1), R(1, 2), t(1),
            R(2, 0), R(2, 1), R(2, 2), t(2),
            0.0, 0.0, 0.0, 1.0);

        return out;
    }

    cv::Matx<double, 4, 4> cConverter::ogv2ocv(const Eigen::Matrix<double, 3, 4>& ogv_mat)
    {
        cv::Matx34d ocv_mat;
        cv::eigen2cv(ogv_mat, ocv_mat);

        return cv::Matx<double, 4, 4>(
            ocv_mat(0, 0), ocv_mat(0, 1), ocv_mat(0, 2), ocv_mat(0, 3),
            ocv_mat(1, 0), ocv_mat(1, 1), ocv_mat(1, 2), ocv_mat(1, 3),
            ocv_mat(2, 0), ocv_mat(2, 1), ocv_mat(2, 2), ocv_mat(2, 3),
            0.0, 0.0, 0.0, 1.0);
    }
    g2o::SE3Quat cConverter::toSE3Quat(const cv::Matx44d& homCV)
    {
        Eigen::Matrix<double, 3, 3> R;
        R << homCV(0, 0), homCV(0, 1), homCV(0, 2),
            homCV(1, 0), homCV(1, 1), homCV(1, 2),
            homCV(2, 0), homCV(2, 1), homCV(2, 2);

        Eigen::Matrix<double, 3, 1> t(homCV(0, 3), homCV(1, 3), homCV(2, 3));

        return g2o::SE3Quat(R, t);
    }

    cv::Matx44d cConverter::toCvMat(const g2o::SE3Quat& SE3)
    {
        Eigen::Matrix<double, 4, 4> eigMat = SE3.to_homogeneous_matrix();
        return toCvMat(eigMat);
    }

    cv::Matx44d cConverter::toCvMat(const g2o::Sim3& Sim3)
    {
        Eigen::Matrix3d eigR = Sim3.rotation().toRotationMatrix();
        Eigen::Vector3d eigt = Sim3.translation();
        double s = Sim3.scale();
        return toCvSE3(s*eigR, eigt);
    }

    cv::Matx44d cConverter::toCvMat(const Eigen::Matrix<double, 4, 4>& m)
    {
        cv::Matx44d cvMat = cv::Matx44d::eye();
        cv::eigen2cv(m, cvMat);
        return cvMat;
    }

    cv::Matx33d cConverter::toCvMat(const Eigen::Matrix3d& m)
    {
        cv::Matx33d cvMat = cv::Matx33d::eye();
        cv::eigen2cv(m, cvMat);
        return cvMat;
    }


    cv::Matx44d cConverter::toCvSE3(const Eigen::Matrix<double, 3, 3>& R,
        const Eigen::Matrix<double, 3, 1> &t)
    {
        cv::Matx44d cvMat = cv::Matx44d::eye();
        for (int i = 0; i < 3; ++i)
            for (int j = 0; j < 3; ++j)
                cvMat(i, j) = R(i, j);

        for (int i = 0; i < 3; ++i)
            cvMat(i, 3) = t(i);

        return cvMat;
    }


    Eigen::Matrix<double, 3, 1> cConverter::toVector3d(const cv::Vec4d& cvVector)
    {
        Eigen::Matrix<double, 3, 1> v;
        v << cvVector(0), cvVector(1), cvVector(2);
        return v;
    }

    Eigen::Matrix<double, 3, 1> cConverter::toVector3d(const cv::Vec3d& cvVector)
    {
        Eigen::Matrix<double, 3, 1> v;
        v << cvVector(0), cvVector(1), cvVector(2);

        return v;
    }

    Eigen::Matrix<double, 3, 3> cConverter::toMatrix3d(const cv::Matx33d& cvMat3)
    {
        Eigen::Matrix<double, 3, 3> M;

        M << cvMat3(0, 0), cvMat3(0, 1), cvMat3(0, 2),
            cvMat3(1, 0), cvMat3(1, 1), cvMat3(1, 2),
            cvMat3(2, 0), cvMat3(2, 1), cvMat3(2, 2);

        return M;
    }
    cv::Mat cConverter::toMat(const cv::Matx44d& matx44d)
    {
        cv::Mat out = cv::Mat::zeros(4, 4, CV_64FC1);
        for (int c = 0; c < 4; ++c)
            for (int r = 0; r < 4; ++r)
                out.ptr<double>(r)[c] = matx44d(r, c);
        return out;
    }