手把手带你死磕ORBSLAM3源代码(十四)System.cc void System::SaveTrajectoryTUM类代码分析

2023-12-27 13:16:32

目录

一.前言

二.代码

2.1完整代码

2.2 sort函数

函数原型

参数

使用示例

性能特点

2.3 ofstream函数

基本用法

文件打开模式

2.4 list< >::iterator介绍

基本用法

迭代器操作

注意点

2.5 Sophus::SE3f介绍

2.6 Eigen::Vector3f介绍

2.7 Eigen::Quaternionf介绍

2.8 setprecision介绍


一.前言

这部分代码的基本逻辑是:

  1. 检查传感器是否为单目传感器,如果是,则不能使用该函数保存轨迹。
  2. 获取所有的关键帧,并按照它们的ID进行排序
  3. 将所有的关键帧进行变换,使得第一个关键帧位于原点
  4. 遍历所有的帧,对于每一帧,如果它没有被丢失,就计算它的位姿并保存到文件中。位姿是相对于第一个关键帧的,并且会考虑到回环闭合的情况

二.代码

2.1完整代码

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();
    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.
    Sophus::SE3f 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).
    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<Sophus::SE3f>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
        lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
    {
        if(*lbL)
            continue;

        KeyFrame* pKF = *lRit;

        Sophus::SE3f Trw;

        // 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;

        Sophus::SE3f Tcw = (*lit) * Trw;
        Sophus::SE3f Twc = Tcw.inverse();

        Eigen::Vector3f twc = Twc.translation();
        Eigen::Quaternionf q = Twc.unit_quaternion();

        f << setprecision(6) << *lT << " " <<  setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
    }
    f.close();
    // cout << endl << "trajectory saved!" << endl;
}

2.2 sort函数

C++的sort函数是一个非常强大且通用的排序算法,它属于C++标准库中的<algorithm>头文件。sort函数可以对容器(如数组、向量等)中的元素进行排序。以下是关于sort函数的一些基本介绍:

函数原型

template <class RandomIt>  
void sort(RandomIt first, RandomIt last);  
  
template <class RandomIt, class Compare>  
void sort(RandomIt first, RandomIt last, Compare comp);
  • firstlast 是指向要排序的元素范围的迭代器。
  • comp 是一个可选的比较函数,用于定义排序的准则。

参数

  • first: 指向要排序范围的第一个元素的迭代器。
  • last: 指向要排序范围的最后一个元素之后位置的迭代器。
  • comp: 可选的二元比较函数,接受两个参数并返回一个bool值。如果第一个参数应该在排序后出现在第二个参数之前,则返回true。如果不提供此函数,将使用元素类型的<操作符进行比较。

使用示例

下面是一个使用sort函数对整数向量进行排序的简单示例:

#include <iostream>  
#include <vector>  
#include <algorithm> // 包含sort函数  
  
int main() {  
    // 创建一个整数向量  
    std::vector<int> numbers = {5, 2, 9, 1, 5, 6};  
      
    // 使用sort函数对向量进行排序  
    std::sort(numbers.begin(), numbers.end());  
      
    // 输出排序后的向量  
    for (int num : numbers) {  
        std::cout << num << " ";  
    }  
    std::cout << std::endl; // 输出: 1 2 5 5 6 9  
      
    return 0;  
}

性能特点

  • sort函数的平均时间复杂度为O(N log N),其中N是要排序元素的数量。这使得它在处理大量数据时非常高效。
  • sort函数使用了称为快速排序(Quick Sort)的内部算法,但也结合了其他算法(如归并排序和插入排序)来优化特定情况下的性能。
  • sort函数是稳定的,这意味着相等的元素在排序后将保持其原始顺序。
  • 由于其通用性,sort函数可以处理任何类型的元素,只要这些元素支持比较操作。这使得它在C++编程中非常有用和灵活。

2.3 ofstream函数

C++的ofstream(Output File Stream)是C++标准库中的一个类,用于创建、打开和写入文件。它是C++文件I/O(输入/输出)机制的一部分,定义在<fstream>头文件中。ofstream提供了一种方便的方式来将数据写入文件。

基本用法

要使用ofstream,你需要包含<fstream>头文件,然后创建一个ofstream对象,指定你想要打开或创建的文件名,以及可选的文件打开模式。下面是一个简单的例子:

#include <fstream>  
#include <iostream>  
  
int main() {  
    // 创建一个ofstream对象并打开文件  
    std::ofstream outfile("example.txt");  
      
    // 检查文件是否成功打开  
    if (!outfile) {  
        std::cerr << "无法打开文件" << std::endl;  
        return 1;  
    }  
      
    // 向文件写入数据  
    outfile << "Hello, World!" << std::endl;  
      
    // 关闭文件  
    outfile.close();  
      
    return 0;  
}

??? 在这个例子中,我们创建了一个名为example.txt的文件,并向其中写入了字符串"Hello, World!"。如果文件已经存在,它的内容将被覆盖;如果文件不存在,它将被创建。

文件打开模式

ofstream的构造函数接受一个可选的第二个参数,用于指定文件打开模式。以下是一些常用的文件打开模式:

  • ios::out: 用于输出操作(写入文件)。这是默认的模式。
  • ios::app: 在文件末尾追加数据,而不是覆盖现有内容。
  • ios::trunc: 截断文件,删除其内容。这是默认行为(当不使用ios::app时)。
  • ios::binary: 以二进制模式打开文件(适用于非文本数据)。

