void load_grid(const char *grid_f,const char *mask_f)
{
if(verbose)
std::cout<<"Loading grid:"<<grid_f<<" and mask:"<<mask_f<<" ... ";
minc::def3d::Pointer grid (minc::def3d::New());
minc::mask3d::Pointer mask(minc::mask3d::New());
load_minc<minc::def3d>(grid_f, grid);
load_minc<minc::mask3d>(mask_f, mask);
minc::def3d_iterator it(grid, grid->GetRequestedRegion() );
int cnt=0;
for(it.GoToBegin();!it.IsAtEnd();++it)
{
tag_point p;
minc::def3d::IndexType idx=it.GetIndex();
grid->TransformIndexToPhysicalPoint(idx,p);
minc::mask3d::IndexType idx_m;
if(!mask->TransformPhysicalPointToIndex(p,idx_m) || !mask->GetPixel(idx_m)) continue;
ideal.push_back(p);
p[0]+=it.Value()[0];
p[1]+=it.Value()[1];
p[2]+=it.Value()[2];
measured.push_back(p);
cnt++;
}
if(verbose)
std::cout<<cnt<<" nodes"<<std::endl;
}
void calculate_basis(void)
{
if(!cache_basis) return;
basis_x.resize(ideal.size());
//basis_y.resize(ideal.size());
//basis_z.resize(ideal.size());
tag_points::const_iterator i=ideal.begin();
fittings::iterator mx=basis_x.begin();
//fittings::iterator my=basis_y.begin();
//fittings::iterator mz=basis_z.begin();
basis_vector bas(order);
for(; i!=ideal.end(); i++, mx++/*,my++,mz++*/)
{
mx->resize(order);
//my->resize(order);
//mz->resize(order);
//all basis are the same
fun_x.generate_basis(bas,order,*i);
for(int k=0;k<order;k++)
{
(*mx)[k]=bas[k];
}
}
}
bool remove_outliers(void)
{
minc::MNK_Gauss_Polinomial pol_x(order);
minc::MNK_Gauss_Polinomial pol_y(order);
minc::MNK_Gauss_Polinomial pol_z(order);
distances.resize(ideal.size());
tag_points::const_iterator j=measured.begin();
tag_points::const_iterator i=ideal.begin();
fittings::const_iterator bx=basis_x.begin();
int k=0;
int max_k=-1;
int cnt=0;
max_distance=0.0;
sd=0.0;
basis_vector bas_x(order);
for(;i!=ideal.end();i++, j++, k++)
{
if(cache_basis)
{
bas_x=*bx;
} else {
fun_x.generate_basis(bas_x,order,*i);
}
cnt++;
tag_point moved;
moved[0]=pol_x.fit(bas_x, coeff[0]);
moved[1]=pol_y.fit(bas_x, coeff[1]);
moved[2]=pol_z.fit(bas_x, coeff[2]);
double d=(*j).SquaredEuclideanDistanceTo(moved);
distances[k]=d;
if(!mask[k]&&d>max_distance) { max_distance=d; max_k=k;}
if(cache_basis)
{
bx++;
}
}
max_distance=sqrt(max_distance);
build_index();
sd=evaluate_distance(keep);
if(verbose)
cout<<sd<<":"<<max_distance<<"\t";
return true;
}
double evaluate_distance(double keep)
{
int size=ideal.size()*keep;
double sd=0.0;
for(int i=0;i<ideal.size();i++)
{
if(i<size) {
sd+=distances[index[i]];
mask[index[i]]=false;
} else mask[index[i]]=true;
}
sd/=size;
return sqrt(sd);
}
bool fit_tags(bool condition=false)
{
_last_distance=0.0;
if(measured.size()!=ideal.size())
REPORT_ERROR("Mismatching number of tags!");
mask.resize(ideal.size(),false);
calculate_basis();
int i=0;
do {
fit_coeff(condition);
i++;
//std::cout<<"\t"<<i;
} while(remove_outliers() && i<_max_iterations);
return sd<max_dev;
}
bool remove_outliers(void)
{
/* minc::MNK_Gauss < tag_point, basis_functions_x> pol_x(order);
minc::MNK_Gauss < tag_point, basis_functions_y> pol_y(order);
minc::MNK_Gauss < tag_point, basis_functions_z> pol_z(order);*/
minc::MNK_Gauss_Polinomial pol_x(order);
minc::MNK_Gauss_Polinomial pol_y(order);
minc::MNK_Gauss_Polinomial pol_z(order);
distances.resize(ideal.size());
tag_points::const_iterator j=measured.begin();
tag_points::const_iterator i=ideal.begin();
fittings::const_iterator bx=basis_x.begin();
fittings::const_iterator by=basis_y.begin();
fittings::const_iterator bz=basis_z.begin();
int k=0;
int max_k=-1;
int cnt=0;
max_distance=0.0;
sd=0.0;
for(;i!=ideal.end();i++, j++, k++, bx++, by++, bz++)
{
//if(mask[k]) continue;
cnt++;
tag_point moved;
moved[0]=pol_x.fit(*bx, coeff[0]);
moved[1]=pol_y.fit(*by, coeff[1]);
moved[2]=pol_z.fit(*bz, coeff[2]);
double d=(*j).SquaredEuclideanDistanceTo(moved);
distances[k]=d;
//sd+=d;
if(!mask[k]&&d>max_distance) { max_distance=d; max_k=k;}
}
max_distance=sqrt(max_distance);
build_index();
sd=evaluate_distance(keep);
if(verbose)
cout<<sd<<":"<<max_distance<<"\t";
return true;
}
void calculate_basis(void)
{
basis_x.resize(ideal.size());
basis_y.resize(ideal.size());
basis_z.resize(ideal.size());
//tag_points::const_iterator j=measured.begin();
tag_points::const_iterator i=ideal.begin();
fittings::iterator mx=basis_x.begin();
fittings::iterator my=basis_y.begin();
fittings::iterator mz=basis_z.begin();
basis_vector bas(order);
for(; i!=ideal.end(); i++, mx++,my++,mz++)
{
mx->resize(order);
my->resize(order);
mz->resize(order);
//all basis are the same
fun_x.generate_basis(bas,order,*i);
for(int k=0;k<order;k++)
{
/*(*mx)[k]=fun_x(k,*i);
(*my)[k]=fun_y(k,*i);
(*mz)[k]=fun_z(k,*i);*/
(*mx)[k]=bas[k];
(*my)[k]=bas[k];
(*mz)[k]=bas[k];
}
/*
fun_x.generate_basis(*mx,order,*i);
fun_y.generate_basis(*my,order,*i);
fun_z.generate_basis(*mz,order,*i);*/
}
}
void reset_index(void)
{
index.resize(ideal.size());
for(int i=0;i<ideal.size();i++)
index[i]=i;
}
void fit_coeff(bool condition=false)
{
if(ideal.size()!=measured.size())
REPORT_ERROR("Mismatching number of points!");
if(ideal.size()!=mask.size())
REPORT_ERROR("Mismatching number of points!");
reset_index();
coeff.resize(3);
coeff[0].resize(order);
coeff[1].resize(order);
coeff[2].resize(order);
/* minc::MNK_Gauss_opt<tag_point> pol_x(limit_linear?order-3:order);
minc::MNK_Gauss_opt<tag_point> pol_y(limit_linear?order-3:order);
minc::MNK_Gauss_opt<tag_point> pol_z(limit_linear?order-3:order);*/
minc::MNK_Gauss_Polinomial pol_x(limit_linear?order-3:order);
minc::MNK_Gauss_Polinomial pol_y(limit_linear?order-3:order);
minc::MNK_Gauss_Polinomial pol_z(limit_linear?order-3:order);
tag_points::const_iterator j=measured.begin();
tag_points::const_iterator i=ideal.begin();
fitting_mask::const_iterator m=mask.begin();
fittings::const_iterator bx=basis_x.begin();
fittings::const_iterator by=basis_y.begin();
fittings::const_iterator bz=basis_z.begin();
for(; i!=ideal.end(); i++, j++, m++, bx++, by++, bz++)
{
if(*m) continue;
//this is a quick hack
if(limit_linear)
{
//TODO: fix indexing
pol_x.accumulate((*bx), (*j)[0]-(*i)[0]);
pol_y.accumulate((*by), (*j)[1]-(*i)[1]);
pol_z.accumulate((*bz), (*j)[2]-(*i)[2]);
} else {
pol_x.accumulate(*bx, (*j)[0]);
pol_y.accumulate(*by, (*j)[1]);
pol_z.accumulate(*bz, (*j)[2]);
}
}
//if(condition)
//{
double cond_x,cond_y,cond_z;
if(limit_linear)
{
cond_x=pol_x.solve_unstable(coeff[0],0.01,verbose);
cond_y=pol_y.solve_unstable(coeff[1],0.01,verbose);
cond_z=pol_z.solve_unstable(coeff[2],0.01,verbose);
coeff[0][1]=coeff[1][2]=coeff[2][0]=1.0;
coeff[0][0]=coeff[0][2]=0;
coeff[1][0]=coeff[1][1]=0;
coeff[2][1]=coeff[2][2]=0;
} else {
cond_x=pol_x.solve_unstable(coeff[0],0.01,verbose);
cond_y=pol_y.solve_unstable(coeff[1],0.01,verbose);
cond_z=pol_z.solve_unstable(coeff[2],0.01,verbose);
}
if(condition)
{
cout<<"cond_x="<<cond_x<<"\t";
cout<<"cond_y="<<cond_y<<"\t";
cout<<"cond_z="<<cond_z<<endl;
}
}
void fit_coeff(bool condition=false)
{
if(ideal.size()!=measured.size())
REPORT_ERROR("Mismatching number of points!");
if(ideal.size()!=mask.size())
REPORT_ERROR("Mismatching number of points!");
reset_index();
coeff.resize(3);
coeff[0].resize(order);
coeff[1].resize(order);
coeff[2].resize(order);
minc::MNK_Gauss_Polinomial pol_x(limit_linear?order-3:order);
minc::MNK_Gauss_Polinomial pol_y(limit_linear?order-3:order);
minc::MNK_Gauss_Polinomial pol_z(limit_linear?order-3:order);
tag_points::const_iterator j=measured.begin();
tag_points::const_iterator i=ideal.begin();
fitting_mask::const_iterator m=mask.begin();
fittings::const_iterator bx=basis_x.begin();
basis_vector bas_x(order);
for(; i!=ideal.end(); i++, j++, m++)
{
if(!*m)
{
if(cache_basis)
{
bas_x=*bx;
} else {
fun_x.generate_basis(bas_x,order,*i);
}
if(limit_linear)
{
//TODO: fix indexing
basis_vector _bas(bas_x.begin()+3,bas_x.end());
pol_x.accumulate(_bas, (*j)[0]-(*i)[0]);
pol_y.accumulate(_bas, (*j)[1]-(*i)[1]);
pol_z.accumulate(_bas, (*j)[2]-(*i)[2]);
} else {
pol_x.accumulate(bas_x, (*j)[0]);
pol_y.accumulate(bas_x, (*j)[1]);
pol_z.accumulate(bas_x, (*j)[2]);
}
}
if(cache_basis)
{
bx++;
}
}
double cond_x,cond_y,cond_z;
if(limit_linear)
{
fittings _coeff(3);
_coeff[0].resize(order-3);
_coeff[1].resize(order-3);
_coeff[2].resize(order-3);
cond_x=pol_x.solve_unstable(_coeff[0],0.01,verbose);
cond_y=pol_y.solve_unstable(_coeff[1],0.01,verbose);
cond_z=pol_z.solve_unstable(_coeff[2],0.01,verbose);
coeff[0][1]=coeff[1][2]=coeff[2][0]=1.0;
coeff[0][0]=coeff[0][2]=0;
coeff[1][0]=coeff[1][1]=0;
coeff[2][1]=coeff[2][2]=0;
for(size_t _j=3; _j<order; _j++) {
for(size_t _k=0;_k<3;_k++)
coeff[_k][_j]=_coeff[_k][_j-3];
}
} else {
cond_x=pol_x.solve_unstable(coeff[0],0.01,verbose);
cond_y=pol_y.solve_unstable(coeff[1],0.01,verbose);
cond_z=pol_z.solve_unstable(coeff[2],0.01,verbose);
}
if(condition)
{
cout<<"cond_x="<<cond_x<<"\t";
cout<<"cond_y="<<cond_y<<"\t";
cout<<"cond_z="<<cond_z<<endl;
}
}
