Skip to content

G
get_freespace
Commit 72c0f559 authored by liuji's avatar liuji
Browse files
Options

local_gpu_cluster_box

parent 1e5ba04d
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 1415 additions and 0 deletions
test_project/linefit_ground_segmentation_local_gpu_test/CMakeLists.txt 0 → 100755
View file @ 72c0f559
cmake_minimum_required(VERSION 3.10)
project(linefit_ground_segmentation_local_gpu_test)
set(TRT_MAIN_PATH "/host/home/juefx/liuji_workspace/nvidia_lib/TensorRT-8.5.1.7")
#enable_language(CUDA)
add_definitions(-std=c++17)
option(CUDA_USE_STATIC_CUDA_RUNTIME OFF)
find_package(PCL REQUIRED)
find_package(yaml-cpp REQUIRED)
find_package(Boost 1.65.1 REQUIRED system)
find_package(CUDA REQUIRED)
find_package(Eigen3 REQUIRED)
include_directories(${Eigen_INCLUDE_DIRS})
include_directories(${PCL_INCLUDE_DIRS})
include_directories(${catkin_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
set(PCL_PTH "pcl-1.8")
set(CUDA_NVCC_PLAGS ${CUDA_NVCC_PLAGS};-std=c++17;-g;-G;-gencode;arch=compute_72;code=sm_72)
# use jf preprocess
#include_directories("/usr/include/${PCL_PTH}" /usr/include/eigen3)
include_directories(include)
# include_directories(./cuda/include)
if (CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
message("xavier on")
# add_definitions(-DWERXAVIER)
include_directories(/usr/local/cuda/targets/aarch64-linux/include)
link_directories(/usr/local/cuda/targets/aarch64-linux/lib)
# find_package (OpenCV 4.0.0 REQUIRED)
# include_directories ("/usr/include/opencv4/")
else()
# add_compile_definitions(THRUST_IGNORE_CUB_VERSION_CHECK) # for cuda version >= 11
# message( SEND_ERROR "only valid in xavier!" )
# add_definitions(-DBOOST_ERROR_CODE_HEADER_ONLY)
# add_definitions(-DWERGPU)
message("=> xavier off")
include_directories("${TRT_MAIN_PATH}/include")
include_directories(/usr/local/cuda/include)
link_directories("${TRT_MAIN_PATH}/lib" )
link_directories(/usr/local/cuda/lib64)
link_directories(/usr/local/lib)
link_directories(/usr/lib/x86_64-linux-gnu)
endif()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Ofast -Wfatal-errors -D_MWAITXINTRIN_H_INCLUDED")
# add_subdirectory(cuda)
file(GLOB gpu_source_files "src/*.cu")
cuda_add_library(dbscan_cuda SHARED ${gpu_source_files})
# target_include_directories(dbscan_cuda
# SYSTEM PUBLIC
# $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
# $<INSTALL_INTERFACE:include>
# ${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES}
# PRIVATE ${PCL_INCLUDE_DIRS}
# )
target_link_libraries(dbscan_cuda nvinfer cudart)
target_link_libraries(dbscan_cuda ${PCL_LIBRARIES})
target_compile_options(dbscan_cuda
PRIVATE
$<$<COMPILE_LANGUAGE:CUDA>:-expt-extended-lambda -Xcompiler -fPIC -Xcudafe --diag_suppress=esa_on_defaulted_function_ignored>
)
#target_include_directories(myplugins PRIVATE)
add_executable(linefit_ground_segmentation_test_jf_single ${PROJECT_SOURCE_DIR}/src/ground_segmentation_node_single_test.cc
${PROJECT_SOURCE_DIR}/src/ground_segmentation.cc ${PROJECT_SOURCE_DIR}/src/segment.cc ${PROJECT_SOURCE_DIR}/src/bin.cc)
add_executable(linefit_ground_segmentation_test_jf_mult ${PROJECT_SOURCE_DIR}/src/ground_segmentation_node_mult_test.cc
${PROJECT_SOURCE_DIR}/src/ground_segmentation.cc ${PROJECT_SOURCE_DIR}/src/segment.cc ${PROJECT_SOURCE_DIR}/src/bin.cc)
# ground_segmentation_node.cc原始代码
add_executable(linefit_ground_segmentation_test_jf ${PROJECT_SOURCE_DIR}/src/ground_segmentation_node.cc
${PROJECT_SOURCE_DIR}/src/ground_segmentation.cc ${PROJECT_SOURCE_DIR}/src/segment.cc ${PROJECT_SOURCE_DIR}/src/bin.cc)
set(LIBRARIES_TO_LINK dbscan_cuda nvinfer cudart yaml-cpp -lm -pthread -lboost_system ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${OpenCV_LIBS} ${PCL_LIBRARIES})
# target_link_libraries(linefit_ground_segmentation_test_jf dbscan_cuda)
# target_link_libraries(linefit_ground_segmentation_test_jf nvinfer)
# target_link_libraries(linefit_ground_segmentation_test_jf cudart)
# target_link_libraries(linefit_ground_segmentation_test_jf -lm -pthread -lboost_system)
# target_link_libraries(linefit_ground_segmentation_test_jf ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${OpenCV_LIBS} ${PCL_LIBRARIES})
# target_link_libraries(linefit_ground_segmentation_test_jf yaml-cpp)
#target_link_libraries(linefit_ground_segmentation_test_jf -lpcl_filters yaml-cpp)
target_link_libraries(linefit_ground_segmentation_test_jf ${LIBRARIES_TO_LINK})
target_link_libraries(linefit_ground_segmentation_test_jf_single ${LIBRARIES_TO_LINK})
target_link_libraries(linefit_ground_segmentation_test_jf_mult ${LIBRARIES_TO_LINK})
add_definitions(-O2 -pthread)
test_project/linefit_ground_segmentation_local_gpu_test/cuda/include/dbscan/CudaUtils.cuh 0 → 100755
View file @ 72c0f559
/**
* @file CudaUtils.cuh
*
* @author btran
*
*/
#pragma once
#include <cstdio>
#include <cstdlib>
#include <cuda_runtime.h>
namespace cuda
{
namespace utils
{
cudaError_t warmUpGPU();
} // namespace utils
} // namespace cuda
inline void HandleError(cudaError_t err, const char* file, int line)
{
if (err != cudaSuccess) {
printf("%s in %s at line %d\n", cudaGetErrorString(err), file, line);
exit(EXIT_FAILURE);
}
}
#define HANDLE_ERROR(err) (HandleError(err, __FILE__, __LINE__))
test_project/linefit_ground_segmentation_local_gpu_test/cuda/include/dbscan/DBSCAN.cuh 0 → 100755
View file @ 72c0f559
/**
* @file DBSCAN.cuh
*
* @author btran
*
*/
#pragma once
#include <vector>
namespace clustering
{
namespace cuda
{
template <typename PointType> class DBSCAN
{
public:
struct Param {
int pointDimension;
double eps;
int minPoints;
};
explicit DBSCAN(const Param& param);
~DBSCAN();
std::vector<std::vector<int>> run(const PointType*, int numPoints) const;
private:
Param m_param;
mutable PointType* m_dPoints;
mutable int m_allocatedSize;
};
} // namespace cuda
} // namespace clustering
test_project/linefit_ground_segmentation_local_gpu_test/cuda/src/CudaUtils.cu 0 → 100755
View file @ 72c0f559
/**
* @file CudaUtils.cu
*
* @author btran
*
*/
#include <dbscan/CudaUtils.cuh>
namespace cuda
{
namespace utils
{
namespace
{
__global__ void warmUpGPUKernel()
{
int idx = blockIdx.x * blockDim.x + threadIdx.x;
++idx;
}
} // namespace
cudaError_t warmUpGPU()
{
warmUpGPUKernel<<<1, 1>>>();
cudaDeviceSynchronize();
return cudaGetLastError();
}
} // namespace utils
} // namespace cuda
test_project/linefit_ground_segmentation_local_gpu_test/cuda/src/DBSCAN.cu 0 → 100755
View file @ 72c0f559
/**
* @file DBSCAN.cu
*
* @author btran
*
*/
#include <dbscan/CudaUtils.cuh>
#include <dbscan/DBSCAN.cuh>
#include <pcl/point_types.h>
#include <thrust/device_vector.h>
#include <thrust/host_vector.h>
#include "Types.cuh"
namespace clustering
{
namespace cuda
{
namespace
{
cudaDeviceProp dProperties;
template <typename PointType>
__global__ void makeGraphStep1Kernel(const PointType* __restrict__ points, Node* nodes, int* nodeDegs, int numPoints,
float eps, int minPoints)
{
int tid = threadIdx.x + blockIdx.x * blockDim.x;
while (tid < numPoints) {
Node* node = &nodes[tid];
const PointType* point = &points[tid];
for (int i = 0; i < numPoints; ++i) {
if (i == tid) {
continue;
}
const PointType* curPoint = &points[i];
float diffx = point->x - curPoint->x;
float diffy = point->y - curPoint->y;
float diffz = point->z - curPoint->z;
float sqrDist = diffx * diffx + diffy * diffy + diffz * diffz;
if (sqrDist < eps * eps) {
node->numNeighbors++;
}
}
if (node->numNeighbors >= minPoints) {
node->type = NodeType::CORE;
}
nodeDegs[tid] = node->numNeighbors;
tid += blockDim.x * gridDim.x;
}
}
template <typename PointType>
__global__ void makeGraphStep2Kernel(const PointType* __restrict__ points, const int* __restrict__ neighborStartIndices,
int* adjList, int numPoints, float eps)
{
int tid = threadIdx.x + blockIdx.x * blockDim.x;
while (tid < numPoints) {
const PointType* point = &points[tid];
int startIdx = neighborStartIndices[tid];
int countNeighbors = 0;
for (int i = 0; i < numPoints; ++i) {
if (i == tid) {
continue;
}
const PointType* curPoint = &points[i];
float diffx = point->x - curPoint->x;
float diffy = point->y - curPoint->y;
float diffz = point->z - curPoint->z;
float sqrDist = diffx * diffx + diffy * diffy + diffz * diffz;
if (sqrDist < eps * eps) {
adjList[startIdx + countNeighbors++] = i;
}
}
tid += blockDim.x * gridDim.x;
}
}
__global__ void BFSKernel(const Node* __restrict__ nodes, const int* __restrict__ adjList,
const int* __restrict__ neighborStartIndices, char* Fa, char* Xa, int numPoints)
{
int tid = threadIdx.x + blockIdx.x * blockDim.x;
while (tid < numPoints) {
if (Fa[tid]) {
Fa[tid] = false;
Xa[tid] = true;
int startIdx = neighborStartIndices[tid];
for (int i = 0; i < nodes[tid].numNeighbors; ++i) {
int nIdx = adjList[startIdx + i];
Fa[nIdx] = 1 - Xa[nIdx];
}
}
tid += blockDim.x * gridDim.x;
}
}
void BFS(Node* hNodes, Node* dNodes, const int* adjList, const int* neighborStartIndices, int v, int numPoints,
std::vector<int>& curCluster)
{
thrust::device_vector<char> dXa(numPoints, false);
thrust::device_vector<char> dFa(numPoints, false);
dFa[v] = true;
int numThreads = 256;
int numBlocks = std::min(dProperties.maxGridSize[0], (numPoints + numThreads - 1) / numThreads);
int countFa = 1;
while (countFa > 0) {
BFSKernel<<<numBlocks, numThreads>>>(dNodes, adjList, neighborStartIndices,
thrust::raw_pointer_cast(dFa.data()), thrust::raw_pointer_cast(dXa.data()),
numPoints);
countFa = thrust::count(thrust::device, dFa.begin(), dFa.end(), true);
}
thrust::host_vector<char> hXa = dXa;
for (int i = 0; i < numPoints; ++i) {
if (hXa[i]) {
hNodes[i].visited = true;
curCluster.emplace_back(i);
}
}
}
template <typename PointType> Graph makeGraph(const PointType* points, int numPoints, float eps, int minPoints)
{
Graph graph;
graph.nodes.resize(numPoints);
graph.neighborStartIndices.resize(numPoints);
thrust::device_vector<int> dNodeDegs(numPoints);
int numThreads = 256;
int numBlocks = std::min(dProperties.maxGridSize[0], (numPoints + numThreads - 1) / numThreads);
makeGraphStep1Kernel<PointType><<<numBlocks, numThreads>>>(points, thrust::raw_pointer_cast(graph.nodes.data()),
thrust::raw_pointer_cast(dNodeDegs.data()), numPoints,
eps, minPoints);
thrust::exclusive_scan(dNodeDegs.begin(), dNodeDegs.end(), graph.neighborStartIndices.begin());
int totalEdges = dNodeDegs.back() + graph.neighborStartIndices.back();
graph.adjList.resize(totalEdges);
makeGraphStep2Kernel<PointType>
<<<numBlocks, numThreads>>>(points, thrust::raw_pointer_cast(graph.neighborStartIndices.data()),
thrust::raw_pointer_cast(graph.adjList.data()), numPoints, eps);
return graph;
}
} // namespace
template <typename PointType>
DBSCAN<PointType>::DBSCAN(const Param& param)
: m_param(param)
, m_dPoints(nullptr)
, m_allocatedSize(-1)
{
HANDLE_ERROR(cudaGetDeviceProperties(&dProperties, 0));
}
template <typename PointType> DBSCAN<PointType>::~DBSCAN()
{
if (m_dPoints) {
HANDLE_ERROR(cudaFree(m_dPoints));
m_dPoints = nullptr;
}
}
template <typename PointType>
std::vector<std::vector<int>> DBSCAN<PointType>::run(const PointType* points, int numPoints) const
{
if (numPoints <= 0) {
throw std::runtime_error("number of points must be more than 0");
}
if (m_allocatedSize < numPoints) {
m_allocatedSize = numPoints;
if (m_dPoints) {
HANDLE_ERROR(cudaFree(m_dPoints));
}
HANDLE_ERROR(cudaMalloc((void**)&m_dPoints, numPoints * sizeof(PointType)));
}
HANDLE_ERROR(cudaMemcpy(m_dPoints, points, numPoints * sizeof(PointType), cudaMemcpyHostToDevice));
auto graph = makeGraph<PointType>(m_dPoints, numPoints, m_param.eps, m_param.minPoints);
thrust::host_vector<Node> hNodes = graph.nodes;
std::vector<std::vector<int>> clusterIndices;
for (int i = 0; i < numPoints; ++i) {
auto& curHNode = hNodes[i];
if (curHNode.visited || curHNode.type != NodeType::CORE) {
continue;
}
std::vector<int> curCluster;
curCluster.emplace_back(i);
curHNode.visited = true;
BFS(hNodes.data(), thrust::raw_pointer_cast(graph.nodes.data()), thrust::raw_pointer_cast(graph.adjList.data()),
thrust::raw_pointer_cast(graph.neighborStartIndices.data()), i, numPoints, curCluster);
clusterIndices.emplace_back(std::move(curCluster));
}
return clusterIndices;
}
#undef INSTANTIATE_TEMPLATE
#define INSTANTIATE_TEMPLATE(DATA_TYPE) template class DBSCAN<DATA_TYPE>;
INSTANTIATE_TEMPLATE(pcl::PointXYZ);
INSTANTIATE_TEMPLATE(pcl::PointXYZI);
INSTANTIATE_TEMPLATE(pcl::PointXYZRGB);
INSTANTIATE_TEMPLATE(pcl::PointNormal);
INSTANTIATE_TEMPLATE(pcl::PointXYZRGBNormal);
INSTANTIATE_TEMPLATE(pcl::PointXYZINormal);
#undef INSTANTIATE_TEMPLATE
} // namespace cuda
} // namespace clustering
test_project/linefit_ground_segmentation_local_gpu_test/cuda/src/Types.cuh 0 → 100755
View file @ 72c0f559
/**
* @file Types.cuh
*
* @author btran
*
*/
#pragma once
#include <thrust/device_vector.h>
namespace clustering
{
namespace cuda
{
enum class NodeType : int { CORE, NOISE };
struct Node {
__host__ __device__ Node()
: type(NodeType::NOISE)
, numNeighbors(0)
, visited(false)
{
}
NodeType type;
int numNeighbors;
char visited;
};
struct Graph {
thrust::device_vector<Node> nodes;
thrust::device_vector<int> neighborStartIndices;
thrust::device_vector<int> adjList;
};
} // namespace cuda
} // namespace clustering
test_project/linefit_ground_segmentation_local_gpu_test/include/dbscan/CudaUtils.cuh 0 → 100755
View file @ 72c0f559
/**
* @file CudaUtils.cuh
*
* @author btran
*
*/
#pragma once
#include <cstdio>
#include <cstdlib>
#include <cuda_runtime.h>
namespace cuda
{
namespace utils
{
cudaError_t warmUpGPU();
} // namespace utils
} // namespace cuda
inline void HandleError(cudaError_t err, const char* file, int line)
{
if (err != cudaSuccess) {
printf("%s in %s at line %d\n", cudaGetErrorString(err), file, line);
exit(EXIT_FAILURE);
}
}
#define HANDLE_ERROR(err) (HandleError(err, __FILE__, __LINE__))
test_project/linefit_ground_segmentation_local_gpu_test/include/dbscan/DBSCAN.cuh 0 → 100755
View file @ 72c0f559
/**
* @file DBSCAN.cuh
*
* @author btran
*
*/
#pragma once
#include <vector>
namespace clustering
{
namespace cuda
{
template <typename PointType> class DBSCAN
{
public:
struct Param {
int pointDimension;
double eps;
int minPoints;
};
explicit DBSCAN(const Param& param);
~DBSCAN();
std::vector<std::vector<int>> run(const PointType*, int numPoints) const;
private:
Param m_param;
mutable PointType* m_dPoints;
mutable int m_allocatedSize;
};
} // namespace cuda
} // namespace clustering
test_project/linefit_ground_segmentation_local_gpu_test/include/dbscan/Types.cuh 0 → 100755
View file @ 72c0f559
/**
* @file Types.cuh
*
* @author btran
*
*/
#pragma once
#include <thrust/device_vector.h>
namespace clustering
{
namespace cuda
{
enum class NodeType : int { CORE, NOISE };
struct Node {
__host__ __device__ Node()
: type(NodeType::NOISE)
, numNeighbors(0)
, visited(false)
{
}
NodeType type;
int numNeighbors;
char visited;
};
struct Graph {
thrust::device_vector<Node> nodes;
thrust::device_vector<int> neighborStartIndices;
thrust::device_vector<int> adjList;
};
} // namespace cuda
} // namespace clustering
test_project/linefit_ground_segmentation_local_gpu_test/include/ground_segmentation/bin.h 0 → 100755
View file @ 72c0f559
#ifndef GROUND_SEGMENTATION_BIN_H_
#define GROUND_SEGMENTATION_BIN_H_
#include <atomic>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
class Bin {
public:
/*设定了一个最小Z点的结构体，其中是一个二维结构，在x-y平面的线段长度，以及z轴上的数值*/
struct MinZPoint {
MinZPoint() : z(0), d(0) {}
MinZPoint(const double& d, const double& z) : z(z), d(d) {}
bool operator==(const MinZPoint& comp) {return z == comp.z && d == comp.d;}
double z;
double d;
};
private:
std::atomic<bool> has_point_;
std::atomic<double> min_z;
std::atomic<double> min_z_range;
public:
Bin();
/// \brief Fake copy constructor to allow vector<vector<Bin> > initialization.
Bin(const Bin& segment);
void addPoint(const pcl::PointXYZ& point);
void addPoint(const double& d, const double& z);
MinZPoint getMinZPoint();
inline bool hasPoint() {return has_point_;}
};
#endif /* GROUND_SEGMENTATION_BIN_H_ */
test_project/linefit_ground_segmentation_local_gpu_test/include/ground_segmentation/ground_segmentation.h 0 → 100755
View file @ 72c0f559
#ifndef GROUND_SEGMENTATION_H_
#define GROUND_SEGMENTATION_H_
#include <mutex>
#include <glog/logging.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include<pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include "ground_segmentation/segment.h"
struct GroundSegmentationParams {
GroundSegmentationParams() :
visualize(false),
r_min_square(0.3 * 0.3),
r_max_square(20*20),
n_bins(30),
n_segments(180),
max_dist_to_line(0.15),
max_slope(1),
n_threads(4),
max_error_square(0.01),
long_threshold(2.0),
max_long_height(0.1),
max_start_height(0.2),
sensor_height(0.2),
line_search_angle(0.2) {}
// Visualize estimated ground.
bool visualize;
// Minimum range of segmentation.
double r_min_square;
// Maximum range of segmentation.
double r_max_square;
// Number of radial bins.
int n_bins;
// Number of angular segments.
int n_segments;
// Maximum distance to a ground line to be classified as ground.
double max_dist_to_line;
// Max slope to be considered ground line.
double max_slope;
// Max error for line fit.
double max_error_square;
// Distance at which points are considered far from each other.
double long_threshold;
// Maximum slope for
double max_long_height;
// Maximum heigh of starting line to be labelled ground.
double max_start_height;
// Height of sensor above ground.
double sensor_height;
// How far to search for a line in angular direction [rad].
double line_search_angle;
// Number of threads.
int n_threads;
};
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef std::pair<pcl::PointXYZ, pcl::PointXYZ> PointLine;
class GroundSegmentation {
const GroundSegmentationParams params_;
// Access with segments_[segment][bin].
std::vector<Segment> segments_;
// Bin index of every point.
std::vector<std::pair<int, int> > bin_index_;
// 2D coordinates (d, z) of every point in its respective segment.
std::vector<Bin::MinZPoint> segment_coordinates_;
// Visualizer.
std::shared_ptr<pcl::visualization::PCLVisualizer> viewer_;
void assignCluster(std::vector<int>* segmentation);
void assignClusterThread(const unsigned int& start_index,
const unsigned int& end_index,
std::vector<int>* segmentation);
void insertPoints(const PointCloud& cloud);
void insertionThread(const PointCloud& cloud,
const size_t start_index,
const size_t end_index);
void getMinZPoints(PointCloud* out_cloud);
void getLines(std::list<PointLine>* lines);
void lineFitThread(const unsigned int start_index, const unsigned int end_index,
std::list<PointLine> *lines, std::mutex* lines_mutex);
pcl::PointXYZ minZPointTo3d(const Bin::MinZPoint& min_z_point, const double& angle);
void getMinZPointCloud(PointCloud* cloud);
void visualizePointCloud(const PointCloud::ConstPtr& cloud,
const std::string& id = "point_cloud");
void visualizeLines(const std::list<PointLine>& lines);
void visualize(const std::list<PointLine>& lines, const PointCloud::ConstPtr& cloud, const PointCloud::ConstPtr& ground_cloud, const PointCloud::ConstPtr& obstacle_cloud);
public:
GroundSegmentation(const GroundSegmentationParams& params = GroundSegmentationParams());
void segment(const PointCloud& cloud, std::vector<int>* segmentation);
};
#endif // GROUND_SEGMENTATION_H_
\ No newline at end of file
test_project/linefit_ground_segmentation_local_gpu_test/include/ground_segmentation/segment.h 0 → 100755
View file @ 72c0f559
#ifndef GROUND_SEGMENTATION_SEGMENT_H_
#define GROUND_SEGMENTATION_SEGMENT_H_
#include <list>
#include <map>
#include "ground_segmentation/bin.h"
class Segment {
public:
typedef std::pair<Bin::MinZPoint, Bin::MinZPoint> Line;
typedef std::pair<double, double> LocalLine;
private:
// Parameters. Description in GroundSegmentation.
const double max_slope_;
const double max_error_;
const double long_threshold_;
const double max_long_height_;
const double max_start_height_;
const double sensor_height_;
std::vector<Bin> bins_;
std::list<Line> lines_;
LocalLine fitLocalLine(const std::list<Bin::MinZPoint>& points);
double getMeanError(const std::list<Bin::MinZPoint>& points, const LocalLine& line);
double getMaxError(const std::list<Bin::MinZPoint>& points, const LocalLine& line);
Line localLineToLine(const LocalLine& local_line, const std::list<Bin::MinZPoint>& line_points);
public:
Segment(const unsigned int& n_bins,
const double& max_slope,
const double& max_error,
const double& long_threshold,
const double& max_long_height,
const double& max_start_height,
const double& sensor_height);
double verticalDistanceToLine(const double& d, const double &z);
void fitSegmentLines();
inline Bin& operator[](const size_t& index) {
return bins_[index];
}
inline std::vector<Bin>::iterator begin() {
return bins_.begin();
}
inline std::vector<Bin>::iterator end() {
return bins_.end();
}
bool getLines(std::list<Line>* lines);
};
#endif /* GROUND_SEGMENTATION_SEGMENT_H_ */
test_project/linefit_ground_segmentation_local_gpu_test/src/AppUtility.hpp 0 → 100755
View file @ 72c0f559
/**
* @file AppUtility.hpp
*
* @author btran
*
*/
#pragma once
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <utility>
#include <vector>
namespace
{
inline pcl::PointIndices::Ptr toPclPointIndices(const std::vector<int>& indices)
{
pcl::PointIndices::Ptr pclIndices(new pcl::PointIndices);
pclIndices->indices = indices;
return pclIndices;
}
template <typename POINT_CLOUD_TYPE>
inline std::vector<typename pcl::PointCloud<POINT_CLOUD_TYPE>::Ptr>
toClusters(const std::vector<std::vector<int>>& clusterIndices,
const typename pcl::PointCloud<POINT_CLOUD_TYPE>::Ptr& inPcl)
{
std::vector<typename pcl::PointCloud<POINT_CLOUD_TYPE>::Ptr> clusters;
clusters.reserve(clusterIndices.size());
for (const auto& curIndices : clusterIndices) {
typename pcl::PointCloud<POINT_CLOUD_TYPE>::Ptr curPcl(new pcl::PointCloud<POINT_CLOUD_TYPE>);
pcl::copyPointCloud(*inPcl, *toPclPointIndices(curIndices), *curPcl);
clusters.emplace_back(curPcl);
}
return clusters;
}
inline std::vector<std::array<std::uint8_t, 3>> generateColorCharts(const uint16_t numSources,
const std::uint16_t seed = 2020)
{
std::srand(seed);
std::vector<std::array<std::uint8_t, 3>> colors;
colors.reserve(numSources);
for (std::uint16_t i = 0; i < numSources; ++i) {
colors.emplace_back(std::array<uint8_t, 3>{static_cast<uint8_t>(std::rand() % 256),
static_cast<uint8_t>(std::rand() % 256),
static_cast<uint8_t>(std::rand() % 256)});
}
return colors;
}
template <typename POINT_CLOUD_TYPE>
inline std::pair<std::vector<std::array<std::uint8_t, 3>>,
std::vector<pcl::visualization::PointCloudColorHandlerCustom<POINT_CLOUD_TYPE>>>
initPclColorHandlers(const std::vector<typename pcl::PointCloud<POINT_CLOUD_TYPE>::Ptr>& inPcls)
{
using PointCloudType = POINT_CLOUD_TYPE;
using PointCloud = pcl::PointCloud<PointCloudType>;
std::vector<pcl::visualization::PointCloudColorHandlerCustom<PointCloudType>> pclColorHandlers;
pclColorHandlers.reserve(inPcls.size());
auto colorCharts = ::generateColorCharts(inPcls.size());
for (int i = 0; i < inPcls.size(); ++i) {
const auto& curColor = colorCharts[i];
pclColorHandlers.emplace_back(pcl::visualization::PointCloudColorHandlerCustom<PointCloudType>(
inPcls[i], curColor[0], curColor[1], curColor[2]));
}
return std::make_pair(colorCharts, pclColorHandlers);
}
pcl::visualization::PCLVisualizer::Ptr initializeViewer()
{
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
pcl::PointXYZ o(0.1, 0, 0);
viewer->addSphere(o, 0.1, "sphere", 0);
viewer->setBackgroundColor(0.05, 0.05, 0.05, 0);
viewer->addCoordinateSystem(0.5);
viewer->setCameraPosition(-26, 0, 3, 10, -1, 0.5, 0, 0, 1);
return viewer;
}
} // namespace
test_project/linefit_ground_segmentation_local_gpu_test/src/CudaUtils.cu 0 → 100755
View file @ 72c0f559
/**
* @file CudaUtils.cu
*
* @author btran
*
*/
#include <dbscan/CudaUtils.cuh>
namespace cuda
{
namespace utils
{
namespace
{
__global__ void warmUpGPUKernel()
{
int idx = blockIdx.x * blockDim.x + threadIdx.x;
++idx;
}
} // namespace
cudaError_t warmUpGPU()
{
warmUpGPUKernel<<<1, 1>>>();
cudaDeviceSynchronize();
return cudaGetLastError();
}
} // namespace utils
} // namespace cuda
test_project/linefit_ground_segmentation_local_gpu_test/src/DBSCAN.cu 0 → 100755
View file @ 72c0f559
/**
* @file DBSCAN.cu
*
* @author btran
*
*/
// #include <dbscan/CudaUtils.cuh>
#include <dbscan/DBSCAN.cuh>
#include <pcl/point_types.h>
#include <thrust/device_vector.h>
#include <thrust/host_vector.h>
#include "dbscan/Types.cuh"
#include "dbscan/CudaUtils.cuh"
namespace clustering
{
namespace cuda
{
namespace
{
cudaDeviceProp dProperties;
template <typename PointType>
__global__ void makeGraphStep1Kernel(const PointType* __restrict__ points, Node* nodes, int* nodeDegs, int numPoints,
float eps, int minPoints)
{
int tid = threadIdx.x + blockIdx.x * blockDim.x;
while (tid < numPoints) {
Node* node = &nodes[tid];
const PointType* point = &points[tid];
for (int i = 0; i < numPoints; ++i) {
if (i == tid) {
continue;
}
const PointType* curPoint = &points[i];
float diffx = point->x - curPoint->x;
float diffy = point->y - curPoint->y;
float diffz = point->z - curPoint->z;
float sqrDist = diffx * diffx + diffy * diffy + diffz * diffz;
if (sqrDist < eps * eps) {
node->numNeighbors++;
}
}
if (node->numNeighbors >= minPoints) {
node->type = NodeType::CORE;
}
nodeDegs[tid] = node->numNeighbors;
tid += blockDim.x * gridDim.x;
}
}
template <typename PointType>
__global__ void makeGraphStep2Kernel(const PointType* __restrict__ points, const int* __restrict__ neighborStartIndices,
int* adjList, int numPoints, float eps)
{
int tid = threadIdx.x + blockIdx.x * blockDim.x;
while (tid < numPoints) {
const PointType* point = &points[tid];
int startIdx = neighborStartIndices[tid];
int countNeighbors = 0;
for (int i = 0; i < numPoints; ++i) {
if (i == tid) {
continue;
}
const PointType* curPoint = &points[i];
float diffx = point->x - curPoint->x;
float diffy = point->y - curPoint->y;
float diffz = point->z - curPoint->z;
float sqrDist = diffx * diffx + diffy * diffy + diffz * diffz;
if (sqrDist < eps * eps) {
adjList[startIdx + countNeighbors++] = i;
}
}
tid += blockDim.x * gridDim.x;
}
}
__global__ void BFSKernel(const Node* __restrict__ nodes, const int* __restrict__ adjList,
const int* __restrict__ neighborStartIndices, char* Fa, char* Xa, int numPoints)
{
int tid = threadIdx.x + blockIdx.x * blockDim.x;
while (tid < numPoints) {
if (Fa[tid]) {
Fa[tid] = false;
Xa[tid] = true;
int startIdx = neighborStartIndices[tid];
for (int i = 0; i < nodes[tid].numNeighbors; ++i) {
int nIdx = adjList[startIdx + i];
Fa[nIdx] = 1 - Xa[nIdx];
}
}
tid += blockDim.x * gridDim.x;
}
}
void BFS(Node* hNodes, Node* dNodes, const int* adjList, const int* neighborStartIndices, int v, int numPoints,
std::vector<int>& curCluster)
{
thrust::device_vector<char> dXa(numPoints, false);
thrust::device_vector<char> dFa(numPoints, false);
dFa[v] = true;
int numThreads = 256;
int numBlocks = std::min(dProperties.maxGridSize[0], (numPoints + numThreads - 1) / numThreads);
int countFa = 1;
while (countFa > 0) {
BFSKernel<<<numBlocks, numThreads>>>(dNodes, adjList, neighborStartIndices,
thrust::raw_pointer_cast(dFa.data()), thrust::raw_pointer_cast(dXa.data()),
numPoints);
countFa = thrust::count(thrust::device, dFa.begin(), dFa.end(), true);
}
thrust::host_vector<char> hXa = dXa;
for (int i = 0; i < numPoints; ++i) {
if (hXa[i]) {
hNodes[i].visited = true;
curCluster.emplace_back(i);
}
}
}
template <typename PointType> Graph makeGraph(const PointType* points, int numPoints, float eps, int minPoints)
{
Graph graph;
graph.nodes.resize(numPoints);
graph.neighborStartIndices.resize(numPoints);
thrust::device_vector<int> dNodeDegs(numPoints);
int numThreads = 256;
int numBlocks = std::min(dProperties.maxGridSize[0], (numPoints + numThreads - 1) / numThreads);
makeGraphStep1Kernel<PointType><<<numBlocks, numThreads>>>(points, thrust::raw_pointer_cast(graph.nodes.data()),
thrust::raw_pointer_cast(dNodeDegs.data()), numPoints,
eps, minPoints);
thrust::exclusive_scan(dNodeDegs.begin(), dNodeDegs.end(), graph.neighborStartIndices.begin());
int totalEdges = dNodeDegs.back() + graph.neighborStartIndices.back();
graph.adjList.resize(totalEdges);
makeGraphStep2Kernel<PointType>
<<<numBlocks, numThreads>>>(points, thrust::raw_pointer_cast(graph.neighborStartIndices.data()),
thrust::raw_pointer_cast(graph.adjList.data()), numPoints, eps);
return graph;
}
} // namespace
template <typename PointType>
DBSCAN<PointType>::DBSCAN(const Param& param)
: m_param(param)
, m_dPoints(nullptr)
, m_allocatedSize(-1)
{
HANDLE_ERROR(cudaGetDeviceProperties(&dProperties, 0));
}
template <typename PointType> DBSCAN<PointType>::~DBSCAN()
{
if (m_dPoints) {
HANDLE_ERROR(cudaFree(m_dPoints));
m_dPoints = nullptr;
}
}
template <typename PointType>
std::vector<std::vector<int>> DBSCAN<PointType>::run(const PointType* points, int numPoints) const
{
if (numPoints <= 0) {
throw std::runtime_error("number of points must be more than 0");
}
if (m_allocatedSize < numPoints) {
m_allocatedSize = numPoints;
if (m_dPoints) {
HANDLE_ERROR(cudaFree(m_dPoints));
}
HANDLE_ERROR(cudaMalloc((void**)&m_dPoints, numPoints * sizeof(PointType)));
}
HANDLE_ERROR(cudaMemcpy(m_dPoints, points, numPoints * sizeof(PointType), cudaMemcpyHostToDevice));
auto graph = makeGraph<PointType>(m_dPoints, numPoints, m_param.eps, m_param.minPoints);
thrust::host_vector<Node> hNodes = graph.nodes;
std::vector<std::vector<int>> clusterIndices;
for (int i = 0; i < numPoints; ++i) {
auto& curHNode = hNodes[i];
if (curHNode.visited || curHNode.type != NodeType::CORE) {
continue;
}
std::vector<int> curCluster;
curCluster.emplace_back(i);
curHNode.visited = true;
BFS(hNodes.data(), thrust::raw_pointer_cast(graph.nodes.data()), thrust::raw_pointer_cast(graph.adjList.data()),
thrust::raw_pointer_cast(graph.neighborStartIndices.data()), i, numPoints, curCluster);
clusterIndices.emplace_back(std::move(curCluster));
}
return clusterIndices;
}
#undef INSTANTIATE_TEMPLATE
#define INSTANTIATE_TEMPLATE(DATA_TYPE) template class DBSCAN<DATA_TYPE>;
INSTANTIATE_TEMPLATE(pcl::PointXYZ);
INSTANTIATE_TEMPLATE(pcl::PointXYZI);
INSTANTIATE_TEMPLATE(pcl::PointXYZRGB);
INSTANTIATE_TEMPLATE(pcl::PointNormal);
INSTANTIATE_TEMPLATE(pcl::PointXYZRGBNormal);
INSTANTIATE_TEMPLATE(pcl::PointXYZINormal);
#undef INSTANTIATE_TEMPLATE
} // namespace cuda
} // namespace clustering
test_project/linefit_ground_segmentation_local_gpu_test/src/bin.cc 0 → 100755
View file @ 72c0f559
#include "ground_segmentation/bin.h"
#include <limits>
Bin::Bin() : min_z(std::numeric_limits<double>::max()), has_point_(false) {}
Bin::Bin(const Bin& bin) : min_z(std::numeric_limits<double>::max()),
has_point_(false) {}
void Bin::addPoint(const pcl::PointXYZ& point) {
const double d = sqrt(point.x * point.x + point.y * point.y);
addPoint(d, point.z);
}
/*添加点*/
void Bin::addPoint(const double& d, const double& z) {
has_point_ = true;
if (z < min_z) {
min_z = z;
min_z_range = d;
}
}
/*判断是否有点，如果有点存在的话，将我们之前算好的min_z和min_z_range传入到point之中*/
Bin::MinZPoint Bin::getMinZPoint() {
MinZPoint point;
if (has_point_) {
point.z = min_z;
point.d = min_z_range;
}
return point;
}
test_project/linefit_ground_segmentation_local_gpu_test/src/segment.cc 0 → 100755
View file @ 72c0f559
#include "ground_segmentation/segment.h"
Segment::Segment(const unsigned int& n_bins,
const double& max_slope,
const double& max_error,
const double& long_threshold,
const double& max_long_height,
const double& max_start_height,
const double& sensor_height) :
bins_(n_bins),
max_slope_(max_slope),
max_error_(max_error),
long_threshold_(long_threshold),
max_long_height_(max_long_height),
max_start_height_(max_start_height),
sensor_height_(sensor_height){}
/*分割线拟合*/
void Segment::fitSegmentLines() {
// Find first point.
auto line_start = bins_.begin();
/*在整个的bins中找第一个点*/
while (!line_start->hasPoint()) {
++line_start;
// Stop if we reached last point.
if (line_start == bins_.end()) return;
}
// Fill lines.
bool is_long_line = false;
double cur_ground_height = -sensor_height_;
/*将线的第一个点的信息传递到*/
std::list<Bin::MinZPoint> current_line_points(1, line_start->getMinZPoint());
LocalLine cur_line = std::make_pair(0,0);
/*从第一个点开始对于bins上的每一个点都进行遍历操作*/
for (auto line_iter = line_start+1; line_iter != bins_.end(); ++line_iter) {
/*如果我们设定的线上有点，则进行后面的操作*/
if (line_iter->hasPoint()) {
Bin::MinZPoint cur_point = line_iter->getMinZPoint();
/*如果两个的线的长度大于我们设定的阈值，则我们认为这两个点之间是长线*/
if (cur_point.d - current_line_points.back().d > long_threshold_) is_long_line = true;
/*针对于后面几次遍历而言的，至少有三个点的情况下*/
if (current_line_points.size() >= 2) {
// Get expected z value to possibly reject far away points.
/*获取远离点的z值*/
double expected_z = std::numeric_limits<double>::max();
/*如果是长线段且当前线段的点数大于2，则我们获取到期待的z，这个在第一次迭代的时候不会被执行*/
if (is_long_line && current_line_points.size() > 2) {
expected_z = cur_line.first * cur_point.d + cur_line.second;
}
/*将当前点插入到current_line之中*/
current_line_points.push_back(cur_point);
/*对于当前线点传入到本地线拟合中，得到最后的结果*/
cur_line = fitLocalLine(current_line_points);
/*将我们经过本地线拟合之后的*/
const double error = getMaxError(current_line_points, cur_line);
// Check if not a good line.
/*将算出来的误差和最大误差进行比较，判断是否是一个合格的线*/
if (error > max_error_ ||
std::fabs(cur_line.first) > max_slope_ ||
is_long_line && std::fabs(expected_z - cur_point.z) > max_long_height_) {
// Add line until previous point as ground.
/*添加线直到浅一点是地面点*/
current_line_points.pop_back();
// Don't let lines with 2 base points through.
/*不要让有两个基点的线穿过*/
if (current_line_points.size() >= 3) {
/*对于当前线点进行本地拟合，得到一条新的线*/
const LocalLine new_line = fitLocalLine(current_line_points);
/*将进行处理后的点放入到线中*/
lines_.push_back(localLineToLine(new_line, current_line_points));
/*计算出当前地面点的高度*/
cur_ground_height = new_line.first * current_line_points.back().d + new_line.second;
}
// Start new line.
is_long_line = false;
/*erase在删除的过程中还是减少vector的size*/
current_line_points.erase(current_line_points.begin(), --current_line_points.end());
--line_iter;
}
// Good line, continue.
else { }
}
/*在有1到2个点的情况下的处理*/
else {
// Not enough points.
/*判断是否满足添加条件，添加这些点*/
if (cur_point.d - current_line_points.back().d < long_threshold_ &&
std::fabs(current_line_points.back().z - cur_ground_height) < max_start_height_) {
// Add point if valid.
current_line_points.push_back(cur_point);
}
/*开始一条新的线*/
else {
// Start new line.
current_line_points.clear();
current_line_points.push_back(cur_point);
}
}
}
}
// Add last line.
/*添加最后一条线*/
if (current_line_points.size() > 2) {
const LocalLine new_line = fitLocalLine(current_line_points);
lines_.push_back(localLineToLine(new_line, current_line_points));
}
}
/*本地线到线，得到两个点，构建出一条直线*/
Segment::Line Segment::localLineToLine(const LocalLine& local_line,
const std::list<Bin::MinZPoint>& line_points) {
Line line;
const double first_d = line_points.front().d;
const double second_d = line_points.back().d;
/*跟去前面的值来进行locl_line的处理*/
const double first_z = local_line.first * first_d + local_line.second;
const double second_z = local_line.first * second_d + local_line.second;
line.first.z = first_z;
line.first.d = first_d;
line.second.z = second_z;
line.second.d = second_d;
return line;
}
/*到线的垂直距离*/
double Segment::verticalDistanceToLine(const double &d, const double &z) {
static const double kMargin = 0.1;
double distance = -1;
for (auto it = lines_.begin(); it != lines_.end(); ++it) {
/*这里设定了论文中要求的距离*/
/*针对于d点，按照设定的余量在前后范围内找到两个点*/
if (it->first.d - kMargin < d && it->second.d + kMargin > d) {
/*这里是先算出斜率，将传入的两个点传入到直线中，算出一个最近的额z值差，也就是垂直的距离*/
/*算出找到的两个点之间的斜率*/
const double delta_z = it->second.z - it->first.z;
const double delta_d = it->second.d - it->first.d;
const double expected_z = (d - it->first.d)/delta_d *delta_z + it->first.z;//(delta_z/delta_d)是一个斜率
//算出最终的距离
distance = std::fabs(z - expected_z);
}
}
return distance;
}
double Segment::getMeanError(const std::list<Bin::MinZPoint> &points, const LocalLine &line) {
double error_sum = 0;
for (auto it = points.begin(); it != points.end(); ++it) {
const double residual = (line.first * it->d + line.second) - it->z;
error_sum += residual * residual;
}
return error_sum/points.size();
}
double Segment::getMaxError(const std::list<Bin::MinZPoint> &points, const LocalLine &line) {
double max_error = 0;
for (auto it = points.begin(); it != points.end(); ++it) {
const double residual = (line.first * it->d + line.second) - it->z;
const double error = residual * residual;
if (error > max_error) max_error = error;
}
return max_error;
}
/*本地线拟合*/
Segment::LocalLine Segment::fitLocalLine(const std::list<Bin::MinZPoint> &points) {
const unsigned int n_points = points.size();
Eigen::MatrixXd X(n_points, 2);
Eigen::VectorXd Y(n_points);
unsigned int counter = 0;
for (auto iter = points.begin(); iter != points.end(); ++iter) {
X(counter, 0) = iter->d;
X(counter, 1) = 1;
Y(counter) = iter->z;
++counter;
}
Eigen::VectorXd result = X.colPivHouseholderQr().solve(Y);
LocalLine line_result;
line_result.first = result(0);
line_result.second = result(1);
return line_result;
}
bool Segment::getLines(std::list<Line> *lines) {
if (lines_.empty()) {
return false;
}
else {
*lines = lines_;
return true;
}
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment