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

前言

相信大家在slam学习中，一定会遇到slam系统的性能评估问题。虽然有EVO这样的开源评估工具，我们也需要自己了解系统生成的trajectory.txt的含义，方便我们更好的理解相机的运行跟踪过程。

项目配置如下：

数据解读：

这是KITTI数据集 中的轨迹 (trajectory.txt)，通常用于视觉里程计（VO）或SLAM 的评估。文件格式通常是 12个浮点数 ，表示一个 4×4 变换矩阵（刚体变换），具体来说，每一行表示一个位姿（相机的姿态和位置），格式如下：

解析每列数据：

1. 前3×3部分（列1到列9）：旋转矩阵 RRR（描述相机的旋转）
2. 列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的相机运动理解。