Visual Servoing Platform version 3.5.0
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
53vpKinect::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.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)
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
125void 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
151void 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
245void 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
301void dummy_vpKinect(){};
302#endif // VISP_HAVE_LIBFREENECT
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:131
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:88
unsigned int getWidth() const
Definition: vpImage.h:246
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
unsigned int getHeight() const
Definition: vpImage.h:188
void stop()
Definition: vpKinect.cpp:116
void warpRGBFrame(const vpImage< vpRGBa > &Irgb, const vpImage< float > &Idepth, vpImage< vpRGBa > &IrgbWarped)
Definition: vpKinect.cpp:245
bool getDepthMap(vpImage< float > &map)
Definition: vpKinect.cpp:173
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:231
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)
Class that allows protection by mutex.
Definition: vpMutex.h:171
void unlock()
Definition: vpMutex.h:111
void lock()
Definition: vpMutex.h:95
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:152
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)
#define vpERROR_TRACE
Definition: vpDebug.h:393