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;
}
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;
}
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;
}
}
