gxt_kt/guangpo_lidar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first update

Browse Source
This commit is contained in:
gxt_kt 2024-07-02 00:15:12 +08:00
commit 8b714d86bc
18 changed files with 1922 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

231
CMakeLists.txt Normal file
View File

@ -0,0 +1,231 @@
cmake_minimum_required(VERSION 3.0.2)
project(guangpo_lidar)
set(CMAKE_CXX_STANDARD 20)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
set(CXX_FLAGS "-Wall")
set(CMAKE_CXX_FLAGS, "${CXX_FLAGS}")
find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
# yaml-cpp
find_package(yaml-cpp REQUIRED)
include_directories(yaml_config)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
cv_bridge
pcl_ros
roscpp
rospy
sensor_msgs
std_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# sensor_msgs# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES guangpo_lidar
# CATKIN_DEPENDS cv_bridge pcl_ros roscpp rospy sensor_msgs std_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
${catkin_INCLUDE_DIRS}
src
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/guangpo_lidar.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/guangpo_lidar_node.cpp)
set(SRC src/main_ros.cpp src/render/render.cpp src/processPointClouds.cpp)
list(APPEND SRC yaml_config/yaml_config.cpp)
add_executable(${PROJECT_NAME}_node ${SRC})
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_node ${PCL_LIBRARIES})
target_link_libraries(${PROJECT_NAME}_node yaml-cpp)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_guangpo_lidar.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

77
package.xml Normal file
View File

@ -0,0 +1,77 @@
<?xml version="1.0"?>
<package format="2">
<name>guangpo_lidar</name>
<version>0.0.0</version>
<description>The guangpo_lidar package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="ubuntu@todo.todo">ubuntu</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/guangpo_lidar</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>cv_bridge</build_export_depend>
<build_export_depend>pcl_ros</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>pcl_ros</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

52
src/cluster3d.cpp Normal file
View File

@ -0,0 +1,52 @@
//
// Created by hyin on 2020/3/25.
//
#include "cluster3d.h"
using namespace lidar_obstacle_detection;
template<typename PointT>
ClusterPts<PointT>::~ClusterPts() {}
template<typename PointT>
void ClusterPts<PointT>::clusterHelper(int ind, PtCdtr<PointT> cloud, std::vector<int> &cluster, KdTree *tree) {
processed[ind] = true;
cluster.push_back(ind);
std::vector<int> nearest_point = tree->search(cloud->points[ind], distanceTol);
for (int nearest_id:nearest_point) {
if (!processed[nearest_id]) {
clusterHelper(nearest_id, cloud, cluster, tree);
}
}
}
template<typename PointT>
std::vector<PtCdtr<PointT>> ClusterPts<PointT>::EuclidCluster(PtCdtr<PointT> cloud) {
KdTree *tree = new KdTree;
for (int ind = 0; ind < num_points; ind++) {
tree->insert(cloud->points[ind], ind);
}
for (int ind = 0; ind < num_points; ind++) {
if (processed[ind]) {
ind++;
continue;
}
std::vector<int> cluster_ind;
PtCdtr<PointT> cloudCluster(new pcl::PointCloud<PointT>);
clusterHelper(ind, cloud, cluster_ind, tree);
int cluster_size = cluster_ind.size();
if (cluster_size >= minClusterSize && cluster_size <= maxClusterSize) {
for (int i = 0; i < cluster_size; i++) {
cloudCluster->points.push_back(cloud->points[cluster_ind[i]]);
}
cloudCluster->width = cloudCluster->points.size();
cloudCluster->height = 1;
clusters.push_back(cloudCluster);
}
}
return clusters;
}

44
src/cluster3d.h Normal file
View File

@ -0,0 +1,44 @@
//
// Created by hyin on 2020/3/25.
//
#ifndef PLAYBACK_CLUSTER3D_H
#define PLAYBACK_CLUSTER3D_H
#include <pcl/common/common.h>
#include <chrono>
#include <string>
#include "kdtree3d.h"
namespace lidar_obstacle_detection {
// shorthand for point cloud pointer
template<typename PointT>
using PtCdtr = typename pcl::PointCloud<PointT>::Ptr;
template<typename PointT>
class ClusterPts {
private:
int num_points;
float distanceTol;
int minClusterSize;
int maxClusterSize;
std::vector<bool> processed;
std::vector<PtCdtr<PointT>> clusters;
public:
ClusterPts(int nPts, float cTol, int minSize, int maxSize) : num_points(nPts), distanceTol(cTol),
minClusterSize(minSize), maxClusterSize(maxSize) {
processed.assign(num_points, false);
}
~ClusterPts();
void
clusterHelper(int ind, PtCdtr<PointT> cloud, std::vector<int> &cluster, KdTree *tree);
std::vector<PtCdtr<PointT>> EuclidCluster(PtCdtr<PointT> cloud);
};
}
#endif //PLAYBACK_CLUSTER3D_H

194
src/environment.cpp Normal file
View File

