44 #include <visp/vpConfig.h>
47 #if defined(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES)
51 #include <visp/vpKinect.h>
52 #include <visp/vpXmlParserCamera.h>
58 : Freenect::FreenectDevice(ctx, index),
60 m_new_rgb_frame(false),
61 m_new_depth_map(false),
62 m_new_depth_image(false),
63 height(480), width(640)
65 dmap.
resize(height, width);
66 IRGB.
resize(height, width);
67 vpPoseVector r(-0.0266,-0.0047,-0.0055,0.0320578,0.0169041,-0.0076519 );
87 std::cout <<
"vpKinect::start LOW depth map resolution 240x320" << std::endl;
96 std::cout <<
"vpKinect::start MEDIUM depth map resolution 480x640" << std::endl;
105 #if defined(VISP_HAVE_ACCESS_TO_NAS) && defined(VISP_HAVE_XML2)
107 char cameraXmlFile[FILENAME_MAX];
108 sprintf(cameraXmlFile,
"/udd/fspindle/robot/Viper850/Viper850-code/include/const_camera_Viper850.xml");
131 void vpKinect::VideoCallback(
void* rgb, uint32_t )
135 uint8_t* rgb_ =
static_cast<uint8_t*
>(rgb);
136 for (
unsigned i = 0; i< height;i++){
137 for (
unsigned j = 0 ; j < width ; j++)
139 IRGB[i][j].R = rgb_[3*(width*i +j)+0];
140 IRGB[i][j].G = rgb_[3*(width*i +j)+1];
141 IRGB[i][j].B = rgb_[3*(width*i +j)+2];
145 m_new_rgb_frame =
true;
158 void vpKinect::DepthCallback(
void* depth, uint32_t )
162 uint16_t* depth_ =
static_cast<uint16_t*
>(depth);
163 for (
unsigned i = 0; i< height;i++){
164 for (
unsigned j = 0 ; j < width ; j++)
166 dmap[i][j] = 0.1236f * tan(depth_[width*i +j] / 2842.5f + 1.1863f);
167 if(depth_[width*i +j]>1023){
172 m_new_depth_map =
true;
173 m_new_depth_image =
true;
183 if (!m_new_depth_map)
186 m_new_depth_map =
false;
198 m_depth_mutex.
lock();
199 if (!m_new_depth_map && !m_new_depth_image)
206 m_new_depth_map =
false;
207 m_new_depth_image =
false;
211 vpERROR_TRACE(1,
"Image size does not match vpKinect DM resolution");
213 for(
unsigned int i = 0; i < hd; i++)
214 for(
unsigned int j = 0; j < wd; j++){
215 map[i][j] = tempMap[i<<1][j<<1];
217 if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
218 Imap[i][j] = (
unsigned char)(255*map[i][j]/5);
225 for (
unsigned i = 0; i< height;i++)
226 for (
unsigned j = 0 ; j < width ; j++){
227 map[i][j] = tempMap[i][j];
229 if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
230 Imap[i][j] = (
unsigned char)(255*map[i][j]/5);
246 if (!m_new_rgb_frame)
249 m_new_rgb_frame =
false;
259 vpERROR_TRACE(1,
"Idepth image size does not match vpKinect DM resolution");
263 IrgbWarped.
resize(hd, wd);
265 double x1=0., y1=0., x2=0., y2=0., Z1, Z2;
272 for (
unsigned int i = 0; i< hd;i++)
273 for (
unsigned int j = 0 ; j < wd ; j++){
278 if (std::fabs(Z1+1) <= std::numeric_limits<double>::epsilon()){
288 if (std::fabs(Z2) > std::numeric_limits<double>::epsilon()){
293 std::cout<<
"Z2 = 0 !!"<<std::endl;
298 unsigned int u_ = (
unsigned int)u;
299 unsigned int v_ = (
unsigned int)v;
301 if ((u_<width)&&(v_<height)){
302 IrgbWarped[i][j] = Irgb[v_][u_];
305 IrgbWarped[i][j] = 0;
314 #endif // VISP_HAVE_LIBFREENECT