Rotate point cloud into proper world space

gistfile1.txt

// 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));
    }
}