Revision history [back]

Here you can find a short code to store depth data into CV image:

unsigned int depthSamples = msg->image().width() * msg->image().height();
float f;
// cppchecker recommends using sizeof(varname)
unsigned int depthBufferSize = depthSamples * sizeof(f);

float *depthBuffer = new float[depthSamples];
uint16_t *imageBufferHalf = new uint16_t[depthSamples];

memcpy(depthBuffer, msg->image().data().c_str(), depthBufferSize);

float maxDepth = 0;
for (unsigned int i = 0; i < msg->image().height() * msg->image().width(); ++i) {
    if (depthBuffer[i] > maxDepth && !std::isinf(depthBuffer[i])) {
        maxDepth = depthBuffer[i];
    }
}

cv::Mat image(msg->image().height(), msg->image().width(), CV_8UC3);
unsigned int idx = 0;
double factor = 255 / maxDepth;
for (unsigned int j = 0; j < msg->image().height(); ++j) {
  for (unsigned int i = 0; i < msg->image().width(); ++i) {
    float d = depthBuffer[idx++];
    if (d > maxDepth)
        d = maxDepth;
    d = 255 - (d * factor);
    cv::Vec3b color;
    color[0] = d;
    color[1] = d;
    color[2] = d;
    image.at<cv::Vec3b>(cv::Point(i, j)) = color;
  }
}