okvis代码学习_1——okvis_app/src/okvis_app_synchronous.cpp

本文参考各网站大佬的各种总结讲解，在此致谢。针对程序我自己不明白不清楚的部分，做注释。

本文仅供学习，如果有错误的地方，请大家指点指点

/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 * 
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Jun 26, 2013
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *    Modified: Andreas Forster (an.forster@gmail.com)
 *********************************************************************************/

/**
 * @file okvis_app_synchronous.cpp
 * @brief This file processes a dataset.
 
 This node goes through a dataset in order and waits until all processing is done
 before adding a new message to algorithm

 * @author Stefan Leutenegger
 * @author Andreas Forster
 */

#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <memory>
#include <functional>
#include <atomic>

#include <Eigen/Core>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
#pragma GCC diagnostic ignored "-Woverloaded-virtual"
#include <opencv2/opencv.hpp>
#pragma GCC diagnostic pop
#include <okvis/VioParametersReader.hpp>
#include <okvis/ThreadedKFVio.hpp>

#include <boost/filesystem.hpp>

// 绘图类
// PoseViewer类的主要作用是画出小黑窗口，实时显示运动轨迹。
// 在该类中，可以定义输出数据流savetrajectory（ofstream savetrajectory），将轨迹保存在“results.txt”中。
class 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; // 原子变量
  }

  // this we can register as a callback
  // 轨迹图绘制函数（ROS版本下好像没有）
// **************************************** 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();  // S到W的平移向量，投影表示在W中                     // 获取平移矩阵
    Eigen::Matrix3d C = T_WS.C();  // Returns the rotation matrix ，S到W的旋转矩阵    // 获取旋转矩阵
    _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; // x方向的边界
    if (r[1] - _frameScale < _min_y)
      _min_y = r[1] - _frameScale; // y方向的边界
    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 画出轨迹
    //等待showing_的信号，showing为原子变量；而且此处应该省略了{std::this_thread::yield();}
    //showing为true的时候，将可以将本线程的CPU时间片放弃，并允许其他线程(dispaly)运行, 停止绘制
    while (showing_) {
    }
    drawing_ = true; // drawing_为true的时候, 线程(display)放弃运行,停止显示
    // erase
    _image.setTo(cv::Scalar(10, 10, 10)); // 填充背景颜色
    drawPath(); // 绘制路径
    // draw axes
    // 绘制坐标系
    Eigen::Vector3d e_x = C.col(0);
    Eigen::Vector3d e_y = C.col(1);
    Eigen::Vector3d e_z = C.col(2);
    // back返回当前vector容器中末尾元素的引用。
    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); // 画x轴，颜色为蓝色
    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); // 画y轴，颜色为绿
    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); // 画z轴，颜色为红色

    // 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,则线程display开始争抢当前线程publishFullStateAsCallback的时间,开启显示
    drawing_ = false; // notify
  }
  // **************************************** 3. 显示函数 ****************************************
  void display()
  {
      // 当drawing_为true时,将display线程的时间让给其他线程(绘图线程)
    while (drawing_) {
    }
    showing_ = true; // showing为真,意味着将线程publishFullStateAsCallback的时间让给当前线程(display), 不进行绘制
    cv::imshow("OKVIS Top View", _image); // 绘制图像
    showing_ = false; // showing_为假意味着线程publishFullStateAsCallback开始争抢当前线程(display)的时间, 开始绘制
    cv::waitKey(1);   // 1s显示一次
  }
 private:
  // 3.1 将公尺米为单位的坐标转化到像素坐标系中 / 将二维点坐标转换为图像坐标系坐标
  cv::Point2d convertToImageCoordinates(const cv::Point2d & pointInMeters) const
  {
    cv::Point2d pt = (pointInMeters - cv::Point2d(_min_x, _min_y)) * _scale;
    // 将y方向反转，因为一般我们视角是从上向下看得
    return cv::Point2d(pt.x, imageSize - pt.y); // reverse y for more intuitive top-down plot
  }
  // 路径绘制函数
  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_;
};

// this is just a workbench. 这里的大部分内容都会用到前端类
// this is just a workbench. most of the stuff here will go into the Frontend class.
int main(int argc, char **argv)  // argc 是外部命令参数的个数，argv[] 存放各参数的内容
{
// 运行时，窗口输入： argv[0]运行程序  argv[1]配置文件路径   argv[2]数据集路径   
// **************************************** 1.  读入argv[0]，运行程序。出错时输出警告信息****************************************
// Initialize Google's logging library.
  google::InitGoogleLogging(argv[0]);  // 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]));  // atof表示将字符串转换为浮点数,并在类Duration中将时间戳的前十位和后9位分开
  }

// **************************************** 2. 读入arg[1]路径下的配置文件 ****************************************
  // read configuration file
// 2.1 将argv[1]得到的配置文件名赋给configFilename
  std::string configFilename(argv[1]); //读取参数文件

// 2.2 读取configFilename文件下的配置文件，将所有参数和配置文件组合起来
  okvis::VioParametersReader vio_parameters_reader(configFilename); //读取参数文件到类中
// 2.3 得到parameters下的所有参数设置
  okvis::VioParameters parameters;
  vio_parameters_reader.getParameters(parameters);                  //将读取到的参数赋值给变量parameters

// ************************************** 3. 得到配置文件里的参数设置parameter后，开启系统(okvis_estimator) **************************************
  // 系统入口在ThreadedKFVio类的构造函数中（ ThreadedKFVio类继承自VioInterface，该类的定义在okvis_multisensor_processing文件夹）
  // 定义一个ThreadedKFVio类对象okvis_estimator，并且将获得的参数设置传入（一旦使用该类，系统在该类的构造函数中将进行初始化，系统正式开启！
  okvis::ThreadedKFVio okvis_estimator(parameters);//用参数构造ThreadedKFVio类，成功后SLAM系统开始运行

// 3.1 定义窗口显示类（显示运动轨迹窗口）   PoseViewer poseViewer；       
  // 定义了一个poseViewer类的对象poseViewer
  PoseViewer poseViewer;//定义画图工具

// 3.2 调用对象okvis_estimator的全状态回调函数 setFullStateCallback（）；
  // bind是一组用于函数绑定的模板。在对某个函数进行绑定时，可以指定部分参数或全部参数，也可以不指定任何参数，还可以调整各个参数间的顺序。
  // bind表示绑定函数publishFullStateAsCallback, 其是poseViewer中的成员函数, 其余四个参数固定, 延迟到我们需要的时候调用
  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);
  //设置okvis_estimator类中的变量blocking_为真, 且执行时间限制优化
  // 设置阻塞变量，addmeasurement()函数是否应立即返回（阻塞= false），或者仅在处理完成时返回
  okvis_estimator.setBlocking(true);      // 使IMU与图像帧尽可能对齐

// **************************************** 4. 读入argv[2]路径下的IMU数据 ****************************************
// 4.1 the folder path
  // "path"被定义为string类型，并将argv[2]得到的值赋给"path"
  std::string path(argv[2]);              // 数据文件所在的路径  

// 4.2 通过配置文件得到相机数量numCameras
  const unsigned int numCameras = parameters.nCameraSystem.numCameras();//return The number of cameras.

// 4.3 open the IMU file，打开IMU文件，读入IMU数据，“imu_file”是类"ifstream"的对象，作用是读入数据
  std::string line;
  std::ifstream imu_file(path + "/imu0/data.csv");  // IMU文件的路径  // 读取IMU的csv文件的行
  //没有的话则直接检测错误
  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;     // 一共有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();  //清楚eof的标记   清除eof的标记  eof文件结束符
  // C++中seekg函数的功能是：设置输入文件流的文件流指针位置
  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 开始读取图像名称（实际上是图像的时间标签）
  // image_names为一个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
  //push_back()函数:将一个新的元素加到vector的最后面，位置为当前最后一个元素的下一个元素  push_back（参数为要插入的值）
    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++;//图像数量加一
        // ".at(i)"提供访问vector向量的入口，i为哪一个数据需要访问
        image_names.at(i).push_back(it->path().filename().string()); // 储存文件名到容器image_names中 
      } 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;//第i个相机中有多少个图像
    // the filenames are not going to be sorted. So do this here
    std::sort(image_names.at(i).begin(), image_names.at(i).end());//对文件名按照时间顺序进行排序
  }

// **************************************** 6. 定义了两个迭代器，通过从头到尾访问图像文件名容器，得到对应的图像 ****************************************
  // 迭代器iterator允许程序员检查容器内元素，并实现元素遍历的数据类型。跟下标操作类似。
  // 6.1 每个相机设置一个迭代器，从头到尾访问每个相机的图像
  std::vector < std::vector < std::string > ::iterator
      > cam_iterators(numCameras);//定义一个2维的容器,每一维都是一个迭代器
  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);//从0开始
  // 开始程序运行函数
  while (true) {
      // 窗口显示函数的调用
    okvis_estimator.display(); // 线程图像及特征点匹配显示窗口
    poseViewer.display();      // 位姿显示窗口（黑色窗口）

      // check if at the end
// 检查是否已读取全部图像，读取完毕后输出信息  
    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 定义类存放图像的真实时间（秒、纳秒）
  // add images
    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); // 读取图像到矩阵filtered中 // 读取路径下的灰度图像（文件名由迭代器访问图像文件名容器得来)  
      // 读取小于秒单位(小数点后)的时间戳
      // 时间标签总共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)); // stoi表示字符串转数值 // 时间 
      if (start == okvis::Time(0.0)) {
        start = t; // 将初始第一帧的时间戳赋值给start
      }

// **************************************** 9. get all IMU measurements till then 得到所有IMU的测量结果 ****************************************
   // 9.1 定义类存放IMU数据的真实时间       
      //start为初始帧的时间戳
      okvis::Time t_imu = start;
   // 9.2 获取所有的IMU测量，如果完成，则输出信息（当IMU的时间标签小于图像时，添加IMU值） 
      do {
          //IMU数据读取错误
        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);  // 一行IMU数据
        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的时间戳赋值给t_imu

    // Ⅳ 加入IMU测量值进行（阻塞）处理 
        // add the IMU measurement for (blocking) processing
        //imu比初始帧图像早小于1s,或者imu比初始帧图像晚
        if (t_imu - start + okvis::Duration(1.0) > deltaT) {
        // 添加IMU测量（测量数据的时间标签、加速度、角速度）
          okvis_estimator.addImuMeasurement(t_imu, acc, gyr); // 添加IMU数据到okvis_estimator中
        }

        // 当IMU的时间标签小于图像时，添加IMU值
      } while (t_imu <= t); // 当imu比当前帧晚,直接跳出循环(由于是左右图都进行一次,所以一般一帧图像会对应其前面临近几帧和后面两帧IMU)

    // Ⅴ 将图像添加到前端进行（阻塞）处理
      // add the image to the frontend for (blocking) processing
      //初始帧之后的所有帧都加入到okvis_estimator中
      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;
}