7 Replies Latest reply on Jun 8, 2017 1:23 PM by Intel Corporation

    why all the pos3d values are zero while posUVZ values are not zero and how to fix it?

    MonaJalal

      In the following snippet, when I print the posUVZ values, they are non-zero but after I pass them to ProjectDepthToCamera(wxhDepth, posUVZ, pos3D) all the pos3D values happen to be zero. Any though on why is this happening and how to fix it?

       

      /***
      Reads the depth data from the sensor and fills in the matrix
      ***/

      void SR300Camera::fillInZCoords()
      {

        
      PXCImage::ImageData depthImage;
        
      PXCImage *depthMap = sample->depth;
        depthMap
      ->AcquireAccess(PXCImage::ACCESS_READ, &depthImage);
        
      PXCImage::ImageInfo imgInfo = depthMap->QueryInfo();
        
      int depth_stride = depthImage.pitches[0] / sizeof(pxcU16);
        
      Projection * projection = device->CreateProjection();
        
      unsigned int wxhDepth = depth_width * depth_height;
        
      // create the array of depth coordinates + depth value (posUVZ) within the defined ROI
        
      PXCPoint3DF32* posUVZ = new PXCPoint3DF32[wxhDepth];
        pxcU16
      *dpixels = (pxcU16*)depthImage.planes[0];
        
      unsigned int dpitch = depthImage.pitches[0] / sizeof(pxcU16); /* aligned width */

        
      for (unsigned int yy = 0, k = 0; yy < depth_height; yy++)
        
      {
        
      for (unsigned int xx = 0; xx < depth_width; xx++, k++)
        
      {
        posUVZ
      [k].x = (pxcF32)xx;
        posUVZ
      [k].y = (pxcF32)yy;
        posUVZ
      [k].z = (pxcF32)dpixels[yy * dpitch + xx];
        
      // cout << "xx is " << posUVZ[k].x << endl;
        
      // cout << "yy is " << posUVZ[k].y << endl;
        
      // cout << "zz is " << posUVZ[k].z<< endl;
        
      }
        
      }


        
      // convert the array of depth coordinates + depth value (posUVZ) into the world coordinates (pos3D) in mm
        
      PXCPoint3DF32* pos3D = new PXCPoint3DF32[wxhDepth];

        projection
      ->ProjectDepthToCamera(wxhDepth, posUVZ, pos3D);
        
      /*
        if (projection->ProjectDepthToCamera(wxhDepth, posUVZ, pos3D) < PXC_STATUS_NO_ERROR)
        {
        delete[] posUVZ;
        delete[] pos3D;
        cout << "projection unsucessful";
        return;
        }
        */


        
      for (unsigned int yy = 0, k = 0; yy < depth_height; yy++)
        
      {
        
      for (unsigned int xx = 0; xx < depth_width; xx++, k++)
        
      {
        cout
      << "xx is " << pos3D[k].x*1000.0 << endl;
        cout
      << "yy is " << pos3D[k].y*1000.0 << endl;
        cout
      << "zz is " << pos3D[k].z*1000.0 << endl;
        xyzBuffer
      .push_back(cv::Point3f(pos3D[k].x, pos3D[k].y, pos3D[k].z));
        
      }
        
      }

        
      /*
        for (int idx = 0; idx < wxhDepth; idx++) {

        cout << "x is " << pos3D[idx].x*1000.0 << endl;
        cout << "y is " << pos3D[idx].y*1000.0 << endl;
        cout << "z is " << pos3D[idx].z*1000.0 << endl;
        xyzBuffer.push_back(cv::Point3f(pos3D[idx].x, pos3D[idx].y, pos3D[idx].z));
        }
        */


        
      //xyzMap = cv::Mat(xyzMap.size(), xyzMap.type, &pos3D);
        xyzMap
      = cv::Mat(xyzBuffer);
        cout
      << "xyzMap = " << endl << " " << xyzMap << endl << endl;

        projection
      ->Release();
        
      delete[] posUVZ;
        
      delete[] pos3D;

      };

        • 1. Re: why all the pos3d values are zero while posUVZ values are not zero and how to fix it?
          Intel Corporation
          This message was posted on behalf of Intel Corporation

          Hi MonaJalal,

          Thanks for reaching out.

          I will investigate more about this, and I will let you know as soon as I have helpful information.

          Have a nice day.

          Regards,
          Leonardo R.

          • 2. Re: why all the pos3d values are zero while posUVZ values are not zero and how to fix it?
            Intel Corporation
            This message was posted on behalf of Intel Corporation

            Hi MonaJalal,

            Have you checked the Projection Sample in the SDK?  https://software.intel.com/sites/landingpage/realsense/camera-sdk/v2016r3/documentation/html/index.html?sample_projection_cpp.html. I was checking the source code and it uses the function ProjectDepthToCamera, so I recommend you to check that sample and use it as a reference for your code.

            Have a nice day.

            Regards,
            Leonardo R.

            • 3. Re: why all the pos3d values are zero while posUVZ values are not zero and how to fix it?
              MonaJalal

              As you see above I have used the same function. Here's the same question I asked in Stackoverflow with better indentation c++ - all the pos3d values are zero while posUVZ values are not zero - Stack Overflow

              • 4. Re: why all the pos3d values are zero while posUVZ values are not zero and how to fix it?
                Intel Corporation
                This message was posted on behalf of Intel Corporation

                Hi MonaJalal,

                I have checked your code, but I didn't find any errors. Normally in these cases we bring the tools and samples to help the customer with their custom codes, that's the reason that I have provided you the sample code, to help you to check how the function is used and try to adapt it to your code to make sure that it will work. 

                Having said this, I want to test your code under the same conditions that you are using it, so could you please provide the solution to test it? Please include any detail that you consider helpful.

                I can't assure you that I'm going to find the problem and/or fix it, but I will try my best to help you with this.

                I will be waiting for your reply.

                Regards,
                Leonardo R.

                • 5. Re: why all the pos3d values are zero while posUVZ values are not zero and how to fix it?
                  MonaJalal

                  Hi Leonardo,

                   

                  Here's the complete solution

                   

                  QueryVertices_allZero.zip - Google Drive

                   

                  But basically only this file (SR300Camera.cpp) has problem:

                   

                  #define NOMINMAX
                  #define _WINSOCKAPI_

                  #include "SR300Camera.h"
                  #include "RealSense/SenseManager.h"
                  #include "RealSense/SampleReader.h"
                  #include "util_render.h"
                  #include "Visualizer.h"
                  #include "RealSense/Session.h"
                  #include <opencv2/opencv.hpp>


                  #include <iostream>

                  /***
                  Private constructor for the SR300 Camera depth sensor
                  ***/
                  using namespace std;
                  //using namespace Intel::RealSense;

                  PXCSenseManager *pp = PXCSenseManager::CreateInstance();
                  PXCCapture::Device *device;
                  PXCCaptureManager *cm;

                  const int depth_fps = 30;
                  const int depth_width = 640;
                  const int depth_height = 480;
                  cv::Size bufferSize;
                  const PXCCapture::Sample *sample;
                  //std::vector<CV_32FC3> xyzBuffer;
                  std::vector<cv::Point3f> xyzBuffer;


                  SR300Camera::SR300Camera(bool use_live_sensor)
                  {

                  CONFIDENCE_THRESHHOLD = (60.0f / 255.0f*500.0f);
                  X_DIMENSION = depth_width;
                  Y_DIMENSION = depth_height;

                  int num_pixels = X_DIMENSION * Y_DIMENSION;
                  PXCPoint3DF32* vertices = new PXCPoint3DF32[num_pixels];

                  if (!use_live_sensor) {
                    return;
                  }

                  int res;
                  std::cout << "Trying to open SR300 camera\n";

                   

                  if (!pp) {
                    wprintf_s(L"Unable to create the SenseManager\n");
                  }

                  cm = pp->QueryCaptureManager();
                  //Status sts= STATUS_NO_ERROR;
                  Status sts = STATUS_DATA_UNAVAILABLE; //mona is this the correct initialization?
                  UtilRender renderc(L"Color"), renderd(L"Depth"), renderi(L"IR"), renderr(L"Right"), renderl(L"Left");

                  do {
                    bool revert = false;
                    pp->EnableStream(PXCCapture::StreamType::STREAM_TYPE_DEPTH,
                     X_DIMENSION, Y_DIMENSION, depth_fps);
                    revert = true;
                    sts = pp->Init();
                    if (sts < Status::STATUS_NO_ERROR) {

                     if (revert) {

                      pp->Close();
                      pp->EnableStream(PXCCapture::STREAM_TYPE_DEPTH);
                      sts = pp->Init();
                      if (sts < Status::STATUS_NO_ERROR) {
                       pp->Close();
                       //pp->EnableStream(Capture::STREAM_TYPE_COLOR);
                       sts = pp->Init();
                      }
                     }
                     if (sts < Status::STATUS_NO_ERROR) {
                      wprintf_s(L"Failed to locate any video stream(s)\n");
                      pp->Release();
                     }
                    }

                    device = pp->QueryCaptureManager()->QueryDevice();

                    device->ResetProperties(PXCCapture::STREAM_TYPE_ANY);

                    for (int nframes = 0; ; nframes++) {
                     sts = pp->AcquireFrame(false);

                     if (sts < Status::STATUS_NO_ERROR) {
                      if (sts == Status::STATUS_STREAM_CONFIG_CHANGED) {
                       wprintf_s(L"Stream configuration was changed, re-initilizing\n");
                       pp->Close();
                      }
                      break;
                     }

                     sample = pp->QuerySample();

                     if (sample) {
                      if (sample->depth && !renderd.RenderFrame(sample->depth)) break;
                      PXCImage::ImageData depthImage;
                      PXCImage *depthMap = sample->depth;
                      //depthMap->AcquireAccess(Image::ACCESS_READ, &depthImage);
                      depthMap->AcquireAccess(PXCImage::ACCESS_WRITE, &depthImage); //mona is this the correct flag?
                      PXCImage::ImageInfo imgInfo = depthMap->QueryInfo();
                      int depth_stride = depthImage.pitches[0] / sizeof(pxcU16);
                      PXCProjection * projection = device->CreateProjection();
                      cout << "projection is: " << projection;
                      cout << "device is: " << device;
                      pxcU16 *dpixels = (pxcU16*)depthImage.planes[0];
                      unsigned int dpitch = depthImage.pitches[0] / sizeof(pxcU16);
                      PXCPoint3DF32 *pos3D = new PXCPoint3DF32[num_pixels];

                      projection->QueryVertices(depthMap, pos3D);

                      for (int k = 0; k < num_pixels; k++) {
                       if (pos3D[k].x != 0) {
                        cout << " xx is: " << pos3D[k].x << endl;
                       }
                       if (pos3D[k].y != 0) {
                        cout << " yy is: " << pos3D[k].y << endl;
                       }
                       if (pos3D[k].z != 0) {
                        cout << " zz is: " << pos3D[k].z << endl;
                       }
                      }

                     }
                     printf("opened sensor\n");
                     pp->ReleaseFrame();
                     printf("acquired image\n");
                     cout << "xyzMap = " << endl << " " << xyzMap << endl << endl;

                    }

                  } while (sts == Status::STATUS_STREAM_CONFIG_CHANGED);

                  //numRows = width and numCol = height? ?
                  //numPixels = dd.img.numRows * dd.img.numColumns; // Number of pixels in camera
                  numPixels = depth_width * depth_height;
                  dists = new float[3 * numPixels]; // Dists contains XYZ values. needs to be 3x the size of numPixels
                  amps = new float[numPixels];
                  //frame = cvCreateImage(cvSize(dd.img.numColumns, dd.img.numRows), 8, 3); // Create the frame
                  frame = cvCreateImage(cvSize(depth_width, depth_height), 8, 3); // Create the frame

                  KF.init(6, 3, 0);
                  KF.transitionMatrix = (cv::Mat_<float>(6, 6) << 1, 0, 0, 1, 0, 0,
                    0, 1, 0, 0, 1, 0,
                    0, 0, 1, 0, 0, 1,
                    0, 0, 0, 1, 0, 0,
                    0, 0, 0, 0, 1, 0,
                    0, 0, 0, 0, 0, 1);
                  measurement = cv::Mat_<float>::zeros(3, 1);

                  //Initaite Kalman
                  KF.statePre.setTo(0);
                  setIdentity(KF.measurementMatrix);
                  setIdentity(KF.processNoiseCov, cv::Scalar::all(.001)); // Adjust this for faster convergence - but higher noise
                  setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-1));
                  setIdentity(KF.errorCovPost, cv::Scalar::all(.1));
                  }

                  /***
                  Public deconstructor for the SR300 Camera depth sensor
                  ***/
                  SR300Camera::~SR300Camera() {};

                  void SR300Camera::destroyInstance()
                  {
                  printf("closing sensor\n");
                  pp->Release();
                  printf("sensor closed\n");
                  }

                   

                   

                   


                  /***
                  Create xyzMap, zMap, ampMap, and flagMap from sensor input
                  ***/
                  void SR300Camera::update()
                  {
                  initilizeImages();

                  fillInAmps();
                  }

                   

                  /***
                  Reads the amplitude data from the sensor and fills in the matrix
                  ***/
                  void SR300Camera::fillInAmps()
                  {
                  //int res = pmdGetAmplitudes(hnd, amps, numPixels * sizeof(float));
                  //float * dataPtr = amps;
                  //ampMap.data = (uchar *)amps;
                  }

                  /*
                  void SR300Camera::ConvertPXCImageToOpenCVMat(PXCImage *inImg, cv::Mat *outImg) const
                  {
                  int cvDataType;
                  int cvDataWidth;


                  PXCImage::ImageData data;
                  inImg->AcquireAccess(PXCImage::ACCESS_READ, &data);
                  PXCImage::ImageInfo imgInfo = inImg->QueryInfo();

                  switch (data.format) {
                   
                  case PXCImage::PIXEL_FORMAT_YUY2:
                  case PXCImage::PIXEL_FORMAT_NV12:
                    throw(0); // Not implemented
                  case PXCImage::PIXEL_FORMAT_RGB32:
                    cvDataType = CV_8UC4;
                    cvDataWidth = 4;
                    break;
                  case PXCImage::PIXEL_FORMAT_RGB24:
                    cvDataType = CV_8UC3;
                    cvDataWidth = 3;
                    break;
                  case PXCImage::PIXEL_FORMAT_Y8: 
                    cvDataType = CV_8U;
                    cvDataWidth = 1;
                    break;


                  case PXCImage::PIXEL_FORMAT_DEPTH:
                  case PXCImage::PIXEL_FORMAT_DEPTH_RAW:
                    cvDataType = CV_16U;
                    cvDataWidth = 2;
                    break;
                  case PXCImage::PIXEL_FORMAT_DEPTH_F32:
                    cvDataType = CV_32F;
                    cvDataWidth = 4;
                    break;


                  case PXCImage::PIXEL_FORMAT_Y16:         
                    cvDataType = CV_16U;
                    cvDataWidth = 2;
                    break;
                  case PXCImage::PIXEL_FORMAT_Y8_IR_RELATIVE:  
                    cvDataType = CV_8U;
                    cvDataWidth = 1;
                    break;
                  }

                  // suppose that no other planes
                  if (data.planes[1] != NULL) throw(0); // not implemented
                              // suppose that no sub pixel padding needed
                  if (data.pitches[0] % cvDataWidth != 0) throw(0); // not implemented

                  outImg->create(imgInfo.height, data.pitches[0] / cvDataWidth, cvDataType);

                  memcpy(outImg->data, data.planes[0], imgInfo.height*imgInfo.width*cvDataWidth * sizeof(pxcBYTE));

                  inImg->ReleaseAccess(&data);
                  }
                  */

                  /***
                  Returns the X value at (i, j)
                  ***/
                  float SR300Camera::getX(int i, int j) const
                  {
                  //int flat = j * dd.img.numColumns * 3 + i * 3;
                  int flat = j * depth_width * 3 + i * 3;
                  return dists[flat];
                  }

                  /***
                  Returns the Y value at (i, j)
                  ***/
                  float SR300Camera::getY(int i, int j) const
                  {
                  //int flat = j * dd.img.numColumns * 3 + i * 3;
                  int flat = j * depth_width * 3 + i * 3;
                  return dists[flat + 1];
                  }

                  /***
                  Returns the Z value at (i, j)
                  ***/
                  float SR300Camera::getZ(int i, int j) const
                  {
                  //int flat = j * dd.img.numColumns * 3 + i * 3;
                  int flat = j * depth_width * 3 + i * 3;
                  return dists[flat + 2];
                  }

                  • 6. Re: why all the pos3d values are zero while posUVZ values are not zero and how to fix it?
                    Intel Corporation
                    This message was posted on behalf of Intel Corporation

                    Hi MonaJalal,

                    Thanks for the code.

                    I am going to test your code, just to make sure, are you using the SDK R2 or the SDK R3?

                    I appreciate your patience.
                     
                    Regards,
                    Leonardo R.

                    • 7. Re: why all the pos3d values are zero while posUVZ values are not zero and how to fix it?
                      Intel Corporation
                      This message was posted on behalf of Intel Corporation

                      Hi MonaJalal,

                       

                      I tried my best to find the issue in your code, but unfortunately I was not able to find it. As I mentioned before, the best recommendation that I can give you is to use the SDK sample https://software.intel.com/sites/landingpage/realsense/camera-sdk/v2016r3/documentation/html/index.html?sample_projection_cpp.html, just consider that the function ProjectDepthToCamera(wxhDepth, posUVZ, pos3D) uses a large amount of pixels and a lot of them are zero.

                       

                      I really appreciate your patience.

                       

                      Regards,
                      Leonardo R.