SR300Camera.cpp

#define NOMINMAX
#define _WINSOCKAPI_

#include "SR300Camera.h"

#include "util_render.h"
#include "Visualizer.h"
#include "Converter.h"

#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;
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) {
                cv::Mat img;
                Converter::ConvertPXCImageToOpenCVMat(sample->depth, &img);

                if (sample->depth && !renderd.RenderFrame(sample->depth)) break;
                PXCImage::ImageData depthImage;
                PXCImage *depthMap = sample->depth;
                depthMap->AcquireAccess(PXCImage::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;
                    }*/
                    //xyzBuffer.push_back(cv::Point3f(pos3D[k].x, pos3D[k].y, pos3D[k].z));
                    xyzBuffer.emplace_back(cv::Point3f(pos3D[k].x, pos3D[k].y, pos3D[k].z));


                }

                //xyzMap = cv::Mat(xyzBuffer);
                cv::Mat xyzMap{ xyzBuffer };


            }
            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;
}




/***
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];
}