4 Replies Latest reply on Mar 29, 2018 9:02 AM by velchivz

    Help mapping color to point

    velchivz

      Hello everyone,

       

      This has been asked before, and I believe I am doing the right steps but cannot get it work. I am trying to upgrade from RealSense to RealSense2 using SR300 camera. I am trying to abstract getting a point cloud with color (x, y, z, r, g, b) for other uses. I am trying to map the color to the depth points as follows:

       

      Camera() : pipe(), pipeProfile(), depthScale(0) {
           rs2::config cameraConfig;
           cameraConfig.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16);
           cameraConfig.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGB8);
           pipeProfile = pipe.start(cameraConfig);
           depthScale = pipeProfile.get_device().first<rs2::depth_sensor>().get_depth_scale();
      }
      
      void Camera::getCapture() {
           rs2::frameset frames = pipe.wait_for_frames();
           rs2::depth_frame depthFrame = frames.get_depth_frame();
           rs2::video_frame colorFrame = frames.get_color_frame();
           
           rs2_intrinsics depth_intrinsic = depthFrame.get_profile().as<rs2::video_stream_profile>().get_intrinsics();
           rs2_intrinsics color_intrinsic = colorFrame.get_profile().as<rs2::video_stream_profile>().get_intrinsics();
           rs2_extrinsics depth_to_color = depthFrame.get_profile().get_extrinsics_to(colorFrame.get_profile());
      
      
           const uint16_t* depthImage = (const uint16_t*)depthFrame.get_data();
           const unsigned char* colorImage = (const unsigned char*)colorFrame.get_data();
      
           for (int dy = 0; dy < depth_intrinsic.height; dy++) {
                for (int dx = 0; dx < depth_intrinsic.width; dx++) {
                // get depth value at dy, dx
                unsigned int i = dy * depth_intrinsic.width + dx;
                uint16_t depth_value = depthImage[i];
      
      
                // if the depth value is 0, we ignore this points.
                if (depth_value == 0) {
                     continue;
                }
      
      
                float depth_in_meters = depth_value * depthScale;
      
      
                // map the pixel coordinate in depth image to pixel coordinate in color image
                float depth_pixel[2] = {(float)dx - 0.5f, (float)dy - 0.5f};
                float depth_point[3], color_point[3], color_pixel[2];
                rs2_deproject_pixel_to_point(depth_point, &depth_intrinsic, depth_pixel, depth_in_meters);
      
      
                // if it's out of acceptable range, skip it
                if (depth_point[2] > maxAcceptDistance) {
                     continue;
                }
      
      
                rs2_transform_point_to_point(color_point, &depth_to_color, depth_point);
                rs2_project_point_to_pixel(color_pixel, &color_intrinsic, color_point);
      
      
                // get closest color pixel obtained from calculations and use as the point's color, or white if ouside color image
                const int cx = static_cast<int>(color_pixel[0] + 0.5f);
                const int cy = static_cast<int>(color_pixel[1] + 0.5f);     
      
      
                // if outside intrinsic parameters, skip the point
                if (cx < 0 || cy < 0 || cx > color_intrinsic.width || cy > color_intrinsic.height) {
                     continue;
                }
                const unsigned char* offset = static_cast<const unsigned char*>(colorImage + (cy * color_intrinsic.width + cx) * 3);
      
      
                float x = -1 * depth_point[0];
                float y = -1 * depth_point[1];
                float z = depth_point[2];
                unsigned char r = *(offset);
                unsigned char g = *(offset + 1);
                unsigned char b = *(offset + 2);
                }
           }
      }   
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      

       

      I only left the redundant parts. Basically, in the end, (r, g, b) are 0 (or they appear black in the image). Any suggestions?

       

      PS. (Code editor is weird).

        • 1. Re: Help mapping color to point
          MartyG

          Have you seen the SDK 2.0 example for creating a point cloud?

           

          librealsense/examples/pointcloud at master · IntelRealSense/librealsense · GitHub

          • 2. Re: Help mapping color to point
            velchivz

            Hello MartyG,

             

            I did see the example, but I couldn't figure out how to use the (u, v) values to obtain the (r, g, b) values from the color frame. Any help with that? Thanks.

            • 3. Re: Help mapping color to point
              MartyG

              I'll link RealSense stream programming expert jb455  into this discussion to seek his wide knowledge on the subject.

              • 4. Re: Help mapping color to point
                velchivz

                So I know that once you do

                 

                rs2::pointcloud pc;
                rs2::points points;
                
                rs2::frameset frames = pipe.wait_for_frames();
                rs2::depth_frame depth = frames.get_depth_frame();
                rs2::video_frame color_frame = frames.get_color_frame();
                points = pc.calculate(depth);
                pc.map_to(color_frame);
                

                 

                Then, you can get the texture coordinates that correspond to each vertex:

                 

                const rs2::vertex* ptr = points.get_vertices();
                const rs2::texture_coordinate* tex_coords = points.get_texture_coordinates();
                const unsigned char* colorStream = static_cast<const unsigned char*>(color_frame.get_data());
                

                 

                Each texture coordinate has (u, v) value which ranges from [0-1] that can be mapped to the color frame. I did this:

                 

                int x = tex_coords[i].u * color_intrinsic.width;
                int y = tex_coords[i].v * color_intrinsic.height;
                
                int colorLocation = y * color_intrinsic.width + x;
                unsigned char color[3] = {colorStream[colorLocation], colorStream[colorLocation +1], colorStream[colorLocation +2]};
                

                 

                However, everything comes back as a single color. I stepped through and it seems both u and v are always 0.

                • 5. Re: Help mapping color to point
                  velchivz

                  For anyone that may read this, I had to swap the map_to and calculate statements. However, you also need to get some captures. do as follows:

                   

                  rs2::pointcloud pc;  
                  rs2::points points; 
                  
                  for (int i = 0; i < 10; i++) pipe.wait_for_frames();
                    
                  rs2::frameset frames = pipe.wait_for_frames();  
                  rs2::depth_frame depth = frames.get_depth_frame();  
                  rs2::video_frame color_frame = frames.get_color_frame();  
                  pc.map_to(color_frame);
                  points = pc.calculate(depth);
                  
                  int width = color_frame.get_width();
                  int height = color_frame.get_height();
                    
                  const rs2::vertex* ptr = points.get_vertices();  
                  const rs2::texture_coordinate* tex_coords = points.get_texture_coordinates();  
                  const unsigned char* colorStream = static_cast<const unsigned char*>(color_frame.get_data()); 
                  
                  for (int i = 0; i < points.size(); i++) {
                       
                  int x = static_cast<int>(textureCoordinates[i].u * width);
                  int y = static_cast<int>(textureCoordinates[i].v * height);
                  if (y < 0 || y >= height) {
                  continue;
                  }
                  if (x < 0 || x >= width) {
                  continue;
                  }
                  if (rs_vertices[i].z <= 0 || rs_vertices[i].z > maxAcceptDistance) {          // Rule out values further than this value
                  continue;
                  }
                  
                  
                  int colorLocation = (y * width + x) * 3;
                  unsigned char color[3] = {colorData[colorLocation], colorData[colorLocation +1], colorData[colorLocation +2]};   // RGB
                  
                  float coordinates[3] = { ptr[i].x, ptr[i].y, ptr[i].z };
                  }