@ -0,0 +1,194 @@
//
// Created by hyin on 2020/3/25.
//
// for exploring self-driving car sensors
#include "render/render.h"
#include "processPointClouds.h"
// using templates for processPointClouds so also include .cpp to help linker
#include "processPointClouds.cpp"
#include "yaml_config.h"
#include "obstacles.hpp"
using namespace lidar_obstacle_detection;
// Test read Lidar data
void cityBlock(pcl::visualization::PCLVisualizer::Ptr &viewer, ProcessPointClouds<pcl::PointXYZI> *pointProcessorI,
const pcl::PointCloud<pcl::PointXYZI>::Ptr &inputCloud) {
// ----------------------------------------------------
// -----Open 3D viewer and display City Block -----
// ----------------------------------------------------
// Setting hyper parameters
// FilterCloud
// float filterRes = 0.4;
float filterRes=yaml_config["filterRes"].as<float>();
// Eigen::Vector4f minpoint(-10, -6.5, -2, 1);
// Eigen::Vector4f maxpoint(30, 6.5, 1, 1);
double max_x=yaml_config["roi"]["max_x"].as<double>();
double max_y=yaml_config["roi"]["max_y"].as<double>();
double max_z=yaml_config["roi"]["max_z"].as<double>();
double min_x=yaml_config["roi"]["min_x"].as<double>();
double min_y=yaml_config["roi"]["min_y"].as<double>();
double min_z=yaml_config["roi"]["min_z"].as<double>();
Eigen::Vector4f maxpoint(max_x,max_y,max_z,1);
Eigen::Vector4f minpoint(min_x,min_y,min_z,1);
// SegmentPlane
// int maxIterations = 40;
int maxIterations=yaml_config["ransac"]["maxIterations"].as<int>();
// float distanceThreshold = 0.3;
float distanceThreshold=yaml_config["ransac"]["distanceThreshold"].as<float>();
// 聚类参数设置
// Clustering
// float clusterTolerance = 0.5;
// int minsize = 10;
// int maxsize = 140;
float clusterTolerance = yaml_config["cluster"]["clusterTolerance"].as<float>();
int minsize = yaml_config["cluster"]["minsize"].as<int>();
int maxsize = yaml_config["cluster"]["maxsize"].as<int>();
// 1. 降采样+设置roi区域+去除车辆相关点云
// First:Filter cloud to reduce amount of points
pcl::PointCloud<pcl::PointXYZI>::Ptr filteredCloud = pointProcessorI->FilterCloud(inputCloud, filterRes, minpoint,
maxpoint);
// 2. 把点云分离出路面
// Second: Segment the filtered cloud into obstacles and road
std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, pcl::PointCloud<pcl::PointXYZI>::Ptr> segmentCloud = pointProcessorI->RansacSegmentPlane(
filteredCloud, maxIterations, distanceThreshold);
// renderPointCloud(viewer, segmentCloud.first, "obstCloud", Color(1, 0, 0));
renderPointCloud(viewer, segmentCloud.second, "planeCloud", Color(0, 1, 0));
// renderPointCloud(viewer,inputCloud,"inputCloud");
// Third: Cluster different obstacle cloud
// std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> cloudClusters = pointProcessorI->EuclideanClustering(segmentCloud.first,
// clusterTolerance,
// minsize, maxsize);
std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> cloudClusters = pointProcessorI->GxtEuclideanClustering(segmentCloud.first,
clusterTolerance,
minsize, maxsize);
std::vector<Eigen::Vector3f> centroids;
std::vector<Eigen::Vector3f> sizes;
std::vector<Eigen::Quaternionf> orientations;
std::cout << "begin ExtractObstacles" << std::endl;
ExtractObstacles(cloudClusters,centroids,sizes,orientations);
visualizeObstacles(cloudClusters,centroids,sizes,orientations);
int clusterId = 0;
std::vector<Color> colors = {Color(1, 0, 0), Color(0, 1, 0), Color(0, 0, 1)};
for (pcl::PointCloud<pcl::PointXYZI>::Ptr cluster : cloudClusters) {
std::cout << "cluster size";
pointProcessorI->numPoints(cluster);
renderPointCloud(viewer, cluster, "obstCLoud" + std::to_string(clusterId),
colors[clusterId % colors.size()]);
// Fourth: Find bounding boxes for each obstacle cluster
Box box = pointProcessorI->BoundingBox(cluster);
renderBox(viewer, box, clusterId);
++clusterId;
}
}
//setAngle: SWITCH CAMERA ANGLE {XY, TopDown, Side, FPS}
void initCamera(CameraAngle setAngle, pcl::visualization::PCLVisualizer::Ptr &viewer) {
viewer->setBackgroundColor(0, 0, 0);
// set camera position and angle
viewer->initCameraParameters();
// distance away in meters
int distance = 16;
switch (setAngle) {
case XY :
viewer->setCameraPosition(-distance, -distance, distance, 1, 1, 0);
break;
case TopDown :
viewer->setCameraPosition(0, 0, distance, 1, 0, 1);
break;
case Side :
viewer->setCameraPosition(0, -distance, 0, 0, 0, 1);
break;
case FPS :
viewer->setCameraPosition(-10, 0, 0, 0, 0, 1);
}
if (setAngle != FPS)
viewer->addCoordinateSystem(1.0);
}
// 计算旋转矩阵
Eigen::Matrix4d TransforMatrix(Eigen::Vector3d translation,double roll,double pitch,double yaw) {
Eigen::Quaterniond q = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity(); // 初始化为单位矩阵
transformation_matrix.block<3, 3>(0, 0) = q.toRotationMatrix(); // 设置旋转矩阵部分
transformation_matrix.block<3, 1>(0, 3) = translation; // 设置平移向量部分
std::cout << "旋转矩阵：" << std::endl << transformation_matrix << std::endl;
return transformation_matrix;
}
// char* argv[] means array of char pointers, whereas char** argv means pointer to a char pointer.
int main(int argc, char **argv) {
std::cout << "starting enviroment" << std::endl;
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
CameraAngle setAngle = XY;
initCamera(setAngle, viewer);
// For simpleHighway function
// simpleHighway(viewer);
// cityBlock(viewer);
// while (!viewer->wasStopped ())
// {
// viewer->spinOnce ();
// }
//
std::string pcd_files_path = "src/sensors/data/pcd/data_1";
if (argc >= 2) {
pcd_files_path = argv[1];
}
std::cout << "Read pcd file path: " << pcd_files_path << std::endl;
// Stream cityBlock function
ProcessPointClouds<pcl::PointXYZI> *pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
std::vector<std::filesystem::path> stream = pointProcessorI->streamPcd(pcd_files_path);
auto streamIterator = stream.begin();
pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloudI;
Eigen::Vector3d trans;
trans<<0.3643,0.2078,1.21;
Eigen::Matrix4d tran_matrix=TransforMatrix(trans,-179.5*M_PI/180.0,0.5*M_PI/180.0,360*M_PI/180.0);
Eigen::Affine3d affine3d_transform(tran_matrix); // 转换为Affine3d类型
Eigen::Affine3f affine3f_transform = affine3d_transform.cast<float>(); // 转换为Affine3f类型
affine3f_transform=affine3f_transform.inverse();
//
while (!viewer->wasStopped()) {
// Clear viewer
viewer->removeAllPointClouds();
viewer->removeAllShapes();
// Load pcd and run obstacle detection process
inputCloudI = pointProcessorI->loadPcd((*streamIterator).string());
// 坐标系转换
pcl::transformPointCloud(*inputCloudI, *inputCloudI, affine3f_transform);
cityBlock(viewer, pointProcessorI, inputCloudI);
streamIterator++;
if (streamIterator == stream.end()) {
streamIterator = stream.begin();
}
viewer->spinOnce();
}
}

110
src/kdtree3d.h Normal file
View File

