Mapping depth to color in OpenGL

Question

I have a code that map Depth value with Color camera image.

I used Realsense ZR300 to capture (x,y,z) information.

My difficulty now is I can't map depth to color information.

From nearest to furthest, the color change from one color to another color with different color information.

I made two subplot and like to plot Depth color in the first subplot and normal color image in the second subplot.

My code is as follow.

int main() try
{
    // Create a context object. This object owns the handles to all connected realsense devices.
    rs::context ctx;
    printf("There are %d connected RealSense devices.\n", ctx.get_device_count());
    if(ctx.get_device_count() == 0) return EXIT_FAILURE;

    // This tutorial will access only a single device, but it is trivial to extend to multiple devices
    rs::device * dev = ctx.get_device(0);
    printf("\nUsing device 0, an %s\n", dev->get_name());
    printf("    Serial number: %s\n", dev->get_serial());
    printf("    Firmware version: %s\n", dev->get_firmware_version());

    // Configure all streams to run at VGA resolution at 60 frames per second
    dev->enable_stream(rs::stream::depth, 640, 480, rs::format::z16, 60);
    dev->enable_stream(rs::stream::color, 640, 480, rs::format::rgb8, 60);
    dev->start();

    // Open a GLFW window to display our output
    glfwInit();
    GLFWwindow * win = glfwCreateWindow(1280, 480, "Depth & Color images", nullptr, nullptr);
    glfwMakeContextCurrent(win);
    while(!glfwWindowShouldClose(win))
    {
        // Wait for new frame data
        glfwPollEvents();
        dev->wait_for_frames();

        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);
        rs::intrinsics depth_intrin = dev->get_stream_intrinsics(rs::stream::depth);
        const uint16_t * depth_image = (const uint16_t *)dev->get_frame_data(rs::stream::depth);
        float scale = dev->get_depth_scale();
        glBegin(GL_POINTS);
        for(int dy=0; dy<depth_intrin.height; ++dy)
        {
            for(int dx=0; dx<depth_intrin.width; ++dx)
            {
                 uint16_t depth_value = depth_image[dy * depth_intrin.width + dx];
                 float depth_in_meters = depth_value * scale;
                 if(depth_value == 0) continue;
                 rs::float2 depth_pixel = {(float)dx, (float)dy};
                 rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
                 //////////////////Here I need to plot Depth to Color////////////////////////
                 ////////////////////////////////////////////////////////////////////////////





                 ///////////////////////////////////////////////////////////////////////////               

            }
        }
        glEnd();


        //glPixelTransferf(GL_RED_SCALE, 0xFFFF * dev->get_depth_scale() / 2.0f);
        //glDrawPixels(640, 480, GL_RED, GL_UNSIGNED_SHORT, dev->get_frame_data(rs::stream::depth));
        //glPixelTransferf(GL_RED_SCALE, 1.0f);

        // Display color image as RGB triples
        glRasterPos2f(0, 1);
        glDrawPixels(640, 480, GL_RGB, GL_UNSIGNED_BYTE, dev->get_frame_data(rs::stream::color));

        glfwSwapBuffers(win);
    }

    return EXIT_SUCCESS;
}
catch(const rs::error & e)
{
    // Method calls against librealsense objects may throw exceptions of type rs::error
    printf("rs::error was thrown when calling %s(%s):\n", e.get_failed_function().c_str(), e.get_failed_args().c_str());
    printf("    %s\n", e.what());
    return EXIT_FAILURE;
}

Answer 1

I did as follow and I have x,y,d information and can plot depth mapping to RED color upto 4 meters.

    while(!glfwWindowShouldClose(win))
    {

        // Wait for new frame data
        glfwPollEvents();
        dev->wait_for_frames();
        //FPS
        auto t1 = std::chrono::high_resolution_clock::now();
        time += std::chrono::duration<float>(t1-t0).count();
        t0 = t1;
        ++frames;
        if(time > 0.5f)
        {
            fps = frames / time;
            frames = 0;
            time = 0;
        }
        //
        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);
        rs::intrinsics depth_intrin = dev->get_stream_intrinsics(rs::stream::depth);
        const uint16_t * depth_image = (const uint16_t *)dev->get_frame_data(rs::stream::depth);
        float scale = dev->get_depth_scale();

        for(int dy=0; dy<depth_intrin.height; ++dy)
        {
            for(int dx=0; dx<depth_intrin.width; ++dx)
            {
                 uint16_t depth_value = depth_image[dy * depth_intrin.width + dx];
                 float depth_in_meters = depth_value * scale;

            }
        }
        glPixelTransferf(GL_RED_SCALE, 0xFFFF * dev->get_depth_scale() / 4.0f);
        glDrawPixels(640, 480, GL_RED, GL_UNSIGNED_SHORT, dev->get_frame_data(rs::stream::depth));
        glPixelTransferf(GL_RED_SCALE, 1.0f);
       // glDrawPixels( 640, 480, GL_RGB, GL_UNSIGNED_INT, data );

        // Display color image as RGB triples
        glRasterPos2f(0, 1);
        glDrawPixels(640, 480, GL_RGB, GL_UNSIGNED_BYTE, dev->get_frame_data(rs::stream::color));


        //ss.str(""); ss << fps << " FPS";
        //printf("FPS  %s\n", ss.str().c_str());
        draw_text(0, 0, ss.str().c_str());
        glfwSwapBuffers(win);

    }