/** This method returns the angle that will form a straight line from position a to position b. a and b are [x, y] vectors. */
const double Utility::findAngleFromAToB(const std::vector<double> a, const std::vector<double> b) const
{
double result;
// If the positions are the same, return the orientation the robot already has
if(fabs(positionDistance(a, b)) < 0.01 && a.size() > 2)
{
return a.at(2);
}
// Find the distances in x,y directions and Euclidean distance
double d_x = b.at(0) - a.at(0);
double d_y = b.at(1) - a.at(1);
// Fails when vector from a to be is in 3rd quadrant
//result = atan(d_y / d_x);
double euc_dist = sqrt( pow(d_x,2) + pow(d_y,2) );
// If the positions are the same,
// Set the result to the starting orientation if one is provided
// Or to 0 if no starting orientation is provided
if(euc_dist <= 0.0001)
{
result = 0;
}
// If b is in the 1st or 2nd quadrants
else if(d_y > 0)
{
result = acos(d_x / euc_dist);
}
// If b is in the 3rd quadrant, d_y<0 & d_x<0
else if(d_x < 0)
{
result = -PI - asin(d_y / euc_dist);
}
// If b is in the 4th quadrant, d_y<=0 & d_x>=0
else
{
result = asin(d_y / euc_dist);
}
return result;
} // End findAngleFromAToB
// Cluster the existent clusters into well defined bins method (needed by grid-based filters)
void poseEstimationSV::clusterClusters(std::vector<cluster> & clusters)
{
//std::cout << "clusters before:" << clusters.size() << std::endl;
bool newMerge=true;
int it=0;
while(newMerge)
{
//std::cout << "cycle number: " << ++it << std::endl;
newMerge=false;
for(std::vector<cluster>::iterator c1=clusters.begin(); c1 < clusters.end(); ++c1)
{
for(std::vector<cluster>::iterator c2=c1+1; c2 < clusters.end();)
{
if( ( orientationDistance(*c1,*c2) >= pointPair::angleStepCos ) && ( positionDistance(*c1, *c2)) <= (model->maxModelDistSquared*0.05*0.05 ) )
{
if(isnan(acos(orientationDistance(*c1, *c2))))
{
std::cout << c1->meanPose.transform.rotation.w() << " " << c1->meanPose.transform.rotation.x() << " " << c1->meanPose.transform.rotation.y() << " " << c1->meanPose.transform.rotation.z() << std::endl;
std::cout << c2->meanPose.transform.rotation.w() << " " << c2->meanPose.transform.rotation.x() << " " << c2->meanPose.transform.rotation.y() << " " << c2->meanPose.transform.rotation.z() << std::endl;
std::cout << orientationDistance(*c1, *c2) << std::endl;
exit(1);
}
if(positionDistance(*c1, *c2) <= (model->maxModelDistSquared*0.05*0.05 ))
{
// average orientation
Eigen::Quaternion<float> q((c1->meanPose.transform.rotation.w()*c1->meanPose.votes)+(c2->meanPose.transform.rotation.w()*c2->meanPose.votes),
(c1->meanPose.transform.rotation.x()*c1->meanPose.votes)+(c2->meanPose.transform.rotation.x()*c2->meanPose.votes),
(c1->meanPose.transform.rotation.y()*c1->meanPose.votes)+(c2->meanPose.transform.rotation.y()*c2->meanPose.votes),
(c1->meanPose.transform.rotation.z()*c1->meanPose.votes)+(c2->meanPose.transform.rotation.z()*c2->meanPose.votes));
// normalize quaternion
q.normalize();
c1->meanPose.transform.rotation=q;
// average position
float votationSumInv=1/(c1->meanPose.votes+c2->meanPose.votes);
Eigen::Translation3f transAux;
transAux.x()=( (c1->meanPose.transform.translation.x()*c1->meanPose.votes) + (c2->meanPose.transform.translation.x()*c2->meanPose.votes) )*votationSumInv;
transAux.y()=( (c1->meanPose.transform.translation.y()*c1->meanPose.votes) + (c2->meanPose.transform.translation.y()*c2->meanPose.votes) )*votationSumInv;
transAux.z()=( (c1->meanPose.transform.translation.z()*c1->meanPose.votes) + (c2->meanPose.transform.translation.z()*c2->meanPose.votes) )*votationSumInv;
/*transAux.x()=( c1->meanPose.transform.translation.x() + c2->meanPose.transform.translation.x() )*0.5;
transAux.y()=( c1->meanPose.transform.translation.y() + c2->meanPose.transform.translation.y() )*0.5;
transAux.z()=( c1->meanPose.transform.translation.z() + c2->meanPose.transform.translation.z() )*0.5;*/
//std::cout << "VERRR ISTO:" << positionDistance(*c1, *c2) << " "<<acos(orientationDistance(*c1,*c2))*RAD_TO_DEG << std::endl;
//std::cout << "TRANS BEFORE:" << c1->meanPose.transform.translation.x() << " " << c1->meanPose.transform.translation.y() << " " << c1->meanPose.transform.translation.z() << std::endl;
c1->meanPose.transform.translation=transAux;
//std::cout << "TRANS AFTER:" << c1->meanPose.transform.translation.x() << " " << c1->meanPose.transform.translation.y() << " " << c1->meanPose.transform.translation.z() << std::endl;
c1->meanPose.votes+=c2->meanPose.votes;
}
// remove cluster 2
c2=clusters.erase(c2);
newMerge=true;
}
else
++c2;
}
}
}
//std::cout << "clusters after:" << clusters.size() << std::endl;
}
// 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;
}
// CLUSTERING METHOD
std::vector<cluster> poseEstimationSV::poseClustering(std::vector<pose> & bestPoses)
{
// Sort clusters
std::sort(bestPoses.begin(), bestPoses.end(), pose::compare);
std::vector<poseCluster> centroids;
std::vector<cluster> clusters;
float _orientationDistance;
float _positionDistance;
bool found;
int nearestCentroid;
///////////////////////////////
// Initialize first centroid //
///////////////////////////////
// If there are no poses, return
if(bestPoses.empty())
return clusters;
centroids.reserve(bestPoses.size());
centroids.push_back(poseCluster(0,bestPoses[0]));
// For each pose
for(size_t p=1; p< bestPoses.size(); ++p)
{
found=false;
for(size_t c=0; c < centroids.size(); ++c)
{
// Compute (squared) distance to the cluster center
_positionDistance=positionDistance(bestPoses[p],bestPoses[centroids[c].poseIndex]);
if(_positionDistance<=model->halfDistanceStepSquared)
continue;
_orientationDistance=orientationDistance(bestPoses[p],bestPoses[centroids[c].poseIndex]);
// If the cluster is nearer than a threshold add current pose to the cluster
if(_orientationDistance>=pointPair::angleStepCos) // VER ISTO
{
nearestCentroid=c;
found=true;
break;
}
}
if(found)
{
//std::cout << " yes:" << "pose:"<< p <<"nearest centroid:" << nearestCentroid << " " << 2*acos(_orientationDistance)*RAD_TO_DEG << " "<< _positionDistance << " " << bestPoses[p].votes << std::endl;
centroids[nearestCentroid].update(bestPoses[p]);
}
else
{
//if(2*acos(_orientationDistance)*RAD_TO_DEG< 6.000 && _positionDistance<model->halfDistanceStepSquared)
// std::cout << "angle:" << pointPair::angleStep*RAD_TO_DEG << "angle threshold: " <<2*acos(pointPair::angleStepCos)*RAD_TO_DEG<< " no:" << 2*acos(_orientationDistance)*RAD_TO_DEG << " "<< _positionDistance << std::endl;
centroids.push_back(poseCluster(p,bestPoses[p]));
}
//std::cout << p << std::endl;
}
//////////////////////
// Compute clusters //
//////////////////////
//std::sort(centroids.begin(), centroids.end(), poseCluster::compare);
// Normalize centroids
float totalModelVotes=static_cast<float>(model->modelCloud->size()*(model->modelCloud->size()-1));
//std::cout << totalModelVotes << " " << model->totalSurfaceArea << std::endl;
//std::cout << "Best centroid score before: " << centroids[0].votes << std::endl;
//std::cout << "Best centroid score after: " << centroids[0].votes << std::endl;
// end normalize centroids
//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;
if(filterOn)
{
std::cout << "Best centroid score: " << centroids[0].votes << std::endl;
std::cout << "Best centroid score(normalized): " << (float) centroids[0].votes/totalModelVotes << std::endl;
clusters.reserve(centroids.size());
for(size_t c=0; c < centroids.size(); ++c)
//for(size_t c=0; c < 1; ++c)
{
clusters.push_back(cluster(pose (centroids[c].votes, transformation(centroids[c].rotation(),centroids[c].translation() ) ),centroids[c].poses ) );
}
clusterClusters(clusters);
std::sort(clusters.begin(), clusters.end(), cluster::compare);
}
else
{
std::sort(centroids.begin(), centroids.end(), poseCluster::compare);
//clusters.push_back(cluster(pose (centroids[0].votes, transformation(centroids[0].rotation(),centroids[0].translation() ) ),centroids[0].poses )); //METER
for(size_t c=0; c < centroids.size(); ++c) //TIRAR DEPOIS DOS TESTS
//for(size_t c=0; c < 1; ++c)
{
clusters.push_back(cluster(pose (centroids[c].votes, transformation(centroids[c].rotation(),centroids[c].translation() ) ),centroids[c].poses ) );
}
}
for(size_t c=0; c < clusters.size(); ++c)
{
clusters[c].normalizeVotes(model->totalSurfaceArea/totalModelVotes); // normalize votes weighs by the argument factor
//std::cout << "centroid poses:" << centroids[c].poses.size()<< "centroid score after: " << centroids[c].votes << std::endl;
}
/*for(size_t p=0; p< 1; ++p)
{
Eigen::Vector3f eulerAngles;
objectModel::quaternionToEuler(clusters[p].meanPose.transform.rotation, eulerAngles[0], eulerAngles[1], eulerAngles[2]);
float x,y,z;
std::cout << "1. EULER ANGLES: "<< std::endl << eulerAngles.transpose()*RAD_TO_DEG << std::endl;
pcl::getTranslationAndEulerAngles (clusters[p].meanPose.getTransformation(), x, y, z, eulerAngles[0], eulerAngles[1], eulerAngles[2]);
std::cout << "2. EULER ANGLES: "<< std::endl << eulerAngles.transpose()*RAD_TO_DEG << std::endl << std::endl;
}*/
return clusters;
}
