Creat a 3D map from 3 Depth Images

[takuro_m_]

Hi all,

I try to creat a 3D map from 3 Depth Images. This trying is based on FF_ObjectTracking of sample. And I use Intel Realsense SR300.

First, I got coordinates (x,y,z) from 3 Depth Images . Second, I transformed the coordinates to "world coordinate system". Third, I exported the world coordinates to texts. Finally, I created a 3D map from the texts.

Now, I have a trouble at Second Step maybe.

I show Code:

void getDepthData(PXCImage* depthFrame, int DEPTH_HEIGHT, int DEPTH_WIDTH , int count, PXCProjection *project) // This is my new function
{
    int i, j;
    float x, y, z;

    FILE *p,*manage,*inv;
    char buf[100];
    float inv_rotMat[4][4]; // This is Inverse matrix

    PXCImage::ImageData Depthdata;

    unsigned short *outdata;
    PXCPoint3DF32 test[640*480],intest[640*480];

    // Get Depth data
    auto sts = depthFrame->AcquireAccess(PXCImage::ACCESS_READ,PXCImage::PIXEL_FORMAT_DEPTH, &Depthdata);
    if (sts < PXC_STATUS_NO_ERROR) {
        return;
    }

    outdata = (unsigned short *)Depthdata.planes[0];

//Get Inverse matrix

    sprintf(buf, "Depth_%d.asc", count);
    fopen_s(&p,buf,"w");
    fopen_s(&manage, "ManageFile.txt", "a");
    fprintf(manage, "%s\n", buf);
    sprintf(buf, "inv_Matrix_%d.txt", count);
    fopen_s(&inv, buf, "r");
    for (i = 0; i < 4; i++) {
        for (j = 0; j < 4; j++) {
            fscanf_s(inv, "%f", &inv_rotMat);
        }
    }

    int intnum;

    project->QueryVertices(depthFrame,  test);// Get coordinates(x,y,z) 640*480
    intnum=0;

//Transform the coordinates by Inverse matrix

    for (i = -DEPTH_HEIGHT / 2; i < DEPTH_HEIGHT / 2; i++) {
        for (j = -DEPTH_WIDTH / 2; j < DEPTH_WIDTH / 2; j++) {
            if (test[intnum].z != 0) {
                x = inv_rotMat[0][0] * (float)test[intnum].x + inv_rotMat[0][1] * (float)test[intnum].y + inv_rotMat[0][2] * (float)test[intnum].z + inv_rotMat[0][3];
                y = inv_rotMat[1][0] * (float)test[intnum].x + inv_rotMat[1][1] * (float)test[intnum].y + inv_rotMat[1][2] * (float)test[intnum].z + inv_rotMat[1][3];
                z = inv_rotMat[2][0] * (float)test[intnum].x + inv_rotMat[2][1] * (float)test[intnum].y + inv_rotMat[2][2] * (float)test[intnum].z + inv_rotMat[2][3];
                fprintf_s(p, "%f %f %f\n", x, y, z); //Export the "world coordinates"
                
            }
            intnum++;
        }
    }

    fclose(inv);
    fclose(p);
    fclose(manage);

    depthFrame->ReleaseAccess(&Depthdata);
}

This function is called in ProjectPoint function of main.cpp .

like this

getDepthData(sample->depth,info.height,info.width,filenum,projection);

A result is no good. Because it can not align the coordinates. The coordinates are not matched.

Plese help me!

Best regards,

Takuro.