int FPCSRegistrationTools::FindCongruentBases(
KDTree* tree,
float delta,
const CCVector3* base[4],
std::vector<Base>& results)
{
unsigned i, j, a, b;
float r1, r2, d1, d2;
CCVector3 e, r, s, u, v;
const CCVector3 *p0, *p1, *p2, *p3, *q0, *q1;
std::vector<unsigned> pointsIndexes;
SimpleCloud tmpCloud1, tmpCloud2;
Base quad;
GenericIndexedCloud *cloud;
std::vector<IndexPair> match, pairs1, pairs2;
IndexPair idxPair;
//Compute reference base invariants (r1, r2)
p0 = base[0];
p1 = base[1];
p2 = base[2];
p3 = base[3];
e = *p1-*p0;
d1 = e.norm();
e = *p3-*p2;
d2 = e.norm();
if(!LinesIntersections(*p0, *p1, *p2, *p3, e, r1, r2))
return 0;
//Find all pairs which are d1-appart and d2-appart
cloud = tree->GetAssociatedCloud();
unsigned count = cloud->size();
pointsIndexes.reserve(count);
if (pointsIndexes.capacity() < count) //not enough memory
return -1;
pointsIndexes.clear();
for(i=0; i<count; i++)
{
q0 = cloud->getPoint(i);
idxPair.a = i;
//Extract all points from the cloud which are d1-appart (up to delta) from q0
tree->FindPointsLyingToDistance(q0->u, d1, delta, pointsIndexes);
for(j=0; j<pointsIndexes.size(); j++)
{
//As ||pi-pj|| = ||pj-pi||, we only take care of pairs that verify i<j
if(pointsIndexes[j]>i)
{
idxPair.b = pointsIndexes[j];
pairs1.push_back(idxPair);
}
}
pointsIndexes.clear();
//Extract all points from the cloud which are d2-appart (up to delta) from q0
tree->FindPointsLyingToDistance(q0->u, d2, delta, pointsIndexes);
for(j=0; j<pointsIndexes.size(); j++)
{
if(pointsIndexes[j]>i)
{
idxPair.b = pointsIndexes[j];
pairs2.push_back(idxPair);
}
}
pointsIndexes.clear();
}
//Select among the pairs the ones that can be congruent to the base "base"
count = pairs1.size();
if (!tmpCloud1.reserve(count*2)) //not enough memory
return -2;
for(i=0; i<count; i++)
{
//generate the two intermediate points from r1 in pairs1[i]
q0 = cloud->getPoint(pairs1[i].a);
q1 = cloud->getPoint(pairs1[i].b);
e.x = q0->x+r1*(q1->x-q0->x);
e.y = q0->y+r1*(q1->y-q0->y);
e.z = q0->z+r1*(q1->z-q0->z);
tmpCloud1.addPoint(e);
e.x = q1->x+r1*(q0->x-q1->x);
e.y = q1->y+r1*(q0->y-q1->y);
e.z = q1->z+r1*(q0->z-q1->z);
tmpCloud1.addPoint(e);
}
count = pairs2.size();
if (!tmpCloud2.reserve(count*2)) //not enough memory
return -3;
for(i=0; i<count; i++)
{
//generate the two intermediate points from r2 in pairs2[i]
q0 = cloud->getPoint(pairs2[i].a);
q1 = cloud->getPoint(pairs2[i].b);
e.x = q0->x+r2*(q1->x-q0->x);
e.y = q0->y+r2*(q1->y-q0->y);
e.z = q0->z+r2*(q1->z-q0->z);
tmpCloud2.addPoint(e);
e.x = q1->x+r2*(q0->x-q1->x);
e.y = q1->y+r2*(q0->y-q1->y);
e.z = q1->z+r2*(q0->z-q1->z);
tmpCloud2.addPoint(e);
}
//build kdtree for nearest neighbour fast research
KDTree *intermediatesTree = new KDTree();
if (!intermediatesTree->BuildFromCloud(&tmpCloud1))
{
delete intermediatesTree;
return -4;
}
//Find matching (up to delta) intermediate points in tmpCloud1 and tmpCloud2
count = tmpCloud2.size();
match.reserve(count);
if (match.capacity() < count) //not enough memory
{
delete intermediatesTree;
return -5;
}
for(i=0; i<count; i++)
{
q0 = tmpCloud2.getPoint(i);
if(intermediatesTree->FindNearestNeighbour(q0->u, a, delta))
{
idxPair.a = i;
idxPair.b = a;
match.push_back(idxPair);
}
}
//Find bases from matching intermediate points indexes
results.clear();
count = match.size();
if(count>0)
{
results.reserve(count);
if (results.capacity() < count) //not enough memory
{
delete intermediatesTree;
return -6;
}
for(i=0; i<count; i++)
{
a = match[i].a / 2;
b = match[i].b / 2;
if((match[i].b%2) == 0)
{
quad.a = pairs1[b].a;
quad.b = pairs1[b].b;
}
else
{
quad.a = pairs1[b].b;
quad.b = pairs1[b].a;
}
if((match[i].a%2) == 0)
{
quad.c = pairs2[a].a;
quad.d = pairs2[a].b;
}
else
{
quad.c = pairs2[a].b;
quad.d = pairs2[a].a;
}
results.push_back(quad);
}
}
delete intermediatesTree;
tmpCloud1.clear();
tmpCloud2.clear();
return (int)results.size();
}
