22 #ifndef __PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_ 23 #define __PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_ 26 #include <pcl/point_cloud.h> 27 #include <pcl/point_types.h> 29 #include <core/threading/thread.h> 30 #include <aspect/clock.h> 31 #include <aspect/configurable.h> 32 #include <aspect/logging.h> 33 #include <aspect/blackboard.h> 34 #include <aspect/tf.h> 35 #include <aspect/pointcloud.h> 37 #include <Eigen/StdVector> 38 #include <pcl/segmentation/sac_segmentation.h> 39 #include <pcl/filters/voxel_grid.h> 40 #include <pcl/features/normal_3d.h> 42 #include <pcl/ModelCoefficients.h> 43 #include <pcl/filters/extract_indices.h> 44 #include <pcl/filters/passthrough.h> 45 #include <pcl/sample_consensus/method_types.h> 46 #include <pcl/sample_consensus/model_types.h> 48 #include <pcl/filters/statistical_outlier_removal.h> 54 class Position3DInterface;
55 class SwitchInterface;
57 #ifdef USE_TIMETRACKER 62 #ifdef HAVE_VISUAL_DEBUGGING 76 OldCentroid(
const unsigned int &
id,
const Eigen::Vector4f ¢roid)
77 : id_(id), age_(0), centroid_(centroid) { }
81 : id_(other.getId()), age_(other.getAge()), centroid_(other.getCentroid()) { }
84 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
87 unsigned int getId()
const {
return id_; }
90 const Eigen::Vector4f&
getCentroid()
const {
return centroid_; }
94 unsigned int getAge()
const {
return age_; }
96 void age() { age_++; }
122 virtual void finalize();
124 #ifdef HAVE_VISUAL_DEBUGGING 129 typedef pcl::PointXYZ PointType;
132 typedef pcl::PointXYZRGB ColorPointType;
134 typedef Cloud::Ptr CloudPtr;
135 typedef Cloud::ConstPtr CloudConstPtr;
137 typedef ColorCloud::Ptr ColorCloudPtr;
138 typedef ColorCloud::ConstPtr ColorCloudConstPtr;
140 typedef std::map<unsigned int, Eigen::Vector4f, std::less<unsigned int>,
141 Eigen::aligned_allocator<std::pair<const unsigned int, Eigen::Vector4f>>>
143 typedef std::list<OldCentroid, Eigen::aligned_allocator<OldCentroid> > OldCentroidVector;
144 typedef std::vector<fawkes::Position3DInterface *> PosIfsVector;
148 bool is_visible,
const Eigen::Vector4f ¢roid = Eigen::Vector4f(0, 0, 0, 0),
149 const Eigen::Quaternionf &rotation = Eigen::Quaternionf(1, 0, 0, 0));
151 CloudPtr simplify_polygon(CloudPtr polygon,
float sqr_dist_threshold);
152 CloudPtr generate_table_model(
const float length,
const float width,
153 const float thickness,
const float step,
const float max_error);
154 CloudPtr generate_table_model(
const float length,
const float width,
155 const float step,
const float max_error = 0.01);
156 bool is_polygon_edge_better(PointType &cb_br_p1p, PointType &cb_br_p2p, PointType &br_p1p, PointType &br_p2p);
157 bool compute_bounding_box_scores(Eigen::Vector3f& cluster_dim, std::vector < Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> >& scores);
158 double compute_similarity(
double d1,
double d2);
160 void convert_colored_input();
162 std::vector<pcl::PointIndices> extract_object_clusters(CloudConstPtr input);
164 ColorCloudPtr colorize_cluster(CloudConstPtr input_cloud,
const std::vector<int> &cluster,
const uint8_t color[]);
166 unsigned int cluster_objects(CloudConstPtr input, ColorCloudPtr tmp_clusters, std::vector<ColorCloudPtr> &tmp_obj_clusters);
169 void delete_old_centroids(OldCentroidVector centroids,
unsigned int age);
170 void delete_near_centroids(CentroidMap reference, OldCentroidVector centroids,
172 void remove_high_centroids(Eigen::Vector4f table_centroid, CentroidMap centroids);
173 Eigen::Vector4f fit_cylinder(ColorCloudConstPtr obj_in_base_frame, Eigen::Vector4f
const ¢roid, uint
const ¢roid_i);
174 std::map<unsigned int, int> track_objects(std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> new_centroids);
183 ColorCloudConstPtr colored_input_;
184 CloudPtr converted_input_;
185 CloudConstPtr input_;
188 std::vector<fawkes::RefPtr<pcl::PointCloud<ColorPointType> > > f_obj_clusters_;
189 std::vector<pcl::PointCloud<ColorPointType>::Ptr> obj_clusters_;
190 std::map<unsigned int, double> obj_shape_confidence_;
193 std::map<unsigned int, signed int> best_obj_guess_;
196 double table_inclination_;
198 std::vector < Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > known_obj_dimensions_;
200 pcl::VoxelGrid<PointType> grid_;
201 pcl::SACSegmentation<PointType> seg_;
203 PosIfsVector pos_ifs_;
206 Eigen::Vector4f table_centroid;
208 std::list<unsigned int> free_ids_;
214 float cfg_depth_filter_min_x_;
215 float cfg_depth_filter_max_x_;
216 float cfg_voxel_leaf_size_;
217 unsigned int cfg_segm_max_iterations_;
218 float cfg_segm_distance_threshold_;
219 float cfg_segm_inlier_quota_;
220 float cfg_max_z_angle_deviation_;
221 float cfg_table_min_cluster_quota_;
222 float cfg_table_downsample_leaf_size_;
223 float cfg_table_cluster_tolerance_;
224 float cfg_table_min_height_;
225 float cfg_table_max_height_;
226 bool cfg_table_model_enable_;
227 float cfg_table_model_length_;
228 float cfg_table_model_width_;
229 float cfg_table_model_step_;
230 float cfg_horizontal_va_;
231 float cfg_vertical_va_;
232 float cfg_cluster_tolerance_;
233 unsigned int cfg_cluster_min_size_;
234 unsigned int cfg_cluster_max_size_;
235 std::string cfg_base_frame_;
236 std::string cfg_result_frame_;
237 std::string cfg_input_pointcloud_;
238 uint cfg_centroid_max_age_;
239 float cfg_centroid_max_distance_;
240 float cfg_centroid_min_distance_;
241 float cfg_centroid_max_height_;
242 bool cfg_cylinder_fitting_;
243 bool cfg_track_objects_;
244 bool cfg_verbose_cylinder_fitting_;
247 CloudPtr table_model_;
249 CloudPtr simplified_polygon_;
251 unsigned int loop_count_;
253 CentroidMap centroids_;
254 CentroidMap cylinder_params_;
255 OldCentroidVector old_centroids_;
258 std::map<uint, std::vector<double>> obj_likelihoods_;
260 #ifdef USE_TIMETRACKER 262 unsigned int tt_loopcount_;
263 unsigned int ttc_full_loop_;
264 unsigned int ttc_msgproc_;
265 unsigned int ttc_convert_;
266 unsigned int ttc_voxelize_;
267 unsigned int ttc_plane_;
268 unsigned int ttc_extract_plane_;
269 unsigned int ttc_plane_downsampling_;
270 unsigned int ttc_cluster_plane_;
271 unsigned int ttc_convex_hull_;
272 unsigned int ttc_simplify_polygon_;
273 unsigned int ttc_find_edge_;
274 unsigned int ttc_transform_;
275 unsigned int ttc_transform_model_;
276 unsigned int ttc_extract_non_plane_;
277 unsigned int ttc_polygon_filter_;
278 unsigned int ttc_table_to_output_;
279 unsigned int ttc_cluster_objects_;
280 unsigned int ttc_visualization_;
281 unsigned int ttc_hungarian_;
282 unsigned int ttc_old_centroids_;
283 unsigned int ttc_obj_extraction_;
286 #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.
unsigned int age_
The number of loops the centroid has been invisible for.
Main thread of tabletop objects plugin.
Fawkes library namespace.
Thread aspect to provide and access point clouds.
virtual void run()
Code to execute in the thread.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW unsigned int getId() const
get the ID of the centroid
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.
OldCentroid(const OldCentroid &other)
Copy constructor.
SwitchInterface Fawkes BlackBoard Interface.
Position3DInterface Fawkes BlackBoard Interface.
Thread aspect to log output.
OldCentroid(const unsigned int &id, const Eigen::Vector4f ¢roid)
Constructor.
Thread aspect to access configuration data.
unsigned int getAge() const
get the age of the centroid
RefPtr<> is a reference-counting shared smartpointer.
This class is used to save old centroids in order to check for reappearance.
void age()
increment the age of the centroid
const Eigen::Vector4f & getCentroid() const
get the position of the centroid