This repository has been archived by the owner on Nov 26, 2020. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 2
/
CRForestEstimator.cpp
335 lines (234 loc) · 9.83 KB
/
CRForestEstimator.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
#ifdef DWIN
#include "stdafx.h"
#include <windows.h>
#endif
#include "CRForestEstimator.h"
#include <vector>
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace std;
using namespace cv;
bool CRForestEstimator::loadForest(const char* treespath, int ntrees){
// Init forest with number of trees
crForest = new CRForest( ntrees );
// Load forest
if( !crForest->loadForest( treespath ) )
return false;
return true;
}
Rect CRForestEstimator::getBoundingBox(const Mat& im3D){
Rect bbox;
int min_x = im3D.cols;
int min_y = im3D.rows;
int max_x = 0;
int max_y = 0;
int p_width = crForest->getPatchWidth();
int p_height = crForest->getPatchHeight();
for(int y = 0; y < im3D.rows; y++)
{
const Vec3f* Mi = im3D.ptr<Vec3f>(y);
for(int x = 0; x < im3D.cols; x++){
if( Mi[x][2] > 0) {
min_x = MIN(min_x,x); min_y = MIN(min_y,y);
max_x = MAX(max_x,x); max_y = MAX(max_y,y);
}
}
}
int new_w = max_x - min_x + p_width;
int new_h = max_y - min_y + p_height;
bbox.x = MIN( im3D.cols-1, MAX( 0 , min_x - p_width/2 ) );
bbox.y = MIN( im3D.rows-1, MAX( 0 , min_y - p_height/2) );
bbox.width = MAX(0, MIN( new_w, im3D.cols-bbox.x));
bbox.height = MAX(0, MIN( new_h, im3D.rows-bbox.y));
return bbox;
}
void CRForestEstimator::estimate( const Mat & im3D,
std::vector< cv::Vec<float,6> >& means, //output
std::vector< std::vector< Vote > >& clusters, //all clusters
std::vector< Vote >& votes, //all votes
int stride,
float max_variance,
float prob_th,
float larger_radius_ratio,
float smaller_radius_ratio,
bool verbose , int threshold ){
unsigned int max_clusters = 20;
int max_ms_iterations = 10;
int p_width = crForest->getPatchWidth();
int p_height = crForest->getPatchHeight();
//get bounding box around the data in the image
Rect bbox = getBoundingBox(im3D);
vector<Mat> channels;
split(im3D, channels);
int rows = im3D.rows;
int cols = im3D.cols;
//integral image of the depth channel
Mat depthInt( rows+1, cols+1, CV_64FC1);
integral( channels[2], depthInt );
//feature channels vector, in our case it contains only the depth integral image, but it could have other channels (e.g., greyscale)
std::vector< cv::Mat > featureChans;
featureChans.push_back(depthInt);
//mask
Mat mask( rows, cols, CV_32FC1); mask.setTo(0);
cv::threshold( channels[2], mask, 10, 1, THRESH_BINARY);
//integral image of the mask
Mat maskIntegral( rows+1, cols+1, CV_64FC1); maskIntegral.setTo(0);
integral( mask, maskIntegral );
//defines the test patch
Rect roi = Rect(0,0,p_width,p_height);
int min_no_pixels = p_width*p_height/10;
int half_w = roi.width/2;
int half_h = roi.height/2;
//process each patch
for(roi.y=bbox.y; roi.y<bbox.y+bbox.height-p_height; roi.y+=stride) {
float* rowX = channels[0].ptr<float>(roi.y + half_h);
float* rowY = channels[1].ptr<float>(roi.y + half_h);
float* rowZ = channels[2].ptr<float>(roi.y + half_h);
double* maskIntY1 = maskIntegral.ptr<double>(roi.y);
double* maskIntY2 = maskIntegral.ptr<double>(roi.y + roi.height);
for(roi.x=bbox.x; roi.x<bbox.x+bbox.width-p_width; roi.x+=stride) {
//discard if the middle of the patch does not have depth data
if( rowZ[roi.x + half_w] <= 0.f )
continue;
//discard if the patch is filled with data for less than 10%
if( (maskIntY1[roi.x] + maskIntY2[roi.x + roi.width] - maskIntY1[roi.x + roi.width] - maskIntY2[roi.x]) <= min_no_pixels )
continue;
//send the patch down the trees and retrieve leaves
std::vector< const LeafNode* > leaves = crForest->regressionIntegral( featureChans, maskIntegral, roi );
//go through the results
for(unsigned int t=0;t<leaves.size();++t){
//discard bad votes
if ( leaves[t]->pfg < prob_th || leaves[t]->trace > max_variance )
continue;
Vote v;
//add the 3D location under the patch center to the vote for the head center
v.vote[0] = leaves[t]->mean.at<float>(0) + rowX[roi.x + half_w];
v.vote[1] = leaves[t]->mean.at<float>(1) + rowY[roi.x + half_w];
v.vote[2] = leaves[t]->mean.at<float>(2) + rowZ[roi.x + half_w];
//angles, leave as in the leaf
v.vote[3] = leaves[t]->mean.at<float>(3);
v.vote[4] = leaves[t]->mean.at<float>(4);
v.vote[5] = leaves[t]->mean.at<float>(5);
v.trace = &(leaves[t]->trace);
v.conf = &(leaves[t]->pfg);
votes.push_back(v);
}
} // end for x
} // end for y
if(verbose)
cout << endl << "votes : " << votes.size() << endl;
Vec<float,POSE_SIZE> temp_mean;
vector< vector< Vote* > > temp_clusters;
vector< Vec<float,POSE_SIZE> > cluster_means;
//radius for clustering votes
float large_radius = AVG_FACE_DIAMETER2/(larger_radius_ratio*larger_radius_ratio);
//cluster using the head centers
for(unsigned int l=0;l<votes.size();++l){
bool found = false;
float best_dist = FLT_MAX;
unsigned int best_cluster = 0;
//for each cluster
for(unsigned int c=0; ( c<cluster_means.size() && found==false ); ++c){
float norm = 0;
for(int n=0;n<3;++n)
norm += (votes[l].vote[n]-cluster_means[c][n])*(votes[l].vote[n]-cluster_means[c][n]);
//is the offset smaller than the radius?
if( norm < large_radius ){
best_cluster = c;
found = true;
//add (pointer to) vote to the closest cluster (well, actually, the first cluster found within the distance)
temp_clusters[best_cluster].push_back( &(votes[l]) );
//update cluster's mean
((cluster_means[best_cluster])) = 0;
if ( temp_clusters[best_cluster].size() > 0 ){
for(unsigned int i=0;i < temp_clusters[best_cluster].size(); ++i)
((cluster_means[best_cluster])) = ((cluster_means[best_cluster])) + temp_clusters[best_cluster][i]->vote;
float div = float(MAX(1,temp_clusters[best_cluster].size()));
for(int n=0;n<POSE_SIZE;++n)
cluster_means[best_cluster][n] /= div;
}
}
}
//create a new cluster
if( !found && temp_clusters.size() < max_clusters ){
vector< Vote* > new_cluster;
new_cluster.push_back( &(votes[l]) );
temp_clusters.push_back( new_cluster );
Vec<float,POSE_SIZE> vote( votes[l].vote );
cluster_means.push_back( vote );
}
}
if(verbose){
cout << cluster_means.size() << " CLUSTERS ";
for(unsigned int c = 0 ; c<cluster_means.size(); ++c)
cout << temp_clusters[c].size() << " ";
cout << endl;
}
std::vector< std::vector< Vote* > > new_clusters; //after MS
vector< Vec<float,POSE_SIZE> > new_means;
int count = 0;
float ms_radius2 = AVG_FACE_DIAMETER*AVG_FACE_DIAMETER/(smaller_radius_ratio*smaller_radius_ratio);
//threshold defining if the cluster belongs to a head: it depends on the stride and on the number of trees
int th = cvRound((double)threshold*crForest->getSize()/(double)(stride*stride));
//do MS for each cluster
for(unsigned int c=0;c<cluster_means.size();++c){
if(verbose){
cout << endl << "MS cluster " << c << " : ";
for(int n=0;n<3;++n)
cout << cluster_means[c][n] << " ";
cout << endl;
}
if ( temp_clusters[c].size() <= th ){
if(verbose)
cout << "skipping cluster " << endl;
continue;
}
vector< Vote* > new_cluster;
for(unsigned int it=0; it<max_ms_iterations; ++it){
count = 0;
temp_mean = 0;
new_cluster.clear();
//for each vote in the cluster
for(unsigned int idx=0; idx < temp_clusters[c].size() ;++idx){
float norm = 0;
for(int n=0;n<3;++n)
norm += (temp_clusters[c][idx]->vote[n]-cluster_means[c][n])*(temp_clusters[c][idx]->vote[n]-cluster_means[c][n]);
if( norm < ms_radius2 ){
temp_mean = temp_mean + temp_clusters[c][idx]->vote;
new_cluster.push_back( temp_clusters[c][idx] );
count++;
}
}
for(int n=0;n<POSE_SIZE;++n)
temp_mean[n] /= (float)MAX(1,count);
float distance_to_previous_mean2 = 0;
for(int n=0;n<3;++n){
distance_to_previous_mean2 += (temp_mean[n]-cluster_means[c][n])*(temp_mean[n]-cluster_means[c][n]);
}
//cout << it << " " << distance_to_previous_mean2 << endl;
//update the mean of the cluster
cluster_means[c] = temp_mean;
if( distance_to_previous_mean2 < 1 )
break;
}
new_clusters.push_back( new_cluster );
new_means.push_back( cluster_means[c] );
}
for(unsigned int c=0; c < new_clusters.size() ;++c){
if( new_clusters[c].size() < th ) //discard clusters with not enough votes
continue;
vector< Vote > cluster;
cluster_means[c] = 0;
//for each vote in the cluster
for(unsigned int k=0; k < new_clusters[c].size(); k++ ){
cluster_means[c] = cluster_means[c] + new_clusters[c][k]->vote;
cluster.push_back( *(new_clusters[c][k]) );
}
float div = (float)MAX(1,new_clusters[c].size());
for(int n=0;n<POSE_SIZE;++n)
cluster_means[c][n] /= div;
means.push_back( cluster_means[c] );
clusters.push_back( cluster );
}
}