main.cpp

//NOMINMAX would help to avoid the following error
//1>C:\Program Files\PCL 1.8.0\include\pcl-1.8\pcl/io/pcd_io.h(281):
//error C2589: '(': illegal token on right side of '::'
#define NOMINMAX

#define _WINSOCKAPI_
#include <limits>
// C++ Libraries
#include <stdio.h>
#include <iostream>
#include <string>
#include <time.h>

// OpenCV Libraries
#include <opencv/cxcore.h>
#include "opencv2/highgui/highgui.hpp"

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

// OpenARK Libraries
#include "Webcam.h"
#include "Visualizer.h"
#include "Hand.h"
#include "Plane.h"
#include "Calibration.h"
#include "Util.h"
#include "UDPSender.h"
#include "Object3D.h"
#include "StreamingAverager.h"

int main() {
    clock_t starttime = clock();
    DepthCamera* sr300 = new SR300Camera();
    //RGBCamera *cam = new Webcam(1);
    int frame = 0;
    //Calibration::XYZToUnity(*pmd, 4, 4, 3);
    cv::FileStorage fs;
    fs.open("RT_Transform.txt", cv::FileStorage::READ);

    cv::Mat r, t;
    fs["R"] >> r;
    fs["T"] >> t;

    fs.release();

    UDPSender u = UDPSender();
    cv::namedWindow("Results", CV_WINDOW_NORMAL);

    StreamingAverager handAverager = StreamingAverager(4, 0.1);
    StreamingAverager paleeteAverager = StreamingAverager(6, 0.05);

    while (true)
    {
        sr300->update();

        /**
        std::string filename = "..//OpenARK_Datasets//TwoHandDataSet1//img" + std::to_string(frame) + ".yml";
        if (!pmd->readImage(filename))
        break;
        **/

        // Loading image from sensor

        sr300->removeNoise();
        cv::imshow("XYZ Map", Visualizer::visualizeXYZMap(sr300->getXYZMap()));
        if (sr300->badInput) {
            continue;
        }

        // Classifying objects in the scene

        sr300->computeClusters(0.02, 500);
        std::vector<cv::Mat> clusters = sr300->getClusters();
        std::vector<Object3D> objects;

        int handObjectIndex = -1, planeObjectIndex = -1;
        for (int i = 0; i < clusters.size(); i++) {
            Object3D obj = Object3D(clusters[i].clone());
            if (obj.hasHand) {
                handObjectIndex = i;
            }

            if (obj.hasPlane) {
                planeObjectIndex = i;
            }
            objects.push_back(obj);

        }

        // Interprate the relationship between the objects

        bool clicked = false, paletteFound = false;
        Object3D handObject, planeObject;
        cv::Point paletteCenter(-1. - 1);
        cv::Mat mask = cv::Mat::zeros(sr300->getXYZMap().rows, sr300->getXYZMap().cols, CV_8UC1);
        if (planeObjectIndex != -1 && handObjectIndex != -1) {
            planeObject = objects[planeObjectIndex];
            handObject = objects[handObjectIndex];

            clicked = handObject.getHand().touchObject(planeObject.getPlane().getPlaneEquation(), planeObject.getPlane().R_SQUARED_DISTANCE_THRESHOLD * 5);
            cv::Mat scene = Visualizer::visualizePlaneRegression(sr300->getXYZMap(), planeObject.getPlane().getPlaneEquation(), planeObject.getPlane().R_SQUARED_DISTANCE_THRESHOLD, clicked);
            //scene = Visualizer::visualizeHand(scene, handObject.getHand().pointer_finger_ij, handObject.getHand().shape_centroid_ij);
            if (planeObject.leftEdgeConnected) {
                Visualizer::visualizePlanePoints(mask, planeObject.getPlane().getPlaneIndicies());
                cv::Moments m = cv::moments(mask, false);
                paletteCenter = cv::Point(m.m10 / m.m00, m.m01 / m.m00);
                cv::circle(scene, paletteCenter, 2, cv::Scalar(0, 0, 255), 2);
                paletteFound = true;
            }
            cv::imshow("Results", scene);
        }
        else if (handObjectIndex != -1) {
            handObject = objects[handObjectIndex];
            //cv::imshow("Results", Visualizer::visualizeHand(pmd->getXYZMap(), handObject.getHand().pointer_finger_ij, handObject.getHand().shape_centroid_ij));
        }
        else if (planeObjectIndex != -1) {
            planeObject = objects[planeObjectIndex];
            cv::Mat scene = Visualizer::visualizePlaneRegression(sr300->getXYZMap(), planeObject.getPlane().getPlaneEquation(), planeObject.getPlane().R_SQUARED_DISTANCE_THRESHOLD, clicked);
            if (planeObject.leftEdgeConnected) {
                Visualizer::visualizePlanePoints(mask, planeObject.getPlane().getPlaneIndicies());
                cv::Moments m = cv::moments(mask, false);
                paletteCenter = cv::Point(m.m10 / m.m00, m.m01 / m.m00);
                cv::circle(scene, paletteCenter, 2, cv::Scalar(0, 0, 255), 2);
                paletteFound = true;
            }
            cv::imshow("Results", scene);
        }

        // Organize the data and send to game engine

        std::string handX = "-", handY = "-", handZ = "-";
        std::string paletteX = "-", paletteY = "-", paletteZ = "-";
        std::string clickStatus = "2";
        std::string num_fingers = "0";
        if (handObjectIndex != -1) {
            cv::Vec3f handPos = handAverager.addDataPoint(objects[handObjectIndex].getHand().fingers_xyz[0]);
            //float hand_pt[3] = { objects[handObjectIndex].getHand().pointer_finger_xyz[0], objects[handObjectIndex].getHand().pointer_finger_xyz[1], objects[handObjectIndex].getHand().pointer_finger_xyz[2]};
            float hand_pt[3] = { handPos[0], handPos[1], handPos[2] };
            cv::Mat hand_mat = cv::Mat(3, 1, CV_32FC1, &hand_pt);
            hand_mat = r*hand_mat + t;
            handX = std::to_string(hand_mat.at<float>(0, 0));
            handY = std::to_string(hand_mat.at<float>(1, 0));
            handZ = std::to_string(hand_mat.at<float>(2, 0));
            num_fingers = std::to_string(objects[handObjectIndex].getHand().fingers_xyz.size());
        }
        else {
            handAverager.addEmptyPoint();
        }
        if (paletteFound) {
            cv::Vec3f pt = paleeteAverager.addDataPoint(sr300->getXYZMap().at<cv::Vec3f>(paletteCenter.y, paletteCenter.x));
            float palette_pt[3] = { pt[0], pt[1], pt[2] };
            cv::Mat palette_mat = cv::Mat(3, 1, CV_32FC1, &palette_pt);
            palette_mat = r*palette_mat + t;
            paletteX = std::to_string(palette_mat.at<float>(0, 0));
            paletteY = std::to_string(palette_mat.at<float>(1, 0));
            paletteZ = std::to_string(palette_mat.at<float>(2, 0));
        }
        else {
            paleeteAverager.addEmptyPoint();
        }
        if (clicked) {
            clickStatus = "1";
        }

        std::string tempS = "";
        tempS = handX + "%" + handY + "%" + handZ + "%" + paletteX + "%" + paletteY + "%" + paletteZ + "%" + clickStatus + "%" + num_fingers;
        u.send(tempS);



        /**** Start: Write Frames to File ****/
        //std::string filename = "img" + std::to_string(frame) + ".yml";
        //pmd->writeImage(filename);
        //std::cout << filename << std::endl;
        /**** End: Write Frames to File ****/



        /**** Start: Loop Break Condition ****/
        int c = cvWaitKey(1);
        if (c == 'q' || c == 'Q' || c == 27) {
            break;
        }
        /**** End: Loop Break Condition ****/
        frame++;
    }

    sr300->destroyInstance();
    return 0;
}