@ -0,0 +1,110 @@
//
// Created by hyin on 2020/3/25.
//
#ifndef PLAYBACK_KDTREE3D_H
#define PLAYBACK_KDTREE3D_H
#include <pcl/impl/point_types.hpp>
#include <vector>
struct Node {
pcl::PointXYZI point;
int id;
Node *left;
Node *right;
Node(pcl::PointXYZI arr, int setId) : point(arr), id(setId), left(NULL), right(NULL) {}
};
struct KdTree {
Node *root;
KdTree() : root(NULL) {}
void insertHelper(Node **node, int depth, pcl::PointXYZI point, int id) {
// Tree is empty
if (*node == NULL) {
*node = new Node{point, id};
} else {
// calculate current din
int cd = depth % 3;
if (cd == 0) {
if (point.x < (*node)->point.x) {
insertHelper(&((*node)->left), depth + 1, point, id);
} else {
insertHelper(&((*node)->right), depth + 1, point, id);
}
} else if (cd == 1) {
if (point.y < (*node)->point.y) {
insertHelper(&((*node)->left), depth + 1, point, id);
} else {
insertHelper(&((*node)->right), depth + 1, point, id);
}
} else {
if (point.z < (*node)->point.z) {
insertHelper(&((*node)->left), depth + 1, point, id);
} else {
insertHelper(&((*node)->right), depth + 1, point, id);
}
}
}
}
void insert(pcl::PointXYZI point, int id) {
// the function should create a new node and place correctly with in the root
insertHelper(&root, 0, point, id);
}
void searchHelper(pcl::PointXYZI target, Node *node, int depth, float distanceTol, std::vector<int> &ids) {
if (node != NULL) {
float delta_x = node->point.x - target.x;
float delta_y = node->point.y - target.y;
float delta_z = node->point.z - target.z;
if ((delta_x >= -distanceTol && delta_x <= distanceTol) &&
(delta_y >= -distanceTol && delta_y <= distanceTol) &&
(delta_z >= -distanceTol && delta_z <= distanceTol)) {
float distance = sqrt(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z);
if (distance <= distanceTol) {
ids.push_back(node->id);
}
}
// check across boundary
if (depth % 3 == 0) {
if (delta_x > -distanceTol) {
searchHelper(target, node->left, depth + 1, distanceTol, ids);
}
if (delta_x < distanceTol) {
searchHelper(target, node->right, depth + 1, distanceTol, ids);
}
} else if (depth % 3 == 1) {
if (delta_y > -distanceTol) {
searchHelper(target, node->left, depth + 1, distanceTol, ids);
}
if (delta_y < distanceTol) {
searchHelper(target, node->right, depth + 1, distanceTol, ids);
}
} else {
if (delta_z > -distanceTol) {
searchHelper(target, node->left, depth + 1, distanceTol, ids);
}
if (delta_z < distanceTol) {
searchHelper(target, node->right, depth + 1, distanceTol, ids);
}
}
}
}
// return a list of point ids in the tree that are within distance of target
std::vector<int> search(pcl::PointXYZI target, float distanceTol)
{
std::vector<int> ids;
searchHelper(target, root, 0, distanceTol, ids);
return ids;
}
};
#endif //PLAYBACK_KDTREE3D_H

69
src/main_ros.cpp Normal file
View File

@ -0,0 +1,69 @@
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include "render/render.h"
#include "processPointClouds.h"
// using templates for processPointClouds so also include .cpp to help linker
#include "processPointClouds.cpp"
#include "yaml_config.h"
#include "obstacles.hpp"
using namespace lidar_obstacle_detection;
class PCLSubscriber {
public:
PCLSubscriber(ros::NodeHandle& nh) {
std::string sub_topic=yaml_config["sub_cloud_topic"].as<std::string>();
std::cout << "订阅点云话题" << sub_topic << std::endl;
std::string pub_topic=yaml_config["pub_cloud_topic"].as<std::string>();
std::cout << "发布点云话题" << pub_topic << std::endl;
sub_ = nh.subscribe<sensor_msgs::PointCloud2>(sub_topic, 10, &PCLSubscriber::pointCloudCallback, this);
pub_ = nh.advertise<sensor_msgs::PointCloud2>(pub_topic, 1);
}
void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) {
pcl::PCLPointCloud2 pcl_pc2;
pcl_conversions::toPCL(*msg, pcl_pc2);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromPCLPointCloud2(pcl_pc2, *cloud);
processPointCloud(cloud);
publishCloud(cloud);
}
private:
void processPointCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud) {
// 在这里添加对点云数据的处理逻辑
ROS_INFO("收到包含 %lu 个点的点云数据", cloud->size());
}
void publishCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud) {
// 将 pcl::PointCloud 转换为 sensor_msgs::PointCloud2
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*cloud, msg);
msg.header.frame_id = "world";
msg.header.stamp = ros::Time::now();
pub_.publish(msg);
}
ros::Subscriber sub_;
ros::Publisher pub_;
};
int main(int argc, char** argv) {
ros::init(argc, argv, "pcl_subscriber");
ros::NodeHandle nh;
PCLSubscriber subscriber(nh);
ros::spin();
return 0;
}

157
src/obstacles.hpp Normal file
View File

@ -0,0 +1,157 @@
#pragma once
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>
#include <pcl/common/pca.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/common.h>
#include <pcl/common/eigen.h>
#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
// reference https://pointclouds.org/documentation/tutorials/moment_of_inertia.html
/**
* 从聚类结果中提取障碍物信息
* @param obstacle_clouds 输出的障碍物点云集合
* @param obstacle_centroids 输出的障碍物质心集合
* @param obstacle_sizes 输出的障碍物尺寸集合
* @param obstacle_orientations 输出的障碍物旋转集合
*/
void ExtractObstacles(
std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>& obstacle_clouds,
std::vector<Eigen::Vector3f>& obstacle_centroids,
std::vector<Eigen::Vector3f>& obstacle_sizes,
std::vector<Eigen::Quaternionf>& obstacle_orientations)
{
obstacle_centroids.clear();
obstacle_sizes.clear();
obstacle_orientations.clear();
for (const auto &obstacle_cloud : obstacle_clouds) {
pcl::MomentOfInertiaEstimation<pcl::PointXYZI> feature_extractor;
feature_extractor.setInputCloud(obstacle_cloud);
feature_extractor.compute();
pcl::PointXYZI min_point_OBB;
pcl::PointXYZI max_point_OBB;
pcl::PointXYZI position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB,
rotational_matrix_OBB);
pcl::visualization::PCLVisualizer::Ptr viewer(
new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0);
viewer->addCoordinateSystem(1.0);
viewer->initCameraParameters();
viewer->addPointCloud<pcl::PointXYZI>(obstacle_cloud, "sample cloud");
Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
Eigen::Quaternionf quat(rotational_matrix_OBB);
// 添加可视化
// viewer->addCube(position, quat, max_point_OBB.x - min_point_OBB.x,
// max_point_OBB.y - min_point_OBB.y,
// max_point_OBB.z - min_point_OBB.z, "OBB");
// viewer->setShapeRenderingProperties(
// pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
// pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "OBB");
obstacle_centroids.push_back(position);
obstacle_orientations.push_back(quat);
Eigen::Vector3f size;
size << (max_point_OBB.x - min_point_OBB.x) ,(max_point_OBB.y - min_point_OBB.y),(max_point_OBB.z - min_point_OBB.z);
obstacle_sizes.push_back(size);
}
// debug
if(0) {
std::cout << obstacle_centroids.size();
for(int i=0;i < obstacle_centroids.size();i++) {
std::cout << "====" << std::endl;
std::cout << obstacle_centroids[i] << std::endl;
std::cout << obstacle_sizes[i] << std::endl;
// std::cout << obstacle_orientations[i] << std::endl;
}
}
}
/**
* 可视化障碍物
* @param viewer PCL可视化器
* @param obstacle_clouds 障碍物点云集合
* @param obstacle_centroids 障碍物质心集合
* @param obstacle_sizes 障碍物尺寸集合
* @param obstacle_orientations 障碍物旋转集合
*/
void visualizeObstacles(
const std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>& cloudClusters,
const std::vector<Eigen::Vector3f>& obstacle_centroids,
const std::vector<Eigen::Vector3f>& obstacle_sizes,
const std::vector<Eigen::Quaternionf>& obstacle_orientations)
{
// 创建可视化窗口
std::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0);
// 绘制包围盒
for (size_t i = 0; i < obstacle_centroids.size(); i++)
{
std::stringstream cloud_name;
cloud_name << "cloud_" << i;
viewer->addPointCloud<pcl::PointXYZI>(cloudClusters[i], cloud_name.str());
std::stringstream box_name;
box_name << "box_" << i;
viewer->addCube(obstacle_centroids[i], obstacle_orientations[i], obstacle_sizes[i][0],
obstacle_sizes[i][1],
obstacle_sizes[i][2], box_name.str());
// 设置颜色
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,
0.5,0,0, box_name.str());
viewer->setShapeRenderingProperties(
pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, box_name.str());
}
// 启动可视化
while (!viewer->wasStopped()) {
viewer->spinOnce();
// std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
}
// 计算旋转矩阵
Eigen::Matrix4d TransforMatrix(Eigen::Vector3d translation,double roll,double pitch,double yaw) {
Eigen::Quaterniond q = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity(); // 初始化为单位矩阵
transformation_matrix.block<3, 3>(0, 0) = q.toRotationMatrix(); // 设置旋转矩阵部分
transformation_matrix.block<3, 1>(0, 3) = translation; // 设置平移向量部分
std::cout << "旋转矩阵：" << std::endl << transformation_matrix << std::endl;
return transformation_matrix;
}

