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