Пример #1
0
void ConvertCv2tgPoseWorld(	const cv::Mat_<double> &R, const cv::Mat_<double> &T, 
                                const TomGine::tgPose &camPose, TomGine::tgPose &pose)
{
    mat3 r;
    vec3 t, rvec;
    TomGine::tgPose pose_cam_coord, camPoseT;
    camPoseT = camPose.Transpose();

    t.x = T(0,0);
    t.y = T(1,0);
    t.z = T(2,0);
    rvec.x = R(0,0);
    rvec.y = R(1,0);
    rvec.z = R(2,0);
    r.fromRotVector(rvec);
    r = r.transpose();

    pose_cam_coord.SetPose(r,t);

    pose = camPoseT * pose_cam_coord;
}
Пример #2
0
// Converts pose from Camera coordinates to world space
void ConvertCam2World(const TomGine::tgPose& pC, const TomGine::tgPose& camPose, TomGine::tgPose& pW){

    pW = camPose.Transpose() * pC;
}