typedef std::pair<cv::Mat, cv::Mat> cv_trans_t;

cv_trans_t isometry3fTocvMat(Eigen::Isometry3f& pose){
    cv::Mat rot, tra;
    cv::eigen2cv(pose.rotation(),rot);
    std::cout << "rot cv mat: \n" << rot << std::endl;
    cv::eigen2cv(Eigen::Matrix<float,3,1>(pose.translation()),tra);
    std::cout << "tra cv mat: \n" << tra << std::endl;

    cv::Rodrigues(rot,rot);
    cv_trans_t output;
    output.first = rot;
    output.second = tra;
    return output;
}