Fawkes API  Fawkes Development Version
pcl_db_pipeline.h
1 
2 /***************************************************************************
3  * pcl_db_pipeline.h - PCL DB processing pipeline base class
4  *
5  * Created: Wed Aug 21 17:24:18 2013
6  * Copyright 2012-2013 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #ifndef __PLUGINS_PERCEPTION_PCL_DB_PCL_DB_PIPELINE_H_
23 #define __PLUGINS_PERCEPTION_PCL_DB_PCL_DB_PIPELINE_H_
24 
25 #include "mongodb_tf_transformer.h"
26 
27 #include <Eigen/Core>
28 
29 #include <pcl/point_types.h>
30 #include <pcl/point_cloud.h>
31 
32 #include <logging/logger.h>
33 #include <config/config.h>
34 #include <pcl_utils/utils.h>
35 #include <pcl_utils/transforms.h>
36 #include <pcl_utils/storage_adapter.h>
37 #include <pcl_utils/comparisons.h>
38 #ifdef USE_TIMETRACKER
39 # include <utils/time/tracker.h>
40 #endif
41 #include <utils/time/tracker_macros.h>
42 
43 #define USE_ALIGNMENT
44 #define USE_ICP_ALIGNMENT
45 // define USE_NDT_ALIGNMENT
46 
47 #define CFG_PREFIX "/perception/pcl-db/"
48 
49 #include <pcl/point_cloud.h>
50 #include <pcl/point_types.h>
51 #include <pcl/filters/approximate_voxel_grid.h>
52 #include <pcl/filters/voxel_grid.h>
53 #include <pcl/filters/passthrough.h>
54 #include <pcl/filters/extract_indices.h>
55 #include <pcl/filters/conditional_removal.h>
56 #include <pcl/segmentation/sac_segmentation.h>
57 #include <pcl/surface/convex_hull.h>
58 
59 #include <mongo/client/dbclient.h>
60 #include <mongo/client/gridfs.h>
61 
62 #ifdef HAVE_MONGODB_VERSION_H
63 // we are using mongo-cxx-driver which renamed QUERY to MONGO_QUERY
64 # define QUERY MONGO_QUERY
65 #endif
66 
67 static const uint8_t cluster_colors[12][3] =
68  { {176, 0, 30}, {0, 0, 255}, {255, 90, 0}, {137, 82, 39}, {56, 23, 90}, {99, 0, 30},
69  {255, 0, 0}, {0, 255, 0}, {255, 255, 0}, {255, 0, 255}, {0, 255, 255}, {27, 117, 196}};
70 
71 typedef enum {
72  APPLICABLE = 0,
73  TYPE_MISMATCH,
74  NO_POINTCLOUD,
75  QUERY_FAILED
76 } ApplicabilityStatus;
77 
78 /** Database point cloud pipeline base class.
79  * Common functionality for pcl-db-* plugins operating on
80  * point clouds restored from MongoDB.
81  * @author Tim Niemueller
82  */
83 template <typename PointType>
85 {
86  protected:
87  /** Basic point cloud type. */
89 
90  /** Colored point type */
91  typedef pcl::PointXYZRGB ColorPointType;
92  /** Type for colored point clouds based on ColorPointType. */
94  /** Shared pointer to cloud. */
95  typedef typename Cloud::Ptr CloudPtr;
96  /** Shared pointer to constant cloud. */
97  typedef typename Cloud::ConstPtr CloudConstPtr;
98 
99  /** Shared pointer to colored cloud. */
100  typedef typename ColorCloud::Ptr ColorCloudPtr;
101  /** Shared pointer to constant colored cloud. */
102  typedef typename ColorCloud::ConstPtr ColorCloudConstPtr;
103 
104  public:
105  /** Constructor.
106  * @param mongodb_client MongoDB client
107  * @param config configuration
108  * @param logger Logger
109  * @param output output point cloud
110  */
111  PointCloudDBPipeline(mongo::DBClientBase *mongodb_client,
112  fawkes::Configuration *config, fawkes::Logger *logger,
113  ColorCloudPtr output)
114  : mongodb_client_(mongodb_client), logger_(logger), output_(output)
115  {
116  name_ = "PCL_DB_Pipeline";
117 
119  (long)round(config->get_float(CFG_PREFIX"pcl-age-tolerance") * 1000.);
120  std::vector<float> transform_range = config->get_floats(CFG_PREFIX"transform-range");
121  if (transform_range.size() != 2) {
122  throw fawkes::Exception("Transform range must be a list with exactly two elements");
123  }
124  if (transform_range[1] < transform_range[0]) {
125  throw fawkes::Exception("Transform range start cannot be smaller than end");
126  }
127  cfg_transform_range_[0] = (long)round(transform_range[0] * 1000.);
128  cfg_transform_range_[1] = (long)round(transform_range[1] * 1000.);
129  }
130 
131  /** Destructor. */
133  {
134  }
135 
136  /** Check if this pipeline instance is suitable for the given times.
137  * Retrieves information about the point clouds for the specified
138  * \p times and checks if this pipeline (depending on the template
139  * parameter) is suitable for the processing of these pipelines.
140  * @param times times for which to check the point clouds
141  * @param database ddatabase from which to retrieve the information
142  * @param collection collection from which to retrieve the information
143  * @return applicability status
144  */
145  ApplicabilityStatus
146  applicable(std::vector<long long> &times, std::string &database,
147  std::string &collection)
148  {
149  const unsigned int num_clouds = times.size();
150 
151 #if PCL_VERSION_COMPARE(>=,1,7,0)
152  std::vector<pcl::PCLPointField> pfields;
153 #else
154  std::vector<sensor_msgs::PointField> pfields;
155 #endif
156  pcl::for_each_type<typename pcl::traits::fieldList<PointType>::type>
157  (pcl::detail::FieldAdder<PointType>(pfields));
158 
159  std::string fq_collection = database + "." + collection;
160  try {
161  for (unsigned int i = 0; i < num_clouds; ++i) {
162 #if __cplusplus >= 201103L
163  std::unique_ptr<mongo::DBClientCursor> cursor =
164 #else
165  std::auto_ptr<mongo::DBClientCursor> cursor =
166 #endif
167  mongodb_client_->query(fq_collection,
168  QUERY("timestamp" << mongo::LTE << times[i]
169  << mongo::GTE << (times[i] - cfg_pcl_age_tolerance_))
170  .sort("timestamp", -1),
171  /* limit */ 1);
172 
173  if (cursor->more()) {
174  mongo::BSONObj p = cursor->next();
175  mongo::BSONObj pcldoc = p.getObjectField("pointcloud");
176  std::vector<mongo::BSONElement> fields = pcldoc["field_info"].Array();
177 
178  if (fields.size() == pfields.size()) {
179  for (unsigned int i = 0; i < pfields.size(); ++i) {
180 #if PCL_VERSION_COMPARE(>=,1,7,0)
181  pcl::PCLPointField &pf = pfields[i];
182 #else
183  sensor_msgs::PointField &pf = pfields[i];
184 #endif
185 
186  bool found = false;
187  for (unsigned int j = 0; j < fields.size(); ++j) {
188  if ((fields[j]["name"].String() == pf.name) &&
189  (fields[j]["offset"].Int() == (int)pf.offset) &&
190  (fields[j]["datatype"].Int() == pf.datatype) &&
191  (fields[j]["count"].Int() == (int)pf.count) )
192  {
193  found = true;
194  break;
195  }
196  }
197  if (! found) {
198  logger_->log_warn(name_, "Type mismatch (fields) for pointcloud "
199  "at timestamp %lli", times[i]);
200  return TYPE_MISMATCH;
201  }
202  }
203  } else {
204  logger_->log_warn(name_, "Type mismatch (num fields) for pointcloud "
205  "at timestamp %lli", times[i]);
206  return TYPE_MISMATCH;
207  }
208  } else {
209  logger_->log_warn(name_, "No pointclouds for timestamp %lli in %s",
210  times[i], fq_collection.c_str());
211  return NO_POINTCLOUD;
212  }
213  }
214  } catch (mongo::DBException &e) {
215  logger_->log_warn(name_, "MongoDB query failed: %s", e.what());
216  return QUERY_FAILED;
217  }
218 
219  return APPLICABLE;
220  }
221 
222  protected: // methods
223  /** Read a file from MongoDB GridFS.
224  * @param dataptr Pointer to buffer to read data to. Make sure it is of
225  * sufficient size.
226  * @param database database from which to read the file
227  * @param filename name of file to read from GridFS.
228  */
229  void
230  read_gridfs_file(void *dataptr, std::string &database, std::string filename)
231  {
232  char *tmp = (char *)dataptr;
233  mongo::GridFS gridfs(*mongodb_client_, database);
234 
235  mongo::GridFile file =
236  gridfs.findFile(filename);
237  if (! file.exists()) {
238  logger_->log_warn(name_, "Grid file does not exist");
239  return;
240  }
241 
242  size_t bytes = 0;
243  for (int c = 0; c < file.getNumChunks(); ++c) {
244  mongo::GridFSChunk chunk = file.getChunk(c);
245  int len = 0;
246  const char *chunk_data = chunk.data(len);
247  memcpy(tmp, chunk_data, len);
248  tmp += len;
249  bytes += len;
250  //logger_->log_info(name_, "Read chunk %i of %i bytes", c, len);
251  }
252  //logger_->log_info(name_, "%zu bytes restored", bytes);
253  }
254 
255  /** Retrieve point clouds from database.
256  * @param times timestamps for when to read the point clouds. The method
257  * will retrieve the point clouds with the minimum difference between the
258  * desired and actual times.
259  * @param actual_times upon return contains the actual times of the point
260  * clouds retrieved based on the desired @p times.
261  * @param database name of the database to retrieve data from
262  * @param collection name of the collection to retrieve data from.
263  * @return vector of shared pointers to retrieved point clouds
264  */
265  std::vector<CloudPtr>
266  retrieve_clouds(std::vector<long long> &times, std::vector<long long> &actual_times,
267  std::string &database, std::string &collection)
268  {
269 #ifdef HAVE_MONGODB_VERSION_H
270  // mongodb-cxx-driver dropped ensureIndex and names it createIndex
271  mongodb_client_->createIndex(database + "." + collection,
272  mongo::fromjson("{timestamp:1}"));
273 #else
274  mongodb_client_->ensureIndex(database + "." + collection,
275  mongo::fromjson("{timestamp:1}"));
276 #endif
277 
278  const unsigned int num_clouds = times.size();
279  std::vector<CloudPtr> pcls(num_clouds);
280 
281  // retrieve point clouds
282  for (unsigned int i = 0; i < num_clouds; ++i) {
283 
284 #if __cplusplus >= 201103L
285  std::unique_ptr<mongo::DBClientCursor> cursor =
286 #else
287  std::auto_ptr<mongo::DBClientCursor> cursor =
288 #endif
289  mongodb_client_->query(database + "." + collection,
290  QUERY("timestamp" << mongo::LTE << times[i]
291  << mongo::GTE << (times[i] - cfg_pcl_age_tolerance_))
292  .sort("timestamp", -1),
293  /* limit */ 1);
294 
295  if (cursor->more()) {
296  mongo::BSONObj p = cursor->next();
297  mongo::BSONObj pcldoc = p.getObjectField("pointcloud");
298  std::vector<mongo::BSONElement> fields = pcldoc["field_info"].Array();
299 
300  long long timestamp = p["timestamp"].Long();
301  double age = (double) (times[i] - timestamp) / 1000.;
302  logger_->log_info(name_, "Restoring point cloud at %lli with age %f sec",
303  timestamp, age);
304 
305  // reconstruct point cloud
306  CloudPtr lpcl(new Cloud());
307  pcls[i] = lpcl;
308 
309  actual_times[i] = p["timestamp"].Number();
310  fawkes::Time actual_time((long)actual_times[i]);
311 
312  lpcl->header.frame_id = pcldoc["frame_id"].String();
313  lpcl->is_dense = pcldoc["is_dense"].Bool();
314  lpcl->width = pcldoc["width"].Int();
315  lpcl->height = pcldoc["height"].Int();
316  fawkes::pcl_utils::set_time(lpcl, actual_time);
317  lpcl->points.resize(pcldoc["num_points"].Int());
318 
319  read_gridfs_file(&lpcl->points[0], database,
320  pcldoc.getFieldDotted("data.filename").String());
321  } else {
322  logger_->log_warn(name_, "Cannot retrieve document for time %li", times[i]);
323  return std::vector<CloudPtr>();
324  }
325  }
326 
327  return pcls;
328  }
329 
330 
331  protected: // members
332  const char *name_; /**< Name of the pipeline. */
333 
334  long cfg_pcl_age_tolerance_; /**< Age tolerance for retrieved point clouds. */
335  long cfg_transform_range_[2]; /**< Transform range start and end times. */
336 
337  mongo::DBClientBase *mongodb_client_; /**< MongoDB client to retrieve data. */
338 
339  fawkes::Logger *logger_; /**< Logger for informative messages. */
340 
341  ColorCloudPtr output_; /**< The final (colored) output of the pipeline. */
342 
343 };
344 
345 /** Convert applicability status to readable string.
346  * @param status the status to convert
347  * @return readable string for status
348  */
349 inline const char *
350 to_string(ApplicabilityStatus status)
351 {
352  switch (status) {
353  case APPLICABLE: return "Applicable";
354  case TYPE_MISMATCH: return "PointCloud in database does not match type";
355  case NO_POINTCLOUD: return "For at least one time no pointcloud found";
356  case QUERY_FAILED: return "MongoDB query failed";
357  default: return "Unknown error";
358  }
359 }
360 
361 #endif
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Cloud::Ptr CloudPtr
Shared pointer to cloud.
pcl::PointCloud< PointType > Cloud
Basic point cloud type.
mongo::DBClientBase * mongodb_client_
MongoDB client to retrieve data.
A class for handling time.
Definition: time.h:91
virtual ~PointCloudDBPipeline()
Destructor.
long cfg_pcl_age_tolerance_
Age tolerance for retrieved point clouds.
pcl::PointCloud< ColorPointType > ColorCloud
Type for colored point clouds based on ColorPointType.
void read_gridfs_file(void *dataptr, std::string &database, std::string filename)
Read a file from MongoDB GridFS.
pcl::PointXYZRGB ColorPointType
Colored point type.
Cloud::ConstPtr CloudConstPtr
Shared pointer to constant cloud.
Database point cloud pipeline base class.
fawkes::Logger * logger_
Logger for informative messages.
ColorCloud::Ptr ColorCloudPtr
Shared pointer to colored cloud.
Base class for exceptions in Fawkes.
Definition: exception.h:36
PointCloudDBPipeline(mongo::DBClientBase *mongodb_client, fawkes::Configuration *config, fawkes::Logger *logger, ColorCloudPtr output)
Constructor.
const char * name_
Name of the pipeline.
virtual std::vector< float > get_floats(const char *path)=0
Get list of values from configuration which is of type float.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
ColorCloudPtr output_
The final (colored) output of the pipeline.
ColorCloud::ConstPtr ColorCloudConstPtr
Shared pointer to constant colored cloud.
long cfg_transform_range_[2]
Transform range start and end times.
Interface for configuration handling.
Definition: config.h:67
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
ApplicabilityStatus applicable(std::vector< long long > &times, std::string &database, std::string &collection)
Check if this pipeline instance is suitable for the given times.
std::vector< CloudPtr > retrieve_clouds(std::vector< long long > &times, std::vector< long long > &actual_times, std::string &database, std::string &collection)
Retrieve point clouds from database.
Interface for logging.
Definition: logger.h:34