示例#1
0
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();
}
示例#2
0
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;

}
示例#3
0
/*!
  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]);
  }
}
示例#4
0
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());
  }
}