SR300Camera.cpp with tests

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



const int depth_fps = 30;
int depth_width;
int depth_height;
cv::Size bufferSize;
const PXCCapture::Sample *sample;
PXCSenseManager *sm = PXCSenseManager::CreateInstance();
PXCSession *session = sm->QuerySession();

PXCCapture::Device *device;
PXCCaptureManager *cm;

UtilRender renderc(L"Color"), renderd(L"Depth"), renderi(L"IR"), renderr(L"Right"), renderl(L"Left");
int num_pixels;
vector<cv::Point3f>  xyzBuffer;

SR300Camera::SR300Camera(bool use_live_sensor) {

    session->SetCoordinateSystem(PXCSession::CoordinateSystem::COORDINATE_SYSTEM_FRONT_DEFAULT);
    //X_DIMENSION = depth_width;
    //Y_DIMENSION = depth_height;

    if (!sm) {
        wprintf_s(L"Unable to create the SenseManager\n");
    }
    cm = sm->QueryCaptureManager();
    Status sts = STATUS_DATA_UNAVAILABLE;

    sm->EnableStream(PXCCapture::StreamType::STREAM_TYPE_DEPTH,
        X_DIMENSION, Y_DIMENSION, depth_fps);
    //revert = true;
    sts = sm->Init();
    if (sts < Status::STATUS_NO_ERROR) {
        sm->Close();
        sm->EnableStream(PXCCapture::STREAM_TYPE_DEPTH);
        sts = sm->Init();
        if (sts < Status::STATUS_NO_ERROR) {
            sm->Close();
            //pp->EnableStream(Capture::STREAM_TYPE_COLOR);
            sts = sm->Init();
        }
    }
    device = cm->QueryDevice();
    //device->ResetProperties(PXCCapture::STREAM_TYPE_ANY);

}


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

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




/***
Create xyzMap, zMap, ampMap, and flagMap from sensor input
***/
void SR300Camera::update()
{
    initilizeImages();
    fillInAmps();
    fillInZCoords();
    sm->ReleaseFrame();
}

