SLAM各种并行加速方法

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

前言

CPU并行加速

CPU并行加速的本质就是通过硬件并发（hardware concurrency）的形式来实现。这种的操作方式是通过单个进程里多线程，从而实现共享地址空间，全局变量，指针，引用。但是这种方式相对而言更加传统，但是同时更加具有普适性。

其中操作是使用以pthread为代表的多线程并行加速

pthread

这是一个pthread的简单示例代码。

class helloFromObject{
public:
    void operator()() const{
        std::cout << "Hello, My Second thread!" << std::endl;
    }
};
 
int main() {
    std::cout << "Hello, Coconut Cat!" << std::endl;
    std::thread bthread((helloFromObject()));
    bthread.join();
    return 0;
}

我们可以发现pthread这种多线程加速v-slam场景下都有着充分的应用。

OpenMP

openmp作为另一种CPU提速方式，在SLAM的特征提取中拥有比较良好的加速代码。当然thread对于openmp还是有一定的影响的，每个thread分配给omp的线程可能减小或者是在thread里面继续调用omp再开线程会带来更大的成本，导致omp单独执行时变慢。

#include <cv.h>
#include <highgui.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <omp.h>
#include "bench_source/harris.h"
 
// CHECK_FINAL_RESULT toggles the display of the input and output images
//#define CHECK_FINAL_RESULT
// Experiment : try to run multiples pipelines in parallel
//#define RUN_PARALLEL = true
// Check if image to matrix translation produces the correst output
//#define CHECK_LOADING_DATA
using namespace std;
 
static int minruns = 1;
 
int main(int argc, char ** argv)
{
  int i, j, run;
  int R, C, nruns;
  double begin, end;
  double init, finish;
  double stime, avgt;
  cv::Mat image, loaded_data;
  cv::Scalar sc;
  cv::Size size;
 
  float *t_res;
  float *t_data;
 
  /* Might be unused depending on preprocessor macro definitions */
  (void)t_res;
  (void)t_data;
  (void)loaded_data;
 
#ifdef VERSION_ALIGNED
    float ** data;
    float ** res;
    printf("VERSION_ALIGNED\n");
    int cache_line_size = get_cache_line_size();
#else
    float * data;
    float * res;
#endif
 
  if ( argc != 3 )
  {
      printf("usage: harris <Image_Path> <Nruns>\n");
     return -1;
  }
 
  printf("Loading image ....\n");
  image = cv::imread( argv[1], 1 );
  printf("%s successfully loaded !\n\n", argv[1]);
 
  if ( !image.data )
  {
      printf("No image data ! Are you sure %s is an image ?\n", argv[1]);
      return -1;
  }
 
  // Convert image input to grayscale floating point
  cv::cvtColor(image, image, cv::COLOR_BGR2GRAY);
  size = image.size();
  C = size.width;
  R = size.height;
  nruns = max(minruns, atoi(argv[2]));
 
  printf("_________________________________________\n");
  printf("Values settings :\n");
  printf("Nruns : %i || %s [%i, %i]\n", nruns, argv[1], R, C);
  printf("_________________________________________\n");
 
#ifdef VERSION_ALIGNED
    res = alloc_array_lines(R, C, cache_line_size);
#else
    res = (float *) calloc(R * C, sizeof(*res));
#endif
 
  if(res == NULL)
  {
    printf("Error while allocating result table of size %ld B\n",
      (sizeof(*res) * C * R ));
    return -1;
  }
 
#ifdef VERSION_ALIGNED
    data = (float **) alloc_array_lines(R, C, cache_line_size);
    for(i= 0; i < R;i++){
      for(j = 0; j < C;j++){
        sc = image.at<uchar>(i, j) ;
        data[i][j] = (float) sc.val[0]/255;
      }
    }
#else
    data = (float *) malloc(R*C*sizeof(float));
    for(i= 0; i < R;i++){
      for(j = 0; j < C;j++){
        sc = image.at<uchar>(i, j) ;
        data[i*C+j] = (float) sc.val[0]/255;
      }
    }
#endif
 
  // Running tests
  avgt = 0.0f;
  /*
  Do not use clock here we need elapsed "wall clock time", not total CPU time.
  */
  init =  omp_get_wtime();
 
#ifdef RUN_PARALLEL
    #pragma omp parallel for shared(avgt)
#endif
  for(run = 0; run <= nruns; run++)
  {
    begin = omp_get_wtime();
    pipeline_harris(C, R, data, res);
    end = omp_get_wtime();
    stime = end - begin;
    if(run !=0){
      printf("Run %i : \t\t %f ms\n", run, (double) stime * 1000.0 );
#ifdef RUN_PARALLEL
        #pragma omp atomic
#endif
      avgt += stime;
    }
  }
 
  finish =  omp_get_wtime();
 
  if(avgt == 0)
  {
    printf("Error : running didn't take time !");
    return -1;
  }
  printf("Average time : %f ms\n", (double) (1000.0 * avgt / (nruns)));
  printf("Total time : %f ms\n", (double) (finish - init) * 1000.0);
 
#ifdef RUN_PARALLEL
    printf("Gain total times to run %i instances in parallel / serial time :\n ", nruns);
    printf("\t %f\n",(double) (finish-init)/(avgt));
#endif
 
  // Checking images using OpenCV
 
#ifdef VERSION_ALIGNED
    t_res = (float *) malloc(sizeof(float)*R*C);
    for(int i = 0; i < R; i++){
      for(int j = 0; j < C; j++){
        t_res[i * C + j] = res[i][j];
      }
    }
#else
   t_res = res;
#endif
 
#ifdef CHECK_LOADING_DATA
#ifdef VERSION_ALIGNED
    t_data = (float *) malloc(sizeof(float)*R*C);
    for(int i = 0; i < R; i++){
      for(int j = 0; j < C; j++){
        t_data[i * C + j] = data[i][j];
      }
    }
    loaded_data = cv::Mat(R,C,CV_32F, t_data);
#else
    loaded_data = cv::Mat(R,C,CV_32F, data);
#endif
 
    cv::namedWindow( "Check data", cv::WINDOW_NORMAL);
    cv::imshow( "Check data", loaded_data);
    cv::waitKey(0);
    cv::destroyAllWindows();
    loaded_data.release();
 
#ifdef VERSION_ALIGNED
      free(t_data);
#endif
 
#endif /* CHECK_LOADING_DATA */
 
#ifdef CHECK_FINAL_RESULT
    cv::Mat imres = cv::Mat(R, C, CV_32F, t_res);
    cv::namedWindow( "Input", cv::WINDOW_NORMAL );
    cv::imshow( "Input", image );
    cv::namedWindow( "Output", cv::WINDOW_NORMAL );
    cv::imshow( "Output", imres * 65535.0 );
    cv::waitKey(0);
    cv::destroyAllWindows();
    imres.release();
    free(t_res);
#endif /* CHECK_FINAL_RESULT */
 
#ifdef VERSION_ALIGNED
    free(res);
#endif
 
  image.release();
  free(data);
  return 0;
 
}