这些模式可以通过按位或运算符(|)组合在一起使用。例如:

std::ofstream outfile("example.txt", std::ios::app | std::ios::binary);

2.4 list< >::iterator介绍

??? list<>::iterator 是 C++ 标准库 std::list 容器的一个内部类型,用于表示指向 list 中元素的迭代器。std::list 是一个双向链表,因此它的迭代器支持前向和后向遍历。

基本用法

你可以使用 begin()end() 成员函数来获取 list 的起始和结束迭代器。注意,end() 返回的迭代器指向列表的“尾后位置”,即最后一个元素之后的位置,而不是指向最后一个元素。

#include <iostream>  
#include <list>  
  
int main() {  
    std::list<int> myList = {1, 2, 3, 4, 5};  
      
    for (std::list<int>::iterator it = myList.begin(); it != myList.end(); ++it) {  
        std::cout << *it << " ";  
    }  
      
    return 0;  
}

这个示例将输出:1 2 3 4 5

迭代器操作

list<>::iterator 支持以下基本操作:

  • *it: 解引用迭代器,获取它指向的元素的值。
  • it->member: 如果列表的元素是对象,使用这个箭头运算符来访问对象的成员。
  • ++it: 前向移动迭代器到下一个元素。
  • --it: 后向移动迭代器到上一个元素。
  • it1 == it2it1 != it2: 比较两个迭代器是否相等或不相等。

注意点

  • 由于 std::list 是双向链表,所以它的迭代器不支持随机访问。你不能使用 it + nit - n 这样的操作来跳跃式地移动迭代器。
  • 使用迭代器时,要确保它指向的是有效的元素。不要解引用 end() 返回的迭代器,因为它不指向任何元素。
  • 当使用迭代器遍历容器时,如果容器在迭代过程中被修改(例如添加或删除元素),那么迭代器的有效性可能会受到影响。在这种情况下,最好使用返回的迭代器来继续遍历,或者避免在迭代过程中修改容器。

2.5 Sophus::SE3f介绍

??? Sophus::SE3f是一个用于表示3D刚体变换的李群SE(3)的浮点版本。它包含了一个旋转部分(SO(3))和一个平移部分(R^3)。Sophus库是一个用于处理李群和李代数的C++库,它提供了对SO(3)、SE(3)等常见李群的支持,以及相应的李代数so(3)、se(3)等。Sophus::SE3f类可以用于执行刚体变换的操作,例如旋转和平移,以及相关的数学运算。

2.6 Eigen::Vector3f介绍

??? Eigen::Vector3f是Eigen库中的一个类型定义,表示一个三维向量。在Eigen中,向量只是一种特殊形式的矩阵,有一行或者一列。大多数情况下,一列比较多,这样的向量也被称为列向量。Eigen::Vector3f实际上是Eigen::Matrix<float, 3, 1>的别名,表示这是一个3x1的浮点数矩阵。其中,“f”表示单精度浮点数。

2.7 Eigen::Quaternionf介绍

Eigen::Quaternionf是Eigen库中的一个类,用于表示四元数。四元数是一种扩展的复数,用于表示和操作3D空间中的旋转。相比于其他表示旋转的方式,如欧拉角和旋转矩阵,四元数具有一些优势,如避免万向锁问题,提供更平滑的插值等。

Eigen::Quaternionf类中的“f”表示该四元数的元素是浮点数(float)。一个Eigen::Quaternionf对象包含四个浮点数元素:一个实部和三个虚部。这些元素可以用来表示3D空间中的一个旋转。

Eigen::Quaternionf类提供了许多方法和运算符,用于四元数的各种操作,如乘法、共轭、求逆、归一化等。此外,Eigen库还提供了将四元数与3D向量和旋转矩阵之间进行转换的功能。

总的来说,Eigen::Quaternionf是一个功能强大的类,用于在3D图形、机器人学、物理模拟等领域中表示和操作旋转。

2.8 setprecision介绍

在C++中,setprecision是一个用于控制浮点数输出精度的操作符。它是C++标准库 <iomanip> 头文件中的一个函数。

setprecision函数用于设置输出流中浮点数的精度,即小数点后的位数或有效数字的数量。它接受一个整数参数,表示所需的精度级别。

下面是使用setprecision的示例代码:

#include <iostream>  
#include <iomanip> // 包含setprecision的头文件  
  
int main() {  
    double pi = 3.141592653589793;  
      
    std::cout << "默认精度:" << pi << std::endl;  
    std::cout << "设置精度为5:" << std::setprecision(5) << pi << std::endl;  
    std::cout << "设置精度为10:" << std::setprecision(10) << pi << std::endl;  
      
    return 0;  
}

输出结果:

默认精度:3.14159  
设置精度为5:3.1416  
设置精度为10:3.141592654

在上面的示例中,我们首先打印了默认情况下的浮点数pi的值。然后,我们使用setprecision(5)将精度设置为5,并打印了结果。最后,我们使用setprecision(10)将精度设置为10,并再次打印了结果。可以看到,setprecision函数成功地控制了浮点数输出的精度。

需要注意的是,setprecision函数仅对输出流有效,不会改变浮点数的内部表示或精度。它只影响在输出流中显示的浮点数的格式。

文章来源:https://blog.csdn.net/anananajiushiwo/article/details/135240187
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。