Example #1
0
int createRealsenseDevice(int devID,char * devName,unsigned int width,unsigned int height,unsigned int framerate)
{
    /* This call will access only a single device, based on the devID identifier..*/
    device[devID].dev = rs_get_device(ctx, devID , &e);
    if (check_error())
    {
      fprintf(stderr,"Cannot get a device context for device %u \n",devID);
      return 0;
    }

    printf("\nUsing device 0, an %s\n", rs_get_device_name(device[devID].dev, &e));                       check_error();
    printf("    Serial number: %s\n", rs_get_device_serial(device[devID].dev, &e));                       check_error();
    printf("    Firmware version: %s\n", rs_get_device_firmware_version(device[devID].dev, &e));          check_error();

    /* Configure all streams to run at VGA resolution at 60 frames per second */
    rs_enable_stream(device[devID].dev, RS_STREAM_DEPTH, width, height, RS_FORMAT_Z16, framerate, &e);    check_error();
    rs_enable_stream(device[devID].dev, RS_STREAM_COLOR, width, height, RS_FORMAT_RGB8, framerate, &e);   check_error();
    rs_start_device(device[devID].dev, &e);                                                               check_error();

    device[devID].colorStreamToUse=RS_STREAM_COLOR;
    device[devID].depthStreamToUse=RS_STREAM_DEPTH_ALIGNED_TO_COLOR;
    device[devID].frameCount=0;
    device[devID].hasInit=1;
  return 1;
}
CameraWrapper::CameraWrapper(int frames)
{
    // init camera
    e = 0;
    ctx = rs_create_context(RS_API_VERSION, &e);
    check_error();
    printf("There are %d connected RealSense devices.\n", rs_get_device_count(ctx, &e));
    check_error();
    if(rs_get_device_count(ctx, &e) == 0)
    {
        std::cout << "camera init broken" << std::cout;
    }
    else
    {
        dev = rs_get_device(ctx, 0, &e);
        check_error();
        printf("\nUsing device 0, an %s\n", rs_get_device_name(dev, &e));
        check_error();
        printf("    Serial number: %s\n", rs_get_device_serial(dev, &e));
        check_error();
        printf("    Firmware version: %s\n", rs_get_device_firmware_version(dev, &e));
        check_error();

        // set controls
        // NOTE find good laser power, max 15
        //rs_enable_stream_preset(dev, RS_STREAM_DEPTH, RS_PRESET_BEST_QUALITY, &e);

        rs_enable_stream(dev, RS_STREAM_DEPTH, 640, 480, RS_FORMAT_Z16, 30, &e);
        check_error();
        rs_set_device_option(dev, RS_OPTION_F200_ACCURACY, 1, &e);
        check_error();
        rs_set_device_option(dev, RS_OPTION_F200_FILTER_OPTION, 0, &e);
        check_error();
        rs_enable_stream(dev, RS_STREAM_COLOR, 1920, 1080, RS_FORMAT_RGB8, 30, &e);
//        rs_enable_stream_preset(dev, RS_STREAM_COLOR, RS_PRESET_BEST_QUALITY, &e);
        check_error();
        rs_enable_stream(dev, RS_STREAM_INFRARED, 640,480, RS_FORMAT_Y8, 30, &e);
        //rs_enable_stream_preset(dev, RS_STREAM_INFRARED, RS_PRESET_BEST_QUALITY, &e);
        check_error();
        rs_start_device(dev, &e);
        check_error();

        // get intrinsics and convert them to opencv mat
        // IR and DEPTH are the same for f200, so this is basically overhead
        rs_get_stream_intrinsics(dev, RS_STREAM_DEPTH, &depth_intrin, &e);
        check_error();
        rs_get_device_extrinsics(dev, RS_STREAM_DEPTH, RS_STREAM_COLOR, &depth_to_color, &e);
        check_error();
        rs_get_stream_intrinsics(dev, RS_STREAM_COLOR, &color_intrin, &e);
        check_error();
        rs_get_stream_intrinsics(dev, RS_STREAM_INFRARED, &ir_intrin, &e);
        check_error();
        depthScale = rs_get_device_depth_scale(dev, &e);
        check_error();
    }

    // set number of frames to record
    setStackSize(frames);

}
int main()
{
    /* Turn on logging. We can separately enable logging to console or to file, and use different severity filters for each. */
    rs_log_to_console(RS_LOG_SEVERITY_WARN, &e);
    check_error();
    /*rs_log_to_file(RS_LOG_SEVERITY_DEBUG, "librealsense.log", &e);
    check_error();*/

    /* Create a context object. This object owns the handles to all connected realsense devices. */
    rs_context * ctx = rs_create_context(RS_API_VERSION, &e);
    check_error();
    printf("There are %d connected RealSense devices.\n", rs_get_device_count(ctx, &e));
    check_error();
    if(rs_get_device_count(ctx, &e) == 0) return EXIT_FAILURE;

    /* This tutorial will access only a single device, but it is trivial to extend to multiple devices */
    rs_device * dev = rs_get_device(ctx, 0, &e);
    check_error();
    printf("\nUsing device 0, an %s\n", rs_get_device_name(dev, &e));
    check_error();
    printf("    Serial number: %s\n", rs_get_device_serial(dev, &e));
    check_error();
    printf("    Firmware version: %s\n", rs_get_device_firmware_version(dev, &e));
    check_error();

    /* Configure depth and color to run with the device's preferred settings */
    rs_enable_stream_preset(dev, RS_STREAM_DEPTH, RS_PRESET_BEST_QUALITY, &e);
    check_error();
    rs_enable_stream_preset(dev, RS_STREAM_COLOR, RS_PRESET_BEST_QUALITY, &e);
    check_error();
    rs_start_device(dev, RS_SOURCE_VIDEO, &e);
    check_error();

    /* Open a GLFW window to display our output */
    glfwInit();
    GLFWwindow * win = glfwCreateWindow(1280, 960, "librealsense tutorial #3", NULL, NULL);
    glfwSetCursorPosCallback(win, on_cursor_pos);
    glfwSetMouseButtonCallback(win, on_mouse_button);
    glfwMakeContextCurrent(win);
    while(!glfwWindowShouldClose(win))
    {
        /* Wait for new frame data */
        glfwPollEvents();
        rs_wait_for_frames(dev, &e);
        check_error();

        /* Retrieve our images */
        const uint16_t * depth_image = (const uint16_t *)rs_get_frame_data(dev, RS_STREAM_DEPTH, &e);
        check_error();
        const uint8_t * color_image = (const uint8_t *)rs_get_frame_data(dev, RS_STREAM_COLOR, &e);
        check_error();

        /* Retrieve camera parameters for mapping between depth and color */
        rs_intrinsics depth_intrin, color_intrin;
        rs_extrinsics depth_to_color;
        rs_get_stream_intrinsics(dev, RS_STREAM_DEPTH, &depth_intrin, &e);
        check_error();
        rs_get_device_extrinsics(dev, RS_STREAM_DEPTH, RS_STREAM_COLOR, &depth_to_color, &e);
        check_error();
        rs_get_stream_intrinsics(dev, RS_STREAM_COLOR, &color_intrin, &e);
        check_error();
        float scale = rs_get_device_depth_scale(dev, &e);
        check_error();

        /* Set up a perspective transform in a space that we can rotate by clicking and dragging the mouse */
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        gluPerspective(60, (float)1280/960, 0.01f, 20.0f);
        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
        gluLookAt(0,0,0, 0,0,1, 0,-1,0);
        glTranslatef(0,0,+0.5f);
        glRotated(pitch, 1, 0, 0);
        glRotated(yaw, 0, 1, 0);
        glTranslatef(0,0,-0.5f);

        /* We will render our depth data as a set of points in 3D space */
        glPointSize(2);
        glEnable(GL_DEPTH_TEST);
        glBegin(GL_POINTS);

        int dx, dy;
        for(dy=0; dy<depth_intrin.height; ++dy)
        {
            for(dx=0; dx<depth_intrin.width; ++dx)
            {
                /* Retrieve the 16-bit depth value and map it into a depth in meters */
                uint16_t depth_value = depth_image[dy * depth_intrin.width + dx];
                float depth_in_meters = depth_value * scale;

                /* Skip over pixels with a depth value of zero, which is used to indicate no data */
                if(depth_value == 0) continue;

                /* Map from pixel coordinates in the depth image to pixel coordinates in the color image */
                float depth_pixel[2] = {(float)dx, (float)dy};
                float depth_point[3], color_point[3], color_pixel[2];
                rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth_in_meters);
                rs_transform_point_to_point(color_point, &depth_to_color, depth_point);
                rs_project_point_to_pixel(color_pixel, &color_intrin, color_point);

                /* Use the color from the nearest color pixel, or pure white if this point falls outside the color image */
                const int cx = (int)roundf(color_pixel[0]), cy = (int)roundf(color_pixel[1]);
                if(cx < 0 || cy < 0 || cx >= color_intrin.width || cy >= color_intrin.height)
                {
                    glColor3ub(255, 255, 255);
                }
                else
                {
                    glColor3ubv(color_image + (cy * color_intrin.width + cx) * 3);
                }

                /* Emit a vertex at the 3D location of this depth pixel */
                glVertex3f(depth_point[0], depth_point[1], depth_point[2]);
            }
        }
        glEnd();

        glfwSwapBuffers(win);
    }
    
    return EXIT_SUCCESS;
}
Example #4
0
int main()
{
    /* Create a context object. This object owns the handles to all connected realsense devices. */
    rs_context * ctx = rs_create_context(RS_API_VERSION, &e);
    check_error();
    printf("There are %d connected RealSense devices.\n", rs_get_device_count(ctx, &e));
    check_error();
    if(rs_get_device_count(ctx, &e) == 0) return EXIT_FAILURE;

    /* This tutorial will access only a single device, but it is trivial to extend to multiple devices */
    rs_device * dev = rs_get_device(ctx, 0, &e);
    check_error();
    printf("\nUsing device 0, an %s\n", rs_get_device_name(dev, &e));
    check_error();
    printf("    Serial number: %s\n", rs_get_device_serial(dev, &e));
    check_error();
    printf("    Firmware version: %s\n", rs_get_device_firmware_version(dev, &e));
    check_error();

    /* Configure depth to run at VGA resolution at 30 frames per second */
    rs_enable_stream(dev, RS_STREAM_DEPTH, 640, 480, RS_FORMAT_Z16, 30, &e);
    check_error();
    rs_start_device(dev, &e);
    check_error();

    /* Determine depth value corresponding to one meter */
    const uint16_t one_meter = (uint16_t)(1.0f / rs_get_device_depth_scale(dev, &e));
    check_error();

    while(1)
    {
        /* This call waits until a new coherent set of frames is available on a device
           Calls to get_frame_data(...) and get_frame_timestamp(...) on a device will return stable values until wait_for_frames(...) is called */
        rs_wait_for_frames(dev, &e);

        /* Retrieve depth data, which was previously configured as a 640 x 480 image of 16-bit depth values */
        const uint16_t * depth_frame = (const uint16_t *)(rs_get_frame_data(dev, RS_STREAM_DEPTH, &e));

        /* Print a simple text-based representation of the image, by breaking it into 10x20 pixel regions and and approximating the coverage of pixels within one meter */
        char buffer[(640/10+1)*(480/20)+1];
        char * out = buffer;
        int coverage[64] = {0}, x,y,i;
        for(y=0; y<480; ++y)
        {
            for(x=0; x<640; ++x)
            {
                int depth = *depth_frame++;
                if(depth > 0 && depth < one_meter) ++coverage[x/10];
            }

            if(y%20 == 19)
            {
                for(i=0; i<64; ++i)
                {
                    *out++ = " .:nhBXWW"[coverage[i]/25];
                    coverage[i] = 0;
                }
                *out++ = '\n';
            }
        }
        *out++ = 0;
        printf("\n%s", buffer);
    }
    
    return EXIT_SUCCESS;
}
int RealSenseDevice::open()
{

    rs_error * e = 0;
    ctx = rs_create_context(RS_API_VERSION, &e);

    int devices = rs_get_device_count(ctx, &e);
    if (devices == 0) {

        std::cerr << "no realsense capture device" << std::endl;
        return 1;
    }

    dev = rs_get_device(ctx, 0, &e);

    std::cout << "Realsense Device: " << rs_get_device_name(dev, &e) << std::endl;
    std::cout << "Realsense Serial: " << rs_get_device_serial(dev, &e) << std::endl;
    std::cout << "Realsense FW ver: " << rs_get_device_firmware_version(dev, &e) << std::endl;

    int framerate = 30;
    rs_enable_stream(dev, depthStream, 0, 0, RS_FORMAT_Z16, framerate, &e);

    int depthWidth = rs_get_stream_width(dev,depthStream,NULL);
    int depthHeight = rs_get_stream_height(dev,depthStream,NULL);

    rs_enable_stream(dev, colorStream, depthWidth, depthHeight, RS_FORMAT_RGB8, framerate, &e);


    rs_get_stream_intrinsics(dev,colorStream,&intrinsics,0);



    std::cout << "width:" << intrinsics.width << " height:" << intrinsics.height
              << " focalx:" << intrinsics.fx << " focaly:" << intrinsics.fy
              << " ppx:" << intrinsics.ppx << " ppy:" << intrinsics.ppy
              << " coeff0:" << intrinsics.coeffs[0]
              << " coeff1:" << intrinsics.coeffs[1]
              << " coeff2:" << intrinsics.coeffs[2]
              << " coeff3:" << intrinsics.coeffs[3]
              << std::endl;



    int colorWidth = rs_get_stream_width(dev,colorStream,NULL);
    int colorHeight = rs_get_stream_height(dev,colorStream,NULL);
    depthScale = rs_get_device_depth_scale(dev,0);

    std::cout << "depth image " << depthWidth << "x" << depthHeight << " scale:" << depthScale << std::endl;
    std::cout << "color image " << colorWidth << "x" << colorHeight << std::endl;



    rs_start_device(dev, &e);

    int res = pthread_create(&_thread, NULL, RealSenseDevice_freenect_threadfunc, this);

    if(res){
        std::cerr << "error starting realsense thread " << res << std::endl;
        return 1;
    }

    depth_index = 0;

    return 0;
}
int main()
{
    /* Create a context object. This object owns the handles to all connected realsense devices. */
    rs_context * ctx = rs_create_context(RS_API_VERSION, &e);
    check_error();
    printf("There are %d connected RealSense devices.\n", rs_get_device_count(ctx, &e));
    check_error();
    if(rs_get_device_count(ctx, &e) == 0) return EXIT_FAILURE;

    /* This tutorial will access only a single device, but it is trivial to extend to multiple devices */
    rs_device * dev = rs_get_device(ctx, 0, &e);
    check_error();
    printf("\nUsing device 0, an %s\n", rs_get_device_name(dev, &e));
    check_error();
    printf("    Serial number: %s\n", rs_get_device_serial(dev, &e));
    check_error();
    printf("    Firmware version: %s\n", rs_get_device_firmware_version(dev, &e));
    check_error();

    /* Configure all streams to run at VGA resolution at 60 frames per second */
    rs_enable_stream(dev, RS_STREAM_DEPTH, 640, 480, RS_FORMAT_Z16, 60, &e);
    check_error();
    rs_enable_stream(dev, RS_STREAM_COLOR, 640, 480, RS_FORMAT_RGB8, 60, &e);
    check_error();
    rs_enable_stream(dev, RS_STREAM_INFRARED, 640, 480, RS_FORMAT_Y8, 60, &e);
    check_error();
    rs_enable_stream(dev, RS_STREAM_INFRARED2, 640, 480, RS_FORMAT_Y8, 60, NULL); /* Pass NULL to ignore errors */
    rs_start_device(dev, &e);
    check_error();

    /* Open a GLFW window to display our output */
    glfwInit();
    GLFWwindow * win = glfwCreateWindow(1280, 960, "librealsense tutorial #2", NULL, NULL);
    glfwMakeContextCurrent(win);
    while(!glfwWindowShouldClose(win))
    {
        /* Wait for new frame data */
        glfwPollEvents();
        rs_wait_for_frames(dev, &e);
        check_error();

        glClear(GL_COLOR_BUFFER_BIT);
        glPixelZoom(1, -1);

        /* Display depth data by linearly mapping depth between 0 and 2 meters to the red channel */
        glRasterPos2f(-1, 1);
        glPixelTransferf(GL_RED_SCALE, 0xFFFF * rs_get_device_depth_scale(dev, &e) / 2.0f);
        check_error();
        glDrawPixels(640, 480, GL_RED, GL_UNSIGNED_SHORT, rs_get_frame_data(dev, RS_STREAM_DEPTH, &e));
        check_error();
        glPixelTransferf(GL_RED_SCALE, 1.0f);

        /* Display color image as RGB triples */
        glRasterPos2f(0, 1);
        glDrawPixels(640, 480, GL_RGB, GL_UNSIGNED_BYTE, rs_get_frame_data(dev, RS_STREAM_COLOR, &e));
        check_error();

        /* Display infrared image by mapping IR intensity to visible luminance */
        glRasterPos2f(-1, 0);
        glDrawPixels(640, 480, GL_LUMINANCE, GL_UNSIGNED_BYTE, rs_get_frame_data(dev, RS_STREAM_INFRARED, &e));
        check_error();

        /* Display second infrared image by mapping IR intensity to visible luminance */
        if(rs_is_stream_enabled(dev, RS_STREAM_INFRARED2, NULL))
        {
            glRasterPos2f(0, 0);
            glDrawPixels(640, 480, GL_LUMINANCE, GL_UNSIGNED_BYTE, rs_get_frame_data(dev, RS_STREAM_INFRARED2, &e));
        }

        glfwSwapBuffers(win);
    }
    
    return EXIT_SUCCESS;
}