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; }
topic pattern [[[ out | in | both ] qos-level] local-prefix remote-prefix]
max_qos value Limit the QoS value allowed for clients connecting to this listener. Defaults to 2, which means any QoS can be used. Set to 0 or 1 to limit to those QoS values. This makes use of an MQTT v5 feature to notify clients of the limitation. MQTT v3.1.1 clients will not be aware of the limitation. Clients publishing to this listener with a too-high QoS will be disconnected.
Quality of Service MQTT defines three levels of Quality of Service (QoS). The QoS defines how hard the broker/client will try to ensure that a message is received. Messages may be sent at any QoS level, and clients may attempt to subscribe to topics at any QoS level. This means that the client chooses the maximum QoS it will receive. For example, if a message is published at QoS 2 and a client is subscribed with QoS 0, the message will be delivered to that client with QoS 0. If a second client is also subscribed to the same topic, but with QoS 2, then it will receive the same message but with QoS 2. For a second example, if a client is subscribed with QoS 2 and a message is published on QoS 0, the client will receive it on QoS 0.
Higher levels of QoS are more reliable, but involve higher latency and have higher bandwidth requirements.
0: The broker/client will deliver the message once, with no confirmation. 1: The broker/client will deliver the message at least once, with confirmation required. 2: The broker/client will deliver the message exactly once by using a four step handshake.