461
src/processPointClouds.cpp Normal file
View File

@ -0,0 +1,461 @@
// PCL lib Functions for processing point clouds
#include "processPointClouds.h"
#include "yaml_config.h"
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_color_handlers.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_clusters.h>
using namespace lidar_obstacle_detection;
//constructor:
template<typename PointT>
ProcessPointClouds<PointT>::ProcessPointClouds() {}
//de-constructor:
template<typename PointT>
ProcessPointClouds<PointT>::~ProcessPointClouds() {}
template<typename PointT>
void ProcessPointClouds<PointT>::numPoints(PtCdtr<PointT> cloud) { std::cout << cloud->points.size() << std::endl; }
template<typename PointT>
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>
ProcessPointClouds<PointT>::RansacSegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceTol) {
#if 0
// Count time
auto startTime = std::chrono::steady_clock::now();
srand(time(NULL));
int num_points = cloud->points.size();
auto cloud_points = cloud->points;
Ransac<PointT> RansacSeg(maxIterations, distanceTol, num_points);
// Get inliers from RANSAC implementation
std::unordered_set<int> inliersResult = RansacSeg.Ransac3d(cloud);
auto endTime = std::chrono::steady_clock::now();
auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
std::cout << "plane ransac-segment took " << elapsedTime.count() << " milliseconds" << std::endl;
PtCdtr<PointT> out_plane(new pcl::PointCloud<PointT>());
PtCdtr<PointT> in_plane(new pcl::PointCloud<PointT>());
for (int i = 0; i < num_points; i++) {
PointT pt = cloud_points[i];
if (inliersResult.count(i)) {
out_plane->points.push_back(pt);
} else {
in_plane->points.push_back(pt);
}
}
return std::pair<PtCdtr<PointT>, PtCdtr<PointT>>(in_plane, out_plane);
#else
// 使用pcl的分割，分离出地面
// Count time
auto startTime = std::chrono::steady_clock::now();
// Create the segmentation object
pcl::SACSegmentation<PointT> seg;
// Optional
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(maxIterations);
seg.setDistanceThreshold(distanceTol);
// Segment the largest planar component
// PtCdtr<PointT> cloud_plane(new pcl::PointCloud<PointT>());
// PtCdtr<PointT> cloud_out(new pcl::PointCloud<PointT>());
pcl::ModelCoefficients::Ptr cloud_out(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr cloud_plane(new pcl::PointIndices);
seg.setInputCloud(cloud);
seg.segment(*cloud_plane, *cloud_out);
auto endTime = std::chrono::steady_clock::now();
auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
std::cout << "plane ransac-segment took " << elapsedTime.count() << " milliseconds" << std::endl;
PtCdtr<PointT> in_plane(new pcl::PointCloud<PointT>());
PtCdtr<PointT> out_plane(new pcl::PointCloud<PointT>());
// Extract the inliers
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud(cloud);
extract.setIndices(cloud_plane);
extract.setNegative(false);
extract.filter(*in_plane);
// Extract the outliers
extract.setNegative(true);
extract.filter(*out_plane);
#endif
bool if_view_segment = yaml_config["view"]["segment"].as<bool>();
if (if_view_segment) {
// 创建可视化窗口
pcl::visualization::PCLVisualizer::Ptr viewer( new pcl::visualization::PCLVisualizer("RANSAC Plane Segmentation"));
viewer->setWindowName("My Plane Segmentation Viewer");
viewer->setBackgroundColor(0, 0, 0);
// 添加坐标系
viewer->addCoordinateSystem(1.0);
// 添加栅格
for (float x = -10.0; x <= 10.0; x += 1.0) {
viewer->addLine(pcl::PointXYZ(x, -10, 0), pcl::PointXYZ(x, 10, 0),0.5,0.5,0.5,
std::string("grid_")+std::to_string(x)+"y");
}
for (float y = -10.0; y <= 10.0; y += 1.0) {
viewer->addLine(pcl::PointXYZ(-10, y, 0), pcl::PointXYZ(10, y, 0),0.5,0.5,0.5,
std::string("grid_")+std::to_string(y)+"x");
}
// for (float x = -10.0; x <= 10.0; x += 1.0) {
// for (float y = -10.0; y <= 10.0; y += 1.0) {
// // viewer->addLine(pcl::PointXYZ(x, y, 0), pcl::PointXYZ(x, y + 1, 0),
// // 0.5, 0.5, 0.5,
// // "grid_" + std::to_string(static_cast<int>(x)) + "_" +
// // std::to_string(static_cast<int>(y)));
// // viewer->addLine(pcl::PointXYZ(x, y, 0), pcl::PointXYZ(x + 1, y, 0),
// // 0.5, 0.5, 0.5,
// // "grid_" + std::to_string(static_cast<int>(x)) + "_" +
// // std::to_string(static_cast<int>(y)));
// }
// }
// 可视化地面点云
pcl::visualization::PointCloudColorHandlerCustom<PointT> plane_color( in_plane, 255, 0, 0);
viewer->addPointCloud<PointT>(in_plane, plane_color, "plane");
viewer->setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "plane");
// 可视化非地面点云
pcl::visualization::PointCloudColorHandlerCustom<PointT> non_plane_color( out_plane, 0, 255, 0);
viewer->addPointCloud<PointT>(out_plane, non_plane_color, "non_plane");
viewer->setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "non_plane");
// 可视化整个点云
// pcl::visualization::PointCloudColorHandlerCustom<PointT>
// full_cloud_color(cloud, 255, 255, 255);
// viewer->addPointCloud<PointT>(cloud, full_cloud_color, "full_cloud");
// viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
// 2, "full_cloud");
// 显示可视化窗口
while (!viewer->wasStopped()) {
viewer->spinOnce();
}
}
return std::make_pair(out_plane, in_plane);
}
template<typename PointT>
PtCdtr<PointT> ProcessPointClouds<PointT>::FilterCloud(PtCdtr<PointT> cloud, float filterRes, Eigen::Vector4f minPoint,
Eigen::Vector4f maxPoint) {
// Time segmentation process
auto startTime = std::chrono::steady_clock::now();
// TODO:: Fill in the function to do voxel grid point reduction and region based filtering
// Create the filtering object: downsample the dataset using a leaf size of .2m
// 体素降采样
pcl::VoxelGrid<PointT> vg;
PtCdtr<PointT> cloudFiltered(new pcl::PointCloud<PointT>);
vg.setInputCloud(cloud);
vg.setLeafSize(filterRes, filterRes, filterRes);
vg.filter(*cloudFiltered);
// 设置roi区域
PtCdtr<PointT> cloudRegion(new pcl::PointCloud<PointT>);
pcl::CropBox<PointT> region(true);
region.setMin(minPoint);
region.setMax(maxPoint);
region.setInputCloud(cloudFiltered);
region.filter(*cloudRegion);
// 去除屋顶点云
std::vector<int> indices;
pcl::CropBox<PointT> roof(true);
// roof.setMin(Eigen::Vector4f(-1.5, -1.7, -1, 1));
// roof.setMax(Eigen::Vector4f(2.6, 1.7, -0.4, 1));
double max_x=yaml_config["roof"]["max_x"].as<double>();
double max_y=yaml_config["roof"]["max_y"].as<double>();
double max_z=yaml_config["roof"]["max_z"].as<double>();
double min_x=yaml_config["roof"]["min_x"].as<double>();
double min_y=yaml_config["roof"]["min_y"].as<double>();
double min_z=yaml_config["roof"]["min_z"].as<double>();
roof.setMin(Eigen::Vector4f(min_x, min_y, min_z, 1));
roof.setMax(Eigen::Vector4f(max_x, max_y, max_z, 1));
roof.setInputCloud(cloudRegion);
roof.filter(indices);
pcl::PointIndices::Ptr inliers{new pcl::PointIndices};
for (int point : indices) {
inliers->indices.push_back(point);
}
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud(cloudRegion);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*cloudRegion);
auto endTime = std::chrono::steady_clock::now();
auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
std::cout << "filtering took " << elapsedTime.count() << " milliseconds" << std::endl;
// return cloud;
return cloudRegion;
}
template<typename PointT>
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>
ProcessPointClouds<PointT>::SeparateClouds(pcl::PointIndices::Ptr inliers, PtCdtr<PointT> cloud) {
// TODO: Create two new point clouds, one cloud with obstacles and other with segmented plane
PtCdtr<PointT> obstCloud(new pcl::PointCloud<PointT>());
PtCdtr<PointT> planeCloud(new pcl::PointCloud<PointT>());
for (int index : inliers->indices) {
planeCloud->points.push_back(cloud->points[index]);
}
// create extraction object
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*obstCloud);
std::pair<PtCdtr<PointT>, PtCdtr<PointT>> segResult(obstCloud,
planeCloud);
// std::pair<PtCdtr<PointT>, PtCdtr<PointT>> segResult(cloud, cloud);
return segResult;
}
template<typename PointT>
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>
ProcessPointClouds<PointT>::SegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceThreshold) {
// Time segmentation process
auto startTime = std::chrono::steady_clock::now();
// pcl::PointIndices::Ptr inliers; // Build on the stack
pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // Build on the heap
// TODO:: Fill in this function to find inliers for the cloud.
pcl::ModelCoefficients::Ptr coefficient(new pcl::ModelCoefficients);
pcl::SACSegmentation<PointT> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(maxIterations);
seg.setDistanceThreshold(distanceThreshold);
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficient);
if (inliers->indices.size() == 0) {
std::cerr << "Could not estimate a planar model for the given dataset" << std::endl;
}
auto endTime = std::chrono::steady_clock::now();
auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
std::cout << "plane segmentation took " << elapsedTime.count() << " milliseconds" << std::endl;
std::pair<PtCdtr<PointT>, PtCdtr<PointT>> segResult = SeparateClouds(
inliers, cloud);
return segResult;
}
template<typename PointT>
std::vector<PtCdtr<PointT>>
ProcessPointClouds<PointT>::Clustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize, int maxSize) {
// Time clustering process
auto startTime = std::chrono::steady_clock::now();
std::vector<PtCdtr<PointT>> clusters;
// TODO:: Fill in the function to perform euclidean clustering to group detected obstacles
// Build Kd-Tree Object
typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
// Input obstacle point cloud to create KD-tree
tree->setInputCloud(cloud);
std::vector<pcl::PointIndices> clusterIndices; // this is point cloud indice type
pcl::EuclideanClusterExtraction<PointT> ec; // clustering object
ec.setClusterTolerance(clusterTolerance);
ec.setMinClusterSize(minSize);
ec.setMaxClusterSize(maxSize);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud); // feed point cloud
ec.extract(clusterIndices); // get all clusters Indice
// For each cluster indice
for (pcl::PointIndices getIndices: clusterIndices) {
PtCdtr<PointT> cloudCluster(new pcl::PointCloud<PointT>);
// For each point indice in each cluster
for (int index:getIndices.indices) {
cloudCluster->points.push_back(cloud->points[index]);
}
cloudCluster->width = cloudCluster->points.size();
cloudCluster->height = 1;
cloudCluster->is_dense = true;
clusters.push_back(cloudCluster);
}
auto endTime = std::chrono::steady_clock::now();
auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
std::cout << "clustering took " << elapsedTime.count() << " milliseconds and found " << clusters.size()
<< " clusters" << std::endl;
return clusters;
}
/**
* 使用欧式聚类对点云进行分割
* @param input_cloud 输入点云
* @param cluster_tolerance 聚类距离阈值(单位:米)
* @param min_cluster_size 最小聚类尺寸
* @param max_cluster_size 最大聚类尺寸
* @param output_clouds 输出聚类结果点云向量
*/
template<typename PointT>
std::vector<PtCdtr<PointT>> ProcessPointClouds<PointT>::GxtEuclideanClustering(
PtCdtr<PointT> input_cloud,
float cluster_tolerance,
int min_cluster_size,
int max_cluster_size) {
// 创建欧式聚类对象
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance(cluster_tolerance);
ec.setMinClusterSize(min_cluster_size);
ec.setMaxClusterSize(max_cluster_size);
ec.setSearchMethod(pcl::search::KdTree<pcl::PointXYZI>::Ptr(new pcl::search::KdTree<PointT>));
// 提取聚类
std::vector<pcl::PointIndices> cluster_indices;
ec.setInputCloud(input_cloud);
ec.extract(cluster_indices);
std::vector<PtCdtr<PointT>> output_clouds;
// 生成聚类结果点云
output_clouds.clear();
for (const auto& indices : cluster_indices)
{
PtCdtr<PointT> cluster(new pcl::PointCloud<PointT>);
for (int index : indices.indices)
{
cluster->push_back(input_cloud->at(index));
}
output_clouds.push_back(cluster);
}
if (false) {
// 可视化聚类结果
pcl::visualization::PCLVisualizer viewer("Euclidean Clustering");
viewer.setWindowName("My Plane Segmentation Viewer");
viewer.setBackgroundColor(0, 0, 0);
int j = 0;
for (const auto &indices : cluster_indices) {
pcl::PointCloud<pcl::PointXYZI>::Ptr cluster( new pcl::PointCloud<pcl::PointXYZI>);
for (int index : indices.indices) { cluster->push_back(input_cloud->at(index)); }
viewer.addPointCloud<pcl::PointXYZI>(cluster, "cluster_" + std::to_string(j++)); viewer.setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2,
"cluster_" + std::to_string(j - 1));
viewer.setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_COLOR,
(float)rand() / (float)RAND_MAX, (float)rand() / (float)RAND_MAX,
(float)rand() / (float)RAND_MAX,
"cluster_" + std::to_string(j - 1));
}
viewer.spin();
// // 显示可视化窗口
// while (!viewer.wasStopped()) {
// viewer.spinOnce();
// }
}
return output_clouds;
}
template<typename PointT>
std::vector<PtCdtr<PointT>>
ProcessPointClouds<PointT>::EuclideanClustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize,
int maxSize) {
// Time clustering process
auto startTime = std::chrono::steady_clock::now();
// std::vector<PtCdtr<PointT>> clusters;
ClusterPts<PointT> clusterPoints(cloud->points.size(), clusterTolerance, minSize, maxSize);
std::vector<PtCdtr<PointT>> clusters = clusterPoints.EuclidCluster(cloud);
auto endTime = std::chrono::steady_clock::now();
auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
std::cout << "KDTree clustering took " << elapsedTime.count() << " milliseconds and found " << clusters.size()
<< " clusters" << std::endl;
return clusters;
}
template<typename PointT>
Box ProcessPointClouds<PointT>::BoundingBox(PtCdtr<PointT> cluster) {
// Find bounding box for one of the clusters
PointT minPoint, maxPoint;
pcl::getMinMax3D(*cluster, minPoint, maxPoint);
Box box;
box.x_min = minPoint.x;
box.y_min = minPoint.y;
box.z_min = minPoint.z;
box.x_max = maxPoint.x;
box.y_max = maxPoint.y;
box.z_max = maxPoint.z;
return box;
}
template<typename PointT>
void ProcessPointClouds<PointT>::savePcd(PtCdtr<PointT> cloud, std::string file) {
pcl::io::savePCDFileASCII(file, *cloud);
std::cerr << "Saved " << cloud->points.size() << " data points to " + file << std::endl;
}
template<typename PointT>
PtCdtr<PointT> ProcessPointClouds<PointT>::loadPcd(std::string file) {
PtCdtr<PointT> cloud(new pcl::PointCloud<PointT>);
if (pcl::io::loadPCDFile<PointT>(file, *cloud) == -1) { //* load the file
PCL_ERROR ("Couldn't read file \n");
}
std::cerr << "Loaded " << cloud->points.size() << " data points from " + file << std::endl;
return cloud;
}
template<typename PointT>
std::vector<std::filesystem::path> ProcessPointClouds<PointT>::streamPcd(std::string dataPath) {
std::vector<std::filesystem::path> paths(std::filesystem::directory_iterator{dataPath},
std::filesystem::directory_iterator{});
// sort files in accending order so playback is chronological
sort(paths.begin(), paths.end());
return paths;
}

