double get(int_8 i) {
if (i>=0 && i<data.size()) return data[i];
else {
cout << "Var-get: out of bounds" << endl;
exit(1);
}
}
double* operator[](int_8 const &index) {
if (index>=0 && index<data.size()) return &data[index];
else {
cout << "Var: out of bounds " << endl;
exit(1);
}
}
ScalarType GraphNodeCtr::deformableValue(VecX& srNeigDis,VecX & resNeigDis)
{
ScalarType totle = 0.0;
ScalarType molecule = 0.0;
ScalarType denominator = 1.0;
IndexType neigNum = resNeigDis.rows();
for (IndexType v_iter = 1;v_iter < neigNum; v_iter++)
{
denominator = srNeigDis(v_iter,0);
molecule = abs(resNeigDis(v_iter,0) - denominator);
assert(denominator > 1e-7);
totle += molecule/denominator;
}
return totle/(neigNum - 1);
}
int KalmanFilter::init(VecX mu, MatX R, MatX C, MatX Q)
{
int nb_states;
nb_states = mu.size();
this->initialized = true;
this->mu = mu;
this->B = MatX::Zero(nb_states, nb_states);
this->R = R;
this->C = C;
this->Q = Q;
this->S = MatX::Identity(nb_states, nb_states);
this->I = MatX::Identity(nb_states, nb_states);
this->K = MatX::Zero(nb_states, nb_states);
this->mu_p = VecX::Zero(nb_states);
this->S_p = MatX::Zero(nb_states, nb_states);
return 0;
}
void BiCGSTAB() {
int iter = 0;
double rho, alpha, beta, omega, rho_new;
VecX<double> r(A.data.size()), r0(A.data.size()), p(A.data.size());
VecX<double> t(A.data.size()), v(A.data.size()), s(A.data.size());
err = 0;
//check if t is initialized as zero;
//cout<< t << endl;
*error = (b - A*(*x));
if (error->abs() < tol) { err = tol; it = 0; return; }
r0 = *error;
rho = 1; alpha = 1; omega = 1;
for (; iter < itmax; ++iter) {
rho_new = r0*(*error);
beta = (rho_new/rho)*(alpha/omega);
rho = rho_new;
p = (*error) + beta*(p - omega*v);
v = A*p;
//cout << " r0v: " << r0*v << " p: " << p << endl;
alpha = rho/(r0*v);
s = *error - alpha*v;
t = A*s;
//cout << " t: " << t << " s: " << s << endl;
omega = (t*s)/(t*t);
(*x) = (*x) + alpha*p + omega*s;
*error = s - omega*t;
err = sqrt(*error*(*error));
if (err < tol) break;
//if (iter%50 == 0)
//cout << "BiCGstab: " << iter << ": "<< log10(err) << endl;
}
it = iter;
//err = sqrt(rsnew);
//cout << "BiCGstab: " << iter << ": " << log10(err) << " DONE!" << endl;
}
void set(double a) { data.data.assign(data.size(), a); }
void clear() {
A.empty();
b.empty();
}
//-------------------------
// Methods
//-------------------------
void resize(int_8 const &a) {
N = a;
A.resize(a);
b.resize(a);
}
