Fawkes API  Fawkes Development Version
pcl_db_retrieve_pipeline.h
1 
2 /***************************************************************************
3  * pcl_db_retrieve_pipeline.h - Retrieve point clouds from MongoDB
4  * Template class for variying point types
5  *
6  * Created: Thu Aug 22 11:02:59 2013
7  * Copyright 2012-2013 Tim Niemueller [www.niemueller.de]
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #ifndef _PLUGINS_PERCEPTION_PCL_DB_PCL_DB_RETRIEVE_PIPELINE_H_
24 #define _PLUGINS_PERCEPTION_PCL_DB_PCL_DB_RETRIEVE_PIPELINE_H_
25 
26 #include "mongodb_tf_transformer.h"
27 #include "pcl_db_pipeline.h"
28 
29 #include <pcl_utils/transforms.h>
30 #include <pcl_utils/utils.h>
31 #include <tf/transformer.h>
32 #ifdef USE_TIMETRACKER
33 # include <utils/time/tracker.h>
34 #endif
35 #include <utils/time/tracker_macros.h>
36 
37 #define CFG_PREFIX_RETRV "/perception/pcl-db-retrieve/"
38 
39 #include <mongo/client/dbclient.h>
40 #include <mongo/client/gridfs.h>
41 #include <pcl/point_cloud.h>
42 #include <pcl/point_types.h>
43 
44 /** Point cloud retrieve pipeline.
45  * This pipeline retrieves a point cloud from the database at a given point
46  * in time. The process assums a coordinate frame which is fixed in both,
47  * the recorded transforms and the current transform tree. Using this fixed
48  * frame it first transforms the point cloud to the fixed frame using the
49  * recorded transforms in the database, and then to transform it to the
50  * desired sensor frame using the current data.
51  * @author Tim Niemueller
52  */
53 template <typename PointType>
55 {
56 public:
57  /** Constructor.
58  * @param mongodb_client MongoDB client
59  * @param config configuration
60  * @param logger Logger
61  * @param output output point cloud
62  * @param transformer TF transformer for point cloud transformations between
63  * coordinate reference frames
64  * @param original input point cloud
65  * @param output output point cloud
66  */
67  PointCloudDBRetrievePipeline(mongo::DBClientBase * mongodb_client,
68  fawkes::Configuration * config,
69  fawkes::Logger * logger,
70  fawkes::tf::Transformer *transformer,
73  : PointCloudDBPipeline<PointType>(mongodb_client, config, logger, output),
74  tf_(transformer),
75  original_(original)
76  {
77  this->name_ = "PCL_DB_RetrievePL";
78 
79  cfg_fixed_frame_ = config->get_string(CFG_PREFIX_RETRV "fixed-frame");
80  cfg_sensor_frame_ = config->get_string(CFG_PREFIX_RETRV "sensor-frame");
81 
82 #ifdef USE_TIMETRACKER
83  tt_ = new fawkes::TimeTracker();
84  tt_loopcount_ = 0;
85  ttc_retrieve_ = tt_->add_class("Full Retrieve");
86  ttc_retrieval_ = tt_->add_class("Retrieval");
87  ttc_transforms_ = tt_->add_class("Transforms");
88 #endif
89  }
90 
91  /** Destructor. */
93  {
94 #ifdef USE_TIMETRACKER
95  delete tt_;
96 #endif
97  }
98 
99  /** Retrieve point clouds.
100  * @param timestamp time for which to retrieve the point cloud
101  * @param database database to retrieve from
102  * @param collection collection from which to retrieve the data
103  * @param target_frame coordinate frame to transform to
104  * @param actual_time upon return contains the actual time for
105  * which a point cloud was retrieved
106  */
107  void
108  retrieve(long long timestamp,
109  std::string &database,
110  std::string &collection,
111  std::string &target_frame,
112  long long & actual_time)
113  {
114  TIMETRACK_START(ttc_retrieve_);
115 
116  this->output_->points.clear();
117  this->output_->height = 1;
118  this->output_->width = 0;
119  this->output_->is_dense = false;
120 
121  std::vector<long long> times(1, timestamp);
122  std::vector<long long> actual_times(1, 0);
123  std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> pcls(1);
124 
125  TIMETRACK_START(ttc_retrieval_);
126 
127  pcls =
128  PointCloudDBPipeline<PointType>::retrieve_clouds(times, actual_times, database, collection);
129  if (pcls.empty()) {
130  this->logger_->log_warn(this->name_, "No point clouds found for desired timestamp");
131  TIMETRACK_ABORT(ttc_retrieval_);
132  TIMETRACK_ABORT(ttc_retrieve_);
133  return;
134  }
135 
136  copy_output(pcls[0], original_, 128, 128, 128);
137  actual_time = actual_times[0];
138 
139  if (target_frame == "SENSOR") {
140  target_frame = cfg_sensor_frame_;
141  }
142 
143  if (target_frame != "") {
144  // perform transformation
145 
146  TIMETRACK_INTER(ttc_retrieval_, ttc_transforms_);
147 
148  // retrieve transforms
149  fawkes::tf::MongoDBTransformer transformer(this->mongodb_client_, database);
150 
151  transformer.restore(/* start */ actual_times[0] + this->cfg_transform_range_[0],
152  /* end */ actual_times[0] + this->cfg_transform_range_[1]);
153  this->logger_->log_debug(this->name_,
154  "Restored transforms for %zu frames for range (%lli..%lli)",
155  transformer.get_frame_caches().size(),
156  /* start */ actual_times[0] + this->cfg_transform_range_[0],
157  /* end */ actual_times[0] + this->cfg_transform_range_[1]);
158 
159  fawkes::Time source_time;
160  fawkes::pcl_utils::get_time(pcls[0], source_time);
161  fawkes::tf::StampedTransform transform_recorded;
162  transformer.lookup_transform(cfg_fixed_frame_,
163  pcls[0]->header.frame_id,
164  source_time,
165  transform_recorded);
166 
167  fawkes::tf::StampedTransform transform_current;
168  tf_->lookup_transform(cfg_sensor_frame_, cfg_fixed_frame_, transform_current);
169 
170  fawkes::tf::Transform transform = transform_current * transform_recorded;
171 
172  try {
173  fawkes::pcl_utils::transform_pointcloud(*pcls[0], transform);
174  } catch (fawkes::Exception &e) {
175  this->logger_->log_warn(this->name_, "Failed to transform point cloud, exception follows");
176  this->logger_->log_warn(this->name_, e);
177  }
178 
179  // retrieve point clouds
180  TIMETRACK_END(ttc_transforms_);
181  }
182 
183  copy_output(pcls[0], this->output_);
184 
185  TIMETRACK_END(ttc_retrieve_);
186 
187 #ifdef USE_TIMETRACKER
188  //if (++tt_loopcount_ >= 5) {
189  // tt_loopcount_ = 0;
190  tt_->print_to_stdout();
191  //}
192 #endif
193  }
194 
195 private: //methods
196  void
199  {
200  size_t num_points = in->points.size();
201  out->header.frame_id = in->header.frame_id;
202  out->points.resize(num_points);
203  out->height = 1;
204  out->width = num_points;
205 
206  for (size_t p = 0; p < num_points; ++p) {
207  const PointType & ip = in->points[p];
208  typename PointCloudDBPipeline<PointType>::ColorPointType &op = out->points[p];
209 
210  op.x = ip.x;
211  op.y = ip.y;
212  op.z = ip.z;
213 
214  op.r = ip.r;
215  op.g = ip.g;
216  op.b = ip.b;
217  }
218  }
219 
220  void
223  const int r,
224  const int g,
225  const int b)
226  {
227  size_t num_points = in->points.size();
228  out->header.frame_id = in->header.frame_id;
229  out->points.resize(num_points);
230  out->height = 1;
231  out->width = num_points;
232 
233  for (size_t p = 0; p < num_points; ++p) {
234  const PointType & ip = in->points[p];
235  typename PointCloudDBPipeline<PointType>::ColorPointType &op = out->points[p];
236 
237  op.x = ip.x;
238  op.y = ip.y;
239  op.z = ip.z;
240 
241  op.r = r;
242  op.g = g;
243  op.b = b;
244  }
245  }
246 
247  void
248  copy_output(pcl::PointCloud<pcl::PointXYZ>::Ptr & in,
250  const int r = 255,
251  const int g = 255,
252  const int b = 255)
253  {
254  size_t num_points = in->points.size();
255  out->header.frame_id = in->header.frame_id;
256  out->points.resize(num_points);
257  out->height = 1;
258  out->width = num_points;
259 
260  for (size_t p = 0; p < num_points; ++p) {
261  const PointType & ip = in->points[p];
262  typename PointCloudDBPipeline<PointType>::ColorPointType &op = out->points[p];
263 
264  op.x = ip.x;
265  op.y = ip.y;
266  op.z = ip.z;
267 
268  op.r = r;
269  op.g = g;
270  op.b = b;
271  }
272  }
273 
274 private: // members
275  std::string cfg_fixed_frame_;
276  std::string cfg_sensor_frame_;
277 
279 
281 
282 #ifdef USE_TIMETRACKER
283  fawkes::TimeTracker *tt_;
284  unsigned int tt_loopcount_;
285  unsigned int ttc_retrieve_;
286  unsigned int ttc_retrieval_;
287  unsigned int ttc_transforms_;
288 #endif
289 };
290 
291 #endif
PointCloudDBRetrievePipeline(mongo::DBClientBase *mongodb_client, fawkes::Configuration *config, fawkes::Logger *logger, fawkes::tf::Transformer *transformer, typename PointCloudDBPipeline< PointType >::ColorCloudPtr original, typename PointCloudDBPipeline< PointType >::ColorCloudPtr output)
Constructor.
mongo::DBClientBase * mongodb_client_
MongoDB client to retrieve data.
A class for handling time.
Definition: time.h:92
void retrieve(long long timestamp, std::string &database, std::string &collection, std::string &target_frame, long long &actual_time)
Retrieve point clouds.
pcl::PointXYZRGB ColorPointType
Colored point type.
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:35
Transform that contains a timestamp and frame IDs.
Definition: types.h:91
Time tracking utility.
Definition: tracker.h:36
const char * name_
Name of the pipeline.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
ColorCloudPtr output_
The final (colored) output of the pipeline.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
Point cloud retrieve pipeline.
long cfg_transform_range_[2]
Transform range start and end times.
std::vector< TimeCacheInterfacePtr > get_frame_caches() const
Get all currently existing caches.
Coordinate transforms between any two frames in a system.
Definition: transformer.h:64
Interface for configuration handling.
Definition: config.h:64
Read transforms from MongoDB and answer queries.
void restore(fawkes::Time &start, fawkes::Time &end)
Restore transforms from database.
virtual ~PointCloudDBRetrievePipeline()
Destructor.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
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:41