一、主函数——okvis_app_synchronous.cpp
1. 引用库、函数
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <okvis/VioParametersReader.hpp>
#include <okvis/ThreadedKFVio.hpp>
#include <boost/filesystem.hpp>2. 包含的两部分
class PoseViewer// PoseViewer类的主要作用是画出小黑窗口，实时显示运动轨迹
// this is just a workbench. 这里的大部分内容都会用到前端类
int main(int argc, char **argv) //argc 是外部命令参数的个数，argv[] 存放各参数的内容2.1 main函数流程
系统入口在ThreadedKFVio类的构造函数中（ ThreadedKFVio类继承自VioInterface，该类的定义在okvis_multisensor_processing文件夹)
int main(int argc, char **argv) //argc 是外部命令参数的个数，argv[] 存放各参数的内容
{
// 运行时，窗口输入： argv[0]运行程序 argv[1]配置文件路径 argv[2]数据集路径
//1. 读入argv[0]，运行程序。出错时输出警告信息
google::InitGoogleLogging(argv[0]);
FLAGS_stderrthreshold = 0; // INFO: 0, WARNING: 1, ERROR: 2, FATAL: 3
FLAGS_colorlogtostderr = 1;
if (argc != 3 && argc != 4) {
LOG(ERROR)<< // 出错，输出警告信息
"Usage: ./" << argv[0] << " configuration-yaml-file dataset-folder [skip-first-seconds]";
return -1;
}
okvis::Duration deltaT(0.0); // 持续时间 delT(0.0)默认值为0.0秒
if (argc == 4) { // 也可以读入第四个参数argv[3]
deltaT = okvis::Duration(atof(argv[3]));
}
//2.读入arg[1]路径下的配置文件
//2.1 将argv[1]得到的配置文件名赋给configFilename
std::string configFilename(argv[1]);
//2.2 读取configFilename文件下的配置文件，将所有参数和配置文件组合起来
okvis::/VioParametersReader vio_parameters_reader(configFilename);
//2.4 得到parameters下的所有参数设置
okvis::VioParameters parameters;
vio_parameters_reader.getParameters(parameters);
//3. 得到配置文件里的参数设置parameter后，开启系统(okvis_estimator)
//系统入口在ThreadedKFVio类的构造函数中（ ThreadedKFVio类继承自VioInterface，该类的定义在okvis_multisensor_processing文件夹）
// 定义一个ThreadedKFVio类对象okvis_estimator，并且将获得的参数设置传入（一旦使用该类，系统在该类的构造函数中将进行初始化，系统正式开启！
okvis::ThreadedKFVio okvis_estimator(parameters);
// 3.1 定义窗口显示类（显示运动轨迹窗口） PoseViewer poseViewer；//定义了一个poseViewer类的对象poseViewer
PoseViewer poseViewer;
// 3.2 调用对象okvis_estimator的全状态回调函数 setFullStateCallback（）；
okvis_estimator.setFullStateCallback(
std::bind(&PoseViewer::publishFullStateAsCallback, &poseViewer,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4));
// 3.3 调用对象okvis_estimator的阻塞设置函数 okvis_estimator.setBlocking(true);
// 设置阻塞变量，addmeasurement()函数是否应立即返回（阻塞= false），或者仅在处理完成时返回
okvis_estimator.setBlocking(true); // 使IMU与图像帧尽可能对齐
// 4 读入argv[2]路径下的IMU数据
// 4.1 the folder path
std::string path(argv[2]); // "path"被定义为string类型，并将argv[2]得到的值赋给"path"
// 4.2 通过配置文件得到相机数量numCameras
const unsigned int numCameras = parameters.nCameraSystem.numCameras();
// 4.3 open the IMU file，打开IMU文件，读入IMU数据，“imu_file”是类"ifstream"的对象，作用是读入数据
std::string line;
std::ifstream imu_file(path + "/imu0/data.csv"); // 读取IMU的csv文件的行
// 如果没有发现IMU文件，告警
if (!imu_file.good()) {
LOG(ERROR)<< "no imu file found at " << path+"/imu0/data.csv";
return -1;
}
// 如果IMU文件中某一行缺少IMU数据，告警
int number_of_lines = 0;
while (std::getline(imu_file, line))
++number_of_lines;
LOG(INFO)<< "No. IMU measurements: " << number_of_lines-1;
if (number_of_lines - 1 <= 0) {
LOG(ERROR)<< "no imu messages present in " << path+"/imu0/data.csv";
return -1;
}
// set reading position to second line 将读取位置设置为第二行
imu_file.clear();
imu_file.seekg(0, std::ios::beg);
std::getline(imu_file, line);
// 5. 打开Camera文件夹，读取图像文件名（还没有读取图像）
std::vector<okvis::Time> times;
okvis::Time latest(0);
int num_camera_images = 0;
// 5.1 给每个相机定义一个vector，向量内的数据类型为“ std::string”（字符串类型），存放图像名称
// 5.2 开始读取图像名称（实际上是图像的时间标签）
std::vector < std::vector < std::string >> image_names(numCameras);
// 5.3 读取给定文件路径下的所有文件的名称，用的是folder(/path)函数
for (size_t i = 0; i < numCameras; ++i) {
num_camera_images = 0;
std::string folder(path + "/cam" + std::to_string(i) + "/data");
// 5.4 不断在vector中push_back图像名称，也就是在vector的末尾插入string
for (auto it = boost::filesystem::directory_iterator(folder);
it != boost::filesystem::directory_iterator(); it++) {
if (!boost::filesystem::is_directory(it->path())) { //we eliminate directories
num_camera_images++;
image_names.at(i).push_back(it->path().filename().string()); // ".at(i)"提供访问vector向量的入口，i为哪一个数据需要访问
} else {
continue;
}
}
// 5.5 如果文件夹内没有图像，输出告警信息
if (num_camera_images == 0) {
LOG(ERROR)<< "no images at " << folder;
return 1;
}
// 5.6 读取完毕后，对vector向量image_names里的元素进行排序 。因为图像名称是时间标签，事件标签值反映图像的先后顺序，按大小顺序排列即可
LOG(INFO)<< "No. cam " << i << " images: " << num_camera_images;
// the filenames are not going to be sorted. So do this here
std::sort(image_names.at(i).begin(), i/mage_names.at(i).end());
}
// 6. 定义了两个迭代器，通过从头到尾访问图像文件名容器，得到对应的图像
// 迭代器iterator允许程序员检查容器内元素，并实现元素遍历的数据类型。跟下标操作类似。
// 6.1 每个相机设置一个迭代器，从头到尾访问每个相机的图像
std::vector < std::vector < std::string > ::iterator
> cam_iterators(numCameras);
for (size_t i = 0; i < numCameras; ++i) {
cam_iterators.at(i) = image_names.at(i).begin(); // 为迭代器赋初值，迭代器的初值就是图像文件名容器的起始值
}
// 7. 打开窗口显示
int counter = 0;
okvis::Time start(0.0);
while (true) {
okvis_estimator.display(); // 线程图像及特征点匹配显示窗口
poseViewer.display(); // 位姿显示窗口（黑色窗口）
// 检查是否已读取全部图像，读取完毕后输出信息
for (size_t i = 0; i < numCameras; ++i) {
if (cam_iterators[i] == image_names[i].end()) { // 判断条件为，迭代器的值为图像文件名容器的最后一个值
std::cout << std::endl << "Finished. Press any key to exit." << std::endl << std::flush;
cv::waitKey();
return 0;
}
}
// 8. 根据文件名开始读入图像
// 8.1 定义类存放图像的真实时间（秒、纳秒）
okvis::Time t; // 将图像文件名（时间标签）转换为真实的时间（秒、 纳秒）？？？
// 8.2 调用OpenCV库，读取每个相机的图像
for (size_t i = 0; i < numCameras; ++i) {
cv::Mat filtered = cv::imread(
path + "/cam" + std::to_string(i) + "/data/" + *cam_iterators.at(i),
cv::IMREAD_GRAYSCALE); // 读取路径下的灰度图像（文件名由迭代器访问图像文件名容器得来
// 时间标签总/共18位，前9位位秒，后9位为纳秒
std::string nanoseconds = cam_iterators.at(i)->substr(
cam_iterators.at(i)->size() - 13, 9);
std::string seconds = cam_iterators.at(i)->substr(
0, cam_iterators.at(i)->size() - 13);
t = okvis::Time(std::stoi(seconds), std::stoi(nanoseconds)); // 时间
if (start == okvis::Time(0.0)) {
start = t;
}
// 9. get all IMU measurements till then
// 9.1 定义类存放IMU数据的真实时间
okvis::Time t_imu = start;
// 9.2 获取所有的IMU测量，如果完成，则输出信息（当IMU的时间标签小于图像时，添加IMU值）
do {
if (!std::getline(imu_file, line)) {
std::cout << std::endl << "Finished. Press any key to exit." << std::endl << std::flush;
cv::waitKey();
return 0;
}
std::stringstream stream(line);
std::string s;
std::getline(stream, s, ',');
// Ⅰ 根据时间标签，计算IMU数据的真实时间
std::string nanoseconds = s.substr(s.size() - 9, 9);
std::string seconds = s.substr(0, s.size() - 9);
// Ⅱ 读取陀螺仪 gyr[ ] 数据（x,y,z）
Eigen::Vector3d gyr;
for (int j = 0; j < 3; ++j) {
std::getline(stream, s, ',');
gyr[j] = std::stof(s); // stof():将字符串转换为数字
}
// Ⅲ 读取加速度计 acc[ ] 数据（x,y,z）
Eigen::Vector3d acc;
for (int j = 0; j < 3; ++j) {
std::getline(stream, s, /','); // 从s中读取至多n个字符保存在stream对应的数组中,遇到“，”则读取终止
acc[j] = std::stof(s);
}
t_imu = okvis::Time(std::stoi(seconds), std::stoi(nanoseconds));
// Ⅳ 加入IMU测量值进行（阻塞）处理
if (t_imu - start + okvis::Duration(1.0) > deltaT) {
// 添加IMU测量（测量数据的时间标签、加速度、角速度）
okvis_estimator.addImuMeasurement(t_imu, acc, gyr);
}
} while (t_imu <= t); // 当IMU的时间标签小于图像时，添加IMU值
// Ⅴ 将图像添加到前端进行（阻塞）处理
if (t - start > deltaT) {
okvis_estimator.addImage(t, i, filtered); // 添加新的图像（图像时间标签、相机编号、图像）
}
cam_iterators[i]++;
}
++counter;
// display progress显示进度
if (counter % 20 == 0) {
std::cout << "\rProgress: "
<< int(double(counter) / double(num_camera_images) * 100) << "% "
<< std::flush;
}
}
std::cout << std::endl << std::flush;
return 0
}
2.2 Pose Viewer类
PoseViewer类的主要作用是画出小黑窗口，实时显示运动轨迹。在该类中，可以定义输出数据流savetrajectory（ofstream savetrajectory），将轨迹保存在“results.txt”中。
class PoseViewer // PoseViewer类的主要作用是画出小黑窗口，实时显示运动轨迹
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
constexpr static const double imageSize = 500.0;
// 1. 构造函数：OpenCV打开和显示窗口
PoseViewer()
{
cv::namedWindow("OKVIS Top View");
_image.create(imageSize, imageSize, CV_8UC3);
drawing_ = false;
showing_ = false;
}
// 2. 回调函数 void publishFullStateAsCallback() ，用回调函数发布所有状态（包括位置和速度），显示实时的运动轨迹（小黑窗口）
// 2.1 函数需要的输入参数包括：time时间、Transformation变换矩阵T_WS、速度和偏差矩阵SpeedAndBias
void publishFullStateAsCallback(
const okvis::Time & /*t*/, const okvis::kinematics::Transformation & T_WS, // 时间，变换矩阵
const Eigen::Matrix<double, 9, 1> & speedAndBiases, // 速度和偏差矩阵
const Eigen::Matrix<double, 3, 1> & /*omega_S*/) // 角速率矩阵（陀螺仪测量.?）
{
// just append the path
// 2.2 获取平移矩阵（T_WS.r()）、旋转矩阵（T_WS.C()）
Eigen::Vector3d r = T_WS.r(); // 获取平移矩阵
Eigen::Matrix3d C = T_WS.C(); // 获取旋转矩阵
_path.push_back(cv::Point2d(r[0], r[1]));
_heights.push_back(r[2]);
// 2.3 maintain scaling 保持缩放
if (r[0] - _frameScale < _min_x)
_min_x = r[0] - _frameScale;
if (r[1] - _frameScale < _min_y)
_min_y = r[1] - _frameScale;
if (r[2] < _min_z)
_min_z = r[2];
if (r[0] + _frameScale > _max_x)
_max_x = r[0] + _frameScale;
if (r[1。] + _frameScale > _max_y)
_max_y = r[1] + _frameScale;
if (r[2] > _max_z)
_max_z = r[2];
_scale = std::min(imageSize / (_max_x - _min_x), imageSize / (_max_y - _min_y));
// 2.4 draw it 画出轨迹
while (showing_) {
}
drawing_ = true;
// erase
_image.setTo(cv::Scalar(10, 10, 10));
drawPath(); // 绘制路径
// 2.5 draw axes
Eigen::Vector3d e_x = C.col(0);
Eigen::Vector3d e_y = C.col(1);
Eigen::Vector3d e_z = C.col(2);
cv::line(
_image,
convertToImageCoordinates(_path.back()),
convertToImageCoordinates(
_path.back() + cv::Point2d(e_x[0], e_x[1]) * _frameScale),
cv::Scalar(0, 0, 255), 1, CV_AA);
cv::line(
_image,
convertToImageCoordinates(_path.back()),
convertToImageCoordinates(
_path.back() + cv::Point2d(e_y[0], e_y[1]) * _frameScale),
cv::Scalar(0, 255, 0), 1, CV_AA);
cv::line(
_image,
convertToImageCoordinates(_path.back()),
convertToImageCoordinates(
_path.back() + cv::Point2d(e_z[0], e_z[1]) * _frameScale),
cv::Scalar(255, 0, 0), 1, CV_AA);
// some text:
std::stringstream postext;
postext << "position = [" << r[0] << ", " << r[1] << ", " << r[2] << "]";
cv::putText(_image, postext.str(), cv::Point(15,15),
cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255,255,255), 1);
std::stringstream veltext;
veltext << "velocity = [" << speedAndBiases[0] << ", " << speedAndBiases[1] << ", " << speedAndBiases[2] << "]";
cv::putText(_image, veltext.str(), cv::Point(15,35),
cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255,255,255), 1);
drawing_ = false; // notify
}
// 3. 显示函数
void display()
{
while (drawing_) {
}
showing_ = true;
cv::imshow("OKVIS Top View", _image);
showing_ = false;
cv::waitKey(1);
}
private:
// 3.1 将二维点坐标转换为图像坐标系坐标
cv::Point2d convertToImageCoordinates(const cv::Point2d & pointInMeters) const
{
cv::Point2d pt = (pointInMeters - cv::Point2d(_min_x, _min_y)) * _scale;
return cv::Point2d(pt.x, imageSize - pt.y); // reverse y for more intuitive top-down plot反转更直观的自上而下的绘图
}
// 3.2 画出轨迹
void drawPath()
{
for (size_t i = 0; i + 1 < _path.size(); ) {
cv::Point2d p0 = convertToImageCoordinates(_path[i]);
cv::Point2d p1 = convertToImageCoordinates(_path[i + 1]);
cv::Point2d diff = p1-p0;
if(diff.dot(diff)<2.0){
_path.erase(_path.begin() + i + 1); // clean short segment
_heights.erase(_heights.begin() + i + 1);
continue;
}
double rel_height = (_heights[i] - _min_z + _heights[i + 1] - _min_z)
* 0.5 / (_max_z - _min_z);
cv::line(
_image,
p0,
p1,
rel_height * cv::Scalar(255, 0, 0)
+ (1.0 - rel_height) * cv::Scalar(0, 0, 255),
1, CV_AA);
i++;
}
}
cv::Mat _image;
std::vector<cv::Point2d> _path;
std::vector<double> _heights;
double _scale = 1.0;
double _min_x = -0.5;
double _min_y = -0.5;
double _min_z = -0.5;
double _max_x = 0.5;
double _max_y = 0.5;
double _max_z = 0.5;
const double _frameScale = 0.2; // [m]
std::atomic_bool drawing_;
std::atomic_bool showing_;
};
