Visual Servoing Platform  version 3.6.1 under development (2024-12-12)
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/vpDebug.h>
48 #include <visp3/core/vpXmlParserCamera.h>
49 #include <visp3/sensor/vpKinect.h>
50 
51 BEGIN_VISP_NAMESPACE
55 vpKinect::vpKinect(freenect_context *ctx, int index)
56  : Freenect::FreenectDevice(ctx, index), m_rgb_mutex(), m_depth_mutex(), RGBcam(), IRcam(), rgbMir(), irMrgb(),
57  DMres(DMAP_LOW_RES), hd(240), wd(320), dmap(), IRGB(), m_new_rgb_frame(false), m_new_depth_map(false),
58  m_new_depth_image(false), height(480), width(640)
59 {
60  dmap.resize(height, width);
61  IRGB.resize(height, width);
62  vpPoseVector r(-0.0266, -0.0047, -0.0055, 0.0320578, 0.0169041,
63  -0.0076519);
66  rgbMir.buildFrom(r);
67  irMrgb = rgbMir.inverse();
68 }
69 
74 
76 {
77  DMres = res;
78  height = 480;
79  width = 640;
82  if (DMres == DMAP_LOW_RES) {
83  std::cout << "vpKinect::start LOW depth map resolution 240x320" << std::endl;
84  IRcam.initPersProjWithDistortion(303.06, 297.89, 160.75, 117.9, -0.27, 0);
85  hd = 240;
86  wd = 320;
87  }
88  else {
89  std::cout << "vpKinect::start MEDIUM depth map resolution 480x640" << std::endl;
90 
91  IRcam.initPersProjWithDistortion(606.12, 595.78, 321.5, 235.8, -0.27, 0);
92 
93  hd = 480;
94  wd = 640;
95  }
96 
97 #if defined(VISP_HAVE_VIPER850_DATA) && defined(VISP_HAVE_PUGIXML)
98  vpXmlParserCamera cameraParser;
99  std::string cameraXmlFile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_camera_Viper850.xml");
100  cameraParser.parse(RGBcam, cameraXmlFile, "Generic-camera", vpCameraParameters::perspectiveProjWithDistortion, width,
101  height);
102 #else
103  // RGBcam.initPersProjWithoutDistortion(525.53, 524.94, 309.9, 282.8);//old
104  // RGBcam.initPersProjWithDistortion(536.76, 537.25, 313.45,
105  // 273.27,0.04,-0.04);//old
106  // RGBcam.initPersProjWithoutDistortion(512.0559503505,511.9352058050,310.6693938678,267.0673901049);//new
107  RGBcam.initPersProjWithDistortion(522.5431816996, 522.7191431808, 311.4001982614, 267.4283562142, 0.0477365207,
108  -0.0462326418); // new
109 #endif
110 
111  this->startVideo();
112  this->startDepth();
113 }
114 
116 {
117  this->stopVideo();
118  this->stopDepth();
119 }
120 
124 void vpKinect::VideoCallback(void *rgb, uint32_t /* timestamp */)
125 {
126  std::lock_guard<std::mutex> lock(m_rgb_mutex);
127  uint8_t *rgb_ = static_cast<uint8_t *>(rgb);
128  for (unsigned i = 0; i < height; i++) {
129  for (unsigned j = 0; j < width; j++) {
130  IRGB[i][j].R = rgb_[3 * (width * i + j) + 0];
131  IRGB[i][j].G = rgb_[3 * (width * i + j) + 1];
132  IRGB[i][j].B = rgb_[3 * (width * i + j) + 2];
133  }
134  }
135 
136  m_new_rgb_frame = true;
137 }
138 
149 void vpKinect::DepthCallback(void *depth, uint32_t /* timestamp */)
150 {
151  std::lock_guard<std::mutex> lock(m_depth_mutex);
152  uint16_t *depth_ = static_cast<uint16_t *>(depth);
153  for (unsigned i = 0; i < height; i++) {
154  for (unsigned j = 0; j < width; j++) {
155  dmap[i][j] =
156  0.1236f * tan(depth_[width * i + j] / 2842.5f + 1.1863f); // formula from
157  // http://openkinect.org/wiki/Imaging_Information
158  if (depth_[width * i + j] > 1023) { // Depth cannot be computed
159  dmap[i][j] = -1;
160  }
161  }
162  }
163  m_new_depth_map = true;
164  m_new_depth_image = true;
165 }
166 
171 {
172  std::lock_guard<std::mutex> lock(m_depth_mutex);
173  if (!m_new_depth_map)
174  return false;
175  map = this->dmap;
176  m_new_depth_map = false;
177  return true;
178 }
179 
184 {
185  vpImage<float> tempMap;
186  m_depth_mutex.lock();
187  if (!m_new_depth_map && !m_new_depth_image) {
188  m_depth_mutex.unlock();
189  return false;
190  }
191  tempMap = dmap;
192 
193  m_new_depth_map = false;
194  m_new_depth_image = false;
195  m_depth_mutex.unlock();
196 
197  if ((Imap.getHeight() != hd) || (map.getHeight() != hd))
198  vpERROR_TRACE(1, "Image size does not match vpKinect DM resolution");
199  if (DMres == DMAP_LOW_RES) {
200  for (unsigned int i = 0; i < hd; i++)
201  for (unsigned int j = 0; j < wd; j++) {
202  map[i][j] = tempMap[i << 1][j << 1];
203  // if (map[i][j] != -1)
204  if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
205  Imap[i][j] = (unsigned char)(255 * map[i][j] / 5);
206  else
207  Imap[i][j] = 255;
208  }
209  }
210  else {
211  for (unsigned i = 0; i < height; i++)
212  for (unsigned j = 0; j < width; j++) {
213  map[i][j] = tempMap[i][j];
214  // if (map[i][j] != -1)
215  if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
216  Imap[i][j] = (unsigned char)(255 * map[i][j] / 5);
217  else
218  Imap[i][j] = 255;
219  }
220  }
221 
222  return true;
223 }
224 
229 {
230  std::lock_guard<std::mutex> lock(m_rgb_mutex);
231  if (!m_new_rgb_frame)
232  return false;
233  I_RGB = this->IRGB;
234  m_new_rgb_frame = false;
235  return true;
236 }
237 
242 void vpKinect::warpRGBFrame(const vpImage<vpRGBa> &Irgb, const vpImage<float> &Idepth, vpImage<vpRGBa> &IrgbWarped)
243 {
244  if ((Idepth.getHeight() != hd) || (Idepth.getWidth() != wd)) {
245  vpERROR_TRACE(1, "Idepth image size does not match vpKinect DM resolution");
246  }
247  else {
248  if ((IrgbWarped.getHeight() != hd) || (IrgbWarped.getWidth() != wd))
249  IrgbWarped.resize(hd, wd);
250  IrgbWarped = 0;
251  double x1 = 0., y1 = 0., x2 = 0., y2 = 0., Z1, Z2;
252  vpImagePoint imgPoint(0, 0);
253  double u = 0., v = 0.;
254  vpColVector P1(4), P2(4);
255 
256  for (unsigned int i = 0; i < hd; i++)
257  for (unsigned int j = 0; j < wd; j++) {
259  vpPixelMeterConversion::convertPoint(IRcam, j, i, x1, y1);
260  Z1 = Idepth[i][j];
261  // if (Z1!=-1){
262  if (std::fabs(Z1 + 1) <= std::numeric_limits<double>::epsilon()) {
263  P1[0] = x1 * Z1;
264  P1[1] = y1 * Z1;
265  P1[2] = Z1;
266  P1[3] = 1;
267 
269  P2 = rgbMir * P1;
270  Z2 = P2[2];
271  // if (Z2!= 0){
272  if (std::fabs(Z2) > std::numeric_limits<double>::epsilon()) {
273  x2 = P2[0] / P2[2];
274  y2 = P2[1] / P2[2];
275  }
276  else
277  std::cout << "Z2 = 0 !!" << std::endl;
278 
281  vpMeterPixelConversion::convertPoint(RGBcam, x2, y2, u, v);
282 
283  unsigned int u_ = (unsigned int)u;
284  unsigned int v_ = (unsigned int)v;
286  if ((u_ < width) && (v_ < height)) {
287  IrgbWarped[i][j] = Irgb[v_][u_];
288  }
289  else
290  IrgbWarped[i][j] = 0;
291  }
292  }
293  }
294 }
295 END_VISP_NAMESPACE
296 #elif !defined(VISP_BUILD_SHARED_LIBS)
297 // Work around to avoid warning: libvisp_sensor.a(vpKinect.cpp.o) has no symbols
298 void dummy_vpKinect() { };
299 #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:191
vpHomogeneousMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
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:242
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:544
unsigned int getHeight() const
Definition: vpImage.h:181
void stop()
Definition: vpKinect.cpp:115
void warpRGBFrame(const vpImage< vpRGBa > &Irgb, const vpImage< float > &Idepth, vpImage< vpRGBa > &IrgbWarped)
Definition: vpKinect.cpp:242
bool getDepthMap(vpImage< float > &map)
Definition: vpKinect.cpp:170
void start(vpKinect::vpDMResolution res=DMAP_LOW_RES)
Definition: vpKinect.cpp:75
vpKinect(freenect_context *ctx, int index)
Definition: vpKinect.cpp:55
vpDMResolution
Definition: vpKinect.h:130
@ DMAP_LOW_RES
Definition: vpKinect.h:131
bool getRGB(vpImage< vpRGBa > &IRGB)
Definition: vpKinect.cpp:228
virtual ~vpKinect()
Definition: vpKinect.cpp:73
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:203
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)