├── thumbs.png
├── Platform.x64
├── src
    ├── SkeletonListener.cpp
    ├── Point3D.cpp
    ├── main.cpp
    ├── SkeletonDepth.cpp
    ├── SkeletonPoints.cpp
    ├── DrawAux.cpp
    ├── SampleViewer.cpp
    └── Skeleton.cpp
├── include
    ├── DrawAux.h
    ├── SkeletonListener.h
    ├── Point3D.h
    ├── SkeletonDepth.h
    ├── SkeletonPoints.h
    ├── SampleViewer.h
    └── Skeleton.h
├── INSTALL.txt
├── Makefile.noDepth
├── Platform.Arm
├── CommonTargets.mak
├── Platform.x86
├── LICENSE
├── Makefile.depth
├── Makefile
├── README.md
├── CommonDefs.mak
└── CommonCppMakefile


/thumbs.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/derzu/BodySkeletonTracker/HEAD/thumbs.png


--------------------------------------------------------------------------------
/Platform.x64:
--------------------------------------------------------------------------------
1 | # take this file's dir
2 | COMMON_MAK_DIR = $(dir $(lastword $(MAKEFILE_LIST)))
3 | 
4 | # everything is the same as in x86
5 | include $(COMMON_MAK_DIR)Platform.x86
6 | 


--------------------------------------------------------------------------------
/src/SkeletonListener.cpp:
--------------------------------------------------------------------------------
 1 | #include <SkeletonListener.h>
 2 | 
 3 | SkeletonListener::SkeletonListener() {
 4 | }
 5 | 
 6 | SkeletonListener::~SkeletonListener() {
 7 | }
 8 | 
 9 | 
10 | 


--------------------------------------------------------------------------------
/src/Point3D.cpp:
--------------------------------------------------------------------------------
 1 | #include "Point3D.h"
 2 | 
 3 | Point3D::Point3D(int x, int y, int z) : cv::Point(x,y) {
 4 | 	this->z = z;
 5 | }
 6 | 
 7 | Point3D::Point3D(int x, int y) : cv::Point(x,y) {
 8 | 	this->z = 0;
 9 | }
10 | 
11 | Point3D::Point3D() : cv::Point(0,0) {
12 | 	this->z = 0;
13 | }
14 | 
15 | Point3D::~Point3D() {
16 | }
17 | 


--------------------------------------------------------------------------------
/include/DrawAux.h:
--------------------------------------------------------------------------------
 1 | #include <opencv2/core.hpp>
 2 | #include "Point3D.h"
 3 | 
 4 | class DrawAux {
 5 | 
 6 | 	public:
 7 | 		static float euclideanDist(cv::Point& p, cv::Point& q);
 8 | 		static float euclideanDist3D(Point3D& p, Point3D& q);
 9 | 		static cv::Mat * thinning(cv::Mat &binarized);
10 | 		static std::vector<cv::Point> * lineBresenham(cv::Point p1, cv::Point p2);
11 | 
12 | };
13 | 


--------------------------------------------------------------------------------
/include/SkeletonListener.h:
--------------------------------------------------------------------------------
 1 | #ifndef SKELETON_LISTENER_H
 2 | #define SKELETON_LISTENER_H
 3 | 
 4 | #include <SkeletonPoints.h>
 5 | #include <vector>
 6 | #include <opencv2/highgui.hpp>
 7 | 
 8 | class SkeletonListener {
 9 | 
10 | 	public:
11 | 		SkeletonListener();
12 | 		virtual ~SkeletonListener();
13 | 		virtual std::vector<cv::Rect> *onEvent(SkeletonPoints * sp, int afa, Point3D * closest)=0;
14 | };
15 | 
16 | 
17 | #endif
18 | 


--------------------------------------------------------------------------------
/include/Point3D.h:
--------------------------------------------------------------------------------
 1 | #ifndef POINT3D_H
 2 | #define POINT3D_H
 3 | 
 4 | #include <opencv2/highgui.hpp>
 5 | 
 6 | /**
 7 |  * This class is a 3D Point based on the 2D point of the OpenCV.
 8 |  * 
 9 |  * @author derzu
10 |  **/
11 | class Point3D : public cv::Point {
12 | 	public:
13 | 		Point3D(int x, int y, int z);
14 | 		Point3D(int x, int y);
15 | 		Point3D();
16 | 		virtual ~Point3D();
17 | 		
18 | 		int z;
19 | };
20 | 
21 | 
22 | #endif
23 | 


--------------------------------------------------------------------------------
/INSTALL.txt:
--------------------------------------------------------------------------------
 1 | #### Requirement ####
 2 | Opencv:  version used: opencv-3.2.0
 3 | OpenNI2: version used: OpenNI-Linux-x64-2.3 (http://www.orbbec3d.net/Tools_SDK_OpenNI/2-Linux.zip)
 4 |          Required just if used the with the Depth Camera.
 5 | 
 6 | 
 7 | #### Compile #### (Just tested on Linux Ubuntu 14.04)
 8 |  - Makefile.noDepth:
 9 |       Uses you webcam
10 |  - Makefile.depth
11 |       Uses a depth camera, needs OpenNI2 driver.
12 |  - Default Makefile is the Makefile.depth
13 |  
14 | 
15 | 


--------------------------------------------------------------------------------
/Makefile.noDepth:
--------------------------------------------------------------------------------
 1 | CXX = g++
 2 | 
 3 | CXXFLAGS += -c -Wall $(shell pkg-config --cflags opencv)
 4 | LDFLAGS += $(shell pkg-config --libs --static opencv) -std=c++11 -pthread -I include
 5 | 
 6 | all: BodySkeletonTracker
 7 | 
 8 | BodySkeletonTracker:
 9 | 	$(CXX) src/main.cpp src/Skeleton.cpp src/SampleViewer.cpp src/Point3D.cpp src/SkeletonPoints.cpp src/DrawAux.cpp $(LDFLAGS) -o Bin/x64-Release/BodySkeletonTracker
10 | 
11 | clean: ; rm -f src/*.o Bin/x64-Release/BodySkeletonTracker *~ src/*~ include/*~
12 | 


--------------------------------------------------------------------------------
/Platform.Arm:
--------------------------------------------------------------------------------
 1 | ifeq "$(CFG)" "Release"
 2 | 
 3 |     # Hardware specifying flags
 4 |     CFLAGS += -march=armv7-a -mtune=cortex-a9 -mfpu=neon -mfloat-abi=hard #-mcpu=cortex-a8
 5 | 
 6 |     # Optimization level, minus currently buggy optimizing methods (which break bit-exact)
 7 |     CFLAGS += -O3 -fno-tree-pre -fno-strict-aliasing
 8 | 
 9 |     # More optimization flags
10 |     CFLAGS += -ftree-vectorize -ffast-math -funsafe-math-optimizations #-fsingle-precision-constant
11 | 
12 |     DEFINES += XN_NEON
13 |     CFLAGS += -flax-vector-conversions
14 | endif
15 | 


--------------------------------------------------------------------------------
/CommonTargets.mak:
--------------------------------------------------------------------------------
 1 | #############################################################################
 2 | # Targets
 3 | #############################################################################
 4 | .PHONY: all clean clean-$(OUTPUT_FILE)
 5 | 
 6 | # define the target 'all' (it is first, and so, default)
 7 | all: $(OUTPUT_FILE)
 8 | 
 9 | # Intermediate directory
10 | $(INT_DIR):
11 | 	mkdir -p $(INT_DIR)
12 | 
13 | # Output directory
14 | $(OUT_DIR):
15 | 	mkdir -p $(OUT_DIR)
16 | 
17 | # Final output file
18 | $(OUTPUT_FILE): $(SRC_FILES_LIST) | $(OUT_DIR)
19 | 
20 | clean-$(OUTPUT_FILE):
21 | 	rm -rf $(OUTPUT_FILE)
22 | 	
23 | clean: clean-$(OUTPUT_FILE)


--------------------------------------------------------------------------------
/Platform.x86:
--------------------------------------------------------------------------------
 1 | # some defaults
 2 | export GLUT_SUPPORTED=1
 3 | 
 4 | ifndef SSE_GENERATION
 5 | 	SSE_GENERATION = 3
 6 | endif
 7 | 
 8 | ifeq ("$(OSTYPE)","Darwin")
 9 | 	# Making the binary a universal one (x86 + x64)
10 | 	# Removed -arch i386 so compilation will work with libusb from older compiler
11 | 	CFLAGS += -arch x86_64
12 | 	LDFLAGS += -arch x86_64
13 | endif
14 | 
15 | ifeq ($(SSE_GENERATION), 2)
16 | 	CFLAGS += -msse2
17 | else
18 | 	ifeq ($(SSE_GENERATION), 3)
19 | 		CFLAGS += -msse3
20 | 		ifeq ($(SSSE3_ENABLED), 1)
21 | 			CFLAGS += -mssse3
22 | 		endif
23 | 	else
24 | 		DUMMY:=($error "Only SSE2 and SSE3 are supported")
25 | 	endif
26 | endif
27 | 


--------------------------------------------------------------------------------
/src/main.cpp:
--------------------------------------------------------------------------------
 1 | #include "SampleViewer.h"
 2 | 
 3 | int main(int argc, char** argv)
 4 | {
 5 | #ifdef DEPTH
 6 | 	int rc = openni::STATUS_OK;
 7 | 	const char* deviceURI = openni::ANY_DEVICE;
 8 | #else
 9 | 	int rc = 1;
10 | 	const char* deviceURI = NULL;
11 | #endif
12 | 
13 | #ifdef DEPTH
14 | printf("main::Compiled with Depth\n");
15 | #else
16 | printf("main::Compiled witout Depth\n");
17 | #endif
18 | 
19 | 	/*if (argc > 1)
20 | 	{
21 | 		deviceURI = argv[1];
22 | 	}*/
23 | 	SampleViewer sampleViewer("Body Skeleton Tracker", deviceURI);
24 | 	rc = sampleViewer.init();
25 | #ifdef DEPTH
26 | 	if (rc != openni::STATUS_OK)
27 | #else
28 | 	if (rc != 0)
29 | #endif
30 | 	{
31 | 		return 1;
32 | 	}
33 | 	sampleViewer.run();
34 | }
35 | 
36 | 


--------------------------------------------------------------------------------
/include/SkeletonDepth.h:
--------------------------------------------------------------------------------
 1 | #ifndef SKELETON_DEPTH_H
 2 | #define SKELETON_DEPTH_H
 3 | 
 4 | #include <OpenNI.h>
 5 | #include <opencv2/highgui.hpp>
 6 | #include "Point3D.h"
 7 | 
 8 | class SkeletonDepth {
 9 |     private:
10 | 	int width, height;
11 | 	int subSample;
12 | 	float rgb[3];
13 | 	int diff_w;
14 | 	int diff_h;
15 | 	int diff;
16 | 	int lineSize;
17 | 	int max;
18 | 	float maxDiff;
19 | 	float dist;
20 | 	Point3D * closest;
21 | 	Point3D * furthest;
22 | 
23 | 	const float * paintDepthCopyPixel(const openni::DepthPixel* pDepth, int x, int y, cv::Mat &binarized);
24 | 	void setDiffH(int d);
25 | 	void setDiffW(int d);
26 | 
27 |     public:
28 | 	SkeletonDepth(int, int, int);
29 | 	void paintDepthCopy(openni::RGB888Pixel*m_pTexMap, openni::VideoFrameRef *depthFrame, cv::Mat &binarized, short depthMat[]);
30 | 	void prepareAnalisa(Point3D * closest, Point3D * furthest);
31 | };
32 | 
33 | 
34 | #endif // SKELETON_DEPTH_H
35 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2018 Derzu
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/Makefile.depth:
--------------------------------------------------------------------------------
 1 | include CommonDefs.mak
 2 | 
 3 | BIN_DIR = Bin
 4 | 
 5 | INC_DIRS = include
 6 | 
 7 | SRC_FILES = src/*.cpp
 8 | 
 9 | ifeq ("$(OSTYPE)","Darwin")
10 | 	CFLAGS += -DMACOS
11 | 	LDFLAGS += -framework -framework
12 | else
13 | 	CFLAGS += -DUNIX -DGLX_GLXEXT_LEGACY
14 | 	USED_LIBS +=
15 | endif
16 | 
17 | USED_LIBS += OpenNI2 MWClosestPoint
18 | 
19 | #if DEPTH camera like Orbbec Astra, or Asus Xtion add this flag DEPTH. If not comment the following line
20 | CFLAGS += -DDEPTH
21 | 
22 | EXE_NAME = BodySkeletonTracker
23 | 
24 | #opencv
25 | CFLAGS += $(shell pkg-config --cflags opencv) -g -std=c++11 -pthread 
26 | LDFLAGS += $(shell pkg-config --libs --static opencv)
27 | 
28 | ifndef OPENNI2_INCLUDE
29 |     $(error OPENNI2_INCLUDE is not defined. Please define it or 'source' the OpenNIDevEnvironment file from the installation)
30 | else ifndef OPENNI2_REDIST
31 |     $(error OPENNI2_REDIST is not defined. Please define it or 'source' the OpenNIDevEnvironment file from the installation)
32 | endif
33 | 
34 | INC_DIRS += $(OPENNI2_INCLUDE)
35 | 
36 | include CommonCppMakefile
37 | 
38 | .PHONY: copy-redist
39 | copy-redist:
40 | 	cp -R $(OPENNI2_REDIST)/* $(OUT_DIR)
41 | 
42 | $(OUTPUT_FILE): copy-redist
43 | 
44 | 


--------------------------------------------------------------------------------
/Makefile:
--------------------------------------------------------------------------------
 1 | include CommonDefs.mak
 2 | 
 3 | BIN_DIR = Bin
 4 | 
 5 | INC_DIRS = include
 6 | 
 7 | SRC_FILES = src/*.cpp
 8 | 
 9 | ifeq ("$(OSTYPE)","Darwin")
10 | 	CFLAGS += -DMACOS
11 | 	LDFLAGS += -framework -framework
12 | else
13 | 	CFLAGS += -DUNIX -DGLX_GLXEXT_LEGACY
14 | 	USED_LIBS +=
15 | endif
16 | 
17 | USED_LIBS += OpenNI2
18 | 
19 | #if DEPTH camera like Orbbec Astra, or Asus Xtion add this flag DEPTH. If not comment the following line
20 | CFLAGS += -DDEPTH
21 | 
22 | #compile executable
23 | EXE_NAME = BodySkeletonTracker
24 | #compile dinamic lib
25 | #LIB_NAME = BodySkeletonTracker 
26 | #compile static lib
27 | #SLIB_NAME = BodySkeletonTracker
28 | 
29 | #opencv
30 | #CFLAGS += -g -std=c++11 
31 | LDFLAGS += $(shell pkg-config --libs --static opencv)
32 | 
33 | ifndef OPENNI2_INCLUDE
34 |     $(error OPENNI2_INCLUDE is not defined. Please define it or 'source' the OpenNIDevEnvironment file from the installation)
35 | else ifndef OPENNI2_REDIST
36 |     $(error OPENNI2_REDIST is not defined. Please define it or 'source' the OpenNIDevEnvironment file from the installation)
37 | endif
38 | 
39 | INC_DIRS += $(OPENNI2_INCLUDE)
40 | 
41 | include CommonCppMakefile
42 | 
43 | .PHONY: copy-redist
44 | copy-redist:
45 | 	cp -R $(OPENNI2_REDIST)/* $(OUT_DIR)
46 | 
47 | $(OUTPUT_FILE): copy-redist
48 | 
49 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # BodySkeletonTracker
 2 | # Description
 3 | Human body skeleton detection and tracking from video camera in real time. It was developed for an Orbbec Astra camera DRGB (Depth-RGB), uses the OpenNI2 driver, it should also works with Asus Xtion and Prime Sense. It works with normal webcam, in the case of background is smooth.
 4 | 
 5 | Currently it detects just the joints of head, shoulders, elbows and hands. A new version can be developed to also detect also feets and knees.
 6 | 
 7 | # Descrição
 8 | Detecção e rastreamento de um esqueleto humano a partir de câmera de vídeo em tempo real. Foi desenvolvido para a câmera DRGB (Depth-RGB) Orbbec Astra, utilizando o driver OpenNI2, funciona também com o Asus Xtion e Prime Sense. Funciona também com webcam normal, caso o background seja liso e branco.
 9 | 
10 | # Video Exemple
11 | 
12 | [![IMAGE ALT TEXT HERE](https://github.com/derzu/BodySkeletonTracker/blob/master/thumbs.png)](https://www.youtube.com/watch?v=9XYmkTN2RQY)
13 | 
14 | # Requirements
15 |  - OpenCV library (just for line/circle drawing and window frame showing)
16 |  - OpenNI2 library  
17 |  - It was developed and tested on Ubuntu 14.04. Should work at any Linux, maybe Mac and Windows too.  
18 | 
19 | # Compile and run
20 |  $ source ~/Downloads/OpenNI-Linux-x64-2.3/OpenNIDevEnvironment     (set your path appropriately)  
21 |  $ cd BodySkeletonTracker  
22 |  $ make  
23 |  $ cd Bin/x64-Release/  
24 |  $ ./BodySkeletonTracker  
25 | 


--------------------------------------------------------------------------------
/include/SkeletonPoints.h:
--------------------------------------------------------------------------------
 1 | #ifndef SKELETON_POINTS_H
 2 | #define SKELETON_POINTS_H
 3 | 
 4 | #include <Point3D.h>
 5 | 
 6 | #define BUF_SIZE 3
 7 | #define MAX_BODY_POINTS 8
 8 | 
 9 | /**
10 |  * This class has all the Skeleton Body info with the main 3D Points.
11 |  * 
12 |  * @author derzu
13 |  * 
14 |  **/
15 | class SkeletonPoints {
16 | 
17 | 	public:
18 | 		SkeletonPoints();
19 | 		virtual ~SkeletonPoints();
20 | 		void computePoint(int type);
21 | 		static void quick_sort(int *a, int left, int right);
22 | 
23 | 		// Main body points
24 | 		Point3D rightHand, rightElbow, rightShoulder;
25 | 		Point3D leftHand,  leftElbow,  leftShoulder;
26 | 		Point3D head;
27 | 		Point3D center;
28 | 
29 | 		// pointer to the main body points
30 | 		Point3D * bodyPoints[MAX_BODY_POINTS];
31 | 
32 | 		// Main body points CONSTANTS TYPES values
33 | 		static const int HEAD          = 0;
34 | 		static const int RIGHT_HAND    = 1;
35 | 		static const int RIGHT_ELBOW   = 2;
36 | 		static const int RIGHT_SHOULDER= 3;
37 |     		static const int LEFT_HAND     = 4;
38 |     		static const int LEFT_ELBOW    = 5;
39 |     		static const int LEFT_SHOULDER = 6;
40 | 		static const int CENTER        = 7;
41 | 
42 | 
43 | 		// Main body points vectors (history)
44 | 		Point3D pointsV[MAX_BODY_POINTS][BUF_SIZE];
45 | 
46 | 		// Main body points vector heads (last elemment added)
47 | 		unsigned char vHead[MAX_BODY_POINTS];
48 | 	private:
49 | 		void init();
50 | 		int addToVector(int type, Point3D *el);
51 | 		Point3D getMeanVector(int type);
52 | 		Point3D getMedianaVector(int type);
53 | 
54 | 
55 | 
56 | };
57 | 
58 | #endif
59 | 