69
src/processPointClouds.h Normal file
View File

@ -0,0 +1,69 @@
// PCL lib Functions for processing point clouds
#ifndef PROCESSPOINTCLOUDS_H_
#define PROCESSPOINTCLOUDS_H_
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/transforms.h>
#include <unordered_set>
#include <iostream>
#include <string>
#include <vector>
#include <ctime>
#include <chrono>
#include <filesystem>
#include "render/box.h"
#include "ransac3d.cpp"
#include "cluster3d.cpp"
namespace lidar_obstacle_detection {
// shorthand for point cloud pointer
template<typename PointT>
using PtCdtr = typename pcl::PointCloud<PointT>::Ptr;
template<typename PointT>
class ProcessPointClouds {
public:
//constructor
ProcessPointClouds();
//deconstructor
~ProcessPointClouds();
void numPoints(PtCdtr<PointT> cloud);
PtCdtr<PointT>FilterCloud(PtCdtr<PointT> cloud, float filterRes, Eigen::Vector4f minPoint,Eigen::Vector4f maxPoint);
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>SeparateClouds(pcl::PointIndices::Ptr inliers, PtCdtr<PointT> cloud);
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>RansacSegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceTol);
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>SegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceTol);
std::vector<PtCdtr<PointT>>EuclideanClustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize,int maxSize);
std::vector<PtCdtr<PointT>> GxtEuclideanClustering( PtCdtr<PointT> input_cloud, float cluster_tolerance, int min_cluster_size, int max_cluster_size);
std::vector<PtCdtr<PointT>>Clustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize, int maxSize);
Box BoundingBox(PtCdtr<PointT> cluster);
void savePcd(PtCdtr<PointT> cloud, std::string file);
PtCdtr<PointT> loadPcd(std::string file);
std::vector<std::filesystem::path> streamPcd(std::string dataPath);
};
}
#endif /* PROCESSPOINTCLOUDS_H_ */

