a simple motion tracking app for kinect and Cinder

kinectrecorder.cpp

#include "cinder/app/App.h"
#include "cinder/app/RendererGl.h"
#include "cinder/gl/gl.h"

#include "cinder/Rand.h"
#include <fstream>
#include "Kinect2.h"
#include <iostream>
#include "cinder/Log.h"

#include "cinder/Utilities.h"
#include "cinder/CameraUi.h"
#include <sstream>
#include <boost/algorithm/string.hpp>
#include <boost/tokenizer.hpp>
#include <sstream>

#include "CinderImGui.h"

#define SCALE_FACTOR 20

using namespace ci;
using namespace ci::app;
using namespace std;

enum Mode { IDLE, RECORDING, PLAYBACK }; //enum to keep track whether we're in playback mode, recording mode, or idle
enum Menu { MAIN, RECORD, PLAY};

typedef std::vector<vec3>	skeletonInstance; //A CONTAINER THAT HOLDS VEC3s OF EACH JOINT FOR A SINGLE ANIMATION FRAME
/*
TO DO:
- add countdown for recording
- merge playback app
*/

class KinectCapGuiApp : public App {
  public:

	KinectCapGuiApp();

	void setup() override;
	void keyDown( KeyEvent event ) override;
	void update() override;
	void draw() override;

	void writeCSV();
	std::vector<std::vector<vec3>> mJoints;
	void setupFile();
	void displayGui();
	void recordData();
	void setupGlsl();

	gl::TextureRef		mRecordingTex, mPlayBackTex;

	Mode	mMode;
	Menu	mMenu;

	//SETUP GLOBAL UI VARIABLES
	ImGui::Options opts;

	ImGuiStyle style;

	//VARIABLE FOR PLAYBACK
	std::vector<vec3> parseJointString(std::string jointString);
	void loadCSV();
	void printData();

	std::vector<std::string>		mFrameStrings;
	std::vector<std::vector<vec3>>	mSkeletonContainer;

	gl::GlslProgRef			mGlsl;
	gl::BatchRef			mBatch, mPlane;

	CameraPersp				mCam;
	CameraUi				mCamUi;

	bool					run, animate, fileLoaded, isPaused, mFullScreen;

	int count = 0; //variable to keep track of the frameNumber
	int totalFrames;

private:
	Kinect2::BodyFrame			mBodyFrame;
	ci::Channel8uRef			mChannelBodyIndex;
	ci::Channel16uRef			mChannelDepth;
	Kinect2::DeviceRef			mDevice;

	ofstream myfile;
	boolean record;

	int countdownTimer; 

	std::string fileName;
};

KinectCapGuiApp::KinectCapGuiApp()
{
	mDevice = Kinect2::Device::create();
	mDevice->start();
	mDevice->connectBodyEventHandler([&](const Kinect2::BodyFrame frame) {
		mBodyFrame = frame;
	});
	mDevice->connectBodyIndexEventHandler([&](const Kinect2::BodyIndexFrame frame)
	{
		mChannelBodyIndex = frame.getChannel();
	});
	mDevice->connectDepthEventHandler([&](const Kinect2::DepthFrame frame)
	{
		mChannelDepth = frame.getChannel();
	});

	auto img = loadImage(loadAsset("rec.png"));
	mRecordingTex = gl::Texture::create(img);
	mRecordingTex->bind(0);

	auto img1 = loadImage(loadAsset("play.png"));
	mPlayBackTex = gl::Texture::create(img1);
	mPlayBackTex->bind(1);
	countdownTimer = 0;
	totalFrames = 0;

	//initialize the UI
	ui::initialize();
	fileName = "Enter a filename";
}

void KinectCapGuiApp::setup()
{
	mCam.lookAt(vec3(0, 5, 20), vec3(0));
	mCam.setPerspective(60.f, getWindowAspectRatio(), 1.f, 1000.f);

	setFrameRate(60);
	record = false;
	animate = false;
	fileLoaded = false;
	isPaused = false;
	mMode = IDLE;
	mMenu = MAIN;
	mFullScreen = isFullScreen();
	setupGlsl();
}

