各种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;
}