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