Save frame from TangoService_connectOnFrameAvailable

Question

How can I save a frame via TangoService_connectOnFrameAvailable() and display it correctly on my computer? As this reference page mentions, the pixels are stored in the HAL_PIXEL_FORMAT_YV12 format. In my callback function for TangoService_connectOnFrameAvailable, I save the frame like this:

static void onColorFrameAvailable(void* context, TangoCameraId id, const TangoImageBuffer* buffer) 
{
  ...
  std::ofstream fp;
  fp.open(imagefile, std::ios::out | std::ios::binary );
  int offset = 0;
  for(int i = 0; i < buffer->height*2 + 1; i++) {
    fp.write((char*)(buffer->data + offset), buffer->width);
    offset += buffer->stride;
  }
  fp.close();
}

Then to get rid of the meta data in the first row and to display the image I run:

$ dd if="input.raw" of="new.raw" bs=1 skip=1280
$ vooya new.raw

I was careful to make sure in vooya that the channel order is yvu. The resulting output is:

What am I doing wrong in saving the image and displaying it?

UPDATE per Mark Mullin's response:

int offset = buffer->stride; // header offset
// copy Y channel
for(int i = 0; i < buffer->height; i++) {
  fp.write((char*)(buffer->data + offset), buffer->width);
  offset += buffer->stride;
}
// copy V channel
for(int i = 0; i < buffer->height / 2; i++) {
  fp.write((char*)(buffer->data + offset), buffer->width / 2);
  offset += buffer->stride / 2;
}
// copy U channel
for(int i = 0; i < buffer->height / 2; i++) {
  fp.write((char*)(buffer->data + offset), buffer->width / 2);
  offset += buffer->stride / 2;
}

This now shows the picture below, but there are still some artifacts; I wonder if that's from the Tango tablet camera or my processing of the raw data... any thoughts?

Answer 1

Can't say exactly what you're doing wrong AND tango images often have artifacts in them - yours are new, but I often see baby blue as a color where glare seems to be annoying deeper systems, and as it begins to loose sync with the depth system under load, you'll often see what looks like a shiny grid (its the IR pattern, I think) - At the end, any rational attempt to handle the image with openCV etc failed, so I hand wrote the decoder with some help from SO thread here

That said, given imagebuffer contains a pointer to the raw data from Tango, and various other variables like height and stride are filled in from the data received in the callback, then this logic will create an RGBA map - yeah, I optimized the math in it, so it's a little ugly - it's slower but functionally equivalent twin is listed second. My own experience says its a horrible idea to try and do this decode right in the callback (I believe Tango is capable of loosing sync with the flash for depth for purely spiteful reasons), so mine runs at the render stage.

Fast

uchar* pData = TangoData::cameraImageBuffer;
uchar* iData = TangoData::cameraImageBufferRGBA;
int size = (int)(TangoData::imageBufferStride * TangoData::imageBufferHeight);
float invByte = 0.0039215686274509803921568627451;  // ( 1 / 255)

int halfi, uvOffset, halfj, uvOffsetHalfj;
float y_scaled, v_scaled, u_scaled;
int uOffset = size / 4 + size;
int halfstride = TangoData::imageBufferStride / 2;
for (int i = 0; i < TangoData::imageBufferHeight; ++i)
{
    halfi = i / 2;
    uvOffset = halfi * halfstride;
    for (int j = 0; j < TangoData::imageBufferWidth; ++j)
    {
        halfj = j / 2;
        uvOffsetHalfj = uvOffset + halfj;
        y_scaled = pData[i * TangoData::imageBufferStride + j] * invByte;
        v_scaled = 2 * (pData[uvOffsetHalfj + size] * invByte - 0.5f) * Vmax;
        u_scaled = 2 * (pData[uvOffsetHalfj + uOffset] * invByte - 0.5f) * Umax;
        *iData++ = (uchar)((y_scaled + 1.13983f * v_scaled) * 255.0);;
        *iData++ = (uchar)((y_scaled - 0.39465f * u_scaled - 0.58060f * v_scaled) * 255.0);
        *iData++ = (uchar)((y_scaled + 2.03211f * u_scaled) * 255.0);
        *iData++ = 255;
    }
}

Understandable

for (int i = 0; i < TangoData::imageBufferHeight; ++i)
{
    for (int j = 0; j < TangoData::imageBufferWidth; ++j)
    {
        uchar y = pData[i * image->stride + j];
        uchar v = pData[(i / 2) * (TangoData::imageBufferStride / 2) + (j / 2) + size];
        uchar u = pData[(i / 2) * (TangoData::imageBufferStride / 2) + (j / 2) + size + (size / 4)];
        YUV2RGB(y, u, v);
        *iData++ = y;
        *iData++ = u;
        *iData++ = v;
        *iData++ = 255;
    }
}