AR_Process::AR_Process(void)
{
// State Equation
F.resize(1,1);
F(0,0) = 0.8;
f.resize(1);
f(0) = 0.;
G.resize(1,1);
G.identity();
Qw.resize(1);
Qw(0,0)= 0.1;
// Observation noise
H.resize(1,1);
H(0,0) = 1;
h.resize(1);
h(0) = 0.;
Qv.resize(1);
Qv(0,0)=1;
// Init state
X0.resize(1);
X0(0) = 10.;
R0.resize(1);
R0.zero();
}
Van_Der_Pol::Van_Der_Pol(void)
{
lambda = 3.;
Qw.resize(1);
Qw(0,0)= 1.;
Qv.resize(1);
Qv(0,0)=0.1;
X0.resize(2);
X0(0) = 0.5;
X0(1) = 0.5;
R0.resize(2);
R0.zero();
R0(0,0)=0.;
R0(1,1)=.1;
Ts=.1;
}
/*!
Method which enables to compute a NURBS curve passing through a set of data points.
The result of the method is composed by a knot vector, a set of control points and a set of associated weights.
\param l_crossingPoints : The list of data points which have to be interpolated.
\param l_p : Degree of the NURBS basis functions. This value need to be > 0.
\param l_knots : The knot vector
\param l_controlPoints : the list of control points.
\param l_weights : the list of weights.
*/
void vpNurbs::globalCurveInterp(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
{
if (l_p == 0) {
//vpERROR_TRACE("Bad degree of the NURBS basis functions");
throw(vpException(vpException::badValue,
"Bad degree of the NURBS basis functions")) ;
}
l_knots.clear();
l_controlPoints.clear();
l_weights.clear();
unsigned int n = (unsigned int)l_crossingPoints.size()-1;
unsigned int m = n+l_p+1;
double d = 0;
for(unsigned int k=1; k<=n; k++)
d = d + distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1);
//Compute ubar
std::vector<double> ubar;
ubar.push_back(0.0);
for(unsigned int k=1; k<n; k++)
ubar.push_back(ubar[k-1]+distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1)/d);
ubar.push_back(1.0);
//Compute the knot vector
for(unsigned int k = 0; k <= l_p; k++)
l_knots.push_back(0.0);
double sum = 0;
for(unsigned int k = 1; k <= l_p; k++)
sum = sum + ubar[k];
//Centripetal method
for(unsigned int k = 1; k <= n-l_p; k++)
{
l_knots.push_back(sum/l_p);
sum = sum - ubar[k-1] + ubar[l_p+k-1];
}
for(unsigned int k = m-l_p; k <= m; k++)
l_knots.push_back(1.0);
vpMatrix A(n+1,n+1);
vpBasisFunction* N;
for(unsigned int i = 0; i <= n; i++)
{
unsigned int span = findSpan(ubar[i], l_p, l_knots);
N = computeBasisFuns(ubar[i], span, l_p, l_knots);
for (unsigned int k = 0; k <= l_p; k++) A[i][span-l_p+k] = N[k].value;
delete[] N;
}
//vpMatrix Ainv = A.inverseByLU();
vpMatrix Ainv;
A.pseudoInverse(Ainv);
vpColVector Qi(n+1);
vpColVector Qj(n+1);
vpColVector Qw(n+1);
for (unsigned int k = 0; k <= n; k++)
{
Qi[k] = l_crossingPoints[k].get_i();
Qj[k] = l_crossingPoints[k].get_j();
}
Qw = 1;
vpColVector Pi = Ainv*Qi;
vpColVector Pj = Ainv*Qj;
vpColVector Pw = Ainv*Qw;
vpImagePoint pt;
for (unsigned int k = 0; k <= n; k++)
{
pt.set_ij(Pi[k],Pj[k]);
l_controlPoints.push_back(pt);
l_weights.push_back(Pw[k]);
}
}
inline void converter<point_t>::knot_insertion(point_container_t& P,
std::multiset<value_type>& knots,
std::size_t order,
value_type t) const {
typedef typename point_t::value_type value_type;
// copy knotvector for subscript [] access
std::vector<value_type> kv_cpy(knots.begin(), knots.end());
// get parameter
std::size_t p = order - 1; // degree
std::size_t s = knots.count(t); // multiplicity
std::size_t r = std::max(std::size_t(0), p - s); // number of insertions
// get knotspan
std::size_t k = std::distance(knots.begin(), knots.upper_bound(t));
std::size_t np = P.size(); // number of control points
// start computation
std::size_t nq = np + r;
// helper arrays
std::vector<point_t> Qw(nq);
std::vector<point_t> Rw(p - s + 1);
// copy unaffected points and transform into homogenous coords
for (size_t i = 0; i <= k - p; ++i) {
Qw[i] = P[i].as_homogenous();
}
for (size_t i = k - s - 1; i <= np - 1; ++i) {
Qw[i + r] = P[i].as_homogenous();
}
// helper points
for (size_t i = 0; i <= p - s; ++i) {
Rw[i] = P[k - p + i - 1].as_homogenous();
}
// do knot insertion itself
std::size_t L = 0;
for (std::size_t j = 1; j <= r; ++j) {
L = k - p + j;
for (std::size_t i = 0; i <= p - j - s; ++i) {
value_type alpha =
(t - kv_cpy[L + i - 1]) / (kv_cpy[i + k] - kv_cpy[L + i - 1]);
Rw[i] = alpha * Rw[i + 1] + value_type(1.0 - alpha) * Rw[i];
}
Qw[L - 1] = Rw[0];
Qw[k + r - j - s - 1] = Rw[p - j - s];
}
// insert knots
for (std::size_t i = 0; i < r; ++i) {
knots.insert(t);
}
// copy new control points
P.clear();
// transform back to euclidian space
for (typename std::vector<point_t>::iterator i = Qw.begin(); i != Qw.end();
++i) {
P.push_back((*i).as_euclidian());
}
}
