Statistics
| Branch: | Revision:

root / rgbdslam / src / node.cpp @ 9240aaa3

History | View | Annotate | Download (39.4 KB)

1
/* This file is part of RGBDSLAM.
2
 * 
3
 * RGBDSLAM is free software: you can redistribute it and/or modify
4
 * it under the terms of the GNU General Public License as published by
5
 * the Free Software Foundation, either version 3 of the License, or
6
 * (at your option) any later version.
7
 * 
8
 * RGBDSLAM is distributed in the hope that it will be useful,
9
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11
 * GNU General Public License for more details.
12
 * 
13
 * You should have received a copy of the GNU General Public License
14
 * along with RGBDSLAM.  If not, see <http://www.gnu.org/licenses/>.
15
 */
16

    
17

    
18
#include "node.h"
19
#include <cmath>
20
#include <ctime>
21
#include <Eigen/Geometry>
22
#include "pcl/ros/conversions.h"
23
#include <pcl/common/transformation_from_correspondences.h>
24
//#include <opencv2/highgui/highgui.hpp>
25
#include <qtconcurrentrun.h>
26
#include <QtConcurrentMap> 
27

    
28
#ifdef USE_SIFT_GPU
29
#include "sift_gpu_feature_detector.h"
30
#endif
31

    
32
//#include <math.h>
33
#include <fstream>
34
#ifdef USE_ICP_BIN
35
#include "gicp-fallback.h"
36
#endif
37

    
38
#ifdef USE_ICP_CODE
39
#include "../gicp/transform.h"
40
#endif
41

    
42
//#include <iostream>
43
#include <Eigen/StdVector>
44
#include "misc.h"
45
#include <pcl/filters/voxel_grid.h>
46

    
47
Node::Node(const cv::Mat visual,
48
           cv::Ptr<cv::FeatureDetector> detector,
49
           cv::Ptr<cv::DescriptorExtractor> extractor,
50
           pointcloud_type::Ptr point_cloud,
51
           const cv::Mat detection_mask)
52
: id_(0), 
53
  flannIndex(NULL),
54
  base2points_(tf::Transform::getIdentity(), point_cloud->header.stamp,ParameterServer::instance()->get<std::string>("base_frame_name"), point_cloud->header.frame_id),
55
  ground_truth_transform_(tf::Transform::getIdentity(), point_cloud->header.stamp, ParameterServer::instance()->get<std::string>("ground_truth_frame_name"), ParameterServer::instance()->get<std::string>("base_frame_name")),
56
  initial_node_matches_(0)
57
{
58
#ifdef USE_ICP_CODE
59
  gicp_initialized = false;
60
#endif
61
  struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
62

    
63
#ifdef USE_SIFT_GPU
64
  float* descriptors = NULL;
65
  if(ParameterServer::instance()->get<std::string>("feature_detector_type") == "SIFTGPU"){
66
    SiftGPUFeatureDetector* siftgpu = SiftGPUFeatureDetector::GetInstance();
67
    descriptors = siftgpu->detect(visual, feature_locations_2d_);
68
    if (descriptors == NULL) {
69
      ROS_FATAL("Can't run SiftGPU");
70
    }
71
    clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", "Feature Detection and Descriptor Extraction runtime: "<< elapsed <<" s");
72
  } else {
73
#endif
74
    ROS_FATAL_COND(detector.empty(), "No valid detector!");
75
    detector->detect( visual, feature_locations_2d_, detection_mask);// fill 2d locations
76
    clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", "Feature Detection runtime: "<< elapsed <<" s");
77
#ifdef USE_SIFT_GPU
78
  }
79
#endif
80

    
81

    
82
  // get pcl::Pointcloud to extract depthValues a pixel positions
83
  struct timespec starttime5; clock_gettime(CLOCK_MONOTONIC, &starttime5);
84
  // TODO: If batch sending/saving of clouds would be removed, the pointcloud wouldn't have to be saved
85
  // which would slim down the Memory requirements
86
  //pcl::fromROSMsg(*point_cloud,(*pc_col));
87
  pc_col = point_cloud;
88
  clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime5.tv_sec); elapsed += (finish.tv_nsec - starttime5.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
89

    
90
  // project pixels to 3dPositions and create search structures for the gicp
91
#ifdef USE_SIFT_GPU
92
  if(ParameterServer::instance()->get<std::string>("feature_detector_type") == "SIFTGPU"){
93
    // removes also unused descriptors from the descriptors matrix
94
    // build descriptor matrix
95
    projectTo3DSiftGPU(feature_locations_2d_, feature_locations_3d_, pc_col, descriptors, feature_descriptors_); //takes less than 0.01 sec
96

    
97
    if (descriptors != NULL) delete descriptors;
98
  }else{
99
#endif
100
    projectTo3D(feature_locations_2d_, feature_locations_3d_, pc_col); //takes less than 0.01 sec
101
    // projectTo3d need a dense cloud to use the points.at(px.x,px.y)-Call
102
    struct timespec starttime2; clock_gettime(CLOCK_MONOTONIC, &starttime2);
103
    extractor->compute(visual, feature_locations_2d_, feature_descriptors_); //fill feature_descriptors_ with information 
104
    clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime2.tv_sec); elapsed += (finish.tv_nsec - starttime2.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", "Feature Extraction runtime: "<< elapsed <<" s");
105
#ifdef USE_SIFT_GPU
106
  }
107
#endif
108
  assert(feature_locations_2d_.size() == feature_locations_3d_.size());
109
  ROS_INFO_NAMED("statistics", "Feature Count of Node:\t%d", (int)feature_locations_2d_.size());
110
  size_t max_keyp = ParameterServer::instance()->get<int>("max_keypoints");
111
  if(feature_locations_2d_.size() > max_keyp) {
112
    feature_locations_2d_.resize(max_keyp);
113
    feature_locations_3d_.resize(max_keyp);
114
    feature_descriptors_ = feature_descriptors_.rowRange(0,max_keyp-1);
115
  }
116

    
117
#ifdef USE_ICP_CODE
118
  struct timespec starttime4; clock_gettime(CLOCK_MONOTONIC, &starttime4);
119
  createGICPStructures(); 
120
  clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime4.tv_sec); elapsed += (finish.tv_nsec - starttime4.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
121
#endif
122

    
123
  if((!ParameterServer::instance()->get<bool>("use_glwidget") ||
124
      !ParameterServer::instance()->get<bool>("use_gui")) &&
125
     !ParameterServer::instance()->get<bool>("store_pointclouds"))
126
  {
127
    ROS_WARN("Clearing out points");
128
    this->clearPointCloud();
129
  } else if(ParameterServer::instance()->get<double>("voxelfilter_size") > 0.0) {
130
    double vfs = ParameterServer::instance()->get<double>("voxelfilter_size");
131
    pcl::VoxelGrid<point_type> sor;
132
    sor.setLeafSize(vfs,vfs,vfs);
133
    pointcloud_type::ConstPtr const_cloud_ptr = boost::make_shared<pointcloud_type> (*pc_col);                                                                 
134
    sor.setInputCloud (const_cloud_ptr);
135
    sor.filter (*pc_col);
136
  }
137
  clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
138

    
139
}
140

    
141
Node::~Node() {
142
    if (ParameterServer::instance()->get<std::string> ("feature_detector_type").compare("ORB") != 0) {
143
        if (flannIndex)
144
            delete flannIndex;
145
    }
146
}
147

    
148
void Node::setGroundTruthTransform(tf::StampedTransform gt){
149
    ground_truth_transform_ = gt;
150
}
151
void Node::setBase2PointsTransform(tf::StampedTransform b2p){
152
    base2points_ = b2p;
153
}
154
tf::StampedTransform Node::getGroundTruthTransform(){
155
    return ground_truth_transform_;
156
}
157
tf::StampedTransform Node::getBase2PointsTransform(){
158
    return base2points_;
159
}
160

    
161
void Node::publish(const char* frame, ros::Time timestamp, ros::Publisher pub){
162
    sensor_msgs::PointCloud2 cloudMessage;
163
    pcl::toROSMsg((*pc_col),cloudMessage);
164
    cloudMessage.header.frame_id = frame;
165
    cloudMessage.header.stamp = timestamp;
166
    pub.publish(cloudMessage);
167
    ROS_INFO("Pointcloud with id %i sent with frame %s", id_, frame);
168
}
169

    
170
#ifdef USE_ICP_CODE
171
bool Node::getRelativeTransformationTo_ICP_code(const Node* target_node,Eigen::Matrix4f& transformation,
172
    const Eigen::Matrix4f* initial_transformation){
173
  //std::clock_t starttime_icp = std::clock();
174
  dgc_transform_t initial;
175

    
176
  // use optional matrix as first guess in icp
177
  if (initial_transformation == NULL){
178
    gicpSetIdentity(initial); 
179
  }else {
180
    Eigen2GICP(*initial_transformation,initial);
181
  }
182

    
183

    
184
  dgc_transform_t final_trafo;
185
  dgc_transform_identity(final_trafo);
186

    
187

    
188
  assert(gicp_initialized && target_node->gicp_initialized);
189

    
190
  unsigned int iterations = target_node->gicp_point_set->AlignScan(this->gicp_point_set, initial, final_trafo, gicp_d_max);
191

    
192

    
193
  GICP2Eigen(final_trafo,transformation);
194

    
195
  //clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
196

    
197
  return iterations < gicp_max_iterations;
198

    
199
}
200

    
201
# endif
202

    
203
#ifdef USE_ICP_BIN
204
bool Node::getRelativeTransformationTo_ICP_bin(const Node* target_node,
205
    Eigen::Matrix4f& transformation,
206
    const Eigen::Matrix4f* initial_transformation){
207
  std::clock_t starttime_icp = std::clock();
208

    
209
  bool converged;
210

    
211
  if (initial_transformation != NULL)
212
  {
213
    pointcloud_type pc2;
214
    pcl::transformPointCloud((*pc_col),pc2,*initial_transformation);
215
    converged = gicpfallback(pc2,target_node->(*pc_col), transformation);
216
  }
217
  else {
218
    converged = gicpfallback((*pc_col),target_node->(*pc_col), transformation); }
219

    
220
  // Paper
221
  // clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
222

    
223
  return converged;
224
}
225
#endif
226

    
227
// build search structure for descriptor matching
228
void Node::buildFlannIndex() {
229
  if (ParameterServer::instance()->get<bool> ("use_flann") && 
230
      ParameterServer::instance()->get<std::string> ("feature_detector_type").compare("ORB") != 0) 
231
  {
232
    struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
233
    //KDTreeIndexParams When passing an object of this type the index constructed will 
234
    //consist of a set of randomized kd-trees which will be searched in parallel.
235
    flannIndex = new cv::flann::Index(feature_descriptors_, cv::flann::KDTreeIndexParams(16));
236
    ROS_DEBUG("Built flannIndex (address %p) for Node %i", flannIndex, this->id_);
237
    clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
238
  }
239
}
240

    
241

    
242
#ifdef USE_ICP_CODE
243
void Node::createGICPStructures(unsigned int max_count){
244

    
245
  gicp_point_set = new dgc::gicp::GICPPointSet();
246

    
247
  dgc::gicp::GICPPoint g_p;
248
  g_p.range = -1;
249
  for(int k = 0; k < 3; k++) {
250
    for(int l = 0; l < 3; l++) {
251
      g_p.C[k][l] = (k == l)?1:0;
252
    }
253
  }
254

    
255
  int step = 1;
256
  if ((*pc_col).points.size()>max_count)
257
    step = ceil((*pc_col).points.size()*1.0/max_count);
258

    
259
  int cnt = 0;
260
  for (unsigned int i=0; i<(*pc_col).points.size(); i++ ){
261
    point_type  p = (*pc_col).points.at(i);
262
    if (!(isnan(p.x) || isnan(p.y) || isnan(p.z))) {
263
      // add points to pointset for icp
264
      if (cnt++%step == 0){
265
        g_p.x=p.x;
266
        g_p.y=p.y;
267
        g_p.z=p.z;
268
        gicp_point_set->AppendPoint(g_p);
269
      }
270
    }
271
  }
272
  ROS_WARN("gicp_point_set.Size() %i", gicp_point_set->Size() );
273

    
274

    
275
  std::clock_t starttime_gicp = std::clock();
276
  // build search structure for gicp:
277
  gicp_point_set->SetDebug(false);
278
  gicp_point_set->SetGICPEpsilon(gicp_epsilon);
279
  gicp_point_set->BuildKDTree();
280
  gicp_point_set->ComputeMatrices();
281
  gicp_point_set->SetMaxIterationInner(8); // as in test_gicp->cpp
282
  gicp_point_set->SetMaxIteration(gicp_max_iterations);
283
  clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
284

    
285
  ROS_INFO_STREAM("time for creating the structure: " << ((std::clock()-starttime_gicp*1.0) / (double)CLOCKS_PER_SEC));
286
  ROS_INFO_STREAM("current: " << std::clock() << " " << "start_time: " << starttime_gicp);
287

    
288
  gicp_initialized = true;
289

    
290
}
291
#endif
292

    
293
//TODO: This function seems to be resistant to paralellization probably due to knnSearch
294
int Node::findPairsFlann(const Node* other, std::vector<cv::DMatch>* matches) const {
295
  struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
296
  assert(matches->size()==0);
297
  // number of neighbours found (two, to compare the best matches for distinctness
298
  const int k = 2;
299

    
300
  // number of neighbors found (has to be two, see l. 57)
301
  double sum_distances = 0.0;
302
  ParameterServer* ps = ParameterServer::instance();
303
  const int min_kp = ps->get<int> ("min_keypoints");
304
  //using BruteForceMatcher for ORB features
305
  if (!ps->get<bool> ("use_flann") ||
306
       ps->get<std::string> ("feature_detector_type") == "ORB")
307
  {
308

    
309
    cv::Ptr<cv::DescriptorMatcher> matcher;
310
    std::string brute_force_type("BruteForce"); //L2 per default
311
    if(ps->get<std::string> ("feature_detector_type") == "ORB"){
312
      brute_force_type.append("-HammingLUT");
313
    }
314
    matcher = cv::DescriptorMatcher::create(brute_force_type);
315
    std::vector< std::vector<cv::DMatch> > bruteForceMatches;
316
    matcher->knnMatch(feature_descriptors_, other->feature_descriptors_, bruteForceMatches, k);
317
    int dist_ratio_fac = 0.6;
318
    if ((int)bruteForceMatches.size() < min_kp) dist_ratio_fac = 1.0; //if necessary use possibly bad descriptors
319
    for (unsigned int i = 0; i < bruteForceMatches.size(); i++) {
320
        cv::DMatch m1 = bruteForceMatches[i][0];
321
        cv::DMatch m2 = bruteForceMatches[i][1];
322
        if (m1.distance < 0.6 * m2.distance) {//this check seems crucial to matching quality
323
            matches->push_back(m1);
324
            sum_distances += m1.distance;
325
        }
326
    }
327
    //matcher->match(feature_descriptors_, other->feature_descriptors_, *matches);
328
  } 
329
  else {
330
    if (other->flannIndex == NULL) {
331
        ROS_FATAL("Node %i in findPairsFlann: flann Index of Node %i was not initialized", this->id_, other->id_);
332
        return -1;
333
    }
334
    // compare
335
    // http://opencv-cocoa.googlecode.com/svn/trunk/samples/c/find_obj.cpp
336
    cv::Mat indices(feature_descriptors_.rows, k, CV_32S);
337
    cv::Mat dists(feature_descriptors_.rows, k, CV_32F);
338

    
339
    // get the best two neighbors
340
    other->flannIndex->knnSearch(feature_descriptors_, indices, dists, k, cv::flann::SearchParams(64));
341
    //64: The number of times the tree(s) in the index should be recursively traversed. A higher value for this parameter would give better search precision, but also take more time. If automatic configuration was used when the index was created, the number of checks required to achieve the specified precision was also computed, in which case this parameter is ignored.
342

    
343
    int* indices_ptr = indices.ptr<int> (0);
344
    float* dists_ptr = dists.ptr<float> (0);
345

    
346
    cv::DMatch match;
347
    int dist_ratio_fac = 0.6;
348
    if (indices.rows < min_kp) dist_ratio_fac = 1.0; //if necessary use possibly bad descriptors
349
    for (int i = 0; i < indices.rows; ++i) {
350
      if (dists_ptr[2 * i] < 0.6 * dists_ptr[2 * i + 1]) {
351
        match.queryIdx = i;
352
        match.trainIdx = indices_ptr[2 * i];
353
        match.distance = dists_ptr[2 * i];
354
        sum_distances += match.distance;
355

    
356
        assert(match.trainIdx < other->feature_descriptors_.rows);
357
        assert(match.queryIdx < feature_descriptors_.rows);
358

    
359
        matches->push_back(match);
360
      }
361
    }
362
  }
363

    
364
  ROS_INFO_NAMED("statistics", "count_matrix(%3d, %3d) =  %4d;",
365
                 this->id_+1, other->id_+1, (int)matches->size());
366
  ROS_INFO_NAMED("statistics", "dista_matrix(%3d, %3d) =  %f;",
367
                 this->id_+1, other->id_+1, sum_distances/ (float)matches->size());
368
  ROS_DEBUG_NAMED("statistics", "Feature Matches between Nodes %3d (%4d features) and %3d (%4d features):\t%4d",
369
                  this->id_, (int)this->feature_locations_2d_.size(),
370
                  other->id_, (int)other->feature_locations_2d_.size(),
371
                  (int)matches->size());
372

    
373
  //ROS_INFO("matches size: %i, rows: %i", (int) matches->size(), feature_descriptors_.rows);
374

    
375
  clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
376
  return matches->size();
377
}
378

    
379

    
380

    
381
#ifdef USE_SIFT_GPU
382
void Node::projectTo3DSiftGPU(std::vector<cv::KeyPoint>& feature_locations_2d,
383
    std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
384
    const pointcloud_type::Ptr point_cloud, float* descriptors_in, cv::Mat& descriptors_out){
385

    
386
  struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
387

    
388
  cv::Point2f p2d;
389

    
390
  if(feature_locations_3d.size()){
391
    ROS_INFO("There is already 3D Information in the FrameInfo, clearing it");
392
    feature_locations_3d.clear();
393
  }
394

    
395
  std::list<int> featuresUsed;
396

    
397
  int index = -1;
398
  for(unsigned int i = 0; i < feature_locations_2d.size(); /*increment at end of loop*/){
399
    ++index;
400

    
401
    p2d = feature_locations_2d[i].pt;
402
    if (p2d.x >= point_cloud->width || p2d.x < 0 ||
403
        p2d.y >= point_cloud->height || p2d.y < 0 ||
404
        std::isnan(p2d.x) || std::isnan(p2d.y)){ //TODO: Unclear why points should be outside the image or be NaN
405
      ROS_WARN_STREAM("Ignoring invalid keypoint: " << p2d); //Does it happen at all? If not, remove this code block
406
      feature_locations_2d.erase(feature_locations_2d.begin()+i);
407
      continue;
408
    }
409

    
410
    point_type p3d = point_cloud->at((int) p2d.x,(int) p2d.y);
411

    
412
    if ( isnan(p3d.x) || isnan(p3d.y) || isnan(p3d.z)){
413
      feature_locations_2d.erase(feature_locations_2d.begin()+i);
414
      continue;
415
    }
416

    
417
    featuresUsed.push_back(index);  //save id for constructing the descriptor matrix
418
    feature_locations_3d.push_back(Eigen::Vector4f(p3d.x, p3d.y, p3d.z, 1.0));
419
    i++; //Only increment if no element is removed from vector
420
  }
421

    
422
  //create descriptor matrix
423
  int size = feature_locations_3d.size();
424
  descriptors_out = cv::Mat(size, 128, CV_32F);
425
  for (int y = 0; y < size && featuresUsed.size() > 0; ++y) {
426
    int id = featuresUsed.front();
427
    featuresUsed.pop_front();
428

    
429
    for (int x = 0; x < 128; ++x) {
430
      descriptors_out.at<float>(y, x) = descriptors_in[id * 128 + x];
431
    }
432
  }
433

    
434
  clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
435
}
436
#endif
437

    
438
void Node::projectTo3D(std::vector<cv::KeyPoint>& feature_locations_2d,
439
    std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
440
    pointcloud_type::ConstPtr point_cloud){
441

    
442
  struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
443

    
444
  cv::Point2f p2d;
445

    
446
  if(feature_locations_3d.size()){
447
    ROS_INFO("There is already 3D Information in the FrameInfo, clearing it");
448
    feature_locations_3d.clear();
449
  }
450

    
451
  for(unsigned int i = 0; i < feature_locations_2d.size(); /*increment at end of loop*/){
452
    p2d = feature_locations_2d[i].pt;
453
    if (p2d.x >= point_cloud->width || p2d.x < 0 ||
454
        p2d.y >= point_cloud->height || p2d.y < 0 ||
455
        std::isnan(p2d.x) || std::isnan(p2d.y)){ //TODO: Unclear why points should be outside the image or be NaN
456
      ROS_WARN_STREAM("Ignoring invalid keypoint: " << p2d); //Does it happen at all? If not, remove this code block
457
      feature_locations_2d.erase(feature_locations_2d.begin()+i);
458
      continue;
459
    }
460

    
461
    point_type p3d = point_cloud->at((int) p2d.x,(int) p2d.y);
462

    
463
    //ROS_INFO("3d: %f, %f, %f, 2d: %f, %f", p3d.x, p3d.y, p3d.z, p2d.x, p2d.y);
464

    
465
    if ( isnan(p3d.x) || isnan(p3d.y) || isnan(p3d.z)){
466
      feature_locations_2d.erase(feature_locations_2d.begin()+i);
467
      continue;
468
    }
469

    
470
    feature_locations_3d.push_back(Eigen::Vector4f(p3d.x, p3d.y, p3d.z, 1.0));
471
    i++; //Only increment if no element is removed from vector
472
  }
473

    
474
  clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
475
}
476

    
477

    
478

    
479
void Node::computeInliersAndError(const std::vector<cv::DMatch>& matches,
480
                                  const Eigen::Matrix4f& transformation,
481
                                  const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& origins,
482
                                  const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& earlier,
483
                                  std::vector<cv::DMatch>& inliers, //output var
484
                                  double& mean_error,
485
                                  std::vector<double>& errors,
486
                                  double squaredMaxInlierDistInM) const{ //output var
487

    
488
  struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
489

    
490
  inliers.clear();
491
  errors.clear();
492

    
493
  std::vector<std::pair<float,int> > dists;
494
  std::vector<cv::DMatch> inliers_temp;
495

    
496
  assert(matches.size() > 0);
497
  mean_error = 0.0;
498
  for (unsigned int j = 0; j < matches.size(); j++){ //compute new error and inliers
499

    
500
    unsigned int this_id = matches[j].queryIdx;
501
    unsigned int earlier_id = matches[j].trainIdx;
502

    
503
    Eigen::Vector4f vec = (transformation * origins[this_id]) - earlier[earlier_id];
504

    
505
    double error = vec.dot(vec);
506

    
507
    if(error > squaredMaxInlierDistInM)
508
      continue; //ignore outliers
509
    if(!(error >= 0.0)){
510
      ROS_DEBUG_STREAM("Transformation for error !> 0: " << transformation);
511
      ROS_DEBUG_STREAM(error << " " << matches.size());
512
    }
513
    error = sqrt(error);
514
    dists.push_back(std::pair<float,int>(error,j));
515
    inliers_temp.push_back(matches[j]); //include inlier
516

    
517
    mean_error += error;
518
    errors.push_back(error);
519
  }
520

    
521
  if (inliers_temp.size()<3){ //at least the samples should be inliers
522
    ROS_WARN_COND(inliers_temp.size() > 3, "No inliers at all in %d matches!", (int)matches.size()); // only warn if this checks for all initial matches
523
    mean_error = 1e9;
524
  } else {
525
    mean_error /= inliers_temp.size();
526

    
527
    // sort inlier ascending according to their error
528
    sort(dists.begin(),dists.end());
529

    
530
    inliers.resize(inliers_temp.size());
531
    for (unsigned int i=0; i<inliers_temp.size(); i++){
532
      inliers[i] = matches[dists[i].second];
533
    }
534
  }
535
  if(!(mean_error>0)) ROS_DEBUG_STREAM("Transformation for mean error !> 0: " << transformation);
536
  if(!(mean_error>0)) ROS_DEBUG_STREAM(mean_error << " " << inliers_temp.size());
537
  clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
538

    
539
}
540

    
541
template<class InputIterator>
542
Eigen::Matrix4f Node::getTransformFromMatches(const Node* earlier_node,
543
                                              InputIterator iter_begin,
544
                                              InputIterator iter_end,
545
                                              bool& valid, 
546
                                              float max_dist_m) const 