void KinectCapGuiApp::keyDown ( KeyEvent event )
{
		switch (event.getChar())
		{

		case 'r':



			break;

		case 's':
			myfile.close();
			console() << "writing file" << endl;
			break;

		case 'q':
			mFullScreen = !mFullScreen;

			setFullScreen(mFullScreen);
			console() << mFullScreen << endl;
			break;

		default:
			break;
		}

}

void KinectCapGuiApp::update()
{
	displayGui();
}

void KinectCapGuiApp::draw()
{
	const gl::ScopedViewport scopedViewport(ivec2(0), getWindowSize());
	const gl::ScopedMatrices scopedMatrices;
	const gl::ScopedBlendAlpha scopedBlendAlpha;
	gl::setMatricesWindow(getWindowSize());
	gl::clear();
	gl::color(ColorAf::white());

	switch (mMode)
	{

		case IDLE:
			gl::clear(0);
			if (mChannelDepth) {
				gl::enable(GL_TEXTURE_2D);
				const gl::TextureRef tex = gl::Texture::create(*Kinect2::channel16To8(mChannelDepth));
				gl::draw(tex, tex->getBounds(), Rectf(getWindowBounds()));
			}
		break;

	case RECORDING:
		gl::disableDepthRead();
		gl::disableDepthWrite();

		gl::color(Color(1., 1., 1.));

		
		if (mChannelDepth) {
			gl::enable(GL_TEXTURE_2D);
			const gl::TextureRef tex = gl::Texture::create(*Kinect2::channel16To8(mChannelDepth));
			gl::draw(tex, tex->getBounds(), Rectf(getWindowBounds()));
		}

		if (mChannelBodyIndex) {
			gl::enable(GL_TEXTURE_2D);


			gl::pushMatrices();
			gl::scale(vec2(getWindowSize()) / vec2(mChannelBodyIndex->getSize()));
			gl::disable(GL_TEXTURE_2D);
			for (const Kinect2::Body &body : mBodyFrame.getBodies()) {
				if (body.isTracked()) {
					gl::color(ColorAf::white());
					for (const auto& joint : body.getJointMap()) {
						console() << joint.first << endl;
						if (joint.second.getTrackingState() == TrackingState::TrackingState_Tracked) {
							vec2 pos(mDevice->mapCameraToDepth(joint.second.getPosition()));
							if (record) {
								myfile << to_string(joint.second.getPosition().x) + ",";
								myfile << to_string(joint.second.getPosition().y) + ",";
								myfile << to_string(joint.second.getPosition().z) + ",";
							}
							gl::drawSolidCircle(pos, 5.0f, 32);
							vec2 parent(mDevice->mapCameraToDepth(body.getJointMap().at(joint.second.getParentJoint()).getPosition()));
							gl::drawLine(pos, parent);
						}
					}
					myfile << "\n";
				}
			}
			mRecordingTex->bind(0);
			gl::setMatricesWindow(getWindowSize());
			Rectf drawRect(10, 10, 40, 40); //display the recording symbol
			gl::draw(mRecordingTex, drawRect);
		}

	

		/*
		if (mChannelBodyIndex) {
			gl::enable(GL_TEXTURE_2D);
			gl::pushMatrices();
			gl::scale(vec2(getWindowSize()) / vec2(mChannelBodyIndex->getSize()));
			gl::disable(GL_TEXTURE_2D);
			for (const Kinect2::Body &body : mBodyFrame.getBodies()) {
				if (body.isTracked()) {
					console() << "should be recording" << endl;
						gl::color(ColorAf::white());
						for (const auto& joint : body.getJointMap()) {
							console() << joint.first << endl;
							if (joint.second.getTrackingState() == TrackingState::TrackingState_Tracked) {

								vec2 pos(mDevice->mapCameraToDepth(joint.second.getPosition()));
								myfile << to_string(joint.second.getPosition().x) + ",";
								myfile << to_string(joint.second.getPosition().y) + ",";
								myfile << to_string(joint.second.getPosition().z) + ",";

								gl::drawSolidCircle(pos, 5.0f, 32);
								vec2 parent(mDevice->mapCameraToDepth(body.getJointMap().at(joint.second.getParentJoint()).getPosition()));
								gl::drawLine(pos, parent);
							}
						}
						myfile << "\n";
					}
			}

			mRecordingTex->bind(0);
			gl::setMatricesWindow(getWindowSize());
			Rectf drawRect(10, 10, 40, 40); //display the recording symbol
			gl::draw(mRecordingTex, drawRect);

		}
		*/

		break;


	case PLAYBACK:
		gl::clear(Color(0.3, 0.3, 0.3));
		if (animate)
		{

			gl::setMatrices(mCam);

			{
				gl::color(Color(1., 1., 1.));
				gl::enableWireframe();
				gl::ScopedMatrices push;
				gl::translate(vec3(0., -20., 0.));
				mPlane->draw();
				gl::disableWireframe();

			}

			gl::color(Color(1., 1., 1.));

			for (auto j : mSkeletonContainer[count])
			{
				gl::ScopedMatrices push;
				gl::translate(j);
				mBatch->draw();
			}
			if( !isPaused) count++;

			if (count == mSkeletonContainer.size()) count = 0;
			mPlayBackTex->bind();

			//gl::clear(GL_COLOR_BUFFER_BIT);
			gl::setMatricesWindow(getWindowSize());
			Rectf drawRect(10, 10, 30, 30); //display the recording symbol
			gl::draw(mPlayBackTex, drawRect);

		}
		break;


	default:
		break;

	}



}


