1

I found out from one site that it is possible to find distance from the raw depth video output of the Kinect through the 2 bytes assigned to a particular pixel as shown in this link - tutorial. Based on this I written a code to find out the distance of the middle point form the Kinect sensor .I compiled it and run the code on ubuntu and it is showing the output. The output is showing some values as distance. The values are coming around 150->1147. I hope it is showing the distance in mm. But I am not sure it is right or wrong. I am providing the code below. Please make me sure that it is correct or if changes are required please help me with this. Thank you all !
Code:

    #include <opencv/cv.h>
    #include <opencv/highgui.h>
    #include <stdio.h>
    #include "libfreenect_cv.h"

    int getDist(IplImage *depth){
      int x = depth->width/2;
      int y = depth->height/2;
      printf("width= %d and height %d \n",x,y);
      int d = depth->imageData[x*2+y*640*2+1];
      printf("1st value is %d \n",d);
      d= d << 8;
      d= d+depth->imageData[x*2+y*640*2];
      return d;
    }
    IplImage *GlViewColor(IplImage *depth)
    {
        static IplImage *image = 0;
        if (!image) image = cvCreateImage(cvSize(640,480), 8, 3);
        unsigned char *depth_mid = (unsigned char*)(image->imageData);
        int i;
        for (i = 0; i < 640*480; i++) {
            int lb = ((short *)depth->imageData)[i] % 256;
            int ub = ((short *)depth->imageData)[i] / 256;
            switch (ub) {
                case 0:
                    depth_mid[3*i+2] = 255;
                    depth_mid[3*i+1] = 255-lb;
                    depth_mid[3*i+0] = 255-lb;
                    break;
                case 1:
                    depth_mid[3*i+2] = 255;
                    depth_mid[3*i+1] = lb;
                    depth_mid[3*i+0] = 0;
                    break;
                case 2:
                    depth_mid[3*i+2] = 255-lb;
                    depth_mid[3*i+1] = 255;
                    depth_mid[3*i+0] = 0;
                    break;
                case 3:
                    depth_mid[3*i+2] = 0;
                    depth_mid[3*i+1] = 255;
                    depth_mid[3*i+0] = lb;
                    break;
                case 4:
                    depth_mid[3*i+2] = 0;
                    depth_mid[3*i+1] = 255-lb;
                    depth_mid[3*i+0] = 255;
                    break;
                case 5:
                    depth_mid[3*i+2] = 0;
                    depth_mid[3*i+1] = 0;
                    depth_mid[3*i+0] = 255-lb;
                    break;
                default:
                    depth_mid[3*i+2] = 0;
                    depth_mid[3*i+1] = 0;
                    depth_mid[3*i+0] = 0;
                    break;
            }
        }
        return image;
    }

    int main(int argc, char **argv)
    {
        while (cvWaitKey(100) != 27) {
            IplImage *image = freenect_sync_get_rgb_cv(0);
            if (!image) {
                printf("Error: Kinect not connected?\n");
                return -1;
            }
            cvCvtColor(image, image, CV_RGB2BGR);
            IplImage *depth = freenect_sync_get_depth_cv(0);
            if (!depth) {
                printf("Error: Kinect not connected?\n");
                return -1;
            }
            cvShowImage("RGB", image);
            //int d = getDist(depth);
            printf("value is %d \n",getDist(depth));
            cvShowImage("Depth", GlViewColor(depth));//GlViewColor(depth)

        }
        cvDestroyWindow("RGB");
        cvDestroyWindow("Depth");
        //cvReleaseImage(image);
        //cvReleaseImage(depth);
        return 0;
    }

0 Answers0