Skip to content

Commit f4fc07b

Browse files
bochen87bochen87
committed
Make nodelet out of patchwork
1 parent f95a6f0 commit f4fc07b

File tree

6 files changed

+1384
-1126
lines changed

6 files changed

+1384
-1126
lines changed

‎CMakeLists.txt‎

Lines changed: 33 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -1,21 +1,12 @@
1-
option(BUILD_JSK_PKGS "Enable building of required components of jsk_recognition_msgs and jsk_rviz_plugins" ON)
2-
3-
if(BUILD_JSK_PKGS)
4-
add_subdirectory(include/jsk_recognition_msgs)
5-
# add_subdirectory(include/jsk_rviz_plugins) #TODO: allow building of rviz plugins as well
6-
endif()
7-
8-
cmake_minimum_required(VERSION 3.0.2)
1+
cmake_minimum_required(VERSION 3.1)
92
project(patchworkpp)
103

11-
add_compile_options(-std=c++17)
12-
set(CMAKE_BUILD_TYPE "Release")
13-
14-
set(CMAKE_CXX_STANDARD 14)
4+
set(CMAKE_CXX_STANDARD 17)
155
set(CMAKE_CXX_STANDARD_REQUIRED ON)
16-
set(CMAKE_CXX_EXTENSIONS OFF)
176

