Project Tango: преобразование глубины изображения из данных XYZij

Question

В настоящее время я пытаюсь отфильтровать информацию о глубине с помощью OpenCV. По этой причине мне нужно преобразовать информацию о глубине проекта Tango XYZij в изображение, подобное карте глубины. (Как и выход Microsoft Kinect) К сожалению, официальные API lacking the ij part of XYZij. Вот почему я пытаюсь проецировать часть XYZ с помощью проекции внутренней камеры, wich is explained in the official C API Dokumentation. Мой текущий подход выглядит следующим образом:

float fx = static_cast<float>(ccIntrinsics.fx); 
    float fy = static_cast<float>(ccIntrinsics.fy); 
    float cx = static_cast<float>(ccIntrinsics.cx); 
    float cy = static_cast<float>(ccIntrinsics.cy); 

    float k1 = static_cast<float>(ccIntrinsics.distortion[0]); 
    float k2 = static_cast<float>(ccIntrinsics.distortion[1]); 
    float k3 = static_cast<float>(ccIntrinsics.distortion[2]); 

    for (int k = 0; k < xyz_ij->xyz_count; ++k) { 

     float X = xyz_ij->xyz[k][0]; 
     float Y = xyz_ij->xyz[k][1]; 
     float Z = xyz_ij->xyz[k][2]; 

     float ru = sqrt((pow(X, 2) + pow(Y, 2))/pow(Z, 2)); 
     float rd = ru + k1 * pow(ru, 3) + k2 * pow(ru, 5) + k3 * pow(ru, 7); 

     int x = X/Z * fx * rd/ru + cx; 
     int y = X/Z * fy * rd/ru + cy; 

     // drawing into OpenCV Mat in red 
     depth.at<cv::Vec3b>(x, y)[0] = 240; 

    } 

Полученную карту глубины можно увидеть в правом нижнем углу. Но похоже, что этот расчет приводит к линейному представлению ... Кто-нибудь уже сделал что-то подобное? Правильно ли расположены точки XYZ для этой проекции?

Answer 1

Я действительно нашел решение ... Просто пропустил расчет искажений, как в rgb-depth-sync-example. Мой код теперь выглядит следующим образом:

float fx = static_cast<float>(ccIntrinsics.fx); 
    float fy = static_cast<float>(ccIntrinsics.fy); 
    float cx = static_cast<float>(ccIntrinsics.cx); 
    float cy = static_cast<float>(ccIntrinsics.cy); 

    int width = static_cast<int>(ccIntrinsics.width); 
    int height = static_cast<int>(ccIntrinsics.height); 

    for (int k = 0; k < xyz_ij->xyz_count; ++k) { 

     float X = xyz_ij->xyz[k * 3][0]; 
     float Y = xyz_ij->xyz[k * 3][1]; 
     float Z = xyz_ij->xyz[k * 3][2]; 

     int x = static_cast<int>(fx * (X/Z) + cx); 
     int y = static_cast<int>(fy * (Y/Z) + cy); 

     uint8_t depth_value = UCHAR_MAX - ((Z * 1000) * UCHAR_MAX/4500); 

     cv::Point point(y % height, x % width); 
     line(depth, point, point, cv::Scalar(depth_value, depth_value, depth_value), 4.5); 

    } 

И результат рабочего OpenCV выглядит следующим образом: