ProjectDepthToCamera

[Michael_S_18]

Hello,

I'm currently trying to project a 2D depth image to 3D point cloud data. Before, I used QueryVertices top directly get the 3D data but it only gives digital data where I want it to be more detailed and without the value getting rounded.

So, currently I'm trying to use ProjectDepthToCamera in PXCProjection to convert the data, but I couldn't get it to work.

Here is a part of my source code:

    PXCCapture::Sample *sample = sm->QuerySample();
    colorFrame = sample->color;
    depthFrame = sample->depth;

    PXCImage::ImageData imgData = {};
    pxcStatus sts = depthFrame->AcquireAccess
        (PXCImage::ACCESS_READ, PXCImage::PIXEL_FORMAT_DEPTH_RAW, &imgData);

    points_buf = (unsigned short*)imgData.planes[0];

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_ptr
        (new pcl::PointCloud<pcl::PointXYZRGB>(m_depthSize));

    for(int j = 0; j < CAP_DEPTH_HEIGHT; j++)
        for(int i = 0; i < CAP_DEPTH_WIDTH; i++)
        {
            unsigned int idx = 3 *( j * CAP_DEPTH_WIDTH + i );

            m_depthPoints[idx/3].x = (pxcF32)points_buf[idx  ];
            m_depthPoints[idx/3].y = (pxcF32)points_buf[idx+1];
            m_depthPoints[idx/3].z = (pxcF32)points_buf[idx+2];
        }

    pxcStatus sts = m_projection->ProjectDepthToCamera(m_depthSize, m_depthPoints, m_worldPoints);
    if( sts < PXC_STATUS_NO_ERROR) cout << "failed to map depth to world" << endl;
    for(int j = 0; j < CAP_DEPTH_HEIGHT; j++)
        for(int i = 0; i < CAP_DEPTH_WIDTH; i++)
        {
            unsigned int idx = j * CAP_DEPTH_WIDTH + i;
            cloud_ptr->points[idx].x = -m_worldPoints[idx].x;
            cloud_ptr->points[idx].y =  m_worldPoints[idx].y;
            cloud_ptr->points[idx].z =  m_worldPoints[idx].z;
        }
~~~~~~~~

m_depthSize is int value, m_depthPoints and m_worldPoints is PXCPoint3DF32.

Anybody knows what I'm doing wrong? Or maybe someone can tell me how to project it without using ProjectDepthToCamera?

Thank you,

Michael

[Michael_S_18]

Any help would be very thankful, anyone?

[jb455]

Where is your error? I'm doing something like this to get depth values of colour points, but in C#. Basic pseudocode is:

1. Get depth image, imageData, depth pixels (depthdata.ToFloatArray) & create projection.
2. Create array of colour points
3. Give the colour array and depthData to projection.MapColorToDepth (I usually only need to do a couple of points at a time which is fine though one use case requires doing this for the whole colour image which takes significant processing time - the docs state this method isn't designed for such a large number of points but none of the other provided methods work.)
4. The MapColorToDepth method annoyingly only outputs X&Y coordinates in the depth image so we have to loop through the output array from the previous step and get the depth value of each point from the depthdata.ToFloatArray variable.
5. Et voila! We now have an array of depth values aligned to the colour image.

I have tried doing this a number of different ways, in particular using the UVMap and InvUVMap though neither of those methods seem to do what they're supposed to (in C# at least. The only working samples I could find of them are in C++ and they work fine but when I translate them to C# they stop working).

[Michael_S_18]

Actually it's not an error, the function worked, but the result is messed up and I can't get what's wrong with it.

Now I just realized that I put weird values into the depth points I passed onto ProjectDepthToCamera because I was confused between depthFrame and imgData. Problem solved.

Although, thank you James, for the explanation about mapping depth to color image. I was kinda confused about that one, too.

Sorry for the ruckus and thanks again.

Best regards,

Michael.

[jb455]

Was the mistake you found in this line:  points_buf = (unsigned short*)imgData.planes[0];?

I thought I'd try to get this working in C# but that's where I'm stuck. What was your solution?

Thanks!

[Michael_S_18]

Sort of. I made a mistake on how I insert the depth data into m_depthPoints, so I changed it to:

memcpy(depthImg.data, imgData.planes[0], imgData.pitches[0] * CAP_DEPTH_HEIGHT); //depthImg is cv::Mat
    for(int j = 0; j < CAP_DEPTH_HEIGHT; j++)
        for(int i = 0; i < CAP_DEPTH_WIDTH; i++)
        {
            unsigned int idx = j * CAP_DEPTH_WIDTH + i;
            m_depthPoints[idx].x = i;
            m_depthPoints[idx].y = j;
            m_depthPoints[idx].z = depthImg.at<USHORT>(idx) * depth_unit/1000;  //depth_unit is value of device.QueryDepthUnit()
        }   

This way I can use ProjectDepthToCamera fine, giving me world coordinates of the depth data in mm.

I did project world coordinates to color image using ProjectCameraToColor to give texture to my point cloud (using this, I can get the color information of each point in the world coordinates). I don't use MapDepthToColor or MapColorToDepth so I'm not sure how it works, but if it doesn't solve your problem, how about trying ProjectColorToCamera followed by ProjectCameraToDepth?