OKVIS 代码框架

版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/fuxingyin/article/details/53428523

整理部分代码中公式

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);
}

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

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();

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;
      }

特征匹配在 dataAssociationAndInitialization 中完成:

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

匹配成功的 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();

版权声明:本文为博主原创文章,未经博主允许不得转载。

没有更多推荐了,返回首页

私密
私密原因:
请选择设置私密原因
  • 广告
  • 抄袭
  • 版权
  • 政治
  • 色情
  • 无意义
  • 其他
其他原因:
120
出错啦
系统繁忙,请稍后再试

关闭