From 5687fe5057657f3dbc82904bcf6896f5579f9415 Mon Sep 17 00:00:00 2001 From: gfuse Date: Wed, 5 May 2021 14:35:12 +0200 Subject: [PATCH] add fast kernel --- GPU_version/svo/CMakeLists.txt | 90 ++++------- GPU_version/svo/include/svo/cl_class.h | 197 +++++++++++++++++++++++++ GPU_version/svo/kernel/fast-gray.cl | 103 +++++++++++++ GPU_version/svo/src/cl_class.cpp | 56 +++++++ 4 files changed, 384 insertions(+), 62 deletions(-) create mode 100644 GPU_version/svo/include/svo/cl_class.h create mode 100644 GPU_version/svo/kernel/fast-gray.cl create mode 100644 GPU_version/svo/src/cl_class.cpp diff --git a/GPU_version/svo/CMakeLists.txt b/GPU_version/svo/CMakeLists.txt index ed4b593..4f0c7c5 100644 --- a/GPU_version/svo/CMakeLists.txt +++ b/GPU_version/svo/CMakeLists.txt @@ -1,16 +1,7 @@ -################################################################################ -# user build settings - -SET(TRACE TRUE) -SET(HAVE_G2O FALSE) -SET(USE_ROS TRUE) # Set FALSE if you want to build this package without Catkin -SET(DEBUG_OUTPUT FALSE) # Only relevant if build without ROS - -################################################################################ - SET(PROJECT_NAME gpu_svo) PROJECT(${PROJECT_NAME}) CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) + SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo SET(CMAKE_VERBOSE_MAKEFILE OFF) SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") @@ -18,18 +9,9 @@ set(CMAKE_C_STANDARD 14) set(CMAKE_CXX_STANDARD 14) # Set definitions -IF(TRACE) - ADD_DEFINITIONS(-DSVO_TRACE) -ENDIF() -IF(HAVE_G2O) - ADD_DEFINITIONS(-DUSE_BUNDLE_ADJUSTMENT) -ENDIF() -IF(USE_ROS) - ADD_DEFINITIONS(-DSVO_USE_ROS) -ENDIF() -IF(DEBUG_OUTPUT) - ADD_DEFINITIONS(-DSVO_DEBUG_OUTPUT) -ENDIF() +ADD_DEFINITIONS(-DSVO_TRACE) +ADD_DEFINITIONS(-DSVO_USE_ROS) + # Set build flags, set ARM_ARCHITECTURE environment variable on Odroid # Set build flags. Set IS_ARM on odroid board as environment variable @@ -50,49 +32,44 @@ SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer - FIND_PACKAGE(OpenCV REQUIRED) FIND_PACKAGE(Eigen REQUIRED) FIND_PACKAGE(Sophus REQUIRED) -FIND_PACKAGE(fast REQUIRED) FIND_PACKAGE(Boost REQUIRED COMPONENTS thread system) FIND_PACKAGE(GTSAM REQUIRED) +FIND_PACKAGE(OpenCL REQUIRED) +FIND_PACKAGE(vikit_common REQUIRED) set(GTSAM_INCLUDE_DIRS "${GTSAM_INCLUDE_DIR}") -IF(USE_ROS) - FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules vikit_common vikit_ros) - catkin_package( +FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules vikit_common vikit_ros) +catkin_package( DEPENDS Eigen OpenCV Sophus Boost fast GTSAM CATKIN_DEPENDS roscpp roslib vikit_common vikit_ros INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} - ) -ELSE() - FIND_PACKAGE(vikit_common REQUIRED) - SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) - SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) -ENDIF() +) + # Include dirs INCLUDE_DIRECTORIES( include - ${Eigen_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${Sophus_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${fast_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} + kernel + ${Eigen_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Sophus_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${OpenCL_INCLUDE_DIRS} + ${GTSAM_INCLUDE_DIR} + ${catkin_INCLUDE_DIRS} ) # Set link libraries -LIST(APPEND LINK_LIBS +LIST(APPEND LINK_LIBS + gtsam ${OpenCV_LIBS} ${Sophus_LIBRARIES} ${Boost_LIBRARIES} - ${fast_LIBRARIES} + ${OpenCL_LIBRARIES} + ${catkin_LIBRARIES} ) -IF(USE_ROS) - INCLUDE_DIRECTORIES(${catkin_INCLUDE_DIRS}) - LIST(APPEND LINK_LIBS ${catkin_LIBRARIES}) -ELSE() - INCLUDE_DIRECTORIES(${vikit_common_INCLUDE_DIRS}) - LIST(APPEND LINK_LIBS ${vikit_common_LIBRARIES}) -ENDIF() + # Set sourcefiles LIST(APPEND SOURCEFILES @@ -109,24 +86,13 @@ LIST(APPEND SOURCEFILES src/feature_detection.cpp src/depth_filter.cpp src/config.cpp - src/sparse_img_align.cpp src/imu_integration.cpp) - -# Add g2o if available -IF(HAVE_G2O) - FIND_PACKAGE(G2O REQUIRED) - INCLUDE_DIRECTORIES( - ${G2O_INCLUDE_DIR} - /usr/include/suitesparse # for cholmod - ) - LIST(APPEND LINK_LIBS - ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_DENSE} - ${G2O_SOLVER_PCG} ${G2O_TYPES_SBA} cholmod cxsparse - ) - LIST(APPEND SOURCEFILES src/bundle_adjustment.cpp) -ENDIF(HAVE_G2O) + src/sparse_img_align.cpp + src/imu_integration.cpp + src/cl_class.cpp) # Create svo library ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCEFILES}) -TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${LINK_LIBS} gtsam) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${LINK_LIBS} ) set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 14) set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON) +target_compile_features(${PROJECT_NAME} PRIVATE cxx_range_for) diff --git a/GPU_version/svo/include/svo/cl_class.h b/GPU_version/svo/include/svo/cl_class.h new file mode 100644 index 0000000..c0b378a --- /dev/null +++ b/GPU_version/svo/include/svo/cl_class.h @@ -0,0 +1,197 @@ +// +// Created by root on 4/27/21. +// + +#ifndef VIO_OPENCL_CL_CLASS_H +#define VIO_OPENCL_CL_CLASS_H +#include +#include +#include +#include +#include +#include +#define __CL_ENABLE_EXCEPTIONS +#include +#include + +class kernel{ +public: + kernel(cl::Program* program,std::string name){_kernel = new cl::Kernel(*program,name.c_str());}; + ~kernel(){ + //for(auto i:_buffers)i.first.first.; + _buffers.clear(); + _images.clear(); + }; + template + int32_t write(size_t id/*buffer ID*/,T* buf,cl::CommandQueue* queue,cl::Context* context,size_t buf_size){ + try{ + _buffers.push_back(std::pair,size_t>(std::pair(cl::Buffer(*context, CL_MEM_READ_WRITE, sizeof(T) * buf_size), + buf_size),id)); + queue->enqueueWriteBuffer(_buffers.back().first.first,CL_TRUE, 0, sizeof(T) * buf_size, buf); + _kernel->setArg(id,_buffers.back().first.first); + return CL_SUCCESS; + }catch (std::exception& err){ + std::cerr<<"Error C:Kernel, F:write\n"<(cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, + cl::ImageFormat(CL_R, CL_UNSIGNED_INT8), + buf.size().width, + buf.size().height, + 0, + reinterpret_cast(buf.data)),id)); + /* }else{ + _images.push_back(std::pair(cl::Image2D(*context, CL_MEM_READ_WRITE, + cl::ImageFormat(CL_RGBA, CL_UNORM_INT8), + buf.size().width, + buf.size().height),id)); + }*/ + + _kernel->setArg(id,_images.back().first); + return CL_SUCCESS; + }catch (std::exception& err){ + std::cerr<<"Error C:Kernel, F:write image\n"< + int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){ + try{ + if(id>_buffers.size())return CL_MAP_FAILURE; + for(auto i:_buffers)if(i.second==id) + queue->enqueueWriteBuffer(i.first.first,CL_TRUE, 0, sizeof(T) * i.first.second, buf); + return CL_SUCCESS; + }catch (std::exception& err){ + std::cerr<<"Reload buffer on GPU failed\n"<(buf.data)); + return CL_SUCCESS; + }catch (std::exception& err){ + std::cerr<<"Reload buffer on GPU failed\n"<,size_t>> _buffers; + std::vector> _images; + +}; +class read_cl{ +public: + read_cl(std::string path){ + src_str=(char *)malloc(0x100000); + FILE *fp; + fp = fopen(path.c_str(), "r"); + size = fread(src_str, 1, 0x100000, fp); + fclose(fp); + } + void print(){ + std::cerr< + int32_t write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, int size,T* buf) { + try{ + _kernels.at(id1).write(id2,buf,queue,context,size); + return CL_SUCCESS; + }catch (std::exception& err){ + std::cerr<<"Write buffer on GPU failed\n"< + int32_t write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T& value){ + _kernels.at(id1)._kernel->setArg(id2,value); + }; + template + int32_t reload_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T* buf) { + try{ + _kernels.at(id1).reload(id2,buf,queue); + return CL_SUCCESS; + }catch (std::exception& err){ + std::cerr<<"Reload buffer on GPU failed\n"< + int32_t run(size_t id1/*kernal ID*/,std::size_t x=1,std::size_t y=1,std::size_t z=1,size_t id2=0/*last id of the kernel related to the output*/, + size_t size_out=0/*out put buffer size*/, + T* out= nullptr) { + try{ + //std::cerr<<"kernel: "<<_kernels.at(id1)._kernel->getInfo(); + //std::cerr<<" ,Args: "<<_kernels.at(id1)._kernel->getInfo()<<" ,Run out: "; + cl::Buffer out_buf(*context, CL_MEM_READ_WRITE, sizeof(T) * size_out); + _kernels.at(id1)._kernel->setArg(id2,out_buf); + if(z>1 && y>1){ + std::cerr<enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/)<<'\n'; + }else if(z<2 && y>1){ + queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y)/*Global*/, cl::NullRange/*local*/); + //std::cerr<enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y)/*Global*/, cl::NullRange/*local*/)<<'\n'; + /*std::cerr<<_kernels.at(id1)._kernel->getArgInfo(0)<<'\n'; + std::cerr<<_kernels.at(id1)._kernel->getArgInfo(1)<<'\n'; + std::cerr<<_kernels.at(id1)._kernel->getArgInfo(2)<<'\n'; + std::cerr<<_kernels.at(id1)._kernel->getArgInfo(3)<<'\n';*/ + }else{ + std::cerr<enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x)/*Global*/, cl::NullRange/*local*/)<<'\n'; + }; + queue->enqueueReadBuffer(out_buf, CL_TRUE, 0, sizeof(T) * size_out, out); + //wait for kernel to finish + queue->finish(); + return CL_SUCCESS; + } catch (std::exception& err) { + std::cerr<<"RUN kernel Error\n"< _kernels; + cl::Context* context = NULL; + cl::Device device; + cl::Program* program = NULL; + cl::CommandQueue* queue=NULL; +}; + +#endif //VIO_OPENCL_CL_CLASS_H diff --git a/GPU_version/svo/kernel/fast-gray.cl b/GPU_version/svo/kernel/fast-gray.cl new file mode 100644 index 0000000..e9ba8f4 --- /dev/null +++ b/GPU_version/svo/kernel/fast-gray.cl @@ -0,0 +1,103 @@ +// Copyright (C) 2011 Dmitri Nikulin +// Copyright (C) 2011 Monash University +// +// Permission is hereby granted, free of charge, to any person +// obtaining a copy of this software and associated documentation +// files (the "Software"), to deal in the Software without +// restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the +// Software is furnished to do so, subject to the following +// conditions: +// +// The above copyright notice and this permission notice shall be +// included in all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES +// OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +// WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +// OTHER DEALINGS IN THE SOFTWARE. + +// Enable OpenCL 32-bit integer atomic functions. + +__kernel void fast_gray( + __read_only image2d_t image, + __global int2 * corners, + __global int * icorner, + int fast_count +) { + + // Prepare a suitable OpenCL image sampler. + sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST; + + // Use global work item as 2D image coordinates. + int const x = get_global_id(0); + int const y = get_global_id(1); + if(x>16 && x16){ + int2 const xy = (int2)(x, y); + // Read the candidate pixel. + int const p00 = read_imageui(image, sampler, xy).x; + + // Read other pixels in a circle around the candidate pixel. + int const p01 = read_imageui(image, sampler, xy + (int2)( 0, 3)).x; + int const p05 = read_imageui(image, sampler, xy + (int2)( 3, 0)).x; + int const p09 = read_imageui(image, sampler, xy + (int2)( 0, -3)).x; + int const p13 = read_imageui(image, sampler, xy + (int2)(-3, 0)).x; + + // Check the absolute difference of each circle pixel. + int const d01 = (abs(p01 - p00) > FAST_THRESH); + int const d05 = (abs(p05 - p00) > FAST_THRESH); + int const d09 = (abs(p09 - p00) > FAST_THRESH); + int const d13 = (abs(p13 - p00) > FAST_THRESH); + + // Check if any two adjacent circle pixels have a high absolute difference. + + if ((d01 && d05) ||(d05 && d09) ||(d09 && d13) ||(d13 && d01)) { + // Read other pixels in a circle around the candidate pixel. + int const p02 = read_imageui(image, sampler, xy + (int2)( 1, 3)).x; + int const p03 = read_imageui(image, sampler, xy + (int2)( 2, 2)).x; + int const p04 = read_imageui(image, sampler, xy + (int2)( 3, 1)).x; + int const p06 = read_imageui(image, sampler, xy + (int2)( 3, -1)).x; + int const p07 = read_imageui(image, sampler, xy + (int2)( 2, -2)).x; + int const p08 = read_imageui(image, sampler, xy + (int2)( 1, -3)).x; + int const p10 = read_imageui(image, sampler, xy + (int2)(-1, -3)).x; + int const p11 = read_imageui(image, sampler, xy + (int2)(-2, -2)).x; + int const p12 = read_imageui(image, sampler, xy + (int2)(-3, -1)).x; + int const p14 = read_imageui(image, sampler, xy + (int2)(-3, 1)).x; + int const p15 = read_imageui(image, sampler, xy + (int2)(-2, 2)).x; + int const p16 = read_imageui(image, sampler, xy + (int2)(-1, 3)).x; + // Select the maximum score. + int sco = p00; + sco = max(sco, p01); + sco = max(sco, p02); + sco = max(sco, p03); + sco = max(sco, p04); + sco = max(sco, p05); + sco = max(sco, p06); + sco = max(sco, p07); + sco = max(sco, p08); + sco = max(sco, p09); + sco = max(sco, p10); + sco = max(sco, p11); + sco = max(sco, p12); + sco = max(sco, p13); + sco = max(sco, p14); + sco = max(sco, p15); + + // Keep this score if it is as good as the maximum. + if (p00 >= sco) { + // Atomically append to corner buffer. + int const icorn = atom_inc(icorner); + if (icorn < fast_count) + corners[icorn] = xy; + } + } + + } + +} + diff --git a/GPU_version/svo/src/cl_class.cpp b/GPU_version/svo/src/cl_class.cpp new file mode 100644 index 0000000..59ba287 --- /dev/null +++ b/GPU_version/svo/src/cl_class.cpp @@ -0,0 +1,56 @@ +// +// Created by root on 4/27/21. +// +#include "svo/cl_class.h" +opencl::opencl() { + std::vector all_platforms; + cl::Platform::get(&all_platforms); + if (all_platforms.size() == 0){ + std::cerr << " No platforms found. Check OpenCL installation!\n"; + exit(1); + } + //get default device of the default platform + std::vector all_devices; + for(auto i:all_platforms){ + std::cout << "Find platform: " << i.getInfo() << "\n"; + i.getDevices(CL_DEVICE_TYPE_GPU, &all_devices); + } + + if (all_devices.size() == 0){ + std::cout << " No devices found. Check OpenCL installation!\n"; + exit(1); + } + device = all_devices[0]; + std::cout << "CL_DEVICE_NAME: " << device.getInfo() <<'\n' + << "CL_DEVICE_OPENCL_C_VERSION: " <()<<'\n' + << "CL_DEVICE_BUILT_IN_KERNELS: " <()<<'\n' + << "CL_DEVICE_COMPILER_AVAILABLE: " <()<<'\n' + << "CL_DEVICE_LOCAL_MEM_SIZE: " <()<<'\n' + << "CL_DEVICE_GLOBAL_MEM_SIZE: " <()<<'\n'; + + context=new cl::Context({ device }); + cl::Program::Sources sources; + read_cl int_add("../kernel/int_add.cl"); + sources.push_back({ int_add.src_str, int_add.size }); + read_cl double_add("../kernel/double_add.cl"); + sources.push_back({ double_add.src_str, double_add.size }); + read_cl corner_10_less("../kernel/is_corner_10_less.cl"); + sources.push_back({ corner_10_less.src_str, corner_10_less.size }); + read_cl prefast("../kernel/prefast-gray.cl"); + sources.push_back({ prefast.src_str, prefast.size }); + //read_cl fast("../kernel/fast-gray.cl"); + // sources.push_back({ fast.src_str, fast.size}); + program=new cl::Program(*context, sources); + if(program->build({ device },"-DFAST_THRESH=40 -DFAST_RING=15") !=0) + std::cout << " Error building: " << program->getBuildInfo(device)<<'\n' + <getBuildInfo(device) << '\n'; + queue=new cl::CommandQueue(*context,device,0,NULL); +} +opencl::~opencl() { + queue->finish(); +} +void opencl::clear_buf() { + _kernels.clear(); + queue->flush(); +} +