| 236 | } |
| 237 | |
| 238 | bool Publisher::saveKeyframes(const std::string& filename, const std::vector<KeyframeDump>& keyframes) const { |
| 239 | |
| 240 | // Save keyframes to a text file with header and lines: |
| 241 | std::string txt_file = filename + ".txt"; |
| 242 | |
| 243 | // Save .txt file |
| 244 | std::ofstream ofs(txt_file, std::ios::out); |
| 245 | if (!ofs.is_open()) { |
| 246 | RCLCPP_ERROR(node_->get_logger(), "Failed to open keyframes file: %s", txt_file.c_str()); |
| 247 | return false; |
| 248 | } |
| 249 | ofs.setf(std::ios::fixed, std::ios::floatfield); |
| 250 | ofs.precision(9); |
| 251 | ofs << "#ID, timestamp, qx, qy, qz, qw, tx, ty, tz" << std::endl; |
| 252 | for (const auto& kf : keyframes) { |
| 253 | // Convert Timestamp (ns) to sec.nsec with 9-digit zero-padded nanoseconds |
| 254 | const uint64_t sec_part = static_cast<uint64_t>(kf.stamp) / 1000000000ULL; |
| 255 | const uint64_t nsec_part = static_cast<uint64_t>(kf.stamp) % 1000000000ULL; |
| 256 | ofs << kf.id << ", " << sec_part << "." << std::setw(9) << std::setfill('0') << nsec_part |
| 257 | << std::setfill(' ') // reset fill for subsequent fields |
| 258 | << ", " << kf.qx << ", " << kf.qy << ", " << kf.qz << ", " << kf.qw |
| 259 | << ", " << kf.tx << ", " << kf.ty << ", " << kf.tz << std::endl; |
| 260 | } |
| 261 | ofs.close(); |
| 262 | RCLCPP_INFO(node_->get_logger(), "Keyframes saved to: %s", txt_file.c_str()); |
| 263 | |
| 264 | return true; |
| 265 | } |
| 266 | |
| 267 | void Publisher::publishPrimitiveEstimator(const std::pair<Timestamp, Eigen::Matrix4d>& primitive_estimator_pose) { |
| 268 | Eigen::Matrix3d rot = primitive_estimator_pose.second.block<3, 3>(0, 0); |