const float *depth_data = (float*)depth->data; const unsigned int *rgb_data = (unsigned int*)rgb->data; float *undistorted_data = (float*)undistorted->data; unsigned int *registered_data = (unsigned int*)registered->data; const int *map_dist = distort_map; const float *map_x = depth_to_color_map_x; const int *map_yi = depth_to_color_map_yi;
const int size_depth = 512 * 424; const int size_color = 1920 * 1080; const float color_cx = color.cx + 0.5f; // 0.5f added for later rounding
// size of filter map with a border of filter_height_half on top and bottom so that no check for borders is needed. // since the color image is wide angle no border to the sides is needed. const int size_filter_map = size_color + 1920 * filter_height_half * 2; // offset to the important data const int offset_filter_map = 1920 * filter_height_half;
// map for storing the min z values used for each color pixel float *filter_map = NULL; // pointer to the beginning of the important data float *p_filter_map = NULL;
// map for storing the color offset for each depth pixel int *depth_to_c_off = color_depth_map ? color_depth_map : new int[size_depth]; int *map_c_off = depth_to_c_off;
// initializing the depth_map with values outside of the Kinect2 range if(enable_filter){ filter_map = bigdepth ? (float*)bigdepth->data : new float[size_filter_map]; p_filter_map = filter_map + offset_filter_map;
/* Fix depth distortion, and compute pixel to use from 'rgb' based on depth measurement, * stored as x/y offset in the rgb data. */
// iterating over all pixels from undistorted depth and registered color image // the four maps have the same structure as the images, so their pointers are increased each iteration as well for(int i = 0; i < size_depth; ++i, ++undistorted_data, ++map_dist, ++map_x, ++map_yi, ++map_c_off){ // getting index of distorted depth pixel const int index = *map_dist;
// check if distorted depth pixel is outside of the depth image if(index < 0){ *map_c_off = -1; *undistorted_data = 0; continue; }
// getting depth value for current pixel const float z = depth_data[index]; *undistorted_data = z;
// checking for invalid depth value if(z <= 0.0f){ *map_c_off = -1; continue; }
// calculating x offset for rgb image based on depth value const float rx = (*map_x + (color.shift_m / z)) * color.fx + color_cx; const int cx = rx; // same as round for positive numbers (0.5f was already added to color_cx) // getting y offset for depth image const int cy = *map_yi; // combining offsets const int c_off = cx + cy * 1920;
// check if c_off is outside of rgb image // checking rx/cx is not needed because the color image is much wider then the depth image if(c_off < 0 || c_off >= size_color){ *map_c_off = -1; continue; }
// saving the offset for later *map_c_off = c_off;
if(enable_filter){ // setting a window around the filter map pixel corresponding to the color pixel with the current z value int yi = (cy - filter_height_half) * 1920 + cx - filter_width_half; // index of first pixel to set for(int r = -filter_height_half; r <= filter_height_half; ++r, yi += 1920) // index increased by a full row each iteration { float *it = p_filter_map + yi; for(int c = -filter_width_half; c <= filter_width_half; ++c, ++it) { // only set if the current z is smaller if(z < *it) *it = z; } } } }
/* Construct 'registered' image. */
// reseting the pointers to the beginning map_c_off = depth_to_c_off; undistorted_data = (float*)undistorted->data;
/* Filter drops duplicate pixels due to aspect of two cameras. */ if(enable_filter){ // run through all registered color pixels and set them based on filter results for(int i = 0; i < size_depth; ++i, ++map_c_off, ++undistorted_data, ++registered_data){ const int c_off = *map_c_off;
// check if offset is out of image if(c_off < 0){ *registered_data = 0; continue; } f const float min_z = p_filter_map[c_off]; const float z = *undistorted_data;
// check for allowed depth noise *registered_data = (z - min_z) / z > filter_tolerance ? 0 : *(rgb_data + c_off); }
if (!bigdepth) delete[] filter_map; } else { // run through all registered color pixels and set them based on c_off for(int i = 0; i < size_depth; ++i, ++map_c_off, ++registered_data){ const int c_off = *map_c_off;
// check if offset is out of image *registered_data = c_off < 0 ? 0 : *(rgb_data + c_off); } } if (!color_depth_map) delete[] depth_to_c_off; }
