inline
void showBining(int num_pitch, float res_pitch, int min_pitch, int num_yaw, float res_yaw, int min_yaw, std::vector<std::vector<int> > & yaw_pitch_bins)
{
std::cout << "\t\t";
for (int j = 0; j < num_pitch; j++)
{
std::cout << pcl_round (static_cast<float>(min_pitch) + res_pitch * static_cast<float>(j)) << "\t";
}
std::cout << std::endl;
for (int i = 0; i < num_yaw; i++)
{
std::cout << pcl_round (static_cast<float>(min_yaw) + res_yaw * static_cast<float>(i)) << " => \t\t";
for (int j = 0; j < num_pitch; j++)
{
//std::cout << std::setprecision(3) << yaw_pitch_bins[i][j] / static_cast<float>(max_elems) << "\t";
std::cout << yaw_pitch_bins[i][j] << "\t";
}
std::cout << std::endl;
}
}
template <typename PointInT, typename PointOutT> void
pcl::ESFEstimation<PointInT, PointOutT>::computeESF (
PointCloudIn &pc, std::vector<float> &hist)
{
const int binsize = 64;
unsigned int sample_size = 20000;
srand (static_cast<unsigned int> (time (0)));
int maxindex = static_cast<int> (pc.points.size ());
int index1, index2, index3;
std::vector<float> d2v, d1v, d3v, wt_d3;
std::vector<int> wt_d2;
d1v.reserve (sample_size);
d2v.reserve (sample_size * 3);
d3v.reserve (sample_size);
wt_d2.reserve (sample_size * 3);
wt_d3.reserve (sample_size);
float h_in[binsize] = {0};
float h_out[binsize] = {0};
float h_mix[binsize] = {0};
float h_mix_ratio[binsize] = {0};
float h_a3_in[binsize] = {0};
float h_a3_out[binsize] = {0};
float h_a3_mix[binsize] = {0};
float h_d1[binsize] = {0};
float h_d3_in[binsize] = {0};
float h_d3_out[binsize] = {0};
float h_d3_mix[binsize] = {0};
float ratio=0.0;
float pih = static_cast<float>(M_PI) / 2.0f;
float a,b,c,s;
int th1,th2,th3;
int vxlcnt = 0;
int pcnt1,pcnt2,pcnt3;
for (size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
{
// get a new random point
index1 = rand()%maxindex;
index2 = rand()%maxindex;
index3 = rand()%maxindex;
if (index1==index2 || index1 == index3 || index2 == index3)
{
nn_idx--;
continue;
}
Eigen::Vector4f p1 = pc.points[index1].getVector4fMap ();
Eigen::Vector4f p2 = pc.points[index2].getVector4fMap ();
Eigen::Vector4f p3 = pc.points[index3].getVector4fMap ();
// A3
Eigen::Vector4f v21 (p2 - p1);
Eigen::Vector4f v31 (p3 - p1);
Eigen::Vector4f v23 (p2 - p3);
a = v21.norm (); b = v31.norm (); c = v23.norm (); s = (a+b+c) * 0.5f;
if (s * (s-a) * (s-b) * (s-c) <= 0.001f)
continue;
v21.normalize ();
v31.normalize ();
v23.normalize ();
//TODO: .dot gives nan's
th1 = static_cast<int> (pcl_round (acos (fabs (v21.dot (v31))) / pih * (binsize-1)));
th2 = static_cast<int> (pcl_round (acos (fabs (v23.dot (v31))) / pih * (binsize-1)));
th3 = static_cast<int> (pcl_round (acos (fabs (v23.dot (v21))) / pih * (binsize-1)));
if (th1 < 0 || th1 >= binsize)
{
nn_idx--;
continue;
}
if (th2 < 0 || th2 >= binsize)
{
nn_idx--;
continue;
}
if (th3 < 0 || th3 >= binsize)
{
nn_idx--;
continue;
}
//pcl::PointXYZ cog(((rand()%100)-50.0f) / 100.0f,((rand()%100)-50.0f) / 100.0f,((rand()%100)-50.0f) / 100.0f);
// D1
// d1v.push_back( pcl::euclideanDistance(cog, pc.points[index1]) );
// D2
d2v.push_back (pcl::euclideanDistance (pc.points[index1], pc.points[index2]));
d2v.push_back (pcl::euclideanDistance (pc.points[index1], pc.points[index3]));
d2v.push_back (pcl::euclideanDistance (pc.points[index2], pc.points[index3]));
int vxlcnt_sum = 0;
int p_cnt = 0;
// IN, OUT, MIXED, Ratio line tracing, index1->index2
{
const int xs = p1[0] < 0.0? static_cast<int>(floor(p1[0])+GRIDSIZE_H): static_cast<int>(ceil(p1[0])+GRIDSIZE_H-1);
const int ys = p1[1] < 0.0? static_cast<int>(floor(p1[1])+GRIDSIZE_H): static_cast<int>(ceil(p1[1])+GRIDSIZE_H-1);
const int zs = p1[2] < 0.0? static_cast<int>(floor(p1[2])+GRIDSIZE_H): static_cast<int>(ceil(p1[2])+GRIDSIZE_H-1);
const int xt = p2[0] < 0.0? static_cast<int>(floor(p2[0])+GRIDSIZE_H): static_cast<int>(ceil(p2[0])+GRIDSIZE_H-1);
const int yt = p2[1] < 0.0? static_cast<int>(floor(p2[1])+GRIDSIZE_H): static_cast<int>(ceil(p2[1])+GRIDSIZE_H-1);
const int zt = p2[2] < 0.0? static_cast<int>(floor(p2[2])+GRIDSIZE_H): static_cast<int>(ceil(p2[2])+GRIDSIZE_H-1);
wt_d2.push_back (this->lci (xs, ys, zs, xt, yt, zt, ratio, vxlcnt, pcnt1));
if (wt_d2.back () == 2)
h_mix_ratio[static_cast<int> (pcl_round (ratio * (binsize-1)))]++;
vxlcnt_sum += vxlcnt;
p_cnt += pcnt1;
}
// IN, OUT, MIXED, Ratio line tracing, index1->index3
{
const int xs = p1[0] < 0.0? static_cast<int>(floor(p1[0])+GRIDSIZE_H): static_cast<int>(ceil(p1[0])+GRIDSIZE_H-1);
const int ys = p1[1] < 0.0? static_cast<int>(floor(p1[1])+GRIDSIZE_H): static_cast<int>(ceil(p1[1])+GRIDSIZE_H-1);
const int zs = p1[2] < 0.0? static_cast<int>(floor(p1[2])+GRIDSIZE_H): static_cast<int>(ceil(p1[2])+GRIDSIZE_H-1);
const int xt = p3[0] < 0.0? static_cast<int>(floor(p3[0])+GRIDSIZE_H): static_cast<int>(ceil(p3[0])+GRIDSIZE_H-1);
const int yt = p3[1] < 0.0? static_cast<int>(floor(p3[1])+GRIDSIZE_H): static_cast<int>(ceil(p3[1])+GRIDSIZE_H-1);
const int zt = p3[2] < 0.0? static_cast<int>(floor(p3[2])+GRIDSIZE_H): static_cast<int>(ceil(p3[2])+GRIDSIZE_H-1);
wt_d2.push_back (this->lci (xs, ys, zs, xt, yt, zt, ratio, vxlcnt, pcnt2));
if (wt_d2.back () == 2)
h_mix_ratio[static_cast<int>(pcl_round (ratio * (binsize-1)))]++;
vxlcnt_sum += vxlcnt;
p_cnt += pcnt2;
}
// IN, OUT, MIXED, Ratio line tracing, index2->index3
{
const int xs = p2[0] < 0.0? static_cast<int>(floor(p2[0])+GRIDSIZE_H): static_cast<int>(ceil(p2[0])+GRIDSIZE_H-1);
const int ys = p2[1] < 0.0? static_cast<int>(floor(p2[1])+GRIDSIZE_H): static_cast<int>(ceil(p2[1])+GRIDSIZE_H-1);
const int zs = p2[2] < 0.0? static_cast<int>(floor(p2[2])+GRIDSIZE_H): static_cast<int>(ceil(p2[2])+GRIDSIZE_H-1);
const int xt = p3[0] < 0.0? static_cast<int>(floor(p3[0])+GRIDSIZE_H): static_cast<int>(ceil(p3[0])+GRIDSIZE_H-1);
const int yt = p3[1] < 0.0? static_cast<int>(floor(p3[1])+GRIDSIZE_H): static_cast<int>(ceil(p3[1])+GRIDSIZE_H-1);
const int zt = p3[2] < 0.0? static_cast<int>(floor(p3[2])+GRIDSIZE_H): static_cast<int>(ceil(p3[2])+GRIDSIZE_H-1);
wt_d2.push_back (this->lci (xs,ys,zs,xt,yt,zt,ratio,vxlcnt,pcnt3));
if (wt_d2.back () == 2)
h_mix_ratio[static_cast<int>(pcl_round(ratio * (binsize-1)))]++;
vxlcnt_sum += vxlcnt;
p_cnt += pcnt3;
}
// D3 ( herons formula )
d3v.push_back (sqrt (sqrt (s * (s-a) * (s-b) * (s-c))));
if (vxlcnt_sum <= 21)
{
wt_d3.push_back (0);
h_a3_out[th1] += static_cast<float> (pcnt3) / 32.0f;
h_a3_out[th2] += static_cast<float> (pcnt1) / 32.0f;
h_a3_out[th3] += static_cast<float> (pcnt2) / 32.0f;
}
else
if (p_cnt - vxlcnt_sum < 4)
{
h_a3_in[th1] += static_cast<float> (pcnt3) / 32.0f;
h_a3_in[th2] += static_cast<float> (pcnt1) / 32.0f;
h_a3_in[th3] += static_cast<float> (pcnt2) / 32.0f;
wt_d3.push_back (1);
}
else
{
h_a3_mix[th1] += static_cast<float> (pcnt3) / 32.0f;
h_a3_mix[th2] += static_cast<float> (pcnt1) / 32.0f;
h_a3_mix[th3] += static_cast<float> (pcnt2) / 32.0f;
wt_d3.push_back (static_cast<float> (vxlcnt_sum) / static_cast<float> (p_cnt));
}
}
// Normalizing, get max
float maxd1 = 0;
float maxd2 = 0;
float maxd3 = 0;
for (size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
{
// get max of Dx
if (d1v[nn_idx] > maxd1)
maxd1 = d1v[nn_idx];
if (d2v[nn_idx] > maxd2)
maxd2 = d2v[nn_idx];
if (d2v[sample_size + nn_idx] > maxd2)
maxd2 = d2v[sample_size + nn_idx];
if (d2v[sample_size*2 +nn_idx] > maxd2)
maxd2 = d2v[sample_size*2 +nn_idx];
if (d3v[nn_idx] > maxd3)
maxd3 = d3v[nn_idx];
}
// Normalize and create histogram
int index;
for (size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
{
h_d1[static_cast<int>(pcl_round (d1v[nn_idx] / maxd1 * (binsize-1)))]++ ;
if (wt_d3[nn_idx] >= 0.999) // IN
{
index = static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
if (index >= 0 && index < binsize)
h_d3_in[index]++;
}
else
{
if (wt_d3[nn_idx] <= 0.001) // OUT
{
index = static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
if (index >= 0 && index < binsize)
h_d3_out[index]++ ;
}
else
{
index = static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
if (index >= 0 && index < binsize)
h_d3_mix[index]++;
}
}
}
//normalize and create histogram
for (size_t nn_idx = 0; nn_idx < d2v.size(); ++nn_idx )
{
if (wt_d2[nn_idx] == 0)
h_in[static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++ ;
if (wt_d2[nn_idx] == 1)
h_out[static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++;
if (wt_d2[nn_idx] == 2)
h_mix[static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++ ;
}
//float weights[10] = {1, 1, 1, 1, 1, 1, 1, 1 , 1 , 1};
float weights[10] = {0.5f, 0.5f, 0.5f, 0.5f, 0.5f, 1.0f, 1.0f, 2.0f, 2.0f, 2.0f};
hist.reserve (binsize * 10);
for (int i = 0; i < binsize; i++)
hist.push_back (h_a3_in[i] * weights[0]);
for (int i = 0; i < binsize; i++)
hist.push_back (h_a3_out[i] * weights[1]);
for (int i = 0; i < binsize; i++)
hist.push_back (h_a3_mix[i] * weights[2]);
for (int i = 0; i < binsize; i++)
hist.push_back (h_d3_in[i] * weights[3]);
for (int i = 0; i < binsize; i++)
hist.push_back (h_d3_out[i] * weights[4]);
for (int i = 0; i < binsize; i++)
hist.push_back (h_d3_mix[i] * weights[5]);
for (int i = 0; i < binsize; i++)
hist.push_back (h_in[i]*0.5f * weights[6]);
for (int i = 0; i < binsize; i++)
hist.push_back (h_out[i] * weights[7]);
for (int i = 0; i < binsize; i++)
hist.push_back (h_mix[i] * weights[8]);
for (int i = 0; i < binsize; i++)
hist.push_back (h_mix_ratio[i]*0.5f * weights[9]);
float sm = 0;
for (size_t i = 0; i < hist.size (); i++)
sm += hist[i];
for (size_t i = 0; i < hist.size (); i++)
hist[i] /= sm;
}
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computePointSPFHSignature (const Eigen::Vector4f ¢roid_p,
const Eigen::Vector4f ¢roid_n,
const pcl::PointCloud<PointInT> &cloud,
const pcl::PointCloud<PointNT> &normals,
const std::vector<int> &indices)
{
Eigen::Vector4f pfh_tuple;
// Reset the whole thing
hist_f1_.setZero (nr_bins_f1_);
hist_f2_.setZero (nr_bins_f2_);
hist_f3_.setZero (nr_bins_f3_);
hist_f4_.setZero (nr_bins_f4_);
// Get the bounding box of the current cluster
//Eigen::Vector4f min_pt, max_pt;
//pcl::getMinMax3D (cloud, indices, min_pt, max_pt);
//double distance_normalization_factor = (std::max)((centroid_p - min_pt).norm (), (centroid_p - max_pt).norm ());
//Instead of using the bounding box to normalize the VFH distance component, it is better to use the max_distance
//from any point to centroid. VFH is invariant to rotation about the roll axis but the bounding box is not,
//resulting in different normalization factors for point clouds that are just rotated about that axis.
double distance_normalization_factor = 1.0;
if ( normalize_distances_ ) {
Eigen::Vector4f max_pt;
pcl::getMaxDistance (cloud, indices, centroid_p, max_pt);
distance_normalization_factor = (centroid_p - max_pt).norm ();
}
// Factorization constant
float hist_incr;
if (normalize_bins_) {
hist_incr = 100.0 / (float)(indices.size () - 1);
} else {
hist_incr = 1.0;
}
float hist_incr_size_component;
if (size_component_)
hist_incr_size_component = hist_incr;
else
hist_incr_size_component = 0.0;
// Iterate over all the points in the neighborhood
for (size_t idx = 0; idx < indices.size (); ++idx)
{
// Compute the pair P to NNi
if (!computePairFeatures (centroid_p, centroid_n, cloud.points[indices[idx]].getVector4fMap (),
normals.points[indices[idx]].getNormalVector4fMap (), pfh_tuple[0], pfh_tuple[1],
pfh_tuple[2], pfh_tuple[3]))
continue;
// Normalize the f1, f2, f3, f4 features and push them in the histogram
int h_index = floor (nr_bins_f1_ * ((pfh_tuple[0] + M_PI) * d_pi_));
if (h_index < 0)
h_index = 0;
if (h_index >= nr_bins_f1_)
h_index = nr_bins_f1_ - 1;
hist_f1_ (h_index) += hist_incr;
h_index = floor (nr_bins_f2_ * ((pfh_tuple[1] + 1.0) * 0.5));
if (h_index < 0)
h_index = 0;
if (h_index >= nr_bins_f2_)
h_index = nr_bins_f2_ - 1;
hist_f2_ (h_index) += hist_incr;
h_index = floor (nr_bins_f3_ * ((pfh_tuple[2] + 1.0) * 0.5));
if (h_index < 0)
h_index = 0;
if (h_index >= nr_bins_f3_)
h_index = nr_bins_f3_ - 1;
hist_f3_ (h_index) += hist_incr;
if (normalize_distances_)
h_index = floor (nr_bins_f4_ * (pfh_tuple[3] / distance_normalization_factor));
else
h_index = pcl_round (pfh_tuple[3] * 100);
if (h_index < 0)
h_index = 0;
if (h_index >= nr_bins_f4_)
h_index = nr_bins_f4_ - 1;
hist_f4_ (h_index) += hist_incr_size_component;
}
}
void
pcl::Permutohedral::initOLD (const std::vector<float> &feature, const int feature_dimension, const int N)
{
// Compute the lattice coordinates for each feature [there is going to be a lot of magic here
N_ = N;
d_ = feature_dimension;
HashTableOLD hash_table(d_, N_*(d_+1));
// Allocate the class memory
if (offsetOLD_) delete [] offsetOLD_;
offsetOLD_ = new int[ (d_+1)*N_ ];
if (barycentricOLD_) delete [] barycentricOLD_;
barycentricOLD_ = new float[ (d_+1)*N_ ];
// Allocate the local memory
float * scale_factor = new float[d_];
float * elevated = new float[d_+1];
float * rem0 = new float[d_+1];
float * barycentric = new float[d_+2];
int * rank = new int[d_+1];
short * canonical = new short[(d_+1)*(d_+1)];
short * key = new short[d_+1];
// Compute the canonical simplex
for (int i=0; i<=d_; i++){
for (int j=0; j<=d_-i; j++)
canonical[i*(d_+1)+j] = static_cast<short> (i);
for (int j=d_-i+1; j<=d_; j++)
canonical[i*(d_+1)+j] = static_cast<short> (i - (d_+1));
}
// Expected standard deviation of our filter (p.6 in [Adams etal 2010])
float inv_std_dev = std::sqrt (2.0f / 3.0f)* static_cast<float>(d_+1);
// Compute the diagonal part of E (p.5 in [Adams etal 2010])
for (int i=0; i<d_; i++)
scale_factor[i] = 1.0f / std::sqrt (static_cast<float>(i+2)*static_cast<float>(i+1)) * inv_std_dev;
// Compute the simplex each feature lies in
for (int k=0; k<N_; k++)
{
//for (int k = 0; k < 500; k++){
// Elevate the feature (y = Ep, see p.5 in [Adams etal 2010])
//const float * f = feature + k*feature_size;
int index = k * feature_dimension;
// sm contains the sum of 1..n of our faeture vector
float sm = 0;
for (int j = d_; j > 0; j--)
{
//float cf = f[j-1]*scale_factor[j-1];
float cf = feature[index + j-1] * scale_factor[j-1];
elevated[j] = sm - static_cast<float>(j)*cf;
sm += cf;
}
elevated[0] = sm;
// Find the closest 0-colored simplex through rounding
float down_factor = 1.0f / static_cast<float>(d_+1);
float up_factor = static_cast<float>(d_+1);
int sum = 0;
for (int i=0; i<=d_; i++){
int rd = static_cast<int> (pcl_round (down_factor * elevated[i]));
rem0[i] = static_cast<float >(rd) * up_factor;
sum += rd;
}
// Find the simplex we are in and store it in rank (where rank describes what position coorinate i has in the sorted order of the features values)
for (int i=0; i<=d_; i++)
rank[i] = 0;
for (int i=0; i<d_; i++)
{
double di = elevated[i] - rem0[i];
for (int j=i+1; j<=d_; j++)
if (di < elevated[j] - rem0[j])
rank[i]++;
else
rank[j]++;
}
// If the point doesn't lie on the plane (sum != 0) bring it back
for (int i=0; i<=d_; i++){
rank[i] += sum;
if (rank[i] < 0){
rank[i] += d_+1;
rem0[i] += static_cast<float> (d_+1);
}
else if (rank[i] > d_){
rank[i] -= d_+1;
rem0[i] -= static_cast<float> (d_+1);
}
}
// Compute the barycentric coordinates (p.10 in [Adams etal 2010])
for (int i=0; i<=d_+1; i++)
barycentric[i] = 0;
for (int i=0; i<=d_; i++){
float v = (elevated[i] - rem0[i])*down_factor;
barycentric[d_-rank[i] ] += v;
barycentric[d_-rank[i]+1] -= v;
}
// Wrap around
barycentric[0] += 1.0f + barycentric[d_+1];
// Compute all vertices and their offset
for (int remainder = 0; remainder <= d_; remainder++)
{
for (int i = 0; i < d_; i++)
key[i] = static_cast<short int> (rem0[i] + canonical[remainder * (d_ + 1) + rank[i]]);
offsetOLD_[k*(d_+1)+remainder] = hash_table.find (key, true);
barycentricOLD_[k*(d_+1)+remainder] = barycentric[remainder];
baryOLD_.push_back (barycentric[remainder]);
}
}
delete [] scale_factor;
delete [] elevated;
delete [] rem0;
delete [] barycentric;
delete [] rank;
delete [] canonical;
delete [] key;
// Find the Neighbors of each lattice point
// Get the number of vertices in the lattice
M_ = hash_table.size();
// Create the neighborhood structure
if(blur_neighborsOLD_) delete[] blur_neighborsOLD_;
blur_neighborsOLD_ = new Neighbors[ (d_+1)*M_ ];
short * n1 = new short[d_+1];
short * n2 = new short[d_+1];
// For each of d+1 axes,
for (int j = 0; j <= d_; j++){
for (int i=0; i<M_; i++){
const short * key = hash_table.getKey(i);
//std::cout << *key << std::endl;
for (int k=0; k<d_; k++){
n1[k] = static_cast<short int> (key[k] - 1);
n2[k] = static_cast<short int> (key[k] + 1);
}
n1[j] = static_cast<short int> (key[j] + d_);
n2[j] = static_cast<short int> (key[j] - d_);
//std::cout << *n1 << " | " << *n2 << std::endl;
blur_neighborsOLD_[j*M_+i].n1 = hash_table.find(n1);
blur_neighborsOLD_[j*M_+i].n2 = hash_table.find(n2);
/*
std::cout << hash_table.find(n1) << " | " <<
hash_table.find(n2) << std::endl;
*/
}
}
delete[] n1;
delete[] n2;
}
void pcl::face_detection::FaceDetectorDataProvider<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::initialize(std::string & data_dir)
{
std::string start;
std::string ext = std::string ("pcd");
bf::path dir = data_dir;
std::vector < std::string > files;
getFilesInDirectory (dir, start, files, ext);
//apart from loading the file names, we will do some bining regarding pitch and yaw
std::vector < std::vector<int> > yaw_pitch_bins;
std::vector < std::vector<std::vector<std::string> > > image_files_per_bin;
float res_yaw = 15.f;
float res_pitch = res_yaw;
int min_yaw = -75;
int min_pitch = -60;
int num_yaw = static_cast<int>((std::abs (min_yaw) * 2) / static_cast<int>(res_yaw + 1.f));
int num_pitch = static_cast<int>((std::abs (min_pitch) * 2) / static_cast<int>(res_pitch + 1.f));
yaw_pitch_bins.resize (num_yaw);
image_files_per_bin.resize (num_yaw);
for (int i = 0; i < num_yaw; i++)
{
yaw_pitch_bins[i].resize (num_pitch);
image_files_per_bin[i].resize (num_pitch);
for (int j = 0; j < num_pitch; j++)
{
yaw_pitch_bins[i][j] = 0;
}
}
for (size_t i = 0; i < files.size (); i++)
{
std::stringstream filestream;
filestream << data_dir << "/" << files[i];
std::string file = filestream.str ();
std::string pose_file (files[i]);
boost::replace_all (pose_file, ".pcd", "_pose.txt");
Eigen::Matrix4f pose_mat;
pose_mat.setIdentity (4, 4);
std::stringstream filestream_pose;
filestream_pose << data_dir << "/" << pose_file;
pose_file = filestream_pose.str ();
bool result = readMatrixFromFile (pose_file, pose_mat);
if (result)
{
Eigen::Vector3f ea = pose_mat.block<3, 3> (0, 0).eulerAngles (0, 1, 2);
ea *= 57.2957795f; //transform it to degrees to do the binning
int y = static_cast<int>(pcl_round ((ea[0] + static_cast<float>(std::abs (min_yaw))) / res_yaw));
int p = static_cast<int>(pcl_round ((ea[1] + static_cast<float>(std::abs (min_pitch))) / res_pitch));
if (y < 0)
y = 0;
if (p < 0)
p = 0;
if (p >= num_pitch)
p = num_pitch - 1;
if (y >= num_yaw)
y = num_yaw - 1;
assert (y >= 0 && y < num_yaw);
assert (p >= 0 && p < num_pitch);
yaw_pitch_bins[y][p]++;
image_files_per_bin[y][p].push_back (file);
}
}
pcl::face_detection::showBining (num_pitch, res_pitch, min_pitch, num_yaw, res_yaw, min_yaw, yaw_pitch_bins);
int max_elems = 0;
int total_elems = 0;
for (int i = 0; i < num_yaw; i++)
{
for (int j = 0; j < num_pitch; j++)
{
total_elems += yaw_pitch_bins[i][j];
if (yaw_pitch_bins[i][j] > max_elems)
max_elems = yaw_pitch_bins[i][j];
}
}
float average = static_cast<float> (total_elems) / (static_cast<float> (num_pitch + num_yaw));
std::cout << "The average number of image per bin is:" << average << std::endl;
std::cout << "Total number of images in the dataset:" << total_elems << std::endl;
//reduce unbalance from dataset by capping the number of images per bin, keeping at least a certain min
if (min_images_per_bin_ != -1)
{
std::cout << "Reducing unbalance of the dataset." << std::endl;
for (int i = 0; i < num_yaw; i++)
{
for (int j = 0; j < num_pitch; j++)
{
if (yaw_pitch_bins[i][j] >= min_images_per_bin_)
{
std::random_shuffle (image_files_per_bin[i][j].begin (), image_files_per_bin[i][j].end ());
image_files_per_bin[i][j].resize (min_images_per_bin_);
yaw_pitch_bins[i][j] = min_images_per_bin_;
}
for (size_t ii = 0; ii < image_files_per_bin[i][j].size (); ii++)
{
image_files_.push_back (image_files_per_bin[i][j][ii]);
}
}
}
}
pcl::face_detection::showBining (num_pitch, res_pitch, min_pitch, num_yaw, res_yaw, min_yaw, yaw_pitch_bins);
std::cout << "Total number of images in the dataset:" << image_files_.size () << std::endl;
}
