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