|
3
|
1 #include <stdio.h>
|
|
2
|
2 #include <XnVNite.h>
|
|
|
3
|
|
|
4 #include "KinectTrack.h"
|
|
|
5
|
|
3
|
6 #define N_COLORS 6
|
|
|
7
|
|
2
|
8 XnVPointDrawer::XnVPointDrawer(XnUInt32 nHistory, xn::DepthGenerator depthGenerator) :
|
|
|
9 XnVPointControl("XnVPointDrawer"),
|
|
|
10 mNHistorySize(nHistory), mDepthGenerator(depthGenerator), mBDrawDM(false), mBFrameID(false) {
|
|
|
11 mPFPositionBuffer = new XnFloat[nHistory*3];
|
|
|
12 }
|
|
|
13
|
|
|
14 XnVPointDrawer::~XnVPointDrawer() {
|
|
|
15 std::map<XnUInt32, std::list<XnPoint3D> >::iterator iter;
|
|
|
16 for (iter = mHistory.begin(); iter != mHistory.end(); ++iter)
|
|
|
17 {
|
|
|
18 iter->second.clear();
|
|
|
19 }
|
|
|
20 mHistory.clear();
|
|
|
21
|
|
|
22 delete []mPFPositionBuffer;
|
|
|
23 }
|
|
|
24
|
|
|
25 void XnVPointDrawer::setDepthMap(XnBool bDrawDM) {
|
|
|
26 mBDrawDM = bDrawDM;
|
|
|
27 }
|
|
|
28
|
|
3
|
29 void XnVPointDrawer::Update(XnVMessage* pMessage) {
|
|
|
30 // PointControl's Update calls all callbacks for each hand
|
|
|
31 XnVPointControl::Update(pMessage);
|
|
|
32
|
|
|
33 if (mBDrawDM)
|
|
|
34 {
|
|
|
35 // Draw depth map
|
|
|
36 xn::DepthMetaData depthMD;
|
|
|
37 mDepthGenerator.GetMetaData(depthMD);
|
|
|
38 //drawDepthMap(depthMD);
|
|
|
39 }
|
|
|
40 if (mBFrameID)
|
|
|
41 {
|
|
|
42 // Print out frame ID
|
|
|
43 xn::DepthMetaData depthMD;
|
|
|
44 mDepthGenerator.GetMetaData(depthMD);
|
|
|
45 //DrawFrameID(depthMD.FrameID());
|
|
|
46 }
|
|
|
47
|
|
|
48 // Draw hands
|
|
|
49 Draw();
|
|
|
50 }
|
|
|
51
|
|
|
52 int XnVPointDrawer::getPosition(float &x, float &y, float &z) const {
|
|
|
53 std::map<XnUInt32, std::list<XnPoint3D> >::const_iterator itr;
|
|
|
54 itr = mHistory.begin();
|
|
|
55 if (itr == mHistory.end()) return -1;
|
|
|
56 std::list<XnPoint3D>::const_iterator itr2;
|
|
|
57 itr2 = itr->second.begin();
|
|
|
58 if (itr2 == itr->second.end()) return -1;
|
|
|
59 XnPoint3D pt(*itr2);
|
|
|
60 x = pt.X;
|
|
|
61 y = pt.Y;
|
|
|
62 z = pt.Z;
|
|
|
63 return 1;
|
|
|
64 }
|
|
|
65 void XnVPointDrawer::Draw() const {
|
|
|
66 std::map<XnUInt32, std::list<XnPoint3D> >::const_iterator itr;
|
|
|
67 itr = mHistory.begin();
|
|
|
68 if (itr == mHistory.end()) return;
|
|
|
69 std::list<XnPoint3D>::const_iterator itr2;
|
|
|
70 itr2 = itr->second.begin();
|
|
|
71 if (itr2 == itr->second.end()) return;
|
|
|
72 XnPoint3D pt(*itr2);
|
|
|
73 // printf("%f, %f, %f\n", pt.X, pt.Y, pt.Z);
|
|
|
74
|
|
|
75
|
|
|
76 // std::map<XnUInt32, std::list<XnPoint3D> >::const_iterator PointIterator;
|
|
|
77
|
|
|
78 // // Go over each existing hand
|
|
|
79 // for (PointIterator = mHistory.begin();
|
|
|
80 // PointIterator != mHistory.end();
|
|
|
81 // ++PointIterator)
|
|
|
82 // {
|
|
|
83 // // Clear buffer
|
|
|
84 // XnUInt32 nPoints = 0;
|
|
|
85 // XnUInt32 i = 0;
|
|
|
86 // XnUInt32 Id = PointIterator->first;
|
|
|
87
|
|
|
88 // // Go over all previous positions of current hand
|
|
|
89 // std::list<XnPoint3D>::const_iterator PositionIterator;
|
|
|
90 // for (PositionIterator = PointIterator->second.begin();
|
|
|
91 // PositionIterator != PointIterator->second.end();
|
|
|
92 // ++PositionIterator, ++i)
|
|
|
93 // {
|
|
|
94 // // Add position to buffer
|
|
|
95 // XnPoint3D pt(*PositionIterator);
|
|
|
96 // printf("%f, %f, %f\n", pt.X, pt.Y, pt.Z);
|
|
|
97 // mPFPositionBuffer[3*i] = pt.X;
|
|
|
98 // mPFPositionBuffer[3*i + 1] = pt.Y;
|
|
|
99 // mPFPositionBuffer[3*i + 2] = 0;//pt.Z();
|
|
|
100 // break;
|
|
|
101 // }
|
|
|
102
|
|
|
103 // // Set color
|
|
|
104 // XnUInt32 nColor = Id % N_COLORS;
|
|
|
105 // //XnUInt32 nSingle = GetPrimaryID();
|
|
|
106 // if (Id == GetPrimaryID())
|
|
|
107 // nColor = 6;
|
|
|
108 // }
|
|
|
109 }
|
|
|
110
|
|
|
111 static XnBool bShouldPrint = false;
|
|
|
112 void XnVPointDrawer::OnPointCreate(const XnVHandPointContext* cxt)
|
|
|
113 {
|
|
|
114 printf("** %d\n", cxt->nID);
|
|
|
115 // Create entry for the hand
|
|
|
116 mHistory[cxt->nID].clear();
|
|
|
117 bShouldPrint = true;
|
|
|
118 OnPointUpdate(cxt);
|
|
|
119 bShouldPrint = true;
|
|
|
120 }
|
|
|
121 // Handle new position of an existing hand
|
|
|
122 void XnVPointDrawer::OnPointUpdate(const XnVHandPointContext* cxt)
|
|
|
123 {
|
|
|
124 // positions are kept in projective coordinates, since they are only used for drawing
|
|
|
125 XnPoint3D ptProjective(cxt->ptPosition);
|
|
|
126
|
|
|
127 if (bShouldPrint)printf("Point (%f,%f,%f)", ptProjective.X, ptProjective.Y, ptProjective.Z);
|
|
|
128 mDepthGenerator.ConvertRealWorldToProjective(1, &ptProjective, &ptProjective);
|
|
|
129 if (bShouldPrint)printf(" -> (%f,%f,%f)\n", ptProjective.X, ptProjective.Y, ptProjective.Z);
|
|
|
130
|
|
|
131 // Add new position to the history buffer
|
|
|
132 mHistory[cxt->nID].push_front(ptProjective);
|
|
|
133 // Keep size of history buffer
|
|
|
134 if (mHistory[cxt->nID].size() > mNHistorySize)
|
|
|
135 mHistory[cxt->nID].pop_back();
|
|
|
136 bShouldPrint = false;
|
|
|
137 }
|
|
|
138
|
|
|
139 // Handle destruction of an existing hand
|
|
|
140 void XnVPointDrawer::OnPointDestroy(XnUInt32 nID)
|
|
|
141 {
|
|
|
142 // No need for the history buffer
|
|
|
143 mHistory.erase(nID);
|
|
|
144 }
|
