sr300camera.cpp test

#include "SR300Camera.h"
#include "RealSense/SenseManager.h"
#include "RealSense/SampleReader.h"
#include "util_render.h"

/***
Private constructor for the PMD depth sensor
***/

using namespace Intel::RealSense;

SenseManager *pp = SenseManager::CreateInstance();
int depth_width;
int depth_height;


SR300Camera::SR300Camera(bool use_live_sensor)
{

    CONFIDENCE_THRESHHOLD = (60.0f / 255.0f*500.0f);
    //INVALID_FLAG_VALUE = PMD_FLAG_INVALID;
    X_DIMENSION = 176;
    Y_DIMENSION = 120;

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

    /* Sets file recording or playback */
    CaptureManager *cm = pp->QueryCaptureManager();

    // Create stream renders
    //UtilRender renderc(L"Color"), renderd(L"Depth"), renderi(L"IR"), renderr(L"Right"), renderl(L"Left");
    Status sts;
    UtilRender renderc(L"Color"), renderd(L"Depth"), renderi(L"IR"), renderr(L"Right"), renderl(L"Left");


    do {
        bool revert = false;

        DataDesc desc = {};
        if (cm->QueryCapture()) {
            cm->QueryCapture()->QueryDeviceInfo(0, &desc.deviceInfo);
        }
        else {
            desc.deviceInfo.streams = Capture::STREAM_TYPE_COLOR | Capture::STREAM_TYPE_DEPTH;
            revert = true;
        }
        pp->EnableStreams(&desc);

        /* Initializes the pipeline */

        sts = pp->Init();
        if (sts < Status::STATUS_NO_ERROR) {
            if (revert) {
                /* Enable a single stream */
                pp->Close();
                pp->EnableStream(Capture::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();
            }
        }

        /* Reset all properties */
        Capture::Device *device = pp->QueryCaptureManager()->QueryDevice();
        device->ResetProperties(Capture::STREAM_TYPE_ANY);

        for (int nframes = 0; ; nframes++) {
            /* Waits until new frame is available and locks it for application processing */
            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;
            }

            /* Render streams, unless -noRender is selected */
            const PXCCapture::Sample *sample = pp->QuerySample();
            //const Capture::Sample *sample = pp->QuerySample();
            if (sample) {
                cv::Mat img;
                //Copy all the vertices from Point3DF32 to the mat image
                //QueryVertices(Image *depth, Point3DF32 *vertices)
                //sample->color->QueryInfo()
                depth_width = sample->depth->QueryInfo().width;
                depth_height = sample->depth->QueryInfo().height;


                //cv::imshow("depth from OpenARK", Visualizer::visualizeDepthMap(img))
                if (sample->depth && !renderd.RenderFrame(sample->depth)) break;
                if (sample->color && !renderc.RenderFrame(sample->color)) break;
                if (sample->ir && !renderi.RenderFrame(sample->ir))    break;

            }


            printf("opened sensor\n");

            // Updating the sensor is necessary before any data can be retrieved
            //res = pmdUpdate(hnd);
            /* Releases lock so pipeline can process next frame */
            pp->ReleaseFrame();
            printf("acquired image\n");
        }

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




    /*res = pmdOpen(&hnd, SOURCE_PLUGIN, SOURCE_PARAM, PROC_PLUGIN, PROC_PARAM); //Open the PMD sensor
    if (res != PMD_OK)
    {
        pmdGetLastError(0, err, 128);
        fprintf(stderr, "Could not connect: %s\n", err);
        return;
    }

    */


    // res: Structure which contains various meta-information about the data delivered by your Nano.
    // It is advisabe to always use the data delivered by this struct (for example the width and height of the imager
    // and the image format). Please refer to the PMSDSK documentation for more information

    /*
    res = pmdGetSourceDataDescription(hnd, &dd);
    if (sts < STATUS_NO_ERROR) {
        pp->Release();
        return;
    }
    */

    /*
    if (res != PMD_OK)
    {
        pmdGetLastError(hnd, err, 128);
        fprintf(stderr, "Couldn't get data description: %s\n", err);
        pp->Release();
        //pmdClose(hnd);
        return;
    }

    printf("retrieved source data description\n");
    if (dd.subHeaderType != PMD_IMAGE_DATA)
    {
        fprintf(stderr, "Source data is not an image!\n");
        pmdClose(hnd);
        return;
    }
    */
    //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));

    /* mona will test this
    setIdentity(KF.measurementMatrix);
    setIdentity(KF.processNoiseCov, Scalar::all(smoothness));
    setIdentity(KF.measurementNoiseCov, Scalar::all(rapidness));
    setIdentity(KF.errorCovPost, Scalar::all(.1));
    */
}

/***
Public deconstructor for he PMD 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();
    fillInZCoords();
    // Flags. Helps with denoising.
    /*
    unsigned *flags = new unsigned[ampMap.cols*ampMap.rows];
    int res = pmdGetFlags(hnd, flags, numPixels * sizeof(unsigned));
    flagMap.data = (uchar *)flags;

    pmdUpdate(hnd);
    delete(flags);
    */
    pp->ReleaseFrame(); //correct?
    pxcStatus sts = pp->AcquireFrame(true);
    if (sts < PXC_STATUS_NO_ERROR)
        return;



}


/***
Reads the depth data from the sensor and fills in the matrix
***/
void SR300Camera::fillInZCoords()
{
    int res = pmdGet3DCoordinates(hnd, dists, 3 * numPixels * sizeof(float)); //store x,y,z coordinates dists (type: float*)
    //float * zCoords = new float[1]; //store z-Coordinates of dists in zCoords
    xyzMap = cv::Mat(xyzMap.size(), xyzMap.type(), dists);
}

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