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; }
// 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; }