--------------------------------------------------------------------------------
/include/SampleViewer.h:
--------------------------------------------------------------------------------
 1 | #ifndef SAMPLEVIEWER_H
 2 | #define SAMPLEVIEWER_H
 3 | 
 4 | #include "Skeleton.h"
 5 | 
 6 | 
 7 | #ifdef DEPTH
 8 | 
 9 | #include "SkeletonDepth.h"
10 | #include <OpenNI.h>
11 | 
12 | #endif
13 | 
14 | #include "SkeletonListener.h"
15 | 
16 | class SampleViewer
17 | {
18 | public:
19 | 	SampleViewer(const char* strSampleName, const char* deviceUri);
20 | 	virtual ~SampleViewer();
21 | 
22 | 	 int init();
23 | 	 int run();
24 | 	 void registerListener(SkeletonListener * listener);
25 | 
26 | protected:
27 | 	virtual void display();
28 | 
29 | 	void finalize();
30 | 
31 | private:
32 | 	SampleViewer(const SampleViewer&);
33 | 	SampleViewer& operator=(SampleViewer&);
34 | 	void notifyListeners(SkeletonPoints * sp, int afa, Point3D *closest, cv::Mat &frame);
35 | 
36 | 	static SampleViewer* ms_self;
37 | 	
38 | 	char			m_strSampleName[50];
39 | 	unsigned int		width;
40 | 	unsigned int		height;
41 | 	int subSample;
42 | 	long frameCount;
43 | 
44 | 	unsigned char * m_pTexMap;
45 | 	
46 | 	cv::VideoWriter * outputVideo;
47 | 
48 | 	// Detecta o esqueleto
49 | 	Skeleton * skel;
50 | 
51 | 	// lista de listeners que receberao o esqueleto.	
52 | 	std::vector<SkeletonListener*> listeners;
53 | 
54 | #ifdef DEPTH
55 | 	// OPENNI2
56 | 	int initOpenNI(const char* deviceUri);
57 | 	Point3D* getClosestPoint(openni::VideoFrameRef *frame, Point3D *furthest);
58 | 	openni::VideoFrameRef * getNextFrame();
59 | 	openni::VideoStream depth;
60 | 	openni::Device device;
61 | 
62 | 	SkeletonDepth * skelD;
63 | #else
64 | 	// OPENCV
65 | 	cv::VideoCapture capture;
66 | #endif
67 | };
68 | 
69 | 
70 | #endif // VIEWER_H
71 | 


--------------------------------------------------------------------------------
/CommonDefs.mak:
--------------------------------------------------------------------------------
 1 | ifndef _COMMON_DEFS_MAKE_
 2 | _COMMON_DEFS_MAKE_=1
 3 | 
 4 | # some defaults
 5 | ifndef CFG
 6 |     CFG = Release
 7 | endif
 8 | 
 9 | # find out the platform on which we're running
