22 #ifndef _PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_ 23 #define _PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_ 26 #include <aspect/blackboard.h> 27 #include <aspect/clock.h> 28 #include <aspect/configurable.h> 29 #include <aspect/logging.h> 30 #include <aspect/pointcloud.h> 31 #include <aspect/tf.h> 32 #include <core/threading/thread.h> 33 #include <pcl/ModelCoefficients.h> 34 #include <pcl/features/normal_3d.h> 35 #include <pcl/filters/extract_indices.h> 36 #include <pcl/filters/passthrough.h> 37 #include <pcl/filters/statistical_outlier_removal.h> 38 #include <pcl/filters/voxel_grid.h> 39 #include <pcl/point_cloud.h> 40 #include <pcl/point_types.h> 41 #include <pcl/sample_consensus/method_types.h> 42 #include <pcl/sample_consensus/model_types.h> 43 #include <pcl/segmentation/sac_segmentation.h> 45 #include <Eigen/StdVector> 50 class Position3DInterface;
51 class SwitchInterface;
53 #ifdef USE_TIMETRACKER 58 #ifdef HAVE_VISUAL_DEBUGGING 73 OldCentroid(
const unsigned int &
id,
const Eigen::Vector4f ¢roid)
103 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
115 const Eigen::Vector4f &
162 #ifdef HAVE_VISUAL_DEBUGGING 167 typedef pcl::PointXYZ PointType;
170 typedef pcl::PointXYZRGB ColorPointType;
172 typedef Cloud::Ptr CloudPtr;
173 typedef Cloud::ConstPtr CloudConstPtr;
175 typedef ColorCloud::Ptr ColorCloudPtr;
176 typedef ColorCloud::ConstPtr ColorCloudConstPtr;
178 typedef std::map<
unsigned int,
180 std::less<unsigned int>,
181 Eigen::aligned_allocator<std::pair<const unsigned int, Eigen::Vector4f>>>
183 typedef std::list<OldCentroid, Eigen::aligned_allocator<OldCentroid>> OldCentroidVector;
184 typedef std::vector<fawkes::Position3DInterface *> PosIfsVector;
189 const Eigen::Vector4f & centroid = Eigen::Vector4f(0, 0, 0, 0),
190 const Eigen::Quaternionf & rotation = Eigen::Quaternionf(1, 0, 0, 0));
192 CloudPtr simplify_polygon(CloudPtr polygon,
float sqr_dist_threshold);
193 CloudPtr generate_table_model(
const float length,
195 const float thickness,
197 const float max_error);
198 CloudPtr generate_table_model(
const float length,
201 const float max_error = 0.01);
202 bool is_polygon_edge_better(PointType &cb_br_p1p,
203 PointType &cb_br_p2p,
206 bool compute_bounding_box_scores(
207 Eigen::Vector3f & cluster_dim,
208 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> &scores);
209 double compute_similarity(
double d1,
double d2);
211 void convert_colored_input();
213 std::vector<pcl::PointIndices> extract_object_clusters(CloudConstPtr input);
215 ColorCloudPtr colorize_cluster(CloudConstPtr input_cloud,
216 const std::vector<int> &cluster,
217 const uint8_t color[]);
219 unsigned int cluster_objects(CloudConstPtr input,
220 ColorCloudPtr tmp_clusters,
221 std::vector<ColorCloudPtr> &tmp_obj_clusters);
224 void delete_old_centroids(OldCentroidVector centroids,
unsigned int age);
226 delete_near_centroids(CentroidMap reference, OldCentroidVector centroids,
float min_distance);
227 void remove_high_centroids(Eigen::Vector4f table_centroid, CentroidMap centroids);
228 Eigen::Vector4f fit_cylinder(ColorCloudConstPtr obj_in_base_frame,
229 Eigen::Vector4f
const ¢roid,
230 uint
const & centroid_i);
231 std::map<unsigned int, int> track_objects(
232 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> new_centroids);
246 ColorCloudConstPtr colored_input_;
247 CloudPtr converted_input_;
248 CloudConstPtr input_;
251 std::vector<fawkes::RefPtr<pcl::PointCloud<ColorPointType>>> f_obj_clusters_;
252 std::vector<pcl::PointCloud<ColorPointType>::Ptr> obj_clusters_;
253 std::map<unsigned int, double> obj_shape_confidence_;
256 std::map<unsigned int, signed int> best_obj_guess_;
259 double table_inclination_;
261 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> known_obj_dimensions_;
263 pcl::VoxelGrid<PointType> grid_;
264 pcl::SACSegmentation<PointType> seg_;
266 PosIfsVector pos_ifs_;
269 Eigen::Vector4f table_centroid;
271 std::list<unsigned int> free_ids_;
277 float cfg_depth_filter_min_x_;
278 float cfg_depth_filter_max_x_;
279 float cfg_voxel_leaf_size_;
280 unsigned int cfg_segm_max_iterations_;
281 float cfg_segm_distance_threshold_;
282 float cfg_segm_inlier_quota_;
283 float cfg_max_z_angle_deviation_;
284 float cfg_table_min_cluster_quota_;
285 float cfg_table_downsample_leaf_size_;
286 float cfg_table_cluster_tolerance_;
287 float cfg_table_min_height_;
288 float cfg_table_max_height_;
289 bool cfg_table_model_enable_;
290 float cfg_table_model_length_;
291 float cfg_table_model_width_;
292 float cfg_table_model_step_;
293 float cfg_horizontal_va_;
294 float cfg_vertical_va_;
295 float cfg_cluster_tolerance_;
296 unsigned int cfg_cluster_min_size_;
297 unsigned int cfg_cluster_max_size_;
298 std::string cfg_base_frame_;
299 std::string cfg_result_frame_;
300 std::string cfg_input_pointcloud_;
301 uint cfg_centroid_max_age_;
302 float cfg_centroid_max_distance_;
303 float cfg_centroid_min_distance_;
304 float cfg_centroid_max_height_;
305 bool cfg_cylinder_fitting_;
306 bool cfg_track_objects_;
307 bool cfg_verbose_cylinder_fitting_;
310 CloudPtr table_model_;
312 CloudPtr simplified_polygon_;
314 unsigned int loop_count_;
316 CentroidMap centroids_;
317 CentroidMap cylinder_params_;
318 OldCentroidVector old_centroids_;
321 std::map<uint, std::vector<double>> obj_likelihoods_;
323 #ifdef USE_TIMETRACKER 325 unsigned int tt_loopcount_;
326 unsigned int ttc_full_loop_;
327 unsigned int ttc_msgproc_;
328 unsigned int ttc_convert_;
329 unsigned int ttc_voxelize_;
330 unsigned int ttc_plane_;
331 unsigned int ttc_extract_plane_;
332 unsigned int ttc_plane_downsampling_;
333 unsigned int ttc_cluster_plane_;
334 unsigned int ttc_convex_hull_;
335 unsigned int ttc_simplify_polygon_;
336 unsigned int ttc_find_edge_;
337 unsigned int ttc_transform_;
338 unsigned int ttc_transform_model_;
339 unsigned int ttc_extract_non_plane_;
340 unsigned int ttc_polygon_filter_;
341 unsigned int ttc_table_to_output_;
342 unsigned int ttc_cluster_objects_;
343 unsigned int ttc_visualization_;
344 unsigned int ttc_hungarian_;
345 unsigned int ttc_old_centroids_;
346 unsigned int ttc_obj_extraction_;
349 #ifdef HAVE_VISUAL_DEBUGGING Thread aspect to access to BlackBoard.
Thread aspect that allows to obtain the current time from the clock.
virtual void run()
Stub to see name in backtrace for easier debugging.
const Eigen::Vector4f & get_centroid() const
Get the position of the centroid.
unsigned int age_
The number of loops the centroid has been invisible for.
Main thread of tabletop objects plugin.
virtual ~OldCentroid()
Destructor.
Fawkes library namespace.
Thread aspect to provide and access point clouds.
OldCentroid & operator=(const OldCentroid &other)
Assignment operator.
A class for handling time.
Eigen::Vector4f centroid_
The position of centroid.
Thread class encapsulation of pthreads.
Base class for virtualization thread.
unsigned int id_
The ID of the centroid.
virtual ~TabletopObjectsThread()
Destructor.
OldCentroid(const OldCentroid &other)
Copy constructor.
SwitchInterface Fawkes BlackBoard Interface.
Position3DInterface Fawkes BlackBoard Interface.
virtual void finalize()
Finalize the thread.
TabletopObjectsThread()
Constructor.
Thread aspect to log output.
unsigned int get_age() const
Get the age of the centroid.
OldCentroid(const unsigned int &id, const Eigen::Vector4f ¢roid)
Constructor.
Thread aspect to access configuration data.
RefPtr<> is a reference-counting shared smartpointer.
virtual void init()
Initialize the thread.
This class is used to save old centroids in order to check for reappearance.
virtual void loop()
Code to execute in the thread.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW unsigned int get_id() const
Get the ID of the centroid.
void increment_age()
Increment the age of the centroid.