GPU并行加速

Nvidia 的CUDA工具箱中提高了免费的GPU加速的快速傅里叶变换（FFT）、基本线性代数子程序（BLAST）、图像与视频处理库（NPP）。用户只要把源代码中CPU版本的快速傅里叶变换、快速傅里叶变换和图像与视频处理库替换成相应的GPU版，即可得到性能加速。除了Nvidia提供的函数库以外，第三方的GPU函数库有：

• CUDA数据并行基元库（cuDPP）：

• http://www.gpgpu.org/developer/cudpp/

• CULA工具：由EM Photonics公司推出， CUDA GPU中的LAPACK：

• http://www.culatools.com/

• MAGMA：由Dongarra’s Group推出，CUDA GPU和多核CPU中的LAPACK：

  http://icl.cs.utk.edu/magma/

• <ahref="http://alice.loria.fr/index.php/publications.html?redirect=1&Paper=CNC@2008">雅可比预处理共轭梯度（JCG）

• GPULib：针对接口描述语言（IDL）以及矩阵实验室（MATLAB）的数学函数库：

  http://www.txcorp.com/products/GPULib/

• GPU VSIPL信号处理库：

  http://gpu-vsipl.gtri.gatech.edu/

• 计算机视觉（CV）以及成像库：

  http://openvidia.sourceforge.net/

• OpenCurrent:规则网格系统中CUDA加速PDE(partial
  differential equation，偏微分方程)开源数据库解决方案：

  http://code.google.com/p/opencurrent/

• CUDA / GPU中的libSVM

• Multisvm：利用CUDA的多等级SVM：

  http://code.google.com/p/multisvm/

• cuSVM：支持矢量分类与衰减的CUDA使用方法：

  http://patternsonascreen.net/cuSVM.html

