C++ Kinect v2＆freenect2：奥行きデータを実世界座標に変換する方法

Question

Kinect v2カメラ（Linux）を使用して実世界のxyz座標を計算しようとしていますが、計算結果が間違っています。ここで

コードです：

cv::Point3f xyzWorld={0.0f}; 

xyzWorld.z = pointDepth; 
xyzWorld.x = (float) ((float)x -(depthcx)) * xyzWorld.z/depthfx; 
xyzWorld.y = (float) ((float)y - (depthcy)) * xyzWorld.z/depthfy; 
xyzWorld.z = pointDepth; 

return xyzWorld; 

私はこの問題はfx、fy、cxとcyの深度値が原因だと思います。

誰かが私を助けることができますか？

私はfreenect2を使用しています。

Answer 1

理由だけOpenNi実装を使用していない

OniStatus VideoStream::convertDepthToWorldCoordinates(float depthX, float depthY, float depthZ, float* pWorldX, float* pWorldY, float* pWorldZ) 
{ 
    if (m_pSensorInfo->sensorType != ONI_SENSOR_DEPTH) 
    { 
     m_errorLogger.Append("convertDepthToWorldCoordinates: Stream is not from DEPTH\n"); 
     return ONI_STATUS_NOT_SUPPORTED; 
    } 

    float normalizedX = depthX/m_worldConvertCache.resolutionX - .5f; 
    float normalizedY = .5f - depthY/m_worldConvertCache.resolutionY; 

    OniVideoMode videoMode; 
    int size = sizeof(videoMode); 
    getProperty(ONI_STREAM_PROPERTY_VIDEO_MODE, &videoMode, &size); 

    float const convertToMillimeters = (videoMode.pixelFormat == ONI_PIXEL_FORMAT_DEPTH_100_UM) ? 10.f : 1.f; 
    *pWorldX = (normalizedX * depthZ * m_worldConvertCache.xzFactor)/convertToMillimeters; 
    *pWorldY = (normalizedY * depthZ * m_worldConvertCache.yzFactor)/convertToMillimeters; 
    *pWorldZ = depthZ/convertToMillimeters; 

    return ONI_STATUS_OK; 
} 

と

OniStatus VideoStream::convertWorldToDepthCoordinates(float worldX, float worldY, float worldZ, float* pDepthX, float* pDepthY, float* pDepthZ) 
{ 
    if (m_pSensorInfo->sensorType != ONI_SENSOR_DEPTH) 
    { 
     m_errorLogger.Append("convertWorldToDepthCoordinates: Stream is not from DEPTH\n"); 
     return ONI_STATUS_NOT_SUPPORTED; 
    } 

    *pDepthX = m_worldConvertCache.coeffX * worldX/worldZ + m_worldConvertCache.halfResX; 
    *pDepthY = m_worldConvertCache.halfResY - m_worldConvertCache.coeffY * worldY/worldZ; 
    *pDepthZ = worldZ; 
    return ONI_STATUS_OK; 
} 

と世界の変換キャッシュ：

void VideoStream::refreshWorldConversionCache() 
{ 
    if (m_pSensorInfo->sensorType != ONI_SENSOR_DEPTH) 
    { 
     return; 
    } 

    OniVideoMode videoMode; 
    int size = sizeof(videoMode); 
    getProperty(ONI_STREAM_PROPERTY_VIDEO_MODE, &videoMode, &size); 

    size = sizeof(float); 
    float horizontalFov; 
    float verticalFov; 
    getProperty(ONI_STREAM_PROPERTY_HORIZONTAL_FOV, &horizontalFov, &size); 
    getProperty(ONI_STREAM_PROPERTY_VERTICAL_FOV, &verticalFov, &size); 

    m_worldConvertCache.xzFactor = tan(horizontalFov/2) * 2; 
    m_worldConvertCache.yzFactor = tan(verticalFov/2) * 2; 
    m_worldConvertCache.resolutionX = videoMode.resolutionX; 
    m_worldConvertCache.resolutionY = videoMode.resolutionY; 
    m_worldConvertCache.halfResX = m_worldConvertCache.resolutionX/2; 
    m_worldConvertCache.halfResY = m_worldConvertCache.resolutionY/2; 
    m_worldConvertCache.coeffX = m_worldConvertCache.resolutionX/m_worldConvertCache.xzFactor; 
    m_worldConvertCache.coeffY = m_worldConvertCache.resolutionY/m_worldConvertCache.yzFactor; 
} 

struct WorldConversionCache 
    { 
     float xzFactor; 
     float yzFactor; 
     float coeffX; 
     float coeffY; 
     int resolutionX; 
     int resolutionY; 
     int halfResX; 
     int halfResY; 
    } m_worldConvertCache; 

OpenNI GitHub repository

から取られたすべての

水平と垂直のフレームは、各フレームの説明から直接取得できます。