547
{
548
  pcl::TransformationFromCorrespondences tfc;
549
  valid = true;
550
  std::vector<Eigen::Vector3f> t, f;
551

    
552
  for ( ;iter_begin!=iter_end; iter_begin++) {
553
    int this_id    = iter_begin->queryIdx;
554
    int earlier_id = iter_begin->trainIdx;
555

    
556
    Eigen::Vector3f from(this->feature_locations_3d_[this_id][0],
557
                         this->feature_locations_3d_[this_id][1],
558
                         this->feature_locations_3d_[this_id][2]);
559
    Eigen::Vector3f  to (earlier_node->feature_locations_3d_[earlier_id][0],
560
                         earlier_node->feature_locations_3d_[earlier_id][1],
561
                         earlier_node->feature_locations_3d_[earlier_id][2]);
562
    if (max_dist_m > 0) {  //storing is only necessary, if max_dist is given
563
      f.push_back(from);
564
      t.push_back(to);    
565
    }
566
    tfc.add(from, to, 1.0/to(2));//the further, the less weight b/c of accuracy decay
567
  }
568

    
569

    
570
  // find smalles distance between a point and its neighbour in the same cloud
571
  // je groesser das dreieck aufgespannt ist, desto weniger fallen kleine positionsfehler der einzelnen
572
  // Punkte ist Gewicht!
573

    
574
  if (max_dist_m > 0)
575
  {  
576
    //float min_neighbour_dist = 1e6;
577
    Eigen::Matrix4f foo;
578

    
579
    valid = true;
580
    for (uint i=0; i<f.size(); i++)
581
    {
582
      float d_f = (f.at((i+1)%f.size())-f.at(i)).norm();
583
      float d_t = (t.at((i+1)%t.size())-t.at(i)).norm();
584

    
585
      if ( abs(d_f-d_t) > max_dist_m ) {
586
        valid = false;
587
        return Eigen::Matrix4f();
588
      }
589
    }
590
    //here one could signal that some samples are very close, but as the transformation is validated elsewhere we don't
591
    //if (min_neighbour_dist < 0.5) { ROS_INFO...}
592
  }
593
  // get relative movement from samples
594
  return tfc.getTransformation().matrix();
595
}
596

    
597

    
598
///Find transformation with largest support, RANSAC style.
599
///Return false if no transformation can be found
600
bool Node::getRelativeTransformationTo(const Node* earlier_node,
601
                                       std::vector<cv::DMatch>* initial_matches,
602
                                       Eigen::Matrix4f& resulting_transformation,
603
                                       float& rmse, 
604
                                       std::vector<cv::DMatch>& matches) const
