手把手带你死磕ORBSLAM3源代码(十四）System.cc void System::SaveTrajectoryTUM类代码分析
目录
一.前言
这部分代码的基本逻辑是：
- 检查传感器是否为单目传感器，如果是，则不能使用该函数保存轨迹。
- 获取所有的关键帧，并按照它们的ID进行排序。
- 将所有的关键帧进行变换，使得第一个关键帧位于原点。
- 遍历所有的帧，对于每一帧，如果它没有被丢失，就计算它的位姿并保存到文件中。位姿是相对于第一个关键帧的，并且会考虑到回环闭合的情况。
二.代码
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);first和last是指向要排序的元素范围的迭代器。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 == it2和it1 != it2: 比较两个迭代器是否相等或不相等。
注意点
- 由于
std::list是双向链表，所以它的迭代器不支持随机访问。你不能使用it + n或it - 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函数仅对输出流有效，不会改变浮点数的内部表示或精度。它只影响在输出流中显示的浮点数的格式。
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：veading@qq.com进行投诉反馈，一经查实，立即删除！