void System::SaveTrajectoryTUM(const string &filename)
{
cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
if(mSensor==MONOCULAR)
{
cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl;
return;
}
vector<KeyFrame *> vpKFs = mpAtlas->GetAllKeyFrames(); //获取地图集中所有的关键帧
vector<KeyFrame*> vpKFs = mpAtlas->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.
cv::Mat Two = vpKFs[0]->GetPoseInverse();
ofstream f;
f.open(filename.c_str());
f << fixed;
// Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
// We need to get first the keyframe pose and then concatenate the relative transformation.
// Frames not localized (tracking failure) are not saved.
// For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
// which is true when tracking failed (lbL).
//每一帧都有一个参考关键帧,相对于参考关键帧有一个相对位姿,所以每一帧的位姿是按相对参考关键帧位姿保存的
//这样保存的目的在于:关键帧位姿在localmapping中是不断调整的,所以认为更加准确,如果直接保存
//Frame的位姿,那就没那么准确
list<ORB_SLAM3::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();//参考关键帧
list<double>::iterator lT = mpTracker->mlFrameTimes.begin(); //时间戳
list<bool>::iterator lbL = mpTracker->mlbLost.begin(); //是否跟踪失败
for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(), //相对参考关键帧的位姿
lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
{
if(*lbL)
continue;
KeyFrame* pKF = *lRit;
cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
// If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.
while(pKF->isBad())//如果参考关键帧是坏的(可能是检查冗余时被剔除了),则用其父帧,并记录父帧到原参考关键帧的位姿变换
{
Trw = Trw*pKF->mTcp;
pKF = pKF->GetParent();
}
Trw = Trw * pKF->GetPose() * Two; //参考关键帧的位姿
cv::Mat Tcw = (*lit) * Trw; //参考关键帧到当前帧的相对位姿*参考关键帧位姿=当前帧位姿
cv::Mat Rwc = Tcw.rowRange(0, 3).colRange(0, 3).t(); //相机到世界的旋转
cv::Mat twc = -Rwc * Tcw.rowRange(0, 3).col(3); //相机到世界的平移
vector<float> q = Converter::toQuaternion(Rwc); //以四元数的形式输出
f << setprecision(6) << *lT << " " << setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
}
f.close();
// cout << endl << "trajectory saved!" << endl;
}
ORB_SLAM3 SaveTrajectoryTUM 注释
最新推荐文章于 2022-09-25 14:12:52 发布