Creat a 3D map from 3 Depth Images

takuro_m_ — Mon, 01 Aug 2016 15:51:14 GMT

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.

topic Creat a 3D map from 3 Depth Images in Software Archive

Creat a 3D map from 3 Depth Images