double Timer::do_leave(const char *func_name)
{
const double tim = Tac();
const string s = func_name;
TCallData &d = m_data[s];
if (!d.open_calls.empty())
{
const double At = tim - d.open_calls.top();
d.open_calls.pop();
d.mean_t+=At;
if (d.n_calls==1)
{
d.min_t= At;
d.max_t= At;
}
else
{
if (d.min_t>At) d.min_t = At;
if (d.max_t<At) d.max_t = At;
}
return At;
}
else return 0; // This shouldn't happen!
}
void Timer::do_enter(const char *func_name)
{
const string s = func_name;
map<string,TCallData>::iterator it=m_data.find(s);
if(it==m_data.end())
{
#ifdef MUTI_THREAD
pi::ScopedMutex lock(mutex);
#endif
it=m_data.insert(make_pair(s,TCallData())).first;
}
TCallData &d = it->second;
d.n_calls++;
d.open_calls.push(0); // Dummy value, it'll be written below
d.open_calls.top() =Tac(); // to avoid possible delays.
}
// CLUSTERING METHOD
std::vector<poseEstimationSI::clusterPtr> poseEstimationSI::poseClustering(std::vector < posePtr > & bestPoses)
{
// Sort clusters
std::sort(bestPoses.begin(), bestPoses.end(), pose::compare);
std::vector< boost::shared_ptr<poseCluster> > centroids;
std::vector< boost::shared_ptr<cluster> > transformations;
float _orientationDistance;
float _positionDistance;
float _scaleDistance;
bool found;
int nearestCentroid;
///////////////////////////////
// Initialize first centroid //
///////////////////////////////
// If there are no poses, return
if(bestPoses.empty())
return transformations;
centroids.push_back(boost::shared_ptr<poseCluster> (new poseCluster(0,bestPoses[0])));
//centroids.back()->poses.push_back(bestPoses[0]);
Tic();
// For each pose
for(size_t p=1; p< bestPoses.size(); ++p)
{
found=false;
for(size_t c=0; c < centroids.size(); ++c)
{
//std::cout << std::endl << "\tcheck if centroid:" << std::endl;
// Compute (squared) distance to the cluster center
_orientationDistance=orientationDistance(bestPoses[p],bestPoses[centroids[c]->poseIndex]);
_positionDistance=positionDistance(bestPoses[p],bestPoses[centroids[c]->poseIndex]);
_scaleDistance=scaleDistance(bestPoses[p],bestPoses[centroids[c]->poseIndex]);
// If the cluster is nearer than a threshold add current pose to the cluster
//if((_orientationDistance>=poseAngleStepCos) && _positionDistance<=((model->maxModelDistSquared)*0.05*0.05))
//if((_orientationDistance>=cos(acos(poseAngleStepCos)) ) && _positionDistance<=((model->maxModelDistSquared)*0.1*0.1))
if((_orientationDistance>=pointPair::angleStepCos) && (_positionDistance<=model->halfDistanceStepSquared) &&(equalFloat(_scaleDistance,0.0))) // VER ISTO
{
nearestCentroid=c;
found=true;
break;
}
}
if(found)
{
centroids[nearestCentroid]->update(bestPoses[p]);
}
else
{
boost::shared_ptr<poseCluster> clus(new poseCluster(p,bestPoses[p]));
centroids.push_back(clus);
}
}
Tac();
//////////////////////
// Compute clusters //
//////////////////////
std::sort(centroids.begin(), centroids.end(), poseCluster::compare);
//std::cout << std::endl << "Number of poses:" <<bestPoses.size() << std::endl << std::endl;
//std::cout << std::endl << "Best pose votes:" <<bestPoses[0]->votes << std::endl << std::endl;
//for(size_t c=0; c < centroids.size(); ++c)
if(filterOn)
{
for(size_t c=0; c < centroids.size(); ++c)
//for(size_t c=0; c < 1; ++c)
{
//std::cout << centroids[c]->votes << std::endl;
transformations.push_back(boost::shared_ptr<cluster> (new cluster( boost::shared_ptr<pose> (new pose (centroids[c]->votes, boost::shared_ptr<transformation>( new transformation(centroids[c]->rotation(),centroids[c]->translation(),centroids[c]->scaling() ) ) ) ),centroids[c]->poses ) ) );
}
}
else
{
transformations.push_back(boost::shared_ptr<cluster> (new cluster( boost::shared_ptr<pose> (new pose (centroids[0]->votes, boost::shared_ptr<transformation>( new transformation(centroids[0]->rotation(),centroids[0]->translation(),centroids[0]->scaling() ) ) ) ),centroids[0]->poses ) ) );
}
return transformations;
}
// METHOD THAT RECEIVES SURFLET CLOUDS (USED FOR MIAN DATASED)
std::vector<cluster> poseEstimationSV::poseEstimationCore(pcl::PointCloud<pcl::PointNormal>::Ptr cloud)
{
int bestPoseAlpha;
int bestPosePoint;
int bestPoseVotes;
pcl::PointIndices normals_nan_indices;
float alpha;
unsigned int alphaBin,index;
// Iterators
std::vector<int>::iterator sr; // scene reference point
pcl::PointCloud<pcl::PointNormal>::iterator si; // scene paired point
std::vector<pointPairSV>::iterator sameFeatureIt; // same key on hash table
std::vector<boost::shared_ptr<pose> >::iterator bestPosesIt;
Eigen::Vector4f feature;
/*std::cout << "\tDownsample dense surflet cloud... " << std::endl;
cout << "\t\tSurflet cloud size before downsampling: " << cloud->size() << endl;
// Create the filtering object
pcl::VoxelGrid<pcl::PointNormal> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (model->distanceStep,model->distanceStep,model->distanceStep);
sor.filter (*cloudNormals);
cout << "\t\tSurflet cloud size after downsampling: " << cloudNormals->size() << endl;
std::cout<< "\tDone" << std::endl;*/
cloudNormals=cloud;
/*boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer2 = objectModel::viewportsVis(cloudNormals);
while (!viewer2->wasStopped ())
{
viewer2->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}*/
/*viewer2 = objectModel::viewportsVis(cloudNormals);
while (!viewer2->wasStopped ())
{
viewer2->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}*/
/*std::cout<< " Re-estimate normals... " << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr modelCloudPoint(new pcl::PointCloud<pcl::PointXYZ>);
for(si=cloudNormals->begin(); si<cloudNormals->end(); ++si)
{
modelCloudPoint->push_back(pcl::PointXYZ(si->x,si->y,si->z));
}
model->computeNormals(modelCloudPoint,cloudNormals);
std::cout << " Done" << std::endl;*/
//////////////////////////////////////////////////////////////////////////////
// Filter again to remove spurious normals nans (and it's associated point) //
//////////////////////////////////////////////////////////////////////////////
for (unsigned int i = 0; i < cloudNormals->points.size(); ++i)
{
if (isnan(cloudNormals->points[i].normal[0]) || isnan(cloudNormals->points[i].normal[1]) || isnan(cloudNormals->points[i].normal[2]))
{
normals_nan_indices.indices.push_back(i);
}
}
pcl::ExtractIndices<pcl::PointNormal> nan_extract;
nan_extract.setInputCloud(cloudNormals);
nan_extract.setIndices(boost::make_shared<pcl::PointIndices> (normals_nan_indices));
nan_extract.setNegative(true);
nan_extract.filter(*cloudWithNormalsDownSampled);
std::cout<< "\tCloud size after removing NaN normals: " << cloudWithNormalsDownSampled->size() << endl;
//////////////
// VOTATION //
//////////////
/////////////////////////////////////////////
// Extract reference points from the scene //
/////////////////////////////////////////////
//pcl::RandomSample< pcl::PointCloud<pcl::PointNormal> > randomSampler;
//randomSampler.setInputCloud(cloudWithNormalsDownSampled);
int numberOfPoints=(int) (cloudWithNormalsDownSampled->size () )*referencePointsPercentage;
int totalPoints=(int) (cloudWithNormalsDownSampled->size ());
std::cout << "\tUniform sample a set of " << numberOfPoints << "(" << referencePointsPercentage*100 << "%)... ";
referencePointsIndices->indices.clear();
extractReferencePointsUniform(referencePointsPercentage,totalPoints);
std::cout << "Done" << std::endl;
//std::cout << referencePointsIndices->indices.size() << std::endl;
//pcl::PointCloud<pcl::PointNormal>::Ptr testeCloud=pcl::PointCloud<pcl::PointNormal>::Ptr (new pcl::PointCloud<pcl::PointNormal>);
/*for(sr=referencePointsIndices->indices.begin(); sr < referencePointsIndices->indices.end(); ++sr)
{
testeCloud->push_back(cloudWithNormalsDownSampled->points[*sr]);
}*/
//std::cout << totalPoints << std::endl;
/*boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer2 = objectModel::viewportsVis(cloudWithNormalsDownSampled);
while (!viewer2->wasStopped ())
{
viewer2->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}*/
//////////////
// Votation //
//////////////
// Clear all transformations stored
std::cout<< "\tVotation... ";
//std::vector<int>::size_type sz;
bestPoses.clear();
//sz = bestPoses.capacity();
std::vector<int> matches_per_feature;
std::vector<double> scene_to_global_time;
std::vector<double> reset_accumulator_time;
std::vector<double> ppf_time;
std::vector<double> hash_time;
std::vector<double> matching_time;
std::vector<double> get_best_peak_time;
//Tic();
for(sr=referencePointsIndices->indices.begin(); sr < referencePointsIndices->indices.begin()+1; ++sr)
//for(sr=referencePointsIndices->indices.begin(); sr < referencePointsIndices->indices.end(); ++sr)
{
Tic();
Eigen::Vector3f scenePoint=cloudWithNormalsDownSampled->points[*sr].getVector3fMap();
Eigen::Vector3f sceneNormal=cloudWithNormalsDownSampled->points[*sr].getNormalVector3fMap ();
Eigen::Vector3f cross=sceneNormal.cross (Eigen::Vector3f::UnitX ()). normalized();
Eigen::Affine3f rotationSceneToGlobal;
// Get transformation from scene frame to global frame
if(sceneNormal.isApprox(Eigen::Vector3f::UnitX (),0.00001))
{
rotationSceneToGlobal=Eigen::AngleAxisf(0.0,Eigen::Vector3f::UnitX ());
}
else
{
cross=sceneNormal.cross (Eigen::Vector3f::UnitX ()). normalized();
if (isnan(cross[0]))
{
std::cout << "YA"<< std::endl;
exit(-1);
rotationSceneToGlobal=Eigen::AngleAxisf(0.0,Eigen::Vector3f::UnitX ());
}
else
{
rotationSceneToGlobal=Eigen::AngleAxisf(acosf (sceneNormal.dot (Eigen::Vector3f::UnitX ())),cross);
}
}
Eigen::Affine3f transformSceneToGlobal = Eigen::Translation3f ( rotationSceneToGlobal* ((-1)*scenePoint)) * rotationSceneToGlobal;
Tac();
scene_to_global_time.push_back(timeEnd);
//////////////////////
// Choose best pose //
//////////////////////
// Reset pose accumulator
Tic();
for(std::vector<std::vector<int> >::iterator accumulatorIt=accumulator.begin();accumulatorIt < accumulator.end(); ++accumulatorIt)
{
std::fill(accumulatorIt->begin(),accumulatorIt->end(),0);
}
Tac();
reset_accumulator_time.push_back(timeEnd);
for(si=cloudWithNormalsDownSampled->begin(); si < cloudWithNormalsDownSampled->end();++si)
{
// if same point, skip point pair
if( (cloudWithNormalsDownSampled->points[*sr].x==si->x) && (cloudWithNormalsDownSampled->points[*sr].y==si->y) && (cloudWithNormalsDownSampled->points[*sr].z==si->z))
{
continue;
}
float squaredDist=(cloudWithNormalsDownSampled->points[*sr].x-si->x)*
(cloudWithNormalsDownSampled->points[*sr].x-si->x)+
(cloudWithNormalsDownSampled->points[*sr].y-si->y)*
(cloudWithNormalsDownSampled->points[*sr].y-si->y)+
(cloudWithNormalsDownSampled->points[*sr].z-si->z)*
(cloudWithNormalsDownSampled->points[*sr].z-si->z);
if(squaredDist>model->maxModelDistSquared)
{
continue;
}
Tic();
float dist=sqrt(squaredDist);
// Compute PPF
pointPairSV PPF=pointPairSV(cloudWithNormalsDownSampled->points[*sr],*si,transformSceneToGlobal);
Tac();
ppf_time.push_back(timeEnd);
Tic();
// Compute index
index=PPF.getHash(*si,model->distanceStepInverted);
Tac();
hash_time.push_back(timeEnd);
// If distance between point pairs is bigger than the maximum for this model, skip point pair
if(index>=pointPairSV::maxHash)
{
//std::cout << "DEBUG" << std::endl;
continue;
}
// If there is no similar point pair features in the model, skip point pair and avoid computing the alpha
if(model->hashTable[index].size()==0)
continue;
int matches=0; //Tests
Tic();
// Iterate over similar point pairs
for(sameFeatureIt=model->hashTable[index].begin(); sameFeatureIt<model->hashTable[index].end(); ++sameFeatureIt)
{
// Vote on the reference point and angle (and object)
alpha=sameFeatureIt->alpha-PPF.alpha; // alpha values between [-360,360]
// alpha values should be between [-180,180] ANGLE_MAX = 2*PI
if(alpha<(-PI))
alpha=ANGLE_MAX+alpha;
else if(alpha>(PI))
alpha=alpha-ANGLE_MAX;
alphaBin=static_cast<unsigned int> ( round((alpha+PI)*pointPair::angleStepInverted) ); // division is slower than multiplication
if(alphaBin>=pointPair::angleBins)
{
alphaBin=0;
}
accumulator[sameFeatureIt->id][alphaBin]+=sameFeatureIt->weight;
++matches;//Tests
}
Tac();
matching_time.push_back(timeEnd);
matches_per_feature.push_back(matches);
}
//Tac();
// Choose best pose (highest peak on the accumulator[peak with more votes])
bestPosePoint=0;
bestPoseAlpha=0;
bestPoseVotes=0;
Tic();
for(size_t p=0; p < model->modelCloud->size(); ++p)
{
for(unsigned int a=0; a < pointPair::angleBins; ++a)
{
if(accumulator[p][a]>bestPoseVotes)
{
bestPoseVotes=accumulator[p][a];
bestPosePoint=p;
bestPoseAlpha=a;
}
}
}
Tac();
get_best_peak_time.push_back(timeEnd);
// A candidate pose was found
if(bestPoseVotes!=0)
{
// Compute and store transformation from model to scene
bestPoses.push_back(pose( bestPoseVotes,model->modelToScene(bestPosePoint,transformSceneToGlobal,static_cast<float>(bestPoseAlpha)*pointPair::angleStep-PI) ));
}
else
{
continue;
}
// Choose poses whose votes are a percentage above a given threshold of the best pose
accumulator[bestPosePoint][bestPoseAlpha]=0; // This is more efficient than having an if condition to verify if we are considering the best pose again
for(size_t p=0; p < model->modelCloud->size(); ++p)
{
for(unsigned int a=0; a < pointPair::angleBins; ++a)
{
if(accumulator[p][a]>=accumulatorPeakThreshold*bestPoseVotes)
{
bestPoses.push_back(pose( accumulator[p][a],model->modelToScene(p,transformSceneToGlobal,static_cast<float>(a)*pointPair::angleStep-PI) ));
}
}
}
/* if (sz!=bestPoses.capacity()) {
sz = bestPoses.capacity();
std::cout << "capacity changed: " << sz << '\n';
exit(-1);
}*/
}
//Tac();
std::cout << "Done" << std::endl;
if(bestPoses.size()==0)
{
clusters.clear();
return clusters;
}
//////////////////////
// Compute clusters //
//////////////////////
std::cout << "\tCompute clusters... ";
Tic();
clusters=poseClustering(bestPoses);
std::cout << "Done" << std::endl;
clusters[0].voting_time=timeEnd;
std::cout << timeEnd << " " << clusters[0].voting_time << std::endl;
Tac();
clusters[0].clustering_time=timeEnd;
clusters[0].matches_per_feature=matches_per_feature;
clusters[0].scene_to_global_time=scene_to_global_time;
clusters[0].reset_accumulator_time=reset_accumulator_time;
clusters[0].ppf_time=ppf_time;
clusters[0].hash_time=hash_time;
clusters[0].matching_time=matching_time;
clusters[0].get_best_peak_time=get_best_peak_time;
std::cout << "clusters size:" << clusters.size()<< std::endl;
return clusters;
}
// METHOD THAT RECEIVES POINT CLOUDS (OPEN MP)
std::vector<cluster> poseEstimationSV::poseEstimationCore_openmp(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
Tic();
std::vector <std::vector < pose > > bestPosesAux;
bestPosesAux.resize(omp_get_num_procs());
//int bestPoseAlpha;
//int bestPosePoint;
//int bestPoseVotes;
Eigen::Vector3f scenePoint;
Eigen::Vector3f sceneNormal;
pcl::PointIndices normals_nan_indices;
pcl::ExtractIndices<pcl::PointNormal> nan_extract;
float alpha;
unsigned int alphaBin,index;
// Iterators
//unsigned int sr; // scene reference point
pcl::PointCloud<pcl::PointNormal>::iterator si; // scene paired point
std::vector<pointPairSV>::iterator sameFeatureIt; // same key on hash table
std::vector<boost::shared_ptr<pose> >::iterator bestPosesIt;
Eigen::Vector4f feature;
Eigen::Vector3f _pointTwoTransformed;
std::cout<< "\tCloud size: " << cloud->size() << endl;
//////////////////////////////////////////////
// Downsample point cloud using a voxelgrid //
//////////////////////////////////////////////
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudDownsampled(new pcl::PointCloud<pcl::PointXYZ> ());
// Create the filtering object
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (model->distanceStep,model->distanceStep,model->distanceStep);
sor.filter (*cloudDownsampled);
std::cout<< "\tCloud size after downsampling: " << cloudDownsampled->size() << endl;
// Compute point cloud normals (using cloud before downsampling information)
std::cout<< "\tCompute normals... ";
cloudNormals=model->computeSceneNormals(cloudDownsampled);
std::cout<< "Done" << endl;
/*boost::shared_ptr<pcl_visualization::PCLVisualizer> viewer2 = objectModel::viewportsVis(cloudFilteredNormals);
while (!viewer2->wasStopped ())
{
viewer2->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}*/
/*boost::shared_ptr<pcl_visualization::PCLVisualizer> viewer2 = objectModel::viewportsVis(model->modelCloud);
while (!viewer2->wasStopped ())
{
viewer2->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}*/
//////////////////////////////////////////////////////////////////////////////
// Filter again to remove spurious normals nans (and it's associated point) //
////////////////////////////////////////////////fa//////////////////////////////
for (unsigned int i = 0; i < cloudNormals->points.size(); ++i)
{
if (isnan(cloudNormals->points[i].normal[0]) || isnan(cloudNormals->points[i].normal[1]) || isnan(cloudNormals->points[i].normal[2]))
{
normals_nan_indices.indices.push_back(i);
}
}
nan_extract.setInputCloud(cloudNormals);
nan_extract.setIndices(boost::make_shared<pcl::PointIndices> (normals_nan_indices));
nan_extract.setNegative(true);
nan_extract.filter(*cloudWithNormalsDownSampled);
std::cout<< "\tCloud size after removing NaN normals: " << cloudWithNormalsDownSampled->size() << endl;
/////////////////////////////////////////////
// Extract reference points from the scene //
/////////////////////////////////////////////
//pcl::RandomSample< pcl::PointCloud<pcl::PointNormal> > randomSampler;
//randomSampler.setInputCloud(cloudWithNormalsDownSampled);
// Create the filtering object
int numberOfPoints=(int) (cloudWithNormalsDownSampled->size () )*referencePointsPercentage;
int totalPoints=(int) (cloudWithNormalsDownSampled->size ());
std::cout << "\tUniform sample a set of " << numberOfPoints << "(" << referencePointsPercentage*100 << "%)... ";
referencePointsIndices->indices.clear();
extractReferencePointsUniform(referencePointsPercentage,totalPoints);
std::cout << "Done" << std::endl;
//std::cout << referencePointsIndices->indices.size() << std::endl;
//////////////
// Votation //
//////////////
std::cout<< "\tVotation... ";
omp_set_num_threads(omp_get_num_procs());
//omp_set_num_threads(1);
//int iteration=0;
bestPoses.clear();
#pragma omp parallel for private(alpha,alphaBin,alphaScene,sameFeatureIt,index,feature,si,_pointTwoTransformed) //reduction(+:iteration) //nowait
for(unsigned int sr=0; sr < referencePointsIndices->indices.size(); ++sr)
{
//++iteration;
//std::cout << "iteration: " << iteration << " thread:" << omp_get_thread_num() << std::endl;
//printf("Hello from thread %d, nthreads %d\n", omp_get_thread_num(), omp_get_num_threads());
scenePoint=cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].getVector3fMap();
sceneNormal=cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].getNormalVector3fMap();
// Get transformation from scene frame to global frame
Eigen::Vector3f cross=sceneNormal.cross (Eigen::Vector3f::UnitX ()). normalized();
Eigen::Affine3f rotationSceneToGlobal;
if(isnan(cross[0]))
{
rotationSceneToGlobal=Eigen::AngleAxisf(0.0,Eigen::Vector3f::UnitX ());
}
else
rotationSceneToGlobal=Eigen::AngleAxisf(acosf (sceneNormal.dot (Eigen::Vector3f::UnitX ())),cross);
Eigen::Affine3f transformSceneToGlobal = Eigen::Translation3f ( rotationSceneToGlobal* ((-1)*scenePoint)) * rotationSceneToGlobal;
//////////////////////
// Choose best pose //
//////////////////////
// Reset pose accumulator
for(std::vector<std::vector<int> >::iterator accumulatorIt=accumulatorParallelAux[omp_get_thread_num()].begin();accumulatorIt < accumulatorParallelAux[omp_get_thread_num()].end(); ++accumulatorIt)
{
std::fill(accumulatorIt->begin(),accumulatorIt->end(),0);
}
//std::cout << std::endl;
for(si=cloudWithNormalsDownSampled->begin(); si < cloudWithNormalsDownSampled->end();++si)
{
// if same point, skip point pair
if( (cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].x==si->x) && (cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].y==si->y) && (cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].z==si->z))
{
//std::cout << si->x << " " << si->y << " " << si->z << std::endl;
continue;
}
// Compute PPF
pointPairSV PPF=pointPairSV(cloudWithNormalsDownSampled->points[sr],*si, transformSceneToGlobal);
// Compute index
index=PPF.getHash(*si,model->distanceStepInverted);
// If distance between point pairs is bigger than the maximum for this model, skip point pair
if(index>pointPairSV::maxHash)
{
//std::cout << "DEBUG" << std::endl;
continue;
}
// If there is no similar point pair features in the model, skip point pair and avoid computing the alpha
if(model->hashTable[index].size()==0)
continue;
for(sameFeatureIt=model->hashTable[index].begin(); sameFeatureIt<model->hashTable[index].end(); ++sameFeatureIt)
{
// Vote on the reference point and angle (and object)
alpha=sameFeatureIt->alpha-PPF.alpha; // alpha values between [-360,360]
// alpha values should be between [-180,180] ANGLE_MAX = 2*PI
if(alpha<(-PI))
alpha=ANGLE_MAX+alpha;
else if(alpha>(PI))
alpha=alpha-ANGLE_MAX;
//std::cout << "alpha after: " << alpha*RAD_TO_DEG << std::endl;
//std::cout << "alpha after2: " << (alpha+PI)*RAD_TO_DEG << std::endl;
alphaBin=static_cast<unsigned int> ( round((alpha+PI)*pointPair::angleStepInverted) ); // division is slower than multiplication
//std::cout << "angle1: " << alphaBin << std::endl;
/*alphaBin = static_cast<unsigned int> (floor (alpha) + floor (PI *poseAngleStepInverted));
std::cout << "angle2: " << alphaBin << std::endl;*/
//alphaBin=static_cast<unsigned int> ( floor(alpha*poseAngleStepInverted) + floor(PI*poseAngleStepInverted) );
if(alphaBin>=pointPair::angleBins)
{
alphaBin=0;
//ROS_INFO("naoooo");
//exit(1);
}
//#pragma omp critical
//{std::cout << index <<" "<<sameFeatureIt->id << " " << alphaBin << " " << omp_get_thread_num() << " " << accumulatorParallelAux[omp_get_thread_num()][sameFeatureIt->id][alphaBin] << std::endl;}
accumulatorParallelAux[omp_get_thread_num()][sameFeatureIt->id][alphaBin]+=sameFeatureIt->weight;
}
}
//ROS_INFO("DISTANCE:%f DISTANCE SQUARED:%f", model->maxModelDist, model->maxModel
// Choose best pose (highest peak on the accumulator[peak with more votes])
int bestPoseAlpha=0;
int bestPosePoint=0;
int bestPoseVotes=0;
for(size_t p=0; p < model->modelCloud->size(); ++p)
{
for(unsigned int a=0; a < pointPair::angleBins; ++a)
{
if(accumulatorParallelAux[omp_get_thread_num()][p][a]>bestPoseVotes)
{
bestPoseVotes=accumulatorParallelAux[omp_get_thread_num()][p][a];
bestPosePoint=p;
bestPoseAlpha=a;
}
}
}
// A candidate pose was found
if(bestPoseVotes!=0)
{
// Compute and store transformation from model to scene
//boost::shared_ptr<pose> bestPose(new pose( bestPoseVotes,model->modelToScene(model->modelCloud->points[bestPosePoint],transformSceneToGlobal,static_cast<float>(bestPoseAlpha)*pointPair::angleStep-PI) ));
bestPosesAux[omp_get_thread_num()].push_back(pose( bestPoseVotes,model->modelToScene(bestPosePoint,transformSceneToGlobal,static_cast<float>(bestPoseAlpha)*pointPair::angleStep-PI) ));
//bestPoses.push_back(bestPose);
//std::cout << bestPosesAux[omp_get_thread_num()].size() <<" " <<omp_get_thread_num()<< std::endl;
}
else
{
continue;
}
// Choose poses whose votes are a percentage above a given threshold of the best pose
accumulatorParallelAux[omp_get_thread_num()][bestPosePoint][bestPoseAlpha]=0; // This is more efficient than having an if condition to verify if we are considering the best pose again
for(size_t p=0; p < model->modelCloud->size(); ++p)
{
for(unsigned int a=0; a < pointPair::angleBins; ++a)
{
if(accumulatorParallelAux[omp_get_thread_num()][p][a]>=accumulatorPeakThreshold*bestPoseVotes)
{
// Compute and store transformation from model to scene
//boost::shared_ptr<pose> bestPose(new pose( accumulatorParallelAux[omp_get_thread_num()][p][a],model->modelToScene(model->modelCloud->points[p],transformSceneToGlobal,static_cast<float>(a)*pointPair::angleStep-PI ) ));
//bestPoses.push_back(bestPose);
bestPosesAux[omp_get_thread_num()].push_back(pose( bestPoseVotes,model->modelToScene(bestPosePoint,transformSceneToGlobal,static_cast<float>(bestPoseAlpha)*pointPair::angleStep-PI) ));
//std::cout << bestPosesAux[omp_get_thread_num()].size() <<" " <<omp_get_thread_num()<< std::endl;
}
}
}
}
std::cout << "Done" << std::endl;
for(int i=0; i<omp_get_num_procs(); ++i)
{
for(unsigned int j=0; j<bestPosesAux[i].size(); ++j)
bestPoses.push_back(bestPosesAux[i][j]);
}
std::cout << "\thypothesis number: " << bestPoses.size() << std::endl << std::endl;
if(bestPoses.size()==0)
{
clusters.clear();
return clusters;
}
//////////////////////
// Compute clusters //
//////////////////////
Tac();
std::cout << "\tCompute clusters... ";
Tic();
clusters=poseClustering(bestPoses);
Tac();
std::cout << "Done" << std::endl;
return clusters;
}
bool InvertedIndex::ReadMetaDataFile(bool force, bool nodata, bool wrlocked,
string filename, size_t maxn) {
bool debug = false;
string msg = "Index::ReadMetaDataFile() : ";
if (force || /*nodata ||*/ wrlocked) {
cerr << "Index::ReadMetaDataFile(" << force << ", " << nodata << ", "
<< wrlocked << ")" << endl;
return ShowError(msg+"strange arguments");
}
// obs! feature_target should be solved here!
if (filename=="")
filename = metfile_str;
ifstream met(filename.c_str());
if (!met)
return ShowError(msg+"could not read <"+filename+">");
FloatVectorSet cols(DataSetSize());
string comment = "Automatically generated meta data\n"
"Field names (components):\n";
Tic("ReadMetaDataFile");
size_t linecount=0;
for (;;) {
string line;
getline(met, line);
if (!met)
break;
if (line.find_first_not_of(" \t")==string::npos)
continue;
if (line[0]=='#') {
if (line.find("indices")!=string::npos)
binary_classindices = true;
continue;
}
int i = line.size()-1;
while (i>=0 && isspace(line[i]))
line.erase(i--);
Tic("GroundTruthExpression");
bool expand = false; // was true (and much slower) until 5.4.2006
ground_truth ivec = db->GroundTruthExpression(line, feature_target, -1,
expand);
Tac("GroundTruthExpression");
if (debug)
db->GroundTruthSummaryTable(ivec);
if (ivec.empty()) {
meta_data_fields.clear();
return ShowError(msg+"<"+filename+"> field \"", line, "\" not known");
}
if (!nodata) {
Tic("loop-1");
FloatVector fvec(ivec.size());
for (int j=0; j<fvec.Length(); j++)
fvec[j] = ivec[j];
cols.AppendCopy(fvec);
linecount++;
Tac("loop-1");
}
int ncols = 1;
pair<string,int> mdf(line, ncols);
meta_data_fields.push_back(mdf);
stringstream ss;
ss << " " << line << " (" << ncols << ")";;;
comment += ss.str();
// cout << "!!! [" << line << "] OK !!!" << endl;
if (maxn != 0 && linecount >= maxn)
break;
}
if (!nodata) {
bool check_type = true, check_restr = true; // added 3.5.2006
Tic("loop-2");
data.Delete();
data.VectorLength(cols.Nitems());
for (int i=0; i<cols.VectorLength(); i++) {
if (check_type && !db->ObjectsTargetTypeContains(i, feature_target))
continue;
if (check_restr && !db->OkWithRestriction(i))
continue;
FloatVector v(cols.Nitems(), NULL, db->LabelP(i));
v.Number(i);
for (int j=0; j<v.Length(); j++)
v[j] = cols[j][i];
data.AppendCopy(v);
}
data.SetDescription(comment.c_str());
Tac("loop-2");
}
Tac("ReadMetaDataFile");
string log = "Read meta data file <"+ShortFileName(filename)+">";
if (!nodata)
log += " and processed to "+ToStr(data.Nitems())+" vectors of length "+
ToStr(data.VectorLength());
else
log += " without processing";
WriteLog(log);
return true;
}