目前v-slam算法的主要是依赖CUDA开发加速的，为此，我们在GPU加速部分仅仅对CUDA进行举例介绍，等后面作者涉及到其他的加速后，再来扩充。我们在使用CPU并行计算的同时可以使用GPU加速，从而来耗费一定的GPU计算资源来提升视觉前端处理的能力。

我们先来看一下不带CUDA运算的部分代码：

TicToc t_o;
 
vector<float> err;
if(hasPrediction)
{
    cur_pts = predict_pts;
    cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 1, 
    cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
 
    int succ_num = 0;
    for (size_t i = 0; i < status.size(); i++)
    {
        if (status[i])
            succ_num++;
    }
    if (succ_num < 10)
    cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
}
else
    cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
// reverse check
if(FLOW_BACK)
{
    vector<uchar> reverse_status;
    vector<cv::Point2f> reverse_pts = prev_pts;
    cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 1, 
    cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
    //cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 3); 
    for(size_t i = 0; i < status.size(); i++)
    {
        if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
        {
            status[i] = 1;
        }
        else
            status[i] = 0;
    }
}

下面是使用CUDA并行运算的代码：

TicToc t_og;
cv::cuda::GpuMat prev_gpu_img(prev_img);
cv::cuda::GpuMat cur_gpu_img(cur_img);
cv::cuda::GpuMat prev_gpu_pts(prev_pts);
cv::cuda::GpuMat cur_gpu_pts(cur_pts);
cv::cuda::GpuMat gpu_status;
if(hasPrediction)
{
    cur_gpu_pts = cv::cuda::GpuMat(predict_pts);
    cv::Ptr<cv::cuda::SparsePyrLKOpticalFlow> d_pyrLK_sparse = cv::cuda::SparsePyrLKOpticalFlow::create(
    cv::Size(21, 21), 1, 30, true);
    d_pyrLK_sparse->calc(prev_gpu_img, cur_gpu_img, prev_gpu_pts, cur_gpu_pts, gpu_status);
 
    vector<cv::Point2f> tmp_cur_pts(cur_gpu_pts.cols);
    cur_gpu_pts.download(tmp_cur_pts);
    cur_pts = tmp_cur_pts;
 
    vector<uchar> tmp_status(gpu_status.cols);
    gpu_status.download(tmp_status);
    status = tmp_status;
 
    int succ_num = 0;
    for (size_t i = 0; i < tmp_status.size(); i++)
    {
        if (tmp_status[i])
            succ_num++;
    }
    if (succ_num < 10)
    {
        cv::Ptr<cv::cuda::SparsePyrLKOpticalFlow> d_pyrLK_sparse = cv::cuda::SparsePyrLKOpticalFlow::create(
        cv::Size(21, 21), 3, 30, false);
        d_pyrLK_sparse->calc(prev_gpu_img, cur_gpu_img, prev_gpu_pts, cur_gpu_pts, gpu_status);
 
        vector<cv::Point2f> tmp1_cur_pts(cur_gpu_pts.cols);
        cur_gpu_pts.download(tmp1_cur_pts);
        cur_pts = tmp1_cur_pts;
 
        vector<uchar> tmp1_status(gpu_status.cols);
        gpu_status.download(tmp1_status);
        status = tmp1_status;
    }

我们可以看到该部分的代码仅仅是加入了cv::cuda::GpuMat这类基础的并行运算程序即可达到良好的提速效果，当然Mat对象仅仅存储在内存或者CPU缓存中。为了得到一个GPU能直接访问的opencv 矩阵你必须使用GPU对象 GpuMat 。它的工作方式类似于2维 Mat，唯一的限制是你不能直接引用GPU函数。（因为它们本质上是完全不同的代码，不能混合引用）

#include <opencv2/cudaoptflow.hpp>
#include <opencv2/cudaimgproc.hpp>
#include <opencv2/cudaarithm.hpp>

官网GPU文档：

https://docs.opencv.org/2.4/modules/gpu/doc/gpu.html

注意：GPU并不能对图片的各种通道都能进行有效的处理。GPU只接受通道数为一或四的图片，且数值类型为char或float。GPU不接受double类型的图片数据，否则会有异常抛出。对于三通道的图片数据，要么在图片上再加一个通道变为四通道（但这样会比较消耗GPU内存不推荐）；或者，将图片按通道划分成多个单通道图片依次处理。若对于某些函数，图片中元素的实际位置不影响处理，那么比较好的处理办法是将$\color{red}{图片直接转为单通道}$。

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列三维点云系列结构光系列、手眼标定、相机标定、orb-slam3知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~