Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (33.2 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
//Documentation see header file
19
#include "pcl/ros/conversions.h"
20
#include <pcl/io/io.h>
21
#include "pcl/common/transform.h"
22
#include "pcl_ros/transforms.h"
23
#include "openni_listener.h"
24
#include <cv_bridge/CvBridge.h>
25
#include <opencv2/imgproc/imgproc.hpp>
26
#include <iostream>
27
#include <sstream>
28
#include <string>
29
#include <cv.h>
30
#include <ctime>
31
#include <sensor_msgs/PointCloud2.h>
32
#include <Eigen/Core>
33
#include "node.h"
34
#include "misc.h"
35
//#include <image_geometry/pinhole_camera_model.h>
36
#include "pcl/ros/for_each_type.h"
37

    
38
//For rosbag reading
39
#include <rosbag/view.h>
40
#include <boost/foreach.hpp>
41

    
42

    
43
#include "parameter_server.h"
44
//for comparison with ground truth from mocap and movable cameras on robots
45
#include <tf/transform_listener.h>
46

    
47
typedef message_filters::Subscriber<sensor_msgs::Image> image_sub_type;      
48
typedef message_filters::Subscriber<sensor_msgs::CameraInfo> cinfo_sub_type;      
49
typedef message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub_type;      
50
typedef message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub_type;      
51

    
52
typedef union
53
{
54
  struct /*anonymous*/
55
  {
56
    unsigned char Blue;
57
    unsigned char Green;
58
    unsigned char Red;
59
    unsigned char Alpha;
60
  };
61
  float float_value;
62
  long long_value;
63
} RGBValue;
64

    
65

    
66

    
67
 
68
// Load bag
69
void OpenNIListener::loadBag(const std::string &filename)
70
{
71

    
72
  struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
73
  ROS_INFO("Loading Bagfile %s", filename.c_str());
74
  rosbag::Bag bag;
75
  try{
76
    bag.open(filename, rosbag::bagmode::Read);
77
  } catch(rosbag::BagIOException ex) {
78
    ROS_FATAL("Opening Bagfile %s failed: %s Quitting!", filename.c_str(), ex.what());
79
    ros::shutdown();
80
    return;
81
  }
82

    
83
  ParameterServer* params = ParameterServer::instance();
84
  std::string visua_tpc = params->get<std::string>("topic_image_mono");
85
  std::string depth_tpc = params->get<std::string>("topic_image_depth");
86
  std::string cinfo_tpc = params->get<std::string>("camera_info_topic");
87
  std::string tf_tpc = std::string("/tf");
88

    
89
  // Image topics to load for bagfiles
90
  std::vector<std::string> topics;
91
  topics.push_back(visua_tpc);
92
  topics.push_back(depth_tpc);
93
  topics.push_back(cinfo_tpc);
94
  topics.push_back(tf_tpc);
95

    
96
  rosbag::View view(bag, rosbag::TopicQuery(topics));
97
 // int lc=0; 
98
  // Simulate sending of the messages in the bagfile
99
  BOOST_FOREACH(rosbag::MessageInstance const m, view)
100
  {
101
  //  if(lc++ > 1000) break;
102
    do{ 
103
      usleep(250);
104
      if(!ros::ok()) return;
105
    } while(pause_);
106

    
107
    if (m.getTopic() == visua_tpc || ("/" + m.getTopic() == visua_tpc))
108
    {
109
      sensor_msgs::Image::ConstPtr rgb_img = m.instantiate<sensor_msgs::Image>();
110
      if (rgb_img) rgb_img_sub_->newMessage(rgb_img);
111
      ROS_INFO("Found Message of %s", visua_tpc.c_str());
112
    }
113
    
114
    if (m.getTopic() == depth_tpc || ("/" + m.getTopic() == depth_tpc))
115
    {
116
      sensor_msgs::Image::ConstPtr depth_img = m.instantiate<sensor_msgs::Image>();
117
      if (depth_img) depth_img_sub_->newMessage(depth_img);
118
      ROS_INFO("Found Message of %s", depth_tpc.c_str());
119
    }
120
    if (m.getTopic() == cinfo_tpc || ("/" + m.getTopic() == cinfo_tpc))
121
    {
122
      sensor_msgs::CameraInfo::ConstPtr cam_info = m.instantiate<sensor_msgs::CameraInfo>();
123
      if (cam_info) cam_info_sub_->newMessage(cam_info);
124
      ROS_INFO("Found Message of %s", cinfo_tpc.c_str());
125
    }
126
    if (m.getTopic() == tf_tpc|| ("/" + m.getTopic() == tf_tpc)){
127
      tf::tfMessage::ConstPtr tf_msg = m.instantiate<tf::tfMessage>();
128
      if (tf_msg) tf_pub_.publish(tf_msg);
129
      ROS_INFO("Found Message of %s", tf_tpc.c_str());
130
    }
131
  }
132
  ROS_INFO("Finished processing of Bagfile");
133
  bag.close();
134
  do{ 
135
    future_.waitForFinished(); //Wait if GraphManager ist still computing. 
136
    usleep(1000000); //give it a chance to receive further messages
137
    if(!ros::ok()) return;
138
    ROS_WARN("Waiting for processing to finish.");
139
  } while(graph_mgr_->isBusy());
140

    
141
  if(ParameterServer::instance()->get<bool>("batch_processing")){
142
    graph_mgr_->saveTrajectory(QString(filename.c_str()) + "before_optimization");
143
    graph_mgr_->optimizeGraph(20);
144
    ROS_INFO("Finished with 1st optimization");
145
    graph_mgr_->saveTrajectory(QString(filename.c_str()) + "after1_optimization");
146
    //graph_mgr_->sanityCheck(100.0); //no trajectory is larger than a 100m
147
    //graph_mgr_->pruneEdgesWithErrorAbove(0.5);
148
    //graph_mgr_->optimizeGraph(20);
149
    //ROS_INFO("Finished with 2nd optimization");
150
    //graph_mgr_->saveTrajectory(QString(filename.c_str()) + "after2_optimization");
151
    if(ParameterServer::instance()->get<bool>("store_pointclouds")){
152
      graph_mgr_->saveIndividualClouds(QString(filename.c_str()), false);//not threaded
153
    }
154
    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");
155
    Q_EMIT bagFinished();
156
    ros::shutdown();
157
    usleep(10000000);//10sec to allow all threads to finish (don't know how much is required)
158
    if(ros::ok()){
159
      ROS_ERROR("Should have shutdown meanwhile");
160
      exit(1);
161
    }
162
  }
163
  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");
164
}
165

    
166
OpenNIListener::OpenNIListener(ros::NodeHandle nh, GraphManager* graph_mgr)
167
: graph_mgr_(graph_mgr),
168
  stereo_sync_(NULL), kinect_sync_(NULL), no_cloud_sync_(NULL),
169
  visua_sub_(NULL), depth_sub_(NULL), cloud_sub_(NULL),
170
  depth_mono8_img_(cv::Mat()),
171
  save_bag_file(false),
172
  pause_(ParameterServer::instance()->get<bool>("start_paused")),
173
  getOneFrame_(false),
174
  first_frame_(true),
175
  tflistener_(new tf::TransformListener(nh)),
176
  data_id_(0)
177
{
178
  ParameterServer* params = ParameterServer::instance();
179
  int q = params->get<int>("subscriber_queue_size");
180
  std::string bagfile_name = params->get<std::string>("bagfile_name");
181
  std::string visua_tpc = params->get<std::string>("topic_image_mono");
182
  std::string depth_tpc = params->get<std::string>("topic_image_depth");
183
  std::string cinfo_tpc = params->get<std::string>("camera_info_topic");
184

    
185
  if(bagfile_name.empty()){
186
    std::string cloud_tpc = params->get<std::string>("topic_points");
187
    std::string widev_tpc = params->get<std::string>("wide_topic");
188
    std::string widec_tpc = params->get<std::string>("wide_cloud_topic");
189

    
190
    //All information from Kinect
191
    if(!visua_tpc.empty() && !depth_tpc.empty() && !cloud_tpc.empty())
192
    {   
193
        visua_sub_ = new image_sub_type(nh, visua_tpc, q);
194
        depth_sub_ = new image_sub_type (nh, depth_tpc, q);
195
        cloud_sub_ = new pc_sub_type (nh, cloud_tpc, q);  
196
        kinect_sync_ = new message_filters::Synchronizer<KinectSyncPolicy>(KinectSyncPolicy(q),  *visua_sub_, *depth_sub_, *cloud_sub_),
197
        kinect_sync_->registerCallback(boost::bind(&OpenNIListener::kinectCallback, this, _1, _2, _3));
198
        ROS_INFO_STREAM("Listening to " << visua_tpc << ", " << depth_tpc << " and " << cloud_tpc);
199
    } 
200
    //No cloud, but visual image and depth
201
    else if(!visua_tpc.empty() && !depth_tpc.empty() && !cinfo_tpc.empty() && cloud_tpc.empty())
202
    {   
203
        visua_sub_ = new image_sub_type(nh, visua_tpc, q);
204
        depth_sub_ = new image_sub_type(nh, depth_tpc, q);
205
        cinfo_sub_ = new cinfo_sub_type(nh, cinfo_tpc, q);
206
        no_cloud_sync_ = new message_filters::Synchronizer<NoCloudSyncPolicy>(NoCloudSyncPolicy(q),  *visua_sub_, *depth_sub_, *cinfo_sub_);
207
        no_cloud_sync_->registerCallback(boost::bind(&OpenNIListener::noCloudCallback, this, _1, _2, _3));
208
        ROS_INFO_STREAM("Listening to " << visua_tpc << " and " << depth_tpc);
209
    } 
210

    
211
    //All information from stereo                                               
212
    if(!widec_tpc.empty() && !widec_tpc.empty())
213
    {   
214
      visua_sub_ = new image_sub_type(nh, widev_tpc, q);
215
      cloud_sub_ = new pc_sub_type(nh, widec_tpc, q);
216
      stereo_sync_ = new message_filters::Synchronizer<StereoSyncPolicy>(StereoSyncPolicy(q), *visua_sub_, *cloud_sub_);
217
      if(params->get<bool>("use_wide")){
218
          stereo_sync_->registerCallback(boost::bind(&OpenNIListener::stereoCallback, this, _1, _2));
219
      }
220
      ROS_INFO_STREAM("Listening to " << widev_tpc << " and " << widec_tpc );
221
    } 
222

    
223
    detector_ = createDetector(params->get<std::string>("feature_detector_type"));
224
    extractor_ = createDescriptorExtractor(params->get<std::string>("feature_extractor_type"));
225

    
226
    if(params->get<bool>("concurrent_node_construction")){
227
      ROS_INFO("Threads used by QThreadPool on this Computer %i. Will increase this by one, b/c the QtRos Thread is very lightweight", QThread::idealThreadCount());
228
      QThreadPool::globalInstance()->setMaxThreadCount(QThread::idealThreadCount()*2+2);
229
    }
230

    
231
  } 
232
  else //Bagfile given
233
  {
234

    
235
    tf_pub_ = nh.advertise<tf::tfMessage>("/tf", 10);
236
    //All information from Kinect
237
    if(!visua_tpc.empty() && !depth_tpc.empty() && !cinfo_tpc.empty())
238
    {   
239
      // Set up fake subscribers to capture images
240
      depth_img_sub_ = new BagSubscriber<sensor_msgs::Image>();
241
      rgb_img_sub_ = new BagSubscriber<sensor_msgs::Image>();
242
      cam_info_sub_ = new BagSubscriber<sensor_msgs::CameraInfo>();
243
      no_cloud_sync_ = new message_filters::Synchronizer<NoCloudSyncPolicy>(NoCloudSyncPolicy(q),  *rgb_img_sub_, *depth_img_sub_, *cam_info_sub_);
244
      no_cloud_sync_->registerCallback(boost::bind(&OpenNIListener::noCloudCallback, this, _1, _2, _3));
245
      ROS_INFO_STREAM("Listening to " << visua_tpc << " and " << depth_tpc);
246
    } 
247

    
248

    
249
    detector_ = createDetector(params->get<std::string>("feature_detector_type"));
250
    extractor_ = createDescriptorExtractor(params->get<std::string>("feature_extractor_type"));
251
    QtConcurrent::run(this, &OpenNIListener::loadBag, bagfile_name);
252

    
253
    if(params->get<bool>("concurrent_node_construction")){
254
      ROS_INFO("Threads used by QThreadPool on this Computer %i. Will increase this by two, b/c the QtRos and loadBag threads are lightweight", QThread::idealThreadCount());
255
      QThreadPool::globalInstance()->setMaxThreadCount(QThread::idealThreadCount()+2);
256
    }
257
  }
258
}
259

    
260

    
261
void OpenNIListener::stereoCallback(const sensor_msgs::ImageConstPtr& visual_img_msg, const sensor_msgs::PointCloud2ConstPtr& point_cloud){
262
    struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
263
    ROS_INFO("Received data from stereo cam");
264
    if(++data_id_ % ParameterServer::instance()->get<int>("data_skip_step") != 0){ 
265
      ROS_INFO_THROTTLE(1, "Skipping Frame %i because of data_skip_step setting (this msg is only shown once a sec)", data_id_);
266
      return;
267
    };
268

    
269
    //calculate depthMask
270
    cv::Mat_<uchar> depth_img(visual_img_msg->height, visual_img_msg->width);
271
    float value;
272
    unsigned int pt_ctr = 0;
273
    for(unsigned int y = 0; y < visual_img_msg->height; y++){
274
        for(unsigned int x = 0; x < visual_img_msg->width; x++){
275
            const uchar* value_ch_ptr = &(point_cloud->data[pt_ctr]);
276
            value = *((const float*)value_ch_ptr);
277
            pt_ctr += point_cloud->point_step;
278
            if(value != value){//isnan
279
                depth_img(y,x) = 0;
280
            }else{
281
                depth_img(y,x) = (char)(value*100.0); //Full white at 2.55 meter
282
            }
283
        }
284
    }
285

    
286
    //Get images into OpenCV format
287
    sensor_msgs::CvBridge bridge;
288
    cv::Mat visual_img =  bridge.imgMsgToCv(visual_img_msg, "mono8");
289
    if(visual_img.rows != depth_img.rows ||
290
       visual_img.cols != depth_img.cols ||
291
       point_cloud->width != (uint32_t) visual_img.cols ||
292
       point_cloud->height != (uint32_t) visual_img.rows){
293
      ROS_ERROR("PointCloud, depth and visual image differ in size! Ignoring Data");
294
      return;
295
    }
296

    
297
    if (bagfile_mutex.tryLock() && save_bag_file){
298
       // todo: make the names dynamic
299
       bag.write("/wide_stereo/points2", ros::Time::now(), point_cloud);
300
       bag.write("/wide_stereo/left/image_mono", ros::Time::now(), visual_img_msg);
301
       ROS_INFO_STREAM("Wrote to bagfile " << bag.getFileName());
302
       bagfile_mutex.unlock();
303
       if(pause_) return;
304
    }
305
    pointcloud_type::Ptr pc_col(new pointcloud_type());//will belong to node
306
    if(ParameterServer::instance()->get<bool>("use_gui")){
307
      Q_EMIT newVisualImage(cvMat2QImage(visual_img, 0)); //visual_idx=0
308
      Q_EMIT newDepthImage (cvMat2QImage(depth_img,1));//overwrites last cvMat2QImage
309
    }
310
    pcl::fromROSMsg(*point_cloud,*pc_col);
311
    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");
312
    cameraCallback(visual_img, pc_col, depth_img);
313
}
314

    
315
OpenNIListener::~OpenNIListener(){
316
  delete tflistener_;
317
}
318

    
319
void OpenNIListener::noCloudCallback (const sensor_msgs::ImageConstPtr& visual_img_msg,
320
                                      const sensor_msgs::ImageConstPtr& depth_img_msg,
321
                                      const sensor_msgs::CameraInfoConstPtr& cam_info_msg) 
322
{
323
  struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
324
  ROS_DEBUG("Received data from kinect");
325

    
326
  if(++data_id_ % ParameterServer::instance()->get<int>("data_skip_step") != 0){ 
327
    ROS_INFO_THROTTLE(1, "Skipping Frame %i because of data_skip_step setting (this msg is only shown once a sec)", data_id_);
328
    if(ParameterServer::instance()->get<bool>("use_gui")){//Show the image, even if not using it
329
      sensor_msgs::CvBridge bridge;
330
      cv::Mat depth_float_img = bridge.imgMsgToCv(depth_img_msg);
331
      cv::Mat visual_img =  bridge.imgMsgToCv(visual_img_msg);
332
      if(visual_img.rows != depth_float_img.rows || 
333
         visual_img.cols != depth_float_img.cols){
334
        ROS_ERROR("depth and visual image differ in size! Ignoring Data");
335
        return;
336
      }
337
      depthToCV8UC1(depth_float_img, depth_mono8_img_); //float can't be visualized or used as mask in float format TODO: reprogram keypoint detector to use float values with nan to mask
338
      Q_EMIT newVisualImage(cvMat2QImage(visual_img, 0)); //visual_idx=0
339
      Q_EMIT newDepthImage (cvMat2QImage(depth_mono8_img_,1));//overwrites last cvMat2QImage
340
    }
341
    return;
342
  }
343

    
344
  //Get images into OpenCV format
345
  sensor_msgs::CvBridge bridge;
346
  cv::Mat depth_float_img = bridge.imgMsgToCv(depth_img_msg);
347
  cv::Mat visual_img =  bridge.imgMsgToCv(visual_img_msg);
348
  if(visual_img.rows != depth_float_img.rows || 
349
     visual_img.cols != depth_float_img.cols){
350
    ROS_ERROR("depth and visual image differ in size! Ignoring Data");
351
    return;
352
  }
353
  depthToCV8UC1(depth_float_img, depth_mono8_img_); //float can't be visualized or used as mask in float format TODO: reprogram keypoint detector to use float values with nan to mask
354

    
355
  ros::Time d_time = depth_img_msg->header.stamp;
356
  ros::Time rgb_time = visual_img_msg->header.stamp;
357

    
358
  long rgb_timediff = abs(static_cast<long>(rgb_time.nsec) - static_cast<long>(d_time.nsec));
359
  if(rgb_timediff > 33333333){
360
     ROS_WARN_COND(rgb_timediff > 33333333, "Depth and RGB image off more than 1/30sec: %li (nsec)", rgb_timediff);
361
     ROS_WARN_COND(ParameterServer::instance()->get<bool>("drop_async_frames"), "Asynchronous frames ignored. See parameters if you want to keep async frames.");
362
     ROS_INFO("Depth image time: %d - %d", d_time.sec,   d_time.nsec);
363
     ROS_INFO("RGB   image time: %d - %d", rgb_time.sec, rgb_time.nsec);
364
     if(ParameterServer::instance()->get<bool>("drop_async_frames")){
365
       return;
366
     }
367
  } else {
368
     ROS_DEBUG("Depth image time: %d - %d", d_time.sec,   d_time.nsec);
369
     ROS_DEBUG("RGB   image time: %d - %d", rgb_time.sec, rgb_time.nsec);
370
  }
371

    
372
  if (bagfile_mutex.tryLock() && save_bag_file){
373
     // todo: make the names dynamic
374
     bag.write("/camera/rgb/image_mono", ros::Time::now(), visual_img_msg);
375
     bag.write("/camera/depth/image", ros::Time::now(), depth_img_msg);
376
     ROS_INFO_STREAM("Wrote to bagfile " << bag.getFileName());
377
     bagfile_mutex.unlock();
378
     if(pause_) return;
379
  }
380
  if(ParameterServer::instance()->get<bool>("use_gui")){
381
    Q_EMIT newVisualImage(cvMat2QImage(visual_img, 0)); //visual_idx=0
382
    Q_EMIT newDepthImage (cvMat2QImage(depth_mono8_img_,1));//overwrites last cvMat2QImage
383
  }
384
  pointcloud_type::Ptr pc_col(createXYZRGBPointCloud(depth_img_msg, visual_img_msg, cam_info_msg));
385
  //sensor_msgs::PointCloud2::Ptr pc2(new sensor_msgs::PointCloud2());
386
  //pcl::toROSMsg(*pc, *pc2);
387
  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");
388
  cameraCallback(visual_img, pc_col, depth_mono8_img_);
389
}
390

    
391

    
392

    
393
void OpenNIListener::kinectCallback (const sensor_msgs::ImageConstPtr& visual_img_msg,//got to be mono?
394
                                     const sensor_msgs::ImageConstPtr& depth_img_msg,   
395
                                     const sensor_msgs::PointCloud2ConstPtr& point_cloud) 
396
{
397
  struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
398
  ROS_DEBUG("Received data from kinect");
399

    
400
  //Get images into OpenCV format
401
  sensor_msgs::CvBridge bridge;
402
  cv::Mat depth_float_img = bridge.imgMsgToCv(depth_img_msg);
403
  cv::Mat visual_img =  bridge.imgMsgToCv(visual_img_msg);
404
  if(visual_img.rows != depth_float_img.rows || 
405
     visual_img.cols != depth_float_img.cols ||
406

    
407
     point_cloud->width != (uint32_t) visual_img.cols ||
408
     point_cloud->height != (uint32_t) visual_img.rows){
409
    ROS_ERROR("PointCloud, depth and visual image differ in size! Ignoring Data");
410
    return;
411
  }
412
  depthToCV8UC1(depth_float_img, depth_mono8_img_); //float can't be visualized or used as mask in float format TODO: reprogram keypoint detector to use float values with nan to mask
413

    
414
  ros::Time d_time = depth_img_msg->header.stamp;
415
  ros::Time rgb_time = visual_img_msg->header.stamp;
416
  ros::Time pc_time = point_cloud->header.stamp;
417

    
418
  long rgb_timediff = abs(static_cast<long>(rgb_time.nsec) - static_cast<long>(pc_time.nsec));
419
  if(d_time.nsec != pc_time.nsec ||rgb_timediff > 33333333){
420
     ROS_WARN_COND(d_time.nsec != pc_time.nsec, "PointCloud doesn't correspond to depth image");
421
     ROS_WARN_COND(rgb_timediff > 33333333, "Point cloud and RGB image off more than 1/30sec: %li (nsec)", rgb_timediff);
422
     ROS_WARN_COND(ParameterServer::instance()->get<bool>("drop_async_frames"), "Asynchronous frames ignored. See parameters if you want to keep async frames.");
423
     ROS_INFO("Depth image time: %d - %d", d_time.sec,   d_time.nsec);
424
     ROS_INFO("RGB   image time: %d - %d", rgb_time.sec, rgb_time.nsec);
425
     ROS_INFO("Point cloud time: %d - %d", pc_time.sec,  pc_time.nsec);
426
     if(ParameterServer::instance()->get<bool>("drop_async_frames")){
427
       return;
428
     }
429
  } else {
430
     ROS_DEBUG("Depth image time: %d - %d", d_time.sec,   d_time.nsec);
431
     ROS_DEBUG("RGB   image time: %d - %d", rgb_time.sec, rgb_time.nsec);
432
     ROS_DEBUG("Point cloud time: %d - %d", pc_time.sec,  pc_time.nsec);
433
  }
434

    
435
  if (bagfile_mutex.tryLock() && save_bag_file){
436
     // todo: make the names dynamic
437
     bag.write("/camera/rgb/points", ros::Time::now(), point_cloud);
438
     bag.write("/camera/rgb/image_mono", ros::Time::now(), visual_img_msg);
439
     bag.write("/camera/depth/image", ros::Time::now(), depth_img_msg);
440
     ROS_INFO_STREAM("Wrote to bagfile " << bag.getFileName());
441
     bagfile_mutex.unlock();
442
     if(pause_) return;
443
  }
444
  if(ParameterServer::instance()->get<bool>("use_gui")){
445
    Q_EMIT newVisualImage(cvMat2QImage(visual_img, 0)); //visual_idx=0
446
    Q_EMIT newDepthImage (cvMat2QImage(depth_mono8_img_,1));//overwrites last cvMat2QImage
447
  }
448

    
449
  pointcloud_type::Ptr pc_col(new pointcloud_type());//will belong to node
450
  pcl::fromROSMsg(*point_cloud,*pc_col);
451
  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");
452
  cameraCallback(visual_img, pc_col, depth_mono8_img_);
453
}
454

    
455
void OpenNIListener::cameraCallback(cv::Mat visual_img, pointcloud_type::Ptr point_cloud, cv::Mat depth_mono8_img)
456
{
457
  struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
458
  ROS_WARN_COND(point_cloud ==NULL, "Nullpointer for pointcloud");
459
  if(getOneFrame_) { //if getOneFrame_ is set, unset it and skip check for  pause
460
      getOneFrame_ = false;
461
  } else if(pause_ && !save_bag_file) { //Visualization and nothing else
462
    return; 
463
  }
464
  cv::Mat gray_img; 
465
  if(visual_img.type() == CV_8UC3){
466
    cvtColor(visual_img, gray_img, CV_RGB2GRAY);
467
  } else {
468
    gray_img = visual_img;
469
  }
470

    
471

    
472
  Q_EMIT setGUIStatus("Computing Keypoints and Features");
473

    
474
  //######### Main Work: create new node ##############################################################
475
  Node* node_ptr = new Node(gray_img, detector_, extractor_, point_cloud, depth_mono8_img_);
476

    
477
  // Fill in Transformation ---------------------------------------------------------------------------
478
  //Retrieve the transform between the lens and the base-link at capturing time
479
  std::clock_t tf_time=std::clock();
480
  tf::StampedTransform base2points;
481
  std::string base_frame  = ParameterServer::instance()->get<std::string>("base_frame_name");
482
  ros::Time pc_time = point_cloud->header.stamp;
483
  try{
484
    tflistener_->waitForTransform(base_frame, point_cloud->header.frame_id, pc_time, ros::Duration(0.1));
485
    tflistener_->lookupTransform(base_frame, point_cloud->header.frame_id, pc_time, base2points);
486
  }
487
  catch (tf::TransformException ex){
488
    ROS_WARN("%s",ex.what());
489
    ROS_WARN("Using Standard kinect /openni_camera -> /openni_rgb_optical_frame as transformation");
490
    //emulate the transformation from kinect openni_camera frame to openni_rgb_optical_frame
491
    base2points.setRotation(tf::createQuaternionFromRPY(-1.57,0,-1.57));
492
    base2points.setOrigin(tf::Point(0,-0.04,0));
493
  }
494
  node_ptr->setBase2PointsTransform(base2points);
495

    
496
  std::string gt_frame = ParameterServer::instance()->get<std::string>("ground_truth_frame_name");
497
  if(!gt_frame.empty()){ 
498
    //Retrieve the ground truth data. For the first frame it will be
499
    //set as origin. the rest will be used to compare
500
    tf::StampedTransform ground_truth_transform;
501
    try{
502
      tflistener_->waitForTransform(gt_frame, "/openni_camera", pc_time, ros::Duration(0.1));
503
      tflistener_->lookupTransform(gt_frame, "/openni_camera", pc_time, ground_truth_transform);
504
      tf::StampedTransform b2p;
505
      //HACK to comply with J├╝rgen Sturm's Ground truth, though I can't manage here to get the full transform from tf
506
      b2p.setRotation(tf::createQuaternionFromRPY(-1.57,0,-1.57));
507
      b2p.setOrigin(tf::Point(0,-0.04,0));
508
      ground_truth_transform *= b2p;
509
    }
510
    catch (tf::TransformException ex){
511
      ROS_WARN("%s - Using Identity for Ground Truth",ex.what());
512
      ground_truth_transform = tf::StampedTransform(tf::Transform::getIdentity(), pc_time, "/missing_ground_truth", "/openni_camera");
513
    }
514
    node_ptr->setGroundTruthTransform(ground_truth_transform);
515
  }
516
  ROS_INFO_STREAM_NAMED("timings", "tf time: "<< ( std::clock() - tf_time ) / (double)CLOCKS_PER_SEC  <<"sec"); 
517
  // End: Fill in Transformation -----------------------------------------------------------------------
518

    
519

    
520
  std::clock_t parallel_wait_time=std::clock();
521
  future_.waitForFinished(); //Wait if GraphManager ist still computing. Does this skip on empty qfuture?
522
  ROS_INFO_STREAM_NAMED("timings", "waiting time: "<< ( std::clock() - parallel_wait_time ) / (double)CLOCKS_PER_SEC  <<"sec"); 
523

    
524
//With this define, processNode runs in the background and after finishing visualizes the results
525
//Thus another Callback can be started in the meantime, to create a new node in parallel
526
  if(ParameterServer::instance()->get<bool>("concurrent_node_construction")) {
527
    ROS_DEBUG("Processing Node in parallel");
528
    future_ =  QtConcurrent::run(this, &OpenNIListener::processNode, gray_img, point_cloud, node_ptr);
529
  }
530
  else //regular function call
531
    processNode(gray_img, point_cloud, node_ptr);
532
  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");
533
}
534

    
535
void OpenNIListener::processNode(cv::Mat& visual_img,   // will be drawn to
536
                                 pointcloud_type::Ptr point_cloud,
537
                                 Node* new_node)
538
{
539
  struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
540
  Q_EMIT setGUIStatus("GraphSLAM");
541
  bool has_been_added = graph_mgr_->addNode(new_node);
542

    
543
  //######### Visualization code  #############################################
544
  cv::Mat feature_img = cv::Mat::zeros(visual_img.rows, visual_img.cols, CV_8UC1);
545
  if(ParameterServer::instance()->get<bool>("use_gui")){
546
    if(has_been_added){
547
        graph_mgr_->drawFeatureFlow(feature_img);
548
        Q_EMIT newFeatureFlowImage(cvMat2QImage(visual_img,depth_mono8_img_, feature_img, 2)); //show registration
549
    } else {
550
        cv::drawKeypoints(feature_img, new_node->feature_locations_2d_, feature_img, cv::Scalar(155), 5);
551
        Q_EMIT newFeatureFlowImage(cvMat2QImage(visual_img,depth_mono8_img_, feature_img, 2)); //show registration
552
        delete new_node;
553
    }
554
  }
555
  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");
556
}
557

    
558

    
559
void OpenNIListener::toggleBagRecording(){
560
  bagfile_mutex.lock();
561
  save_bag_file = !save_bag_file;
562
  // save bag
563
  if (save_bag_file)
564
  {
565
    time_t rawtime; 
566
    struct tm * timeinfo;
567
    char buffer [80];
568

    
569
    time ( &rawtime );
570
    timeinfo = localtime ( &rawtime );
571
    // create a nice name
572
    strftime (buffer,80,"kinect_%Y-%m-%d-%H-%M-%S.bag",timeinfo);
573

    
574
    bag.open(buffer, rosbag::bagmode::Write);
575
    ROS_INFO_STREAM("Opened bagfile " << bag.getFileName());
576
  } else {
577
    ROS_INFO_STREAM("Closing bagfile " << bag.getFileName());
578
    bag.close();
579
  }
580
  bagfile_mutex.unlock();
581
}
582

    
583
void OpenNIListener::togglePause(){
584
  pause_ = !pause_;
585
  ROS_INFO("Pause toggled to: %s", pause_? "true":"false");
586
  if(pause_) Q_EMIT setGUIStatus("Processing Thread Stopped");
587
  else Q_EMIT setGUIStatus("Processing Thread Running");
588
}
589
void OpenNIListener::getOneFrame(){
590
  getOneFrame_=true;
591
}
592
/// Create a QImage from image. The QImage stores its data in the rgba_buffers_ indexed by idx (reused/overwritten each call)
593
QImage OpenNIListener::cvMat2QImage(const cv::Mat& channel1, const cv::Mat& channel2, const cv::Mat& channel3, unsigned int idx){
594
  if(rgba_buffers_.size() <= idx){
595
      rgba_buffers_.resize(idx+1);
596
  }
597
  if(channel2.rows != channel1.rows || channel2.cols != channel1.cols ||
598
     channel3.rows != channel1.rows || channel3.cols != channel1.cols){
599
     ROS_ERROR("Image channels to be combined differ in size");
600
  }
601
  if(channel1.rows != rgba_buffers_[idx].rows || channel1.cols != rgba_buffers_[idx].cols){
602
    ROS_DEBUG("Created new rgba_buffer with index %i", idx);
603
    rgba_buffers_[idx] = cv::Mat( channel1.rows, channel1.cols, CV_8UC4); 
604
    printMatrixInfo(rgba_buffers_[idx]);
605
  }
606
  std::vector<cv::Mat> input;
607
  cv::Mat alpha( channel1.rows, channel1.cols, CV_8UC1, cv::Scalar(255)); //TODO this could be buffered for performance
608
  input.push_back(channel1);
609
  input.push_back(channel2);
610
  input.push_back(channel3);
611
  input.push_back(alpha);
612
  cv::merge(input, rgba_buffers_[idx]);
613
  printMatrixInfo(rgba_buffers_[idx]);
614
  return QImage((unsigned char *)(rgba_buffers_[idx].data), 
615
                rgba_buffers_[idx].cols, rgba_buffers_[idx].rows, 
616
                rgba_buffers_[idx].step, QImage::Format_RGB32 );
617
}
618

    
619
/// Create a QImage from image. The QImage stores its data in the rgba_buffers_ indexed by idx (reused/overwritten each call)
620
QImage OpenNIListener::cvMat2QImage(const cv::Mat& image, unsigned int idx){
621
  ROS_DEBUG_STREAM("Converting Matrix of type " << openCVCode2String(image.type()) << " to RGBA");
622
  if(rgba_buffers_.size() <= idx){
623
      rgba_buffers_.resize(idx+1);
624
  }
625
  ROS_DEBUG_STREAM("Target Matrix is of type " << openCVCode2String(rgba_buffers_[idx].type()));
626
  if(image.rows != rgba_buffers_[idx].rows || image.cols != rgba_buffers_[idx].cols){
627
    ROS_DEBUG("Created new rgba_buffer with index %i", idx);
628
    rgba_buffers_[idx] = cv::Mat( image.rows, image.cols, CV_8UC4); 
629
    printMatrixInfo(rgba_buffers_[idx]);
630
  }
631
  cv::Mat alpha( image.rows, image.cols, CV_8UC1, cv::Scalar(255)); //TODO this could be buffered for performance
632
  cv::Mat in[] = { image, alpha };
633
  // rgba[0] -> bgr[2], rgba[1] -> bgr[1],
634
  // rgba[2] -> bgr[0], rgba[3] -> alpha[0]
635

    
636
  if(image.type() == CV_8UC1){
637
    int from_to[] = { 0,0,  0,1,  0,2,  1,3 };
638
    mixChannels( in , 2, &rgba_buffers_[idx], 1, from_to, 4 );
639
  } else {
640
    int from_to[] = { 2,0,  1,1,  0,2,  3,3 }; //BGR+A -> RGBA
641
    mixChannels( in , 2, &rgba_buffers_[idx], 1, from_to, 4 );
642
  }
643
  printMatrixInfo(rgba_buffers_[idx]);
644
  //cv::cvtColor(image, rgba_buffers_, CV_GRAY2RGBA);}
645
  //}
646
  return QImage((unsigned char *)(rgba_buffers_[idx].data), 
647
                rgba_buffers_[idx].cols, rgba_buffers_[idx].rows, 
648
                rgba_buffers_[idx].step, QImage::Format_RGB32 );
649
}
650

    
651

    
652
pointcloud_type* createXYZRGBPointCloud (const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::ImageConstPtr& rgb_msg, const sensor_msgs::CameraInfoConstPtr& cam_info) 
653
{
654
  struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
655
  pointcloud_type* cloud_msg (new pointcloud_type() );
656
  cloud_msg->header.stamp     = depth_msg->header.stamp;
657
  cloud_msg->header.frame_id  = rgb_msg->header.frame_id;
658
  cloud_msg->is_dense         = false;
659

    
660
  float cx, cy, fx,fy;//principal point and focal lengths
661
  unsigned color_step, color_skip;
662

    
663
  cloud_msg->height = depth_msg->height;
664
  cloud_msg->width = depth_msg->width;
665
  cx = cam_info->K[2]; //(cloud_msg->width >> 1) - 0.5f;
666
  cy = cam_info->K[5]; //(cloud_msg->height >> 1) - 0.5f;
667
  fx = 1.0f / cam_info->K[0]; 
668
  fy = 1.0f / cam_info->K[4]; 
669
  int pixel_data_size = 0;
670
  if(rgb_msg->encoding.compare("mono8") == 0) pixel_data_size = 1;
671
  if(rgb_msg->encoding.compare("rgb8") == 0) pixel_data_size = 3;
672
  ROS_ERROR_COND(pixel_data_size == 0, "Unknown image encoding: %s!", rgb_msg->encoding.c_str());
673
  color_step = pixel_data_size * rgb_msg->width / cloud_msg->width;
674
  color_skip = pixel_data_size * (rgb_msg->height / cloud_msg->height - 1) * rgb_msg->width;
675

    
676
  cloud_msg->points.resize (cloud_msg->height * cloud_msg->width);
677

    
678
  const float* depth_buffer = reinterpret_cast<const float*>(&depth_msg->data[0]);
679
  const uint8_t* rgb_buffer = &rgb_msg->data[0];
680

    
681
  // depth_msg already has the desired dimensions, but rgb_msg may be higher res.
682
  int color_idx = 0, depth_idx = 0;
683
  double depth_scaling = ParameterServer::instance()->get<double>("depth_scaling_factor");
684

    
685
  pointcloud_type::iterator pt_iter = cloud_msg->begin ();
686
  for (int v = 0; v < (int)cloud_msg->height; ++v, color_idx += color_skip)
687
  {
688
    for (int u = 0; u < (int)cloud_msg->width; ++u, color_idx += color_step, ++depth_idx, ++pt_iter)
689
    {
690
      point_type& pt = *pt_iter;
691
      float Z = depth_buffer[depth_idx] * depth_scaling;
692

    
693
      // Check for invalid measurements
694
      if (std::isnan (Z))
695
      {
696
        pt.x = pt.y = pt.z = Z;
697
      }
698
      else // Fill in XYZ
699
      {
700
        pt.x = (u - cx) * Z * fx;
701
        pt.y = (v - cy) * Z * fy;
702
        pt.z = Z;
703
      }
704

    
705
      // Fill in color
706
      RGBValue color;
707
      if(pixel_data_size == 3){
708
        color.Red   = rgb_buffer[color_idx];
709
        color.Green = rgb_buffer[color_idx + 1];
710
        color.Blue  = rgb_buffer[color_idx + 2];
711
      } else {
712
        color.Red   = color.Green = color.Blue  = rgb_buffer[color_idx];
713
      }
714
      color.Alpha = 0;
715
      pt.rgb = color.float_value;
716
    }
717
  }
718

    
719
  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");
720
  return cloud_msg;
721
}