void KinectCapGuiApp::setupFile()
{
	// create csv file
	int r = (int)( randFloat() * 1000);
	myfile.open(fileName + ".csv");
	myfile << "Skeleton Data for: " + fileName + "\n";
	vector<std::string> joints = { "SpineBase", "SpineMid", "Neck", "Head", "ShoulderLeft", "ElbowLeft", "WristLeft", "HandLeft",
		"ShoulderRight", "ElbowRight", "WristRight", "HandRight", "HipLeft", "KneeLeft", "AnkleLeft", "FootLeft", "HipRight", "KneeRight",
		"AnkleRight", "FootRight", "SpineShoulder", "HandTipLeft", "ThumbLeft", "HandTipRight", "ThumbRight" };
	for (std::string s : joints) {
		myfile << s + ".x,";
		myfile << s + ".y,";
		myfile << s + ".z,";
	}
	myfile << "\n";
}


void KinectCapGuiApp::displayGui()
{

	opts.window(getWindow());

	ImVec2 spacing = ImVec2(15., 10.);
	ui::PushStyleVar(ImGuiStyleVar_ItemSpacing, spacing);

	switch (mMenu)
	{
	case MAIN:
	
	{
		ui::ScopedWindow window("MAIN MENU", ImGuiWindowFlags_NoMove);
		ui::SetWindowPos(ImVec2(getWindowWidth() - 200, 10));
		ui::Spacing();
		ui::Spacing();

		if (ui::Button("RECORD DATA")) mMenu = RECORD;
		if (ui::Button("PLAYBACK DATA")) mMenu = PLAY, mMode = PLAYBACK;
	}

	break;


	case RECORD:
		mRecordingTex->bind(0);

	{
		ui::ScopedWindow window("RECORDER", ImGuiWindowFlags_NoMove);
		ui::SetWindowPos(ImVec2(getWindowWidth() - 200, 10));
		ui::Text("Enter a filename");
		ui::InputText("", &fileName);

		ui::Spacing();

		if (!record) if (ui::Button("BEGIN RECORDING")) setupFile(), mMode = RECORDING, record = true;
		if (record) if (ui::Button("STOP RECORDING")) record = false, myfile.close(), mMode = IDLE;

		ui::Spacing();
		if (ui::Button("BACK TO MAIN MENU")) mMenu = MAIN, mMode = IDLE, record = false;
	}

	break;

	case PLAY:
		mPlayBackTex->bind(1);
		gl::enableDepth();
	{
		ui::ScopedWindow window("PLAYBACK", ImGuiWindowFlags_NoMove);
		ui::SetWindowPos(ImVec2(getWindowWidth() - 200, 10));
	ui::Spacing();
	if (!fileLoaded)
	{
		if (ui::Button("OPEN FILE")) loadCSV();
	} else {
		if (ui::Button("PLAY")) animate = true, isPaused = false;
		if (ui::Button("PAUSE")) isPaused = true;
		if (ui::Button("STOP")) animate = false;

		//display 
		std::string s = "Framecount: " + std::to_string(count) + " of " + std::to_string(totalFrames);

		char const *pchar = s.c_str();
		ui::Text( + pchar);
	}
	ui::Spacing();
	if (ui::Button("BACK")) mMenu = MAIN, mMode = IDLE, animate = false;
	}

		break;

	default:

		break;
		//add the GUI features

	}

	ui::PopStyleVar();
}

