Saving Key Frames about orb_slam2 HOT 4 CLOSED

void System::keyFrameTimeStamp_sim3(const string &timestampFile, const string &sim3File) {
  cout << endl << "Saving keyFrameTimeStamp_sim3 to " << timestampFile << " & " << sim3File << " ..." << endl;

  vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
  sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);

  // Transform all keyframes so that the first keyframe is at the origin.
  // After a loop closure the first keyframe might not be at the origin.
  ofstream f1;
  f1.open(timestampFile.c_str());
  f1 << fixed;

  ofstream f2;
  f2.open(sim3File.c_str());
  f2 << fixed;

  // For all keyframes
  for(size_t i=0; i<vpKFs.size(); i++) {
      KeyFrame* pKF = vpKFs[i];

      if(pKF->isBad())
          continue;

      // timestamp
      f1 << setprecision(6) << pKF->mTimeStamp << endl;

      // sim3
      cv::Mat pose = pKF->GetPose();

      f2 << setprecision(8) << pose.at<float>(0,0) << " " << pose.at<float>(0,1)  << " " << pose.at<float>(0,2) << " "  << pose.at<float>(0,3) << endl <<
                               pose.at<float>(1,0) << " " << pose.at<float>(1,1)  << " " << pose.at<float>(1,2) << " "  << pose.at<float>(1,3) << endl <<
                               pose.at<float>(2,0) << " " << pose.at<float>(2,1)  << " " << pose.at<float>(2,2) << " "  << pose.at<float>(2,3) << endl <<
                               pose.at<float>(3,0) << " " << pose.at<float>(3,1)  << " " << pose.at<float>(3,2) << " "  << pose.at<float>(3,3) << endl;
  }

  f1.close();
  f2.close();
  cout << endl << "timestamp & sim3 saved!" << endl;
}