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;
87 std::cout <<
"vpKinect::start MEDIUM depth map resolution 480x640" << std::endl;
95 #if defined(VISP_HAVE_VIPER850_DATA) && defined(VISP_HAVE_PUGIXML)
97 std::string cameraXmlFile = std::string(VISP_VIPER850_DATA_PATH) + std::string(
"/include/const_camera_Viper850.xml");
122 void vpKinect::VideoCallback(
void *rgb, uint32_t )
124 std::lock_guard<std::mutex> lock(m_rgb_mutex);
125 uint8_t *rgb_ =
static_cast<uint8_t *
>(rgb);
126 for (
unsigned i = 0; i < height; i++) {
127 for (
unsigned j = 0; j < width; j++) {
128 IRGB[i][j].R = rgb_[3 * (width * i + j) + 0];
129 IRGB[i][j].G = rgb_[3 * (width * i + j) + 1];
130 IRGB[i][j].B = rgb_[3 * (width * i + j) + 2];
134 m_new_rgb_frame =
true;
147 void vpKinect::DepthCallback(
void *depth, uint32_t )
149 std::lock_guard<std::mutex> lock(m_depth_mutex);
150 uint16_t *depth_ =
static_cast<uint16_t *
>(depth);
151 for (
unsigned i = 0; i < height; i++) {
152 for (
unsigned j = 0; j < width; j++) {
154 0.1236f * tan(depth_[width * i + j] / 2842.5f + 1.1863f);
156 if (depth_[width * i + j] > 1023) {
161 m_new_depth_map =
true;
162 m_new_depth_image =
true;
170 std::lock_guard<std::mutex> lock(m_depth_mutex);
171 if (!m_new_depth_map)
174 m_new_depth_map =
false;
184 m_depth_mutex.lock();
185 if (!m_new_depth_map && !m_new_depth_image) {
186 m_depth_mutex.unlock();
191 m_new_depth_map =
false;
192 m_new_depth_image =
false;
193 m_depth_mutex.unlock();
196 vpERROR_TRACE(1,
"Image size does not match vpKinect DM resolution");
198 for (
unsigned int i = 0; i < hd; i++)
199 for (
unsigned int j = 0; j < wd; j++) {
200 map[i][j] = tempMap[i << 1][j << 1];
202 if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
203 Imap[i][j] = (
unsigned char)(255 * map[i][j] / 5);
209 for (
unsigned i = 0; i < height; i++)
210 for (
unsigned j = 0; j < width; j++) {
211 map[i][j] = tempMap[i][j];
213 if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
214 Imap[i][j] = (
unsigned char)(255 * map[i][j] / 5);
228 std::lock_guard<std::mutex> lock(m_rgb_mutex);
229 if (!m_new_rgb_frame)
232 m_new_rgb_frame =
false;
243 vpERROR_TRACE(1,
"Idepth image size does not match vpKinect DM resolution");
247 IrgbWarped.
resize(hd, wd);
249 double x1 = 0., y1 = 0., x2 = 0., y2 = 0., Z1, Z2;
251 double u = 0., v = 0.;
254 for (
unsigned int i = 0; i < hd; i++)
255 for (
unsigned int j = 0; j < wd; j++) {
260 if (std::fabs(Z1 + 1) <= std::numeric_limits<double>::epsilon()) {
270 if (std::fabs(Z2) > std::numeric_limits<double>::epsilon()) {
275 std::cout <<
"Z2 = 0 !!" << std::endl;
281 unsigned int u_ = (
unsigned int)u;
282 unsigned int v_ = (
unsigned int)v;
284 if ((u_ < width) && (v_ < height)) {
285 IrgbWarped[i][j] = Irgb[v_][u_];
288 IrgbWarped[i][j] = 0;
294 #elif !defined(VISP_BUILD_SHARED_LIBS)
296 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)
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)