void KinectCapGuiApp::recordData()
{
		record = true;
		console() << "now recording!" << record;
}

void KinectCapGuiApp::setupGlsl()
{
	run = false;

	mCam.lookAt(vec3(0, 0, 50), vec3(0));
	mCam.setPerspective(60.f, getWindowAspectRatio(), 1.f, 1000.f);
	mCamUi = CameraUi(&mCam, getWindow());

	auto sph = geom::Sphere().subdivisions(32);
	auto plane = geom::Plane().subdivisions(vec2(32,32)) >> geom::Scale(100, 100, 100 );
	auto lambert = gl::ShaderDef().lambert().color();
	auto shader = gl::getStockShader(lambert);

	mPlane = gl::Batch::create(plane, shader);

	mBatch = gl::Batch::create(sph >> geom::Scale(0.4, 0.4, 0.4), shader);
	gl::enableDepth();

}

std::vector<vec3> KinectCapGuiApp::parseJointString(std::string jointString)
{
	std::vector<vec3>   skelly;
	std::vector<float>	currentJoint;

	//SPLIT EACH STRING AT THE COMMAS
	auto tokens = ci::split(jointString, ",", false);

	//console() << "num tokens: " << tokens.size() << endl;

	int counter = 0;
	for (auto t : tokens)
	{
		if (currentJoint.size() == 3) //if the count is greater than 2, ie. 3, it means we have the x,y and z components of the current joint's vec3
		{
			skelly.push_back(vec3(currentJoint[0] * SCALE_FACTOR, currentJoint[1] * SCALE_FACTOR, currentJoint[2] * SCALE_FACTOR)); //add the joint to the skeleton
			currentJoint.clear(); //clear out the current joint to make space for the x,y and z components of the next joint
		}

		if (t.size() > 0) currentJoint.push_back(std::stof(t)); //otherwise parse the string to a float and add is as the current vector component
		counter++;
	}


	return skelly;
}

void KinectCapGuiApp::loadCSV()
{

	try {
		fs::path path = getOpenFilePath("");

		console() << "loading data from CSV file" << endl;
		//LOAD JOINT DATA FROM CSV

		std::string	joints = loadString(loadFile(path));
		//SPLIT THE STRING GENERATED FROM THE CSV FILE INTO LINES
		auto entries = ci::split(joints, "\n", true);

		int count = 0;

		for (auto &entry : entries)
		{

			std::string line = boost::algorithm::trim_copy(boost::copy_range<std::string>(entry));
			if (line.empty()) continue;
			if (count > 2) mSkeletonContainer.push_back(parseJointString(line)); //pass the CSV string into the parseJointString() method, where it'll split up the string into separate joints and return them as a complete skeleton

			count++;
			totalFrames++;
		}

		//loadCSV(loadFile(path));
		//	loadCSV(path);
		console() << "file successfully loaded" << endl;
		fileLoaded = true;
	}
	catch (Exception &exc) {
		CI_LOG_EXCEPTION("failed to load file, not a valid CSV file.", exc);
	}

}


CINDER_APP(KinectCapGuiApp, RendererGl(RendererGl::Options().msaa(16)),
[&](App::Settings *settings){
	settings->setFullScreen();
	settings->setHighDensityDisplayEnabled();
})