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 <config/config.h>
28 #include <logging/logger.h>
29 #include <pcl/point_cloud.h>
30 #include <pcl/point_types.h>
31 #include <pcl_utils/comparisons.h>
32 #include <pcl_utils/storage_adapter.h>
33 #include <pcl_utils/transforms.h>
34 #include <pcl_utils/utils.h>
35 
36 #include <Eigen/Core>
37 #ifdef USE_TIMETRACKER
38 # include <utils/time/tracker.h>
39 #endif
40 #include <utils/time/tracker_macros.h>
41 
42 #define USE_ALIGNMENT
43 #define USE_ICP_ALIGNMENT
44 // define USE_NDT_ALIGNMENT
45 
46 #define CFG_PREFIX "/perception/pcl-db/"
47 
48 #include <pcl/filters/approximate_voxel_grid.h>
49 #include <pcl/filters/conditional_removal.h>
50 #include <pcl/filters/extract_indices.h>
51 #include <pcl/filters/passthrough.h>
52 #include <pcl/filters/voxel_grid.h>
53 #include <pcl/point_cloud.h>
54 #include <pcl/point_types.h>
55 #include <pcl/segmentation/sac_segmentation.h>
56 #include <pcl/surface/convex_hull.h>
57 
58 #include <bsoncxx/builder/basic/document.hpp>
59 #include <mongocxx/client.hpp>
60 #include <mongocxx/exception/gridfs_exception.hpp>
61 #include <mongocxx/exception/operation_exception.hpp>
62 #include <mongocxx/gridfs/bucket.hpp>
63 #include <mongocxx/gridfs/downloader.hpp>
64 
65 #ifdef HAVE_MONGODB_VERSION_H
66 // we are using mongo-cxx-driver which renamed QUERY to MONGO_QUERY
67 # define QUERY MONGO_QUERY
68 #endif
69 
70 static const uint8_t cluster_colors[12][3] = {{176, 0, 30},
71  {0, 0, 255},
72  {255, 90, 0},
73  {137, 82, 39},
74  {56, 23, 90},
75  {99, 0, 30},
76  {255, 0, 0},
77  {0, 255, 0},
78  {255, 255, 0},
79  {255, 0, 255},
80  {0, 255, 255},
81  {27, 117, 196}};
82 
83 typedef enum { APPLICABLE = 0, TYPE_MISMATCH, NO_POINTCLOUD, QUERY_FAILED } ApplicabilityStatus;
84 
85 /** Database point cloud pipeline base class.
86  * Common functionality for pcl-db-* plugins operating on
87  * point clouds restored from MongoDB.
88  * @author Tim Niemueller
89  */
90 template <typename PointType>
92 {
93 protected:
94  /** Basic point cloud type. */
96 
97  /** Colored point type */
98  typedef pcl::PointXYZRGB ColorPointType;
99  /** Type for colored point clouds based on ColorPointType. */
101  /** Shared pointer to cloud. */
102  typedef typename Cloud::Ptr CloudPtr;
103  /** Shared pointer to constant cloud. */
104  typedef typename Cloud::ConstPtr CloudConstPtr;
105 
106  /** Shared pointer to colored cloud. */
107  typedef typename ColorCloud::Ptr ColorCloudPtr;
108  /** Shared pointer to constant colored cloud. */
109  typedef typename ColorCloud::ConstPtr ColorCloudConstPtr;
110 
111 public:
112  /** Constructor.
113  * @param mongodb_client MongoDB client
114  * @param config configuration
115  * @param logger Logger
116  * @param output output point cloud
117  */
118  PointCloudDBPipeline(mongocxx::client * mongodb_client,
119  fawkes::Configuration *config,
120  fawkes::Logger * logger,
121  ColorCloudPtr output)
122  : mongodb_client_(mongodb_client), logger_(logger), output_(output)
123  {
124  name_ = "PCL_DB_Pipeline";
125 
126  cfg_pcl_age_tolerance_ = (long)round(config->get_float(CFG_PREFIX "pcl-age-tolerance") * 1000.);
127  std::vector<float> transform_range = config->get_floats(CFG_PREFIX "transform-range");
128  if (transform_range.size() != 2) {
129  throw fawkes::Exception("Transform range must be a list with exactly two elements");
130  }
131  if (transform_range[1] < transform_range[0]) {
132  throw fawkes::Exception("Transform range start cannot be smaller than end");
133  }
134  cfg_transform_range_[0] = (long)round(transform_range[0] * 1000.);
135  cfg_transform_range_[1] = (long)round(transform_range[1] * 1000.);
136  }
137 
138  /** Destructor. */
140  {
141  }
142 
143  /** Check if this pipeline instance is suitable for the given times.
144  * Retrieves information about the point clouds for the specified
145  * \p times and checks if this pipeline (depending on the template
146  * parameter) is suitable for the processing of these pipelines.
147  * @param times times for which to check the point clouds
148  * @param database ddatabase from which to retrieve the information
149  * @param collection collection from which to retrieve the information
150  * @return applicability status
151  */
152  ApplicabilityStatus
153  applicable(std::vector<long long> &times, std::string &database, std::string &collection)
154  {
155  const unsigned int num_clouds = times.size();
156 
157 #if PCL_VERSION_COMPARE(>=, 1, 7, 0)
158  std::vector<pcl::PCLPointField> pfields;
159 #else
160  std::vector<sensor_msgs::PointField> pfields;
161 #endif
162  pcl::for_each_type<typename pcl::traits::fieldList<PointType>::type>(
163  pcl::detail::FieldAdder<PointType>(pfields));
164 
165  try {
166  for (unsigned int i = 0; i < num_clouds; ++i) {
167  using namespace bsoncxx::builder;
168  auto result = mongodb_client_->database(database)[collection].find_one(
169  basic::make_document(
170  basic::kvp("timestamp",
171  [&](basic::sub_document subdoc) {
172  subdoc.append(basic::kvp("$lt", static_cast<int64_t>(times[i])));
173  subdoc.append(
174  basic::kvp("$gt",
175  static_cast<int64_t>(times[i] - cfg_pcl_age_tolerance_)));
176  })),
177  mongocxx::options::find().sort(basic::make_document(basic::kvp("timestamp", -1))));
178  if (result) {
179  bsoncxx::document::view pcldoc = result->view()["pointcloud"].get_document().view();
180  bsoncxx::array::view fields = pcldoc["field_info"].get_array();
181 
182  if (fields.length() == pfields.size()) {
183  for (unsigned int i = 0; i < pfields.size(); ++i) {
184 #if PCL_VERSION_COMPARE(>=, 1, 7, 0)
185  pcl::PCLPointField &pf = pfields[i];
186 #else
187  sensor_msgs::PointField &pf = pfields[i];
188 #endif
189 
190  bool found = false;
191  for (unsigned int j = 0; j < fields.length(); ++j) {
192  if ((fields[j]["name"].get_utf8().value.to_string() == pf.name)
193  && (fields[j]["offset"].get_int64() == (int64_t)pf.offset)
194  && (fields[j]["datatype"].get_int64() == pf.datatype)
195  && (fields[j]["count"].get_int64() == (int64_t)pf.count)) {
196  found = true;
197  break;
198  }
199  }
200  if (!found) {
202  "Type mismatch (fields) for pointcloud "
203  "at timestamp %lli",
204  times[i]);
205  return TYPE_MISMATCH;
206  }
207  }
208  } else {
210  "Type mismatch (num fields) for pointcloud "
211  "at timestamp %lli",
212  times[i]);
213  return TYPE_MISMATCH;
214  }
215  } else {
217  "No pointclouds for timestamp %lli in %s.%s",
218  times[i],
219  database.c_str(),
220  collection.c_str());
221  return NO_POINTCLOUD;
222  }
223  }
224  } catch (mongocxx::operation_exception &e) {
225  logger_->log_warn(name_, "MongoDB query failed: %s", e.what());
226  return QUERY_FAILED;
227  }
228 
229  return APPLICABLE;
230  }
231 
232 protected: // methods
233  /** Read a file from MongoDB GridFS.
234  * @param dataptr Pointer to buffer to read data to. Make sure it is of
235  * sufficient size.
236  * @param database database from which to read the file
237  * @param file_id The bucket ID of the file to read
238  */
239  void
240  read_gridfs_file(void *dataptr, std::string &database, bsoncxx::types::value file_id)
241  {
242  auto gridfs = mongodb_client_->database(database).gridfs_bucket();
243  try {
244  auto downloader = gridfs.open_download_stream(file_id);
245  auto file_length = downloader.file_length();
246  auto buffer_size = std::min(file_length, static_cast<int64_t>(downloader.chunk_size()));
247  unsigned char *tmp = (unsigned char *)dataptr;
248  while (auto length_read = downloader.read(tmp, static_cast<std::size_t>(buffer_size))) {
249  tmp += length_read;
250  }
251  } catch (mongocxx::gridfs_exception &e) {
252  logger_->log_warn(name_, "Grid file does not exist");
253  return;
254  }
255  }
256 
257  /** Retrieve point clouds from database.
258  * @param times timestamps for when to read the point clouds. The method
259  * will retrieve the point clouds with the minimum difference between the
260  * desired and actual times.
261  * @param actual_times upon return contains the actual times of the point
262  * clouds retrieved based on the desired @p times.
263  * @param database name of the database to retrieve data from
264  * @param collection_name name of the collection to retrieve data from.
265  * @return vector of shared pointers to retrieved point clouds
266  */
267  std::vector<CloudPtr>
268  retrieve_clouds(std::vector<long> &times,
269  std::vector<long> &actual_times,
270  std::string & database,
271  std::string & collection_name)
272  {
273  using namespace bsoncxx::builder;
274  auto collection = mongodb_client_->database(database)[collection_name];
275  collection.create_index(basic::make_document(basic::kvp("timestamp", 1)));
276 
277  const unsigned int num_clouds = times.size();
278  std::vector<CloudPtr> pcls(num_clouds);
279 
280  // retrieve point clouds
281  for (unsigned int i = 0; i < num_clouds; ++i) {
282  auto result = collection.find_one(
283  basic::make_document(basic::kvp("timestamp",
284  [&](basic::sub_document subdoc) {
285  subdoc.append(basic::kvp("$lt", times[i]));
286  subdoc.append(
287  basic::kvp("$gt", times[i] - cfg_pcl_age_tolerance_));
288  })),
289  mongocxx::options::find().sort(basic::make_document(basic::kvp("timestamp", -1))));
290  if (result) {
291  bsoncxx::document::view pcldoc = result->view()["pointcloud"].get_document().view();
292  //bsoncxx::array::view fields = pcldoc["field_info"].get_array();
293  int64_t timestamp = result->view()["timestamp"].get_int64();
294  double age = (double)(times[i] - timestamp) / 1000.;
295 
296  logger_->log_info(name_, "Restoring point cloud at %li with age %f sec", timestamp, age);
297 
298  // reconstruct point cloud
299  CloudPtr lpcl(new Cloud());
300  pcls[i] = lpcl;
301 
302  actual_times[i] = timestamp;
303  fawkes::Time actual_time((long)actual_times[i]);
304 
305  lpcl->header.frame_id = pcldoc["frame_id"].get_utf8().value.to_string();
306  lpcl->is_dense = pcldoc["is_dense"].get_bool();
307  lpcl->width = pcldoc["width"].get_int64();
308  lpcl->height = pcldoc["height"].get_int64();
309  fawkes::pcl_utils::set_time(lpcl, actual_time);
310  lpcl->points.resize(pcldoc["num_points"].get_int64());
311 
312  read_gridfs_file(&lpcl->points[0], database, pcldoc["data"]["id"].get_value());
313  } else {
314  logger_->log_warn(name_, "Cannot retrieve document for time %li", times[i]);
315  return std::vector<CloudPtr>();
316  }
317  }
318 
319  return pcls;
320  }
321 
322 protected: // members
323  const char *name_; /**< Name of the pipeline. */
324 
325  long cfg_pcl_age_tolerance_; /**< Age tolerance for retrieved point clouds. */
326  long cfg_transform_range_[2]; /**< Transform range start and end times. */
327 
328  mongocxx::client *mongodb_client_; /**< MongoDB client to retrieve data. */
329 
330  fawkes::Logger *logger_; /**< Logger for informative messages. */
331 
332  ColorCloudPtr output_; /**< The final (colored) output of the pipeline. */
333 };
334 
335 /** Convert applicability status to readable string.
336  * @param status the status to convert
337  * @return readable string for status
338  */
339 inline const char *
340 to_string(ApplicabilityStatus status)
341 {
342  switch (status) {
343  case APPLICABLE: return "Applicable";
344  case TYPE_MISMATCH: return "PointCloud in database does not match type";
345  case NO_POINTCLOUD: return "For at least one time no pointcloud found";
346  case QUERY_FAILED: return "MongoDB query failed";
347  default: return "Unknown error";
348  }
349 }
350 
351 #endif
PointCloudDBPipeline::ColorCloudConstPtr
ColorCloud::ConstPtr ColorCloudConstPtr
Shared pointer to constant colored cloud.
Definition: pcl_db_pipeline.h:109
PointCloudDBPipeline::CloudConstPtr
Cloud::ConstPtr CloudConstPtr
Shared pointer to constant cloud.
Definition: pcl_db_pipeline.h:104
PointCloudDBPipeline::ColorCloud
pcl::PointCloud< ColorPointType > ColorCloud
Type for colored point clouds based on ColorPointType.
Definition: pcl_db_pipeline.h:100
PointCloudDBPipeline
Database point cloud pipeline base class.
Definition: pcl_db_pipeline.h:92
PointCloudDBPipeline::~PointCloudDBPipeline
virtual ~PointCloudDBPipeline()
Destructor.
Definition: pcl_db_pipeline.h:139
PointCloudDBPipeline::output_
ColorCloudPtr output_
The final (colored) output of the pipeline.
Definition: pcl_db_pipeline.h:332
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
PointCloudDBPipeline::PointCloudDBPipeline
PointCloudDBPipeline(mongocxx::client *mongodb_client, fawkes::Configuration *config, fawkes::Logger *logger, ColorCloudPtr output)
Constructor.
Definition: pcl_db_pipeline.h:118
fawkes::Configuration
Interface for configuration handling.
Definition: config.h:65
PointCloudDBPipeline::logger_
fawkes::Logger * logger_
Logger for informative messages.
Definition: pcl_db_pipeline.h:330
fawkes::Configuration::get_floats
virtual std::vector< float > get_floats(const char *path)=0
Get list of values from configuration which is of type float.
fawkes::Logger
Interface for logging.
Definition: logger.h:42
PointCloudDBPipeline::ColorPointType
pcl::PointXYZRGB ColorPointType
Colored point type.
Definition: pcl_db_pipeline.h:98
PointCloudDBPipeline::retrieve_clouds
std::vector< CloudPtr > retrieve_clouds(std::vector< long > &times, std::vector< long > &actual_times, std::string &database, std::string &collection_name)
Retrieve point clouds from database.
Definition: pcl_db_pipeline.h:268
PointCloudDBPipeline::read_gridfs_file
void read_gridfs_file(void *dataptr, std::string &database, bsoncxx::types::value file_id)
Read a file from MongoDB GridFS.
Definition: pcl_db_pipeline.h:240
PointCloudDBPipeline::mongodb_client_
mongocxx::client * mongodb_client_
MongoDB client to retrieve data.
Definition: pcl_db_pipeline.h:328
pcl::PointCloud< PointType >
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
PointCloudDBPipeline::name_
const char * name_
Name of the pipeline.
Definition: pcl_db_pipeline.h:323
PointCloudDBPipeline::cfg_pcl_age_tolerance_
long cfg_pcl_age_tolerance_
Age tolerance for retrieved point clouds.
Definition: pcl_db_pipeline.h:325
PointCloudDBPipeline::applicable
ApplicabilityStatus applicable(std::vector< long long > &times, std::string &database, std::string &collection)
Check if this pipeline instance is suitable for the given times.
Definition: pcl_db_pipeline.h:153
PointCloudDBPipeline::Cloud
pcl::PointCloud< PointType > Cloud
Basic point cloud type.
Definition: pcl_db_pipeline.h:95
fawkes::Time
A class for handling time.
Definition: time.h:93
PointCloudDBPipeline::cfg_transform_range_
long cfg_transform_range_[2]
Transform range start and end times.
Definition: pcl_db_pipeline.h:326
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
PointCloudDBPipeline::CloudPtr
Cloud::Ptr CloudPtr
Shared pointer to cloud.
Definition: pcl_db_pipeline.h:102
PointCloudDBPipeline::ColorCloudPtr
ColorCloud::Ptr ColorCloudPtr
Shared pointer to colored cloud.
Definition: pcl_db_pipeline.h:107
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36