《视觉SLAM十四讲》ch13 设计SLAM系统相机轨迹实现

前言

相信大家在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_wSE3 T_w_c = T_c_w.inverse();          // 计算世界坐标系到相机的变换 T_w_cEigen::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_wSE3 T_w_c = T_c_w.inverse();          // 计算世界坐标系到相机的变换 T_w_cEigen::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的相机运动理解。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.rhkb.cn/news/36884.html

如若内容造成侵权/违法违规/事实不符，请联系长河编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！