CameraFreenect.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
19 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 */
26 
27 #include <rtabmap/core/camera/CameraFreenect.h>
28 #include <rtabmap/utilite/UThread.h>
29 #include <rtabmap/utilite/UTimer.h>
30 #include <rtabmap/core/util2d.h>
31 #include <opencv2/imgproc/types_c.h>
32 
33 #ifdef RTABMAP_FREENECT
34 #include <libfreenect.h>
35 #ifdef FREENECT_DASH_INCLUDES
36 #include <libfreenect-registration.h>
37 #else
38 #include <libfreenect_registration.h>
39 #endif
40 #endif
41 
42 namespace rtabmap
43 {
44 
45 #ifdef RTABMAP_FREENECT
46 
47 class FreenectDevice : public UThread {
48  public:
49  FreenectDevice(freenect_context * ctx, int index, bool color = true, bool registered = true) :
50  index_(index),
51  color_(color),
52  registered_(registered),
53  ctx_(ctx),
54  device_(0),
55  depthFocal_(0.0f)
56  {
57  UASSERT(ctx_ != 0);
58  }
59 
60  virtual ~FreenectDevice()
61  {
62  this->join(true);
63  if(device_ && freenect_close_device(device_) < 0){} //FN_WARNING("Device did not shutdown in a clean fashion");
64  }
65 
66  const std::string & getSerial() const {return serial_;}
67 
68  bool init()
69  {
70  if(device_)
71  {
72  this->join(true);
73  freenect_close_device(device_);
74  device_ = 0;
75  }
76  serial_.clear();
77  std::vector<std::string> deviceSerials;
78  freenect_device_attributes* attr_list;
79  freenect_device_attributes* item;
80  freenect_list_device_attributes(ctx_, &attr_list);
81  for (item = attr_list; item != NULL; item = item->next) {
82  deviceSerials.push_back(std::string(item->camera_serial));
83  }
84  freenect_free_device_attributes(attr_list);
85 
86  if(freenect_open_device(ctx_, &device_, index_) < 0)
87  {
88  UERROR("FreenectDevice: Cannot open Kinect");
89  return false;
90  }
91 
92  if(index_ >= 0 && index_ < (int)deviceSerials.size())
93  {
94  serial_ = deviceSerials[index_];
95  }
96  else
97  {
98  UERROR("Could not get serial for index %d", index_);
99  }
100 
101  UINFO("color=%d registered=%d", color_?1:0, registered_?1:0);
102 
103  freenect_set_user(device_, this);
104  freenect_frame_mode videoMode = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, color_?FREENECT_VIDEO_RGB:FREENECT_VIDEO_IR_8BIT);
105  freenect_frame_mode depthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, color_ && registered_?FREENECT_DEPTH_REGISTERED:FREENECT_DEPTH_MM);
106  if(!videoMode.is_valid)
107  {
108  UERROR("Freenect: video mode selected not valid!");
109  return false;
110  }
111  if(!depthMode.is_valid)
112  {
113  UERROR("Freenect: depth mode selected not valid!");
114  return false;
115  }
116  UASSERT(videoMode.data_bits_per_pixel == 8 || videoMode.data_bits_per_pixel == 24);
117  UASSERT(depthMode.data_bits_per_pixel == 16);
118  freenect_set_video_mode(device_, videoMode);
119  freenect_set_depth_mode(device_, depthMode);
120  rgbIrBuffer_ = cv::Mat(cv::Size(videoMode.width,videoMode.height), color_?CV_8UC3:CV_8UC1);
121  depthBuffer_ = cv::Mat(cv::Size(depthMode.width,depthMode.height), CV_16UC1);
122  freenect_set_depth_buffer(device_, depthBuffer_.data);
123  freenect_set_video_buffer(device_, rgbIrBuffer_.data);
124  freenect_set_depth_callback(device_, freenect_depth_callback);
125  freenect_set_video_callback(device_, freenect_video_callback);
126 
127  float rgb_focal_length_sxga = 1050.0f;
128  float width_sxga = 1280.0f;
129  float width = freenect_get_current_depth_mode(device_).width;
130  float scale = width / width_sxga;
131  if(color_ && registered_)
132  {
133  depthFocal_ = rgb_focal_length_sxga * scale;
134  }
135  else
136  {
137  freenect_registration reg = freenect_copy_registration(device_);
138  float depth_focal_length_sxga = reg.zero_plane_info.reference_distance / reg.zero_plane_info.reference_pixel_size;
139  freenect_destroy_registration(&reg);
140 
141  depthFocal_ = depth_focal_length_sxga * scale;
142  }
143 
144  UINFO("FreenectDevice: Depth focal = %f", depthFocal_);
145  return true;
146  }
147 
148  float getDepthFocal() const {return depthFocal_;}
149 
150  void getData(cv::Mat & rgb, cv::Mat & depth)
151  {
152  if(this->isRunning())
153  {
154  if(!dataReady_.acquire(1, 5000))
155  {
156  UERROR("Not received any frames since 5 seconds, try to restart the camera again.");
157  }
158  else
159  {
160  UScopeMutex s(dataMutex_);
161  rgb = rgbIrLastFrame_;
162  depth = depthLastFrame_;
163  rgbIrLastFrame_ = cv::Mat();
164  depthLastFrame_= cv::Mat();
165  }
166  }
167  }
168 
169  void getAccelerometerValues(double & x, double & y, double & z)
170  {
171  freenect_update_tilt_state(device_);
172  freenect_raw_tilt_state* state = freenect_get_tilt_state(device_);
173  freenect_get_mks_accel(state, &x,&y,&z);
174  }
175 
176 private:
177  // Do not call directly even in child
178  void VideoCallback(void* rgb)
179  {
180  UASSERT(rgbIrBuffer_.data == rgb);
181  UScopeMutex s(dataMutex_);
182  bool notify = rgbIrLastFrame_.empty();
183 
184  if(color_)
185  {
186  cv::cvtColor(rgbIrBuffer_, rgbIrLastFrame_, CV_RGB2BGR);
187  }
188  else // IrDepth
189  {
190  rgbIrLastFrame_ = rgbIrBuffer_.clone();
191  }
192  if(!depthLastFrame_.empty() && notify)
193  {
194  dataReady_.release();
195  }
196  }
197 
198  // Do not call directly even in child
199  void DepthCallback(void* depth)
200  {
201  UASSERT(depthBuffer_.data == depth);
202  UScopeMutex s(dataMutex_);
203  bool notify = depthLastFrame_.empty();
204  depthLastFrame_ = depthBuffer_.clone();
205  if(!rgbIrLastFrame_.empty() && notify)
206  {
207  dataReady_.release();
208  }
209  }
210 
211  void startVideo() {
212  if(device_ && freenect_start_video(device_) < 0) UERROR("Cannot start RGB callback");
213  }
214  void stopVideo() {
215  if(device_ && freenect_stop_video(device_) < 0) UERROR("Cannot stop RGB callback");
216  }
217  void startDepth() {
218  if(device_ && freenect_start_depth(device_) < 0) UERROR("Cannot start depth callback");
219  }
220  void stopDepth() {
221  if(device_ && freenect_stop_depth(device_) < 0) UERROR("Cannot stop depth callback");
222  }
223 
224  virtual void mainLoopBegin()
225  {
226  if(device_) freenect_set_led(device_, LED_RED);
227  this->startDepth();
228  this->startVideo();
229  }
230 
231  virtual void mainLoop()
232  {
233  timeval t;
234  t.tv_sec = 0;
235  t.tv_usec = 10000;
236  if(freenect_process_events_timeout(ctx_, &t) < 0)
237  {
238  UERROR("FreenectDevice: Cannot process freenect events");
239  this->kill();
240  }
241  }
242 
243  virtual void mainLoopEnd()
244  {
245  if(device_) freenect_set_led(device_, LED_GREEN);
246  this->stopDepth();
247  this->stopVideo();
248  dataReady_.release();
249  }
250 
251  static void freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {
252  FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
253  device->DepthCallback(depth);
254  }
255  static void freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {
256  FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
257  device->VideoCallback(video);
258  }
259 
260  //noncopyable
261  FreenectDevice( const FreenectDevice& );
262  const FreenectDevice& operator=( const FreenectDevice& );
263 
264  private:
265  int index_;
266  bool color_;
267  bool registered_;
268  std::string serial_;
269  freenect_context * ctx_;
270  freenect_device * device_;
271  cv::Mat depthBuffer_;
272  cv::Mat rgbIrBuffer_;
273  UMutex dataMutex_;
274  cv::Mat depthLastFrame_;
275  cv::Mat rgbIrLastFrame_;
276  float depthFocal_;
277  USemaphore dataReady_;
278 };
279 #endif
280 
281 //
282 // CameraFreenect
283 //
284 bool CameraFreenect::available()
285 {
286 #ifdef RTABMAP_FREENECT
287  return true;
288 #else
289  return false;
290 #endif
291 }
292 
293 CameraFreenect::CameraFreenect(int deviceId, Type type, float imageRate, const Transform & localTransform) :
294  Camera(imageRate, localTransform)
295 #ifdef RTABMAP_FREENECT
296  ,
297  deviceId_(deviceId),
298  type_(type),
299  ctx_(0),
300  freenectDevice_(0)
301 #endif
302 {
303 #ifdef RTABMAP_FREENECT
304  if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
305  // claim camera
306  freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
307 #endif
308 }
309 
310 CameraFreenect::~CameraFreenect()
311 {
312 #ifdef RTABMAP_FREENECT
313  if(freenectDevice_)
314  {
315  freenectDevice_->join(true);
316  delete freenectDevice_;
317  freenectDevice_ = 0;
318  }
319  if(ctx_)
320  {
321  if(freenect_shutdown(ctx_) < 0){} //FN_WARNING("Freenect did not shutdown in a clean fashion");
322  }
323 #endif
324 }
325 
326 bool CameraFreenect::init(const std::string & calibrationFolder, const std::string & cameraName)
327 {
328 #ifdef RTABMAP_FREENECT
329  if(freenectDevice_)
330  {
331  freenectDevice_->join(true);
332  delete freenectDevice_;
333  freenectDevice_ = 0;
334  }
335 
336  if(ctx_ && freenect_num_devices(ctx_) > 0)
337  {
338  // look for calibration files
339  bool hardwareRegistration = true;
340  stereoModel_ = StereoCameraModel();
341  if(!calibrationFolder.empty())
342  {
343  // we need the serial, HACK: init a temp device to get it
344  FreenectDevice dev(ctx_, deviceId_);
345  if(!dev.init())
346  {
347  UERROR("CameraFreenect: Init failed!");
348  }
349  std::string calibrationName = dev.getSerial();
350  if(!cameraName.empty())
351  {
352  calibrationName = cameraName;
353  }
354  stereoModel_.setName(calibrationName, "depth", "rgb");
355  hardwareRegistration = !stereoModel_.load(calibrationFolder, calibrationName, false);
356 
357  if(type_ == kTypeIRDepth)
358  {
359  hardwareRegistration = false;
360  }
361 
362 
363  if((type_ == kTypeIRDepth && !stereoModel_.left().isValidForRectification()) ||
364  (type_ == kTypeColorDepth && !stereoModel_.right().isValidForRectification()))
365  {
366  UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
367  calibrationName.c_str(), calibrationFolder.c_str());
368  }
369  else if(type_ == kTypeColorDepth && stereoModel_.right().isValidForRectification() && hardwareRegistration)
370  {
371  UWARN("Missing extrinsic calibration file for camera \"%s\" in \"%s\" folder, default registration is used even if rgb is rectified!",
372  calibrationName.c_str(), calibrationFolder.c_str());
373  }
374  else if(type_ == kTypeColorDepth && stereoModel_.right().isValidForRectification() && !hardwareRegistration)
375  {
376  UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
377  "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
378  }
379  }
380 
381  freenectDevice_ = new FreenectDevice(ctx_, deviceId_, type_==kTypeColorDepth, hardwareRegistration);
382  if(freenectDevice_->init())
383  {
384  freenectDevice_->start();
385  uSleep(3000);
386  return true;
387  }
388  else
389  {
390  UERROR("CameraFreenect: Init failed!");
391  }
392  delete freenectDevice_;
393  freenectDevice_ = 0;
394  }
395  else
396  {
397  UERROR("CameraFreenect: No devices connected!");
398  }
399 #else
400  UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
401 #endif
402  return false;
403 }
404 
405 bool CameraFreenect::isCalibrated() const
406 {
407  return true;
408 }
409 
410 std::string CameraFreenect::getSerial() const
411 {
412 #ifdef RTABMAP_FREENECT
413  if(freenectDevice_)
414  {
415  return freenectDevice_->getSerial();
416  }
417 #endif
418  return "";
419 }
420 
421 SensorData CameraFreenect::captureImage(CameraInfo * info)
422 {
423  SensorData data;
424 #ifdef RTABMAP_FREENECT
425  if(ctx_ && freenectDevice_)
426  {
427  if(freenectDevice_->isRunning())
428  {
429  cv::Mat depth,rgb;
430  freenectDevice_->getData(rgb, depth);
431  if(!rgb.empty() && !depth.empty())
432  {
433  UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
434 
435  // default calibration
436  CameraModel model(
437  freenectDevice_->getDepthFocal(), //fx
438  freenectDevice_->getDepthFocal(), //fy
439  float(rgb.cols/2) - 0.5f, //cx
440  float(rgb.rows/2) - 0.5f, //cy
441  this->getLocalTransform(),
442  0,
443  rgb.size());
444 
445  if(type_==kTypeIRDepth)
446  {
447  if(stereoModel_.left().isValidForRectification())
448  {
449  rgb = stereoModel_.left().rectifyImage(rgb);
450  depth = stereoModel_.left().rectifyImage(depth, 0);
451  model = stereoModel_.left();
452  }
453  }
454  else
455  {
456  if(stereoModel_.right().isValidForRectification())
457  {
458  rgb = stereoModel_.right().rectifyImage(rgb);
459  model = stereoModel_.right();
460 
461  if(stereoModel_.left().isValidForRectification() && !stereoModel_.stereoTransform().isNull())
462  {
463  depth = stereoModel_.left().rectifyImage(depth, 0);
464  depth = util2d::registerDepth(depth, stereoModel_.left().K(), rgb.size(), stereoModel_.right().K(), stereoModel_.stereoTransform());
465  }
466  }
467  }
468  model.setLocalTransform(this->getLocalTransform());
469 
470  data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
471 
472  double x=0,y=0,z=0;
473  freenectDevice_->getAccelerometerValues(x,y,z);
474  if(x != 0.0 && y != 0.0 && z != 0.0)
475  {
476  Transform opticalTransform(0,-1,0,0, 0,0,-1,0, 1,0,0,0);
477  Transform base = this->getLocalTransform()*opticalTransform;
478  // frame of imu on kinect is x->left, y->up, z->forward
479  data.setIMU(IMU(cv::Vec3d(0,0,0), cv::Mat(), cv::Vec3d(x, y, z), cv::Mat(), base*Transform(0,0,1,0, 1,0,0,0, 0,1,0,0)));
480  }
481  }
482  }
483  else
484  {
485  UERROR("CameraFreenect: Re-initialization needed!");
486  delete freenectDevice_;
487  freenectDevice_ = 0;
488  }
489  }
490 #else
491  UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
492 #endif
493  return data;
494 }
495 
496 
497 } // namespace rtabmap
NULL
#define NULL
freenect_context
struct _freenect_context freenect_context
Definition: CameraFreenect.h:36
f
f
rtabmap::Camera::getLocalTransform
const Transform & getLocalTransform() const
Definition: Camera.h:64
rtabmap_oanet.init
def init(descriptorDim, matchThreshold, iterations, cuda, model_path)
Definition: rtabmap_oanet.py:22
glm::scale
GLM_FUNC_DECL detail::tmat4x4< T, P > scale(detail::tmat4x4< T, P > const &m, detail::tvec3< T, P > const &v)
rtabmap::CameraFreenect::CameraFreenect
CameraFreenect(int deviceId=0, Type type=kTypeColorDepth, float imageRate=0.0f, const Transform &localTransform=CameraModel::opticalRotation())
Definition: CameraFreenect.cpp:293
rtabmap::util2d::registerDepth
cv::Mat RTABMAP_EXP registerDepth(const cv::Mat &depth, const cv::Mat &depthK, const cv::Size &colorSize, const cv::Mat &colorK, const rtabmap::Transform &transform)
Definition: util2d.cpp:1362
rtabmap::CameraFreenect::getSerial
virtual std::string getSerial() const
Definition: CameraFreenect.cpp:410
UASSERT
#define UASSERT(condition)
UMutex
Definition: UMutex.h:54
rtabmap::CameraFreenect::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraFreenect.cpp:421
rtabmap::CameraFreenect::isCalibrated
virtual bool isCalibrated() const
Definition: CameraFreenect.cpp:405
rtabmap_superglue.device
string device
Definition: rtabmap_superglue.py:21
state
RecoveryProgressState state
Definition: tools/Recovery/main.cpp:56
rtabmap::CameraModel::rectifyImage
cv::Mat rectifyImage(const cv::Mat &raw, int interpolation=cv::INTER_LINEAR) const
Definition: CameraModel.cpp:662
glm::uint32_t
detail::uint32 uint32_t
Definition: fwd.hpp:916
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::Camera::getNextSeqID
int getNextSeqID()
Definition: Camera.h:84
UERROR
#define UERROR(...)
UWARN
#define UWARN(...)
rtabmap::CameraFreenect::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraFreenect.cpp:326
UTimer::now
static double now()
Definition: UTimer.cpp:80
rtabmap::SensorData::setIMU
void setIMU(const IMU &imu)
Definition: SensorData.h:268
freenect_device
struct _freenect_device freenect_device
Definition: CameraFreenect.h:37
rtabmap::Transform::size
int size() const
Definition: Transform.h:90
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58