Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
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.initPersProjWithDistortion(303.06, 297.89, 160.75, 117.9, -0.27, 0);
83 hd = 240;
84 wd = 320;
85 } else {
86 std::cout << "vpKinect::start MEDIUM depth map resolution 480x640" << std::endl;
87
88 IRcam.initPersProjWithDistortion(606.12, 595.78, 321.5, 235.8, -0.27, 0);
89
90 hd = 480;
91 wd = 640;
92 }
93
94#if defined(VISP_HAVE_VIPER850_DATA)
95 vpXmlParserCamera cameraParser;
96 std::string cameraXmlFile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_camera_Viper850.xml");
97 cameraParser.parse(RGBcam, cameraXmlFile, "Generic-camera", vpCameraParameters::perspectiveProjWithDistortion, width,
98 height);
99#else
100 // RGBcam.initPersProjWithoutDistortion(525.53, 524.94, 309.9, 282.8);//old
101 // RGBcam.initPersProjWithDistortion(536.76, 537.25, 313.45,
102 // 273.27,0.04,-0.04);//old
103 // RGBcam.initPersProjWithoutDistortion(512.0559503505,511.9352058050,310.6693938678,267.0673901049);//new
104 RGBcam.initPersProjWithDistortion(522.5431816996, 522.7191431808, 311.4001982614, 267.4283562142, 0.0477365207,
105 -0.0462326418); // new
106#endif
107
108 this->startVideo();
109 this->startDepth();
110}
111
113{
114 this->stopVideo();
115 this->stopDepth();
116}
117
121void vpKinect::VideoCallback(void *rgb, uint32_t /* timestamp */)
122{
123 vpMutex::vpScopedLock lock(m_rgb_mutex);
124 uint8_t *rgb_ = static_cast<uint8_t *>(rgb);
125 for (unsigned i = 0; i < height; i++) {
126 for (unsigned j = 0; j < width; j++) {
127 IRGB[i][j].R = rgb_[3 * (width * i + j) + 0];
128 IRGB[i][j].G = rgb_[3 * (width * i + j) + 1];
129 IRGB[i][j].B = rgb_[3 * (width * i + j) + 2];
130 }
131 }
132
133 m_new_rgb_frame = true;
134}
135
146void vpKinect::DepthCallback(void *depth, uint32_t /* timestamp */)
147{
148 vpMutex::vpScopedLock lock(m_depth_mutex);
149 uint16_t *depth_ = static_cast<uint16_t *>(depth);
150 for (unsigned i = 0; i < height; i++) {
151 for (unsigned j = 0; j < width; j++) {
152 dmap[i][j] =
153 0.1236f * tan(depth_[width * i + j] / 2842.5f + 1.1863f); // formula from
154 // http://openkinect.org/wiki/Imaging_Information
155 if (depth_[width * i + j] > 1023) { // Depth cannot be computed
156 dmap[i][j] = -1;
157 }
158 }
159 }
160 m_new_depth_map = true;
161 m_new_depth_image = true;
162}
163
168{
169 vpMutex::vpScopedLock lock(m_depth_mutex);
170 if (!m_new_depth_map)
171 return false;
172 map = this->dmap;
173 m_new_depth_map = false;
174 return true;
175}
176
181{
182 vpImage<float> tempMap;
183 m_depth_mutex.lock();
184 if (!m_new_depth_map && !m_new_depth_image) {
185 m_depth_mutex.unlock();
186 return false;
187 }
188 tempMap = dmap;
189
190 m_new_depth_map = false;
191 m_new_depth_image = false;
192 m_depth_mutex.unlock();
193
194 if ((Imap.getHeight() != hd) || (map.getHeight() != hd))
195 vpERROR_TRACE(1, "Image size does not match vpKinect DM resolution");
196 if (DMres == DMAP_LOW_RES) {
197 for (unsigned int i = 0; i < hd; i++)
198 for (unsigned int j = 0; j < wd; j++) {
199 map[i][j] = tempMap[i << 1][j << 1];
200 // if (map[i][j] != -1)
201 if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
202 Imap[i][j] = (unsigned char)(255 * map[i][j] / 5);
203 else
204 Imap[i][j] = 255;
205 }
206 } else {
207 for (unsigned i = 0; i < height; i++)
208 for (unsigned j = 0; j < width; j++) {
209 map[i][j] = tempMap[i][j];
210 // if (map[i][j] != -1)
211 if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
212 Imap[i][j] = (unsigned char)(255 * map[i][j] / 5);
213 else
214 Imap[i][j] = 255;
215 }
216 }
217
218 return true;
219}
220
225{
226 vpMutex::vpScopedLock lock(m_rgb_mutex);
227 if (!m_new_rgb_frame)
228 return false;
229 I_RGB = this->IRGB;
230 m_new_rgb_frame = false;
231 return true;
232}
233
238void vpKinect::warpRGBFrame(const vpImage<vpRGBa> &Irgb, const vpImage<float> &Idepth, vpImage<vpRGBa> &IrgbWarped)
239{
240 if ((Idepth.getHeight() != hd) || (Idepth.getWidth() != wd)) {
241 vpERROR_TRACE(1, "Idepth image size does not match vpKinect DM resolution");
242 } else {
243 if ((IrgbWarped.getHeight() != hd) || (IrgbWarped.getWidth() != wd))
244 IrgbWarped.resize(hd, wd);
245 IrgbWarped = 0;
246 double x1 = 0., y1 = 0., x2 = 0., y2 = 0., Z1, Z2;
247 vpImagePoint imgPoint(0, 0);
248 double u = 0., v = 0.;
249 vpColVector P1(4), P2(4);
250
251 for (unsigned int i = 0; i < hd; i++)
252 for (unsigned int j = 0; j < wd; j++) {
254 vpPixelMeterConversion::convertPoint(IRcam, j, i, x1, y1);
255 Z1 = Idepth[i][j];
256 // if (Z1!=-1){
257 if (std::fabs(Z1 + 1) <= std::numeric_limits<double>::epsilon()) {
258 P1[0] = x1 * Z1;
259 P1[1] = y1 * Z1;
260 P1[2] = Z1;
261 P1[3] = 1;
262
264 P2 = rgbMir * P1;
265 Z2 = P2[2];
266 // if (Z2!= 0){
267 if (std::fabs(Z2) > std::numeric_limits<double>::epsilon()) {
268 x2 = P2[0] / P2[2];
269 y2 = P2[1] / P2[2];
270 } else
271 std::cout << "Z2 = 0 !!" << std::endl;
272
275 vpMeterPixelConversion::convertPoint(RGBcam, x2, y2, u, v);
276
277 unsigned int u_ = (unsigned int)u;
278 unsigned int v_ = (unsigned int)v;
280 if ((u_ < width) && (v_ < height)) {
281 IrgbWarped[i][j] = Irgb[v_][u_];
282 } else
283 IrgbWarped[i][j] = 0;
284 }
285 }
286 }
287}
288
289#elif !defined(VISP_BUILD_SHARED_LIBS)
290// Work around to avoid warning: libvisp_sensor.a(vpKinect.cpp.o) has no
291// symbols
292void dummy_vpKinect(){};
293#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.
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 of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition vpImage.h:795
unsigned int getHeight() const
Definition vpImage.h:184
void stop()
Definition vpKinect.cpp:112
void warpRGBFrame(const vpImage< vpRGBa > &Irgb, const vpImage< float > &Idepth, vpImage< vpRGBa > &IrgbWarped)
Definition vpKinect.cpp:238
bool getDepthMap(vpImage< float > &map)
Definition vpKinect.cpp:167
void start(vpKinect::vpDMResolution res=DMAP_LOW_RES)
Definition vpKinect.cpp:73
@ DMAP_LOW_RES
Definition vpKinect.h:125
bool getRGB(vpImage< vpRGBa > &IRGB)
Definition vpKinect.cpp:224
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:166
void unlock()
Definition vpMutex.h:106
void lock()
Definition vpMutex.h:90
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.
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:388