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 <tf/transformer.h>
30 #include <pcl_utils/utils.h>
31 #include <pcl_utils/transforms.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 <pcl/point_cloud.h>
40 #include <pcl/point_types.h>
41 
42 #include <mongo/client/dbclient.h>
43 #include <mongo/client/gridfs.h>
44 
45 /** Point cloud retrieve pipeline.
46  * This pipeline retrieves a point cloud from the database at a given point
47  * in time. The process assums a coordinate frame which is fixed in both,
48  * the recorded transforms and the current transform tree. Using this fixed
49  * frame it first transforms the point cloud to the fixed frame using the
50  * recorded transforms in the database, and then to transform it to the
51  * desired sensor frame using the current data.
52  * @author Tim Niemueller
53  */
54 template <typename PointType>
56 {
57  public:
58  /** Constructor.
59  * @param mongodb_client MongoDB client
60  * @param config configuration
61  * @param logger Logger
62  * @param output output point cloud
63  * @param transformer TF transformer for point cloud transformations between
64  * coordinate reference frames
65  * @param original input point cloud
66  * @param output output point cloud
67  */
68  PointCloudDBRetrievePipeline(mongo::DBClientBase *mongodb_client,
69  fawkes::Configuration *config, fawkes::Logger *logger,
70  fawkes::tf::Transformer *transformer,
73  : PointCloudDBPipeline<PointType>(mongodb_client, config, logger, output),
74  tf_(transformer), original_(original)
75  {
76  this->name_ = "PCL_DB_RetrievePL";
77 
78  cfg_fixed_frame_ = config->get_string(CFG_PREFIX_RETRV"fixed-frame");
79  cfg_sensor_frame_ = config->get_string(CFG_PREFIX_RETRV"sensor-frame");
80 
81 #ifdef USE_TIMETRACKER
82  tt_ = new fawkes::TimeTracker();
83  tt_loopcount_ = 0;
84  ttc_retrieve_ = tt_->add_class("Full Retrieve");
85  ttc_retrieval_ = tt_->add_class("Retrieval");
86  ttc_transforms_ = tt_->add_class("Transforms");
87 #endif
88  }
89 
90  /** Destructor. */
92  {
93 #ifdef USE_TIMETRACKER
94  delete tt_;
95 #endif
96  }
97 
98  /** Retrieve point clouds.
99  * @param timestamp time for which to retrieve the point cloud
100  * @param database database to retrieve from
101  * @param collection collection from which to retrieve the data
102  * @param target_frame coordinate frame to transform to
103  * @param actual_time upon return contains the actual time for
104  * which a point cloud was retrieved
105  */
106  void
107  retrieve(long long timestamp, std::string &database,
108  std::string &collection, std::string &target_frame,
109  long long &actual_time)
110  {
111  TIMETRACK_START(ttc_retrieve_);
112 
113  this->output_->points.clear();
114  this->output_->height = 1;
115  this->output_->width = 0;
116  this->output_->is_dense = false;
117 
118  std::vector<long long> times(1, timestamp);
119  std::vector<long long> actual_times(1, 0);
120  std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> pcls(1);
121 
122  TIMETRACK_START(ttc_retrieval_);
123 
124  pcls = PointCloudDBPipeline<PointType>::retrieve_clouds(times, actual_times, database, collection);
125  if (pcls.empty()) {
126  this->logger_->log_warn(this->name_, "No point clouds found for desired timestamp");
127  TIMETRACK_ABORT(ttc_retrieval_);
128  TIMETRACK_ABORT(ttc_retrieve_);
129  return;
130  }
131 
132  copy_output(pcls[0], original_, 128, 128, 128);
133  actual_time = actual_times[0];
134 
135  if (target_frame == "SENSOR") {
136  target_frame == cfg_sensor_frame_;
137  }
138 
139  if (target_frame != "") {
140  // perform transformation
141 
142  TIMETRACK_INTER(ttc_retrieval_, ttc_transforms_);
143 
144  // retrieve transforms
146  transformer(this->mongodb_client_, database);
147 
148  transformer.restore(/* start */ actual_times[0] + this->cfg_transform_range_[0],
149  /* end */ actual_times[0] + this->cfg_transform_range_[1]);
150  this->logger_->log_debug(this->name_,
151  "Restored transforms for %zu frames for range (%li..%li)",
152  transformer.get_frame_caches().size(),
153  /* start */ actual_times[0] + this->cfg_transform_range_[0],
154  /* end */ actual_times[0] + this->cfg_transform_range_[1]);
155 
156  fawkes::Time source_time;
157  fawkes::pcl_utils::get_time(pcls[0], source_time);
158  fawkes::tf::StampedTransform transform_recorded;
159  transformer.lookup_transform(cfg_fixed_frame_, pcls[0]->header.frame_id,
160  source_time, transform_recorded);
161 
162  fawkes::tf::StampedTransform transform_current;
163  tf_->lookup_transform(cfg_sensor_frame_, cfg_fixed_frame_, transform_current);
164 
165  fawkes::tf::Transform transform = transform_current * transform_recorded;
166 
167  try {
168  fawkes::pcl_utils::transform_pointcloud(*pcls[0], transform);
169  } catch (fawkes::Exception &e) {
170  this->logger_->log_warn(this->name_,
171  "Failed to transform point cloud, exception follows");
172  this->logger_->log_warn(this->name_, e);
173  }
174 
175  // retrieve point clouds
176  TIMETRACK_END(ttc_transforms_);
177  }
178 
179  copy_output(pcls[0], this->output_);
180 
181  TIMETRACK_END(ttc_retrieve_);
182 
183 #ifdef USE_TIMETRACKER
184  //if (++tt_loopcount_ >= 5) {
185  // tt_loopcount_ = 0;
186  tt_->print_to_stdout();
187  //}
188 #endif
189  }
190 
191  private: //methods
192  void copy_output(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &in,
194  {
195  size_t num_points = in->points.size();
196  out->header.frame_id = in->header.frame_id;
197  out->points.resize(num_points);
198  out->height = 1;
199  out->width = num_points;
200 
201  for (size_t p = 0; p < num_points; ++p) {
202  const PointType &ip = in->points[p];
203  typename PointCloudDBPipeline<PointType>::ColorPointType &op = out->points[p];
204 
205  op.x = ip.x;
206  op.y = ip.y;
207  op.z = ip.z;
208 
209  op.r = ip.r;
210  op.g = ip.g;
211  op.b = ip.b;
212  }
213  }
214 
215  void copy_output(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &in,
217  const int r, const int g, const int b)
218  {
219  size_t num_points = in->points.size();
220  out->header.frame_id = in->header.frame_id;
221  out->points.resize(num_points);
222  out->height = 1;
223  out->width = num_points;
224 
225  for (size_t p = 0; p < num_points; ++p) {
226  const PointType &ip = in->points[p];
227  typename PointCloudDBPipeline<PointType>::ColorPointType &op = out->points[p];
228 
229  op.x = ip.x;
230  op.y = ip.y;
231  op.z = ip.z;
232 
233  op.r = r;
234  op.g = g;
235  op.b = b;
236  }
237  }
238 
239  void copy_output(pcl::PointCloud<pcl::PointXYZ>::Ptr &in,
241  const int r = 255, const int g = 255, const int b = 255)
242  {
243  size_t num_points = in->points.size();
244  out->header.frame_id = in->header.frame_id;
245  out->points.resize(num_points);
246  out->height = 1;
247  out->width = num_points;
248 
249  for (size_t p = 0; p < num_points; ++p) {
250  const PointType &ip = in->points[p];
251  typename PointCloudDBPipeline<PointType>::ColorPointType &op = out->points[p];
252 
253  op.x = ip.x;
254  op.y = ip.y;
255  op.z = ip.z;
256 
257  op.r = r;
258  op.g = g;
259  op.b = b;
260  }
261  }
262 
263 
264  private: // members
265  std::string cfg_fixed_frame_;
266  std::string cfg_sensor_frame_;
267 
269 
271 
272 #ifdef USE_TIMETRACKER
273  fawkes::TimeTracker *tt_;
274  unsigned int tt_loopcount_;
275  unsigned int ttc_retrieve_;
276  unsigned int ttc_retrieval_;
277  unsigned int ttc_transforms_;
278 #endif
279 };
280 
281 #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:91
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:36
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
Time tracking utility.
Definition: tracker.h:38
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:68
Interface for configuration handling.
Definition: config.h:67
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:34