| 375 | } |
| 376 | |
| 377 | void LoopClosure::getKeyframesDump(std::vector<KeyframeDump>& out) const { |
| 378 | out.clear(); |
| 379 | out.reserve(kfMapper_.size()); |
| 380 | for (const auto& kv : kfMapper_) { |
| 381 | const Keyframe* kf = kv.second; |
| 382 | if (!kf) continue; |
| 383 | Eigen::Vector3d T; |
| 384 | Eigen::Matrix3d R; |
| 385 | const_cast<Keyframe*>(kf)->getPose(T, R); // getPose is non-const; safe as it doesn't mutate state here |
| 386 | Eigen::Quaterniond q(R); |
| 387 | KeyframeDump dump; |
| 388 | dump.id = kf->index; |
| 389 | dump.stamp = kf->time_stamp; |
| 390 | dump.qx = q.x(); dump.qy = q.y(); dump.qz = q.z(); dump.qw = q.w(); |
| 391 | dump.tx = T.x(); dump.ty = T.y(); dump.tz = T.z(); |
| 392 | out.push_back(dump); |
| 393 | } |
| 394 | } |