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