prisonerjohn/main.cpp

## main.cpp
#include "ofMain.h"
#include "ofApp.h"

int main()
{
    ofGLFWWindowSettings settings;

    settings.setSize(1280, 720);
    settings.setPosition(ofVec2f(100, 100));
    settings.resizable = true;
    shared_ptr<ofAppBaseWindow> mainWindow = ofCreateWindow(settings);

    settings.setSize(PROJECTOR_RESOLUTION_X, PROJECTOR_RESOLUTION_Y);
    settings.setPosition(ofVec2f(ofGetScreenWidth(), 0));
    settings.resizable = false;
    settings.decorated = false;
    settings.shareContextWith = mainWindow;
    shared_ptr<ofAppBaseWindow> secondWindow = ofCreateWindow(settings);
    secondWindow->setVerticalSync(false);

    shared_ptr<ofApp> mainApp(new ofApp);
    ofAddListener(secondWindow->events().draw, mainApp.get(), &ofApp::drawProjector);

    ofRunApp(mainWindow, mainApp);
    ofRunMainLoop();
}

## ofApp-kinect.cpp
#include "ofApp.h"

void ofApp::setup()
{
    // Texture coordinates from RealSense are normalized (between 0-1).
    // This call normalizes all OF texture coordinates so that they match.
    ofDisableArbTex();

    appMode.set("App Mode", 0, 0, 2);

    chessboardX.set("Chessboard X", 0.5, 0.0, 1.0);
    chessboardY.set("Chessboard Y", 0.5, 0.0, 1.0);
    chessboardSize.set("Chessboard Size", 300, 20, 1280);

    addPairs.set("Add Pairs");
    addPairs.addListener(this, &ofApp::addPointPairs);
    calibrate.set("Calibrate");
    calibrate.addListener(this, &ofApp::calibrateSpaces);
    saveCalib.set("Save Calib");
    saveCalib.addListener(this, &ofApp::saveCalibration);
    loadCalib.set("Load Calib");
    loadCalib.addListener(this, &ofApp::loadCalibration);

    nearThreshold.set("Near Threshold", 0.01f, 0.0f, 0.1f);
    farThreshold.set("Far Threshold", 0.02f, 0.0f, 0.1f);
    minArea.set("Min Area", 0.01f, 0, 0.5f);
    maxArea.set("Max Area", 0.05f, 0, 0.5f);

    flipX.set("Flip X", false);
    flipY.set("Flip Y", false);

    fboProjection.allocate(PROJECTOR_RESOLUTION_X, PROJECTOR_RESOLUTION_Y, GL_RGBA);

    guiPanel.setup("RS Projection", "settings.json");
    guiPanel.add(appMode);
    guiPanel.add(chessboardX);
    guiPanel.add(chessboardY);
    guiPanel.add(chessboardSize);
    guiPanel.add(addPairs);
    guiPanel.add(calibrate);
    guiPanel.add(saveCalib);
    guiPanel.add(loadCalib);
    guiPanel.add(nearThreshold);
    guiPanel.add(farThreshold);
    guiPanel.add(minArea);
    guiPanel.add(maxArea);
    guiPanel.add(flipX);
    guiPanel.add(flipY);

    // Start the Kinect context.
    kinect.setRegistration(true);
    kinect.init();
    kinect.open();

    deviceWidth = kinect.getWidth();
    deviceHeight = kinect.getHeight();
    colorImg.allocate(deviceWidth, deviceHeight, OF_IMAGE_COLOR);
}

