Visual Servoing Platform  version 3.2.1 under development (2019-05-20)
vpKinect.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * API for using a Microsoft Kinect device
33  * Requires libfreenect as a third party library
34  *
35  * Authors:
36  * Celine Teuliere
37  *
38  *****************************************************************************/
39 
40 #include <visp3/core/vpConfig.h>
41 
42 // Note that libfreenect needs libusb-1.0 and libpthread
43 #if defined(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES)
44 
45 #include <limits> // numeric_limits
46 
47 #include <visp3/core/vpXmlParserCamera.h>
48 #include <visp3/sensor/vpKinect.h>
49 
53 vpKinect::vpKinect(freenect_context *ctx, int index)
54  : Freenect::FreenectDevice(ctx, index), m_rgb_mutex(), m_depth_mutex(), RGBcam(), IRcam(), rgbMir(), irMrgb(),
55  DMres(DMAP_LOW_RES), hd(240), wd(320), dmap(), IRGB(), m_new_rgb_frame(false), m_new_depth_map(false),
56  m_new_depth_image(false), height(480), width(640)
57 {
58  dmap.resize(height, width);
59  IRGB.resize(height, width);
60  vpPoseVector r(-0.0266, -0.0047, -0.0055, 0.0320578, 0.0169041,
61  -0.0076519);
62  rgbMir.buildFrom(r);
65  irMrgb = rgbMir.inverse();
66 }
67 
72 
74 {
75  DMres = res;
76  height = 480;
77  width = 640;
80  if (DMres == DMAP_LOW_RES) {
81  std::cout << "vpKinect::start LOW depth map resolution 240x320" << std::endl;
82  // IRcam.setparameters(IRcam.get_px()/2, IRcam.get_py()/2,
83  // IRcam.get_u0()/2, IRcam.get_v0()/2);
84  // IRcam.initPersProjWithoutDistortion(303.06,297.89,160.75,117.9);
85  IRcam.initPersProjWithDistortion(303.06, 297.89, 160.75, 117.9, -0.27, 0);
86  hd = 240;
87  wd = 320;
88  } else {
89  std::cout << "vpKinect::start MEDIUM depth map resolution 480x640" << std::endl;
90 
91  // IRcam.initPersProjWithoutDistortion(606.12,595.78,321.5,235.8);
92  IRcam.initPersProjWithDistortion(606.12, 595.78, 321.5, 235.8, -0.27, 0);
93  // Idmap.resize(height, width);
94  hd = 480;
95  wd = 640;
96  }
97 
98 #if defined(VISP_HAVE_VIPER850_DATA) && defined(VISP_HAVE_PUGIXML)
99  vpXmlParserCamera cameraParser;
100  std::string cameraXmlFile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_camera_Viper850.xml");
101  cameraParser.parse(RGBcam, cameraXmlFile, "Generic-camera", vpCameraParameters::perspectiveProjWithDistortion, width,
102  height);
103 #else
104  // RGBcam.initPersProjWithoutDistortion(525.53, 524.94, 309.9, 282.8);//old
105  // RGBcam.initPersProjWithDistortion(536.76, 537.25, 313.45,
106  // 273.27,0.04,-0.04);//old
107  // RGBcam.initPersProjWithoutDistortion(512.0559503505,511.9352058050,310.6693938678,267.0673901049);//new
108  RGBcam.initPersProjWithDistortion(522.5431816996, 522.7191431808, 311.4001982614, 267.4283562142, 0.0477365207,
109  -0.0462326418); // new
110 #endif
111 
112  this->startVideo();
113  this->startDepth();
114 }
115 
117 {
118  this->stopVideo();
119  this->stopDepth();
120 }
121 
125 void vpKinect::VideoCallback(void *rgb, uint32_t /* timestamp */)
126 {
127  // std::cout << "vpKinect Video callback" << std::endl;
128  vpMutex::vpScopedLock lock(m_rgb_mutex);
129  uint8_t *rgb_ = static_cast<uint8_t *>(rgb);
130  for (unsigned i = 0; i < height; i++) {
131  for (unsigned j = 0; j < width; j++) {
132  IRGB[i][j].R = rgb_[3 * (width * i + j) + 0];
133  IRGB[i][j].G = rgb_[3 * (width * i + j) + 1];
134  IRGB[i][j].B = rgb_[3 * (width * i + j) + 2];
135  }
136  }
137 
138  m_new_rgb_frame = true;
139 }
140 
151 void vpKinect::DepthCallback(void *depth, uint32_t /* timestamp */)
152 {
153  // std::cout << "vpKinect Depth callback" << std::endl;
154  vpMutex::vpScopedLock lock(m_depth_mutex);
155  uint16_t *depth_ = static_cast<uint16_t *>(depth);
156  for (unsigned i = 0; i < height; i++) {
157  for (unsigned j = 0; j < width; j++) {
158  dmap[i][j] =
159  0.1236f * tan(depth_[width * i + j] / 2842.5f + 1.1863f); // formula from
160  // http://openkinect.org/wiki/Imaging_Information
161  if (depth_[width * i + j] > 1023) { // Depth cannot be computed
162  dmap[i][j] = -1;
163  }
164  }
165  }
166  m_new_depth_map = true;
167  m_new_depth_image = true;
168 }
169 
174 {
175  vpMutex::vpScopedLock lock(m_depth_mutex);
176  if (!m_new_depth_map)
177  return false;
178  map = this->dmap;
179  m_new_depth_map = false;
180  return true;
181 }
182 
187 {
188  // vpMutex::vpScopedLock lock(m_depth_mutex);
189  vpImage<float> tempMap;
190  m_depth_mutex.lock();
191  if (!m_new_depth_map && !m_new_depth_image) {
192  m_depth_mutex.unlock();
193  return false;
194  }
195  tempMap = dmap;
196 
197  m_new_depth_map = false;
198  m_new_depth_image = false;
199  m_depth_mutex.unlock();
200 
201  if ((Imap.getHeight() != hd) || (map.getHeight() != hd))
202  vpERROR_TRACE(1, "Image size does not match vpKinect DM resolution");
203  if (DMres == DMAP_LOW_RES) {
204  for (unsigned int i = 0; i < hd; i++)
205  for (unsigned int j = 0; j < wd; j++) {
206  map[i][j] = tempMap[i << 1][j << 1];
207  // if (map[i][j] != -1)
208  if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
209  Imap[i][j] = (unsigned char)(255 * map[i][j] / 5);
210  else
211  Imap[i][j] = 255;
212  }
213  } else {
214  for (unsigned i = 0; i < height; i++)
215  for (unsigned j = 0; j < width; j++) {
216  map[i][j] = tempMap[i][j];
217  // if (map[i][j] != -1)
218  if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
219  Imap[i][j] = (unsigned char)(255 * map[i][j] / 5);
220  else
221  Imap[i][j] = 255;
222  }
223  }
224 
225  return true;
226 }
227 
232 {
233  vpMutex::vpScopedLock lock(m_rgb_mutex);
234  if (!m_new_rgb_frame)
235  return false;
236  I_RGB = this->IRGB;
237  m_new_rgb_frame = false;
238  return true;
239 }
240 
245 void vpKinect::warpRGBFrame(const vpImage<vpRGBa> &Irgb, const vpImage<float> &Idepth, vpImage<vpRGBa> &IrgbWarped)
246 {
247  if ((Idepth.getHeight() != hd) || (Idepth.getWidth() != wd)) {
248  vpERROR_TRACE(1, "Idepth image size does not match vpKinect DM resolution");
249  } else {
250  if ((IrgbWarped.getHeight() != hd) || (IrgbWarped.getWidth() != wd))
251  IrgbWarped.resize(hd, wd);
252  IrgbWarped = 0;
253  double x1 = 0., y1 = 0., x2 = 0., y2 = 0., Z1, Z2;
254  vpImagePoint imgPoint(0, 0);
255  double u = 0., v = 0.;
256  vpColVector P1(4), P2(4);
257 
258  // std::cout <<"rgbMir : "<<rgbMir<<std::endl;
259 
260  for (unsigned int i = 0; i < hd; i++)
261  for (unsigned int j = 0; j < wd; j++) {
263  vpPixelMeterConversion::convertPoint(IRcam, j, i, x1, y1);
264  Z1 = Idepth[i][j];
265  // if (Z1!=-1){
266  if (std::fabs(Z1 + 1) <= std::numeric_limits<double>::epsilon()) {
267  P1[0] = x1 * Z1;
268  P1[1] = y1 * Z1;
269  P1[2] = Z1;
270  P1[3] = 1;
271 
273  P2 = rgbMir * P1;
274  Z2 = P2[2];
275  // if (Z2!= 0){
276  if (std::fabs(Z2) > std::numeric_limits<double>::epsilon()) {
277  x2 = P2[0] / P2[2];
278  y2 = P2[1] / P2[2];
279  } else
280  std::cout << "Z2 = 0 !!" << std::endl;
281 
284  vpMeterPixelConversion::convertPoint(RGBcam, x2, y2, u, v);
285 
286  unsigned int u_ = (unsigned int)u;
287  unsigned int v_ = (unsigned int)v;
289  if ((u_ < width) && (v_ < height)) {
290  IrgbWarped[i][j] = Irgb[v_][u_];
291  } else
292  IrgbWarped[i][j] = 0;
293  }
294  }
295  }
296 }
297 
298 #elif !defined(VISP_BUILD_SHARED_LIBS)
299 // Work arround to avoid warning: libvisp_sensor.a(vpKinect.cpp.o) has no
300 // symbols
301 void dummy_vpKinect(){};
302 #endif // VISP_HAVE_LIBFREENECT
Class that allows protection by mutex.
Definition: vpMutex.h:170
void start(vpKinect::vpDMResolution res=DMAP_LOW_RES)
Definition: vpKinect.cpp:73
vpDMResolution
Definition: vpKinect.h:125
void lock()
Definition: vpMutex.h:95
unsigned int getWidth() const
Definition: vpImage.h:237
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
#define vpERROR_TRACE
Definition: vpDebug.h:393
void unlock()
Definition: vpMutex.h:111
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
XML parser to load and save intrinsic camera parameters.
void resize(const unsigned int h, const unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:863
bool getRGB(vpImage< vpRGBa > &IRGB)
Definition: vpKinect.cpp:231
void warpRGBFrame(const vpImage< vpRGBa > &Irgb, const vpImage< float > &Idepth, vpImage< vpRGBa > &IrgbWarped)
Definition: vpKinect.cpp:245
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
vpHomogeneousMatrix inverse() const
void stop()
Definition: vpKinect.cpp:116
unsigned int getHeight() const
Definition: vpImage.h:179
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, const unsigned int image_width=0, const unsigned int image_height=0)
bool getDepthMap(vpImage< float > &map)
Definition: vpKinect.cpp:173
void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0, const double kud, const double kdu)
virtual ~vpKinect()
Definition: vpKinect.cpp:71
vpKinect(freenect_context *ctx, int index)
Definition: vpKinect.cpp:53