605
{
606
  struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
607

    
608
  assert(initial_matches != NULL);
609
  matches.clear();
610
  
611
  if(initial_matches->size() <= (unsigned int) ParameterServer::instance()->get<int>("min_matches")){
612
    ROS_INFO("Only %d feature matches between %d and %d (minimal: %i)",(int)initial_matches->size() , this->id_, earlier_node->id_, ParameterServer::instance()->get<int>("min_matches"));
613
    return false;
614
  }
615

    
616
  //unsigned int min_inlier_threshold = int(initial_matches->size()*0.2);
617
  unsigned int min_inlier_threshold = (unsigned int) ParameterServer::instance()->get<int>("min_matches");
618
  std::vector<cv::DMatch> inlier; //holds those feature correspondences that support the transformation
619
  double inlier_error; //all squared errors
620
  srand((long)std::clock());
621
  
622
  // a point is an inlier if it's no more than max_dist_m m from its partner apart
623
  const float max_dist_m = ParameterServer::instance()->get<double>("max_dist_for_inliers");
624
  const int ransac_iterations = ParameterServer::instance()->get<int>("ransac_iterations");
625
  std::vector<double> dummy;
626

    
627
  // best values of all iterations (including invalids)
628
  double best_error = 1e6, best_error_invalid = 1e6;
629
  unsigned int best_inlier_invalid = 0, best_inlier_cnt = 0, valid_iterations = 0;
630

    
631
  Eigen::Matrix4f transformation;
632
  
633
  const unsigned int sample_size = 3;// chose this many randomly from the correspondences:
634
  for (int n_iter = 0; n_iter < ransac_iterations; n_iter++) {
635
    //generate a map of samples. Using a map solves the problem of drawing a sample more than once
636
    std::set<cv::DMatch> sample_matches;
637
    std::vector<cv::DMatch> sample_matches_vector;
638
    while(sample_matches.size() < sample_size){
639
      int id = rand() % initial_matches->size();
640
      sample_matches.insert(initial_matches->at(id));
641
      sample_matches_vector.push_back(initial_matches->at(id));
642
    }
643

    
644
    bool valid; // valid is false iff the sampled points clearly aren't inliers themself 
645
    transformation = getTransformFromMatches(earlier_node, sample_matches.begin(), sample_matches.end(),valid,max_dist_m);
646
    if (!valid) continue; // valid is false iff the sampled points aren't inliers themself 
647
    if(transformation!=transformation) continue; //Contains NaN
648
    
649
    //test whether samples are inliers (more strict than before)
650
    computeInliersAndError(sample_matches_vector, transformation, this->feature_locations_3d_, 
651
                           earlier_node->feature_locations_3d_, inlier, inlier_error,  /*output*/
652
                           dummy, max_dist_m*max_dist_m); 
653
    ROS_DEBUG("Transformation from and for %u samples results in an error of %f and %i inliers.", sample_size, inlier_error, (int)inlier.size());
654
    if(inlier_error > 1000) continue; //most possibly a false match in the samples
655
    computeInliersAndError(*initial_matches, transformation, this->feature_locations_3d_, 
656
                           earlier_node->feature_locations_3d_, inlier, inlier_error,  /*output*/
657
                           dummy, max_dist_m*max_dist_m);
658
    ROS_DEBUG("Transformation from %u samples results in an error of %f and %i inliers for all matches (%i).", sample_size, inlier_error, (int)inlier.size(), (int)initial_matches->size());
659

    
660
    // check also invalid iterations
661
    if (inlier.size() > best_inlier_invalid) {
662
      best_inlier_invalid = inlier.size();
663
      best_error_invalid = inlier_error;
664
    }
665
    // ROS_INFO("iteration %d  cnt: %d, best: %d,  error: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100);
666

    
667
    if(inlier.size() < min_inlier_threshold || inlier_error > max_dist_m){
668
      ROS_DEBUG("Skipped iteration: inliers: %i (min %i), inlier_error: %.2f (max %.2f)", (int)inlier.size(), (int) min_inlier_threshold,  inlier_error*100, max_dist_m*100);
669
      continue;
670
    }
671
    // ROS_INFO("Refining iteration from %i samples: all matches: %i, inliers: %i, inlier_error: %f", (int)sample_size, (int)initial_matches->size(), (int)inlier.size(), inlier_error);
672
    valid_iterations++;
673
    //if (inlier_error > 0) ROS_ERROR("size: %i", (int)dummy.size());
674
    assert(inlier_error>=0);
675

    
676
    //Performance hacks:
677
    ///Iterations with more than half of the initial_matches inlying, count twice
678
    if (inlier.size() > initial_matches->size()*0.5) n_iter+=10;
679
    ///Iterations with more than 80% of the initial_matches inlying, count threefold
680
    if (inlier.size() > initial_matches->size()*0.8) n_iter+=20;
681

    
682

    
683

    
684
    if (inlier_error < best_error) { //copy this to the result
685
      resulting_transformation = transformation;
686
      matches = inlier;
687
      assert(matches.size()>= min_inlier_threshold);
688
      best_inlier_cnt = inlier.size();
689
      //assert(matches.size()>= ((float)initial_matches->size())*min_inlier_ratio);
690
      rmse = inlier_error;
691
      best_error = inlier_error;
692
      // ROS_INFO("  new best iteration %d  cnt: %d, best_inlier: %d,  error: %.4f, bestError: %.4f",n_iter, inlier.size(), best_inlier_cnt, inlier_error, best_error);
693

    
694
    }else
695
    {
696
      // ROS_INFO("NO new best iteration %d  cnt: %d, best_inlier: %d,  error: %.4f, bestError: %.4f",n_iter, inlier.size(), best_inlier_cnt, inlier_error, best_error);
697
    }
698

    
699
    //int max_ndx = min((int) min_inlier_threshold,30); //? What is this 30?
700
    double new_inlier_error;
701

    
702
    transformation = getTransformFromMatches(earlier_node, matches.begin(), matches.end(), valid); // compute new trafo from all inliers:
703
    if(transformation!=transformation) continue; //Contains NaN
704
    computeInliersAndError(*initial_matches, transformation,
705
                           this->feature_locations_3d_, earlier_node->feature_locations_3d_,
706
                           inlier, new_inlier_error, dummy, max_dist_m*max_dist_m);
707
    ROS_DEBUG("Refined Transformation from all matches (%i) results in an error of %f and %i inliers for all matches.", (int)initial_matches->size(), inlier_error, (int)inlier.size());
708
    // ROS_INFO("asd recomputed: inliersize: %i, inlier error: %f", (int) inlier.size(),100*new_inlier_error);
709

    
710

    
711
    // check also invalid iterations
712
    if (inlier.size() > best_inlier_invalid)
713
    {
714
      best_inlier_invalid = inlier.size();
715
      best_error_invalid = inlier_error;
716
    }
717

    
718
    if(inlier.size() < min_inlier_threshold || new_inlier_error > max_dist_m){
719
      //inlier.size() < ((float)initial_matches->size())*min_inlier_ratio || 
720
      // ROS_INFO("Skipped iteration: inliers: %i (min %i), inlier_error: %.2f (max %.2f)", (int)inlier.size(), (int) min_inlier_threshold,  inlier_error*100, max_dist_m*100);
721
      continue;
722
    }
723
    // ROS_INFO("Refined iteration from %i samples: all matches %i, inliers: %i, new_inlier_error: %f", (int)sample_size, (int)initial_matches->size(), (int)inlier.size(), new_inlier_error);
724

    
725
    assert(new_inlier_error>=0);
726

    
727
    if (new_inlier_error < best_error) 
728
    {
729
      resulting_transformation = transformation;
730
      matches = inlier;
731
      assert(matches.size()>= min_inlier_threshold);
732
      //assert(matches.size()>= ((float)initial_matches->size())*min_inlier_ratio);
733
      rmse = new_inlier_error;
734
      best_error = new_inlier_error;
735
      // ROS_INFO("  improved: new best iteration %d  cnt: %d, best_inlier: %d,  error: %.2f, bestError: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100, best_error*100);
736
    }else
737
    {
738
      // ROS_INFO("improved: NO new best iteration %d  cnt: %d, best_inlier: %d,  error: %.2f, bestError: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100, best_error*100);
739
    }
740
  } //iterations
741
  ROS_INFO("%i good iterations (from %i), inlier pct %i, inlier cnt: %i, error: %.2f cm",valid_iterations, ransac_iterations, (int) (matches.size()*1.0/initial_matches->size()*100),(int) matches.size(),rmse*100);
742
  // ROS_INFO("best overall: inlier: %i, error: %.2f",best_inlier_invalid, best_error_invalid*100);
743

    
744
  clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
745
  return matches.size() >= min_inlier_threshold;
746
}
747

    
748

    
749
#ifdef USE_ICP_CODE
750
void Node::Eigen2GICP(const Eigen::Matrix4f& m, dgc_transform_t g_m){
751
  for(int i=0;i<4; i++)
752
    for(int j=0;j<4; j++)
753
      g_m[i][j] = m(i,j);
754

    
755
}
756
void Node::GICP2Eigen(const dgc_transform_t g_m, Eigen::Matrix4f& m){
757
  for(int i=0;i<4; i++)
758
    for(int j=0;j<4; j++)
759
      m(i,j) = g_m[i][j];
760
}
761

    
762
void Node::gicpSetIdentity(dgc_transform_t m){
763
  for(int i=0;i<4; i++)
764
    for(int j=0;j<4; j++)
765
      if (i==j)
766
        m[i][j] = 1;
767
      else
768
        m[i][j] = 0;
769
}
770
#endif
771

    
772

    
773
//TODO: Merge this with processNodePair
774
MatchingResult Node::matchNodePair(const Node* older_node){
775
  MatchingResult mr;
776
  if(initial_node_matches_ > ParameterServer::instance()->get<int>("max_connections")) return mr; //enough is enough
777
  const unsigned int min_matches = (unsigned int) ParameterServer::instance()->get<int>("min_matches");// minimal number of feature correspondences to be a valid candidate for a link
778
  // struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
779

    
780
  this->findPairsFlann(older_node, &mr.all_matches); 
781
  ROS_DEBUG("found %i inital matches",(int) mr.all_matches.size());
782
  if (mr.all_matches.size() < min_matches){
783
    ROS_INFO("Too few inliers: Adding no Edge between %i and %i. Only %i correspondences to begin with.",
784
        older_node->id_,this->id_,(int)mr.all_matches.size());
785
  } 
786
  else if (!getRelativeTransformationTo(older_node,&mr.all_matches, mr.ransac_trafo, mr.rmse, mr.inlier_matches) ){ // mr.all_matches.size()/3
787
      ROS_INFO("Found no valid trafo, but had initially %d feature matches",(int) mr.all_matches.size());
788
  } else  {
789
      ++initial_node_matches_; //trafo is accepted
790
      mr.final_trafo = mr.ransac_trafo;
791
      
792
#ifdef USE_ICP_CODE
793
      getRelativeTransformationTo_ICP_code(older_node,mr.icp_trafo, &mr.ransac_trafo);
794
#endif  
795
      
796
#ifdef USE_ICP_BIN
797
      // improve transformation by using the generalized ICP
798
      // std::clock_t starttime_gicp1 = std::clock();
799
      bool converged = getRelativeTransformationTo_ICP_bin(older_node,mr.icp_trafo, &mr.ransac_trafo);
800
      //ROS_INFO_STREAM("Paper: ICP1: " << ((std::clock()-starttime_gicp1*1.0) / (double)CLOCKS_PER_SEC));
801

    
802

    
803
      ROS_INFO("icp: inliers: %i", mr.inlier_matches.size());
804
      
805
      if (!converged) { 
806
        ROS_INFO("ICP did not converge. No Edge added");
807
        return mr;
808
      }
809

    
810
      mr.final_trafo = mr.ransac_trafo * mr.icp_trafo;
811

    
812
      MatchingResult mr_icp;
813
      std::vector<double> errors;
814
      double error;
815
      std::vector<cv::DMatch> inliers;
816
      // check if icp improves alignment:
817
      computeInliersAndError(mr.inlier_matches, mr.final_trafo,
818
          this->feature_locations_3d_, older_node->feature_locations_3d_,
819
          inliers, error, errors, 0.04*0.04); 
820

    
821
      for (uint i=0; i<errors.size(); i++)
822
      {
823
        cout << "error: " << round(errors[i]*10000)/100 << endl;
824
      }
825

    
826
      cout << "error was: " << mr.rmse << " and is now: " << error << endl;
827

    
828
      double roll, pitch, yaw, dist;
829
      mat2components(mr.ransac_trafo, roll, pitch, yaw, dist);
830
      cout << "ransac: " << roll << " "<< pitch << " "<< yaw << "   "<< dist << endl;
831

    
832

    
833
      mat2components(mr.icp_trafo, roll, pitch, yaw, dist);
834
      cout << "icp: " << roll << " "<< pitch << " "<< yaw << "   "<< dist << endl;
835

    
836
      mat2components(mr.final_trafo, roll, pitch, yaw, dist);
837
      cout << "final: " << roll << " "<< pitch << " "<< yaw << "   "<< dist << endl;
838

    
839

    
840
      cout << "ransac: " << endl << mr.ransac_trafo << endl;
841
      cout << "icp: " << endl << mr.icp_trafo << endl;
842
      cout << "total: " << endl << mr.final_trafo << endl;
843

    
844

    
845
      if (error > (mr.rmse+0.02))
846
      {
847
        cout << "#### icp-error is too large, ignoring the connection" << endl;
848
        return mr;
849
      }
850
#endif
851
       
852

    
853
#ifndef USE_ICP_BIN
854
#ifndef USE_ICP_CODE      
855
      mr.final_trafo = mr.ransac_trafo;    
856
#endif
857
#endif
858

    
859
      mr.edge.id1 = older_node->id_;//and we have a valid transformation
860
      mr.edge.id2 = this->id_; //since there are enough matching features,
861
      mr.edge.mean = eigen2G2O(mr.final_trafo.cast<double>());//we insert an edge between the frames
862

    
863
      /*this is a weight, that discounts the information coming from later nodes w.r.t earlier nodes.
864
      int lowest_id = mr.edge.id1 < mr.edge.id2 ? mr.edge.id1 : mr.edge.id2;
865
      double timeline_weight = lowest_id > 0 ? (double)lowest_id : 1.0;
866
      */
867

    
868
      double w = (double)mr.inlier_matches.size();///(double)mr.all_matches.size();
869
      mr.edge.informationMatrix =   Eigen::Matrix<double,6,6>::Identity()*(w*w); //TODO: What do we do about the information matrix? Scale with inlier_count. Should rmse be integrated?)
870
  }
871
  // Paper
872
  return mr;
873
}
874

    
875
void Node::clearFeatureInformation(){
876
  //clear only points, by swapping data with empty vector (so mem really gets freed)
877
        std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > f_l_3d;  ///<backprojected 3d descriptor locations relative to cam position in homogeneous coordinates (last dimension is 1.0)
878
  f_l_3d.swap(feature_locations_3d_);
879
        std::vector<cv::KeyPoint> f_l_2d; ///<Where in the image are the descriptors
880
  f_l_2d.swap(feature_locations_2d_);
881
  feature_descriptors_.release();
882
}
883
void Node::clearPointCloud(){
884
  //clear only points, by swapping data with empty vector (so mem really gets freed)
885
  pc_col->width = 0;
886
  pc_col->height = 0;
887
  pointcloud_type pc_empty;
888
  pc_empty.points.swap(pc_col->points);
889
}