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
70static 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
83typedef 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 */
90template <typename PointType>
92{
93protected:
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
111public:
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
232protected: // 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
322protected: // 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 */
339inline const char *
340to_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
Database point cloud pipeline base class.
pcl::PointCloud< ColorPointType > ColorCloud
Type for colored point clouds based on ColorPointType.
long cfg_transform_range_[2]
Transform range start and end times.
fawkes::Logger * logger_
Logger for informative messages.
ColorCloud::Ptr ColorCloudPtr
Shared pointer to colored cloud.
ColorCloudPtr output_
The final (colored) output of the pipeline.
pcl::PointXYZRGB ColorPointType
Colored point type.
mongocxx::client * mongodb_client_
MongoDB client to retrieve data.
long cfg_pcl_age_tolerance_
Age tolerance for retrieved point clouds.
Cloud::Ptr CloudPtr
Shared pointer to cloud.
PointCloudDBPipeline(mongocxx::client *mongodb_client, fawkes::Configuration *config, fawkes::Logger *logger, ColorCloudPtr output)
Constructor.
pcl::PointCloud< PointType > Cloud
Basic point cloud type.
ApplicabilityStatus applicable(std::vector< long long > &times, std::string &database, std::string &collection)
Check if this pipeline instance is suitable for the given times.
ColorCloud::ConstPtr ColorCloudConstPtr
Shared pointer to constant colored cloud.
Cloud::ConstPtr CloudConstPtr
Shared pointer to constant cloud.
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.
void read_gridfs_file(void *dataptr, std::string &database, bsoncxx::types::value file_id)
Read a file from MongoDB GridFS.
virtual ~PointCloudDBPipeline()
Destructor.
const char * name_
Name of the pipeline.
Interface for configuration handling.
Definition: config.h:68
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::vector< float > get_floats(const char *path)=0
Get list of values from configuration which is of type float.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Interface for logging.
Definition: logger.h:42
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
A class for handling time.
Definition: time.h:93