Пример #1
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";

}
Пример #2
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";
  
}
Пример #3
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;

}