前言
相信大家在slam学习中，一定会遇到slam系统的性能评估问题。虽然有EVO这样的开源评估工具，我们也需要自己了解系统生成的trajectory.txt的含义，方便我们更好的理解相机的运行跟踪过程。
项目配置如下：
数据解读：
这是KITTI数据集中的轨迹 (trajectory.txt)，通常用于视觉里程计（VO）或SLAM的评估。文件格式通常是 12个浮点数，表示一个 4×4 变换矩阵（刚体变换），具体来说，每一行表示一个位姿（相机的姿态和位置），格式如下：
解析每列数据：
- 前3×3部分（列1到列9）：旋转矩阵 RRR（描述相机的旋转）
- 列10到列12：平移向量 ttt（描述相机的位移）
项目解读：
run_kitti_stereo.cpp的vo->Init()初始化前段可视化模块viewer_，后端backend_，地图map_等slam子系统。
进入主线程的特征追踪：
void VisualOdometry::Run() {
while (1) {
LOG(INFO) << "VO is running";
if (Step() == false) {
// 退出，并保存轨迹
frontend_->SaveTrajectory("/home/xulei/下载/slambook2/ch13/trajectory.txt");
break;
}
}
backend_->Stop();
viewer_->Close();
LOG(INFO) << "VO exit";
}bool VisualOdometry::Step() {
if (dataset_->current_image_index_ > 3681) { //判断避免异常退出,数据集大小。
return false;
}
Frame::Ptr new_frame = dataset_->NextFrame();
if (new_frame == nullptr) return false;
auto t1 = std::chrono::steady_clock::now();
bool success = frontend_->AddFrame(new_frame);
auto t2 = std::chrono::steady_clock::now();
auto time_used =
std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
LOG(INFO) << "VO cost time: " << time_used.count() << " seconds.";
return success;
}
} // namespace myslam其中，nextframe（）方法是读取数据集，并进行当前帧的创建。
Frame::Ptr new_frame = dataset_->NextFrame();AddFrame(new_frame)方法是进入特征提取与追踪线程。
bool success = frontend_->AddFrame(new_frame);其中，很重要的一点是跟踪的质量评估：
bool Frontend::AddFrame(myslam::Frame::Ptr frame) {
current_frame_ = frame;
switch (status_) {
case FrontendStatus::INITING:
StereoInit();
break;
case FrontendStatus::TRACKING_GOOD:
case FrontendStatus::TRACKING_BAD:
Track();
break;
case FrontendStatus::LOST:
Reset();
break;
}
// 让线程暂停 0.5 秒
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
last_frame_ = current_frame_;
return true;
}前端的工作大多在Frontend类中完成。
该部分也是我们代码改进，并进一步实现轨迹的保存工作。
注意，我们这里不考虑关键帧的处理，即后端的优化的实时的影响，直接在位姿计算完成就推算世界坐标下的轨迹。
首先，在frontend.h类中，加入方法，与轨迹变量。
void SaveTrajectory(const std::string &filename); // 轨迹保存std::vector<Eigen::Matrix<double, 3, 4>> trajectory_;具体在.cpp文件实现。
找到StereoInit()，Track()函数，将函数替换为：（这里代码比较冗余，也可以封装成一个方法调用）
bool Frontend::StereoInit() {
// **转换到世界坐标系，并存储 3×4 变换矩阵**
SE3 T_c_w = current_frame_->Pose(); // 获取相机位姿 T_c_w
SE3 T_w_c = T_c_w.inverse(); // 计算世界坐标系到相机的变换 T_w_c
Eigen::Matrix3d R_w_c = T_w_c.rotationMatrix().matrix(); // 获取旋转矩阵 (3×3)
Eigen::Vector3d t_w_c = T_w_c.translation(); // 获取平移向量 (3×1)
// 记录 3×4 矩阵
Eigen::Matrix<double, 3, 4> world_pose;
world_pose.block<3,3>(0,0) = R_w_c; // 旋转部分
world_pose.block<3,1>(0,3) = t_w_c; // 平移部分
// **记录轨迹**
trajectory_.push_back(world_pose);
int num_features_left = DetectFeatures();
int num_coor_features = FindFeaturesInRight();
if (num_coor_features < num_features_init_) {
return false;
}
bool build_map_success = BuildInitMap();
if (build_map_success) {
status_ = FrontendStatus::TRACKING_GOOD;
if (viewer_) {
viewer_->AddCurrentFrame(current_frame_);
viewer_->UpdateMap();
}
return true;
}
return false;
}ool Frontend::Track() {
if (last_frame_) {
current_frame_->SetPose(relative_motion_ * last_frame_->Pose());
}
int num_track_last = TrackLastFrame();
tracking_inliers_ = EstimateCurrentPose();
printf("num_track_last = %d", tracking_inliers_);
if (tracking_inliers_ > num_features_tracking_) {
status_ = FrontendStatus::TRACKING_GOOD;
} else if (tracking_inliers_ > num_features_tracking_bad_) {
status_ = FrontendStatus::TRACKING_BAD;
} else {
status_ = FrontendStatus::LOST;
}
InsertKeyframe();
relative_motion_ = current_frame_->Pose() * last_frame_->Pose().inverse();
// **转换到世界坐标系，并存储 3×4 变换矩阵**
SE3 T_c_w = current_frame_->Pose(); // 获取相机位姿 T_c_w
SE3 T_w_c = T_c_w.inverse(); // 计算世界坐标系到相机的变换 T_w_c
Eigen::Matrix3d R_w_c = T_w_c.rotationMatrix().matrix(); // 获取旋转矩阵 (3×3)
Eigen::Vector3d t_w_c = T_w_c.translation(); // 获取平移向量 (3×1)
// 记录 3×4 矩阵
Eigen::Matrix<double, 3, 4> world_pose;
world_pose.block<3,3>(0,0) = R_w_c; // 旋转部分
world_pose.block<3,1>(0,3) = t_w_c; // 平移部分
// **记录轨迹**
trajectory_.push_back(world_pose);
// // **调试输出**
// std::cout << std::fixed << std::setprecision(9);
// std::cout << "Frame " << trajectory_.size()
// << " | R_w_c: \n" << R_w_c
// << "\n t_w_c: " << t_w_c.transpose() << std::endl;
if (viewer_) viewer_->AddCurrentFrame(current_frame_);
return true;
}最后实现其SaveTrajector()函数，保存.txt文件：
void Frontend::SaveTrajectory(const std::string &filename) {
std::ofstream file(filename);
if (!file.is_open()) {
std::cerr << "Error opening trajectory file: " << filename << std::endl;
return;
}
file << std::fixed << std::setprecision(9); // 设定输出精度
for (const auto &pose : trajectory_) {
// 提取旋转和平移部分（假设 trajectory_ 存储的是 3×4 矩阵）
Eigen::Matrix3d R_w_c = pose.block<3,3>(0,0);
Eigen::Vector3d t_w_c = pose.block<3,1>(0,3);
// 写入文件
file << R_w_c(0,0) << " " << R_w_c(0,1) << " " << R_w_c(0,2) << " " << t_w_c(0) << " "
<< R_w_c(1,0) << " " << R_w_c(1,1) << " " << R_w_c(1,2) << " " << t_w_c(1) << " "
<< R_w_c(2,0) << " " << R_w_c(2,1) << " " << R_w_c(2,2) << " " << t_w_c(2) << std::endl;
}
file.close();
std::cout << "Trajectory saved to " << filename << std::endl;
}
重新编译代码，运行：
最后得到，格式为：
0.999990331 -0.002488019 -0.003625839 0.001951625 0.002480646 0.999994849 -0.002036517 -0.007563952 0.003630888 0.002027503 0.999991353 0.681278434
0.999971701 -0.001201005 -0.007426599 -0.014921545 0.001173216 0.999992299 -0.003745129 -0.014889465 0.007431040 0.003736310 0.999965409 1.369789353
0.999938793 -0.000961412 -0.011022030 -0.041936008 0.000902450 0.999985264 -0.005353181 -0.016047572 0.011027014 0.005342906 0.999924926 2.069745033
0.999877013 0.000796693 -0.015662799 -0.064529202 -0.000890743 0.999981611 -0.005998610 -0.025888373 0.015657732 0.006011824 0.999859337 2.795177413
0.999790482 -0.000408300 -0.020465240 -0.073989152 0.000286856 0.999982336 -0.005936741 -0.036340712 0.020467303 0.005929627 0.999772939 3.531565772
0.999620212 0.011536263 -0.025026921 -0.084931933 -0.011736120 0.999900288 -0.007853525 -0.060457271 0.024933825 0.008144261 0.999655928 4.288284960
0.999448852 0.014043954 -0.030079214 -0.119917101 -0.014297214 0.999863991 -0.008221300 -0.075260961 0.029959663 0.008646818 0.999513707 5.057364575
0.999297794 0.014574258 -0.034518265 -0.151109538 -0.014807489 0.999869166 -0.006510771 -0.087394672 0.034418859 0.007017328 0.999382859 5.840754611
0.999154885 0.014253942 -0.038553105 -0.200260214 -0.014383673 0.999891776 -0.003089717 -0.114111359 0.038504892 0.003641641 0.999251776 6.608571213
0.999029138 0.013669141 -0.041880029 -0.238672274 -0.013693447 0.999906197 -0.000293535 -0.124407472 0.041872088 0.000866732 0.999122604 7.415410187
0.998883339 0.015144070 -0.044751887 -0.279161319 -0.015184780 0.999884542 -0.000569862 -0.138168200 0.044738090 0.001248774 0.998997970 8.233449862
0.998772936 0.014903271 -0.047228336 -0.316118436 -0.014963441 0.999887618 -0.000920710 -0.143100781 0.047209307 0.001626278 0.998883695 9.053884639
0.998375407 0.030955991 -0.047835896 -0.362929685 -0.031132074 0.999510955 -0.002940145 -0.168459719 0.047721487 0.004424599 0.998850881 9.881833862
0.998340888 0.030504872 -0.048835694 -0.420649005 -0.030656826 0.999527170 -0.002365388 -0.167085051 0.048740448 0.003858611 0.998804025
进一步用evo评估该系统：
evo_traj kitti trajectory.txt --ref=02.txt -p --plot_mode xz平面误差：
相机的xyz轴的误差：
相机的旋转：
总结
这样实现的方法可能比较取巧，没有考虑后续的BA优化，后续也会持续的优化该系统，希望能给大家提供一些项目的改进灵感与slam的相机运动理解。
