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