SHARE
TWEET

SR300Camera.cpp

lamiastella May 23rd, 2017 66 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "SR300Camera.h"
  2. #include "RealSense/SenseManager.h"
  3. #include "RealSense/SampleReader.h"
  4. #include "util_render.h"
  5. #include "Visualizer.h"
  6.  
  7.  
  8. #include <iostream>
  9.  
  10. /***
  11. Private constructor for the SR300 Camera depth sensor
  12. ***/
  13. using namespace std;
  14. //using namespace Intel::RealSense;
  15.  
  16. PXCSenseManager *pp = PXCSenseManager::CreateInstance();
  17. PXCCapture::Device *device;
  18. PXCCaptureManager *cm;
  19.  
  20. const int depth_fps = 30;
  21. const int depth_width = 640;
  22. const int depth_height = 480;
  23. cv::Size bufferSize;
  24. const PXCCapture::Sample *sample;
  25. //std::vector<CV_32FC3> xyzBuffer;
  26. std::vector<cv::Point3f> xyzBuffer;
  27.  
  28.  
  29. SR300Camera::SR300Camera(bool use_live_sensor)
  30. {
  31.  
  32.     CONFIDENCE_THRESHHOLD = (60.0f / 255.0f*500.0f);
  33.     //INVALID_FLAG_VALUE = PMD_FLAG_INVALID;
  34.     X_DIMENSION = depth_width;
  35.     Y_DIMENSION = depth_height;
  36.  
  37.     int num_pixels = X_DIMENSION * Y_DIMENSION;
  38.     PXCPoint3DF32* vertices = new PXCPoint3DF32[num_pixels];
  39.  
  40.     if (!use_live_sensor) {
  41.         return;
  42.     }
  43.  
  44.     int res;
  45.     std::cout << "Trying to open SR300 camera\n";
  46.  
  47.  
  48.  
  49.     if (!pp) {
  50.         wprintf_s(L"Unable to create the SenseManager\n");
  51.     }
  52.  
  53.     /* Sets file recording or playback */
  54.     cm = pp->QueryCaptureManager();
  55.     //Status sts= STATUS_NO_ERROR;
  56.     Status sts = STATUS_DATA_UNAVAILABLE; //mona is this the correct initialization?
  57.                                           // Create stream renders
  58.     UtilRender renderc(L"Color"), renderd(L"Depth"), renderi(L"IR"), renderr(L"Right"), renderl(L"Left");
  59.  
  60.     do {
  61.         bool revert = false;
  62.         pp->EnableStream(PXCCapture::StreamType::STREAM_TYPE_DEPTH,
  63.             X_DIMENSION, Y_DIMENSION, depth_fps);
  64.         revert = true;
  65.         sts = pp->Init();
  66.         if (sts < Status::STATUS_NO_ERROR) {
  67.  
  68.             if (revert) {
  69.  
  70.                 pp->Close();
  71.                 pp->EnableStream(PXCCapture::STREAM_TYPE_DEPTH);
  72.                 sts = pp->Init();
  73.                 if (sts < Status::STATUS_NO_ERROR) {
  74.                     pp->Close();
  75.                     //pp->EnableStream(Capture::STREAM_TYPE_COLOR);
  76.                     sts = pp->Init();
  77.                 }
  78.             }
  79.             if (sts < Status::STATUS_NO_ERROR) {
  80.                 wprintf_s(L"Failed to locate any video stream(s)\n");
  81.                 pp->Release();
  82.             }
  83.         }
  84.  
  85.         device = pp->QueryCaptureManager()->QueryDevice();
  86.  
  87.         device->ResetProperties(PXCCapture::STREAM_TYPE_ANY);
  88.  
  89.         for (int nframes = 0; ; nframes++) {
  90.             sts = pp->AcquireFrame(false);
  91.  
  92.             if (sts < Status::STATUS_NO_ERROR) {
  93.                 if (sts == Status::STATUS_STREAM_CONFIG_CHANGED) {
  94.                     wprintf_s(L"Stream configuration was changed, re-initilizing\n");
  95.                     pp->Close();
  96.                 }
  97.                 break;
  98.             }
  99.  
  100.             sample = pp->QuerySample();
  101.  
  102.             if (sample) {
  103.                 cv::Mat img;
  104.                 SR300Camera::ConvertPXCImageToOpenCVMat(sample->depth, &img);
  105.                 cv::imshow("depth from OpenARK", Visualizer::visualizeDepthMap(img));
  106.                 if (sample->depth && !renderd.RenderFrame(sample->depth)) break;
  107.                 //if (sample->color && !renderc.RenderFrame(sample->color)) break;
  108.                 //if (sample->ir && !renderi.RenderFrame(sample->ir))    break;
  109.                 PXCImage::ImageData depthImage;
  110.                 PXCImage *depthMap = sample->depth;
  111.                 //depthMap->AcquireAccess(Image::ACCESS_READ, &depthImage);
  112.                 depthMap->AcquireAccess(PXCImage::ACCESS_WRITE, &depthImage);
  113.  
  114.                 PXCImage::ImageInfo imgInfo = depthMap->QueryInfo();
  115.                 int depth_stride = depthImage.pitches[0] / sizeof(pxcU16);
  116.                 PXCProjection * projection = device->CreateProjection();
  117.                 cout << "projection is: " << projection;
  118.                 cout << "device is: " << device;
  119.                 //int num_pixels = depth_height * depth_width;
  120.  
  121.                 //projection->QueryVertices(depthMap, vertices);
  122.                 pxcU16 *dpixels = (pxcU16*)depthImage.planes[0];
  123.                 unsigned int dpitch = depthImage.pitches[0] / sizeof(pxcU16);
  124.  
  125.                 /*for (int k = 0; k < num_pixels; k++) {
  126.                 cout << "xx is " << vertices[k].x << endl;
  127.                 cout << "yy is " << vertices[k].y << endl;
  128.                 cout << "zz is " << vertices[k].z << endl;
  129.                 }*/
  130.                 //cout << "xx is " << vertices[0].x << endl;
  131.                 //cout << "yy is " << vertices[0].y << endl;
  132.                 //cout << "zz is " << vertices[0].z << endl;
  133.                 //cout << "*******************" << endl;
  134.  
  135.                 PXCPoint3DF32* posUVZ = new PXCPoint3DF32[num_pixels];
  136.                 PXCPoint3DF32 *pos3D = new PXCPoint3DF32[num_pixels];
  137.  
  138.                 for (int y = 0; y < Y_DIMENSION; y++) {
  139.                     for (int x = 0; x < X_DIMENSION; x++) {
  140.                         // populate the point with depth image pixel coordinates and the depth value
  141.                         posUVZ[y*Y_DIMENSION + x].x = (pxcF32)x;
  142.                         posUVZ[y*Y_DIMENSION + x].y = (pxcF32)y;
  143.                         posUVZ[y*Y_DIMENSION + x].z = dpixels[nframes*X_DIMENSION*Y_DIMENSION + y*Y_DIMENSION + x];
  144.                     }
  145.                 }
  146.  
  147.                 //projection->ProjectDepthToCamera(num_pixels, posUVZ, pos3D);
  148.  
  149.  
  150.                 /*
  151.                 for (unsigned int yy = 0, k = 0; yy < Y_DIMENSION; yy++)
  152.                 {
  153.                 for (unsigned int xx = 0; xx < X_DIMENSION; xx++, k++)
  154.                 {
  155.                 posUVZ[k].x = (pxcF32)xx;
  156.                 posUVZ[k].y = (pxcF32)yy;
  157.                 posUVZ[k].z = (pxcF32)dpixels[yy * dpitch + xx];
  158.                 //  cout << "xx is " << posUVZ[k].x << endl;
  159.                 // cout << "yy is " << posUVZ[k].y << endl;
  160.                 // cout << "zz is " << posUVZ[k].z<< endl;
  161.                 }
  162.                 }
  163.                 */
  164.  
  165.  
  166.  
  167.  
  168.                 // convert the array of depth coordinates + depth value (posUVZ) into the world coordinates (pos3D) in mm
  169.  
  170.                 //Step 1: Set up the point cloud in camera coordinates and do the projection
  171.  
  172.  
  173.  
  174.                 //projection->ProjectDepthToCamera(num_pixels, posUVZ, pos3D);
  175.  
  176.                 if (projection->ProjectDepthToCamera(num_pixels, posUVZ, pos3D) < PXC_STATUS_NO_ERROR)
  177.                 {
  178.                     delete[] posUVZ;
  179.                     delete[] pos3D;
  180.                     cout << "projection unsucessful";
  181.                     return;
  182.                 }
  183.  
  184.                 cout << "*************" << endl;
  185.                 for (unsigned int yy = 0, k = 0; yy < Y_DIMENSION; yy++)
  186.                 {
  187.                     for (unsigned int xx = 0; xx < X_DIMENSION; xx++, k++)
  188.                     {
  189.                         //cout << "xx is " << pos3D[k].x << endl;
  190.                         //cout << "yy is " << pos3D[k].y << endl;
  191.                         //cout << "zz is " << pos3D[k].z << endl;
  192.                         xyzBuffer.push_back(cv::Point3f(pos3D[k].x, pos3D[k].y, pos3D[k].z));
  193.                     }
  194.                 }
  195.                 cout << "/////////\\\\\\\\" << endl;
  196.                 projection->Release();
  197.                 depthMap->ReleaseAccess(&depthImage); //is this in the right place mona?
  198.                 memcpy(xyzMap.data, xyzBuffer.data(), xyzBuffer.size() * sizeof(CV_32FC3));
  199.             }
  200.             printf("opened sensor\n");
  201.             pp->ReleaseFrame();
  202.             printf("acquired image\n");
  203.             cout << "xyzMap = " << endl << " " << xyzMap << endl << endl;
  204.  
  205.         }
  206.  
  207.     } while (sts == Status::STATUS_STREAM_CONFIG_CHANGED);
  208.  
  209.     //numRows = width and numCol = height? ?
  210.     //numPixels = dd.img.numRows * dd.img.numColumns; // Number of pixels in camera
  211.     numPixels = depth_width * depth_height;
  212.     dists = new float[3 * numPixels]; // Dists contains XYZ values. needs to be 3x the size of numPixels
  213.     amps = new float[numPixels];
  214.     //frame = cvCreateImage(cvSize(dd.img.numColumns, dd.img.numRows), 8, 3); // Create the frame
  215.     frame = cvCreateImage(cvSize(depth_width, depth_height), 8, 3); // Create the frame
  216.  
  217.     KF.init(6, 3, 0);
  218.     KF.transitionMatrix = (cv::Mat_<float>(6, 6) << 1, 0, 0, 1, 0, 0,
  219.         0, 1, 0, 0, 1, 0,
  220.         0, 0, 1, 0, 0, 1,
  221.         0, 0, 0, 1, 0, 0,
  222.         0, 0, 0, 0, 1, 0,
  223.         0, 0, 0, 0, 0, 1);
  224.     measurement = cv::Mat_<float>::zeros(3, 1);
  225.  
  226.     //Initaite Kalman
  227.     KF.statePre.setTo(0);
  228.     setIdentity(KF.measurementMatrix);
  229.     setIdentity(KF.processNoiseCov, cv::Scalar::all(.001)); // Adjust this for faster convergence - but higher noise
  230.     setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-1));
  231.     setIdentity(KF.errorCovPost, cv::Scalar::all(.1));
  232.  
  233.     /* mona will test this
  234.     setIdentity(KF.measurementMatrix);
  235.     setIdentity(KF.processNoiseCov, Scalar::all(smoothness));
  236.     setIdentity(KF.measurementNoiseCov, Scalar::all(rapidness));
  237.     setIdentity(KF.errorCovPost, Scalar::all(.1));
  238.     */
  239. }
  240.  
  241. /***
  242. Public deconstructor for the SR300 Camera depth sensor
  243. ***/
  244. SR300Camera::~SR300Camera() {};
  245.  
  246. void SR300Camera::destroyInstance()
  247. {
  248.     printf("closing sensor\n");
  249.     pp->Release();
  250.     printf("sensor closed\n");
  251. }
  252.  
  253.  
  254.  
  255.  
  256.  
  257.  
  258.  
  259.  
  260. /***
  261. Create xyzMap, zMap, ampMap, and flagMap from sensor input
  262. ***/
  263. void SR300Camera::update()
  264. {
  265.     initilizeImages();
  266.  
  267.     fillInAmps();
  268. }
  269.  
  270.  
  271.  
  272. /***
  273. Reads the amplitude data from the sensor and fills in the matrix
  274. ***/
  275. void SR300Camera::fillInAmps()
  276. {
  277.     //int res = pmdGetAmplitudes(hnd, amps, numPixels * sizeof(float));
  278.     //float * dataPtr = amps;
  279.     //ampMap.data = (uchar *)amps;
  280. }
  281.  
  282.  
  283. void SR300Camera::ConvertPXCImageToOpenCVMat(PXCImage *inImg, cv::Mat *outImg) {
  284.     int cvDataType;
  285.     int cvDataWidth;
  286.  
  287.  
  288.     PXCImage::ImageData data;
  289.     inImg->AcquireAccess(PXCImage::ACCESS_READ, &data);
  290.     PXCImage::ImageInfo imgInfo = inImg->QueryInfo();
  291.  
  292.     switch (data.format) {
  293.         /* STREAM_TYPE_COLOR */
  294.     case PXCImage::PIXEL_FORMAT_YUY2: /* YUY2 image  */
  295.     case PXCImage::PIXEL_FORMAT_NV12: /* NV12 image */
  296.         throw(0); // Not implemented
  297.     case PXCImage::PIXEL_FORMAT_RGB32: /* BGRA layout on a little-endian machine */
  298.         cvDataType = CV_8UC4;
  299.         cvDataWidth = 4;
  300.         break;
  301.     case PXCImage::PIXEL_FORMAT_RGB24: /* BGR layout on a little-endian machine */
  302.         cvDataType = CV_8UC3;
  303.         cvDataWidth = 3;
  304.         break;
  305.     case PXCImage::PIXEL_FORMAT_Y8:  /* 8-Bit Gray Image, or IR 8-bit */
  306.         cvDataType = CV_8U;
  307.         cvDataWidth = 1;
  308.         break;
  309.  
  310.         /* STREAM_TYPE_DEPTH */
  311.     case PXCImage::PIXEL_FORMAT_DEPTH: /* 16-bit unsigned integer with precision mm. */
  312.     case PXCImage::PIXEL_FORMAT_DEPTH_RAW: /* 16-bit unsigned integer with device specific precision (call device->QueryDepthUnit()) */
  313.         cvDataType = CV_16U;
  314.         cvDataWidth = 2;
  315.         break;
  316.     case PXCImage::PIXEL_FORMAT_DEPTH_F32: /* 32-bit float-point with precision mm. */
  317.         cvDataType = CV_32F;
  318.         cvDataWidth = 4;
  319.         break;
  320.  
  321.         /* STREAM_TYPE_IR */
  322.     case PXCImage::PIXEL_FORMAT_Y16:          /* 16-Bit Gray Image */
  323.         cvDataType = CV_16U;
  324.         cvDataWidth = 2;
  325.         break;
  326.     case PXCImage::PIXEL_FORMAT_Y8_IR_RELATIVE:    /* Relative IR Image */
  327.         cvDataType = CV_8U;
  328.         cvDataWidth = 1;
  329.         break;
  330.     }
  331.  
  332.     // suppose that no other planes
  333.     if (data.planes[1] != NULL) throw(0); // not implemented
  334.                                           // suppose that no sub pixel padding needed
  335.     if (data.pitches[0] % cvDataWidth != 0) throw(0); // not implemented
  336.  
  337.     outImg->create(imgInfo.height, data.pitches[0] / cvDataWidth, cvDataType);
  338.  
  339.     memcpy(outImg->data, data.planes[0], imgInfo.height*imgInfo.width*cvDataWidth * sizeof(pxcBYTE));
  340.  
  341.     inImg->ReleaseAccess(&data);
  342. }
  343.  
  344.  
  345. /***
  346. Returns the X value at (i, j)
  347. ***/
  348. float SR300Camera::getX(int i, int j) const
  349. {
  350.     //int flat = j * dd.img.numColumns * 3 + i * 3;
  351.     int flat = j * depth_width * 3 + i * 3;
  352.     return dists[flat];
  353. }
  354.  
  355. /***
  356. Returns the Y value at (i, j)
  357. ***/
  358. float SR300Camera::getY(int i, int j) const
  359. {
  360.     //int flat = j * dd.img.numColumns * 3 + i * 3;
  361.     int flat = j * depth_width * 3 + i * 3;
  362.     return dists[flat + 1];
  363. }
  364.  
  365. /***
  366. Returns the Z value at (i, j)
  367. ***/
  368. float SR300Camera::getZ(int i, int j) const
  369. {
  370.     //int flat = j * dd.img.numColumns * 3 + i * 3;
  371.     int flat = j * depth_width * 3 + i * 3;
  372.     return dists[flat + 2];
  373. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top