63
src/ransac3d.cpp Normal file
View File

@ -0,0 +1,63 @@
//
// Created by hyin on 2020/3/25.
//
#include "ransac3d.h"
using namespace lidar_obstacle_detection;
template<typename PointT>
Ransac<PointT>::~Ransac() {}
template<typename PointT>
std::unordered_set<int> Ransac<PointT>::Ransac3d(PtCdtr<PointT> cloud) {
std::unordered_set<int> inliersResult; // unordered_set element has been unique
// For max iterations
while (maxIterations--) {
std::unordered_set<int> inliers;
while (inliers.size() < 3) {
inliers.insert(rand()%num_points);
}
// TO define plane, need 3 points
float x1, y1, z1, x2, y2, z2, x3, y3, z3;
auto itr = inliers.begin();
x1 = cloud->points[*itr].x;
y1 = cloud->points[*itr].y;
z1 = cloud->points[*itr].z;
itr++;
x2 = cloud->points[*itr].x;
y2 = cloud->points[*itr].y;
z2 = cloud->points[*itr].z;
itr++;
x3 = cloud->points[*itr].x;
y3 = cloud->points[*itr].y;
z3 = cloud->points[*itr].z;
// Calulate plane coefficient
float a, b, c, d, sqrt_abc;
a = (y2 - y1) * (z3 - z1) - (z2 - z1) * (y3 - y1);
b = (z2 - z1) * (x3 - x1) - (x2 - x1) * (z3 - z1);
c = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1);
d = -(a * x1 + b * y1 + c * z1);
sqrt_abc = sqrt(a * a + b * b + c * c);
// Check distance from point to plane
for (int ind = 0; ind < num_points; ind++) {
if (inliers.count(ind) > 0) { // that means: if the inlier in already exist, we dont need do anymore
continue;
}
PointT point = cloud->points[ind];
float x = point.x;
float y = point.y;
float z = point.z;
float dist = fabs(a * x + b * y + c * z + d) / sqrt_abc; // calculate the distance between other points and plane
if (dist < distanceTol) {
inliers.insert(ind);
}
if (inliers.size() > inliersResult.size()) {
inliersResult = inliers;
}
}
}
return inliersResult;
}

