OpenCV矩阵转换Eigen矩阵
注意：
Eigen头文件要在opencv的eigen.hpp之前，否则报错！
Eigen中Matrix 与OpenCV中Mat间的转换
matrix->mat:eigen2cv
vector->mat:eigen2cv
mat->matrix:cv2eigen
mat->vector:cv2eigen
Matx33d EE;
for (int i = 0; i<3; i++)
for (int j = 0; j < 3; j++) {
//std::cout << "E =" << E.at(i, j) << std::endl;
EE(i, j) = E.at(i, j);
//std::cout << "E =" << EE(i, j) << std::endl;
}
std::cout << "EE =" << EE << std::endl; Eigen与Opencv之间的转换，在包含Eigen库的基础上，#include
rr = (cv::Mat_(3, 3) << -0.0001, -0.2, 3, 4, 5, 6, 7, 8, 791); 将Eigen::Matrix转换为cv::Mat
cv::eigen2cv(matrix,mat);
Matx33d EE;
for (int i = 0; i<3; i++)
for (int j = 0; j < 3; j++) {
//std::cout << "E =" << E.at(i, j) << std::endl;
EE(i, j) = E.at(i, j);
std::cout << "E =" << EE(i, j) << std::endl;
}
std::cout << "EE =" << EE << std::endl;
std::string filename3 = "./debug_output/xEx_output.txt";
FILE* fp3 = fopen(filename3.c_str(), "at+");
for (int i = 0; i < points1.size(); i++) {
Vec3d x1(points1[i].x, points1[i].y, 1.);
Vec3d x2(points2[i].x, points2[i].y, 1.);
// Matx33d E(model.ptr());
// E = cv::eigen2cv(E, ee);
// Eigen::Matrix3d er;
//cv::cv2eigen(E, er);
cv::Mat rr;
Eigen::Matrix3d er;
// rr = (cv::Mat_(3, 3) << -0.0001, -0.2, 3, 4, 5, 6, 7, 8, 791);
// cv::cv2eigen(rr, er); //cv to eigen
Matx33d EE(-0.0001, -0.2, 3, 4, 5, 6, 7, 8, 791);
Vec3d Ex1 = EE * x1;
Vec3d Etx2 = EE.t() * x2;
float x2tEx1 = x2.dot(Ex1);
fprintf(fp3,
"%10.8f\n",
x2tEx1);
}
fclose(fp3);
A::BB（目标类型）cConverter::XXXX（const C::DD（代转类型） &XXX）{……}
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 &m)
{
cv::Mat cvMat(3,1,CV_32F);
for(int i=0;i<3;i++)
cvMat.at(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(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(i,j)=m(i,j);
return cvMat.clone();
}
cv::Mat cConverter::Matx44dtoCvMat(const Eigen::Matrix &m)
{
cv::Mat cvMat(4,4,CV_32F);
for(int i=0;i<4;i++)
for(int j=0; j<4; j++)
cvMat.at(i,j)=m(i,j);
return cvMat.clone();
}
Eigen::Matrix cConverter::toVector3d(const cv::Mat &cvVector)
{
Eigen::Matrix v;
v << cvVector.at(0), cvVector.at(1), cvVector.at(2);
return v;
}
Eigen::Matrix cConverter::toMatrix3d(const cv::Mat &cvMat3)
{
Eigen::Matrix M;
M << cvMat3.at(0,0), cvMat3.at(0,1), cvMat3.at(0,2),
cvMat3.at(1,0), cvMat3.at(1,1), cvMat3.at(1,2),
cvMat3.at(2,0), cvMat3.at(2,1), cvMat3.at(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 cConverter::ogv2ocv(const Eigen::Matrix& ogv_mat)
{
cv::Matx34d ocv_mat;
cv::eigen2cv(ogv_mat, ocv_mat);
return cv::Matx(
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 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 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 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& 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& R,
const Eigen::Matrix &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 cConverter::toVector3d(const cv::Vec4d& cvVector)
{
Eigen::Matrix v;
v << cvVector(0), cvVector(1), cvVector(2);
return v;
}
Eigen::Matrix cConverter::toVector3d(const cv::Vec3d& cvVector)
{
Eigen::Matrix v;
v << cvVector(0), cvVector(1), cvVector(2);
return v;
}
Eigen::Matrix cConverter::toMatrix3d(const cv::Matx33d& cvMat3)
{
Eigen::Matrix 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(r)[c] = matx44d(r, c);
return out;
}
参考网址：
https://blog.csdn.net/u011722133/article/details/80118330
https://blog.csdn.net/piaoxuezhong/article/details/79110421
https://blog.csdn.net/u012706484/article/details/78775360