void BaseDaVinciPoseGrabber::convertFromDaVinciPose(const ci::Matrix44f &in_pose, ci::Matrix44f &out_pose){
out_pose.setToIdentity();
ci::Matrix44f flip;
flip.setToIdentity();
flip.at(1, 1) *= -1;
flip.at(2, 2) *= -1;
flip.invert();
out_pose = in_pose * flip;
}
void _TBOX_PREFIX_App::setup()
{
mCubeRotation.setToIdentity();
// Create a blue-green gradient as an OpenGL texture
Surface8u surface( 256, 256, false );
Surface8u::Iter iter = surface.getIter();
while( iter.line() ) {
while( iter.pixel() ) {
iter.r() = 0;
iter.g() = iter.x();
iter.b() = iter.y();
}
}
mTex = gl::Texture( surface );
}
void BasePoseGrabber::convertFromBouguetPose(const ci::Matrix44f &in_pose, ci::Matrix44f &out_pose){
out_pose.setToIdentity();
ci::Vec3f translation = in_pose.getTranslate().xyz();
translation[1] *= -1;
translation[2] *= -1;
out_pose.translate(translation);
ci::Matrix33f flip;
flip.setToIdentity();
flip.at(1, 1) *= -1;
flip.at(2, 2) *= -1;
ci::Matrix33f in_gl_coords = flip * in_pose.subMatrix33(0, 0);
ci::Quatf q(in_gl_coords);
out_pose.rotate(q.getAxis(), q.getAngle());
out_pose.invert(); //bouguet poses (from calibration) are grid poses so invert to get camera poses
}