Dogm robot test

dogm/CMakeLists.txt

@@ -1,9 +1,10 @@
 cmake_minimum_required(VERSION 3.9)
-project(DOGM LANGUAGES CXX CUDA)
+project(dogm LANGUAGES CXX CUDA)
 
 set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
-option(BUILD_TESTS "Build unit tests" ON)
+option(BUILD_TESTS "Build unit tests" OFF)
 
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/modules/")
 find_package(OpenGL REQUIRED)
@@ -27,20 +28,20 @@ if (NOT GLEW_FOUND)
 	set(GLEW_LIBRARIES "" CACHE FILEPATH "" )
 endif()
 
-LIST (APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
-INCLUDE(GoogleTestDownloadAndBuild)
+find_package(OpenCV REQUIRED)
+find_package(Eigen3 REQUIRED)
 
-#if(WIN32)
-#    install(FILES "${OpenCV_DIR}/${OpenCV_ARCH}/${OpenCV_RUNTIME}/bin/opencv_world${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}d.dll" DESTINATION ${CMAKE_INSTALL_BINARY}/debug CONFIGURATIONS Debug)
-#    install(FILES "${OpenCV_DIR}/${OpenCV_ARCH}/${OpenCV_RUNTIME}/bin/opencv_world${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.dll" DESTINATION ${CMAKE_INSTALL_BINARY} CONFIGURATIONS RelWithDebInfo Release)
-#endif(WIN32)
+LIST (APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
+#INCLUDE(GoogleTestDownloadAndBuild)
 
 include_directories(
 	SYSTEM
 	${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES}
 	${GLFW3_INCLUDE_DIR}
 	${GLEW_INCLUDE_DIRS}
 	${GLM_INCLUDE_DIR}
+	${OpenCV_INCLUDE_DIRS}
+	${EIGEN3_INCLUDE_DIR}
 )
 
 set(HEADER_FILES
@@ -50,6 +51,7 @@ set(HEADER_FILES
 	include/dogm/kernel/particle_to_grid.h
 	include/dogm/kernel/predict.h
 	include/dogm/kernel/resampling.h
+	include/dogm/kernel/resampling_parallel.h
 	include/dogm/kernel/statistical_moments.h
 	include/dogm/kernel/update_persistent_particles.h
 	include/dogm/kernel/ego_motion_compensation.h
@@ -68,6 +70,7 @@ set(SRC_FILES
 	src/kernel/particle_to_grid.cu
 	src/kernel/predict.cu
 	src/kernel/resampling.cu
+	src/kernel/resampling_parallel.cu
 	src/kernel/statistical_moments.cu
 	src/kernel/update_persistent_particles.cu
 	src/kernel/ego_motion_compensation.cu
@@ -76,7 +79,7 @@ set(SRC_FILES
 
 source_group(TREE ${CMAKE_CURRENT_SOURCE_DIR} FILES ${SRC_FILES})
 
-add_library(dogm STATIC
+add_library(dogm SHARED
     ${HEADER_FILES}
 	${SRC_FILES}
 )
@@ -91,12 +94,13 @@ string(APPEND CMAKE_CUDA_FLAGS " --expt-extended-lambda")
 #set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} --expt-extended-lambda -Xcudafe --diag_suppress=esa_on_defaulted_function_ignored")
 set(CUDA_PROPAGATE_HOST_FLAGS ON)
 
-set_target_properties(dogm PROPERTIES PUBLIC_HEADER "include/dogm.h;include/dogm_types.h")
+set_target_properties(dogm PROPERTIES PUBLIC_HEADER "include/dogm/dogm.h;include/dogm/dogm_types.h")
 
 target_link_libraries(dogm
-	PRIVATE ${OPENGL_LIBRARY}
-	PRIVATE ${GLFW3_LIBRARY}
-	PRIVATE ${GLEW_LIBRARIES}
+	${OPENGL_LIBRARY}
+	${GLFW3_LIBRARY}
+	${GLEW_LIBRARIES}
+	${OpenCV_LIBS}
 )
 
 #INSTALL(TARGETS dogm
@@ -111,3 +115,31 @@ if(BUILD_TESTS)
 endif()
 
 add_subdirectory(demo)
+
+# Install package.xml for catkin
+install(FILES package.xml DESTINATION share/${PROJECT_NAME}/)
+
+set(CONF_INCLUDE_DIRS "${CMAKE_INSTALL_PREFIX}/include")
+set(DOGM_CMAKE_DIR share/${PROJECT_NAME}/cmake)
+include(CMakePackageConfigHelpers)
+configure_package_config_file(
+	dogm-config.cmake.in
+	${PROJECT_BINARY_DIR}/cmake/${PROJECT_NAME}/dogm-config.cmake
+	PATH_VARS DOGM_CMAKE_DIR
+	INSTALL_DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}
+)
+
+install(
+	FILES ${PROJECT_BINARY_DIR}/cmake/${PROJECT_NAME}/dogm-config.cmake
+	DESTINATION share/${PROJECT_NAME}/
+)
+
+install(TARGETS dogm
+	ARCHIVE DESTINATION lib
+	LIBRARY DESTINATION lib
+)
+
+install(
+	DIRECTORY include/
+	DESTINATION include/
+)

dogm/demo/CMakeLists.txt

@@ -4,19 +4,25 @@ if (NOT OpenCV_FOUND)
 	set(OpenCV_LIBS "" CACHE FILEPATH "" )
 endif()
 
+find_package(CUDA REQUIRED)
+
 add_definitions(-D_USE_MATH_DEFINES)  # Required to make M_PI from cmath available in MSVC
 
 add_executable(demo
 	main.cpp
 )
 
-target_include_directories(demo PUBLIC ${OpenCV_INCLUDE_DIRS} ${GLM_INCLUDE_DIR})
+target_include_directories(demo PUBLIC
+	${OpenCV_INCLUDE_DIRS}
+	${GLM_INCLUDE_DIR}
+)
 
 target_link_libraries(demo
 	dogm
-	utils
-	simulator
+	dogm_utils
+	dogm_simulator
 	${OpenCV_LIBS}
+	${CUDA_CUDART_LIBRARY}
 )
 
 add_subdirectory(utils)

dogm/demo/main.cpp

@@ -80,14 +80,14 @@ int main(int argc, const char** argv)
 
     for (int step = 0; step < num_simulation_steps; ++step)
     {
-        dogm::MeasurementCell* meas_grid = grid_generator.generateGrid(sim_data[step].measurements);
+        dogm::MeasurementCellsSoA meas_grid = grid_generator.generateGrid(sim_data[step].measurements);
 
         const auto update_grid_caller = [&grid_map](auto&&... args) {
             grid_map.updateGrid(std::forward<decltype(args)>(args)...);
         };
 
         cycle_timer.timeFunctionCall(true, update_grid_caller, meas_grid, sim_data[step].ego_pose.x,
-                                     sim_data[step].ego_pose.y, 0.0f, simulation_step_period, true);
+                                     sim_data[step].ego_pose.y, simulation_step_period, true);
 
         const auto cells_with_velocity =
             computeCellsWithVelocity(grid_map, minimum_occupancy_threshold, minimum_velocity_threshold);

dogm/demo/simulator/CMakeLists.txt

@@ -1,10 +1,5 @@
 find_package(OpenGL REQUIRED)
 
-find_package(GLM)
-if (NOT GLM_FOUND)
-	set(GLM_INCLUDE_DIR "" CACHE PATH "")
-endif()
-
 find_package(GLFW3)
 if (NOT GLFW3_FOUND)
 	set(GLFW3_INCLUDE_DIR "" CACHE PATH "")
@@ -45,13 +40,18 @@ SET(SRC_FILES
 
 source_group(TREE ${CMAKE_CURRENT_SOURCE_DIR} FILES ${SRC_FILES})
 
-ADD_LIBRARY(simulator STATIC ${SRC_FILES} ${HEADER_FILES})
+ADD_LIBRARY(dogm_simulator SHARED ${SRC_FILES} ${HEADER_FILES})
 
-TARGET_INCLUDE_DIRECTORIES(simulator
+TARGET_INCLUDE_DIRECTORIES(dogm_simulator
     PUBLIC
     ${CMAKE_CURRENT_SOURCE_DIR}/include
     ${PROJECT_SOURCE_DIR}/include)
 
+TARGET_LINK_LIBRARIES(dogm_simulator
+    ${GLFW3_LIBRARY}
+    ${GLEW_LIBRARIES}
+)
+
 if(BUILD_TESTS)
     add_subdirectory(test)
 endif()

dogm/demo/simulator/include/mapping/kernel/measurement_grid.h

@@ -6,19 +6,15 @@
 
 #include <device_launch_parameters.h>
 
-namespace dogm
-{
-struct MeasurementCell;
-}
 
 __global__ void createPolarGridTextureKernel(cudaSurfaceObject_t polar, const float* __restrict__ measurements,
                                              int width, int height, float resolution);
 
 __global__ void fusePolarGridTextureKernel(cudaSurfaceObject_t polar, const float* __restrict__ measurements, int width,
                                            int height, float resolution);
 
-__global__ void cartesianGridToMeasurementGridKernel(dogm::MeasurementCell* __restrict__ meas_grid,
+__global__ void cartesianGridToMeasurementGridKernel(dogm::MeasurementCellsSoA meas_grid,
                                                      cudaSurfaceObject_t cart, int grid_size);
 
-__global__ void gridArrayToMeasurementGridKernel(dogm::MeasurementCell* __restrict__ meas_grid,
+__global__ void gridArrayToMeasurementGridKernel(dogm::MeasurementCellsSoA meas_grid,
                                                  const float2* __restrict__ grid, int grid_size);

dogm/demo/simulator/include/mapping/laser_to_meas_grid.h

@@ -23,10 +23,10 @@ class LaserMeasurementGrid
     LaserMeasurementGrid(const Params& params, float grid_length, float resolution);
     ~LaserMeasurementGrid();
 
-    dogm::MeasurementCell* generateGrid(const std::vector<float>& measurements);
+    dogm::MeasurementCellsSoA generateGrid(const std::vector<float>& measurements);
 
 private:
-    dogm::MeasurementCell* meas_grid;
+    dogm::MeasurementCellsSoA meas_grid;
     int grid_size;
 
     Params params;

dogm/demo/simulator/mapping/kernel/measurement_grid.cu

@@ -112,7 +112,7 @@ __global__ void fusePolarGridTextureKernel(cudaSurfaceObject_t polar, const floa
     }
 }
 
-__global__ void cartesianGridToMeasurementGridKernel(dogm::MeasurementCell* __restrict__ meas_grid,
+__global__ void cartesianGridToMeasurementGridKernel(dogm::MeasurementCellsSoA meas_grid,
                                                      cudaSurfaceObject_t cart, int grid_size)
 {
     const int x = blockIdx.x * blockDim.x + threadIdx.x;
@@ -123,15 +123,15 @@ __global__ void cartesianGridToMeasurementGridKernel(dogm::MeasurementCell* __re
     {
         float4 color = surf2Dread<float4>(cart, x * sizeof(float4), y);
 
-        meas_grid[index].occ_mass = color.x;
-        meas_grid[index].free_mass = color.y;
+        meas_grid.occ_mass[index] = color.x;
+        meas_grid.free_mass[index] = color.y;
 
-        meas_grid[index].likelihood = 1.0f;
-        meas_grid[index].p_A = 1.0f;
+        meas_grid.likelihood[index] = 1.0f;
+        meas_grid.p_A[index] = 1.0f;
     }
 }
 
-__global__ void gridArrayToMeasurementGridKernel(dogm::MeasurementCell* __restrict__ meas_grid,
+__global__ void gridArrayToMeasurementGridKernel(dogm::MeasurementCellsSoA meas_grid,
                                                  const float2* __restrict__ grid, int grid_size)
 {
     const int x = blockIdx.x * blockDim.x + threadIdx.x;
@@ -142,10 +142,10 @@ __global__ void gridArrayToMeasurementGridKernel(dogm::MeasurementCell* __restri
     {
         float2 masses = grid[index];
 
-        meas_grid[index].occ_mass = masses.x;
-        meas_grid[index].free_mass = masses.y;
+        meas_grid.occ_mass[index] = masses.x;
+        meas_grid.free_mass[index] = masses.y;
 
-        meas_grid[index].likelihood = 1.0f;
-        meas_grid[index].p_A = 1.0f;
+        meas_grid.likelihood[index] = 1.0f;
+        meas_grid.p_A[index] = 1.0f;
     }
 }

dogm/demo/simulator/mapping/laser_to_meas_grid.cu

@@ -12,23 +12,23 @@ LaserMeasurementGrid::LaserMeasurementGrid(const Params& params, float grid_leng
 {
     int grid_cell_count = grid_size * grid_size;
 
-    CHECK_ERROR(cudaMalloc(&meas_grid, grid_cell_count * sizeof(dogm::MeasurementCell)));
+    meas_grid.init(grid_cell_count, true);
 
     renderer = std::make_unique<Renderer>(grid_size, params.fov, grid_length, params.max_range);
 }
 
 LaserMeasurementGrid::~LaserMeasurementGrid()
 {
-    CHECK_ERROR(cudaFree(meas_grid));
+    meas_grid.free();
 }
 
-dogm::MeasurementCell* LaserMeasurementGrid::generateGrid(const std::vector<float>& measurements)
+dogm::MeasurementCellsSoA LaserMeasurementGrid::generateGrid(const std::vector<float>& measurements)
 {
     const int num_measurements = measurements.size();
 
     float* d_measurements;
-    CHECK_ERROR(cudaMalloc(&d_measurements, num_measurements * sizeof(float)));
-    CHECK_ERROR(
+    CUDA_CALL(cudaMalloc(&d_measurements, num_measurements * sizeof(float)));
+    CUDA_CALL(
         cudaMemcpy(d_measurements, measurements.data(), num_measurements * sizeof(float), cudaMemcpyHostToDevice));
 
     const int polar_width = num_measurements;
@@ -47,7 +47,7 @@ dogm::MeasurementCell* LaserMeasurementGrid::generateGrid(const std::vector<floa
     createPolarGridTextureKernel<<<grid_dim, dim_block>>>(polar_surface, d_measurements, polar_width, polar_height,
                                                           params.resolution);
 
-    CHECK_ERROR(cudaGetLastError());
+    CUDA_CALL(cudaGetLastError());
     polar_texture.endCudaAccess(polar_surface);
 
     // render cartesian image to texture using polar texture
@@ -60,11 +60,11 @@ dogm::MeasurementCell* LaserMeasurementGrid::generateGrid(const std::vector<floa
     // transform RGBA texture to measurement grid
     cartesianGridToMeasurementGridKernel<<<cart_grid_dim, dim_block>>>(meas_grid, cartesian_surface, grid_size);
 
-    CHECK_ERROR(cudaGetLastError());
+    CUDA_CALL(cudaGetLastError());
     framebuffer->endCudaAccess(cartesian_surface);
 
-    CHECK_ERROR(cudaFree(d_measurements));
-    CHECK_ERROR(cudaDeviceSynchronize());
+    CUDA_CALL(cudaFree(d_measurements));
+    CUDA_CALL(cudaDeviceSynchronize());
 
     return meas_grid;
 }

dogm/demo/simulator/mapping/opengl/framebuffer.cpp

@@ -23,7 +23,7 @@ Framebuffer::Framebuffer(int width, int height)
 
     glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, texture, 0);
 
-    CHECK_ERROR(cudaGraphicsGLRegisterImage(&resource, texture, GL_TEXTURE_2D, cudaGraphicsRegisterFlagsReadOnly));
+    CUDA_CALL(cudaGraphicsGLRegisterImage(&resource, texture, GL_TEXTURE_2D, cudaGraphicsRegisterFlagsReadOnly));
 
     glBindFramebuffer(GL_FRAMEBUFFER, 0);
 }
@@ -36,23 +36,23 @@ Framebuffer::~Framebuffer()
 
 void Framebuffer::beginCudaAccess(cudaSurfaceObject_t* surfaceObject)
 {
-    CHECK_ERROR(cudaGraphicsMapResources(1, &resource, nullptr));
+    CUDA_CALL(cudaGraphicsMapResources(1, &resource, nullptr));
 
     cudaArray_t cudaArray;
-    CHECK_ERROR(cudaGraphicsSubResourceGetMappedArray(&cudaArray, resource, 0, 0));
+    CUDA_CALL(cudaGraphicsSubResourceGetMappedArray(&cudaArray, resource, 0, 0));
 
     cudaResourceDesc resourceDesc;
     memset(&resourceDesc, 0, sizeof(cudaResourceDesc));
     resourceDesc.resType = cudaResourceTypeArray;
     resourceDesc.res.array.array = cudaArray;
 
-    CHECK_ERROR(cudaCreateSurfaceObject(surfaceObject, &resourceDesc));
+    CUDA_CALL(cudaCreateSurfaceObject(surfaceObject, &resourceDesc));
 }
 
 void Framebuffer::endCudaAccess(cudaSurfaceObject_t surfaceObject)
 {
-    CHECK_ERROR(cudaGraphicsUnmapResources(1, &resource, nullptr));
-    CHECK_ERROR(cudaDestroySurfaceObject(surfaceObject));
+    CUDA_CALL(cudaGraphicsUnmapResources(1, &resource, nullptr));
+    CUDA_CALL(cudaDestroySurfaceObject(surfaceObject));
 }
 
 void Framebuffer::bind()