Visual Servoing Platform  version 3.6.1 under development (2024-04-22)
vpKinect.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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);
64  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.initPersProjWithDistortion(303.06, 297.89, 160.75, 117.9, -0.27, 0);
83  hd = 240;
84  wd = 320;
85  }
86  else {
87  std::cout << "vpKinect::start MEDIUM depth map resolution 480x640" << std::endl;
88 
89  IRcam.initPersProjWithDistortion(606.12, 595.78, 321.5, 235.8, -0.27, 0);
90 
91  hd = 480;
92  wd = 640;
93  }
94 
95 #if defined(VISP_HAVE_VIPER850_DATA) && defined(VISP_HAVE_PUGIXML)
96  vpXmlParserCamera cameraParser;
97  std::string cameraXmlFile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_camera_Viper850.xml");
98  cameraParser.parse(RGBcam, cameraXmlFile, "Generic-camera", vpCameraParameters::perspectiveProjWithDistortion, width,
99  height);
100 #else
101  // RGBcam.initPersProjWithoutDistortion(525.53, 524.94, 309.9, 282.8);//old
102  // RGBcam.initPersProjWithDistortion(536.76, 537.25, 313.45,
103  // 273.27,0.04,-0.04);//old
104  // RGBcam.initPersProjWithoutDistortion(512.0559503505,511.9352058050,310.6693938678,267.0673901049);//new
105  RGBcam.initPersProjWithDistortion(522.5431816996, 522.7191431808, 311.4001982614, 267.4283562142, 0.0477365207,
106  -0.0462326418); // new
107 #endif
108 
109  this->startVideo();
110  this->startDepth();
111 }
112 
114 {
115  this->stopVideo();
116  this->stopDepth();
117 }
118 
122 void vpKinect::VideoCallback(void *rgb, uint32_t /* timestamp */)
123 {
124  std::lock_guard<std::mutex> lock(m_rgb_mutex);
125  uint8_t *rgb_ = static_cast<uint8_t *>(rgb);
126  for (unsigned i = 0; i < height; i++) {
127  for (unsigned j = 0; j < width; j++) {
128  IRGB[i][j].R = rgb_[3 * (width * i + j) + 0];
129  IRGB[i][j].G = rgb_[3 * (width * i + j) + 1];
130  IRGB[i][j].B = rgb_[3 * (width * i + j) + 2];
131  }
132  }
133 
134  m_new_rgb_frame = true;
135 }
136 
147 void vpKinect::DepthCallback(void *depth, uint32_t /* timestamp */)
148 {
149  std::lock_guard<std::mutex> lock(m_depth_mutex);
150  uint16_t *depth_ = static_cast<uint16_t *>(depth);
151  for (unsigned i = 0; i < height; i++) {
152  for (unsigned j = 0; j < width; j++) {
153  dmap[i][j] =
154  0.1236f * tan(depth_[width * i + j] / 2842.5f + 1.1863f); // formula from
155  // http://openkinect.org/wiki/Imaging_Information
156  if (depth_[width * i + j] > 1023) { // Depth cannot be computed
157  dmap[i][j] = -1;
158  }
159  }
160  }
161  m_new_depth_map = true;
162  m_new_depth_image = true;
163 }
164 
169 {
170  std::lock_guard<std::mutex> lock(m_depth_mutex);
171  if (!m_new_depth_map)
172  return false;
173  map = this->dmap;
174  m_new_depth_map = false;
175  return true;
176 }
177 
182 {
183  vpImage<float> tempMap;
184  m_depth_mutex.lock();
185  if (!m_new_depth_map && !m_new_depth_image) {
186  m_depth_mutex.unlock();
187  return false;
188  }
189  tempMap = dmap;
190 
191  m_new_depth_map = false;
192  m_new_depth_image = false;
193  m_depth_mutex.unlock();
194 
195  if ((Imap.getHeight() != hd) || (map.getHeight() != hd))
196  vpERROR_TRACE(1, "Image size does not match vpKinect DM resolution");
197  if (DMres == DMAP_LOW_RES) {
198  for (unsigned int i = 0; i < hd; i++)
199  for (unsigned int j = 0; j < wd; j++) {
200  map[i][j] = tempMap[i << 1][j << 1];
201  // if (map[i][j] != -1)
202  if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
203  Imap[i][j] = (unsigned char)(255 * map[i][j] / 5);
204  else
205  Imap[i][j] = 255;
206  }
207  }
208  else {
209  for (unsigned i = 0; i < height; i++)
210  for (unsigned j = 0; j < width; j++) {
211  map[i][j] = tempMap[i][j];
212  // if (map[i][j] != -1)
213  if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
214  Imap[i][j] = (unsigned char)(255 * map[i][j] / 5);
215  else
216  Imap[i][j] = 255;
217  }
218  }
219 
220  return true;
221 }
222 
227 {
228  std::lock_guard<std::mutex> lock(m_rgb_mutex);
229  if (!m_new_rgb_frame)
230  return false;
231  I_RGB = this->IRGB;
232  m_new_rgb_frame = false;
233  return true;
234 }
235 
240 void vpKinect::warpRGBFrame(const vpImage<vpRGBa> &Irgb, const vpImage<float> &Idepth, vpImage<vpRGBa> &IrgbWarped)
241 {
242  if ((Idepth.getHeight() != hd) || (Idepth.getWidth() != wd)) {
243  vpERROR_TRACE(1, "Idepth image size does not match vpKinect DM resolution");
244  }
245  else {
246  if ((IrgbWarped.getHeight() != hd) || (IrgbWarped.getWidth() != wd))
247  IrgbWarped.resize(hd, wd);
248  IrgbWarped = 0;
249  double x1 = 0., y1 = 0., x2 = 0., y2 = 0., Z1, Z2;
250  vpImagePoint imgPoint(0, 0);
251  double u = 0., v = 0.;
252  vpColVector P1(4), P2(4);
253 
254  for (unsigned int i = 0; i < hd; i++)
255  for (unsigned int j = 0; j < wd; j++) {
257  vpPixelMeterConversion::convertPoint(IRcam, j, i, x1, y1);
258  Z1 = Idepth[i][j];
259  // if (Z1!=-1){
260  if (std::fabs(Z1 + 1) <= std::numeric_limits<double>::epsilon()) {
261  P1[0] = x1 * Z1;
262  P1[1] = y1 * Z1;
263  P1[2] = Z1;
264  P1[3] = 1;
265 
267  P2 = rgbMir * P1;
268  Z2 = P2[2];
269  // if (Z2!= 0){
270  if (std::fabs(Z2) > std::numeric_limits<double>::epsilon()) {
271  x2 = P2[0] / P2[2];
272  y2 = P2[1] / P2[2];
273  }
274  else
275  std::cout << "Z2 = 0 !!" << std::endl;
276 
279  vpMeterPixelConversion::convertPoint(RGBcam, x2, y2, u, v);
280 
281  unsigned int u_ = (unsigned int)u;
282  unsigned int v_ = (unsigned int)v;
284  if ((u_ < width) && (v_ < height)) {
285  IrgbWarped[i][j] = Irgb[v_][u_];
286  }
287  else
288  IrgbWarped[i][j] = 0;
289  }
290  }
291  }
292 }
293 
294 #elif !defined(VISP_BUILD_SHARED_LIBS)
295 // Work around to avoid warning: libvisp_sensor.a(vpKinect.cpp.o) has no symbols
296 void dummy_vpKinect() { };
297 #endif // VISP_HAVE_LIBFREENECT
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpHomogeneousMatrix inverse() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
unsigned int getWidth() const
Definition: vpImage.h:245
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:783
unsigned int getHeight() const
Definition: vpImage.h:184
void stop()
Definition: vpKinect.cpp:113
void warpRGBFrame(const vpImage< vpRGBa > &Irgb, const vpImage< float > &Idepth, vpImage< vpRGBa > &IrgbWarped)
Definition: vpKinect.cpp:240
bool getDepthMap(vpImage< float > &map)
Definition: vpKinect.cpp:168
void start(vpKinect::vpDMResolution res=DMAP_LOW_RES)
Definition: vpKinect.cpp:73
vpDMResolution
Definition: vpKinect.h:124
@ DMAP_LOW_RES
Definition: vpKinect.h:125
bool getRGB(vpImage< vpRGBa > &IRGB)
Definition: vpKinect.cpp:226
virtual ~vpKinect()
Definition: vpKinect.cpp:71
vpKinect(freenect_context *ctx, int index)
Definition: vpKinect.cpp:53
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189
XML parser to load and save intrinsic camera parameters.
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0, bool verbose=true)
#define vpERROR_TRACE
Definition: vpDebug.h:382