40 #include <visp3/core/vpConfig.h>
43 #if defined(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES)
47 #include <visp3/core/vpXmlParserCamera.h>
48 #include <visp3/sensor/vpKinect.h>
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)
58 dmap.resize(height, width);
59 IRGB.resize(height, width);
60 vpPoseVector r(-0.0266, -0.0047, -0.0055, 0.0320578, 0.0169041,
65 irMrgb = rgbMir.inverse();
81 std::cout <<
"vpKinect::start LOW depth map resolution 240x320" << std::endl;
89 std::cout <<
"vpKinect::start MEDIUM depth map resolution 480x640" << std::endl;
98 #if defined(VISP_HAVE_VIPER850_DATA) && defined(VISP_HAVE_PUGIXML)
100 std::string cameraXmlFile = std::string(VISP_VIPER850_DATA_PATH) + std::string(
"/include/const_camera_Viper850.xml");
125 void vpKinect::VideoCallback(
void *rgb, uint32_t )
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];
138 m_new_rgb_frame =
true;
151 void vpKinect::DepthCallback(
void *depth, uint32_t )
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++) {
159 0.1236f * tan(depth_[width * i + j] / 2842.5f + 1.1863f);
161 if (depth_[width * i + j] > 1023) {
166 m_new_depth_map =
true;
167 m_new_depth_image =
true;
176 if (!m_new_depth_map)
179 m_new_depth_map =
false;
190 m_depth_mutex.
lock();
191 if (!m_new_depth_map && !m_new_depth_image) {
197 m_new_depth_map =
false;
198 m_new_depth_image =
false;
202 vpERROR_TRACE(1,
"Image size does not match vpKinect DM resolution");
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];
208 if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
209 Imap[i][j] = (
unsigned char)(255 * map[i][j] / 5);
214 for (
unsigned i = 0; i < height; i++)
215 for (
unsigned j = 0; j < width; j++) {
216 map[i][j] = tempMap[i][j];
218 if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
219 Imap[i][j] = (
unsigned char)(255 * map[i][j] / 5);
234 if (!m_new_rgb_frame)
237 m_new_rgb_frame =
false;
248 vpERROR_TRACE(1,
"Idepth image size does not match vpKinect DM resolution");
251 IrgbWarped.
resize(hd, wd);
253 double x1 = 0., y1 = 0., x2 = 0., y2 = 0., Z1, Z2;
255 double u = 0., v = 0.;
260 for (
unsigned int i = 0; i < hd; i++)
261 for (
unsigned int j = 0; j < wd; j++) {
266 if (std::fabs(Z1 + 1) <= std::numeric_limits<double>::epsilon()) {
276 if (std::fabs(Z2) > std::numeric_limits<double>::epsilon()) {
280 std::cout <<
"Z2 = 0 !!" << std::endl;
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_];
292 IrgbWarped[i][j] = 0;
298 #elif !defined(VISP_BUILD_SHARED_LIBS)
301 void dummy_vpKinect(){};
302 #endif // VISP_HAVE_LIBFREENECT