Created
December 7, 2017 13:36
-
-
Save mattfelsen/8484e1d8ec26ef5443616dd80b10813e to your computer and use it in GitHub Desktop.
Rotate point cloud into proper world space
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
// Transforms to move point cloud into correct world space | |
folder->addSlider("Camera tilt", -75, 75, 0)->bind(cameraTilt); | |
folder->addSlider("Camera roll", -30, 30, 0)->bind(cameraRoll); | |
folder->addSlider("Camera height", -60, 60, 0)->bind(cameraHeight); | |
// Shift around the rotated point cloud so its center sits on the origin | |
folder->addSlider("Shift X", -10, 10, 0)->bind(sceneShiftX); | |
folder->addSlider("Shift Y", -10, 10, 0)->bind(sceneShiftY); | |
folder->addSlider("Shift Z", -10, 10, 0)->bind(sceneShiftZ); | |
folder->addSlider("Floor plane", -100, 100, 0)->bind(floorPlane); | |
// Create matrix for transforming point cloud | |
matrix.makeIdentityMatrix(); | |
matrix.rotate(180, 0, 1, 0); | |
matrix.rotate(-cameraTilt, 1, 0, 0); | |
matrix.rotate(cameraRoll, 0, 0, 1); | |
matrix.translate(0, feetToCentimeters(cameraHeight), 0); | |
// Shift it around once rotated | |
matrix.translate(feetToCentimeters(sceneShiftX), 0, 0); | |
matrix.translate(0, feetToCentimeters(sceneShiftY), 0); | |
matrix.translate(0, 0, feetToCentimeters(sceneShiftZ)); | |
// Build the point cloud and apply the matrix to each vertex | |
auto& depthPix = camera.getDepthSource()->getPixels(); | |
if (!depthPix.isAllocated()) return; | |
mesh.clear(); | |
int step = 1; | |
for (int y = 0; y < depthHeight; y += step) { | |
for (int x = 0; x < depthWidth; x += step) { | |
float dist = depthPix.getColor(x, y).r; | |
ofVec3f point = worldCoords[y * depthWidth + x]; | |
point *= 100; // convert m -> cm | |
point = point * matrix; | |
// skip points that are below the floor plane | |
if (point.y < floorPlane) continue; | |
// Add vertex and give it a brightness value based on | |
// its distance from the floor | |
mesh.addVertex(point); | |
mesh.addColor(ofColor(point.y)); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment