OKVIS代码解读(1)——main函数

一、主函数——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_;
};