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;
89 std::cout <<
"vpKinect::start MEDIUM depth map resolution 480x640" << std::endl;
98 #if defined(VISP_HAVE_VIPER850_DATA) && defined(VISP_HAVE_XML2) 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 Class that allows protection by mutex.
void start(vpKinect::vpDMResolution res=DMAP_LOW_RES)
unsigned int getWidth() const
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
XML parser to load and save intrinsic camera parameters.
void resize(const unsigned int h, const unsigned int w)
resize the image : Image initialization
bool getRGB(vpImage< vpRGBa > &IRGB)
void warpRGBFrame(const vpImage< vpRGBa > &Irgb, const vpImage< float > &Idepth, vpImage< vpRGBa > &IrgbWarped)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
vpHomogeneousMatrix inverse() const
unsigned int getHeight() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
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)
void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0, const double kud, const double kdu)
vpKinect(freenect_context *ctx, int index)