本文参考各网站大佬的各种总结讲解,在此致谢。针对程序我自己不明白不清楚的部分,做注释。
本文仅供学习,如果有错误的地方,请大家指点指点
/*********************************************************************************
* 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;
}