app.cpp

#include "sandbox.h"
#include <numeric>
#include <fstream>
#include <string>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

#define ESCAPE_CHAR 27

Sandbox client;

Mat coloredFrame(Mat frameDepth)
{
    Mat depthFrameColored(frameDepth.size(), CV_8U);
    int width = frameDepth.cols, height = frameDepth.rows;
    static uint32_t histogram[0x10000];
    memset(histogram, 0, sizeof(histogram));

    for (int i = 0; i < height; ++i)
    {
        for (int j = 0; j < width; ++j)
        {
            ++histogram[frameDepth.at<ushort>(i, j)];
        }
    }

    for (int i = 2; i < 0x10000; ++i)
        histogram[i] += histogram[i - 1]; // Build a cumulative histogram for the indices in [1,0xFFFF]

    for (int i = 0; i < height; ++i)
    {
        for (int j = 0; j < width; ++j)
        {
            if (uint16_t d = frameDepth.at<ushort>(i, j))
            {
                int f = histogram[d] * 255 / histogram[0xFFFF]; // 0-255 based on histogram location
                depthFrameColored.at<uchar>(i, j) = static_cast<uchar>(f);
            }
            else
            {
                depthFrameColored.at<uchar>(i, j) = 0;
            }
        }
    }
    bitwise_not(depthFrameColored, depthFrameColored); //reverse colormap
    applyColorMap(depthFrameColored, depthFrameColored, cv::COLORMAP_JET);
    depthFrameColored.setTo(cv::Scalar(0, 0, 0), (frameDepth == 0));
    return depthFrameColored;
}

void showLevel()
{
    char windowName[] = "Sandbox";
    Mat frameData;
    client.getDepthFrame().copyTo(frameData);
    Mat colored;
    do
    {
        client.getDepthFrame().copyTo(frameData);
        colored = coloredFrame(frameData);
        client.showImage(&colored);
    } while (waitKey(10) != ESCAPE_CHAR);
    destroyAllWindows();
}
/*
void showDiff(Mat frameBase)
{
    Mat frameData;
    client.getDepthFrame(&frameData);
    resize(frameBase, frameBase, frameData.size()); //to match with camera frame
    Mat diff(frameData.size(), CV_16S);
    Mat frameColor(frameData.size(), CV_8UC3, Scalar(0, 0, 0));
    int toBlue[] = {0, 2};
    int toRed[] = {0, 0};
    int keyCode;
    do
    {
        client.getDepthFrame(&frameData);
        subtract(frameBase, frameData, diff, noArray(), CV_16S);
        Mat isNeg = diff < -5;
        Mat isPos = diff > 5;
        //colorize red & blue
        mixChannels(&isNeg, 1, &frameColor, 1, toBlue, 1);
        mixChannels(&isPos, 1, &frameColor, 1, toRed, 1);
        client.showImage(frameColor);
        keyCode = waitKey(10);
    } while (keyCode!= ESCAPE_CHAR);
    destroyAllWindows();
}

vector<int> findCercleZ(Mat &rgb)
{
    Mat src_gray;
    cvtColor(rgb, src_gray, CV_BGR2GRAY);
    /// Reduce the noise so we avoid false circle detection
    GaussianBlur(src_gray, src_gray, Size(9, 9), 2, 2);
    vector<Vec3f> circles;
    circles.clear();
    /// Apply the Hough Transform to find the circles
    //source, output, method, inverse ratio of resolution, Minimum distance between detected centers, threeshold canny, threeshold center, min radius, max radius
    HoughCircles(src_gray, circles, CV_HOUGH_GRADIENT, 1, src_gray.rows / 4, 75, 50, 0, 0);
    //doit tester si le cercle est bon (rayon);

    vector<int> result;
    if (!circles.empty())
    {
        for (int i = 0; i < 3; i++)
        {
            result.push_back(round(circles[0][i]));
        }
    }
    return result;
}

void trackCircle()
{
    Mat frameRGB;
    client.getRGBFrame(&frameRGB);
    Mat frameColor(frameRGB.size(), CV_8UC3, Scalar(0, 0, 0));

    do
    {
        client.getRGBFrame(&frameRGB);
        vector<int> cercle = findCercleZ(frameRGB);
        if (!cercle.empty())
        {
            Point circlePosition(cercle[0], cercle[1]);
            cv::circle(frameColor, circlePosition, 3, Scalar(0, 255, 0), -1, 8, 0); // point without transformation
            cout << circlePosition.x << "x" << circlePosition.y << endl;
        }
        client.showImage(frameColor);
        frameColor.setTo(Scalar(0,0,0));

    } while (waitKey(100) != ESCAPE_CHAR);
    destroyAllWindows();
}*/

int main(int argc, char *argv[])
{

    if (argc != 3)
    {
        printf("Usage : %s <addr_ip> <port>\n", argv[0]);
        exit(1);
    }
    //client = Sandbox(argv[1], atoi(argv[2]));
    client = Sandbox();
    cout << "Press: \n 0: Show difference \n 1: Show level \n";
    int f = 0;
    cin >> f;
    if (f == 0)
    {
        //Mat frameData;
        //client.getDepthFrame(&frameData);
        //showDiff(frameData);
    }
    else if (f == 1)
        showLevel();
}