18-
find_package(catkin REQUIRED COMPONENTS
7+
set(PROJECT_CATKIN_DEPS
8+
nodelet
9+
pluginlib
1910
roscpp
2011
rospy
2112
std_msgs
@@ -26,19 +17,16 @@ find_package(catkin REQUIRED COMPONENTS
2617
geometry_msgs
2718
laser_geometry
2819
sensor_msgs
29-
message_generation
3020
jsk_recognition_msgs
3121
)
3222

33-
find_package(OpenCV REQUIRED)
34-
35-
generate_messages(
36-
DEPENDENCIES
37-
std_msgs
38-
geometry_msgs
39-
sensor_msgs
23+
find_package(catkin REQUIRED
24+
COMPONENTS
25+
${PROJECT_CATKIN_DEPS}
4026
)
4127

28+
find_package(OpenCV REQUIRED)
29+
4230
find_package(PCL 1.7 REQUIRED)
4331
find_package(Boost 1.54 REQUIRED)
4432
find_package(OpenMP)
@@ -49,41 +37,42 @@ if (OPENMP_FOUND)
4937
endif()
5038

5139
catkin_package(
52-
INCLUDE_DIRS
53-
LIBRARIES
54-
CATKIN_DEPENDS roscpp rospy std_msgs
40+
INCLUDE_DIRS
41+
include
42+
LIBRARIES
43+
${PROJECT_NAME}
44+
CATKIN_DEPENDS
45+
${PROJECT_CATKIN_DEPS}
5546
)
5647

48+
add_definitions(${catkin_DEFINITIONS})
5749
include_directories(
58-
${catkin_INCLUDE_DIRS}
59-
${PCL_INCLUDE_DIRS}
60-
${OpenCV_INCLUDE_DIRS}
61-
include
50+
include
51+
${catkin_INCLUDE_DIRS}
52+
${PCL_INCLUDE_DIRS}
53+
${OpenCV_INCLUDE_DIRS}
54+
${Boost_INCLUDE_DIRS}
6255
)
6356

64-
add_executable(offline_kitti src/offline_kitti.cpp)
65-
target_link_libraries(offline_kitti ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
66-
add_dependencies(offline_kitti patchworkpp_generate_messages_cpp)
67-
68-
add_executable(demo src/demo.cpp)
69-
target_link_libraries(demo ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
70-
add_dependencies(demo patchworkpp_generate_messages_cpp)
71-
72-
add_executable(video src/video.cpp)
73-
target_link_libraries(video ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
74-
add_dependencies(video patchworkpp_generate_messages_cpp)
75-
57+
add_library(patchworkpp src/patchworkpp.cpp)
58+
target_link_libraries(patchworkpp ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
7659

7760
# ==== Install ====
78-
install(TARGETS demo
61+
install(TARGETS patchworkpp
7962
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
8063
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
8164
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
8265
)
8366

84-
install(DIRECTORY include/${PROJECT_NAME}/
85-
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
67+
install(
68+
DIRECTORY
69+
include/${PROJECT_NAME}/
70+
DESTINATION
71+
${CATKIN_PACKAGE_INCLUDE_DESTINATION}
72+
)
8673

8774
install(DIRECTORY launch config
8875
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
8976
)
77+
78+
install(FILES nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

‎include/patchworkpp/patchworkpp.h‎

Lines changed: 233 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,233 @@
1+
/**
2+
* @file patchworkpp.hpp
3+
* @author Seungjae Lee
4+
* @brief
5+
* @version 0.1
6+
* @date 2022-07-20
7+
*
8+
* @copyright Copyright (c) 2022
9+
*
10+
*/
11+
#ifndef PATCHWORKPP_H
12+
#define PATCHWORKPP_H
13+
14+
#include <patchworkpp/utils.hpp>
15+
16+
#include <Eigen/Dense>
17+
#include <boost/format.hpp>
18+
#include <jsk_recognition_msgs/PolygonArray.h>
19+
#include <nodelet/nodelet.h>
20+
#include <pcl/point_cloud.h>
21+
#include <pcl/point_types.h>
22+
#include <ros/publisher.h>
23+
#include <ros/ros.h>
24+
#include <sensor_msgs/PointCloud2.h>
25+
26+
#include <mutex>
27+
#include <numeric>
28+
#include <queue>
29+
#include <vector>
30+
31+
#define MARKER_Z_VALUE -2.2
32+
#define UPRIGHT_ENOUGH 0.55
33+
#define FLAT_ENOUGH 0.2
34+
#define TOO_HIGH_ELEVATION 0.0
35+
#define TOO_TILTED 1.0
36+
37+
#define NUM_HEURISTIC_MAX_PTS_IN_PATCH 3000
38+
39+
using Eigen::JacobiSVD;
40+
using Eigen::MatrixXf;
41+
using Eigen::VectorXf;
42+
43+
using namespace std;
44+
45+
namespace patchworkpp
46+
{
47+
template<typename PointT>
48+
bool
49+
point_z_cmp(PointT a, PointT b)
50+
{
51+
return a.z < b.z;
52+
}
53+
54+
template<typename PointT>
55+
struct RevertCandidate
56+
{
57+
int concentric_idx;
58+
int sector_idx;
59+
double ground_flatness;
60+
double line_variable;
61+
Eigen::Vector4f pc_mean;
62+
pcl::PointCloud<PointT> regionwise_ground;
63+
64+
RevertCandidate(int _c_idx,
65+
int _s_idx,
66+
double _flatness,
67+
double _line_var,
68+
Eigen::Vector4f _pc_mean,
69+
pcl::PointCloud<PointT> _ground)
70+
: concentric_idx(_c_idx)
71+
, sector_idx(_s_idx)
72+
, ground_flatness(_flatness)
73+
, line_variable(_line_var)
74+
, pc_mean(_pc_mean)
75+
, regionwise_ground(_ground)
76+
{
77+
}
78+
};
79+
80+
template<typename PointT>
81+
class Nodelet : public nodelet::Nodelet
82+
{
83+
public:
84+
Nodelet();
85+
86+
typedef vector<pcl::PointCloud<PointT>> Ring;
87+
typedef vector<Ring> Zone;
88+
89+
void onInit() final;
90+
91+
private:
92+
// Every private member variable is written with the undescore("_") in its end.
93+
94+
ros::NodeHandle node_handle_;
95+
96+
std::recursive_mutex mutex_;
97+
98+
int num_iter_;
99+
int num_lpr_;
100+
int num_min_pts_;
101+
int num_zones_;
102+
int num_rings_of_interest_;
103+
104+
double sensor_height_;
105+
double th_seeds_;
106+
double th_dist_;
107+
double th_seeds_v_;
108+
double th_dist_v_;
109+
double max_range_;
110+
double min_range_;
111+
double uprightness_thr_;
112+
double adaptive_seed_selection_margin_;
113+
double min_range_z2_; // 12.3625
114+
double min_range_z3_; // 22.025
115+
double min_range_z4_; // 41.35
116+
double RNR_ver_angle_thr_;
117+
double RNR_intensity_thr_;
118+
119+
bool verbose_;
120+
bool enable_RNR_;
121+
bool enable_RVPF_;
122+
bool enable_TGR_;
123+
124+
int max_flatness_storage_, max_elevation_storage_;
125+
vector<double> update_flatness_[4];
126+
vector<double> update_elevation_[4];
127+
128+
float d_;
129+
130+
VectorXf normal_;
131+
MatrixXf pnormal_;
132+
VectorXf singular_values_;
133+
Eigen::Matrix3f cov_;
134+
Eigen::Vector4f pc_mean_;
135+
136+
// For visualization
137+
bool visualize_;
138+
139+
vector<int> num_sectors_each_zone_;
140+
vector<int> num_rings_each_zone_;
141+
142+
vector<double> sector_sizes_;
143+
vector<double> ring_sizes_;
144+
vector<double> min_ranges_;
145+
vector<double> elevation_thr_;
146+
vector<double> flatness_thr_;
147+
148+
queue<int> noise_idxs_;
149+
150+
vector<Zone> ConcentricZoneModel_;
151+
152+
jsk_recognition_msgs::PolygonArray poly_list_;
153+
154+
ros::Publisher PlaneViz, pub_revert_pc, pub_reject_pc, pub_normal, pub_noise, pub_vertical, pub_cloud, pub_ground,
155+
pub_non_ground;
156+
pcl::PointCloud<PointT> revert_pc_, reject_pc_, noise_pc_, vertical_pc_;
157+
pcl::PointCloud<PointT> ground_pc_;
158+
159+
pcl::PointCloud<pcl::PointXYZINormal> normals_;
160+
161+
pcl::PointCloud<PointT> regionwise_ground_, regionwise_nonground_;
162+
163+
string cloud_topic_;
164+
ros::Subscriber cloud_sub_;
165+
166+
void inputCloudCallback(const sensor_msgs::PointCloud2::Ptr& cloud_msg);
167+
168+
void pubCloud(ros::Publisher pub,
169+
pcl::PointCloud<PointT> cloud,
170+
const ros::Time& stamp,
171+
std::string frame_id = "map");
172+
173+
void estimate_ground(pcl::PointCloud<PointT> cloud_in,
174+
pcl::PointCloud<PointT>& cloud_ground,
175+
pcl::PointCloud<PointT>& cloud_nonground,
176+
double& time_taken);
177+
178+
void initialize_zone(Zone& z, int num_sectors, int num_rings);
179+
180+
void flush_patches_in_zone(Zone& patches, int num_sectors, int num_rings);
181+
void flush_patches(vector<Zone>& czm);
182+
183+
void pc2czm(const pcl::PointCloud<PointT>& src, vector<Zone>& czm, pcl::PointCloud<PointT>& cloud_nonground);
184+
185+
void reflected_noise_removal(pcl::PointCloud<PointT>& cloud, pcl::PointCloud<PointT>& cloud_nonground);
186+
187+
void temporal_ground_revert(pcl::PointCloud<PointT>& cloud_ground,
188+
pcl::PointCloud<PointT>& cloud_nonground,
189+
vector<double> ring_flatness,
190+
vector<RevertCandidate<PointT>> candidates,
191+
int concentric_idx);
192+
193+
void calc_mean_stdev(vector<double> vec, double& mean, double& stdev);
194+
195+
void update_elevation_thr();
196+
void update_flatness_thr();
197+
198+
double xy2theta(const double& x, const double& y);
199+
200+
double xy2radius(const double& x, const double& y);
201+
202+
void estimate_plane(const pcl::PointCloud<PointT>& ground);
203+
204+
void extract_piecewiseground(const int zone_idx,
205+
const pcl::PointCloud<PointT>& src,
206+
pcl::PointCloud<PointT>& dst,
207+
pcl::PointCloud<PointT>& non_ground_dst);
208+
209+
void extract_initial_seeds(const int zone_idx,
210+
const pcl::PointCloud<PointT>& p_sorted,
211+
pcl::PointCloud<PointT>& init_seeds);
212+
213+
void extract_initial_seeds(const int zone_idx,
214+
const pcl::PointCloud<PointT>& p_sorted,
215+
pcl::PointCloud<PointT>& init_seeds,
216+
double th_seed);
217+
218+
/***
219+
* For visulization of Ground Likelihood Estimation
220+
*/
221+
geometry_msgs::PolygonStamped set_polygons(int zone_idx, int r_idx, int theta_idx, int num_split);
222+
223+
void set_ground_likelihood_estimation_status(const int zone_idx,
224+
const int ring_idx,
225+
const int concentric_idx,
226+
const double z_vec,
227+
const double z_elevation,
228+
const double ground_flatness);
229+
};
230+
231+
} // namespace patchworkpp
232+
233+
#endif

0 commit comments

Comments
 (0)