32
src/ransac3d.h Normal file
View File

@ -0,0 +1,32 @@
//
// Created by hyin on 2020/3/25.
//
#ifndef PLAYBACK_RANSAC3D_H
#define PLAYBACK_RANSAC3D_H
#include <unordered_set>
#include <pcl/common/common.h>
namespace lidar_obstacle_detection {
// shorthand for point cloud pointer
template<typename PointT>
using PtCdtr = typename pcl::PointCloud<PointT>::Ptr;
template<typename PointT>
class Ransac {
private:
int maxIterations;
float distanceTol;
int num_points;
public:
Ransac(int maxIter, float distTol, int nPts) : maxIterations(maxIter), distanceTol(distTol), num_points(nPts) {}
~Ransac();
std::unordered_set<int> Ransac3d(PtCdtr<PointT> cloud);
};
}
#endif //PLAYBACK_RANSAC3D_H

22
src/render/box.h Normal file
View File

@ -0,0 +1,22 @@
#ifndef BOX_H
#define BOX_H
#include <Eigen/Geometry>
struct BoxQ
{
Eigen::Vector3f bboxTransform;
Eigen::Quaternionf bboxQuaternion;
float cube_length;
float cube_width;
float cube_height;
};
struct Box
{
float x_min;
float y_min;
float z_min;
float x_max;
float y_max;
float z_max;
};
#endif

111
src/render/render.cpp Normal file
View File

@ -0,0 +1,111 @@
/* \author Aaron Brown */
// Functions and structs used to render the enviroment
// such as cars and the highway
#include "render.h"
void renderHighway(pcl::visualization::PCLVisualizer::Ptr& viewer)
{
// units in meters
double roadLength = 50.0;
double roadWidth = 12.0;
double roadHeight = 0.2;
viewer->addCube(-roadLength/2, roadLength/2, -roadWidth/2, roadWidth/2, -roadHeight, 0, .2, .2, .2, "highwayPavement");
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, "highwayPavement");
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, .2, .2, .2, "highwayPavement");
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0, "highwayPavement");
viewer->addLine(pcl::PointXYZ(-roadLength/2,-roadWidth/6, 0.01),pcl::PointXYZ(roadLength/2, -roadWidth/6, 0.01),1,1,0,"line1");
viewer->addLine(pcl::PointXYZ(-roadLength/2, roadWidth/6, 0.01),pcl::PointXYZ(roadLength/2, roadWidth/6, 0.01),1,1,0,"line2");
}
int countRays = 0;
void renderRays(pcl::visualization::PCLVisualizer::Ptr& viewer, const Vect3& origin, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
for(pcl::PointXYZ point : cloud->points)
{
viewer->addLine(pcl::PointXYZ(origin.x, origin.y, origin.z), point,1,0,0,"ray"+std::to_string(countRays));
countRays++;
}
}
void clearRays(pcl::visualization::PCLVisualizer::Ptr& viewer)
{
while(countRays)
{
countRays--;
viewer->removeShape("ray"+std::to_string(countRays));
}
}
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::string name, Color color)
{
viewer->addPointCloud<pcl::PointXYZ> (cloud, name);
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);
}
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, std::string name, Color color)
{
if(color.r==-1)
{
// Select color based off of cloud intensity
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> intensity_distribution(cloud,"intensity");
viewer->addPointCloud<pcl::PointXYZI>(cloud, intensity_distribution, name);
}
else
{
// Select color based off input value
viewer->addPointCloud<pcl::PointXYZI> (cloud, name);
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);
}
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, name);
}
// Draw wire frame box with filled transparent color
void renderBox(pcl::visualization::PCLVisualizer::Ptr& viewer, Box box, int id, Color color, float opacity)
{
if(opacity > 1.0)
opacity = 1.0;
if(opacity < 0.0)
opacity = 0.0;
std::string cube = "box"+std::to_string(id);
//viewer->addCube(box.bboxTransform, box.bboxQuaternion, box.cube_length, box.cube_width, box.cube_height, cube);
viewer->addCube(box.x_min, box.x_max, box.y_min, box.y_max, box.z_min, box.z_max, color.r, color.g, color.b, cube);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, cube);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, cube);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cube);
std::string cubeFill = "boxFill"+std::to_string(id);
//viewer->addCube(box.bboxTransform, box.bboxQuaternion, box.cube_length, box.cube_width, box.cube_height, cubeFill);
viewer->addCube(box.x_min, box.x_max, box.y_min, box.y_max, box.z_min, box.z_max, color.r, color.g, color.b, cubeFill);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, cubeFill);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, cubeFill);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity*0.3, cubeFill);
}
void renderBox(pcl::visualization::PCLVisualizer::Ptr& viewer, BoxQ box, int id, Color color, float opacity)
{
if(opacity > 1.0)
opacity = 1.0;
if(opacity < 0.0)
opacity = 0.0;
std::string cube = "box"+std::to_string(id);
viewer->addCube(box.bboxTransform, box.bboxQuaternion, box.cube_length, box.cube_width, box.cube_height, cube);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, cube);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, cube);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cube);
std::string cubeFill = "boxFill"+std::to_string(id);
viewer->addCube(box.bboxTransform, box.bboxQuaternion, box.cube_length, box.cube_width, box.cube_height, cubeFill);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, cubeFill);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, cubeFill);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity*0.3, cubeFill);
}

93
src/render/render.h Normal file
View File

