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