concatenate a rotation and a translation of cv::Mat to a eigen

Question

I am doing a 6-dof transformation with the RANSAC given in OpenCV and I now want to convert two matrices of cv::Mat to an Isometry3d of Eigen but I didn't find good examples about this problem.

e.g.

cv::Mat rot;
cv::Mat trsl;
// the rot is 3-by-3 and trsl is 3-by-1 vector.

Eigen::Isometry3d trsf;

trsf.rotation = rot; 
trsf.translation = trsl;  // I know trsf has two members but it seems not the correct way to do a concatenation.

Anyone give me a hand? Thanks.

Answer 1

Essentially, you need an Eigen::Map to read the opencv data and store it to parts of your trsf:

typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> RMatrix3d;

Eigen::Isometry3d trsf;
trsf.linear() = RMatrix3d::Map(reinterpret_cast<const double*>(rot.data));
trsf.translation() = Eigen::Vector3d::Map(reinterpret_cast<const double*>(trsl.data));

You need to be sure that rot and trsl indeed hold double data (perhaps consider using cv::Mat_<double> instead).