OKvis整理

整理部分代码中公式

OKVIS 中的 propagation 代码公式版 
http://blog.csdn.net/fuxingyin/article/details/53449465

OKVIS 中的 propagation 公式版 
http://blog.csdn.net/fuxingyin/article/details/53449502

OKVIS 计算 liftJacobian 
http://blog.csdn.net/fuxingyin/article/details/53449401

OKVIS IMU 误差公式代码版本 
http://blog.csdn.net/fuxingyin/article/details/53449209

OKVIS IMU 误差公式版本 
http://blog.csdn.net/fuxingyin/article/details/53449365

OKVIS Reprojection Error 
http://blog.csdn.net/fuxingyin/article/details/53455200

OKVIS speedAndBias error 
http://blog.csdn.net/fuxingyin/article/details/53453225

OKVIS RelativePoseError 
http://blog.csdn.net/fuxingyin/article/details/53455306

OKVIS poseError 
http://blog.csdn.net/fuxingyin/article/details/53455412 
公式较多，难免有错误，错误的地方请指教。

多线程初始化

okvis_app_synchronous.cpp 中 main 函数，main 函数中初始化 okvis_estimator，okvis_estimator 初始时构造了几个线程：

// Start all threads.
void ThreadedKFVio::startThreads() {

  // consumer threads
  // frameConsumerLoop 负责特征提取，每个 camera 开一个线程
  for (size_t i = 0; i < numCameras_; ++i) {
    frameConsumerThreads_.emplace_back(&ThreadedKFVio::frameConsumerLoop, this, i);
  }
  // keypointConsumerThreads_ 负责特征匹配，这里只有两个相机，
  // numCameraPairs_ == 1，所以开启一个线程用来特征匹配
  for (size_t i = 0; i < numCameraPairs_; ++i) {
    keypointConsumerThreads_.emplace_back(&ThreadedKFVio::matchingLoop, this);
  }
  // imuConsumerThread_ 负责根据 imu 数据做系统状态估计
  imuConsumerThread_ = std::thread(&ThreadedKFVio::imuConsumerLoop, this);
  positionConsumerThread_ = std::thread(&ThreadedKFVio::positionConsumerLoop,
                                        this);
  // gpsConsumerLoop，magnetometerConsumerLoop，differentialConsumerLoop 都为空，三个线程没用到                                      
  gpsConsumerThread_ = std::thread(&ThreadedKFVio::gpsConsumerLoop, this);
  magnetometerConsumerThread_ = std::thread(
      &ThreadedKFVio::magnetometerConsumerLoop, this);
  differentialConsumerThread_ = std::thread(
      &ThreadedKFVio::differentialConsumerLoop, this);

  // algorithm threads
  // visualizationThread_ 负责将当前帧，关键帧的图片，和匹配的特征显示出来
  visualizationThread_ = std::thread(&ThreadedKFVio::visualizationLoop, this);
  // 优化和边缘化线程
  optimizationThread_ = std::thread(&ThreadedKFVio::optimizationLoop, this);
  // 将优化的结果给 poseViewer，poseViewer 将当前系统的状态量显示出来，并且把相机的轨迹画出来
  publisherThread_ = std::thread(&ThreadedKFVio::publisherLoop, this);
}
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

几个线程并行执行，做位姿估计。

imuConsumerLoop

将 imu data push 进 imuMeasurements_ 给 frameConsumerLoop 处理。IMU data 每获取一次，imuConsumerLoop 就 propagation 一次，如果是刚优化完，则 propagation 的初值设为刚优化完的结果，如果是不是刚优化完，则初值是上一次 propagation 结果。propagation 的结果根据参数可以给 publisherLoop，publisherLoop 负责绘制系统状态的黑色小窗口图像。

frameConsumerLoop

每个相机开启一个 frameConsumerLoop 线程，frameConsumerLoop 对当前的输入图像做特征提取，注意到有个 propagation，在特征提取前会根据 IMU 上一帧状态和当前 IMU 测量预测当前帧状态，当前帧的状态会参与到特征提取中（特征提取是带方向的，见 Frame::describe(const Eigen::Vector3d & extractionDirection) 函数）。特征检测的结果给 matchingLoop。

      // get old T_WS
      propagationTimer.start();
      // 根据当前处理的 multiFrame 的时间戳和 multiFrame 附近的 IMU 测量值对 multiFrame 做位姿估计，估计的位姿用于特征的提取
      okvis::ceres::ImuError::propagation(imuData, parameters_.imu, T_WS,
                                          speedAndBiases, lastTimestamp,
                                          multiFrame->timestamp());
      propagationTimer.stop();
 
 
1
2
3
4
5
6
7

matchingLoop

因为这里是双目相机，只会开启一个 matchingLoop 线程（对于多目相机每两个相机开启一个线程），注意到函数里代码有个 addStates，addStates 会向优化库 ceres 中添加待优化的参数，在参数添加前也会有个 propagation 操作，propagation 给添加的参数赋初始值。addStates 还会添加 IMU 误差。特征匹配的结果给 optimizationLoop。

      // addStates 向当前优化的 problem 添加参数，并且添加 IMU 的测量误差
      if (estimator_.addStates(frame, imuData, asKeyframe)) {
        lastAddedStateTimestamp_ = frame->timestamp();
        addStateTimer.stop();
      } else {
        LOG(ERROR) << "Failed to add state! will drop multiframe.";
        addStateTimer.stop();
        continue;
      }
 
 
1
2
3
4
5
6
7
8
9

特征匹配在 dataAssociationAndInitialization 中完成：

/* 
特征匹配包括 3D 到 2D 匹配和 2D 到 2D 匹配，3D 到 2D 匹配建立特征的重投影误差，2D 到 2D 的匹配初始化新的特征点也建立投影误差，特征匹配完 3D-2D ransac 用于去除误匹配，2D-2d ransac 用于初始化位姿，判断当前帧是否为关键帧
*/
frontend_.dataAssociationAndInitialization(estimator_, T_WS, parameters_,
                                                 map_, frame, &asKeyframe);
 
 
1
2
3
4
5

匹配成功的 3维特征点和 2D 图像特征建立重投影误差，建立重投影误差见函数 ::ceres::ResidualBlockId Estimator::addObservation。

optimizationLoop

负责优化和边缘化，并且把优化和边缘化后的结果根据参数选择可以给 publisherLoop（也可以用 imuConsumerLoop 得到的 IMU 结果），publisherLoop 负责回调 poseViewer 中的 fullStateCallback_ 函数，绘制黑色小窗口图片。把特征匹配结果给 visualizationLoop，visualizationLoop 负责绘制两幅大图像。

visualizationLoop

绘制两幅大图像，两幅大图像实际有四幅图像组成，上面两幅图像是关键帧图像，下面两幅是当前帧图像，中间黑色窗口的图像是 publisherLoop 回调 poseViewer 中的 publishFullStateAsCallback 函数绘制的。 

publisherLoop

回调 poseViewer 中的 publishFullStateAsCallback 函数绘制中间黑色窗口图像。

附

两幅大图像和中间小图像的显示在 main 函数中执行：

    okvis_estimator.display();
    poseViewer.display();