bool AlignPair::Align(
A2Grid &u,
A2GridVert &uv,
const Matrix44d &in, // trasformazione Iniziale (che porta i punti di mov su fix)
Matrix44d &out, // trasformazione calcolata
vector<Point3d> &Pfix, // vertici corrispondenti su src (rossi)
vector<Point3d> &Nfix, // normali corrispondenti su src (rossi)
vector<Point3d> &OPmov, // vertici scelti su trg (verdi) prima della trasformazione in ingresso (Original Point Target)
vector<Point3d> &ONmov, // normali scelti su trg (verdi)
Histogramf &H,
AlignPair::Stat &as)
{
vector<char> beyondCntVec; // vettore per marcare i movvert che sicuramente non si devono usare
// ogni volta che un vertice si trova a distanza oltre max dist viene incrementato il suo contatore;
// i movvert che sono stati scartati piu' di MaxCntDist volte non si guardano piu';
const int maxBeyondCnt=3;
vector< Point3d > movvert;
vector< Point3d > movnorm;
vector<Point3d> Pmov; // vertici scelti dopo la trasf iniziale
status=SUCCESS;
int tt0=clock();
out=in;
int i;
double CosAngleThr=cos(ap.MaxAngleRad);
double StartMinDist=ap.MinDistAbs;
int tt1=clock();
int ttsearch=0;
int ttleast=0;
int nc=0;
as.clear();
as.StartTime=clock();
beyondCntVec.resize(mov->size(),0);
/**************** BEGIN ICP LOOP ****************/
do
{
Stat::IterInfo ii;
Box3d movbox;
InitMov(movvert,movnorm,movbox,out);
H.SetRange(0,StartMinDist,512,2.5);
Pfix.clear();
Nfix.clear();
Pmov.clear();
OPmov.clear();
ONmov.clear();
int tts0=clock();
ii.MinDistAbs=StartMinDist;
int LocSampleNum=min(ap.SampleNum,int(movvert.size()));
Box3d fixbox;
if(u.Empty()) fixbox = uv.bbox;
else fixbox = u.bbox;
for(i=0;i<LocSampleNum;++i)
{
if( beyondCntVec[i] < maxBeyondCnt )
if(! fixbox.IsIn(movvert[i]) )
beyondCntVec[i]=maxBeyondCnt+1;
else
{
double error=StartMinDist;
Point3d closestPoint, closestNormal;
double maxd= StartMinDist;
ii.SampleTested++;
if(u.Empty()) // using the point cloud grid
{
A2Mesh::VertexPointer vp = tri::GetClosestVertex(*fix,uv,movvert[i], maxd, error);
if(error>=StartMinDist) {
ii.DistanceDiscarded++; ++beyondCntVec[i]; continue;
}
if(movnorm[i].dot(vp->N()) < CosAngleThr) {
ii.AngleDiscarded++; continue;
}
closestPoint=vp->P();
closestNormal=vp->N();
}
else // using the standard faces and grid
{
A2Mesh::FacePointer f=vcg::tri::GetClosestFaceBase<vcg::AlignPair::A2Mesh, vcg::AlignPair::A2Grid >(*fix, u, movvert[i], maxd, error, closestPoint);
if(error>=StartMinDist) {
ii.DistanceDiscarded++; ++beyondCntVec[i]; continue;
}
if(movnorm[i].dot(f->N()) < CosAngleThr) {
ii.AngleDiscarded++; continue;
}
Point3d ip;
InterpolationParameters<A2Face,double>(*f,f->N(),closestPoint, ip);
const double IP_EPS = 0.00001;
// If ip[i] == 0 it means that we are on the edge opposite to i
if( (fabs(ip[0])<=IP_EPS && f->IsB(1)) || (fabs(ip[1])<=IP_EPS && f->IsB(2)) || (fabs(ip[2])<=IP_EPS && f->IsB(0)) ){
ii.BorderDiscarded++; continue;
}
closestNormal = f->N();
}
// The sample was accepted. Store it.
Pmov.push_back(movvert[i]);
OPmov.push_back((*mov)[i].P());
ONmov.push_back((*mov)[i].N());
Nfix.push_back( closestNormal );
Pfix.push_back( closestPoint );
H.Add(float(error));
ii.SampleUsed++;
}
} // End for each pmov
int tts1=clock();
//printf("Found %d pairs\n",(int)Pfix.size());
if(!ChoosePoints(Pfix,Nfix,Pmov,OPmov,ap.PassHiFilter,H))
if(int(Pfix.size())<ap.MinPointNum)
{
status = TOO_FEW_POINTS;
ii.Time=clock();
as.I.push_back(ii);
return false;
}
Matrix44d newout;
switch(ap.MatchMode) {
case AlignPair::Param::MMSimilarity : ComputeRotoTranslationScalingMatchMatrix(newout,Pfix,OPmov); break;
case AlignPair::Param::MMRigid : ComputeRigidMatchMatrix(Pfix,OPmov,newout); break;
default :
status = UNKNOWN_MODE;
ii.Time=clock();
as.I.push_back(ii);
return false;
}
// double sum_before=0;
// double sum_after=0;
// for(unsigned int iii=0;iii<Pfix.size();++iii)
// {
// sum_before+=Distance(Pfix[iii], out*OPmov[iii]);
// sum_after+=Distance(Pfix[iii], newout*OPmov[iii]);
// }
// //printf("Distance %f -> %f\n",sum_before/double(Pfix.size()),sum_after/double(Pfix.size()) ) ;
// le passate successive utilizzano quindi come trasformazione iniziale questa appena trovata.
// Nei prossimi cicli si parte da questa matrice come iniziale.
out=newout;
assert(Pfix.size()==Pmov.size());
int tts2=clock();
ttsearch+=tts1-tts0;
ttleast +=tts2-tts1;
ii.pcl50=H.Percentile(.5);
ii.pclhi=H.Percentile(ap.PassHiFilter);
ii.AVG=H.Avg();
ii.RMS=H.RMS();
ii.StdDev=H.StandardDeviation();
ii.Time=clock();
as.I.push_back(ii);
nc++;
// The distance of the next points to be considered is lowered according to the <ReduceFactor> parameter.
// We use 5 times the <ReduceFactor> percentile of the found points.
if(ap.ReduceFactorPerc<1) StartMinDist=max(ap.MinDistAbs*ap.MinMinDistPerc, min(StartMinDist,5.0*H.Percentile(ap.ReduceFactorPerc)));
}
while (
nc<=ap.MaxIterNum &&
H.Percentile(.5) > ap.TrgDistAbs &&
(nc<ap.EndStepNum+1 || ! as.Stable(ap.EndStepNum) )
);
/**************** END ICP LOOP ****************/
int tt2=clock();
Matrix44d ResCopy=out;
Point3d scv,shv,rtv,trv;
Decompose(ResCopy,scv,shv,rtv,trv);
if(math::Abs(1-scv[0])>ap.MaxScale || math::Abs(1-scv[1])>ap.MaxScale || math::Abs(1-scv[2])>ap.MaxScale ) {
status = TOO_MUCH_SCALE;
return false;
}
if(shv[0]>ap.MaxShear || shv[1]>ap.MaxShear || shv[2]>ap.MaxShear ) {
status = TOO_MUCH_SHEAR;
return false;
}
printf("Grid %i %i %i - fn %i\n",u.siz[0],u.siz[1],u.siz[2],fix->fn);
printf("Init %8.3f Loop %8.3f Search %8.3f least sqrt %8.3f\n",
float(tt1-tt0)/CLOCKS_PER_SEC, float(tt2-tt1)/CLOCKS_PER_SEC,
float(ttsearch)/CLOCKS_PER_SEC,float(ttleast)/CLOCKS_PER_SEC );
return true;
}
