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,
81 std::cout <<
"vpKinect::start LOW depth map resolution 240x320" << std::endl;
86 std::cout <<
"vpKinect::start MEDIUM depth map resolution 480x640" << std::endl;
94 #if defined(VISP_HAVE_VIPER850_DATA)
96 std::string cameraXmlFile = std::string(VISP_VIPER850_DATA_PATH) + std::string(
"/include/const_camera_Viper850.xml");
121 void vpKinect::VideoCallback(
void *rgb, uint32_t )
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];
133 m_new_rgb_frame =
true;
146 void vpKinect::DepthCallback(
void *depth, uint32_t )
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++) {
153 0.1236f * tan(depth_[width * i + j] / 2842.5f + 1.1863f);
155 if (depth_[width * i + j] > 1023) {
160 m_new_depth_map =
true;
161 m_new_depth_image =
true;
170 if (!m_new_depth_map)
173 m_new_depth_map =
false;
183 m_depth_mutex.
lock();
184 if (!m_new_depth_map && !m_new_depth_image) {
190 m_new_depth_map =
false;
191 m_new_depth_image =
false;
195 vpERROR_TRACE(1,
"Image size does not match vpKinect DM resolution");
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];
201 if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
202 Imap[i][j] = (
unsigned char)(255 * map[i][j] / 5);
207 for (
unsigned i = 0; i < height; i++)
208 for (
unsigned j = 0; j < width; j++) {
209 map[i][j] = tempMap[i][j];
211 if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
212 Imap[i][j] = (
unsigned char)(255 * map[i][j] / 5);
227 if (!m_new_rgb_frame)
230 m_new_rgb_frame =
false;
241 vpERROR_TRACE(1,
"Idepth image size does not match vpKinect DM resolution");
244 IrgbWarped.
resize(hd, wd);
246 double x1 = 0., y1 = 0., x2 = 0., y2 = 0., Z1, Z2;
248 double u = 0., v = 0.;
251 for (
unsigned int i = 0; i < hd; i++)
252 for (
unsigned int j = 0; j < wd; j++) {
257 if (std::fabs(Z1 + 1) <= std::numeric_limits<double>::epsilon()) {
267 if (std::fabs(Z2) > std::numeric_limits<double>::epsilon()) {
271 std::cout <<
"Z2 = 0 !!" << std::endl;
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_];
283 IrgbWarped[i][j] = 0;
289 #elif !defined(VISP_BUILD_SHARED_LIBS)
292 void dummy_vpKinect(){};
@ 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 ...
unsigned int getWidth() const
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
unsigned int getHeight() const
void warpRGBFrame(const vpImage< vpRGBa > &Irgb, const vpImage< float > &Idepth, vpImage< vpRGBa > &IrgbWarped)
bool getDepthMap(vpImage< float > &map)
void start(vpKinect::vpDMResolution res=DMAP_LOW_RES)
bool getRGB(vpImage< vpRGBa > &IRGB)
vpKinect(freenect_context *ctx, int index)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class that allows protection by mutex.
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)