@ -0,0 +1,93 @@
/* \author Aaron Brown */
// Functions and structs used to render the enviroment
// such as cars and the highway
#ifndef RENDER_H
#define RENDER_H
#include <pcl/visualization/pcl_visualizer.h>
#include "box.h"
#include <iostream>
#include <vector>
#include <string>
struct Color
{
float r, g, b;
Color(float setR, float setG, float setB)
: r(setR), g(setG), b(setB)
{}
};
struct Vect3
{
double x, y, z;
Vect3(double setX, double setY, double setZ)
: x(setX), y(setY), z(setZ)
{}
Vect3 operator+(const Vect3& vec)
{
Vect3 result(x+vec.x,y+vec.y,z+vec.z);
return result;
}
};
enum CameraAngle
{
XY, TopDown, Side, FPS
};
struct Car
{
// units in meters
Vect3 position, dimensions;
std::string name;
Color color;
Car(Vect3 setPosition, Vect3 setDimensions, Color setColor, std::string setName)
: position(setPosition), dimensions(setDimensions), color(setColor), name(setName)
{}
void render(pcl::visualization::PCLVisualizer::Ptr& viewer)
{
// render bottom of car
viewer->addCube(position.x-dimensions.x/2, position.x+dimensions.x/2, position.y-dimensions.y/2, position.y+dimensions.y/2, position.z, position.z+dimensions.z*2/3, color.r, color.g, color.b, name);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, name);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0, name);
// render top of car
viewer->addCube(position.x-dimensions.x/4, position.x+dimensions.x/4, position.y-dimensions.y/2, position.y+dimensions.y/2, position.z+dimensions.z*2/3, position.z+dimensions.z, color.r, color.g, color.b, name+"Top");
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, name+"Top");
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name+"Top");
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0, name+"Top");
}
// collision helper function
bool inbetween(double point, double center, double range)
{
return (center-range <= point) && (center+range >= point);
}
bool checkCollision(Vect3 point)
{
return (inbetween(point.x,position.x,dimensions.x/2)&&inbetween(point.y,position.y,dimensions.y/2)&&inbetween(point.z,position.z+dimensions.z/3,dimensions.z/3))||
(inbetween(point.x,position.x,dimensions.x/4)&&inbetween(point.y,position.y,dimensions.y/2)&&inbetween(point.z,position.z+dimensions.z*5/6,dimensions.z/6));
}
};
void renderHighway(pcl::visualization::PCLVisualizer::Ptr& viewer);
void renderRays(pcl::visualization::PCLVisualizer::Ptr& viewer, const Vect3& origin, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);
void clearRays(pcl::visualization::PCLVisualizer::Ptr& viewer);
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::string name, Color color = Color(1,1,1));
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, std::string name, Color color = Color(-1,-1,-1));
void renderBox(pcl::visualization::PCLVisualizer::Ptr& viewer, Box box, int id, Color color = Color(1,0,0), float opacity=1);
void renderBox(pcl::visualization::PCLVisualizer::Ptr& viewer, BoxQ box, int id, Color color = Color(1,0,0), float opacity=1);
#endif

95
yaml_config/yaml_config.cpp Normal file
View File

@ -0,0 +1,95 @@
#pragma once
#include "yaml_config.h"
#include <iostream>
YAML::Node ReadConfigYamlFile();
static std::string yaml_config_path =
"/media/Projects/guangpo_lidar/src/guangpo_lidar/yaml_config/yaml_config.yaml";
YAML::Node yaml_config = ReadConfigYamlFile();
YAML::Node ReadConfigYamlFile() {
YAML::Node res;
std::cout << __PRETTY_FUNCTION__ << ": " << std::endl;
std::cout << "BEGIN READ FILE: " << yaml_config_path << std::endl;
bool read_successful_flag = false;
try {
// Load the YAML file
res = YAML::LoadFile(yaml_config_path);
read_successful_flag = true;
} catch (const YAML::Exception &e) {
std::cerr << "Error while reading the YAML file: " << yaml_config_path
<< e.what() << std::endl;
// gDebugError("[GXT] : Error while reading the YAML file:") << e.what();
}
if (!read_successful_flag) {
// std::cerr << "backtrace:" << __PRETTY_FUNCTION__ << std::endl;
// std::cerr << "backtrace:" << __PRETTY_FUNCTION__ << std::endl;
// std::cerr << "backtrace:" << __PRETTY_FUNCTION__ << std::endl;
std::cerr << "Error while reading the YAML file!" << yaml_config_path
<< std::endl;
std::cerr << "Error while reading the YAML file!" << yaml_config_path
<< std::endl;
std::cerr << "Error while reading the YAML file!" << yaml_config_path
<< std::endl;
std::terminate();
}
std::cout << "Read yaml config file successfully! " << yaml_config_path
<< std::endl;
return res;
}
// __attribute((constructor)) inline bool
// GxtReadConfigYamlFile(const std::string &yaml_config_path) {
// bool read_successful_flag = false;
// try {
// // Load the YAML file
// YAML::Node config = YAML::LoadFile(yaml_config_path);
//
// read_successful_flag = true;
// // gDebugCol1("READ CONFIG FILE SUCCESSFULLY!");
//
// auto quantize = config["quantize"];
// ADDCONFIG(quantize, quantize_pose_quaternion_bit_width);
// ADDCONFIG(quantize, quantize_pose_quaternion_il);
// ADDCONFIG(quantize, quantize_pose_quaternion_bit_width);
// ADDCONFIG(quantize, quantize_pose_quaternion_il);
//
// ADDCONFIG(quantize, quantize_imu_speed_bit_width);
// ADDCONFIG(quantize, quantize_imu_speed_il);
// ADDCONFIG(quantize, quantize_imu_accbias_bit_width);
// ADDCONFIG(quantize, quantize_imu_accbias_il);
// ADDCONFIG(quantize, quantize_imu_gyrobias_bit_width);
// ADDCONFIG(quantize, quantize_imu_gyrobias_il);
//
// ADDCONFIG(quantize, quantize_inverse_depth_bit_width);
// ADDCONFIG(quantize, quantize_inverse_depth_il);
//
// ADDCONFIG(quantize, quantize_hessian_bit_width);
// ADDCONFIG(quantize, quantize_hessian_il);
// ADDCONFIG(quantize, quantize_b_bit_width);
// ADDCONFIG(quantize, quantize_b_il);
//
// auto flag = config["flag"];
// ADDCONFIG(flag, use_gxt_backend);
// ADDCONFIG(flag, enable_quantize);
// } catch (const YAML::Exception &e) {
// // std::cerr << "Error while reading the YAML file: " << e.what() <<
// // std::endl;
// gDebugError("[GXT] : Error while reading the YAML file:") << e.what();
// }
//
// if (!read_successful_flag) {
// gDebugCol3("\n\n\n==========================================");
// gDebugCol3("[GXT] : Error while reading the YAML file!");
// gDebugCol3("[GXT] : Error while reading the YAML file!");
// gDebugError("[GXT] : Error while reading the YAML file!");
// std::terminate();
// }
//
// return 0;
// }

8
yaml_config/yaml_config.h Normal file
View File

@ -0,0 +1,8 @@
#pragma once
#include <yaml-cpp/yaml.h>
#include <string>
extern YAML::Node yaml_config;

34
yaml_config/yaml_config.yaml Normal file
View File

@ -0,0 +1,34 @@
# 基础ros设置
sub_cloud_topic : "abc"
pub_cloud_topic : "efg"
# 降采样大小
filterRes : 0.05
# 设置roi区域
roi:
min_x : 0
max_x : 20
min_y : -20
max_y : 20
min_z : -1
max_z : 3
# 去除屋顶的点云（车辆相关点云）
roof:
min_x : -1
max_x : 1
min_y : -1
max_y : 1
min_z : -2
max_z : 2
# 设置Ransac的参数
ransac:
maxIterations : 250
distanceThreshold : 0.1
# 设置聚类参数
cluster:
clusterTolerance : 0.2
minsize : 30
maxsize : 1000
# 可视化分割地面点云
view:
segment : false