【点云处理】点云法向量估计及其加速(4)

    上篇文章【点云处理】点云法向量估计及其加速(3)介绍了如何使用pcl提供的gpu版本法向量计算接口对点云发向量计算进行加速。不足之处在于点云k近邻查找依然比较耗时，成为影响整体计算性能的瓶颈。这篇文章就如何优化点云K近邻查找效率进行实验。上一篇文章的示例代码中knn算法实际上主要是分为两个部分：1).KDTree建树;2).根据建好的KDTree做k近邻搜索，从而建立k近邻表。

  for (auto i=0; inearestKSearch(cloud->points[i], 10, neighbors_all[i], dists);
            sizes[i] = neighbors_all[i].size();
  }

我们应该注意到，k近邻搜索这一步我们只是简简单单使用了一个for循环进行逐个点云的搜索，完全没有考虑到这一查找过程的可并行性，所以这里我们首先尝试使用多线程对这一过程进行提速。当我们考虑多线程时可以使用boost::thread库或者自从C++ 11开始支持的std::thread，也可以使用操作系统相关的线程API，如在Linux上，可以使用pthread库。除此之外，还可以使用omp来使用多线程。它的好处是跨平台，使用简单。这里直接使用OpenMP提供的parallel编译指示命令"#pragma omp parallel for"来告诉编译器对后面的for并行执行。编译器会创建一个包含N(在运行时决定,通常为服务器的逻辑核数)个线程的先成组，由他们来并行地运行后面的for语句块。

# pragma omp parallel for          
        for (auto i=0; i dists;
            kdtree->nearestKSearch(cloud->points[i], 10, neighbors_all[i], dists);
            sizes[i] = neighbors_all[i].size();
        }                       

ok，再次编译运行。

提速明显，从处理器的资源利用来看每个核都分担了压力。

                                                                                 图：无omp加速

                                                                                  图：有omp加速 

通过简单调试也可以证实是用上了，因为结果是乱序的。

​
 28     # pragma omp parallel for                        
 29         for (auto i=0; i dists;                
 31             kdtree->nearestKSearch(cloud->points[i], 10, neighbors_all[i], dists);
 32             sizes[i] = neighbors_all[i].size();      
 33             printf("point[%d] is processing...\n", i);
 34         }                 

​

point[53295] is processing...
point[21318] is processing...
point[42636] is processing...
point[31977] is processing...
point[53296] is processing...
point[21319] is processing...
point[53297] is processing...
point[31978] is processing...
point[21320] is processing...
point[53298] is processing...

【补充:我项目中用到的CMakeLists.txt】

cmake_minimum_required(VERSION 3.0.2)
project(calc_normal)

### Compile as C++14, supported in ROS Kinetic and newer
add_compile_options(-std=c++14)
SET(CMAKE_BUILD_TYPE RELEASE CACHE STRING "Set build type release" FORCE)

option(USE_DEBUG "Debug Code" OFF)
option(USE_OPENMP "Use OpenMP" ON)

if(USE_DEBUG)
    add_definitions(-DUSE_DEBUG)
endif()

if(USE_OPENMP)
    find_package(OpenMP REQUIRED)
    if(${OPENMP_FOUND})
        message("Find OpenMP")
        set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
        set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
    endif()
endif(USE_OPENMP)

find_package(CUDA REQUIRED)
find_package(PCL 1.11 REQUIRED)
if(NOT ${PCL_FOUND})
    message("PCL 1.11 Not Found ...Searching 1.12")
    find_package(PCL 1.12 REQUIRED)
endif(NOT ${PCL_FOUND})

message("PCL_INCLUDE_DIRS" ${PCL_INCLUDE_DIRS})
include_directories(${PCL_INCLUDE_DIRS})
message("PCL_LIBRARY_DIRS" ${PCL_LIBRARY_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  message_generation
  sensor_msgs
)

catkin_package(
    CATKIN_DEPENDS
    message_runtime
)

if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
    message("embed_platform on")
    include_directories(${CUDA_INCLUDE_DIRS})
    link_directories(/usr/local/cuda/targets/aarch64-linux/lib)
    link_directories(/usr/local/cuda/lib64)
    set(CUDA_GEN_CODE "-gencode=arch=compute_72,code=sm_72")
    set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS}  ${CUDA_GEN_CODE}")
else()
    message("embed_platform off")
    include_directories(${CUDA_INCLUDE_DIRS})
    link_directories(/usr/local/cuda/lib64)
    set(CUDA_GEN_CODE "-gencode=arch=compute_75,code=sm_75")
    set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS}  ${CUDA_GEN_CODE}")
endif()

include_directories(
    ${catkin_INCLUDE_DIRS}
)

file(GLOB_RECURSE PROJECT_SRC 
    ./src/*.cpp
)

cuda_add_executable(${PROJECT_NAME} src/main.cpp ${PROJECT_SRC})

## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME} cudart)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES})

if(USE_OPENMP)
    if(${OpenMP_FOUND})
       target_link_libraries(${PROJECT_NAME} OpenMP::OpenMP_CXX)
    endif()
endif(USE_OPENMP)