void OrthRoot::setCameraForDraw(ci::Matrix44f& m){
if(mCameraDirty) {
setCinderCamera();
}
setGlCamera();
// Account for src rect translation
if(mSrcRect.x2 > mSrcRect.x1 && mSrcRect.y2 > mSrcRect.y1) {
const float sx = mDstRect.getWidth() / mSrcRect.getWidth(),
sy = mDstRect.getHeight() / mSrcRect.getHeight();
m.translate(ci::Vec3f(-mSrcRect.x1*sx, -mSrcRect.y1*sy, 0.0f));
m.scale(ci::Vec3f(sx, sy, 1.0f));
}
}
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 PoseGrabber::WritePoseToStream(const ci::Matrix44f &camera_pose) {
if (!ofs_.is_open()) {
if (!boost::filesystem::exists(save_dir_)) {
boost::filesystem::create_directory(save_dir_);
}
ofs_.open(ofs_file_);
}
if (!ofs_.is_open()) throw(std::runtime_error("Error, cannot open file"));
ofs_ << camera_pose.inverted() * model_.Body().transform_ << "\n";
ofs_ << "\n";
}
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 SE3DaVinciPoseGrabber::WritePoseToStream(const ci::Matrix44f &camera_pose) {
if (!ofs_.is_open()) {
if (!boost::filesystem::exists(save_dir_)) {
boost::filesystem::create_directory(save_dir_);
}
ofs_.open(ofs_file_);
}
if (!ofs_.is_open()) throw(std::runtime_error("Error, could not open file"));
ofs_ << WriteSE3ToString(camera_pose.inverted() * model_.Shaft().transform_) << "\n";
for (size_t i = 0; i < wrist_dh_params_.size(); ++i){
ofs_ << wrist_dh_params_[i] << "\n";
}
ofs_ << "\n";
}
std::string BasePoseGrabber::WriteSE3ToString(const ci::Matrix44f &mat){
std::stringstream ss;
//ci::Quatf r = mat.subMatrix33(0, 0);
//ci::Vec4f trans = mat.getTranslate();
//ss << trans.x << " " << trans.y << " " << trans.z << " " << r.w << " " << r.v.x << " " << r.v.y << " " << r.v.z << " ";
//return ss.str();
for (int r = 0; r < 4; ++r){
ss << "| ";
for (int c = 0; c < 4; ++c){
ss << mat.at(r, c) << " ";
}
ss << "|\n";
}
return ss.str();
}
void DHDaVinciPoseGrabber::WritePoseToStream(const ci::Matrix44f &camera_pose) {
if (!se3_ofs_.is_open()) {
if (!boost::filesystem::exists(save_dir_)) {
boost::filesystem::create_directory(save_dir_);
}
se3_ofs_.open(se3_ofs_file_);
arm_ofs_.open(arm_ofs_file_);
base_ofs_.open(base_ofs_file_);
}
if (!se3_ofs_.is_open()) throw(std::runtime_error("Error, could not open file"));
if (!arm_ofs_.is_open()) throw(std::runtime_error("Error, could not open file"));
if (!base_ofs_.is_open()) throw(std::runtime_error("Error, could not open file"));
//se3_ofs_ << WriteSE3ToString(camera_pose.inverted() * model_.Shaft().transform_);
//for (size_t i = 4; i < arm_joints_.size(); ++i){
// se3_ofs_ << arm_joints_[i] + arm_offsets_[i] << " ";
//}
//se3_ofs_ << "\n" << std::endl;
se3_ofs_ << WriteSE3ToString(camera_pose.inverted() * model_.Shaft().transform_) << "\n";
for (size_t i = 4; i < arm_joints_.size(); ++i){
se3_ofs_ << arm_joints_[i] + arm_offsets_[i] << "\n";
}
se3_ofs_ << std::endl;
for (size_t i = 0; i < arm_joints_.size(); ++i){
arm_ofs_ << arm_joints_[i] + arm_offsets_[i] << " ";
}
arm_ofs_ << std::endl;
for (size_t i = 0; i < base_joints_.size(); ++i){
base_ofs_ << base_joints_[i] + base_offsets_[i] << " ";
}
base_ofs_ << std::endl;
}
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
}
void _TBOX_PREFIX_App::update()
{
mCubeRotation.rotate( Vec3f( 1, 1, 1 ), 0.03f );
}