//--------------------------------------------------------------
void ofApp::update()
{
    kinect.update();

    if (kinect.isFrameNew())
    {
        colorImg.setFromPixels(kinect.getPixels());
        if (appMode == 0) // Searching.
        {
            renderChessboard();

            colorMat = ofxCv::toCv(colorImg.getPixels());
            cv::Size patternSize = cv::Size(CHESSBOARD_COLS - 1, CHESSBOARD_ROWS - 1);
            int chessFlags = cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK;
            bool foundChessboard = cv::findChessboardCorners(colorMat, patternSize, cvPoints, chessFlags);
            if (foundChessboard)
            {
                cv::Mat grayMat;
                cv::cvtColor(colorMat, grayMat, CV_RGB2GRAY);
                cv::cornerSubPix(grayMat, cvPoints, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
                cv::drawChessboardCorners(colorMat, patternSize, cv::Mat(cvPoints), foundChessboard);
                colorImg.update();
            }
        }
        else if (appMode == 1) // Testing.
        {
            // Map points in both world space and projector space and draw them.
            // If they are calibrated correctly they should be drawn on top of one another.
            glm::vec2 clampedTestPoint = glm::vec2(
                ofClamp(testPoint.x, 0, deviceWidth - 1),
                ofClamp(testPoint.y, 0, deviceHeight - 1));
            glm::vec3 worldPoint = kinect.getWorldCoordinateAt(clampedTestPoint.x, clampedTestPoint.y);
            glm::vec2 projectedPoint = kpToolkit.getProjectedPoint(worldPoint);

            renderTestPoint(projectedPoint);
        }
        else // Rendering.
        {
            // Threshold the depth.
            ofFloatPixels rawDepthPix = kinect.getRawDepthPixels();
            ofFloatPixels thresholdNear, thresholdFar, thresholdResult;
            ofxCv::threshold(rawDepthPix, thresholdNear, nearThreshold);
            ofxCv::threshold(rawDepthPix, thresholdFar, farThreshold, true);
            ofxCv::bitwise_and(thresholdNear, thresholdFar, thresholdResult);
            thresholdImg.setFromPixels(thresholdResult);

            // Find contours.
            contourFinder.setMinAreaNorm(minArea);
            contourFinder.setMaxAreaNorm(maxArea);
            contourFinder.findContours(thresholdImg);

            renderContours();
        }
    }
}

void ofApp::draw()
{
    ofSetColor(255);
    colorImg.draw(0, 0);

    kinect.getDepthTexture().draw(colorImg.getWidth(), 0);

    if (thresholdImg.isAllocated())
    {
        ofPushMatrix();
        ofTranslate(colorImg.getWidth(), kinect.getDepthTexture().getHeight());
        {
            thresholdImg.draw(0, 0);
            contourFinder.draw();
        }
        ofPopMatrix();
    }

    if (appMode == 0) // Searching.
    {
        // Use a string stream to print a multi-line message.
        std::ostringstream oss;
        oss << "SEARCHING MODE" << std::endl
            << "Position the chessboard by dragging the mouse over the RGB image." << std::endl
            << "Adjust the size of the chessboard using LEFT / RIGHT or the GUI sliders." << std::endl
            << "When the pattern is recognized, hit SPACE to save a set of point-pairs." << std::endl
            << "Once a few point-pairs have been detected, calibrate using the GUI button." << std::endl
            << std::endl
            << ofToString(pairsWorld.size()) << " point-pairs collected.";

        ofSetColor(255);
        ofDrawBitmapStringHighlight(oss.str(), 10, 380);
    }
    else if (appMode == 1) // Testing.
    {
        // Use a string stream to print a multi-line message.
        std::ostringstream oss;
        oss << "TESTING MODE" << std::endl
            << "Click on the RGB image to test a calibrated point." << std::endl
            << "The world point will be drawn in RED, the projected point will be drawn in GREEN." << std::endl
            << "If the calibration is successful, both points will be drawn on top of each other." << std::endl
            << "Save the calibration using the GUI button.";

        ofSetColor(255);
        ofDrawBitmapStringHighlight(oss.str(), 10, 380);

        // Draw the test point on screen.
        ofSetColor(255, 0, 0);
        float pointSize = ofMap(cos(ofGetFrameNum() * 0.1), -1, 1, 3, 40);
        ofDrawCircle(testPoint.x, testPoint.y, pointSize);
    }
    else // Rendering.
    {
        // Use a string stream to print a multi-line message.
        std::ostringstream oss;
        oss << "RENDERING MODE" << std::endl
            << "Adjust the depth threshold using the GUI sliders." << std::endl
            << "The thresholded silhouette will mask the rest of the projected image.";
    }

    guiPanel.draw();
}

void ofApp::drawProjector(ofEventArgs& args)
{
    ofBackground(255);
    ofSetColor(255);
    fboProjection.draw(0, 0);
}

void ofApp::keyPressed(int key)
{
    if (key == ' ')
    {
        addPointPairs();
    }
    else if (key == 'c')
    {
        calibrateSpaces();
    }
    else if (key == 's')
    {
        saveCalibration();
    }
    else if (key == 'l')
    {
        loadCalibration();
    }
}

void ofApp::mousePressed(int x, int y, int button)
{
    if (appMode == 0) // Searching
    {
        if (ofGetMousePressed() &&
            ofInRange(x, 0, deviceWidth) &&
            ofInRange(y, 0, deviceHeight))
        {
            // Save the normalized point position.
            chessboardX = ofMap(x, 0, deviceWidth, 0, 1);
            chessboardY = ofMap(y, 0, deviceHeight, 0, 1);
        }
    }
    else if (appMode == 1) // Testing
    {
        testPoint = glm::vec2(x, y);
    }
    else
    {

    }
}

void ofApp::mouseDragged(int x, int y, int button)
{
    if (appMode == 0) // Searching
    {
        if (ofGetMousePressed() &&
            ofInRange(x, 0, deviceWidth) &&
            ofInRange(y, 0, deviceHeight))
        {
            // Save the normalized point position.
            chessboardX = ofMap(x, 0, deviceWidth, 0, 1);
            chessboardY = ofMap(y, 0, deviceHeight, 0, 1);
        }
    }
    else if (appMode == 1) // Testing
    {
        testPoint = glm::vec2(x, y);
    }
}

void ofApp::renderChessboard()
{
    float cellSize = chessboardSize / CHESSBOARD_COLS;

    currProjectorPoints.clear();

    fboProjection.begin();
    {
        // Remap top-left to projection space.
        int boardX = ofMap(chessboardX, 0, 1, 0, fboProjection.getWidth());
        int boardY = ofMap(chessboardY, 0, 1, 0, fboProjection.getHeight());

        // Clear white and draw black cells.
        ofClear(255, 0);
        ofSetColor(0);

        for (int j = 0; j < CHESSBOARD_ROWS; j++)
        {
            for (int i = 0; i < CHESSBOARD_COLS; i++)
            {
                int cellX = boardX + i * cellSize;
                int cellY = boardY + j * cellSize;

                if ((i + j) % 2 == 0)
                {
                    // Only draw black cells.
                    ofDrawRectangle(cellX, cellY, cellSize, cellSize);
                }

                if (i > 0 && j > 0)
                {
                    // Add normalized intersection points to the list.
                    float normX = ofMap(cellX, 0, fboProjection.getWidth(), 0, 1);
                    float normY = ofMap(cellY, 0, fboProjection.getHeight(), 0, 1);
                    currProjectorPoints.push_back(glm::vec2(normX, normY));
                }
            }
        }
    }
    fboProjection.end();
}

void ofApp::renderTestPoint(glm::vec2 projectedPoint)
{
    float pointSize = ofMap(sin(ofGetFrameNum() * 0.1), -1, 1, 3, 40);

    fboProjection.begin();
    {
        ofBackground(255);

        // Point is normalized, so it needs to be mapped to the projector size.
        float projX = ofMap(projectedPoint.x, 0, 1, 0, fboProjection.getWidth());
        float projY = ofMap(projectedPoint.y, 0, 1, 0, fboProjection.getHeight());

        ofSetColor(0, 255, 0);
        ofDrawCircle(projX, projY, pointSize);
    }
    fboProjection.end();
}

void ofApp::renderContours()
{
    fboProjection.begin();
    {
        // Clear white and draw black contours.
        ofClear(255, 0);
        ofSetColor(0);

        for (int i = 0; i < contourFinder.size(); i++)
        {
            // Map contour using calibration and draw to main window
            ofBeginShape();
            std::vector<cv::Point> points = contourFinder.getContour(i);
            for (int j = 0; j < points.size(); j++)
            {
                glm::vec3 worldPoint = kinect.getWorldCoordinateAt(points[j].x, points[j].y);
                if (worldPoint.z > 0)
                {
                    glm::vec2 projectedPoint = kpToolkit.getProjectedPoint(worldPoint);
                    if (ofInRange(projectedPoint.x, 0, 1) && ofInRange(projectedPoint.y, 0, 1))
                    {
                        if (flipX)
                        {
                            projectedPoint.x = 1.0 - projectedPoint.x;
                        }
                        if (flipY)
                        {
                            projectedPoint.y = 1.0 - projectedPoint.y;
                        }
                        ofVertex(projectedPoint.x * fboProjection.getWidth(), projectedPoint.y * fboProjection.getHeight());
                        //ofLog() << "Adding world " << worldPoint << " // point " << projectedPoint << " // proj " << (projectedPoint.x * fboProjection.getWidth()) << ", " << (projectedPoint.y * fboProjection.getHeight());
                    }
                }
            }
            ofEndShape();
        }
    }
    fboProjection.end();
}

void ofApp::addPointPairs()
{
    // Count the number of world points.
    int numDepthPoints = 0;
    for (int i = 0; i < cvPoints.size(); i++)
    {
        glm::vec3 worldPoint = kinect.getWorldCoordinateAt(cvPoints[i].x, cvPoints[i].y);
        if (worldPoint.z > 0)
        {
            numDepthPoints++;
        }
    }

    int chessboardTotal = (CHESSBOARD_COLS - 1) * (CHESSBOARD_ROWS - 1);
    if (numDepthPoints != chessboardTotal)
    {
        ofLogError(__FUNCTION__) << "Only found " << numDepthPoints << " / " << chessboardTotal << " points!";
        return;
    }

    // Found all chessboard points in the world, add both sets to the lists we will use for calibration.
    for (int i = 0; i < cvPoints.size(); i++)
    {
        glm::vec3 worldPoint = kinect.getWorldCoordinateAt(cvPoints[i].x, cvPoints[i].y);
        pairsWorld.push_back(worldPoint);
        pairsProjector.push_back(currProjectorPoints[i]);
        ofLogNotice(__FUNCTION__) << "Adding pair i: " << worldPoint << " => " << currProjectorPoints[i];
    }

    ofLogNotice(__FUNCTION__) << "Added " << chessboardTotal << " point-pairs.";
}

void ofApp::calibrateSpaces()
{
    kpToolkit.calibrate(pairsWorld, pairsProjector);
    appMode = 1;

    pairsWorld.clear();
    pairsProjector.clear();
}

void ofApp::saveCalibration()
{
    if (kpToolkit.saveCalibration("calibration.json"))
    {
        ofLogNotice(__FUNCTION__) << "Calibration saved!";
    }
}

void ofApp::loadCalibration()
{
    if (kpToolkit.loadCalibration("calibration.json"))
    {
        ofLogNotice(__FUNCTION__) << "Calibration loaded!";
        appMode = 2;
    }
}

## ofApp-kinect.h
#pragma once

#include "ofMain.h"

#include "ofxCv.h"
#include "ofxGui.h"
#include "ofxKinect.h"
#include "ofxKinectProjectorToolkit.h"

#define CHESSBOARD_COLS 5
#define CHESSBOARD_ROWS 4

// This must match the display resolution of your projector
#define PROJECTOR_RESOLUTION_X 1920
#define PROJECTOR_RESOLUTION_Y 1080

class ofApp : public ofBaseApp
{
public:
    void setup();
    void update();
    void draw();

    void keyPressed(int key);
    //void keyReleased(int key);
    //void mouseMoved(int x, int y);
    void mouseDragged(int x, int y, int button);
    void mousePressed(int x, int y, int button);
    //void mouseReleased(int x, int y, int button);
    //void mouseEntered(int x, int y);
    //void mouseExited(int x, int y);
    //void windowResized(int w, int h);
    //void dragEvent(ofDragInfo dragInfo);
    //void gotMessage(ofMessage msg);

    void drawProjector(ofEventArgs& args);

    void renderChessboard();
    void renderTestPoint(glm::vec2 projectedPoint);
    void renderContours();

    void addPointPairs();

    void calibrateSpaces();
    void saveCalibration();
    void loadCalibration();

    ofxKinect kinect;
    ofxKinectProjectorToolkit kpToolkit;

    int deviceWidth;
    int deviceHeight;

    ofFbo fboProjection;

    ofImage colorImg;
    cv::Mat colorMat;

    vector<glm::vec2> currProjectorPoints;
    vector<cv::Point2f> cvPoints;
    vector<glm::vec3> pairsWorld;
    vector<glm::vec2> pairsProjector;

    glm::vec2 testPoint;

    ofImage thresholdImg;
    ofxCv::ContourFinder contourFinder;

    ofParameter<int> appMode;

    ofParameter<float> chessboardX;
    ofParameter<float> chessboardY;
    ofParameter<int> chessboardSize;
    ofParameter<void> addPairs;
    ofParameter<void> calibrate;
    ofParameter<void> saveCalib;
    ofParameter<void> loadCalib;

    ofParameter<float> nearThreshold;
    ofParameter<float> farThreshold;
    ofParameter<float> minArea;
    ofParameter<float> maxArea;
    ofParameter<bool> flipX;
    ofParameter<bool> flipY;

    ofxPanel guiPanel;
};

## ofApp-realsense.cpp
#include "ofApp.h"

void ofApp::setup()
{
    // Texture coordinates from RealSense are normalized (between 0-1).
    // This call normalizes all OF texture coordinates so that they match.
    ofDisableArbTex();

    appMode.set("App Mode", 0, 0, 2);

    chessboardX.set("Chessboard X", 0.5, 0.0, 1.0);
    chessboardY.set("Chessboard Y", 0.5, 0.0, 1.0);
    chessboardSize.set("Chessboard Size", 300, 20, 1280);

    addPairs.set("Add Pairs");
    addPairs.addListener(this, &ofApp::addPointPairs);
    calibrate.set("Calibrate");
    calibrate.addListener(this, &ofApp::calibrateSpaces);
    saveCalib.set("Save Calib");
    saveCalib.addListener(this, &ofApp::saveCalibration);
    loadCalib.set("Load Calib");
    loadCalib.addListener(this, &ofApp::loadCalibration);

    nearThreshold.set("Near Threshold", 0.01f, 0.0f, 0.1f);
    farThreshold.set("Far Threshold", 0.02f, 0.0f, 0.1f);
    minArea.set("Min Area", 0.01f, 0, 0.5f);
    maxArea.set("Max Area", 0.05f, 0, 0.5f);

    flipX.set("Flip X", false);
    flipY.set("Flip Y", false);

    fboProjection.allocate(PROJECTOR_RESOLUTION_X, PROJECTOR_RESOLUTION_Y, GL_RGBA);

    guiPanel.setup("RS Projection", "settings.json");
    guiPanel.add(appMode);
    guiPanel.add(chessboardX);
    guiPanel.add(chessboardY);
    guiPanel.add(chessboardSize);
    guiPanel.add(addPairs);
    guiPanel.add(calibrate);
    guiPanel.add(saveCalib);
    guiPanel.add(loadCalib);
    guiPanel.add(nearThreshold);
    guiPanel.add(farThreshold);
    guiPanel.add(minArea);
    guiPanel.add(maxArea);
    guiPanel.add(flipX);
    guiPanel.add(flipY);

    // Start the RealSense context.
    // Devices are added in the deviceAdded() callback function.
    ofAddListener(rsContext.deviceAddedEvent, this, &ofApp::deviceAdded);
    rsContext.setup(false);
}

void ofApp::deviceAdded(std::string& serialNumber)
{
    ofLogNotice(__FUNCTION__) << "Starting device " << serialNumber;
    auto device = rsContext.getDevice(serialNumber);
    device->enableInfrared();
    device->enableDepth();
    device->enableColor();
    device->startPipeline();

    // Work in device depth space (should be 640x360).
    device->alignMode = ofxRealSense2::Device::Align::Color;
    deviceWidth = device->getColorPix().getWidth();
    deviceHeight = device->getColorPix().getHeight();
    colorImg.allocate(deviceWidth, deviceHeight, OF_IMAGE_COLOR);

    // Uncomment this to add the device specific settings to the GUI.
    //guiPanel.add(device->params);
}

//--------------------------------------------------------------
void ofApp::update()
{
    rsContext.update();

    std::shared_ptr<ofxRealSense2::Device> rsDevice = rsContext.getDevice(0);
    if (rsDevice)
    {
        colorImg.setFromPixels(rsDevice->getColorPix());
        if (appMode == 0) // Searching.
        {
            renderChessboard();

            colorMat = ofxCv::toCv(colorImg.getPixels());
            cv::Size patternSize = cv::Size(CHESSBOARD_COLS - 1, CHESSBOARD_ROWS - 1);
            int chessFlags = cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK;
            bool foundChessboard = cv::findChessboardCorners(colorMat, patternSize, cvPoints, chessFlags);
            if (foundChessboard)
            {
                cv::Mat grayMat;
                cv::cvtColor(colorMat, grayMat, CV_RGB2GRAY);
                cv::cornerSubPix(grayMat, cvPoints, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
                cv::drawChessboardCorners(colorMat, patternSize, cv::Mat(cvPoints), foundChessboard);
                colorImg.update();
            }
        }
        else if (appMode == 1) // Testing.
        {
            // Map points in both world space and projector space and draw them.
            // If they are calibrated correctly they should be drawn on top of one another.
            glm::vec2 clampedTestPoint = glm::vec2(
                ofClamp(testPoint.x, 0, deviceWidth - 1),
                ofClamp(testPoint.y, 0, deviceHeight - 1));
            glm::vec3 worldPoint = rsDevice->getWorldPosition(clampedTestPoint.x, clampedTestPoint.y);
            glm::vec2 projectedPoint = kpToolkit.getProjectedPoint(worldPoint);

            renderTestPoint(projectedPoint);
        }
        else // Rendering.
        {
            // Threshold the depth.
            ofFloatPixels rawDepthPix = rsDevice->getRawDepthPix();
            ofFloatPixels thresholdNear, thresholdFar, thresholdResult;
            ofxCv::threshold(rawDepthPix, thresholdNear, nearThreshold);
            ofxCv::threshold(rawDepthPix, thresholdFar, farThreshold, true);
            ofxCv::bitwise_and(thresholdNear, thresholdFar, thresholdResult);
            thresholdImg.setFromPixels(thresholdResult);

            // Find contours.
            contourFinder.setMinAreaNorm(minArea);
            contourFinder.setMaxAreaNorm(maxArea);
            contourFinder.findContours(thresholdImg);

            renderContours();
        }
    }
}

void ofApp::draw()
{
    std::shared_ptr<ofxRealSense2::Device> rsDevice = rsContext.getDevice(0);
    if (rsDevice)
    {
        ofSetColor(255);
        colorImg.draw(0, 0);

        rsDevice->getDepthTex().draw(colorImg.getWidth(), 0);

        if (thresholdImg.isAllocated())
        {
            ofPushMatrix();
            ofTranslate(colorImg.getWidth(), rsDevice->getDepthTex().getHeight());
            {
                thresholdImg.draw(0, 0);
                contourFinder.draw();
            }
            ofPopMatrix();
        }

        if (appMode == 0) // Searching.
        {
            // Use a string stream to print a multi-line message.
            std::ostringstream oss;
            oss << "SEARCHING MODE" << std::endl
                << "Position the chessboard by dragging the mouse over the RGB image." << std::endl
                << "Adjust the size of the chessboard using LEFT / RIGHT or the GUI sliders." << std::endl
                << "When the pattern is recognized, hit SPACE to save a set of point-pairs." << std::endl
                << "Once a few point-pairs have been detected, calibrate using the GUI button." << std::endl
                << std::endl
                << ofToString(pairsWorld.size()) << " point-pairs collected.";

            ofSetColor(255);
            ofDrawBitmapStringHighlight(oss.str(), 10, 380);
        }
        else if (appMode == 1) // Testing.
        {
            // Use a string stream to print a multi-line message.
            std::ostringstream oss;
            oss << "TESTING MODE" << std::endl
                << "Click on the RGB image to test a calibrated point." << std::endl
                << "The world point will be drawn in RED, the projected point will be drawn in GREEN." << std::endl
                << "If the calibration is successful, both points will be drawn on top of each other." << std::endl
                << "Save the calibration using the GUI button.";

            ofSetColor(255);
            ofDrawBitmapStringHighlight(oss.str(), 10, 380);

            // Draw the test point on screen.
            ofSetColor(255, 0, 0);
            float pointSize = ofMap(cos(ofGetFrameNum() * 0.1), -1, 1, 3, 40);
            ofDrawCircle(testPoint.x, testPoint.y, pointSize);
        }
        else // Rendering.
        {
            // Use a string stream to print a multi-line message.
            std::ostringstream oss;
            oss << "RENDERING MODE" << std::endl
                << "Adjust the depth threshold using the GUI sliders." << std::endl
                << "The thresholded silhouette will mask the rest of the projected image.";
        }
    }

    guiPanel.draw();
}

void ofApp::drawProjector(ofEventArgs& args)
{
    ofBackground(255);
    ofSetColor(255);
    fboProjection.draw(0, 0);
}

void ofApp::keyPressed(int key)
{
    if (key == ' ')
    {
        addPointPairs();
    }
    else if (key == 'c')
    {
        calibrateSpaces();
    }
    else if (key == 's')
    {
        saveCalibration();
    }
    else if (key == 'l')
    {
        loadCalibration();
    }
}

void ofApp::mousePressed(int x, int y, int button)
{
    if (appMode == 0) // Searching
    {
        if (ofGetMousePressed() &&
            ofInRange(x, 0, deviceWidth) &&
            ofInRange(y, 0, deviceHeight))
        {
            // Save the normalized point position.
            chessboardX = ofMap(x, 0, deviceWidth, 0, 1);
            chessboardY = ofMap(y, 0, deviceHeight, 0, 1);
        }
    }
    else if (appMode == 1) // Testing
    {
        testPoint = glm::vec2(x, y);
    }
    else
    {

    }
}

void ofApp::mouseDragged(int x, int y, int button)
{
    if (appMode == 0) // Searching
    {
        if (ofGetMousePressed() &&
            ofInRange(x, 0, deviceWidth) &&
            ofInRange(y, 0, deviceHeight))
        {
            // Save the normalized point position.
            chessboardX = ofMap(x, 0, deviceWidth, 0, 1);
            chessboardY = ofMap(y, 0, deviceHeight, 0, 1);
        }
    }
    else if (appMode == 1) // Testing
    {
        testPoint = glm::vec2(x, y);
    }
}

void ofApp::renderChessboard()
{
    float cellSize = chessboardSize / CHESSBOARD_COLS;

    currProjectorPoints.clear();

    fboProjection.begin();
    {
        // Remap top-left to projection space.
        int boardX = ofMap(chessboardX, 0, 1, 0, fboProjection.getWidth());
        int boardY = ofMap(chessboardY, 0, 1, 0, fboProjection.getHeight());

        // Clear white and draw black cells.
        ofClear(255, 0);
        ofSetColor(0);

        for (int j = 0; j < CHESSBOARD_ROWS; j++)
        {
            for (int i = 0; i < CHESSBOARD_COLS; i++)
            {
                int cellX = boardX + i * cellSize;
                int cellY = boardY + j * cellSize;

                if ((i + j) % 2 == 0)
                {
                    // Only draw black cells.
                    ofDrawRectangle(cellX, cellY, cellSize, cellSize);
                }

                if (i > 0 && j > 0)
                {
                    // Add normalized intersection points to the list.
                    float normX = ofMap(cellX, 0, fboProjection.getWidth(), 0, 1);
                    float normY = ofMap(cellY, 0, fboProjection.getHeight(), 0, 1);
                    currProjectorPoints.push_back(glm::vec2(normX, normY));
                }
            }
        }
    }
    fboProjection.end();
}

void ofApp::renderTestPoint(glm::vec2 projectedPoint)
{
    float pointSize = ofMap(sin(ofGetFrameNum() * 0.1), -1, 1, 3, 40);

    fboProjection.begin();
    {
        ofBackground(255);

        // Point is normalized, so it needs to be mapped to the projector size.
        float projX = ofMap(projectedPoint.x, 0, 1, 0, fboProjection.getWidth());
        float projY = ofMap(projectedPoint.y, 0, 1, 0, fboProjection.getHeight());

        ofSetColor(0, 255, 0);
        ofDrawCircle(projX, projY, pointSize);
    }
    fboProjection.end();
}

void ofApp::renderContours()
{
    fboProjection.begin();
    {
        // Clear white and draw black contours.
        ofClear(255, 0);
        ofSetColor(0);

        std::shared_ptr<ofxRealSense2::Device> rsDevice = rsContext.getDevice(0);
        if (rsDevice)
        {
            for (int i = 0; i < contourFinder.size(); i++)
            {
                // Map contour using calibration and draw to main window
                ofBeginShape();
                std::vector<cv::Point> points = contourFinder.getContour(i);
                for (int j = 0; j < points.size(); j++)
                {
                    glm::vec3 worldPoint = rsDevice->getWorldPosition(points[j].x, points[j].y);
                    if (worldPoint.z > 0)
                    {
                        glm::vec2 projectedPoint = kpToolkit.getProjectedPoint(worldPoint);
                        if (ofInRange(projectedPoint.x, 0, 1) && ofInRange(projectedPoint.y, 0, 1))
                        {
                            if (flipX)
                            {
                                projectedPoint.x = 1.0 - projectedPoint.x;
                            }
                            if (flipY)
                            {
                                projectedPoint.y = 1.0 - projectedPoint.y;
                            }
                            ofVertex(projectedPoint.x * fboProjection.getWidth(), projectedPoint.y * fboProjection.getHeight());
                            ofLog() << "Adding world " << worldPoint << " // point " << projectedPoint << " // proj " << (projectedPoint.x * fboProjection.getWidth()) << ", " << (projectedPoint.y * fboProjection.getHeight());
                        }
                    }
                }
                ofEndShape();
            }
        }
    }
    fboProjection.end();
}

void ofApp::addPointPairs()
{
    std::shared_ptr<ofxRealSense2::Device> rsDevice = rsContext.getDevice(0);
    if (!rsDevice)
    {
        ofLogError(__FUNCTION__) << "No RealSense detected!";
        return;
    }

    // Count the number of world points.
    int numDepthPoints = 0;
    for (int i = 0; i < cvPoints.size(); i++)
    {
        glm::vec3 worldPoint = rsDevice->getWorldPosition(cvPoints[i].x, cvPoints[i].y);
        if (worldPoint.z > 0)
        {
            numDepthPoints++;
        }
    }

    int chessboardTotal = (CHESSBOARD_COLS - 1) * (CHESSBOARD_ROWS - 1);
    if (numDepthPoints != chessboardTotal)
    {
        ofLogError(__FUNCTION__) << "Only found " << numDepthPoints << " / " << chessboardTotal << " points!";
        return;
    }

    // Found all chessboard points in the world, add both sets to the lists we will use for calibration.
    for (int i = 0; i < cvPoints.size(); i++)
    {
        glm::vec3 worldPoint = rsDevice->getWorldPosition(cvPoints[i].x, cvPoints[i].y);
        pairsWorld.push_back(worldPoint);
        pairsProjector.push_back(currProjectorPoints[i]);
        ofLogNotice(__FUNCTION__) << "Adding pair i: " << worldPoint << " => " << currProjectorPoints[i];
    }

    ofLogNotice(__FUNCTION__) << "Added " << chessboardTotal << " point-pairs.";
}

void ofApp::calibrateSpaces()
{
    kpToolkit.calibrate(pairsWorld, pairsProjector);
    appMode = 1;

    pairsWorld.clear();
    pairsProjector.clear();
}

void ofApp::saveCalibration()
{
    if (kpToolkit.saveCalibration("calibration.json"))
    {
        ofLogNotice(__FUNCTION__) << "Calibration saved!";
    }
}

void ofApp::loadCalibration()
{
    if (kpToolkit.loadCalibration("calibration.json"))
    {
        ofLogNotice(__FUNCTION__) << "Calibration loaded!";
        appMode = 2;
    }
}

## ofApp-realsense.h
#pragma once

#include "ofMain.h"

#include "ofxCv.h"
#include "ofxGui.h"
#include "ofxKinectProjectorToolkit.h"
#include "ofxRealSense2.h"

#define CHESSBOARD_COLS 5
#define CHESSBOARD_ROWS 4

// This must match the display resolution of your projector
#define PROJECTOR_RESOLUTION_X 1920
#define PROJECTOR_RESOLUTION_Y 1080

class ofApp : public ofBaseApp
{
public:
    void setup();
    void update();
    void draw();

    void keyPressed(int key);
    //void keyReleased(int key);
    //void mouseMoved(int x, int y);
    void mouseDragged(int x, int y, int button);
    void mousePressed(int x, int y, int button);
    //void mouseReleased(int x, int y, int button);
    //void mouseEntered(int x, int y);
    //void mouseExited(int x, int y);
    //void windowResized(int w, int h);
    //void dragEvent(ofDragInfo dragInfo);
    //void gotMessage(ofMessage msg);

    void deviceAdded(std::string& serialNumber);

    void drawProjector(ofEventArgs& args);

    void renderChessboard();
    void renderTestPoint(glm::vec2 projectedPoint);
    void renderContours();

    void addPointPairs();

    void calibrateSpaces();
    void saveCalibration();
    void loadCalibration();

    ofxRealSense2::Context rsContext;
    ofxKinectProjectorToolkit kpToolkit;

    int deviceWidth;
    int deviceHeight;

    ofFbo fboProjection;

    ofImage colorImg;
    cv::Mat colorMat;

    vector<glm::vec2> currProjectorPoints;
    vector<cv::Point2f> cvPoints;
    vector<glm::vec3> pairsWorld;
    vector<glm::vec2> pairsProjector;

    glm::vec2 testPoint;

    ofImage thresholdImg;
    ofxCv::ContourFinder contourFinder;

    ofParameter<int> appMode;

    ofParameter<float> chessboardX;
    ofParameter<float> chessboardY;
    ofParameter<int> chessboardSize;
    ofParameter<void> addPairs;
    ofParameter<void> calibrate;
    ofParameter<void> saveCalib;
    ofParameter<void> loadCalib;

    ofParameter<float> nearThreshold;
    ofParameter<float> farThreshold;
    ofParameter<float> minArea;
    ofParameter<float> maxArea;
    ofParameter<bool> flipX;
    ofParameter<bool> flipY;

    ofxPanel guiPanel;
};
	#include "ofMain.h"
	#include "ofApp.h"

	int main()
	{
	ofGLFWWindowSettings settings;

	settings.setSize(1280, 720);
	settings.setPosition(ofVec2f(100, 100));
	settings.resizable = true;
	shared_ptr<ofAppBaseWindow> mainWindow = ofCreateWindow(settings);

	settings.setSize(PROJECTOR_RESOLUTION_X, PROJECTOR_RESOLUTION_Y);
	settings.setPosition(ofVec2f(ofGetScreenWidth(), 0));
	settings.resizable = false;
	settings.decorated = false;
	settings.shareContextWith = mainWindow;
	shared_ptr<ofAppBaseWindow> secondWindow = ofCreateWindow(settings);
	secondWindow->setVerticalSync(false);

	shared_ptr<ofApp> mainApp(new ofApp);
	ofAddListener(secondWindow->events().draw, mainApp.get(), &ofApp::drawProjector);

	ofRunApp(mainWindow, mainApp);
	ofRunMainLoop();
	}
	#include "ofApp.h"

	void ofApp::setup()
	{
	// Texture coordinates from RealSense are normalized (between 0-1).
	// This call normalizes all OF texture coordinates so that they match.
	ofDisableArbTex();

	appMode.set("App Mode", 0, 0, 2);

	chessboardX.set("Chessboard X", 0.5, 0.0, 1.0);
	chessboardY.set("Chessboard Y", 0.5, 0.0, 1.0);
	chessboardSize.set("Chessboard Size", 300, 20, 1280);

	addPairs.set("Add Pairs");
	addPairs.addListener(this, &ofApp::addPointPairs);
	calibrate.set("Calibrate");
	calibrate.addListener(this, &ofApp::calibrateSpaces);
	saveCalib.set("Save Calib");
	saveCalib.addListener(this, &ofApp::saveCalibration);
	loadCalib.set("Load Calib");
	loadCalib.addListener(this, &ofApp::loadCalibration);

	nearThreshold.set("Near Threshold", 0.01f, 0.0f, 0.1f);
	farThreshold.set("Far Threshold", 0.02f, 0.0f, 0.1f);
	minArea.set("Min Area", 0.01f, 0, 0.5f);
	maxArea.set("Max Area", 0.05f, 0, 0.5f);

	flipX.set("Flip X", false);
	flipY.set("Flip Y", false);

	fboProjection.allocate(PROJECTOR_RESOLUTION_X, PROJECTOR_RESOLUTION_Y, GL_RGBA);

	guiPanel.setup("RS Projection", "settings.json");
	guiPanel.add(appMode);
	guiPanel.add(chessboardX);
	guiPanel.add(chessboardY);
	guiPanel.add(chessboardSize);
	guiPanel.add(addPairs);
	guiPanel.add(calibrate);
	guiPanel.add(saveCalib);
	guiPanel.add(loadCalib);
	guiPanel.add(nearThreshold);
	guiPanel.add(farThreshold);
	guiPanel.add(minArea);
	guiPanel.add(maxArea);
	guiPanel.add(flipX);
	guiPanel.add(flipY);

	// Start the Kinect context.
	kinect.setRegistration(true);
	kinect.init();
	kinect.open();

	deviceWidth = kinect.getWidth();
	deviceHeight = kinect.getHeight();
	colorImg.allocate(deviceWidth, deviceHeight, OF_IMAGE_COLOR);
	}

	//--------------------------------------------------------------
	void ofApp::update()
	{
	kinect.update();

	if (kinect.isFrameNew())
	{
	colorImg.setFromPixels(kinect.getPixels());
	if (appMode == 0) // Searching.
	{
	renderChessboard();

	colorMat = ofxCv::toCv(colorImg.getPixels());
	cv::Size patternSize = cv::Size(CHESSBOARD_COLS - 1, CHESSBOARD_ROWS - 1);
	int chessFlags = cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK;
	bool foundChessboard = cv::findChessboardCorners(colorMat, patternSize, cvPoints, chessFlags);
	if (foundChessboard)
	{
	cv::Mat grayMat;
	cv::cvtColor(colorMat, grayMat, CV_RGB2GRAY);
	cv::cornerSubPix(grayMat, cvPoints, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
	cv::drawChessboardCorners(colorMat, patternSize, cv::Mat(cvPoints), foundChessboard);
	colorImg.update();
	}
	}
	else if (appMode == 1) // Testing.
	{
	// Map points in both world space and projector space and draw them.
	// If they are calibrated correctly they should be drawn on top of one another.
	glm::vec2 clampedTestPoint = glm::vec2(
	ofClamp(testPoint.x, 0, deviceWidth - 1),
	ofClamp(testPoint.y, 0, deviceHeight - 1));
	glm::vec3 worldPoint = kinect.getWorldCoordinateAt(clampedTestPoint.x, clampedTestPoint.y);
	glm::vec2 projectedPoint = kpToolkit.getProjectedPoint(worldPoint);

	renderTestPoint(projectedPoint);
	}
	else // Rendering.
	{
	// Threshold the depth.
	ofFloatPixels rawDepthPix = kinect.getRawDepthPixels();
	ofFloatPixels thresholdNear, thresholdFar, thresholdResult;
	ofxCv::threshold(rawDepthPix, thresholdNear, nearThreshold);
	ofxCv::threshold(rawDepthPix, thresholdFar, farThreshold, true);
	ofxCv::bitwise_and(thresholdNear, thresholdFar, thresholdResult);
	thresholdImg.setFromPixels(thresholdResult);

	// Find contours.
	contourFinder.setMinAreaNorm(minArea);
	contourFinder.setMaxAreaNorm(maxArea);
	contourFinder.findContours(thresholdImg);

	renderContours();
	}
	}
	}

	void ofApp::draw()
	{
	ofSetColor(255);
	colorImg.draw(0, 0);

	kinect.getDepthTexture().draw(colorImg.getWidth(), 0);

	if (thresholdImg.isAllocated())
	{
	ofPushMatrix();
	ofTranslate(colorImg.getWidth(), kinect.getDepthTexture().getHeight());
	{
	thresholdImg.draw(0, 0);
	contourFinder.draw();
	}
	ofPopMatrix();
	}

	if (appMode == 0) // Searching.
	{
	// Use a string stream to print a multi-line message.
	std::ostringstream oss;
	oss << "SEARCHING MODE" << std::endl
	<< "Position the chessboard by dragging the mouse over the RGB image." << std::endl
	<< "Adjust the size of the chessboard using LEFT / RIGHT or the GUI sliders." << std::endl
	<< "When the pattern is recognized, hit SPACE to save a set of point-pairs." << std::endl
	<< "Once a few point-pairs have been detected, calibrate using the GUI button." << std::endl
	<< std::endl
	<< ofToString(pairsWorld.size()) << " point-pairs collected.";

	ofSetColor(255);
	ofDrawBitmapStringHighlight(oss.str(), 10, 380);
	}
	else if (appMode == 1) // Testing.
	{
	// Use a string stream to print a multi-line message.
	std::ostringstream oss;
	oss << "TESTING MODE" << std::endl
	<< "Click on the RGB image to test a calibrated point." << std::endl
	<< "The world point will be drawn in RED, the projected point will be drawn in GREEN." << std::endl
	<< "If the calibration is successful, both points will be drawn on top of each other." << std::endl
	<< "Save the calibration using the GUI button.";

	ofSetColor(255);
	ofDrawBitmapStringHighlight(oss.str(), 10, 380);

	// Draw the test point on screen.
	ofSetColor(255, 0, 0);
	float pointSize = ofMap(cos(ofGetFrameNum() * 0.1), -1, 1, 3, 40);
	ofDrawCircle(testPoint.x, testPoint.y, pointSize);
	}
	else // Rendering.
	{
	// Use a string stream to print a multi-line message.
	std::ostringstream oss;
	oss << "RENDERING MODE" << std::endl
	<< "Adjust the depth threshold using the GUI sliders." << std::endl
	<< "The thresholded silhouette will mask the rest of the projected image.";
	}

	guiPanel.draw();
	}

	void ofApp::drawProjector(ofEventArgs& args)
	{
	ofBackground(255);
	ofSetColor(255);
	fboProjection.draw(0, 0);
	}

	void ofApp::keyPressed(int key)
	{
	if (key == ' ')
	{
	addPointPairs();
	}
	else if (key == 'c')
	{
	calibrateSpaces();
	}
	else if (key == 's')
	{
	saveCalibration();
	}
	else if (key == 'l')
	{
	loadCalibration();
	}
	}

	void ofApp::mousePressed(int x, int y, int button)
	{
	if (appMode == 0) // Searching
	{
	if (ofGetMousePressed() &&
	ofInRange(x, 0, deviceWidth) &&
	ofInRange(y, 0, deviceHeight))
	{
	// Save the normalized point position.
	chessboardX = ofMap(x, 0, deviceWidth, 0, 1);
	chessboardY = ofMap(y, 0, deviceHeight, 0, 1);
	}
	}
	else if (appMode == 1) // Testing
	{
	testPoint = glm::vec2(x, y);
	}
	else
	{

	}
	}

	void ofApp::mouseDragged(int x, int y, int button)
	{
	if (appMode == 0) // Searching
	{
	if (ofGetMousePressed() &&
	ofInRange(x, 0, deviceWidth) &&
	ofInRange(y, 0, deviceHeight))
	{
	// Save the normalized point position.
	chessboardX = ofMap(x, 0, deviceWidth, 0, 1);
	chessboardY = ofMap(y, 0, deviceHeight, 0, 1);
	}
	}
	else if (appMode == 1) // Testing
	{
	testPoint = glm::vec2(x, y);
	}
	}

	void ofApp::renderChessboard()
	{
	float cellSize = chessboardSize / CHESSBOARD_COLS;

	currProjectorPoints.clear();

	fboProjection.begin();
	{
	// Remap top-left to projection space.
	int boardX = ofMap(chessboardX, 0, 1, 0, fboProjection.getWidth());
	int boardY = ofMap(chessboardY, 0, 1, 0, fboProjection.getHeight());

	// Clear white and draw black cells.
	ofClear(255, 0);
	ofSetColor(0);

	for (int j = 0; j < CHESSBOARD_ROWS; j++)
	{
	for (int i = 0; i < CHESSBOARD_COLS; i++)
	{
	int cellX = boardX + i * cellSize;
	int cellY = boardY + j * cellSize;

	if ((i + j) % 2 == 0)
	{
	// Only draw black cells.
	ofDrawRectangle(cellX, cellY, cellSize, cellSize);
	}

	if (i > 0 && j > 0)
	{
	// Add normalized intersection points to the list.
	float normX = ofMap(cellX, 0, fboProjection.getWidth(), 0, 1);
	float normY = ofMap(cellY, 0, fboProjection.getHeight(), 0, 1);
	currProjectorPoints.push_back(glm::vec2(normX, normY));
	}
	}
	}
	}
	fboProjection.end();
	}

	void ofApp::renderTestPoint(glm::vec2 projectedPoint)
	{
	float pointSize = ofMap(sin(ofGetFrameNum() * 0.1), -1, 1, 3, 40);

	fboProjection.begin();
	{
	ofBackground(255);

	// Point is normalized, so it needs to be mapped to the projector size.
	float projX = ofMap(projectedPoint.x, 0, 1, 0, fboProjection.getWidth());
	float projY = ofMap(projectedPoint.y, 0, 1, 0, fboProjection.getHeight());

	ofSetColor(0, 255, 0);
	ofDrawCircle(projX, projY, pointSize);
	}
	fboProjection.end();
	}

	void ofApp::renderContours()
	{
	fboProjection.begin();
	{
	// Clear white and draw black contours.
	ofClear(255, 0);
	ofSetColor(0);

	for (int i = 0; i < contourFinder.size(); i++)
	{
	// Map contour using calibration and draw to main window
	ofBeginShape();
	std::vector<cv::Point> points = contourFinder.getContour(i);
	for (int j = 0; j < points.size(); j++)
	{
	glm::vec3 worldPoint = kinect.getWorldCoordinateAt(points[j].x, points[j].y);
	if (worldPoint.z > 0)
	{
	glm::vec2 projectedPoint = kpToolkit.getProjectedPoint(worldPoint);
	if (ofInRange(projectedPoint.x, 0, 1) && ofInRange(projectedPoint.y, 0, 1))
	{
	if (flipX)
	{
	projectedPoint.x = 1.0 - projectedPoint.x;
	}
	if (flipY)
	{
	projectedPoint.y = 1.0 - projectedPoint.y;
	}
	ofVertex(projectedPoint.x * fboProjection.getWidth(), projectedPoint.y * fboProjection.getHeight());
	//ofLog() << "Adding world " << worldPoint << " // point " << projectedPoint << " // proj " << (projectedPoint.x * fboProjection.getWidth()) << ", " << (projectedPoint.y * fboProjection.getHeight());
	}
	}
	}
	ofEndShape();
	}
	}
	fboProjection.end();
	}

	void ofApp::addPointPairs()
	{
	// Count the number of world points.
	int numDepthPoints = 0;
	for (int i = 0; i < cvPoints.size(); i++)
	{
	glm::vec3 worldPoint = kinect.getWorldCoordinateAt(cvPoints[i].x, cvPoints[i].y);
	if (worldPoint.z > 0)
	{
	numDepthPoints++;
	}
	}

	int chessboardTotal = (CHESSBOARD_COLS - 1) * (CHESSBOARD_ROWS - 1);
	if (numDepthPoints != chessboardTotal)
	{
	ofLogError(__FUNCTION__) << "Only found " << numDepthPoints << " / " << chessboardTotal << " points!";
	return;
	}

	// Found all chessboard points in the world, add both sets to the lists we will use for calibration.
	for (int i = 0; i < cvPoints.size(); i++)
	{
	glm::vec3 worldPoint = kinect.getWorldCoordinateAt(cvPoints[i].x, cvPoints[i].y);
	pairsWorld.push_back(worldPoint);
	pairsProjector.push_back(currProjectorPoints[i]);
	ofLogNotice(__FUNCTION__) << "Adding pair i: " << worldPoint << " => " << currProjectorPoints[i];
	}

	ofLogNotice(__FUNCTION__) << "Added " << chessboardTotal << " point-pairs.";
	}

	void ofApp::calibrateSpaces()
	{
	kpToolkit.calibrate(pairsWorld, pairsProjector);
	appMode = 1;

	pairsWorld.clear();
	pairsProjector.clear();
	}

	void ofApp::saveCalibration()
	{
	if (kpToolkit.saveCalibration("calibration.json"))
	{
	ofLogNotice(__FUNCTION__) << "Calibration saved!";
	}
	}

	void ofApp::loadCalibration()
	{
	if (kpToolkit.loadCalibration("calibration.json"))
	{
	ofLogNotice(__FUNCTION__) << "Calibration loaded!";
	appMode = 2;
	}
	}
	#pragma once

	#include "ofMain.h"

	#include "ofxCv.h"
	#include "ofxGui.h"
	#include "ofxKinect.h"
	#include "ofxKinectProjectorToolkit.h"

	#define CHESSBOARD_COLS 5
	#define CHESSBOARD_ROWS 4

	// This must match the display resolution of your projector
	#define PROJECTOR_RESOLUTION_X 1920
	#define PROJECTOR_RESOLUTION_Y 1080

	class ofApp : public ofBaseApp
	{
	public:
	void setup();
	void update();
	void draw();

	void keyPressed(int key);
	//void keyReleased(int key);
	//void mouseMoved(int x, int y);
	void mouseDragged(int x, int y, int button);
	void mousePressed(int x, int y, int button);
	//void mouseReleased(int x, int y, int button);
	//void mouseEntered(int x, int y);
	//void mouseExited(int x, int y);
	//void windowResized(int w, int h);
	//void dragEvent(ofDragInfo dragInfo);
	//void gotMessage(ofMessage msg);

	void drawProjector(ofEventArgs& args);

	void renderChessboard();
	void renderTestPoint(glm::vec2 projectedPoint);
	void renderContours();

	void addPointPairs();

	void calibrateSpaces();
	void saveCalibration();
	void loadCalibration();

	ofxKinect kinect;
	ofxKinectProjectorToolkit kpToolkit;

	int deviceWidth;
	int deviceHeight;

	ofFbo fboProjection;

	ofImage colorImg;
	cv::Mat colorMat;

	vector<glm::vec2> currProjectorPoints;
	vector<cv::Point2f> cvPoints;
	vector<glm::vec3> pairsWorld;
	vector<glm::vec2> pairsProjector;

	glm::vec2 testPoint;

	ofImage thresholdImg;
	ofxCv::ContourFinder contourFinder;

	ofParameter<int> appMode;

	ofParameter<float> chessboardX;
	ofParameter<float> chessboardY;
	ofParameter<int> chessboardSize;
	ofParameter<void> addPairs;
	ofParameter<void> calibrate;
	ofParameter<void> saveCalib;
	ofParameter<void> loadCalib;

	ofParameter<float> nearThreshold;
	ofParameter<float> farThreshold;
	ofParameter<float> minArea;
	ofParameter<float> maxArea;
	ofParameter<bool> flipX;
	ofParameter<bool> flipY;

	ofxPanel guiPanel;
	};