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 <pcl/point_cloud.h>
40 #include <pcl/point_types.h>
41 
42 #include <mongocxx/client.hpp>
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>
54 class PointCloudDBRetrievePipeline : public PointCloudDBPipeline<PointType>
55 {
56 public:
57  /** Constructor.
58  * @param mongodb_client MongoDB client
59  * @param config configuration
60  * @param logger Logger
61  * @param transformer TF transformer for point cloud transformations between
62  * coordinate reference frames
63  * @param original input point cloud
64  * @param output output point cloud
65  */
66  PointCloudDBRetrievePipeline(mongocxx::client * mongodb_client,
67  fawkes::Configuration * config,
68  fawkes::Logger * logger,
69  fawkes::tf::Transformer *transformer,
72  : PointCloudDBPipeline<PointType>(mongodb_client, config, logger, output),
73  tf_(transformer),
74  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 timestamp,
108  std::string &database,
109  std::string &collection,
110  std::string &target_frame,
111  long & actual_time)
112  {
113  TIMETRACK_START(ttc_retrieve_);
114 
115  this->output_->points.clear();
116  this->output_->height = 1;
117  this->output_->width = 0;
118  this->output_->is_dense = false;
119 
120  std::vector<long> times(1, timestamp);
121  std::vector<long> actual_times(1, 0);
122  std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> pcls(1);
123 
124  TIMETRACK_START(ttc_retrieval_);
125 
126  pcls =
127  PointCloudDBPipeline<PointType>::retrieve_clouds(times, actual_times, database, collection);
128  if (pcls.empty()) {
129  this->logger_->log_warn(this->name_, "No point clouds found for desired timestamp");
130  TIMETRACK_ABORT(ttc_retrieval_);
131  TIMETRACK_ABORT(ttc_retrieve_);
132  return;
133  }
134 
135  copy_output(pcls[0], original_, 128, 128, 128);
136  actual_time = actual_times[0];
137 
138  if (target_frame == "SENSOR") {
139  target_frame = cfg_sensor_frame_;
140  }
141 
142  if (target_frame != "") {
143  // perform transformation
144 
145  TIMETRACK_INTER(ttc_retrieval_, ttc_transforms_);
146 
147  // retrieve transforms
148  fawkes::tf::MongoDBTransformer transformer(this->mongodb_client_, database);
149 
150  transformer.restore(/* start */ actual_times[0] + this->cfg_transform_range_[0],
151  /* end */ actual_times[0] + this->cfg_transform_range_[1]);
152  this->logger_->log_debug(this->name_,
153  "Restored transforms for %zu frames for range (%li..%li)",
154  transformer.get_frame_caches().size(),
155  /* start */ actual_times[0] + this->cfg_transform_range_[0],
156  /* end */ actual_times[0] + this->cfg_transform_range_[1]);
157 
158  fawkes::Time source_time;
159  fawkes::pcl_utils::get_time(pcls[0], source_time);
160  fawkes::tf::StampedTransform transform_recorded;
161  transformer.lookup_transform(cfg_fixed_frame_,
162  pcls[0]->header.frame_id,
163  source_time,
164  transform_recorded);
165 
166  fawkes::tf::StampedTransform transform_current;
167  tf_->lookup_transform(cfg_sensor_frame_, cfg_fixed_frame_, transform_current);
168 
169  fawkes::tf::Transform transform = transform_current * transform_recorded;
170 
171  try {
172  fawkes::pcl_utils::transform_pointcloud(*pcls[0], transform);
173  } catch (fawkes::Exception &e) {
174  this->logger_->log_warn(this->name_, "Failed to transform point cloud, exception follows");
175  this->logger_->log_warn(this->name_, e);
176  }
177 
178  // retrieve point clouds
179  TIMETRACK_END(ttc_transforms_);
180  }
181 
182  copy_output(pcls[0], this->output_);
183 
184  TIMETRACK_END(ttc_retrieve_);
185 
186 #ifdef USE_TIMETRACKER
187  //if (++tt_loopcount_ >= 5) {
188  // tt_loopcount_ = 0;
189  tt_->print_to_stdout();
190  //}
191 #endif
192  }
193 
194 private: //methods
195  void
198  {
199  size_t num_points = in->points.size();
200  out->header.frame_id = in->header.frame_id;
201  out->points.resize(num_points);
202  out->height = 1;
203  out->width = num_points;
204 
205  for (size_t p = 0; p < num_points; ++p) {
206  const PointType & ip = in->points[p];
207  typename PointCloudDBPipeline<PointType>::ColorPointType &op = out->points[p];
208 
209  op.x = ip.x;
210  op.y = ip.y;
211  op.z = ip.z;
212 
213  op.r = ip.r;
214  op.g = ip.g;
215  op.b = ip.b;
216  }
217  }
218 
219  void
222  const int r,
223  const int g,
224  const int b)
225  {
226  size_t num_points = in->points.size();
227  out->header.frame_id = in->header.frame_id;
228  out->points.resize(num_points);
229  out->height = 1;
230  out->width = num_points;
231 
232  for (size_t p = 0; p < num_points; ++p) {
233  const PointType & ip = in->points[p];
234  typename PointCloudDBPipeline<PointType>::ColorPointType &op = out->points[p];
235 
236  op.x = ip.x;
237  op.y = ip.y;
238  op.z = ip.z;
239 
240  op.r = r;
241  op.g = g;
242  op.b = b;
243  }
244  }
245 
246  void
247  copy_output(pcl::PointCloud<pcl::PointXYZ>::Ptr & in,
249  const int r = 255,
250  const int g = 255,
251  const int b = 255)
252  {
253  size_t num_points = in->points.size();
254  out->header.frame_id = in->header.frame_id;
255  out->points.resize(num_points);
256  out->height = 1;
257  out->width = num_points;
258 
259  for (size_t p = 0; p < num_points; ++p) {
260  const PointType & ip = in->points[p];
261  typename PointCloudDBPipeline<PointType>::ColorPointType &op = out->points[p];
262 
263  op.x = ip.x;
264  op.y = ip.y;
265  op.z = ip.z;
266 
267  op.r = r;
268  op.g = g;
269  op.b = b;
270  }
271  }
272 
273 private: // members
274  std::string cfg_fixed_frame_;
275  std::string cfg_sensor_frame_;
276 
278 
280 
281 #ifdef USE_TIMETRACKER
282  fawkes::TimeTracker *tt_;
283  unsigned int tt_loopcount_;
284  unsigned int ttc_retrieve_;
285  unsigned int ttc_retrieval_;
286  unsigned int ttc_transforms_;
287 #endif
288 };
289 
290 #endif
fawkes::tf::StampedTransform
Transform that contains a timestamp and frame IDs.
Definition: types.h:98
PointCloudDBPipeline
Database point cloud pipeline base class.
Definition: pcl_db_pipeline.h:90
fawkes::tf::MongoDBTransformer
Definition: mongodb_tf_transformer.h:42
PointCloudDBRetrievePipeline::~PointCloudDBRetrievePipeline
virtual ~PointCloudDBRetrievePipeline()
Destructor.
Definition: pcl_db_retrieve_pipeline.h:90
PointCloudDBPipeline::output_
ColorCloudPtr output_
The final (colored) output of the pipeline.
Definition: pcl_db_pipeline.h:331
fawkes::tf::Transformer
Definition: transformer.h:71
fawkes::Configuration
Definition: config.h:68
fawkes::tf::MongoDBTransformer::restore
void restore(fawkes::Time &start, fawkes::Time &end)
Restore transforms from database.
Definition: mongodb_tf_transformer.h:59
PointCloudDBPipeline::logger_
fawkes::Logger * logger_
Logger for informative messages.
Definition: pcl_db_pipeline.h:329
PointCloudDBRetrievePipeline::PointCloudDBRetrievePipeline
PointCloudDBRetrievePipeline(mongocxx::client *mongodb_client, fawkes::Configuration *config, fawkes::Logger *logger, fawkes::tf::Transformer *transformer, typename PointCloudDBPipeline< PointType >::ColorCloudPtr original, typename PointCloudDBPipeline< PointType >::ColorCloudPtr output)
Constructor.
Definition: pcl_db_retrieve_pipeline.h:65
PointCloudDBRetrievePipeline
Point cloud retrieve pipeline.
Definition: pcl_db_retrieve_pipeline.h:53
fawkes::Logger
Definition: logger.h:40
PointCloudDBPipeline::ColorPointType
pcl::PointXYZRGB ColorPointType
Colored point type.
Definition: pcl_db_pipeline.h:97
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:267
PointCloudDBPipeline::mongodb_client_
mongocxx::client * mongodb_client_
MongoDB client to retrieve data.
Definition: pcl_db_pipeline.h:327
fawkes::tf::Transformer::get_frame_caches
std::vector< TimeCacheInterfacePtr > get_frame_caches() const
Get all currently existing caches.
Definition: transformer.cpp:194
pcl::PointCloud
Definition: pointcloud.h:36
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
PointCloudDBPipeline::name_
const char * name_
Name of the pipeline.
Definition: pcl_db_pipeline.h:322
fawkes::tf::Transformer::lookup_transform
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
Definition: transformer.cpp:272
fawkes::Time
Definition: time.h:96
PointCloudDBPipeline::cfg_transform_range_
long cfg_transform_range_[2]
Transform range start and end times.
Definition: pcl_db_pipeline.h:325
fawkes::TimeTracker
Definition: tracker.h:40
PointCloudDBPipeline::ColorCloudPtr
ColorCloud::Ptr ColorCloudPtr
Shared pointer to colored cloud.
Definition: pcl_db_pipeline.h:106
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
PointCloudDBRetrievePipeline::retrieve
void retrieve(long timestamp, std::string &database, std::string &collection, std::string &target_frame, long &actual_time)
Retrieve point clouds.
Definition: pcl_db_retrieve_pipeline.h:106
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::Exception
Definition: exception.h:39