Commit 5d5f3c78 authored by Pierre Bürki's avatar Pierre Bürki
Browse files

Create cloud point manager

parent b942796a

src/PointCloudManager.cpp

0 → 100644
+90 −0
Original line number Diff line number Diff line 
#include "PointCloudManager.h"



void PointCloudManager::draw()

{

	//ofLog(OF_LOG_NOTICE) << "Drawing cloud point (" << pointCloud.getVertices().size() << ") points)";

	ofPushMatrix();

	ofNoFill();

	pointCloud.draw();

	auto vs = pointCloud.getVertices();

	if (vs.size() > 0) {

		auto v = vs[12000];

		ofLog(OF_LOG_NOTICE) << "Color at vertex 12000 (of " << vs.size() << "): \n"

			<< "R " << v.r << "\n"

			<< "G " << v.g << "\n"

			<< "B " << v.b;

	}

	ofPopMatrix();

}



void PointCloudManager::updateRGB(RGBFrame::Ptr data)

{

	rgbFrameSize.x = data->getCols();

	rgbFrameSize.y = data->getRows();



	const Color3* rgbData = data->getData();

	uint8_t* colorDataPtr = (uint8_t*)rgbData;

	ofPixels rgbPix;

	// Assuming the data comes from the live feed (hence BGR)

	rgbPix.setFromPixels(colorDataPtr, rgbFrameSize.x, rgbFrameSize.y, OF_PIXELS_BGR);

	rgbTex.loadData(rgbPix);



	createPointCloudIfNotExist(depthFrameSize);



	for (int y = 0; y < depthFrameSize.y; y += skip) {

		for (int x = 0; x < depthFrameSize.x; x += skip) {

			const int index = y * depthFrameSize.x + x;

			const int skippedIndex = (y / skip) * (depthFrameSize.x / skip) + (x / skip);



			pointCloud.setColor(skippedIndex, 10 * rgbPix.getColor(index));

		}

	}

}



void PointCloudManager::updateDepth(DepthFrame::Ptr data)

{

	depthFrameSize.x = data->getCols();

	depthFrameSize.y = data->getRows();



	const unsigned short* depthData = data->getData();

	ofShortPixels pix;

	pix.setFromPixels(depthData, depthFrameSize.x, depthFrameSize.y, OF_PIXELS_GRAY);

	depthTex.loadData(pix);



	createPointCloudIfNotExist(depthFrameSize);

	

	for (int y = 0; y < depthFrameSize.y; y += skip) {

		for (int x = 0; x < depthFrameSize.x; x += skip) {

			const int index = y * depthFrameSize.x + x;

			const int skippedIndex = (y / skip) * (depthFrameSize.x / skip) + (x / skip);



			const unsigned short d = depthData[index];

			Vector3 v = depthSensor->convertProjToRealCoords(x, y, depthData[index]);

			pointCloud.setVertex(skippedIndex, ofxnui::Tracker::fromVector3(v) * 0.001);

		}

	}

}



void PointCloudManager::createPointCloudIfNotExist(glm::vec2 dim)

{

	if (pointCloud.getVertices().size() == 0) {

		ofLog(OF_LOG_NOTICE) << "YOU ONLY SEE THIS ONCE";



		pointCloud.setMode(OF_PRIMITIVE_POINTS);

		// Allocate

		pointCloud.clear();

		int size = ceil(((float)dim.x / skip) * ((float)dim.y / skip));



		vector<glm::vec3> p;

		p.assign(size, glm::vec3(0, 0, 0));

		pointCloud.addVertices(p);



		vector<ofFloatColor> c;

		c.assign(size, ofFloatColor(0, 0, 0, 0.9));

		pointCloud.addColors(c);

	}

}



void PointCloudManager::setDepthSensor(DepthSensor::Ptr sensor) {

	depthSensor = sensor;

}

src/PointCloudManager.h

0 → 100644
+33 −0
Original line number Diff line number Diff line 
#pragma once



#include "ofMain.h"

#include <nuitrack/Nuitrack.h>

#include "ofxNuitrack.h"



using namespace tdv::nuitrack;



class PointCloudManager

{

private:

	DepthSensor::Ptr depthSensor;



	ofTexture rgbTex;

	glm::vec2 rgbFrameSize;



	ofTexture depthTex;

	glm::vec2 depthFrameSize;



	ofVboMesh pointCloud;



	const int skip = 2;



	void createPointCloudIfNotExist(glm::vec2 dim);



public:

	void draw();

	void updateRGB(RGBFrame::Ptr data);

	void updateDepth(DepthFrame::Ptr data);



	void setDepthSensor(DepthSensor::Ptr sensor);

};

the-hole.vcxproj

+2 −0
Original line number Diff line number Diff line
@@ -193,6 +193,7 @@ 
    <ClCompile Include="src\Linef.cpp" />

    <ClCompile Include="src\OrthoCamera.cpp" />

    <ClCompile Include="src\Planef.cpp" />

    <ClCompile Include="src\PointCloudManager.cpp" />

    <ClCompile Include="src\SkeletonFinder.cpp" />

    <ClCompile Include="src\TrackingClient.cpp" />

    <ClCompile Include="src\TrackingNetworkManager.cpp" />
@@ -260,6 +261,7 @@ 
    <ClInclude Include="src\Linef.h" />

    <ClInclude Include="src\OrthoCamera.h" />

    <ClInclude Include="src\Planef.h" />

    <ClInclude Include="src\PointCloudManager.h" />

    <ClInclude Include="src\SkeletonFinder.h" />

    <ClInclude Include="src\TrackingClient.h" />

    <ClInclude Include="src\TrackingNetworkManager.h" />

the-hole.vcxproj.filters

+6 −0
Original line number Diff line number Diff line
@@ -205,6 +205,9 @@ 
    <ClCompile Include="src\SkeletonFinder.cpp">

      <Filter>src</Filter>

    </ClCompile>

    <ClCompile Include="src\PointCloudManager.cpp">

      <Filter>src</Filter>

    </ClCompile>

  </ItemGroup>

  <ItemGroup>

    <Filter Include="src">
@@ -1811,6 +1814,9 @@ 
    <ClInclude Include="src\SkeletonFinder.h">

      <Filter>src</Filter>

    </ClInclude>

    <ClInclude Include="src\PointCloudManager.h">

      <Filter>src</Filter>

    </ClInclude>

  </ItemGroup>

  <ItemGroup>

    <ResourceCompile Include="icon.rc" />