void SR300Camera::fillInZCoords(){
    int res;
    //sts = pp->AcquireFrame(false);
    Status sts = sm ->AcquireFrame(true);


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

    sample = sm->QuerySample();
    PXCImage *depthMap = sample->depth;

    //mona if (sample->depth && !renderd.RenderFrame(sample->depth)) break;
    //renderd.RenderFrame(sample->depth);
    //cv::namedWindow("depth", CV_WINDOW_AUTOSIZE);
    //cv::imshow("depth", sample->depth);

    PXCImage::ImageData depthImage;
    depthMap->AcquireAccess(PXCImage::ACCESS_READ, &depthImage);
    cv::Mat img;
    Converter::ConvertPXCImageToOpenCVMat(depthMap, depthImage, &img);
    cv::imshow("Depth Image by OpenARK", Visualizer::visualizeDepthMap(img));
    //cout << "nrows: " << img.rows << endl;
    //cout << "ncols: " << img.cols << endl;
    //cout << " img channels: " << img.channels() << endl;
    depthMap->AcquireAccess(PXCImage::ACCESS_WRITE, &depthImage); //mona is this the correct flag?
    PXCImage::ImageInfo imgInfo = depthMap->QueryInfo();
    depth_width = imgInfo.width;
    depth_height = imgInfo.height;
    num_pixels = depth_width * depth_height;
    cout << "height is: " << depth_height << endl;
    cout << "width is: " << depth_width << endl;
    PXCProjection * projection = device->CreateProjection();
    pxcU16 *dpixels = (pxcU16*)depthImage.planes[0];
    unsigned int dpitch = depthImage.pitches[0] / sizeof(pxcU16);
    PXCPoint3DF32 *pos3D = new PXCPoint3DF32[num_pixels];

    //sts = projection->QueryVertices(depthMap, pos3D);
    sts = projection->QueryVertices(depthMap, &pos3D[0]);
    if (sts < Status::STATUS_NO_ERROR) {
        wprintf_s(L"Projection was unsuccessful! \n");
        sm->Close();
    }

    xyzBuffer.clear();
    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/1000.0f, pos3D[k].y/1000.0f, pos3D[k].z/1000.0f));

            /*for (int i = 0; i < xyzBuffer.size(); i++) {
                cout << "xyz buffer x: " << xyzBuffer.at(i).x << endl;
                cout << "xyz buffer y: " << xyzBuffer.at(i).y << endl;
                cout << "xyz buffer z: " << xyzBuffer.at(i).z << endl;
            }*/
            //xyzBuffer.emplace_back(cv::Point3f(pos3D[k].x, pos3D[k].y, pos3D[k].z));
            /*if (xyzBuffer.at(k).x != 0) {
                cout << "xyz buffer x: " << xyzBuffer.at(k).x << endl;
            }if (xyzBuffer.at(k).y != 0) {
                cout << "xyz buffer y: " << xyzBuffer.at(k).y << endl;
            }if (xyzBuffer.at(k).z != 0) {
                cout << "xyz buffer z: " << xyzBuffer.at(k).z << endl;
            }*/
    }


    //cout << "vector length is: " << xyzBuffer.size() << endl;
    //cout << "size of " << sizeof(CV_32FC3);

    //cout << "xyzBuffer = " << endl << " " << xyzBuffer << endl << endl;
    //xyzMap = cv::Mat(xyzBuffer);
    //cv::Mat xyzMap{ xyzBuffer };

    /*double max_x = xyzBuffer[0].x;
    double max_y = xyzBuffer[1].y;
    double max_z = xyzBuffer[2].z;
    for (int i = 0; i < xyzBuffer.size(); i++) {
        if (xyzBuffer[i].x > max_x) {
            max_x = xyzBuffer[i].x;
        }
        if (xyzBuffer[i].y > max_y) {
            max_y = xyzBuffer[i].y;
        }
        if (xyzBuffer[i].z > max_z) {
            max_z = xyzBuffer[i].z;
        }
    }
    */
    //xyzMap = (CV_32FC3) xyzBuffer;

    //cv::Point3f pmax= std::accumulate(xyzBuffer.cbegin(), xyzBuffer.cend(), [](const cv::Point3f&a, const cv::Point3f&b) {return cv::Point3f(max(a.x, b.x), max(a.y, b.y), max(a.z, b.z)); }
    //memcpy(xyzMap.data, xyzBuffer.data(), xyzBuffer.size()* xyzMap.elemSize());
    /*cv::Mat xyzBuffMat = cv::Mat(xyzBuffer.size(), 1, CV_32FC3);
    for (int i = 0; i < xyzBuffer.size(); i++) {
        xyzBuffMat.at<float>(i, 0, 0) = xyzBuffer[i].x;
        xyzBuffMat.at<float>(i, 0, 1) = xyzBuffer[i].y;
        xyzBuffMat.at<float>(i, 0, 2) = xyzBuffer[i].z;
    }

    xyzMap = xyzBuffMat.reshape(3, 480);
    cvtColor(xyzMap, xyzMap, CV_RGB2BGR);
    */

    xyzMap = cv::Mat(xyzBuffer, true).reshape(3, 480);
    //cvtColor(xyzMap, xyzMap, CV_RGB2BGR);



    //testMat.convertTo(testMat, CV_32FC3);
    //xyzMap = testMat;
    //xyzMap = cv::Mat(xyzBuffer);
    //cout << "nrows"<<xyzMap.rows << endl;
    //cout << "ncols" << xyzMap.cols << endl;

    //imshow("xyz mona", xyzMap);
    //double min, max;
    //cv::minMaxLoc(xyzMap, &min, &max);
    //cout << "max min: "<<min<<" "<<max<<endl;
    cv::namedWindow("XYZ Image by OpenARK", CV_WINDOW_AUTOSIZE);
    cv::imshow("XYZ Image by OpenARK", Visualizer::visualizeXYZMap(xyzMap));
    //cv::waitKey();

    //cv::destroyWindow("Depth Image by OpenARK");


    //cout << "numrows: " << xyzMap.rows << endl;
    //cout << "numcols: " << xyzMap.cols << endl;
    //cout << "nrows: "<<xyzMap.rows<<endl;
    //cout << "ncols: "<<xyzMap.cols << endl;

    /*double min;
    double max;
    cv::minMaxIdx(xyzMap, &min, &max);
    cv::Mat adjMap;
    cv::convertScaleAbs(xyzMap, adjMap, 255 / max);
    cv::imshow("Out xyz", adjMap);*/
    //cv::imshow("xyz window", xyzMap);
    //cout << "xyzMap = " << endl << " " << xyzMap << endl << endl;
    /*
    double min;
    double max;
    cv::minMaxIdx(xyzMap, &min, &max);
    cv::Mat adjMap;
    // expand your range to 0..255. Similar to histEq();
    //xyzMap.convertTo(adjMap, CV_32FC3, 255 / (max - min), -min);
    xyzMap.convertTo(adjMap, CV_32FC3, 255 / (max - min), (-255 * min / (max - min)));

    cv::Mat falseColorsMap;
    applyColorMap(adjMap, falseColorsMap, cv::COLORMAP_AUTUMN);

    cv::imshow("Out", falseColorsMap); */

/*  dists = new float[3 * numPixels]; // Dists contains XYZ values. needs to be 3x the size of numPixels
    amps = new float[numPixels];
    //frame = cvCreateImage(cvSize(depth_width, depth_height), 8, 3); // Create the frame
    frame = cv::Mat(cv::Size(depth_width, depth_height), CV_8UC3);

    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)); */

    //cv::destroyWindow("Depth Image by OpenARK");


}




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