Ejemplo n.º 1
0
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));
	}
}
Ejemplo n.º 2
0
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;

}
Ejemplo n.º 3
0
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";

}
Ejemplo n.º 4
0
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 );
}
Ejemplo n.º 5
0
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";
  
}
Ejemplo n.º 6
0
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();

}
Ejemplo n.º 7
0
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;

}
Ejemplo n.º 8
0
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

}
Ejemplo n.º 9
0
void _TBOX_PREFIX_App::update()
{
	mCubeRotation.rotate( Vec3f( 1, 1, 1 ), 0.03f );
}