CRForestEstimator.cpp
Go to the documentation of this file.
00001 #ifdef DWIN
00002 #include "stdafx.h"
00003 #include <windows.h>
00004 #endif
00005 
00006 #include "head_pose_estimation/CRForestEstimator.h"
00007 #include <vector>
00008 #include <iostream>
00009 #include <opencv2/highgui/highgui.hpp>
00010 #include <opencv2/imgproc/imgproc.hpp>
00011 
00012 using namespace std;
00013 using namespace cv;
00014 
00015 bool CRForestEstimator::loadForest(const char* treespath, int ntrees){
00016 
00017         // Init forest with number of trees
00018         crForest = new CRForest( ntrees );
00019         // Load forest
00020         if( !crForest->loadForest( treespath ) )
00021                 return false;
00022 
00023         return true;
00024 }
00025 
00026 Rect CRForestEstimator::getBoundingBox(const Mat& im3D){
00027 
00028         Rect bbox;
00029         int min_x = im3D.cols;
00030         int min_y = im3D.rows;
00031         int max_x = 0;
00032         int max_y = 0;
00033         int p_width = crForest->getPatchWidth();
00034         int p_height = crForest->getPatchHeight();
00035 
00036         for(int y = 0; y < im3D.rows; y++)
00037         {
00038             const Vec3f* Mi = im3D.ptr<Vec3f>(y);
00039             for(int x = 0; x < im3D.cols; x++){
00040 
00041                 if( Mi[x][2] > 0) {
00042 
00043                                 min_x = MIN(min_x,x); min_y = MIN(min_y,y);
00044                                 max_x = MAX(max_x,x); max_y = MAX(max_y,y);
00045                         }
00046 
00047             }
00048         }
00049 
00050         int new_w = max_x - min_x + p_width;
00051         int new_h = max_y - min_y + p_height;
00052 
00053         bbox.x = MIN( im3D.cols-1, MAX( 0 , min_x - p_width/2 ) );
00054         bbox.y = MIN( im3D.rows-1, MAX( 0 , min_y - p_height/2) );
00055 
00056         bbox.width  = MAX(0, MIN( new_w, im3D.cols-bbox.x));
00057         bbox.height = MAX(0, MIN( new_h, im3D.rows-bbox.y));
00058 
00059         return bbox;
00060 
00061 }
00062 
00063 void CRForestEstimator::estimate( const Mat & im3D,
00064                                                                    std::vector< cv::Vec<float,6> >& means, //output
00065                                    std::vector< std::vector< Vote > >& clusters, //all clusters
00066                                    std::vector< Vote >& votes, //all votes
00067                                    int stride,
00068                                    float max_variance,
00069                                    float prob_th,
00070                                    float larger_radius_ratio,
00071                                    float smaller_radius_ratio,
00072                                    bool verbose , int threshold ){
00073 
00074     unsigned int max_clusters = 20;
00075     int max_ms_iterations = 10;
00076 
00077         int p_width = crForest->getPatchWidth();
00078         int p_height = crForest->getPatchHeight();
00079 
00080         //get bounding box around the data in the image
00081         Rect bbox = getBoundingBox(im3D);
00082 
00083         vector<Mat> channels;
00084         split(im3D, channels);
00085 
00086         int rows = im3D.rows;
00087         int cols = im3D.cols;
00088 
00089         //integral image of the depth channel
00090         Mat depthInt( rows+1, cols+1, CV_64FC1);
00091         integral( channels[2], depthInt );
00092 
00093         //feature channels vector, in our case it contains only the depth integral image, but it could have other channels (e.g., greyscale)
00094         std::vector< cv::Mat > featureChans;
00095         featureChans.push_back(depthInt);
00096 
00097         //mask
00098     Mat mask( rows, cols, CV_32FC1); mask.setTo(0);
00099         cv::threshold( channels[2], mask, 10, 1, THRESH_BINARY);
00100 
00101         //integral image of the mask
00102         Mat maskIntegral( rows+1, cols+1, CV_64FC1); maskIntegral.setTo(0);
00103         integral( mask, maskIntegral );
00104 
00105         //defines the test patch
00106     Rect roi = Rect(0,0,p_width,p_height);
00107 
00108     int min_no_pixels = p_width*p_height/10;
00109     int half_w = roi.width/2;
00110     int half_h = roi.height/2;
00111 
00112     //process each patch
00113     for(roi.y=bbox.y; roi.y<bbox.y+bbox.height-p_height; roi.y+=stride) {
00114 
00115         float* rowX = channels[0].ptr<float>(roi.y + half_h);
00116         float* rowY = channels[1].ptr<float>(roi.y + half_h);
00117         float* rowZ = channels[2].ptr<float>(roi.y + half_h);
00118 
00119         double* maskIntY1 = maskIntegral.ptr<double>(roi.y);
00120         double* maskIntY2 = maskIntegral.ptr<double>(roi.y + roi.height);
00121 
00122         for(roi.x=bbox.x; roi.x<bbox.x+bbox.width-p_width; roi.x+=stride) {
00123 
00124                 //discard if the middle of the patch does not have depth data
00125                         if( rowZ[roi.x + half_w] <= 0.f )
00126                                 continue;
00127 
00128                         //discard if the patch is filled with data for less than 10%
00129             if( (maskIntY1[roi.x] + maskIntY2[roi.x + roi.width] - maskIntY1[roi.x + roi.width] - maskIntY2[roi.x]) <= min_no_pixels )
00130                continue;
00131 
00132             //send the patch down the trees and retrieve leaves
00133             std::vector< const LeafNode* > leaves = crForest->regressionIntegral( featureChans, maskIntegral, roi );
00134 
00135             //go through the results
00136             for(unsigned int t=0;t<leaves.size();++t){
00137 
00138                 //discard bad votes
00139                 if ( leaves[t]->pfg < prob_th || leaves[t]->trace > max_variance )
00140                     continue;
00141 
00142                 Vote v;
00143 
00144                 //add the 3D location under the patch center to the vote for the head center
00145                 v.vote[0] = leaves[t]->mean.at<float>(0) + rowX[roi.x + half_w];
00146                 v.vote[1] = leaves[t]->mean.at<float>(1) + rowY[roi.x + half_w];
00147                 v.vote[2] = leaves[t]->mean.at<float>(2) + rowZ[roi.x + half_w];
00148 
00149                 //angles, leave as in the leaf
00150                 v.vote[3] = leaves[t]->mean.at<float>(3);
00151                 v.vote[4] = leaves[t]->mean.at<float>(4);
00152                 v.vote[5] = leaves[t]->mean.at<float>(5);
00153 
00154                 v.trace = &(leaves[t]->trace);
00155                 v.conf = &(leaves[t]->pfg);
00156 
00157                 votes.push_back(v);
00158             }
00159 
00160         } // end for x
00161 
00162     } // end for y
00163 
00164     if(verbose)
00165         cout << endl << "votes : " << votes.size() << endl;
00166 
00167 
00168     Vec<float,POSE_SIZE> temp_mean;
00169     vector< vector< Vote* > > temp_clusters;
00170     vector< Vec<float,POSE_SIZE> > cluster_means;
00171 
00172     //radius for clustering votes
00173     float large_radius = AVG_FACE_DIAMETER2/(larger_radius_ratio*larger_radius_ratio);
00174 
00175     //cluster using the head centers
00176     for(unsigned int l=0;l<votes.size();++l){
00177 
00178         bool found = false;
00179         float best_dist = FLT_MAX;
00180         unsigned int best_cluster = 0;
00181 
00182                 //for each cluster
00183                 for(unsigned int c=0; ( c<cluster_means.size() && found==false ); ++c){
00184 
00185                         float norm = 0;
00186                         for(int n=0;n<3;++n)
00187                             norm += (votes[l].vote[n]-cluster_means[c][n])*(votes[l].vote[n]-cluster_means[c][n]);
00188 
00189                         //is the offset smaller than the radius?
00190                         if( norm < large_radius ){
00191 
00192                                 best_cluster = c;
00193                                 found = true;
00194 
00195                                  //add (pointer to) vote to the closest cluster (well, actually, the first cluster found within the distance)
00196                                 temp_clusters[best_cluster].push_back( &(votes[l]) );
00197 
00198                                 //update cluster's mean
00199                                 ((cluster_means[best_cluster])) = 0;
00200                                 if ( temp_clusters[best_cluster].size() > 0 ){
00201 
00202                                         for(unsigned int i=0;i < temp_clusters[best_cluster].size(); ++i)
00203                                                 ((cluster_means[best_cluster])) = ((cluster_means[best_cluster])) + temp_clusters[best_cluster][i]->vote;
00204 
00205                                         float div = float(MAX(1,temp_clusters[best_cluster].size()));
00206                                         for(int n=0;n<POSE_SIZE;++n)
00207                                                 cluster_means[best_cluster][n] /= div;
00208                                 }
00209 
00210                         }
00211 
00212                 }
00213 
00214                 //create a new cluster
00215         if( !found && temp_clusters.size() < max_clusters ){
00216 
00217             vector< Vote* > new_cluster;
00218             new_cluster.push_back( &(votes[l]) );
00219             temp_clusters.push_back( new_cluster );
00220 
00221             Vec<float,POSE_SIZE> vote( votes[l].vote );
00222             cluster_means.push_back( vote );
00223 
00224         }
00225 
00226     }
00227 
00228     if(verbose){
00229         cout << cluster_means.size() << " CLUSTERS ";
00230         for(unsigned int c = 0 ; c<cluster_means.size(); ++c)
00231             cout << temp_clusters[c].size() << " ";
00232         cout << endl;
00233     }
00234 
00235     std::vector< std::vector< Vote* > > new_clusters; //after MS
00236     vector< Vec<float,POSE_SIZE> > new_means;
00237 
00238     int count = 0;
00239     float ms_radius2 = AVG_FACE_DIAMETER*AVG_FACE_DIAMETER/(smaller_radius_ratio*smaller_radius_ratio);
00240 
00241 
00242     //threshold defining if the cluster belongs to a head: it depends on the stride and on the number of trees
00243     int th = cvRound((double)threshold*crForest->getSize()/(double)(stride*stride));
00244 
00245     //do MS for each cluster
00246     for(unsigned int c=0;c<cluster_means.size();++c){
00247 
00248         if(verbose){
00249             cout << endl << "MS cluster " << c << " : ";
00250             for(int n=0;n<3;++n)
00251                 cout << cluster_means[c][n] << " ";
00252             cout << endl;
00253         }
00254 
00255         if ( temp_clusters[c].size() <= th ){
00256             if(verbose)
00257                 cout << "skipping cluster " << endl;
00258             continue;
00259         }
00260 
00261 
00262         vector< Vote* > new_cluster;
00263 
00264         for(unsigned int it=0; it<max_ms_iterations; ++it){
00265 
00266             count = 0;
00267             temp_mean = 0;
00268             new_cluster.clear();
00269 
00270             //for each vote in the cluster
00271             for(unsigned int idx=0; idx < temp_clusters[c].size() ;++idx){
00272 
00273                 float norm = 0;
00274                 for(int n=0;n<3;++n)
00275                         norm += (temp_clusters[c][idx]->vote[n]-cluster_means[c][n])*(temp_clusters[c][idx]->vote[n]-cluster_means[c][n]);
00276 
00277                 if( norm < ms_radius2 ){
00278 
00279                         temp_mean = temp_mean + temp_clusters[c][idx]->vote;
00280                         new_cluster.push_back( temp_clusters[c][idx] );
00281                     count++;
00282 
00283                 }
00284 
00285             }
00286 
00287             for(int n=0;n<POSE_SIZE;++n)
00288                 temp_mean[n] /= (float)MAX(1,count);
00289 
00290             float distance_to_previous_mean2 = 0;
00291             for(int n=0;n<3;++n){
00292                 distance_to_previous_mean2 += (temp_mean[n]-cluster_means[c][n])*(temp_mean[n]-cluster_means[c][n]);
00293             }
00294 
00295             //cout << it << " " << distance_to_previous_mean2 << endl;
00296 
00297             //update the mean of the cluster
00298                         cluster_means[c] = temp_mean;
00299 
00300             if( distance_to_previous_mean2 < 1 )
00301                 break;
00302 
00303         }
00304 
00305                 new_clusters.push_back( new_cluster );
00306                 new_means.push_back( cluster_means[c] );
00307 
00308     }
00309 
00310     for(unsigned int c=0; c < new_clusters.size() ;++c){
00311 
00312         if( new_clusters[c].size() < th ) //discard clusters with not enough votes
00313             continue;
00314 
00315         vector< Vote > cluster;
00316         cluster_means[c] = 0;
00317 
00318         //for each vote in the cluster
00319         for(unsigned int k=0; k < new_clusters[c].size(); k++ ){
00320 
00321                 cluster_means[c] = cluster_means[c] + new_clusters[c][k]->vote;
00322                 cluster.push_back( *(new_clusters[c][k]) );
00323 
00324         }
00325 
00326         float div = (float)MAX(1,new_clusters[c].size());
00327         for(int n=0;n<POSE_SIZE;++n)
00328                 cluster_means[c][n] /= div;
00329 
00330         means.push_back( cluster_means[c] );
00331         clusters.push_back( cluster );
00332 
00333     }
00334 
00335 }


head_pose_estimation
Author(s): Dan Lazewatsky
autogenerated on Thu Feb 11 2016 23:06:59