Created
November 18, 2019 05:48
-
-
Save prisonerjohn/d45211364dc45048d4e2d9fdf5ea4876 to your computer and use it in GitHub Desktop.
Sensing Machines Blob Clipping
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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(); | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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; | |
} | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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; | |
}; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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; | |
} | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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; | |
}; |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment