Pangolin 0.8 与 Sophus 1.22.4 实战：3D轨迹可视化与 ATE/RPE 误差计算

Pangolin 0.8与Sophus 1.22.4实战3D轨迹可视化与ATE/RPE误差计算在SLAM同步定位与地图构建系统的开发过程中轨迹的可视化与精度评估是验证算法有效性的关键环节。本文将基于Pangolin 0.8可视化库和Sophus 1.22.4李代数库构建一个完整的3D轨迹分析工具链涵盖从数据解析、轨迹绘制到定量评估的全流程。1. 环境配置与依赖安装在开始项目前需要确保系统已安装以下关键组件Eigen 3.3.4提供线性代数运算支持Pangolin 0.8实现3D可视化界面Sophus 1.22.4模板类版本处理李群/李代数转换fmt库提供字符串格式化功能安装步骤参考# 安装Eigen sudo apt-get install libeigen3-dev # 安装Pangolin依赖 sudo apt-get install libglew-dev # 编译安装Pangolin git clone https://github.com/stevenlovegrove/Pangolin.git cd Pangolin mkdir build cd build cmake .. make -j8 sudo make install # 安装fmt git clone https://github.com/fmtlib/fmt.git cd fmt mkdir build cd build cmake .. make -j8 sudo make install # 安装Sophus git clone https://github.com/strasdat/Sophus.git cd Sophus mkdir build cd build cmake .. make -j8 sudo make install注意Sophus 1.22.4需要C14及以上标准支持在CMakeLists.txt中需设置set(CMAKE_CXX_STANDARD 14)2. 轨迹数据解析模块我们首先实现一个通用的轨迹数据解析器支持读取标准格式的位姿文件。典型的轨迹文件每行包含时间戳和位姿信息平移向量旋转四元数timestamp tx ty tz qx qy qz qw对应的C解析代码如下#include vector #include fstream #include sophus/se3.hpp using TrajectoryType std::vectorSophus::SE3d, Eigen::aligned_allocatorSophus::SE3d; TrajectoryType ReadTrajectory(const std::string path) { TrajectoryType trajectory; std::ifstream fin(path); if (!fin) { std::cerr Trajectory file not found at path std::endl; return trajectory; } while (!fin.eof()) { double time, tx, ty, tz, qx, qy, qz, qw; fin time tx ty tz qx qy qz qw; Eigen::Quaterniond q(qw, qx, qy, qz); Eigen::Vector3d t(tx, ty, tz); trajectory.emplace_back(q, t); } return trajectory; }3. 3D轨迹可视化实现利用Pangolin库我们可以创建交互式的3D轨迹查看器。以下代码展示了如何绘制包含坐标轴的多条轨迹#include pangolin/pangolin.h void DrawTrajectory(const TrajectoryType poses, const std::string window_title Trajectory Viewer) { // 创建视窗并设置相机参数 pangolin::CreateWindowAndBind(window_title, 1024, 768); glEnable(GL_DEPTH_TEST); glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); pangolin::OpenGlRenderState s_cam( pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000), pangolin::ModelViewLookAt(0, -0.5, -2, 0, 0, 0, 0.0, -1.0, 0.0) ); pangolin::View d_cam pangolin::CreateDisplay() .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f/768.0f) .SetHandler(new pangolin::Handler3D(s_cam)); while (!pangolin::ShouldQuit()) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); d_cam.Activate(s_cam); glClearColor(1.0f, 1.0f, 1.0f, 1.0f); // 绘制坐标系 const float axis_length 0.1f; glLineWidth(3); glBegin(GL_LINES); glColor3f(1.0, 0.0, 0.0); // X轴红色 glVertex3f(0, 0, 0); glVertex3f(axis_length, 0, 0); glColor3f(0.0, 1.0, 0.0); // Y轴绿色 glVertex3f(0, 0, 0); glVertex3f(0, axis_length, 0); glColor3f(0.0, 0.0, 1.0); // Z轴蓝色 glVertex3f(0, 0, 0); glVertex3f(0, 0, axis_length); glEnd(); // 绘制轨迹连线 glLineWidth(2); glBegin(GL_LINES); glColor3f(0.0, 0.0, 0.0); // 轨迹线黑色 for (size_t i 0; i poses.size() - 1; i) { const auto p1 poses[i].translation(); const auto p2 poses[i1].translation(); glVertex3d(p1.x(), p1.y(), p1.z()); glVertex3d(p2.x(), p2.y(), p2.z()); } glEnd(); // 绘制每个位姿的坐标系 glLineWidth(1); for (const auto pose : poses) { Eigen::Vector3d Ow pose.translation(); Eigen::Vector3d Xw pose * (0.1 * Eigen::Vector3d::UnitX()); Eigen::Vector3d Yw pose * (0.1 * Eigen::Vector3d::UnitY()); Eigen::Vector3d Zw pose * (0.1 * Eigen::Vector3d::UnitZ()); glBegin(GL_LINES); glColor3f(1.0, 0.0, 0.0); glVertex3d(Ow.x(), Ow.y(), Ow.z()); glVertex3d(Xw.x(), Xw.y(), Xw.z()); glColor3f(0.0, 1.0, 0.0); glVertex3d(Ow.x(), Ow.y(), Ow.z()); glVertex3d(Yw.x(), Yw.y(), Yw.z()); glColor3f(0.0, 0.0, 1.0); glVertex3d(Ow.x(), Ow.y(), Ow.z()); glVertex3d(Zw.x(), Zw.y(), Zw.z()); glEnd(); } pangolin::FinishFrame(); usleep(5000); // 5ms延迟 } }4. 轨迹误差评估方法在SLAM系统中我们通常使用两种指标评估轨迹精度4.1 绝对轨迹误差ATEATE衡量估计轨迹与真实轨迹的整体对齐程度计算公式为$$ ATE_{all} \sqrt{\frac{1}{N}\sum_{i1}^N |\log(T_{gt,i}^{-1}T_{esti,i})^\vee|_2^2} $$实现代码如下double ComputeATE(const TrajectoryType gt, const TrajectoryType esti) { assert(gt.size() esti.size()); double error_sum 0.0; for (size_t i 0; i gt.size(); i) { Sophus::SE3d error_se3 gt[i].inverse() * esti[i]; double error error_se3.log().norm(); error_sum error * error; } return std::sqrt(error_sum / gt.size()); }4.2 相对位姿误差RPERPE评估固定时间间隔内的位姿变化误差分为旋转和平移分量$$ RPE_{trans} \sqrt{\frac{1}{N-\Delta t}\sum_{i1}^{N-\Delta t} |trans((T_{gt,i}^{-1}T_{gt,i\Delta t})^{-1}(T_{esti,i}^{-1}T_{esti,i\Delta t}))|_2^2} $$实现代码如下std::pairdouble, double ComputeRPE( const TrajectoryType gt, const TrajectoryType esti, int delta 1) { assert(gt.size() esti.size()); double trans_error_sum 0.0; double rot_error_sum 0.0; int count 0; for (size_t i 0; i gt.size() - delta; i) { Sophus::SE3d gt_rel gt[i].inverse() * gt[idelta]; Sophus::SE3d esti_rel esti[i].inverse() * esti[idelta]; Sophus::SE3d error_se3 gt_rel.inverse() * esti_rel; // 平移误差 double trans_error error_se3.translation().norm(); trans_error_sum trans_error * trans_error; // 旋转误差角度 double rot_error error_se3.so3().log().norm(); rot_error_sum rot_error * rot_error; count; } return { std::sqrt(trans_error_sum / count), std::sqrt(rot_error_sum / count) }; }5. 完整项目集成将上述模块整合为完整的评估流程int main(int argc, char** argv) { // 读取轨迹数据 TrajectoryType gt_traj ReadTrajectory(groundtruth.txt); TrajectoryType esti_traj ReadTrajectory(estimated.txt); if (gt_traj.empty() || esti_traj.empty()) { std::cerr Failed to load trajectory files! std::endl; return -1; } // 计算评估指标 double ate ComputeATE(gt_traj, esti_traj); auto [rpe_trans, rpe_rot] ComputeRPE(gt_traj, esti_traj); std::cout Evaluation Results: std::endl; std::cout ATE: ate m std::endl; std::cout RPE (trans): rpe_trans m std::endl; std::cout RPE (rot): rpe_rot rad std::endl; // 可视化轨迹对比 DrawTrajectory(gt_traj, Ground Truth Trajectory); // 可以同时显示两条轨迹做对比 pangolin::CreateWindowAndBind(Trajectory Comparison, 1024, 768); glEnable(GL_DEPTH_TEST); pangolin::OpenGlRenderState s_cam( pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000), pangolin::ModelViewLookAt(0, -0.5, -2, 0, 0, 0, 0.0, -1.0, 0.0) ); pangolin::View d_cam pangolin::CreateDisplay() .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f/768.0f) .SetHandler(new pangolin::Handler3D(s_cam)); while (!pangolin::ShouldQuit()) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); d_cam.Activate(s_cam); glClearColor(1.0f, 1.0f, 1.0f, 1.0f); // 绘制真实轨迹蓝色 glColor3f(0.0f, 0.0f, 1.0f); glBegin(GL_LINE_STRIP); for (const auto pose : gt_traj) { Eigen::Vector3d p pose.translation(); glVertex3d(p.x(), p.y(), p.z()); } glEnd(); // 绘制估计轨迹红色 glColor3f(1.0f, 0.0f, 0.0f); glBegin(GL_LINE_STRIP); for (const auto pose : esti_traj) { Eigen::Vector3d p pose.translation(); glVertex3d(p.x(), p.y(), p.z()); } glEnd(); pangolin::FinishFrame(); usleep(5000); } return 0; }6. CMake工程配置完整的CMakeLists.txt配置示例cmake_minimum_required(VERSION 3.12) project(TrajectoryEvaluation) set(CMAKE_CXX_STANDARD 14) find_package(Eigen3 REQUIRED) find_package(Pangolin REQUIRED) find_package(Sophus REQUIRED) find_package(fmt REQUIRED) add_executable(trajectory_evaluation src/main.cpp src/trajectory_io.cpp src/visualization.cpp src/evaluation.cpp ) target_include_directories(trajectory_evaluation PRIVATE ${EIGEN3_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS} ${Sophus_INCLUDE_DIRS} ) target_link_libraries(trajectory_evaluation ${Pangolin_LIBRARIES} ${Sophus_LIBRARIES} fmt::fmt )7. 高级功能扩展7.1 轨迹对齐Umeyama算法在实际评估前通常需要先对齐估计轨迹与真实轨迹Sophus::SE3d AlignTrajectories( const TrajectoryType gt, TrajectoryType esti) { assert(gt.size() esti.size()); // 计算质心 Eigen::Vector3d gt_center(0, 0, 0); Eigen::Vector3d esti_center(0, 0, 0); for (size_t i 0; i gt.size(); i) { gt_center gt[i].translation(); esti_center esti[i].translation(); } gt_center / gt.size(); esti_center / esti.size(); // 计算去质心坐标 Eigen::Matrix3d W Eigen::Matrix3d::Zero(); for (size_t i 0; i gt.size(); i) { W (gt[i].translation() - gt_center) * (esti[i].translation() - esti_center).transpose(); } // SVD分解求最优旋转 Eigen::JacobiSVDEigen::Matrix3d svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3d R svd.matrixU() * svd.matrixV().transpose(); if (R.determinant() 0) { R.col(2) * -1; } // 计算平移 Eigen::Vector3d t gt_center - R * esti_center; // 应用变换到估计轨迹 Sophus::SE3d T(R, t); for (auto pose : esti) { pose T * pose; } return T; }7.2 误差随时间变化分析通过分析误差随时间的变化可以识别系统在哪些时间段表现不佳void AnalyzeErrorOverTime( const TrajectoryType gt, const TrajectoryType esti, const std::vectordouble timestamps, const std::string output_file) { std::ofstream fout(output_file); fout time,ate_trans,ate_rot\n; for (size_t i 0; i gt.size(); i) { Sophus::SE3d error gt[i].inverse() * esti[i]; double trans_error error.translation().norm(); double rot_error error.so3().log().norm(); fout timestamps[i] , trans_error , rot_error \n; } }7.3 轨迹分段统计将轨迹按距离或时间分段计算各段的误差统计量struct SegmentStats { double max_error; double min_error; double mean_error; double std_dev; }; std::mapint, SegmentStats ComputeSegmentErrors( const TrajectoryType gt, const TrajectoryType esti, int num_segments 10) { std::mapint, SegmentStats results; int points_per_segment gt.size() / num_segments; for (int seg 0; seg num_segments; seg) { int start seg * points_per_segment; int end (seg num_segments-1) ? gt.size() : (seg1)*points_per_segment; std::vectordouble errors; for (int i start; i end; i) { Sophus::SE3d error gt[i].inverse() * esti[i]; errors.push_back(error.translation().norm()); } // 计算统计量 auto [min_it, max_it] std::minmax_element(errors.begin(), errors.end()); double sum std::accumulate(errors.begin(), errors.end(), 0.0); double mean sum / errors.size(); double sq_sum std::inner_product( errors.begin(), errors.end(), errors.begin(), 0.0); double std_dev std::sqrt(sq_sum / errors.size() - mean * mean); results[seg] { *max_it, *min_it, mean, std_dev }; } return results; }8. 性能优化技巧在处理大规模轨迹数据时可采用以下优化策略数据降采样对于可视化不需要每帧都绘制void DownsampleTrajectory(TrajectoryType traj, int factor) { TrajectoryType downsampled; for (size_t i 0; i traj.size(); i factor) { downsampled.push_back(traj[i]); } traj std::move(downsampled); }多线程计算将误差计算任务分配到多个线程#include thread #include mutex std::mutex mtx; void ParallelATECompute( const TrajectoryType gt, const TrajectoryType esti, size_t start, size_t end, double partial_sum) { double local_sum 0.0; for (size_t i start; i end; i) { Sophus::SE3d error gt[i].inverse() * esti[i]; local_sum error.log().squaredNorm(); } std::lock_guardstd::mutex lock(mtx); partial_sum local_sum; } double ComputeATEParallel(const TrajectoryType gt, const TrajectoryType esti) { const int num_threads std::thread::hardware_concurrency(); std::vectorstd::thread threads; double total_sum 0.0; size_t chunk_size gt.size() / num_threads; for (int i 0; i num_threads; i) { size_t start i * chunk_size; size_t end (i num_threads-1) ? gt.size() : (i1)*chunk_size; threads.emplace_back(ParallelATECompute, std::cref(gt), std::cref(esti), start, end, std::ref(total_sum)); } for (auto t : threads) t.join(); return std::sqrt(total_sum / gt.size()); }GPU加速使用CUDA加速矩阵运算需额外配置9. 常见问题排查在实际使用中可能会遇到以下问题及解决方案问题现象可能原因解决方案Pangolin窗口无法显示缺少OpenGL驱动安装显卡驱动sudo apt install mesa-utilsSophus编译错误头文件版本不匹配确保使用模板类版本的Sophus头文件轨迹显示异常坐标系不一致检查数据源坐标系定义必要时进行转换评估指标异常大轨迹未对齐调用AlignTrajectories进行预处理内存占用过高轨迹数据量过大采用降采样或流式处理策略10. 实际应用案例将本工具应用于KITTI数据集评估的典型流程数据准备将KITTI的GPS/IMU数据转换为SE(3)轨迹轨迹对齐使用Umeyama算法对齐估计轨迹与真值评估计算运行ATE/RPE计算得到定量指标可视化分析通过3D视图检查轨迹偏差分布结果导出生成评估报告和误差曲线图void EvaluateKITTITrajectory() { // 读取KITTI格式轨迹 TrajectoryType gt ReadKITTIOdometry(00.txt); TrajectoryType esti ReadLOAMOutput(loam_poses.txt); // 轨迹预处理 DownsampleTrajectory(gt, 5); DownsampleTrajectory(esti, 5); Sophus::SE3d T_align AlignTrajectories(gt, esti); // 计算评估指标 double ate ComputeATE(gt, esti); auto [rpe_trans, rpe_rot] ComputeRPE(gt, esti, 10); // delta10帧 // 可视化 DrawTrajectoryComparison(gt, esti); // 输出结果 std::cout KITTI Sequence 00 Evaluation: std::endl; std::cout Alignment Transform:\n T_align.matrix() std::endl; std::cout ATE: ate m std::endl; std::cout RPE (trans): rpe_trans m std::endl; std::cout RPE (rot): rpe_rot rad std::endl; // 保存误差分析 AnalyzeErrorOverTime(gt, esti, LoadTimestamps(times.txt), error_analysis.csv); }