10 | MACHINE = $(shell uname -m)
11 | ifneq (,$(findstring x86_64,$(MACHINE)))
12 | 	HOST_PLATFORM = x64
13 | else ifneq (,$(findstring x86,$(MACHINE)))
14 | 	HOST_PLATFORM = x86
15 | else ifneq (,$(findstring i686,$(MACHINE)))
16 | 	HOST_PLATFORM = x86
17 | else ifneq (,$(findstring i386,$(MACHINE)))
18 | 	HOST_PLATFORM = x86
19 | else ifneq (,$(findstring arm,$(MACHINE)))
20 | 	HOST_PLATFORM = Arm
21 | else
22 | 	DUMMY:=$(error Can't determine host platform)
23 | endif
24 | 
25 | # now check if this is a cross-compilation or not
26 | ifeq "$(PLATFORM)" ""
27 | 	PLATFORM = $(HOST_PLATFORM)
28 | else
29 | 	ifneq "$(PLATFORM)" "$(HOST_PLATFORM)"
30 | 		# cross compiling. Take CXX and STAGING_DIR from environment
31 | 		PLATFORM_UPPER = $(shell echo $(PLATFORM) | tr 'a-z' 'A-Z')
32 | 		DUMMY:=$(eval CXX = $($(PLATFORM_UPPER)_CXX))
33 | 		DUMMY:=$(eval TARGET_SYS_ROOT = $($(PLATFORM_UPPER)_STAGING))
34 | 		
35 | 		ifeq "$(and $(CXX), $(TARGET_SYS_ROOT))" ""
36 | 			DUMMY:=$(error Cross-Compilation error. Can't find $(PLATFORM_UPPER)_CXX and $(PLATFORM_UPPER)_STAGING)
37 | 		endif
38 | 	endif
39 | endif
40 | 
41 | # expand file list
42 | SRC_FILES_LIST = $(wildcard $(SRC_FILES))
43 | 
44 | # define the intermediate directory
45 | INT_DIR = $(BIN_DIR)/Intermediate/$(PLATFORM)-$(CFG)/$(OUTPUT_NAME)
46 | 
47 | # define output directory
48 | OUT_DIR = $(BIN_DIR)/$(PLATFORM)-$(CFG)
49 | 
50 | # full path to output file
51 | OUTPUT_FILE = $(OUT_DIR)/$(OUTPUT_NAME)
52 | 
53 | # take this file's dir
54 | COMMON_MAK_DIR = $(dir $(lastword $(MAKEFILE_LIST)))
55 | 
56 | # get the OS type
57 | OSTYPE := $(shell uname -s)
58 | 
59 | # platform specific args
60 | include $(COMMON_MAK_DIR)Platform.$(PLATFORM)
61 | 
62 | endif # _COMMON_DEFS_MAKE_
63 | 


--------------------------------------------------------------------------------
/include/Skeleton.h:
--------------------------------------------------------------------------------
 1 | #ifndef SKELETON_H
 2 | #define SKELETON_H
 3 | 
 4 | #include <opencv2/highgui.hpp>
 5 | #include <Point3D.h>
 6 | #include <vector>
 7 | 
 8 | #include "SkeletonPoints.h"
 9 | 
10 | /**
11 |  * This class process a frame and find the main skeleton points, that will be stored at a SkeletonPoints.
12 |  *
13 |  * @author derzu
14 |  **/
15 | class Skeleton {
16 |     private:
17 |         int subSample;
18 | 	int width, height;
19 | 
20 | 	int wC; // w clear
21 | 	int hC; // w clear
22 | 	cv::Point right, left, topCenter, topRight, topLeft, bottomCenter, bottomRight, bottomLeft;
23 | 
24 | 	// Main interest points
25 | 	Point3D maxRight, maxLeft;
26 | 	Point3D maxTopCenter,    maxTopRight,    maxTopLeft;
27 | 	Point3D maxBottomCenter, maxBottomRight, maxBottomLeft;
28 | 	Point3D middleArmRight, middleArmLeft;
29 | 	Point3D middleArmRight45, middleArmLeft45;
30 | 	Point3D maxRightMaxBottom, maxLeftMaxBottom;
31 | 	Point3D middleStraightArmRight, middleStraightArmLeft;
32 | 
33 | 	// Skeleton Points (hands, shoulders, elbow, head)
34 | 	// Pontos do esqueleto (maos, ombros, cotovelos, cabeca)
35 | 	SkeletonPoints * sp;
36 | 
37 | 	// Depth Mat	
38 | 	short * depthMat;
39 | 
40 | 	// afastamento
41 | 	int afa;
42 | 	float afa28;
43 | 	int shift;
44 | 
45 | 	std::vector<Point3D> rightArm;
46 | 	std::vector<Point3D> leftArm;
47 | 
48 | 	bool showSkeleton;
49 | 
50 |     protected:
51 | 	void initialize();
52 | 	void zeraMaximus();
53 | 	void setMaximus();
54 | 	void locateShoulders(cv::Mat &frame);
55 | 	void getSizeRegionRecursive(unsigned char * frame, int x, int y, int *quant);
56 | 	int  getSizeRegion(unsigned char * frame, int x, int y);
57 | 	void clearRegionRecursive(unsigned char * frame, int x, int y);
58 | 	void clearRegion(unsigned char * frame, int x, int y);
59 | 	Point3D * getElbowHard(std::vector<Point3D> &armPoints, int ang);
60 | 	Point3D mediaPoint(cv::Mat * frame);
61 | 	void obtainZ(Point3D &point);
62 | 	void removeSmallsRegions(cv::Mat * frame);
63 | 	void locateMaximus(cv::Mat *frame);
64 | 	int  getMeanDepthValue(cv::Point& p);
65 | 	bool isLineInside(cv::Mat &frame, cv::Point p1, cv::Point p2);
66 | 	int  qPointsLineOutside(cv::Mat &frame, cv::Point p1, cv::Point p2);
67 | 	bool verifyRegion(unsigned char * frame, int x, int y);
68 | 	
69 | 
70 |     public:
71 | 	Skeleton(int width, int height, int subSample);
72 | 	virtual ~Skeleton();
73 | 	void locateMainBodyPoints(cv::Mat &frame);
74 | 	void drawMarkers(cv::Mat &frame);
75 | 	void setDepthMat(short depth[]);
76 | 	void notifyListeners();
77 | 	void drawOverFrame(cv::Mat * skelImg, cv::Mat &frame);
78 | 	void drawOverFrame2(cv::Mat * bin,    cv::Mat &frame);
79 | 	void drawOverFrame(std::vector<Point3D> pontos, cv::Mat &frame);
80 | 	void detectBiggerRegion(cv::Mat &frame);
81 | 	std::vector<Point3D> getSkeletonArm(cv::Mat * skeleton, bool right);
82 | 	SkeletonPoints* getSkeletonPoints();
83 | 	int getAfa();
84 | 	void analyse(cv::Mat * skeleton);
85 | };
86 | 
87 | 
88 | #endif // SKELETON_H
89 | 


--------------------------------------------------------------------------------
/src/SkeletonDepth.cpp:
--------------------------------------------------------------------------------
  1 | #include "SkeletonDepth.h"
  2 | #include <stdio.h>
  3 | #include <opencv2/imgproc.hpp>
  4 | 
  5 | using namespace cv;
  6 | 
  7 | SkeletonDepth::SkeletonDepth(int width, int height, int subSample) {
  8 | 	this->width = width;
  9 | 	this->height = height;
 10 | 	this->subSample = subSample;
 11 | 
 12 | 	lineSize = width/subSample;
 13 | 
 14 | 	diff_w = 0;
 15 | 	diff_h = 0;
 16 | 	diff = 180 + diff_w + diff_h;
 17 | 	dist = 0;
 18 | 	maxDiff = 240;
 19 | 	max = 0;
 20 | }
 21 | 
 22 | 
 23 | 
 24 | void SkeletonDepth::paintDepthCopy(openni::RGB888Pixel*m_pTexMap, openni::VideoFrameRef * depthFrame, cv::Mat &binarized, short depthMat[]) {
 25 | 	float factor[3] = {1, 1, 1};
 26 | 	const float *f;
 27 | 
 28 | 	const openni::DepthPixel* pDepthRow = (const openni::DepthPixel*)depthFrame->getData();
 29 | 	openni::RGB888Pixel* pTexRow = m_pTexMap + depthFrame->getCropOriginY() * width;
 30 | 	int rowSize = depthFrame->getStrideInBytes() / sizeof(openni::DepthPixel);
 31 | 
 32 | 	//printf("sizeof(openni::DepthPixel)=%ld\n", sizeof(openni::DepthPixel));
 33 | 	//printf("sizeof(short)=%ld\n", sizeof(short));
 34 | 
 35 | 	max = 0;
 36 | 	if (pDepthRow && closest) 
 37 | 		for (int y = 0; y < height; ++y)
 38 | 		{
 39 | 			const openni::DepthPixel* pDepth = pDepthRow;
 40 | 			openni::RGB888Pixel* pTex = pTexRow + depthFrame->getCropOriginX();
 41 | 			setDiffH(abs(closest->y-y)/5); // diferenca (height) do ponto atual para o ponto mais proximo
 42 | 			for (int x = 0; x < width; ++x, ++pDepth, ++pTex)
 43 | 			{
 44 | 				if (*pDepth != 0)
 45 | 				{
 46 | 					setDiffW(abs(closest->x-x)/5); // diferenca (width) do ponto atual para o ponto mais proximo
 47 | 					f = paintDepthCopyPixel(pDepth, x, y, binarized);
 48 | 					depthMat[y*width + x] = *pDepth; 
 49 | 					//printf("vals: %d\n", *pDepth);
 50 | 					if (f)
 51 | 						memcpy(factor, f, sizeof(float)*3);
 52 | 
 53 | 					int nHistValue = 256*(1.0f - (*pDepth)/(float)(furthest->z-closest->z));
 54 | 					//printf("::depths::%d::%d - %d::nHistValue = %d::%d\n", *pDepth, furthest->z, closest->z, nHistValue, old);
 55 | 					pTex->r = nHistValue*factor[0];
 56 | 					pTex->g = nHistValue*factor[1];
 57 | 					pTex->b = nHistValue*factor[2];
 58 | 					factor[0] = factor[1] = factor[2] = 1;
 59 | 				}
 60 | 			}
 61 | 			pDepthRow += rowSize;
 62 | 			pTexRow += width;
 63 | 		}
 64 | 	else
 65 | 		printf("2::pDepthRow = NULL\n");
 66 | 
 67 | 	if (max>0)
 68 | 		maxDiff = max;
 69 | }
 70 | 
 71 | 
 72 | 
 73 | 
 74 | 
 75 | const float * SkeletonDepth::paintDepthCopyPixel(const openni::DepthPixel* pDepth, int x, int y, cv::Mat &binarized) {
 76 | 	//diff = 180 + diff_w + diff_h;
 77 | 	diff = 600 + diff_w + diff_h;
 78 | 
 79 | 	if (*pDepth == closest->z)
 80 | 	{
 81 | 		rgb[0] = 0; // R
 82 | 		rgb[1] = 1; // G
 83 | 		rgb[2] = 0; // B
 84 | 
 85 | 		return rgb;
 86 | 	}
 87 | 	else if (*pDepth >= closest->z && *pDepth <= closest->z+diff)
 88 | 	{
 89 | 		if (y%subSample==0 && x%subSample==0)
 90 | 			binarized.data[(y/subSample)*lineSize+x/subSample]=255;
 91 | 
 92 | 		if (diff>max) {
 93 | 			max = diff;
 94 | 		}
 95 | 
 96 | 		dist = ((*pDepth) - closest->z)/maxDiff;
 97 | 		if (dist<1) {
 98 | 			rgb[2] = 1-dist; // R
 99 | 			rgb[0] = dist;   // B
100 | 		}
101 | 		else {
102 | 			rgb[2] = 0; // R
103 | 			rgb[0] = 1; // B
104 | 		}
105 | 		rgb[1] = 0;      // G
106 | 
107 | 		return rgb;
108 | 	}
109 | 
110 | 
111 | 	return 0;
112 | }
113 | 
114 | 
115 | void SkeletonDepth::prepareAnalisa(Point3D * closest, Point3D *furthest) {
116 | 	this->closest = closest;
117 | 	this->furthest = furthest;
118 | }
119 | 
120 | void SkeletonDepth::setDiffH(int d) {
121 | 	diff_h = d;
122 | }
123 | 
124 | void SkeletonDepth::setDiffW(int d) {
125 | 	diff_w = d;
126 | }
127 | 


--------------------------------------------------------------------------------
/src/SkeletonPoints.cpp:
--------------------------------------------------------------------------------
  1 | #include "SkeletonPoints.h"
  2 | 
  3 | /**
  4 |  * This class has all the Skeleton Body info with the main 3D Points.
  5 |  * 
  6 |  * @author derzu
  7 |  * 
  8 |  **/
  9 |  
 10 |  
 11 | 
 12 | /**
 13 |  * The constructor
 14 |  **/
 15 | SkeletonPoints::SkeletonPoints() {
 16 | 	Point3D zero = Point3D(0,0,0);
 17 | 	this->rightHand = zero;
 18 | 	this->rightElbow = zero;
 19 | 	this->rightShoulder = zero;
 20 | 
 21 | 	this->leftHand = zero;
 22 | 	this->leftElbow = zero;
 23 | 	this->leftShoulder = zero;
 24 | 
 25 | 	this->head = zero;
 26 | 
 27 | 	init();
 28 | }
 29 | 
 30 | SkeletonPoints::~SkeletonPoints() {
 31 | 	
 32 | }
 33 | 
 34 | void SkeletonPoints::init() {
 35 | 	bodyPoints[HEAD]           = &head;
 36 | 	bodyPoints[RIGHT_HAND]     = &rightHand;
 37 | 	bodyPoints[RIGHT_ELBOW]    = &rightElbow;
 38 | 	bodyPoints[RIGHT_SHOULDER] = &rightShoulder;
 39 | 	bodyPoints[LEFT_HAND]      = &leftHand;
 40 | 	bodyPoints[LEFT_ELBOW]     = &leftElbow;
 41 | 	bodyPoints[LEFT_SHOULDER]  = &leftShoulder;
 42 | 	bodyPoints[CENTER]         = &center;
 43 | 
 44 | 	bzero(pointsV,  sizeof(Point3D)*MAX_BODY_POINTS*BUF_SIZE);
 45 | 	bzero(vHead,    MAX_BODY_POINTS);
 46 | }
 47 | 
 48 | 
 49 | /**
 50 |  * Add a point to a circular vector.
 51 |  *
 52 |  * @param type the type of the point. The types are declared at the .h file.
 53 |  *
 54 |  * @return 1 if sucess, -1 if error.
 55 |  **/
 56 | int SkeletonPoints::addToVector(int type, Point3D * el) {
 57 | 	if (type>MAX_BODY_POINTS || type<0)
 58 | 		return -1; // error
 59 | 
 60 | 	pointsV[type][(vHead[type]++) % BUF_SIZE] = *el;
 61 | 
 62 | 	return 1;// addition sucessfuly
 63 | }
 64 | 
 65 | 
 66 | /**
 67 |  * Compute the mean point of a vector of 3D Points.
 68 |  *
 69 |  * @param type the type of the point. The types are declared at the .h file.
 70 |  *
 71 |  * @return the mean 3D Point.
 72 |  **/
 73 | Point3D SkeletonPoints::getMeanVector(int type) {
 74 | 	Point3D *vector = pointsV[type];
 75 | 
 76 | 	Point3D m = Point3D(0,0);
 77 | 	int q=0;
 78 | 	for (int i=0 ; i<BUF_SIZE ; i++) {
 79 | 		if (vector[i].x!=0) {
 80 | 			m.x += vector[i].x;
 81 | 			m.y += vector[i].y;
 82 | 			m.z += vector[i].z;
 83 | 			q++;
 84 | 		}
 85 | 	}
 86 | 	if (q>0) {
 87 | 		m.x /= q;
 88 | 		m.y /= q;
 89 | 		m.y /= q;
 90 | 	}
 91 | 
 92 | 	return m;
 93 | }
 94 | 
 95 | 
 96 | /**
 97 |  * Add the point of the specified type to a history vector.
 98 |  * Then change the value of the point to the Mean/Median of the vector of the last BUF_SIZE points. 
 99 |  *
100 |  * @param type the type of the point. The types are declared at the .h file.
101 |  **/
102 | void SkeletonPoints::computePoint(int type) {
103 | 	if (bodyPoints[type]->x!=0)
104 | 		addToVector(type, bodyPoints[type]);
105 | 	//*(bodyPoints[type]) = getMeanVector(type);
106 | 	*(bodyPoints[type]) = getMedianaVector(type);
107 | }
108 | 
109 | 
110 | /**
111 |  * Compute the median point of a vector of 3D Points.
112 |  *
113 |  * @param type the type of the point. The types are declared at the .h file.
114 |  *
115 |  * @return the median 3D Point.
116 |  **/
117 | Point3D SkeletonPoints::getMedianaVector(int type) {
118 | 	Point3D *vector = pointsV[type];
119 | 	int q1=0, q2=0, q3=0;
120 | 	Point3D m = Point3D(0,0);
121 | 
122 | 	// serpara o vetor de pontos em 3 vetores
123 | 	int v1[BUF_SIZE], v2[BUF_SIZE], v3[BUF_SIZE];
124 | 	for (int i=0 ; i<BUF_SIZE ; i++) {
125 | 		v1[i] = vector[i].x;
126 | 		v2[i] = vector[i].y;
127 | 		v3[i] = vector[i].z;
128 | 	}
129 | 
130 | 	quick_sort(v1, 0, BUF_SIZE);
131 | 	quick_sort(v2, 0, BUF_SIZE);
132 | 	quick_sort(v3, 0, BUF_SIZE);
133 | 
134 | 	for (int i=0 ; i<BUF_SIZE ; i++) {
135 | 		if (v1[i]!=0)
136 | 			q1++;
137 | 		if (v2[i]!=0)
138 | 			q2++;
139 | 		if (v3[i]!=0)
140 | 			q3++;
141 | 	}
142 | 	if (q1>0)
143 | 		m.x = v1[q1/2+BUF_SIZE-q1];
144 | 	if (q2>0)
145 | 		m.y = v2[q2/2+BUF_SIZE-q2];
146 | 	if (q3>0)
147 | 		m.z = v3[q3/2+BUF_SIZE-q3];
148 | 
149 | 	return m;
150 | }
151 | 
152 | 
153 | /**
154 |  *  Quick sort function
155 |  *
156 |  **/
157 | void SkeletonPoints::quick_sort(int *a, int left, int right) {
158 |     int i, j, x, y;
159 |      
160 |     i = left;
161 |     j = right;
162 |     x = a[(left + right) / 2];
163 |      
164 |     while(i <= j) {
165 |         while(a[i] < x && i < right) {
166 |             i++;
167 |         }
168 |         while(a[j] > x && j > left) {
169 |             j--;
170 |         }
171 |         if(i <= j) {
172 |             y = a[i];
173 |             a[i] = a[j];
174 |             a[j] = y;
175 |             i++;
176 |             j--;
177 |         }
178 |     }
179 |      
180 |     if(j > left) {
181 |         quick_sort(a, left, j);
182 |     }
183 |     if(i < right) {
184 |         quick_sort(a, i, right);
185 |     }
186 | }
187 | 
188 | 
189 | 


--------------------------------------------------------------------------------
/CommonCppMakefile:
--------------------------------------------------------------------------------
  1 | #############################################################################
  2 | # Primesense template makefile.
  3 | # This file should not be made, but only included from other makefiles.
  4 | # By default, this makefile compiles in release. To compile a debug version:
  5 | #    make CFG=Debug
  6 | #
  7 | # Project makefile should define the following BEFORE including this file:
  8 | # SRC_FILES - a list of all source files
  9 | # Output name under one of the following:
 10 | #     EXE_NAME (executable), 
 11 | #     LIB_NAME (dynamic library) or 
 12 | #     SLIB_NAME (static library) or
 13 | # BIN_DIR - Bin directory (output dir)
 14 | # INC_DIRS - a list of additional include directories
 15 | # LIB_DIRS - a list of additional library directories
 16 | # USED_LIBS - a list of libraries to link with
 17 | # DEFINES - [Optional] additional preprocessor defines
 18 | # CFLAGS - [Optional] additional flags for the compiler
 19 | # LDFLAGS - [Optional] additional flags for the linker
 20 | # SSE_GENERATION - [Optional] The SSE generation to use (default is 3)
 21 | # TARGET_SYS_ROOT - [Optional] The path to the root of the target
 22 | # ALLOW_WARNINGS - [Optional] Allow warnings during compilation
 23 | #############################################################################
 24 | 
 25 | # take this file's dir
 26 | COMMON_CPP_MAKE_FILE_DIR = $(dir $(lastword $(MAKEFILE_LIST)))
 27 | 
 28 | include $(COMMON_CPP_MAKE_FILE_DIR)CommonDefs.mak
 29 | 
 30 | # define a function to figure .o file for each source file (placed under intermediate directory)
 31 | SRC_TO_OBJ = $(addprefix ./$(INT_DIR)/,$(addsuffix .o,$(notdir $(basename $1))))
 32 | 
 33 | # create a list of all object files
 34 | OBJ_FILES = $(call SRC_TO_OBJ,$(SRC_FILES_LIST))
 35 | 
 36 | # define a function to translate any source file to its dependency file (note that the way we create
 37 | # dep files, as a side affect of compilation, always puts the files in the INT_DIR with suffix .d)
 38 | SRC_TO_DEP = $(addprefix ./$(INT_DIR)/,$(addsuffix .d,$(notdir $(basename $1))))
 39 | 
 40 | # create a list of all dependency files
 41 | DEP_FILES = $(call SRC_TO_DEP,$(SRC_FILES_LIST))
 42 | 
 43 | # older version of gcc doesn't support the '=' symbol in include dirs, so we replace it ourselves with sysroot
 44 | INC_DIRS_FROM_SYSROOT = $(patsubst =/%,$(TARGET_SYS_ROOT)/%,$(INC_DIRS))
 45 | 
 46 | # append the -I switch to each include directory
 47 | INC_DIRS_OPTION = $(foreach dir,$(INC_DIRS_FROM_SYSROOT),-I$(dir))
 48 | 
 49 | # append the -L switch to each library directory
 50 | LIB_DIRS_OPTION = $(foreach dir,$(LIB_DIRS),-L$(dir)) -L$(OUT_DIR)
 51 | 
 52 | # append the -l switch to each library used
 53 | USED_LIBS_OPTION = $(foreach lib,$(USED_LIBS),-l$(lib))
 54 | 
 55 | # append the -D switch to each define
 56 | DEFINES_OPTION = $(foreach def,$(DEFINES),-D$(def))
 57 | 
 58 | # tell compiler to use the target system root
 59 | ifdef TARGET_SYS_ROOT
 60 | 	CFLAGS += --sysroot=$(TARGET_SYS_ROOT)
 61 | 	LDFLAGS += --sysroot=$(TARGET_SYS_ROOT)
 62 | endif
 63 | 
 64 | # set Debug / Release flags
 65 | ifeq "$(CFG)" "Debug"
 66 | 	CFLAGS += -O0 -g
 67 | endif
 68 | ifeq "$(CFG)" "Release"
 69 | 	CFLAGS += -O2 -DNDEBUG
 70 | endif
 71 | 
 72 | CFLAGS += $(INC_DIRS_OPTION) $(DEFINES_OPTION)
 73 | CFLAGS += -fPIC -fvisibility=hidden
 74 | 
 75 | ifneq "$(ALLOW_WARNINGS)" "1"
 76 | 	CFLAGS += -Werror
 77 | ifeq ("$(OSTYPE)","Darwin")
 78 | 	CFLAGS += -Wno-deprecated-declarations -Wno-unused-private-field -Wno-unused-const-variable
 79 | endif
 80 | endif
 81 | 
 82 | LDFLAGS += $(LIB_DIRS_OPTION) $(USED_LIBS_OPTION)
 83 | 
 84 | # some lib / exe specifics
 85 | ifneq "$(LIB_NAME)" ""
 86 | 	OUTPUT_NAME = lib$(LIB_NAME).so
 87 | 	ifneq ("$(OSTYPE)","Darwin")
 88 | 		LDFLAGS += -Wl,--no-undefined
 89 | 		OUTPUT_NAME = lib$(LIB_NAME).so
 90 | 		OUTPUT_COMMAND = $(CXX) -o $(OUTPUT_FILE) $(OBJ_FILES) $(LDFLAGS) -shared
 91 | 	else
 92 | 		LDFLAGS += -undefined error
 93 | 		OUTPUT_NAME = lib$(LIB_NAME).dylib
 94 | 		OUTPUT_COMMAND = $(CXX) -o $(OUTPUT_FILE) $(OBJ_FILES) $(LDFLAGS) -dynamiclib -headerpad_max_install_names -install_name $(OUTPUT_NAME)
 95 | 	endif
 96 | endif
 97 | ifneq "$(EXE_NAME)" ""
 98 | 	OUTPUT_NAME = $(EXE_NAME)
 99 | 	# We want the executables to look for the .so's locally first:
100 | 	LDFLAGS += -Wl,-rpath ./
101 | 	OUTPUT_COMMAND = $(CXX) -o $(OUTPUT_FILE) $(OBJ_FILES) $(LDFLAGS)
102 | endif
103 | ifneq "$(SLIB_NAME)" ""
104 | 	OUTPUT_NAME = lib$(SLIB_NAME).a
105 | 
106 | 	ifneq ("$(OSTYPE)","Darwin")
107 | 		OUTPUT_COMMAND = $(AR) -cq $(OUTPUT_FILE) $(OBJ_FILES)
108 | 	else
109 | 		OUTPUT_COMMAND = libtool -static -o $(OUTPUT_FILE) $(OBJ_FILES)
110 | 	endif
111 | endif
112 | 
113 | define CREATE_SRC_TARGETS
114 | # create a target for the object file (the CXX command creates both an .o file
115 | # and a .d file)
116 | ifneq ("$(OSTYPE)","Darwin")
117 | $(call SRC_TO_OBJ,$1) : $1 | $(INT_DIR)
118 | 	$(CXX) -MD -MP -MT "$(call SRC_TO_DEP,$1) $$@" -c $(CFLAGS) -o $$@ $$<
119 | else
120 | $(call SRC_TO_OBJ,$1) : $1 | $(INT_DIR)
121 | 	$(CXX) -x c++ -c $(CFLAGS) -o $$@ $$<
122 | endif
123 | endef
124 | 
125 | #############################################################################
126 | # Targets
127 | #############################################################################
128 | .PHONY: clean-objs clean-defs
129 | 
130 | include $(COMMON_CPP_MAKE_FILE_DIR)CommonTargets.mak
131 | 
132 | # create targets for each source file
133 | $(foreach src,$(SRC_FILES_LIST),$(eval $(call CREATE_SRC_TARGETS,$(src))))
134 | 
135 | # include all dependency files (we don't need them the first time, so we can use -include)
136 | -include $(DEP_FILES)
137 | 
138 | $(OUTPUT_FILE): $(OBJ_FILES)
139 | 	$(OUTPUT_COMMAND)
140 | 
141 | clean-objs:
142 | 	rm -rf $(OBJ_FILES)
143 | 	
144 | clean-defs:
145 | 	rm -rf $(DEP_FILES)
146 | 
147 | clean-temp:
148 | 	rm *~ src/*~ include/*~
149 | 
150 | clean: clean-objs clean-defs clean-temp
151 | 


--------------------------------------------------------------------------------
/src/DrawAux.cpp:
--------------------------------------------------------------------------------
  1 | #include "DrawAux.h"
  2 | #include <opencv2/imgproc.hpp>
  3 | 
  4 | using namespace cv;
  5 | 
  6 | /**
  7 |  * Calculate de Euclidian distance 2D between 2 points.
  8 |  **/
  9 | float DrawAux::euclideanDist(cv::Point& p, cv::Point& q) {
 10 | 	if (p.x==q.x && p.y==q.y)
 11 | 		return 0;
 12 | 	cv::Point diff = p - q;
 13 | 	return cv::sqrt(diff.x*diff.x + diff.y*diff.y);
 14 | }
 15 | 
 16 | 
 17 | /**
 18 |  * Calculate de Euclidian distance 3D between 2 points.
 19 |  **/
 20 | float DrawAux::euclideanDist3D(Point3D& p, Point3D& q) {
 21 | 	if (p.x==q.x && p.y==q.y)
 22 | 		return 0;
 23 | 	return cv::sqrt( (p.x-q.x)*(p.x-q.x) + (p.x-q.x)*(p.x-q.x) + (p.y-q.y)*(p.z-q.z) );
 24 | }
 25 | 
 26 | 
 27 | 
 28 | 
 29 | 
 30 | /**
 31 |  * Code for thinning a binary image using Zhang-Suen algorithm.
 32 |  *
 33 |  * Author:  Nash (nash [at] opencv-code [dot] com) 
 34 |  * Website: http://opencv-code.com
 35 |  * Source: https://raw.githubusercontent.com/bsdnoobz/zhang-suen-thinning/master/thinning.cpp
 36 |  */
 37 | /**
 38 |  * Perform one thinning iteration.
 39 |  * Normally you wouldn't call this function directly from your code.
 40 |  *
 41 |  * Parameters:
 42 |  * 		im    Binary image with range = [0,1]
 43 |  * 		iter  0=even, 1=odd
 44 |  */
 45 | void thinningIteration(cv::Mat& img, int iter)
 46 | {
 47 |     CV_Assert(img.channels() == 1);
 48 |     CV_Assert(img.depth() != sizeof(uchar));
 49 |     CV_Assert(img.rows > 3 && img.cols > 3);
 50 | 
 51 |     cv::Mat marker = cv::Mat::zeros(img.size(), CV_8UC1);
 52 | 
 53 |     int nRows = img.rows;
 54 |     int nCols = img.cols;
 55 | 
 56 |     if (img.isContinuous()) {
 57 |         nCols *= nRows;
 58 |         nRows = 1;
 59 |     }
 60 | 
 61 |     int x, y;
 62 |     uchar *pAbove;
 63 |     uchar *pCurr;
 64 |     uchar *pBelow;
 65 |     uchar *nw, *no, *ne;    // north (pAbove)
 66 |     uchar *we, *me, *ea;
 67 |     uchar *sw, *so, *se;    // south (pBelow)
 68 | 
 69 |     uchar *pDst;
 70 | 
 71 |     // initialize row pointers
 72 |     pAbove = NULL;
 73 |     pCurr  = img.ptr<uchar>(0);
 74 |     pBelow = img.ptr<uchar>(1);
 75 | 
 76 |     for (y = 1; y < img.rows-1; ++y) {
 77 |         // shift the rows up by one
 78 |         pAbove = pCurr;
 79 |         pCurr  = pBelow;
 80 |         pBelow = img.ptr<uchar>(y+1);
 81 | 
 82 |         pDst = marker.ptr<uchar>(y);
 83 | 
 84 |         // initialize col pointers
 85 |         no = &(pAbove[0]);
 86 |         ne = &(pAbove[1]);
 87 |         me = &(pCurr[0]);
 88 |         ea = &(pCurr[1]);
 89 |         so = &(pBelow[0]);
 90 |         se = &(pBelow[1]);
 91 | 
 92 |         for (x = 1; x < img.cols-1; ++x) {
 93 |             // shift col pointers left by one (scan left to right)
 94 |             nw = no;
 95 |             no = ne;
 96 |             ne = &(pAbove[x+1]);
 97 |             we = me;
 98 |             me = ea;
 99 |             ea = &(pCurr[x+1]);
100 |             sw = so;
101 |             so = se;
102 |             se = &(pBelow[x+1]);
103 | 
104 |             int A  = (*no == 0 && *ne == 1) + (*ne == 0 && *ea == 1) + 
105 |                      (*ea == 0 && *se == 1) + (*se == 0 && *so == 1) + 
106 |                      (*so == 0 && *sw == 1) + (*sw == 0 && *we == 1) +
107 |                      (*we == 0 && *nw == 1) + (*nw == 0 && *no == 1);
108 |             int B  = *no + *ne + *ea + *se + *so + *sw + *we + *nw;
109 |             int m1 = iter == 0 ? (*no * *ea * *so) : (*no * *ea * *we);
110 |             int m2 = iter == 0 ? (*ea * *so * *we) : (*no * *so * *we);
111 | 
112 |             if (A == 1 && (B >= 2 && B <= 6) && m1 == 0 && m2 == 0)
113 |                 pDst[x] = 1;
114 |         }
115 |     }
116 | 
117 |     img &= ~marker;
118 | }
119 | 
120 | /**
121 |  * Function for thinning the given binary image
122 |  *
123 |  * Parameters:
124 |  * 		src  The source image, binary with range = [0,255]
125 |  * 		skeleton The destination image
126 |  */
127 | cv::Mat * DrawAux::thinning(cv::Mat &binarized) {
128 | 	Mat * skeleton = new Mat(cv::Size(binarized.cols, binarized.rows), CV_8UC1, cv::Scalar(0));
129 | 
130 | 	*skeleton = binarized.clone();
131 | 	*skeleton /= 255;         // convert to binary image
132 | 
133 | 	cv::Mat prev = cv::Mat::zeros(skeleton->size(), CV_8UC1);
134 | 	cv::Mat diff;
135 | 
136 | 	do {
137 | 		thinningIteration(*skeleton, 0);
138 | 		thinningIteration(*skeleton, 1);
139 | 		cv::absdiff(*skeleton, prev, diff);
140 | 		skeleton->copyTo(prev);
141 | 	} 
142 | 	while (cv::countNonZero(diff) > 0);
143 | 
144 | 	*skeleton *= 255;
145 | 	
146 | 	return skeleton;
147 | }
148 | 
149 | 
150 | 
151 | /**
152 |  * Compute the Bresenham line and return it ponints.
153 |  *
154 |  * @param p1 first point
155 |  * @param p2 second pint 
156 |  *
157 |  * @return vector of points of the line.
158 |  *
159 |  * @autor http://www.roguebasin.com/index.php?title=Bresenham%27s_Line_Algorithm#C.2B.2B
160 |  **/
161 | std::vector<cv::Point> * DrawAux::lineBresenham(cv::Point p1, cv::Point p2)
162 | {
163 |     std::vector<cv::Point> * points = new std::vector<cv::Point>();
164 |     
165 |     int delta_x(p2.x - p1.x);
166 |     // if p1.x == p2.x, then it does not matter what we set here
167 |     signed char const ix((delta_x > 0) - (delta_x < 0));
168 |     delta_x = std::abs(delta_x) << 1;
169 |  
170 |     int delta_y(p2.y - p1.y);
171 |     // if p1.y == p2.y, then it does not matter what we set here
172 |     signed char const iy((delta_y > 0) - (delta_y < 0));
173 |     delta_y = std::abs(delta_y) << 1;
174 |  
175 |     //plot(p1.x, p1.y);
176 |     points->push_back(p1);
177 |  
178 |     if (delta_x >= delta_y)
179 |     {
180 |         // error may go below zero
181 |         int error(delta_y - (delta_x >> 1));
182 |  
183 |         while (p1.x != p2.x)
184 |         {
185 |             if ((error >= 0) && (error || (ix > 0)))
186 |             {
187 |                 error -= delta_x;
188 |                 p1.y += iy;
189 |             }
190 |             // else do nothing
191 |  
192 |             error += delta_y;
193 |             p1.x += ix;
194 |  
195 |             //plot(p1.x, p1.y);
196 |             points->push_back(p1);
197 |         }
198 |     }
199 |     else
200 |     {
201 |         // error may go below zero
202 |         int error(delta_x - (delta_y >> 1));
203 |  
204 |         while (p1.y != p2.y)
205 |         {
206 |             if ((error >= 0) && (error || (iy > 0)))
207 |             {
208 |                 error -= delta_y;
209 |                 p1.x += ix;
210 |             }
211 |             // else do nothing
212 |  
213 |             error += delta_x;
214 |             p1.y += iy;
215 |  
216 |             //plot(p1.x, p1.y);
217 |             points->push_back(p1);
218 |         }
219 |     }
220 |     
221 |     return points;
222 | }
223 | 
224 | 
225 | 


--------------------------------------------------------------------------------
/src/SampleViewer.cpp:
--------------------------------------------------------------------------------
  1 | #include "SampleViewer.h"
  2 | 
  3 | #include <math.h>
  4 | #include <string.h>
  5 | 
  6 | #include <opencv2/objdetect.hpp>
  7 | #include <opencv2/highgui.hpp>
  8 | #include <opencv2/imgproc.hpp>
  9 | 
 10 | #include <iostream>
 11 | 
 12 | #include "DrawAux.h"
 13 | 
 14 | using namespace cv;
 15 | using namespace std;
 16 | 
 17 | #ifdef DEPTH
 18 | #define SAMPLE_READ_WAIT_TIMEOUT 2000 //2000ms
 19 | using namespace openni;
 20 | #endif
 21 | 
 22 | SampleViewer* SampleViewer::ms_self = NULL;
 23 | 
 24 | SampleViewer::SampleViewer(const char* strSampleName, const char* deviceUri)
 25 | {
 26 | #ifdef DEPTH
 27 | 	initOpenNI(deviceUri);
 28 | 	
 29 | 	skelD = NULL;
 30 | #else
 31 |     if( deviceUri==NULL )
 32 |     {
 33 |         int camera = 0;
 34 |         if(!capture.open(camera))
 35 |             cout << "Capture from camera #" <<  camera << " didn't work" << endl;
 36 |     }
 37 |     else
 38 |     {
 39 |         Mat image = imread( deviceUri, 1 );
 40 |         if( image.empty() )
 41 |         {
 42 |             if(!capture.open( deviceUri ))
 43 |                 cout << "Could not read " << deviceUri << endl;
 44 |         }
 45 |     }
 46 | #endif
 47 | 	ms_self = this;
 48 | 	strncpy(m_strSampleName, strSampleName, strlen(strSampleName));
 49 | 
 50 | 	skel = NULL;
 51 | 	subSample = 2;
 52 | 	frameCount = 0;
 53 | }
 54 | 
 55 | #ifdef DEPTH
 56 | int SampleViewer::initOpenNI(const char* deviceUri) {
 57 | 	Status rc = OpenNI::initialize();
 58 | 	if (rc != STATUS_OK)
 59 | 	{
 60 | 		printf("Initialize failed\n%s\n", OpenNI::getExtendedError());
 61 | 		return 1;
 62 | 	}
 63 | 
 64 | 	rc = device.open(deviceUri);
 65 | 	if (rc != STATUS_OK)
 66 | 	{
 67 | 		printf("Couldn't open device\n%s\n", OpenNI::getExtendedError());
 68 | 		return 2;
 69 | 	}
 70 | 
 71 | 	if (device.getSensorInfo(SENSOR_DEPTH) != NULL)
 72 | 	{
 73 | 		rc = depth.create(device, SENSOR_DEPTH);
 74 | 		if (rc != STATUS_OK)
 75 | 		{
 76 | 			printf("Couldn't create depth stream\n%s\n", OpenNI::getExtendedError());
 77 | 			return 3;
 78 | 		}
 79 | 	}
 80 | 
 81 | 	// Get the device vender and name, if Asus Xtion or Primesense set it's resolution to 640x480.
 82 | 	openni::Array<openni::DeviceInfo> deviceInfoList;
 83 | 	OpenNI::enumerateDevices(&deviceInfoList);
 84 | 	for (int i = 0; i < deviceInfoList.getSize(); i++)
 85 | 	{
 86 | 		printf(//"%d: Uri: %s\n"
 87 | 		    "Vendor: %s"
 88 | 		    ", Name: %s\n", /*i, deviceInfoList[i].getUri(),*/ deviceInfoList[i].getVendor(), deviceInfoList[i].getName());
 89 | 		    
 90 | 		//if (strcmp(deviceInfoList[i].getVendor(), "PrimeSense")==0) // Asus Xtion and Primesense devices
 91 | 		{
 92 | 			// set resolution
 93 | 			// depth modes
 94 | 			cout << "Depth modes" << endl;
 95 | 			const openni::SensorInfo* sinfo = device.getSensorInfo(openni::SENSOR_DEPTH);
 96 | 			const openni::Array< openni::VideoMode>& modesDepth = sinfo->getSupportedVideoModes();
 97 | 			bool set = false;
 98 | 			for (int i = 0; i < modesDepth.getSize(); i++) {
 99 | 				printf("%i: %ix%i, %i fps, %i format\n", i, modesDepth[i].getResolutionX(), modesDepth[i].getResolutionY(),
100 | 				modesDepth[i].getFps(), modesDepth[i].getPixelFormat()); //PIXEL_FORMAT_DEPTH_1_MM = 100, PIXEL_FORMAT_DEPTH_100_UM
101 | 			        if (!set && modesDepth[i].getResolutionX() == 640 && modesDepth[i].getResolutionY() == 480) {
102 | 					rc = depth.setVideoMode(modesDepth[i]); // 640x480
103 | 					set = true;     	
104 | 					if (openni::STATUS_OK != rc)
105 | 					    cout << "error: depth fromat not supprted..." << endl;
106 | 				}
107 | 			}
108 | 		}
109 | 	}
110 | 
111 | 
112 | 	rc = depth.start();
113 | 	if (rc != STATUS_OK)
114 | 	{
115 | 		printf("Couldn't start the depth stream\n%s\n", OpenNI::getExtendedError());
116 | 		return 4;
117 | 	}
118 | }
119 | 
120 | 
121 | /**
122 |  * Ler o proximo frame e o retorna.
123 |  **/
124 | VideoFrameRef * SampleViewer::getNextFrame() {
125 | 	int changedStreamDummy;
126 | 	VideoStream* pStream = &depth;
127 | 	int rc = OpenNI::waitForAnyStream(&pStream, 1, &changedStreamDummy, SAMPLE_READ_WAIT_TIMEOUT);
128 | 	if (rc != STATUS_OK)
129 | 	{
130 | 		printf("Wait failed! (timeout is %d ms)\n%s\n", SAMPLE_READ_WAIT_TIMEOUT, OpenNI::getExtendedError());
131 | 		return NULL;
132 | 	}
133 | 	
134 | 	openni::VideoFrameRef *frame = new VideoFrameRef();
135 | 	rc = depth.readFrame(frame);
136 | 	if (rc != STATUS_OK)
137 | 	{
138 | 		printf("Read failed!\n%s\n", OpenNI::getExtendedError());
139 | 		return NULL;
140 | 	}
141 | 
142 | 	if (frame->getVideoMode().getPixelFormat() != PIXEL_FORMAT_DEPTH_1_MM && frame->getVideoMode().getPixelFormat() != PIXEL_FORMAT_DEPTH_100_UM)
143 | 	{
144 | 		printf("Unexpected frame format\n");
145 | 		return NULL;
146 | 	}
147 | 
148 | 	return frame;
149 | }
150 | 
151 | 
152 | Point3D* SampleViewer::getClosestPoint(openni::VideoFrameRef *frame, Point3D *furthest) {
153 | 	Point3D *closestPoint = new Point3D();
154 | 	DepthPixel* pDepth = (DepthPixel*)frame->getData();
155 | 	//DepthPixel p;
156 | 	bool found = false;
157 | 	closestPoint->z = 0xffff;
158 | 	furthest->z = 0;
159 | 	int width = frame->getWidth();
160 | 	int height = frame->getHeight();
161 | 	
162 | 	//printf("new way::%d::%d\n", width, height);
163 | 
164 | 	for (int y = 0; y < height; ++y)
165 | 		for (int x = 0; x < width; ++x, ++pDepth)
166 | 		{
167 | 			//p = pDepth[y*width + x]; 
168 | 			if (*pDepth < closestPoint->z && *pDepth != 0)
169 | 			{
170 | 				closestPoint->x = x;
171 | 				closestPoint->y = y;
172 | 				closestPoint->z = *pDepth;
173 | 				//closestPoint->z = p;
174 | 				found = true;
175 | 			}
176 | 			else if (*pDepth > furthest->z && *pDepth != 0)
177 | 			{
178 | 				furthest->x = x;
179 | 				furthest->y = y;
180 | 				furthest->z = *pDepth;
181 | 			}
182 | 		}
183 | 		
184 | 	//printf("closest::%d\n", closestPoint->z);
185 | 
186 | 	if (!found)
187 | 	{
188 | 		return NULL;
189 | 	}
190 | 
191 | 	return closestPoint;
192 | }
193 | #endif
194 | 
195 | SampleViewer::~SampleViewer()
196 | {
197 | 	finalize();
198 | 
199 | 	if (m_pTexMap)
200 | 		delete[] m_pTexMap;
201 | 
202 | 	ms_self = NULL;
203 | 
204 | 	if (skel)
205 | 		delete skel;
206 | #ifdef DEPTH
207 | 	if (skelD)
208 | 		delete skelD;
209 | #endif
210 | 	if (outputVideo) {
211 | 		outputVideo->release();
212 | 		delete outputVideo;
213 | 	}
214 | }
215 | 
216 | void SampleViewer::finalize()
217 | {
218 | #ifdef DEPTH
219 | 	depth.stop();
220 | 	depth.destroy();
221 | 	device.close();
222 | 	OpenNI::shutdown();
223 | #endif
224 | }
225 | 
226 | int SampleViewer::init()
227 | {
228 | 	m_pTexMap = NULL;
229 | 
230 | 	return 0;
231 | }
232 | 
233 | int SampleViewer::run()	//Does not return
234 | {
235 | #ifdef DEPTH
236 | printf("Compiled with Depth\n");
237 | #else
238 | printf("Compiled without Depth\n");
239 | #endif
240 | 
241 | 	while (1) {
242 | 		display();
243 | 		char c = (char)waitKey(10);
244 | 		//char c = (char)waitKey(100);
245 | 		//char c = (char)waitKey(1000);
246 | 	        if( c == 27 || c == 'q' || c == 'Q' )
247 |         	        break;
248 | 	}
249 | 	//int r = system("killall -9 rostopic");
250 | #ifdef DEPTH
251 | 	return openni::STATUS_OK;
252 | #else
253 | 	return 0;
254 | #endif
255 | }
256 | void SampleViewer::display()
257 | {
258 | 	int sizePixel = 3;
259 | 	Point3D * furthest = NULL;
260 | 	Point3D * closest = NULL;
261 | #ifdef DEPTH
262 | 	sizePixel = sizeof(openni::RGB888Pixel);
263 | 	openni::VideoFrameRef * srcFrame = getNextFrame();
264 | 
265 | 	if (srcFrame==NULL)
266 | 		return;
267 | 	furthest = new Point3D();
268 | 	closest = getClosestPoint(srcFrame, furthest);
269 | 
270 | #else
271 | 	Mat srcFrame;
272 | 	srcFrame.data = NULL;
273 | 	Mat framec;
274 | 	framec.data = NULL;
275 | 	if( capture.isOpened() ) {
276 | 		capture >> framec;
277 | 		if(! framec.empty() ) {
278 | 			srcFrame = cv::Mat(framec.size(), CV_8UC1);
279 | 			cvtColor(framec, srcFrame, CV_RGB2GRAY);
280 | 			//srcFrame = framec.clone();
281 | 		}
282 | 		else
283 | 			printf("null frame\n");
284 | 	}
285 | #endif
286 | 
287 | 
288 | 	// So entra nesses if na primeira vez
289 | #ifdef DEPTH
290 | 	if (m_pTexMap == NULL)
291 | 	{
292 | 		// Texture map init
293 | 		width = srcFrame->getWidth();
294 | 		height = srcFrame->getHeight();
295 | 		if (width==320)
296 | 			subSample = 1;
297 | #else
298 | 	if (m_pTexMap == NULL && srcFrame.data!=NULL)
299 | 	{
300 | 		// TODO pegar da webcam opencv
301 | 		width = srcFrame.cols;
302 | 		height = srcFrame.rows;
303 | #endif
304 | printf("w x h = %d x %d\n", width, height);
305 | 		m_pTexMap = new unsigned char[width * height * sizePixel];
306 | 
307 | 		skel = new Skeleton(width, height, subSample);
308 | #ifdef DEPTH
309 | 		skelD = new SkeletonDepth(width, height, subSample);
310 | #endif
311 | 		cvNamedWindow("Skeleton Traker", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO | CV_GUI_EXPANDED);
312 | 		resizeWindow("Skeleton Traker", width, height);
313 | 		
314 | 		outputVideo = new VideoWriter("skeletonVideo.avi",
315 | 		//outputVideo = new VideoWriter("skeletonVideo.mp4",  
316 | 		       CV_FOURCC('D','I','V','X'),
317 | 		       //CV_FOURCC('M', 'P', '4', '2'),
318 | 		       //CV_FOURCC('L', 'A', 'G', 'S'),
319 | 		       //CV_FOURCC('F', 'M', 'P', '4'),
320 | 		       15, // FPS
321 | 		       cv::Size(width, height));
322 | 	}
323 | 
324 | //printf("sizeof(openni::RGB888Pixel)=%ld\n", sizeof(openni::RGB888Pixel) );
325 | 
326 | 	frameCount++;
327 | 
328 | 	// check if we need to draw depth frame to texture
329 | #ifdef DEPTH
330 | 	if (srcFrame->isValid())
331 | 	{
332 | 		Mat binarized(cv::Size(width/subSample, height/subSample), CV_8UC1, cv::Scalar(0));
333 | 		short depthMat[width*height*sizeof(short)];
334 | 		bzero(depthMat, width*height*sizeof(short));
335 | 		memset(m_pTexMap, 0, width*height*sizePixel);
336 | 
337 | 		skelD->prepareAnalisa(closest, furthest);
338 | 		//paint with color and get the binarized image.
339 | 		skelD->paintDepthCopy((openni::RGB888Pixel*)m_pTexMap, srcFrame, binarized, depthMat);
340 | 		
341 | 		skel->setDepthMat(depthMat);
342 | 
343 | 		// Convert openni::VideoFrameRef (srcFrame) to cv::Mat (frame)
344 | 		Mat frame = Mat(Size(width, height), CV_8UC3);
345 | 		memcpy(frame.data, m_pTexMap, width*height*sizePixel);
346 | #else
347 | 	if( srcFrame.data != NULL )
348 | 	{
349 | 		Mat binarized(cv::Size(width/subSample, height/subSample), CV_8UC1, cv::Scalar(0));
350 | 		Mat frame = framec;
351 | 		Mat frameB;
352 | 
353 | 		cv::threshold(srcFrame, frameB, 70, 255, cv::THRESH_BINARY_INV);		
354 | 		cv::resize(frameB, binarized, binarized.size());
355 | 
356 | 		skel->drawOverFrame2(&frameB, frame);
357 | 		// mode webcam RGB, discard the first 10 frames, because they can be too white.
358 | 		if (frameCount>10) {
359 | #endif
360 | 		skel->detectBiggerRegion(binarized);
361 | 
362 | 		Mat * skeleton = DrawAux::thinning(binarized);
363 | 		skel->analyse(skeleton);
364 | 
365 | 		std::vector<Point3D> rightArm = skel->getSkeletonArm(skeleton, true);
366 | 		std::vector<Point3D> leftArm= skel->getSkeletonArm(skeleton, false);
367 | 
368 | 		skel->locateMainBodyPoints(binarized);
369 | 
370 | 		//skel->drawOverFrame(skeleton, frame);
371 | 		//skel->drawOverFrame(rightArm, frame);
372 | 		//skel->drawOverFrame(leftArm, frame);
373 | 
374 | 		skel->drawMarkers(frame);
375 | 		
376 | 		notifyListeners(skel->getSkeletonPoints(), skel->getAfa(), closest, frame);
377 | 
378 | 		if (skeleton)
379 | 			delete skeleton;
380 | #ifndef DEPTH
381 | 		} // if (frameCount>10)
382 | #endif
383 | 		//imshow("Skeleton Traker", *skeleton);
384 | 		imshow("Skeleton Traker", frame );
385 | 		//imshow("Skeleton Traker", binarized2 );
386 | 		//outputVideo->write(frame);
387 | 	}
388 | 
389 | #ifdef DEPTH
390 | 	if (srcFrame)
391 | 		delete srcFrame;
392 | #endif
393 | 	if (closest)
394 | 		delete closest;
395 | 	if (furthest)
396 | 		delete furthest;
397 | }
398 | 
399 | 
400 | void SampleViewer::notifyListeners(SkeletonPoints * sp, int afa, Point3D *closest, Mat &frame) {
401 | 	//printf("CLOSE=%6d :: head.z=%6d  left/rightHand=%6d::%6d::ombros=%6d::%6d\n", closest->z, sp->head.z, sp->leftHand.z, sp->rightHand.z, sp->leftShoulder.z, sp->rightShoulder.z);
402 | 	std::vector<cv::Rect> * recs;
403 | 	int i;
404 | 	Scalar c;
405 | 	c = Scalar(255,0,0);
406 | 	for (std::vector<SkeletonListener*>::iterator it = listeners.begin(); it != listeners.end(); it++) {
407 | 		recs = (*it)->onEvent(sp, afa, closest);
408 | 		if (recs != NULL && recs->size() > 0) {
409 | 			for (int i = 0 ; i< recs->size() ; i++) {
410 | 				if (i==1) c = Scalar(0,0,255);
411 | 				if (i==2) c = Scalar(0,255,0);
412 | 				rectangle(frame, recs->at(i), c, 3, 8, 0 );
413 | 			}
414 | 				
415 | 		}
416 | 	}
417 | }
418 | 
419 | void SampleViewer::registerListener(SkeletonListener * listener) {
420 | 	listeners.push_back(listener);
421 | }
422 | 
423 | 
424 | 
425 | 


--------------------------------------------------------------------------------
/src/Skeleton.cpp:
--------------------------------------------------------------------------------
   1 | #include "Skeleton.h"
   2 | #include <stdio.h>
   3 | #include <opencv2/imgproc.hpp>
   4 | #include <strings.h>
   5 | #include <list>
   6 | #include <stack>
   7 | #include "DrawAux.h"
   8 | 
   9 | //#define DEBUG 
  10 | 
  11 | using namespace cv;
  12 | 
  13 | /**
  14 |  * This class process a frame and find the main skeleton points, that will be stored at a SkeletonPoints.
  15 |  *
  16 |  * @author derzu
  17 |  **/
  18 | 
  19 | 
  20 | /**
  21 |  * The constructor
  22 |  **/
  23 | Skeleton::Skeleton(int width, int height, int subSample) {
  24 | 	this->width = width;
  25 | 	this->height = height;
  26 | 	this->subSample = subSample;
  27 | 
  28 | 	initialize();
  29 | }
  30 | 
  31 | void Skeleton::initialize() {
  32 | 	afa = 70/subSample;
  33 | 	shift = 50;
  34 | 	afa28 = afa*subSample*2.8;
  35 | 
  36 | 	showSkeleton = false;
  37 | 
  38 | 	sp = new SkeletonPoints();
  39 | }
  40 | 
  41 | 
  42 | Skeleton::~Skeleton() {
  43 | 	//if (sp)
  44 | 	//	delete sp;
  45 | }
  46 | 
  47 | void Skeleton::setDepthMat(short depth[]) {
  48 | 	depthMat = depth;
  49 | }
  50 | 
  51 | 
  52 | /**
  53 |  * Locate some interesting points on the skeleton region:
  54 |  * The max right, max left, max bottom, maxtop.
  55 |  *
  56 |  * @param frame frame where the maximuns will be found.
  57 |  **/
  58 | void Skeleton::locateMaximus(cv::Mat *frame) {
  59 | 	int width = frame->cols;
  60 | 	int height = frame->rows;
  61 | 
  62 | 	zeraMaximus();
  63 | 
  64 | 	int centerWs = sp->center.x/subSample;
  65 | 
  66 | //printf("centerWs=%d\n", centerWs);
  67 | 	for (int y = 0; y < height; y++)
  68 | 	{
  69 | 		for (int x = 0; x < width; x++)
  70 | 		{
  71 | 			if (frame->data[y*width+x] == 255)
  72 | 			{
  73 | 				if (x>=right.x) {
  74 | 					right.x = x;
  75 | 					right.y = y;
  76 | 				}
  77 | 				if (x<left.x) {
  78 | 					left.x = x;
  79 | 					left.y = y;
  80 | 				}
  81 | 				if (x>=centerWs-afa && x<=centerWs+afa) {
  82 | 					if (y<topCenter.y) {
  83 | 						topCenter.x = x;
  84 | 						topCenter.y = y;
  85 | 					}
  86 | 					if (y>bottomCenter.y) {
  87 | 						bottomCenter.x = x;
  88 | 						bottomCenter.y = y;
  89 | 					}
  90 | 				}
  91 | 				if (x>centerWs+afa*1.2) {
  92 | 					if (y<=topRight.y) {
  93 | 						topRight.x = x;
  94 | 						topRight.y = y;
  95 | 					}
  96 | 				}
  97 | 				if (x>centerWs+afa*1.3) {
  98 | 					if (y>=bottomRight.y) {
  99 | 						bottomRight.x = x;
 100 | 						bottomRight.y = y;
 101 | 					}
 102 | 				}
 103 | 				if (x<centerWs-afa*1.2) {
 104 | 					if (y<topLeft.y) {
 105 | 						topLeft.x = x;
 106 | 						topLeft.y = y;
 107 | 					}
 108 | 				}
 109 | 				if (x<centerWs-afa*1.3) {
 110 | 					if (y>bottomLeft.y) {
 111 | 						bottomLeft.x = x;
 112 | 						bottomLeft.y = y;
 113 | 					}
 114 | 				}
 115 | 			}
 116 | 			/*else if (frame->data[y*width+x] == 0) {
 117 | 				//printf("frameZ: %d\n", frame->data[y*width+x]);
 118 | 			}
 119 | 			else 
 120 | 				;//printf("frame: %d\n", frame->data[y*width+x]);*/
 121 | 		}
 122 | 	}
 123 | 
 124 | 	setMaximus();
 125 | }
 126 | 
 127 | 
 128 | 
 129 | /**
 130 |  * Localiza os ombros, tecnica bem simples. Pega a posicao X da cabeca adiciona +-afa e baixa o Y ate chegar em algum ponto valido
 131 |  * Deve receber como parametro a imagem binarizada e nao o esqueleto.
 132 |  *
 133 |  * @param frame frame where the Shoulders will be found.
 134 |  **/
 135 | void Skeleton::locateShoulders(cv::Mat &frame) {
 136 | 	int width = frame.cols;
 137 | 	int height = frame.rows;
 138 | 
 139 | 	int centerWs = sp->center.x/subSample;
 140 | 	int aff = afa-2;
 141 | 
 142 | 	char nAchou1 = 1;
 143 | 	char nAchou2 = 1;
 144 | 
 145 | 	// Y varia do maxTop + 30 ate 2*height/3. Supoe que os ombros estao sempre acima de 2/3 da imagem
 146 | 	for (int y = sp->head.y/subSample+30; y < 2*height/3; y++)
 147 | 	//for (int y = 0; y < height; y++)
 148 | 	{
 149 | 		//printf("Achei y=%d \n", y);
 150 | 		//if (centerWs+aff<width && centerWs-aff>0) 
 151 | 		if (nAchou1 && centerWs+aff<width && (frame.data[y*width+centerWs+aff] == 255))
 152 | 		{
 153 | 			nAchou1 = 0;
 154 | 			sp->rightShoulder.x = (centerWs+aff)*subSample;
 155 | 			sp->rightShoulder.y = (y+10)*subSample; // adiciona 10 ao Y para ir para dentro do braco, nao ficar no ponto da borda.
 156 | 			obtainZ(sp->rightShoulder);
 157 | 			sp->computePoint(SkeletonPoints::RIGHT_SHOULDER);
 158 | 
 159 | 			if (!nAchou2) break;
 160 | 		}
 161 | 		if (nAchou2 && centerWs-aff>0 && (frame.data[y*width+centerWs-aff] == 255))
 162 | 		{
 163 | 			nAchou2 = 0;
 164 | 			sp->leftShoulder.x = (centerWs-aff)*subSample;
 165 | 			sp->leftShoulder.y = (y+10)*subSample; // adiciona 10 ao Y para ir para dentro do braco, nao ficar no ponto da borda.
 166 | 			obtainZ(sp->leftShoulder);
 167 | 			sp->computePoint(SkeletonPoints::LEFT_SHOULDER);
 168 | 
 169 | 			if (!nAchou1) break;
 170 | 		}
 171 | 	}
 172 | 
 173 | 	if (nAchou1 && nAchou2)
 174 | 		showSkeleton = false;
 175 | 	else
 176 | 		showSkeleton = true;
 177 | 
 178 | //printf("0::centerWs = %d %d %d (%d)\n", centerWs, rightShoulder.x, leftShoulder.x, rightShoulder.x-leftShoulder.x);
 179 | }
 180 | 
 181 | 
 182 | 
 183 | void Skeleton::zeraMaximus() {
 184 | 	right.x  = right.y = 0;
 185 | 	left.x   = width; left.y = 0;
 186 | 	topCenter.x    = 0; topCenter.y = height;
 187 | 	topRight.x    = 0; topRight.y = height;
 188 | 	topLeft.x    = 0; topLeft.y = height;
 189 | 	bottomCenter.x = bottomCenter.y = 0;
 190 | 	bottomRight.x = bottomRight.y = 0;
 191 | 	bottomLeft.x = bottomLeft.y = 0;
 192 | 	maxRight.x = 0;
 193 | 	maxLeft.x = 0;
 194 | 	maxTopCenter.x = 0;
 195 | 	maxTopRight.x = 0;
 196 | 	maxTopLeft.x = 0;
 197 | 	maxBottomCenter.x = 0;
 198 | 	maxBottomRight.x = 0;
 199 | 	maxBottomLeft.x = 0;
 200 | 	middleArmRight.x = 0;
 201 | 	middleArmLeft.x = 0;
 202 | 	middleArmRight45.x = 0;
 203 | 	middleArmLeft45.x = 0;
 204 | 	maxRightMaxBottom.x = 0;
 205 | 	maxLeftMaxBottom.x = 0;
 206 | 	middleStraightArmRight.x = 0;
 207 | 	middleStraightArmLeft.x = 0;
 208 | }
 209 | 
 210 | /**
 211 |  * After locate de maximus, they are stored at anothers variables.
 212 |  * Also set the middleStraightArm point.
 213 |  *
 214 |  * @see locateMaximus()
 215 |  **/
 216 | void Skeleton::setMaximus() {
 217 | 	if (right.y>0) {
 218 | 		maxRight.x = right.x*subSample;
 219 | 		maxRight.y = right.y*subSample;
 220 | 		obtainZ(maxRight);
 221 | 	}
 222 | 	if (left.y>0) {
 223 | 		maxLeft.x = left.x*subSample;
 224 | 		maxLeft.y = left.y*subSample;
 225 | 		obtainZ(maxLeft);
 226 | 	}
 227 | 	if (topCenter.x>0)
 228 | 	{
 229 | 		maxTopCenter.x = topCenter.x*subSample;
 230 | 		maxTopCenter.y = topCenter.y*subSample;
 231 | 		obtainZ(maxTopCenter);
 232 | 	}
 233 | 	if (topRight.x>0)
 234 | 	{
 235 | 		maxTopRight.x = topRight.x*subSample;
 236 | 		maxTopRight.y = topRight.y*subSample;
 237 | 		obtainZ(maxTopRight);
 238 | 	}
 239 | 	if (topLeft.x>0)
 240 | 	{
 241 | 		maxTopLeft.x = topLeft.x*subSample;
 242 | 		maxTopLeft.y = topLeft.y*subSample;
 243 | 		obtainZ(maxTopLeft);
 244 | 	}
 245 | 	if (bottomCenter.x>0) {
 246 | 		maxBottomCenter.x = bottomCenter.x*subSample;
 247 | 		maxBottomCenter.y = bottomCenter.y*subSample;
 248 | 		obtainZ(maxBottomCenter);
 249 | 	}
 250 | 	if (bottomRight.x>0) {
 251 | 		maxBottomRight.x = bottomRight.x*subSample;
 252 | 		maxBottomRight.y = bottomRight.y*subSample;
 253 | 		obtainZ(maxBottomRight);
 254 | 	}
 255 | 	if (bottomLeft.x>0) {
 256 | 		maxBottomLeft.x = bottomLeft.x*subSample;
 257 | 		maxBottomLeft.y = bottomLeft.y*subSample;
 258 | 		obtainZ(maxBottomLeft);
 259 | 	}
 260 | 	if (maxRight.x>0 && maxBottomRight.x>0) {
 261 | 			// add 5 para ficar mais para dentro do braco
 262 | 		maxRightMaxBottom = Point3D(maxRight.x-10, maxBottomRight.y);
 263 | 		obtainZ(maxRightMaxBottom);
 264 | 	}
 265 | 	if (maxLeft.x>0 && maxBottomLeft.x>0) {
 266 | 		maxLeftMaxBottom = Point3D(maxLeft.x+10, maxBottomLeft.y);
 267 | 		obtainZ(maxLeftMaxBottom);
 268 | 	}
 269 | 	if (sp->rightHand.x>0 && sp->rightShoulder.x>0) {
 270 | 		middleStraightArmRight = Point3D((sp->rightHand.x+sp->rightShoulder.x)/2, (sp->rightHand.y+sp->rightShoulder.y)/2); // braco esticado
 271 | 		obtainZ(middleStraightArmRight);
 272 | 	}
 273 | 	if (sp->leftHand.x>0 && sp->leftShoulder.x>0) {
 274 | 		middleStraightArmLeft = Point3D((sp->leftHand.x+sp->leftShoulder.x)/2, (sp->leftHand.y+sp->leftShoulder.y)/2); // braco esticado
 275 | 		obtainZ(middleStraightArmLeft);
 276 | 	}
 277 | 
 278 | }
 279 | 
 280 | /**
 281 |  * A partir da posicao xy o ponto z eh buscado na matriz de profundiade.
 282 |  *
 283 |  **/
 284 | void Skeleton::obtainZ(Point3D &point) {
 285 | #ifdef DEPTH
 286 | 	point.z = getMeanDepthValue(point);
 287 | #endif
 288 | }
 289 | 
 290 | /**
 291 |  * Aqui ficam as regras de localização des alguns dos principais pontos do corpo:
 292 |  * Cabeca, ombros, cotovelos e maos.
 293 |  * @param frame matriz binarizada e utilizada para detecao dos ombros.
 294 |  **/
 295 | void Skeleton::locateMainBodyPoints(cv::Mat &frame) {
 296 | 	// Cabeca. Sempre e o ponto mais alto.
 297 | 	if (maxTopCenter.x!=0) {
 298 | 		sp->head = maxTopCenter;
 299 | 	}
 300 | 	sp->computePoint(SkeletonPoints::HEAD);
 301 | 
 302 | 	// Ombros
 303 | 	locateShoulders(frame);
 304 | 
 305 | 	//if (!showSkeleton)
 306 | 		//return;
 307 | 
 308 | 	// Right Hand
 309 | 	sp->rightHand.x = 0;
 310 | 	// Analisando apenas o lado direito do corpo. Sera mao se:
 311 | 	// Certeza absoluta. Ponto mais a direita esta muito longe, entao nao tem como ser um cotovelo, sera uma mao.
 312 | 	// OU os y do maxRight e do maxBottom e maxTop sao bem proximos.
 313 | 	if (maxRight.x!=0 &&
 314 | 	(maxRight.x - sp->center.x > afa28 || (abs(maxRight.y - maxBottomRight.y)<30 && abs(maxRight.y - maxTopRight.y)<30))  ) {
 315 | 		// O ponto mais a direita
 316 | 		sp->rightHand = maxRight;
 317 | 		//printf("\n\nrightHand::case Right 1\n");
 318 | 	// O ponto mais alto tiver a direita do ponto mais baixo. Ou:
 319 | 	// O ponto mais baixo, estiver acima da linha da cintura, e a distancia entre o ponto mais alto e o ombro for grande.
 320 | 	} else if (maxTopRight.x!=0 && ((maxTopRight.x > maxBottomRight.x) || (maxBottomRight.y < sp->center.y+shift && maxTopRight.y < sp->rightShoulder.y+shift && DrawAux::euclideanDist(maxTopRight, sp->rightShoulder)>50))) {
 321 | 		// O ponto mais alto
 322 | 		sp->rightHand = maxTopRight;
 323 | 		//printf("\n\nrightHand::case Top\n");
 324 | 	} else if (maxBottomRight.x!=0) {
 325 | 		// O ponto mais baixo
 326 | 		sp->rightHand = maxBottomRight;
 327 | 		//printf("\n\nrightHand::case Bottom\n");
 328 | 	}
 329 | 	//printf("rightHand::%d %d\n",sp->rightHand.x, sp->rightHand.y);
 330 | 	sp->computePoint(SkeletonPoints::RIGHT_HAND);
 331 | 	//printf("rightHand::%d %d\n",sp->rightHand.x, sp->rightHand.y);
 332 | 
 333 | 
 334 | 	// Left Hand
 335 | 	sp->leftHand.x = 0;
 336 | 	// Analisando apenas o lado esquerdo do corpo. Sera mao se:
 337 | 	
 338 | 	// Certeza absoluta. Ponto mais a esquerda esta muito longe, entao nao tem como ser um cotovelo, sera uma mao.
 339 | 	// OU os y do maxLeft e do maxBottom e maxTop sao bem proximos.
 340 | 	if (maxLeft.x!=0 &&
 341 | 	(sp->center.x-maxLeft.x > afa28 || (abs(maxLeft.y - maxBottomLeft.y)<30 && abs(maxLeft.y - maxTopLeft.y)<30))  ) {
 342 | 		// O ponto mais a esquerda
 343 | 		sp->leftHand = maxLeft;
 344 | 	// Se o ponto mais alto tiver a esquerda do ponto mais baixo. Ou:
 345 | 	// Se o ponto mais baixo, estiver acima da linha da cintura, e a distancia entre o ponto mais alto e o ombro for grande.
 346 | 	} else if (maxTopLeft.x!=0 && ((maxTopLeft.x < maxBottomLeft.x) || (maxBottomLeft.y < sp->center.y+shift && maxTopLeft.y < sp->leftShoulder.y+shift && DrawAux::euclideanDist(maxTopLeft, sp->leftShoulder)>50) )) {
 347 | 		// O ponto mais alto
 348 | 		sp->leftHand = maxTopLeft;
 349 | 	} else if (maxBottomLeft.x!=0) {
 350 | 		// O ponto mais baixo
 351 | 		sp->leftHand = maxBottomLeft;
 352 | 	}
 353 | 	sp->computePoint(SkeletonPoints::LEFT_HAND);
 354 | 
 355 | 
 356 | 	// Right Elbow/Cotovelo
 357 | 	sp->rightElbow.x=0;
 358 | 	int points;
 359 | 	int menor;
 360 | 	Point3D p;
 361 | 	// braco esticado
 362 | 	// Se o ponto middleStraightArm estiver dentro da area do corpo E longe da mao. Analisa variacoes do ponto com 10 pixels em 4 direcoes.
 363 | 	if (middleStraightArmRight.x!=0 &&
 364 | 	qPointsLineOutside(frame, middleStraightArmRight, sp->rightShoulder) < 10 &&
 365 | 	(frame.data[(middleStraightArmRight.y/subSample)*frame.cols + middleStraightArmRight.x/subSample]==255 ||
 366 | 	 //frame.data[(middleStraightArmRight.y/subSample)*frame.cols + (middleStraightArmRight.x+10)/subSample]==255 ||
 367 | 	 //frame.data[(middleStraightArmRight.y/subSample)*frame.cols + (middleStraightArmRight.x-10)/subSample]==255 ||
 368 | 	 frame.data[((middleStraightArmRight.y+10)/subSample)*frame.cols + middleStraightArmRight.x/subSample]==255 ||
 369 | 	 frame.data[((middleStraightArmRight.y-10)/subSample)*frame.cols + middleStraightArmRight.x/subSample]==255 ) ) {
 370 | 		sp->rightElbow = middleStraightArmRight;
 371 | 		//printf("\n\nelbow::case middleStraightArmRight \n" );
 372 | 		//imwrite( "middleStraightArmRight.png", frame );
 373 | 		if (frame.data[(middleStraightArmRight.y/subSample)*frame.cols + middleStraightArmRight.x/subSample]==255)
 374 | 			;
 375 | 		else if (frame.data[((middleStraightArmRight.y+10)/subSample)*frame.cols + middleStraightArmRight.x/subSample]==255)
 376 | 			sp->rightElbow.y = middleStraightArmRight.y+10;
 377 | 		else if (frame.data[((middleStraightArmRight.y-10)/subSample)*frame.cols + middleStraightArmRight.x/subSample]==255)
 378 | 			sp->rightElbow.y = middleStraightArmRight.y-10;
 379 | 	}
 380 | 	else {
 381 | 		menor=99999;
 382 | 		for (std::vector<Point3D>::iterator it = rightArm.begin(); it != rightArm.end(); ++it) {
 383 | 			p = *it;
 384 | 			p *= subSample;
 385 | 			points = qPointsLineOutside(frame, p, sp->rightHand) + qPointsLineOutside(frame, p, sp->rightShoulder);
 386 | 			if (points<menor) {
 387 | 				menor = points;
 388 | 				sp->rightElbow = p;
 389 | 			}		
 390 | 		}
 391 | 		//printf("menor=%d\n", menor);
 392 | 	}
 393 | 	obtainZ(sp->rightElbow);
 394 | 	sp->computePoint(SkeletonPoints::RIGHT_ELBOW);
 395 | 	
 396 | 
 397 | 
 398 | 
 399 | 	// Left Elbow/Cotovelo
 400 | 	sp->leftElbow.x=0;
 401 | 	// Analisando apenas o lado esquerdo do corpo. Sera cotovelo se:
 402 | 
 403 | 
 404 | 
 405 | 
 406 | 
 407 | 	// braco esticado
 408 | 	// Se o ponto middleStraightArm estiver dentro da area do corpo E longe da mao.
 409 | 	if (middleStraightArmLeft.x!=0 &&
 410 | 	qPointsLineOutside(frame, middleStraightArmLeft, sp->leftShoulder) < 10 &&
 411 | 	(frame.data[(middleStraightArmLeft.y/subSample)*frame.cols + middleStraightArmLeft.x/subSample]==255 ||
 412 | 	 //frame.data[(middleStraightArmLeft.y/subSample)*frame.cols + (middleStraightArmLeft.x+10)/subSample]==255 ||
 413 | 	 //frame.data[(middleStraightArmLeft.y/subSample)*frame.cols + (middleStraightArmLeft.x-10)/subSample]==255 ||
 414 | 	 frame.data[((middleStraightArmLeft.y+10)/subSample)*frame.cols + middleStraightArmLeft.x/subSample]==255 ||
 415 | 	 frame.data[((middleStraightArmLeft.y-10)/subSample)*frame.cols + middleStraightArmLeft.x/subSample]==255 ) ) {
 416 | 		sp->leftElbow = middleStraightArmLeft;
 417 | 		//printf("\n\nelbow::case middleStraightArmLeft\n");
 418 | 		if (frame.data[(middleStraightArmLeft.y/subSample)*frame.cols + middleStraightArmLeft.x/subSample]==255)
 419 | 			;
 420 | 		else if (frame.data[((middleStraightArmLeft.y+10)/subSample)*frame.cols + middleStraightArmLeft.x/subSample]==255)
 421 | 			sp->leftElbow.y = middleStraightArmLeft.y+10;
 422 | 		else if (frame.data[((middleStraightArmLeft.y-10)/subSample)*frame.cols + middleStraightArmLeft.x/subSample]==255)
 423 | 			sp->leftElbow.y = middleStraightArmLeft.y-10;
 424 | 	}
 425 | 	else {
 426 | 		menor=99999;
 427 | 		for (std::vector<Point3D>::iterator it = leftArm.begin(); it != leftArm.end(); ++it) {
 428 | 			p = *it;
 429 | 			p *= subSample;
 430 | 			points = qPointsLineOutside(frame, p, sp->leftHand) + qPointsLineOutside(frame, p, sp->leftShoulder);
 431 | 			if (points<menor) {
 432 | 				menor = points;
 433 | 				sp->leftElbow = p;
 434 | 			}		
 435 | 		}
 436 | 		//printf("menor=%d\n", menor);
 437 | 	}
 438 | 	obtainZ(sp->leftElbow);
 439 | 	sp->computePoint(SkeletonPoints::LEFT_ELBOW);
 440 | 
 441 | 
 442 | 
 443 | }
 444 | 
 445 | 
 446 | /**
 447 |  * Verify if the line between the 2 points has any point outside de body region.
 448 |  *
 449 |  * @param frame binarized image where the line will be checked.
 450 |  * @param p1 first point of the line.
 451 |  * @param p1 second point of the line.
 452 |  *
 453 |  * @return true if all the points of the line are inside the body region. false otherwise.
 454 |  **/
 455 | bool Skeleton::isLineInside(Mat &frame, cv::Point p1, cv::Point p2) {
 456 | 	p1.x /= subSample;
 457 | 	p1.y /= subSample;
 458 | 	p2.x /= subSample;
 459 | 	p2.y /= subSample;
 460 | 	std::vector<cv::Point> * lines = DrawAux::lineBresenham(p1, p2);
 461 | 	//printf("isLineInside::count=%ld::%d %d::%d %d\n", lines->size(), p1.x, p1.y, p2.x, p2.y);
 462 | 	int q = 0;
 463 | 	for(int i = 0; i < lines->size(); i++)
 464 | 	{
 465 | 	    Point pl= lines->at(i);
 466 | 	    //printf("p x y = %d %d %d\n", pl.x, pl.y, frame.data[(pl.y/subSample)*frame.cols + pl.x/subSample]);
 467 | 	    if (frame.data[pl.y*frame.cols + pl.x]!=255) {
 468 | 	    	//printf("ponto outside %d!!\n", pl.x);
 469 | 	    	q++;
 470 | 	    	//break;
 471 | 	    }
 472 | 	}
 473 | 	
 474 | 	//printf("quantidade outside=%d\n", q);
 475 | 	if (q>50)
 476 | 		return false;
 477 | 	
 478 | 	if (lines)
 479 | 		delete lines;
 480 | 		
 481 | 	return true;
 482 | }
 483 | 
 484 | int Skeleton::qPointsLineOutside(Mat &frame, cv::Point p1, cv::Point p2) {
 485 | 	p1.x /= subSample;
 486 | 	p1.y /= subSample;
 487 | 	p2.x /= subSample;
 488 | 	p2.y /= subSample;
 489 | 	std::vector<cv::Point> * lines = DrawAux::lineBresenham(p1, p2);
 490 | 	//printf("isLineInside::count=%ld::%d %d::%d %d\n", lines->size(), p1.x, p1.y, p2.x, p2.y);
 491 | 	int q = 0;
 492 | 	for(int i = 0; i < lines->size(); i++)
 493 | 	{
 494 | 	    Point pl= lines->at(i);
 495 | 	    //printf("p x y = %d %d %d\n", pl.x, pl.y, frame.data[(pl.y/subSample)*frame.cols + pl.x/subSample]);
 496 | 	    if (frame.data[pl.y*frame.cols + pl.x]!=255) {
 497 | 	    	//printf("ponto outside %d!!\n", pl.x);
 498 | 	    	q++;
 499 | 	    }
 500 | 	}
 501 | 	
 502 | 	//printf("quantidade outside=%d\n", q);
 503 | 	return q;
 504 | }
 505 | 
 506 | 
 507 | /**
 508 |  * Draw the markers of the body skeleton.
 509 |  *
 510 |  * @param frame the frame where the markers will be drawn.
 511 |  *
 512 |  * @author derzu
 513 |  **/
 514 | void Skeleton::drawMarkers(Mat &frame) {
 515 | 	static int draws = 0;
 516 | 
 517 | 	if (!showSkeleton)
 518 | 		return;
 519 | 
 520 | 	draws++;
 521 | 
 522 | #ifdef DEBUG
 523 | 	if (maxRight.x!=0)
 524 | 		circle( frame, maxRight,        7, Scalar(255,255,0), 2, 8, 0 );
 525 | 	if (maxLeft.x != 0)
 526 | 		circle( frame, maxLeft,         7, Scalar(255,255,0), 2, 8, 0 );
 527 | 	if (maxTopCenter.x != 0)
 528 | 		circle( frame, maxTopCenter,    7, Scalar(0,255,255), 2, 8, 0 );
 529 | 	if (maxTopRight.x != 0)
 530 | 		circle( frame, maxTopRight,     7, Scalar(0,255,255), 2, 8, 0 );	
 531 | 	if (maxTopLeft.x != 0)
 532 | 		circle( frame, maxTopLeft,      7, Scalar(0,255,255), 2, 8, 0 );
 533 | 	if (maxBottomCenter.x != 0)
 534 | 		circle( frame, maxBottomCenter, 7, Scalar(255,0,0),   2, 8, 0 );
 535 | 	if (maxBottomRight.x != 0)
 536 | 		circle( frame, maxBottomRight,  7, Scalar(255,0,0),   2, 8, 0 );
 537 | 	if (maxBottomLeft.x != 0)
 538 | 		circle( frame, maxBottomLeft,   7, Scalar(255,0,0),   2, 8, 0 );
 539 | 	if (middleArmRight.x != 0)
 540 | 		circle( frame, middleArmRight,  11, Scalar(255,255,255), 2, 8, 0 );
 541 | 	if (middleArmLeft.x != 0)
 542 | 		circle( frame, middleArmLeft,   11, Scalar(255,255,255), 2, 8, 0 );
 543 | 	if (middleArmRight45.x != 0)
 544 | 		circle( frame, middleArmRight45,  15, Scalar(0,255,255), 2, 8, 0 );
 545 | 	if (middleArmLeft45.x != 0)
 546 | 		circle( frame, middleArmLeft45,   15, Scalar(0,255,255), 2, 8, 0 );
 547 | 	if (maxRightMaxBottom.x != 0)
 548 | 		circle( frame, maxRightMaxBottom,  7, Scalar(0,0,255), 2, 8, 0 );
 549 | 	if (maxLeftMaxBottom.x != 0)
 550 | 		circle( frame, maxLeftMaxBottom,   7, Scalar(0,0,255), 2, 8, 0 );
 551 | 	if (middleStraightArmRight.x != 0)
 552 | 		circle( frame, middleStraightArmRight,  7, Scalar(0,0,255), 2, 8, 0 );
 553 | 	if (middleStraightArmLeft.x != 0)
 554 | 		circle( frame, middleStraightArmLeft,   7, Scalar(0,0,255), 2, 8, 0 );
 555 | 
 556 | #endif
 557 | 
 558 | 
 559 | //return;
 560 | 
 561 | 	// Desenha os quadrantes (linhas)
 562 | 	Point ini = Point(sp->center.x, 1);	
 563 | 	Point fim = Point(sp->center.x, height-1);
 564 | 	Scalar c = Scalar(0,255,255);
 565 | //	line(frame, ini, fim, c, 1, 8, 0 ); // linha vertical central
 566 | 	ini.x -= afa*subSample; fim.x -= afa*subSample;
 567 | //	line(frame, ini, fim, c, 1, 8, 0 ); // linha vertical esquerda
 568 | 	ini.x += afa*subSample*2; fim.x += afa*subSample*2;
 569 | //	line(frame, ini, fim, c, 1, 8, 0 ); // linha vertical direita
 570 | 
 571 | 	ini = Point(1      , sp->center.y);
 572 | 	fim = Point(width-1, sp->center.y);
 573 | //	line(frame, ini, fim, c, 1, 8, 0 ); // linha horizontal central
 574 | 
 575 | 	// Cabeca/Head
 576 | 	circle( frame, sp->head,           15, Scalar(255,255,0), 2, 8, 0 );
 577 | 
 578 | 	// Center
 579 | 	//circle( frame, sp->center,         20, Scalar(255,255,0), 2, 8, 0 );
 580 | 
 581 | 	// Maos/Hands
 582 | 	if (sp->leftHand.x != 0)
 583 | 		circle( frame, sp->leftHand, 15, Scalar(0,255,0), 2, 8, 0 );
 584 | 	if (sp->rightHand.x != 0)
 585 | 		circle( frame, sp->rightHand, 15, Scalar(0,255,0), 2, 8, 0 );
 586 | 
 587 | 	// Ombros/Shoulders
 588 | 	circle( frame, sp->rightShoulder,  15, Scalar(255,255,0), 2, 8, 0 );
 589 | 	circle( frame, sp->leftShoulder,   15, Scalar(255,255,0), 2, 8, 0 );
 590 | 
 591 | 	c = Scalar(255,255,255);
 592 | 	line(frame, sp->leftShoulder, sp->rightShoulder, c, 2, 8, 0 ); // linha entre os ombros
 593 | 	fim = Point((sp->rightShoulder.x+sp->leftShoulder.x)/2, abs(sp->rightShoulder.y+sp->leftShoulder.y)/2);
 594 | 	line(frame, sp->head, fim, c, 2, 8, 0 ); // linha da cabeca e o centro dos ombros
 595 | 
 596 | 	// linha/line base
 597 | 	ini = fim;
 598 | 	fim = Point(ini.x, maxBottomCenter.y);
 599 | 	line(frame, ini, fim, c, 2, 8, 0 ); // linha central do corpo do esqueleto. Entre o meio dos ombros e a parte de baixo.
 600 | 
 601 | 
 602 | 
 603 | 	// Cotovelos/Elbowsfor (std::vector<Point3D>::iterator it = pontos.begin(); it != pontos.end(); ++it) {
 604 | 	if (sp->rightElbow.x != 0) {
 605 | 		circle( frame, sp->rightElbow, 15, Scalar(255,0,0), 2, 8, 0 );
 606 | 		if (sp->rightShoulder.x!=0 && sp->rightElbow.x!=0)
 607 | 			line(frame, sp->rightShoulder, sp->rightElbow, c, 2, 8, 0 );
 608 | 		if (sp->rightHand.x!=0)
 609 | 			line(frame, sp->rightElbow, sp->rightHand, c, 2, 8, 0 );
 610 | 	} else if (sp->rightShoulder.x!=0 && sp->rightHand.x!=0) {
 611 | 		line(frame, sp->rightShoulder, sp->rightHand, c, 2, 8, 0 );
 612 | 	}
 613 | 	if (sp->leftElbow.x != 0) {
 614 | 		circle( frame, sp->leftElbow, 15, Scalar(255,0,0), 2, 8, 0 );
 615 | 		if (sp->leftShoulder.x!=0 && sp->leftElbow.x!=0)
 616 | 			line(frame, sp->leftShoulder, sp->leftElbow, c, 2, 8, 0 );
 617 | 		if (sp->leftHand.x!=0)
 618 | 			line(frame, sp->leftElbow, sp->leftHand, c, 2, 8, 0 );
 619 | 	} else if (sp->leftShoulder.x!=0 && sp->leftHand.x!=0) {
 620 | 		line(frame, sp->leftShoulder, sp->leftHand, c, 2, 8, 0 );
 621 | 	}
 622 | 
 623 | }
 624 | 
 625 | 
 626 | 
 627 | /**
 628 |  * Desenha os pontos==255 de uma matriz (skelImg) sobre outra matriz (frame).
 629 |  *
 630 |  * @param skelImg Matriz cujos de pontos serao desenhados em frame
 631 |  * @param frame Matriz onde os pontos serao desenhados.
 632 |  **/
 633 | void Skeleton::drawOverFrame(Mat * skelImg, Mat &frame) {
 634 | 	Scalar cor = Scalar(0,255,0);
 635 | 	int w = skelImg->cols;
 636 | 	int h = skelImg->rows;
 637 | 	int x,y;
 638 | 
 639 | 	for (y=0 ; y<h ; y++) {
 640 | 		for (x=0 ; x<w ; x++) {
 641 | 			if (skelImg->data[y*w+x]==255) {
 642 | 				//circle(Mat& img, Point center, int radius, const Scalar& color, int thickness=1, int lineType=8, int shift=0)
 643 | 				circle(frame, Point(x*subSample, y*subSample), 1, cor, 2, 8, 0);
 644 | 				//circle(*frame, Point(x, y), 1, cor, 1, 8, 0);
 645 | 				//frame->data[y*subSample*w*subSample*3 +x*subSample*3    ] = 0;
 646 | 				//frame->data[y*subSample*w*subSample*3 +x*subSample*3 + 1] = 255;
 647 | 				//frame->data[y*subSample*w*subSample*3 +x*subSample*3 + 2] = 0;
 648 | 				//frame->data[y*subSample*w*3 +x*3    ] = 0;
 649 | 				//frame->data[y*subSample*w*3 +x*3 + 1] = 255;
 650 | 				//frame->data[y*subSample*w*3 +x*3 + 2] = 0;
 651 | 			}
 652 | 		}
 653 | 	}
 654 | }
 655 | 
 656 | 
 657 | 
 658 | /**
 659 |  * Desenha os pontos==255 de uma matriz binaria (bin) sobre outra matriz (frame) em um tom de cinza.
 660 |  *
 661 |  * @param bin Matriz cujos de pontos serao desenhados em frame
 662 |  * @param frame Matriz onde os pontos serao desenhados.
 663 |  **/
 664 | void Skeleton::drawOverFrame2(Mat * bin, Mat &frame) {
 665 | 	int w = bin->cols;
 666 | 	int h = bin->rows;
 667 | 	int x,y;
 668 | 
 669 | 	for (y=0 ; y<h ; y++) {
 670 | 		for (x=0 ; x<w ; x++) {
 671 | 			if (bin->data[y*w+x]==255) {
 672 | 				frame.data[y*w*3 + x*3    ] = 200;
 673 | 				frame.data[y*w*3 + x*3 + 1] = 200;
 674 | 				frame.data[y*w*3 + x*3 + 2] = 200;
 675 | 			}
 676 | 		}
 677 | 	}
 678 | }
 679 | 
 680 | 
 681 | /**
 682 |  * Desenha o vetor de pontos sobre a matriz (frame).
 683 |  *
 684 |  * @param pontos vetor com os pontos que serao desenhados em frame
 685 |  * @param frame Matriz onde os pontos serao desenhados.
 686 |  **/
 687 | void Skeleton::drawOverFrame(std::vector<Point3D> pontos, Mat &frame) {
 688 | 	Scalar cor = Scalar(0,255,255);
 689 | 	Point3D p;
 690 | 
 691 | 	for (std::vector<Point3D>::iterator it = pontos.begin(); it != pontos.end(); ++it) {
 692 | 		p = *it;
 693 | 		circle(frame, Point(p.x*subSample, p.y*subSample), 1, cor, 2, 8, 0);
 694 | 	}
 695 | }
 696 | 
 697 | /**
 698 |  * Detecta a maior regiao continua da imagem por "region growing". As demais regioes sao apagadas.
 699 |  *
 700 |  * @param frame Matriz onde a regiao sera localizada.
 701 |  **/
 702 | void Skeleton::detectBiggerRegion(Mat &frame) {
 703 | 	int x,y;
 704 | 	int xM, yM, maior=0;
 705 | 	int size;
 706 | 	wC = frame.cols;
 707 | 	hC = frame.rows;
 708 | 	unsigned char datacp[wC*hC];
 709 | 
 710 | 	maior = 0;
 711 | 	memcpy(datacp, frame.data, wC*hC);
 712 | 
 713 | 	for (y=0 ; y<hC ; y++) {
 714 | 		for (x=0 ; x<wC ; x++) {
 715 | 			if (datacp[y*wC+x]==255) {
 716 | 				size = 0;
 717 | 				//getSizeRegionRecursive(datacp, x, y, &size);
 718 | 				size = getSizeRegion(datacp, x, y);
 719 | 				if (maior < size) {
 720 | 					//printf("maior = %d, size = %d\n", maior, size);
 721 | 					// apaga a regiao que nao eh a maior da imagem original.
 722 | 					if (maior>0)
 723 | 						clearRegion(frame.data, xM, yM);
 724 | 
 725 | 					//printf("size1 = %d, size2 = %d\n", size, size2)	;
 726 | 					maior = size;
 727 | 					xM = x;
 728 | 					yM = y;
 729 | 				}
 730 | 				else
 731 | 					// apaga a regiao que nao eh a maior da imagem original.
 732 | 					clearRegion(frame.data, x, y);
 733 | 
 734 | 				// exclui a regiao para que ela nao seja mais buscada.
 735 | 				clearRegion(datacp, x, y);
 736 | 			}
 737 | 		}
 738 | 	}
 739 | 
 740 | 	// localiza os pontos medios
 741 | 	sp->center = mediaPoint(&frame);
 742 | 	sp->center.x *= subSample;
 743 | 	sp->center.y *= subSample;
 744 | 	obtainZ(sp->center);
 745 | 	//sp->center.y = sp->head.y + ((sp->rightShoulder.y+sp->leftShoulder.y)/2 - sp->head.y)*2;
 746 | 	//centerHV[centerHHead++ % BUF_SIZE] = sp->center.y;
 747 | 	//sp->center.y = calculaMedia(centerHV);
 748 | 	sp->computePoint(SkeletonPoints::CENTER);
 749 | }
 750 | 
 751 | 
 752 | 
 753 | /**
 754 |  * Limpa a imagem removendo regioes pequenas.
 755 |  * 
 756 |  * @param frame imagem a ser limpa.
 757 |  **/
 758 | void Skeleton::removeSmallsRegions(Mat * frame) {
 759 | 	int x,y;
 760 | 	int size;
 761 | 	wC = frame->cols;
 762 | 	hC = frame->rows;
 763 | 	unsigned char datacp[wC*hC];
 764 | 
 765 | 	memcpy(datacp, frame->data, wC*hC);
 766 | 
 767 | 	for (y=0 ; y<hC ; y++) {
 768 | 		for (x=0 ; x<wC ; x++) {
 769 | 			if (datacp[y*wC+x]==255) {
 770 | 				size = 0;
 771 | 				//getSizeRegionRecursive(datacp, x, y, &size);
 772 | 				size = getSizeRegion(datacp, x, y);
 773 | 				if (size<6) {
 774 | 					clearRegion(frame->data, x, y);
 775 | 				}
 776 | 			}
 777 | 		}
 778 | 	}
 779 | }
 780 | 
 781 | void Skeleton::analyse(cv::Mat * skeleton) {
 782 | 	removeSmallsRegions(skeleton);
 783 | 	locateMaximus(skeleton);
 784 | }
 785 | 
 786 | 
 787 | /**
 788 |  * Calcula a media entre os pixels==255 da matriz
 789 |  * 
 790 |  * @return O ponto que tem os valores da medias x e y.
 791 |  **/
 792 | Point3D Skeleton::mediaPoint(Mat * frame) {
 793 | 	int w = frame->cols;
 794 | 	int h = frame->rows;
 795 | 	int x, y;
 796 | 	Point3D media = Point3D(0, 0);
 797 | 	int c=1;
 798 | 
 799 | 	for (y=0 ; y<h ; y++) {
 800 | 		for (x=0 ; x<w ; x++) {
 801 | 			if (frame->data[y*w+x]==255) {
 802 | 				media.x += x;
 803 | 				media.y += y;
 804 | 				c++;
 805 | 			}
 806 | 		}
 807 | 	}
 808 | 	if (c>1) c--;
 809 | 	media.x /= c;
 810 | 	media.y /= c;
 811 | 
 812 | 	return media;
 813 | }
 814 | 
 815 | 
 816 | 
 817 | 
 818 | /**
 819 |  * Localiza um possivel ponto do cotovelo a partir dos pontos do braco
 820 |  * @param arm braco do esquelto
 821 |  * @return o ponto na curva do esquelto do braco
 822 |  **/
 823 | Point3D * Skeleton::getElbowHard(std::vector<Point3D> &arm, int ang) {
 824 | 	Point3D * p = NULL;
 825 | 
 826 | 	// calcula as declividades
 827 | 	double ang1, ang2=0, ang3=0, diff;
 828 | 	for (int i=0; i<(int)arm.size()-1 ; i++) {
 829 | 		//printf("%dx%d - %dx%d\n", arm[i].x, arm[i].y, arm[i+1].x, arm[i+1].y);
 830 | 		if (DrawAux::euclideanDist(arm[i], arm[i+1])<5) {
 831 | 			ang1 = atan2((arm[i].y-arm[i+1].y), (arm[i].x-arm[i+1].x) ) * 180./CV_PI;
 832 | 			//ang1 = atan2(abs(arm[i].y-arm[i+1].y), abs(arm[i].x-arm[i+1].x) ) * 180.;
 833 | 			if (ang==-1) {
 834 | 				if (ang1>-110 && ang1<-70)
 835 | 					ang1 = -90;
 836 | 				/*if (ang1>70 && ang1<110)
 837 | 					ang1 = 90;*/
 838 | 				if (ang1>-20 && ang1<20)
 839 | 					ang1 = 0;
 840 | 				if (ang1>160 || ang1<-160)
 841 | 					ang1 = 180;
 842 | 			}
 843 | 			else {
 844 | 				if (ang1>20 && ang1<70)
 845 | 					ang1 = 45;
 846 | 				if (ang1>-70 && ang1<-20)
 847 | 					ang1 = -45;
 848 | 				if (ang1>-160 && ang1<-110)
 849 | 					ang1 = -135;
 850 | 				if (ang1>110 && ang1<160)
 851 | 					ang1 = 135;
 852 | 			}
 853 | 			diff = abs(ang1-ang2);
 854 | 			
 855 | 			// localiza uma sequencia de 3 ponstos iguais, esses pontos provavelmente estarao apos a curva do cotovelo.
 856 | 
 857 | //if (ang!=-1)			printf("ang: %6.2lf(%6.2lf)\n", ang1, diff);
 858 | 			
 859 | 			// localiza o ponto no meio da curva
 860 | 			if (ang1==ang2 && ang2==ang3 && i>5 &&
 861 | 			(((ang1==0  || ang1==-90 || /*ang1==90 ||*/ ang1==180 || ang1==47) && ang==-1) || 
 862 | 			 ((ang1==45 || ang1==-45 || ang1==-135 || ang1==135) && ang!=-1) ) ) {
 863 | 				p = new Point3D(arm[i].x*subSample, arm[i].y*subSample);
 864 | 				break;
 865 | 			}
 866 | 
 867 | 			// guarda os 3 ultimos angulos
 868 | 			ang3 = ang2;
 869 | 			ang2 = ang1;
 870 | 		}
 871 | 	
 872 | 	}
 873 | //if (ang!=-1)	printf("\n\n");
 874 | 
 875 | 	return p;
 876 | }
 877 | 
 878 | 
 879 | /**
 880 |  * Localiza os pontos do esqueleto do braco direito ou esquerdo.
 881 |  *
 882 |  * @param skeleton matriz de ponstos com o esqueleto
 883 |  * @param right se verdadeiro busca pelo braco direito, falso pelo esquerdo.
 884 |  *
 885 |  * @return vetor de Point3D com os pontos do braco
 886 |  **/
 887 | std::vector<Point3D> Skeleton::getSkeletonArm(Mat * skeleton, bool right) {
 888 | 	int w = skeleton->cols;
 889 | 	int h = skeleton->rows;
 890 | 	int x, y;
 891 | 	//std::list<Point3D> pontos;
 892 | 	std::vector<Point3D> pontos;
 893 | 	std::vector<Point3D> pontos_ordered;
 894 | 	std::vector<Point3D> pontos_ordered_smoth;
 895 | 	Point3D p;
 896 | 
 897 | 	int centerWs = sp->center.x/subSample;
 898 | 	int ini = centerWs-afa*1.2, fim = 0;
 899 | 	if (ini<0) ini = 0;
 900 | 	if (right) {
 901 | 		ini = centerWs+afa*1.2;
 902 | 		if (ini>=w) ini = w-1;
 903 | 		fim = w;
 904 | 	}
 905 | 
 906 | 	// localiza todos os pontos do esqueleto
 907 | 	for (x=ini ; x!=fim ; ) {
 908 | 		for (y=0 ; y<h ; y++) {
 909 | 			if (skeleton->data[y*w+x]==255) {
 910 | 				p = Point3D(x, y);
 911 | 				pontos.push_back(p);
 912 | 			}
 913 | 		}
 914 | 		right ? x++ : x--;
 915 | 	}
 916 | 	
 917 | 	if (right)
 918 | 		rightArm = pontos;
 919 | 	else
 920 | 		leftArm = pontos;
 921 | 		
 922 | 	return pontos; // TODO CONFIRM
 923 | 
 924 | 
 925 | 	Point3D first;
 926 | 	float dist;
 927 | 	float menorDist;
 928 | 	std::vector<Point3D>::iterator closest;
 929 | 	Point3D closestP;
 930 | 	int tam = 0;
 931 | 	
 932 | 	// Ordena os pontos
 933 | 	// acha o mais proximo
 934 | 	if (!pontos.empty()) {
 935 | 		pontos_ordered.push_back(pontos.front());
 936 | 		pontos.erase(pontos.begin());
 937 | 		while (tam!=pontos.size()) {
 938 | 			Point3D first = pontos_ordered.back();
 939 | 			menorDist = 999999;
 940 | 			tam = pontos.size();
 941 | 			std::vector<Point3D>::iterator it;
 942 | 			for (it = pontos.begin(); it != pontos.end(); ++it) {
 943 | 				dist = DrawAux::euclideanDist(first, *it);
 944 | 				if (dist<menorDist && dist<10) {
 945 | 					menorDist = dist;
 946 | 					closest = it;
 947 | 					if (dist<2) {
 948 | 						//printf("dist == %f\n", dist);
 949 | 						break;
 950 | 					}
 951 | 				}
 952 | 			}
 953 | 			if (menorDist < 999999) {
 954 | 				closestP = *closest;
 955 | 				pontos_ordered.push_back(closestP);
 956 | 				pontos.erase(closest);
 957 | 			}
 958 | 		}
 959 | 	}
 960 | 
 961 | 
 962 | 	// Suaviza braco. Aplica uma mascara de media em formato de + de tamanho 7x7
 963 | 	Point3D m;
 964 | 	int q;
 965 | 	for (int i=0; i<pontos_ordered.size() ; i++) {
 966 | 		m = Point3D(0,0);
 967 | 		q = 1;
 968 | 		if (i-3>=0) {
 969 | 			m.x += pontos_ordered[i-3].x;
 970 | 			m.y += pontos_ordered[i-3].y;
 971 | 			q++;
 972 | 		}
 973 | 		if (i-2>=0) {
 974 | 			m.x += pontos_ordered[i-2].x;
 975 | 			m.y += pontos_ordered[i-2].y;
 976 | 			q++;
 977 | 		}
 978 | 		if (i-1>=0) {
 979 | 			m.x += pontos_ordered[i-1].x;
 980 | 			m.y += pontos_ordered[i-1].y;
 981 | 			q++;
 982 | 		}
 983 | 		m.x += pontos_ordered[i].x;
 984 | 		m.y += pontos_ordered[i].y;
 985 | 		if (i+1<pontos_ordered.size()) {
 986 | 			m.x += pontos_ordered[i+1].x;
 987 | 			m.y += pontos_ordered[i+1].y;
 988 | 			q++;
 989 | 		}
 990 | 		if (i+2<pontos_ordered.size()) {
 991 | 			m.x += pontos_ordered[i+2].x;
 992 | 			m.y += pontos_ordered[i+2].y;
 993 | 			q++;
 994 | 		}
 995 | 		if (i+3<pontos_ordered.size()) {
 996 | 			m.x += pontos_ordered[i+3].x;
 997 | 			m.y += pontos_ordered[i+3].y;
 998 | 			q++;
 999 | 		}
1000 | 		m.x /= q;
1001 | 		m.y /= q;
1002 | 		pontos_ordered_smoth.push_back(m);
1003 | 	}
1004 | 
1005 | 	Point3D * el = getElbowHard(pontos_ordered_smoth, -1);
1006 | 	if (el) {
1007 | 		if (right) {
1008 | 			middleArmRight = *el;
1009 | 
1010 | 		} else {
1011 | 			middleArmLeft = *el;
1012 | 
1013 | 		}
1014 | 		delete el;
1015 | 	}
1016 | 	el = getElbowHard(pontos_ordered_smoth, 45);
1017 | 	if (el) {
1018 | 		if (right) {
1019 | 			middleArmRight45 = *el;
1020 | 
1021 | 		} else {
1022 | 			middleArmLeft45 = *el;
1023 | 
1024 | 		}
1025 | 		delete el;
1026 | 	}
1027 | 	
1028 | 
1029 | 	return pontos_ordered_smoth;
1030 | }
1031 | 
1032 | 
1033 | /**
1034 |  * Clear a 8-Connected region with the point specified by the params xy. 
1035 |  * All the region must be with the 255 value and will be replaced by 0.
1036 |  * Keep doint that on the 8-Connected neighborhood recursively.
1037 |  * 
1038 |  * Recursive version
1039 |  *
1040 |  * @param frame the imagem frame
1041 |  * @param x coordinate of the point
1042 |  * @param y coordinate of the point
1043 |  **/
1044 | void Skeleton::clearRegionRecursive(unsigned char * frame, int x, int y) {
1045 | 	if (x<wC && y<hC && x>=0 && y>=0 && frame[y*wC+x]==255) {
1046 | 		frame[y*wC+x]=0;
1047 | 		clearRegion(frame, x-1, y-1);
1048 | 		clearRegion(frame, x  , y-1);
1049 | 		clearRegion(frame, x+1, y-1);
1050 | 		clearRegion(frame, x-1, y);
1051 | 		clearRegion(frame, x+1, y);
1052 | 		clearRegion(frame, x-1, y+1);
1053 | 		clearRegion(frame, x  , y+1);
1054 | 		clearRegion(frame, x+1, y+1);
1055 | 	}
1056 | }
1057 | void Skeleton::clearRegion(unsigned char * frame, int x, int y) {
1058 | 	std::stack<cv::Point> points;
1059 | 	points.push(Point(x, y));
1060 | 	frame[y*wC+x]=0;
1061 | 
1062 | 	Point p;
1063 | 	do {
1064 | 		p = points.top();
1065 | 		points.pop();
1066 | 		x = p.x;
1067 | 		y = p.y;
1068 | 		if (verifyRegion(frame, x-1, y-1)) { points.push(Point(x-1, y-1)); }
1069 | 		if (verifyRegion(frame, x  , y-1)) { points.push(Point(x  , y-1)); }
1070 | 		if (verifyRegion(frame, x+1, y-1)) { points.push(Point(x+1, y-1)); }
1071 | 		if (verifyRegion(frame, x-1, y  )) { points.push(Point(x-1, y  )); }
1072 | 		if (verifyRegion(frame, x+1, y  )) { points.push(Point(x+1, y  )); }
1073 | 		if (verifyRegion(frame, x-1, y+1)) { points.push(Point(x-1, y+1)); }
1074 | 		if (verifyRegion(frame, x  , y+1)) { points.push(Point(x  , y+1)); }
1075 | 		if (verifyRegion(frame, x+1, y+1)) { points.push(Point(x+1, y+1)); }
1076 | 	} while (points.size()>0);
1077 | }
1078 | 
1079 | 
1080 | /**
1081 |  * Get the size of a 8-Connected region with the point specified by the params xy. 
1082 |  * All the region must be with the 255 value and will be replaced by 0.
1083 |  * Keep doint that on the 8-Connected neighborhood recursively.
1084 |  * 
1085 |  * Recursive version
1086 |  *
1087 |  * @param frame the imagem frame
1088 |  * @param x coordinate of the point
1089 |  * @param y coordinate of the point
1090 |  * @param quant the result (size of the region) will be stored here.
1091 |  **/
1092 | void Skeleton::getSizeRegionRecursive(unsigned char * frame, int x, int y, int *quant) {
1093 | 	if (x<wC && y<hC && x>=0 && y>=0 && frame[y*wC+x]==255) {
1094 | 		if (*quant > wC*hC*2/3)
1095 | 			return;
1096 | 		frame[y*wC+x]=0;
1097 | 		(*quant)++;
1098 | 		getSizeRegionRecursive(frame, x-1, y-1, quant);
1099 | 		getSizeRegionRecursive(frame, x  , y-1, quant);
1100 | 		getSizeRegionRecursive(frame, x+1, y-1, quant);
1101 | 		getSizeRegionRecursive(frame, x-1, y,   quant);
1102 | 		getSizeRegionRecursive(frame, x+1, y,   quant);
1103 | 		getSizeRegionRecursive(frame, x-1, y+1, quant);
1104 | 		getSizeRegionRecursive(frame, x  , y+1, quant);
1105 | 		getSizeRegionRecursive(frame, x+1, y+1, quant);
1106 | 	}
1107 | }
1108 | 
1109 | 
1110 | bool Skeleton::verifyRegion(unsigned char * frame, int x, int y) {
1111 | 	if (x<wC && y<hC && x>=0 && y>=0 && frame[y*wC+x]==255)
1112 | 	{
1113 | 		frame[y*wC+x]=0;
1114 | 		return true;
1115 | 	}
1116 | 	else
1117 | 		return false;
1118 | }
1119 | 
1120 | /**
1121 |  * Get the size of a 8-Connected region with the point specified by the params xy. 
1122 |  * All the region must be with the 255 value and will be replaced by 0.
1123 |  * Keep doint that on the 8-Connected neighborhood recursively.
1124 |  * 
1125 |  * Iterative version
1126 |  *
1127 |  * @param frame the imagem frame
1128 |  * @param x coordinate of the point
1129 |  * @param y coordinate of the point
1130 |  * @param quant the result (size of the region) will be stored here.
1131 |  **/
1132 | int Skeleton::getSizeRegion(unsigned char * frame, int x, int y) {
1133 | 	std::stack<cv::Point> points;
1134 | 	points.push(Point(x, y));
1135 | 	frame[y*wC+x]=0;
1136 | 	int quant = 1;
1137 | 
1138 | 	Point p;
1139 | 	do {
1140 | 		p = points.top();
1141 | 		points.pop();
1142 | 		x = p.x;
1143 | 		y = p.y;
1144 | 		if (verifyRegion(frame, x-1, y-1)) { quant++; points.push(Point(x-1, y-1)); }
1145 | 		if (verifyRegion(frame, x  , y-1)) { quant++; points.push(Point(x  , y-1)); }
1146 | 		if (verifyRegion(frame, x+1, y-1)) { quant++; points.push(Point(x+1, y-1)); }
1147 | 		if (verifyRegion(frame, x-1, y  )) { quant++; points.push(Point(x-1, y  )); }
1148 | 		if (verifyRegion(frame, x+1, y  )) { quant++; points.push(Point(x+1, y  )); }
1149 | 		if (verifyRegion(frame, x-1, y+1)) { quant++; points.push(Point(x-1, y+1)); }
1150 | 		if (verifyRegion(frame, x  , y+1)) { quant++; points.push(Point(x  , y+1)); }
1151 | 		if (verifyRegion(frame, x+1, y+1)) { quant++; points.push(Point(x+1, y+1)); }
1152 | 	
1153 | 		if (quant > wC*hC*2/3)
1154 | 			break;
1155 | 	} while (points.size()>0);
1156 | 	
1157 | 	return quant;
1158 | }
1159 | 
1160 | 
1161 | 
1162 | 
1163 | SkeletonPoints* Skeleton::getSkeletonPoints() {
1164 | 	return sp;
1165 | }
1166 | 
1167 | int Skeleton::getAfa() {
1168 | 	return afa*subSample;
1169 | }
1170 | 
1171 | 
1172 | /**
1173 |  * Pega o valor medio de uma regiao 5x5 circundando o ponto x,y. Pontos com valor zero nao sao considerados
1174 |  *
1175 |  * @param center point of the region.
1176 |  **/
1177 | int Skeleton::getMeanDepthValue(cv::Point& p) {
1178 | 	int v;
1179 | 	int q=0;
1180 | 	int t=0;
1181 | 	int w = width;
1182 | 	int h = height;
1183 | 	
1184 | 	if (p.x<w-2 && p.x>1 && p.y>1 && p.y<w-2) {
1185 | 		v = depthMat[(p.y-2)*w + (p.x-2)];
1186 | 		if (v) { t += v; q++; }
1187 | 		v = depthMat[(p.y-2)*w + (p.x-1)];
1188 | 		if (v) { t += v; q++; }
1189 | 		v = depthMat[(p.y-2)*w +  p.x];    
1190 | 		if (v) { t += v; q++; }
1191 | 		v = depthMat[(p.y-2)*w + (p.x+1)];
1192 | 		if (v) { t += v; q++; }
1193 | 		v = depthMat[(p.y-2)*w + (p.x+2)];
1194 | 		if (v) { t += v; q++; }
1195 | 	
1196 | 		v = depthMat[(p.y-1)*w + (p.x-2)];
1197 | 		if (v) { t += v; q++; }
1198 | 		v = depthMat[(p.y-1)*w + (p.x-1)];
1199 | 		if (v) { t += v; q++; }
1200 | 		v = depthMat[(p.y-1)*w +  p.x];    
1201 | 		if (v) { t += v; q++; }
1202 | 		v = depthMat[(p.y-1)*w + (p.x+1)];
1203 | 		if (v) { t += v; q++; }
1204 | 		v = depthMat[(p.y-1)*w + (p.x+2)];
1205 | 		if (v) { t += v; q++; }
1206 | 
1207 | 		v = depthMat[(p.y)*w + (p.x-2)];
1208 | 		if (v) { t += v; q++; }
1209 | 		v = depthMat[(p.y)*w + (p.x-1)];
1210 | 		if (v) { t += v; q++; }
1211 | 		v = depthMat[(p.y)*w +  p.x];    
1212 | 		if (v) { t += v; q++; }
1213 | 		v = depthMat[(p.y)*w + (p.x+1)];
1214 | 		if (v) { t += v; q++; }
1215 | 		v = depthMat[(p.y)*w + (p.x+2)];
1216 | 		if (v) { t += v; q++; }
1217 | 
1218 | 		v = depthMat[(p.y+1)*w + (p.x-2)];
1219 | 		if (v) { t += v; q++; }
1220 | 		v = depthMat[(p.y+1)*w + (p.x-1)];
1221 | 		if (v) { t += v; q++; }
1222 | 		v = depthMat[(p.y+1)*w +  p.x];    
1223 | 		if (v) { t += v; q++; }
1224 | 		v = depthMat[(p.y+1)*w + (p.x+1)];
1225 | 		if (v) { t += v; q++; }
1226 | 		v = depthMat[(p.y+1)*w + (p.x+2)];
1227 | 		if (v) { t += v; q++; }
1228 | 
1229 | 		v = depthMat[(p.y+2)*w + (p.x-2)];
1230 | 		if (v) { t += v; q++; }
1231 | 		v = depthMat[(p.y+2)*w + (p.x-1)];
1232 | 		if (v) { t += v; q++; }
1233 | 		v = depthMat[(p.y+2)*w +  p.x];    
1234 | 		if (v) { t += v; q++; }
1235 | 		v = depthMat[(p.y+2)*w + (p.x+1)];
1236 | 		if (v) { t += v; q++; }
1237 | 		v = depthMat[(p.y+2)*w + (p.x+2)];
1238 | 		if (v) { t += v; q++; }
1239 | 		
1240 | 	}
1241 | 	
1242 | 	if (q>0) {
1243 | 		//printf("v9::%d::%d::%d::%.1f\n", t, q, t/q, (float)t/(float)q);
1244 | 		return t/q;
1245 | 	}
1246 | 	else
1247 | 		return t;
1248 | }
1249 | 
1250 | 
1251 | 


--------------------------------------------------------------------------------