Пример #1
0
  unsigned  long		count(const K& key)
  {

    unsigned long              tot = 0;
    typename std::list<Pair<K, V> >::iterator	debut = _liste.begin();
    typename std::list<Pair<K, V > >::iterator	fin = _liste.end();

    for (; debut != fin; ++debut)
      {
	if (key == debut->getKey())
	  tot++;
      }    
    return tot;
  }
Пример #2
0
  const std::list< Pair<K, V> >* find(const K& key)
  {
    std::list< Pair<K, V> >* new_liste = new std::list< Pair<K, V> >();
    typename std::list<Pair<K, V> >::iterator	debut = _liste.begin();
    typename std::list<Pair<K, V > >::iterator	fin = _liste.end();
        
    for (; debut != fin; ++debut)
      {
	if (debut->getKey() == key)
	  {
	    Pair<K, V>	p(debut->getKey(), debut->getValue());
	    new_liste->push_back(p);
	  }
      }
      return new_liste;
  }
Пример #3
0
void
ITM<ITM_TRAITS>::handleDeletions( node_type best, node_type second )
{
    observation_type & bestCentroid = graph_[best].centroid;
    observation_type & secondCentroid = graph_[second].centroid;

    // Don't know of other way to avoid iterator invalidation than to do this
    // twice.
    out_edge_iterator iChild;
    out_edge_iterator eChild;
    typename std::list<node_type> erase;

    for ( boost::tie( iChild, eChild ) = boost::out_edges( best, graph_ );
            iChild != eChild; ++iChild
        ) {
        node_type child = boost::target( *iChild, graph_ );
        if ( child != best ) { // Do not try to delete self-edges
            observation_type centroid = graph_[child].centroid;
            observation_type center = ( bestCentroid + centroid ) * 0.5;
            if (      distance_( center, secondCentroid )
                      < distance_( center, centroid )
               ) {
                erase.push_back( child );
            }
        }
    }

    typename std::list<node_type>::iterator iErase;
    typename std::list<node_type>::iterator eErase = erase.end();
    for ( iErase = erase.begin(); iErase != eErase; ++iErase ) {
        boost::remove_edge( best, *iErase, graph_ );
        boost::remove_edge( *iErase, best, graph_ );

        boost::tie( iChild, eChild ) = boost::out_edges( *iErase, graph_ );

        // The or condition is to handle self-edges
        if (   iChild == eChild
                || (    std::distance( iChild, eChild ) == 1
                        && boost::target( *iChild, graph_ ) == *iErase )
           ) {
            boost::clear_vertex( *iErase, graph_ );
            boost::remove_vertex( *iErase, graph_ );
        }
    }
}
Пример #4
0
	float do_kmeans( const std::vector<sample_type>& samples)
	{
		for( int iters = 0; iters < max_iters_; ++iters)
		{
			int num_swaps = assign_nearest_cluster( samples);
			
			if( num_swaps == 0)
				break;

			for( typename std::list<cluster_t>::iterator it( clusters_.begin()); it != clusters_.end(); ++it)
			{
				it->num_samples = 0;
				it->radius = traits_type::zero();
				
				for( int i = 0; i < samples.size(); ++i)
				{
					if( labels_[i] == &(*it))
						it->add_sample( samples[i]);
				}
				
				if( !it->empty())
					it->mean /= it->num_samples;

				for( int i = 0; i < samples.size(); ++i)
				{
					if( labels_[i] == &(*it))
						it->update_radius( samples[i]);
				}					
			}
			
			clusters_.erase( std::remove_if( clusters_.begin(), clusters_.end(),
											boost::bind( &kmeans_t::cluster_t::empty, _1)), clusters_.end());
			
			RAMEN_ASSERT( !clusters_.empty());
		}

		float max_radius = traits_type::zero();
		
		for( const_iterator it( begin()); it != end(); ++it)
			max_radius = std::max( max_radius, it->radius);
		
		return max_radius;
	}
Пример #5
0
template <typename Robot> double SAC<Robot>::J(typename std::list<typename Robot::State> const & list_s)
{
	typename std::list<typename Robot::State>::const_iterator itr_state=list_s.cbegin();
	typename std::list<typename Robot::Ref>::const_iterator itr_ref=list_refs.cbegin();

	double J=L(*itr_state, *itr_ref);
	itr_state++;
	std::advance(itr_ref,2);

	size_t N=list_s.size();

	for(int n=2; n<N; n++, itr_state++, std::advance(itr_ref,2))
		J+=2*L(*itr_state, *itr_ref);

	J+=L(*itr_state, *itr_ref);
	J=J*sim.dt/2.0+Phi(*itr_state, *itr_ref);

	return J;
}
Пример #6
0
 /**
  * @param[in] dp Center of 3D point (dp[0], dp[1], dp[2]).
  * @param[in] radius Radius of a sphere.
  * @param[out] result resutl of the points.
  */
 void find(const T &pnt, const double radius, typename std::list<T>& result) {
         if (this->_list.empty()) return;
         if (this->isLeaf()) {
                 typename std::list<T>::iterator iter;
                 const double sqr = radius * radius;
                 for (iter = this->_list.begin() ; iter != this->_list.end() ; iter++) {
                         double check = 0;
                         for ( size_t i = 0 ; i < Dim ; i++) check += (iter->operator[](i) - pnt[i])*(iter->operator[](i) - pnt[i]);
                         if ( check <= sqr )  result.push_back(*iter);
                 }
         } else {
                 const double p = this->_list.begin()->operator[](this->_dim);
                 const double x = pnt[this->_dim];
                 if ( fabs(x-p) <= radius ) {
                         this->_child[0].find(pnt, radius, result);
                         this->_child[1].find(pnt, radius, result);
                 } else {
                         if ( x < p )this->_child[0].find(pnt, radius, result);
                         else        this->_child[1].find(pnt, radius, result);
                 }
         }
         return;
 }
Пример #7
0
static void dispatch(int n, int m, double *x_ptr, double *c_ptr, double *d_ptr, int nlhs, mxArray *plhs[])
{
  // Read parameters
  typename KDTree<K, double>::points_type X;
  
  typename KDTree<K, double>::point_type v;
  
  for (int i = 0; i < n; ++i)
    {
      for (unsigned k = 0; k < K; ++k)
	v[k] = x_ptr[i + (n * k)];
      X.push_back(v);
    }

  typename KDTree<K, double>::points_type C;

  std::vector<double> R;

  for (int i = 0; i < m; ++i)
    {
      for (unsigned k = 0; k < K; ++k)
	v[k] = c_ptr[i + (m * k)];
      C.push_back(v);
      R.push_back(d_ptr[i]);
    }

  // Build kd-tree
  KDTree<K, double> kdtree(X);

  // Compute queries
  std::list<typename KDTree<K, double>::set_type > res_list;
  typename KDTree<K, double>::set_type res;
	
  for (int i = 0; i < m; ++i)
    {
      kdtree.ball_query(C[i], R[i], res);

      res_list.push_back(res);
			
      res.clear();
    }

  // Write output
  if (nlhs > 0)
    {
      const mwSize size[2] = {res_list.size(), 1};
      plhs[0] = mxCreateCellArray(2, size);
			
      int cnt = 0;
      for (typename std::list<typename KDTree<K, double>::set_type>::const_iterator it = res_list.begin(); it != res_list.end(); ++it, ++cnt)
	{
	  if (it->size())
	    {
	      mxArray * pArray = mxCreateDoubleMatrix(it->size(), 1, mxREAL);
	      double * p = mxGetPr(pArray);
				
	      for (typename KDTree<K, double>::set_type::const_iterator it2 = it->begin(); it2 != it->end(); ++it2)
		*p++ = it2->second + 1;
				
	      mxSetCell(plhs[0], cnt, pArray);
	    }
	}
    }

  if (nlhs > 1)
    {
      const mwSize size[2] = {res_list.size(), 1};
      plhs[1] = mxCreateCellArray(2, size);
			
      int cnt = 0;
      for (typename std::list<typename KDTree<K, double>::set_type>::const_iterator it = res_list.begin(); it != res_list.end(); ++it, ++cnt)
	{
	  if (it->size())
	    {
	      mxArray * pArray = mxCreateDoubleMatrix(it->size(), 1, mxREAL);
	      double * p = mxGetPr(pArray);
				
	      for (typename KDTree<K, double>::set_type::const_iterator it2 = it->begin(); it2 != it->end(); ++it2)
		*p++ = it2->first;
				
	      mxSetCell(plhs[1], cnt, pArray);
	    }
	}
    }
}
Пример #8
0
template <typename Robot> double SAC<Robot>::sac_ctrl(typename std::list<typename Robot::U> & list_u, typename Robot::U umin, typename Robot::U umax, typename Robot::U du, typename Robot::U u0)
{
	double J1=J0;

	typename std::list<typename Robot::U>::const_iterator itr_u1=list_u1.cbegin();
	typename std::list<typename Robot::State>::const_iterator itr_state=list_states.cbegin();
	typename std::list<typename Robot::coState>::const_iterator itr_costate=list_costates.cbegin();

	double Ti=params.Tc+params.Ts;

	params.alpha*=J0;

	double t;

	typename Robot::U u_curr;

	for(t=0; t<params.Tcal-1e-10; t+=sim.dt, itr_u1++, itr_state++, itr_costate++)
		list_u2.push_back(*itr_u1);

	if(list_u2.empty())
		u_curr=u0;
	else
		u_curr=list_u2.front();

	for(; t<Ti-1e-10; t+=sim.dt, itr_u1++, itr_state++, itr_costate++)
	{
		Eigen::Matrix<double,Robot::M,Robot::N> h=Robot::h(sys,*itr_state);
		Eigen::Matrix<double,Robot::N,1> gamma=h.transpose()*(itr_costate->costate);
		Eigen::Matrix<double,Robot::N,Robot::N> lamada=gamma*gamma.transpose();

		Eigen::Matrix<double,Robot::N,1> u=*itr_u1+(lamada+params.R).ldlt().solve(gamma*params.alpha);

		typename Robot::U u_min=u_curr-du;
		typename Robot::U u_max=u_curr+du;

		for(int i=0;i<Robot::N;i++)
		{
			if(u_min(i)<umin(i))
				u_min(i)=umin(i);

			if(u_max(i)>umax(i))
				u_max(i)=umax(i);
		}

		for(int i=0;i<Robot::N;i++)
			if(u(i)<u_min(i))
				u(i)=u_min(i);
			else
				if(u(i)>u_max(i))
					u(i)=u_max(i);

		list_u2.push_back(u);
		
		u_curr=u;
	}

	int index=-1;

	double Tc=params.Tc;
	double Tm=params.Tcal+Tc; 

	sim.init(list_states.front());
	for(int n=0; n<params.kmax; n++)
	{
		typename std::list<typename Robot::U>::const_iterator itr_u;

		itr_u=list_u2.begin();

		double t;
		if(!n)
			for(t=sim.dt*1e-3; t<Tm; t+=sim.dt, itr_u++)
				sim.update(*itr_u);
		else
			t=Tm+sim.dt*1e-3;
	
		itr_u=list_u1.begin();
		
		std::advance(itr_u, int(Tm/sim.dt+0.5));

		for(; t<params.Tp; t+=sim.dt, itr_u++)
			sim.update(*itr_u);

		double Jn=J(sim.get_states());
		
		if (Jn<J1)
		{
			J1=Jn;
			index=n;
		}

		Tc/=2.0;
		Tm=params.Tcal+Tc;
		sim.erase_last_n(int((params.Tp-Tm)/sim.dt+0.5));
	}

/*************************************************************************
	for(int n=0; n<params.kmax;n++, Tc/=2.0)
	{
		typename std::list<typename Robot::U>::const_iterator itr_u;

		double Tm=params.Tcal+Tc; 

		sim.init(list_states.front());
		
		itr_u=list_u2.begin();
	
		double t;
		for(t=sim.dt*1e-3; t<Tm; t+=sim.dt, itr_u++)
			sim.update(*itr_u);
	
		itr_u=list_u1.begin();
		
		std::advance(itr_u, int(Tm/sim.dt+0.5));

		for(; t<params.Tp; t+=sim.dt, itr_u++)
			sim.update(*itr_u);

		double Jn=J(sim.get_states());
		
		if (Jn<J1)
		{
			J1=Jn;
			index=n;
		}
	}	
*************************************************************************/

	if(list_u.size()<int(params.Tp/sim.dt))
		list_u=list_u1;

	if(index>=0)
	{
		double Tm=params.Tcal+params.Tc/pow(2.0,index);
		typename std::list<typename Robot::U>::iterator itr_u=list_u.begin();
		typename std::list<typename Robot::U>::const_iterator itr_u1=list_u1.begin();
		typename std::list<typename Robot::U>::const_iterator itr_u2=list_u2.begin();

		double t=0;
		for(; t<Tm-1e-10; t+=sim.dt, itr_u1++)
			*itr_u++=*itr_u2++;

		for(; t<params.Tp-1e-10; t+=sim.dt)
			*itr_u++=*itr_u1++;

//		std::cout<<index<<std::endl;
	}
//	else
//	{
//		std::cout<<"Failture"<<std::endl;
//	}
	return J1;
}
Пример #9
0
template <typename Robot> double SAC<Robot>::sac_ctrl(typename std::list<typename Robot::U> & list_u, typename Robot::U umin, typename Robot::U umax, double K, typename Robot::U u0)
{
	double J1=J0;

	typename std::list<typename Robot::U>::const_iterator itr_u1=list_u1.cbegin();
	typename std::list<typename Robot::State>::const_iterator itr_state=list_states.cbegin();
	typename std::list<typename Robot::coState>::const_iterator itr_costate=list_costates.cbegin();

	double Ti=params.Tc+params.Ts;

	params.alpha*=J0;

	double t;


	for(t=sim.dt*1e-3; t<params.Tcal; t+=sim.dt, itr_u1++, itr_state++, itr_costate++)
		list_u2.push_back(*itr_u1);

	typename Robot::U u_curr;
	if(list_u2.empty())
		u_curr=u0;
	else
		u_curr=list_u2.front();

	for(; t<Ti; t+=sim.dt, itr_u1++, itr_state++, itr_costate++)
	{
		Eigen::Matrix<double,Robot::M,Robot::N> h=Robot::h(sys,*itr_state);
		Eigen::Matrix<double,Robot::N,1> gamma=h.transpose()*(itr_costate->costate);
		Eigen::Matrix<double,Robot::N,Robot::N> lamada=gamma*gamma.transpose();

		Eigen::Matrix<double,Robot::N,1> u=*itr_u1+(lamada+params.R).ldlt().solve(gamma*params.alpha);

		for(int i=0;i<Robot::N;i++)
			if(u(i)<umin(i))
				u(i)=umin(i);
			else
				if(u(i)>umax(i))
					u(i)=umax(i);

		list_u2.push_back(u);
	}

	int index=-1;

	double Aht=exp(-K*sim.dt*0.5);
	std::list<typename Robot::U> list_ud;

	double Tc=params.Tc;

	for(int n=0; n<params.kmax;n++, Tc/=2.0)
	{
		typename std::list<typename Robot::Ref>::const_iterator itr_ref;
		typename std::list<typename Robot::U>::const_iterator itr_u;


		double Tm=params.Tcal+Tc; 

		sim.init(list_states.front());
		
		itr_u=list_u2.begin();
	
		std::list<typename Robot::U> list_ut;

		double t=sim.dt*1e-3;
		for(; t<params.Tcal; t+=sim.dt, itr_u++)
		{
			sim.update(*itr_u);
			list_ut.push_back(*itr_u);
		}

		for(; t<Tm; t+=sim.dt, itr_u++)
		{
			u_curr=*itr_u+Aht*(u_curr-*itr_u);
			sim.update(u_curr);
			list_ut.push_back(u_curr);

			u_curr=*itr_u+Aht*(u_curr-*itr_u);
		}

		itr_u=list_u1.begin();
		std::advance(itr_u, int(Tm/sim.dt+0.5));
		for(; t<params.Tp; t+=sim.dt, itr_u++)
		{
			u_curr=*itr_u+Aht*(u_curr-*itr_u);
			sim.update(u_curr);
			list_ut.push_back(u_curr);

			u_curr=*itr_u+Aht*(u_curr-*itr_u);
		}

		double Jn=J(sim.get_states());
		
		if (Jn<J1)
		{
			J1=Jn;
			index=n;
			list_ud=list_ut;
		}
	}	

	if(list_u.size()<int(params.Tp/sim.dt))
		list_u=list_u1;

	if(index>=0)
	{
		typename std::list<typename Robot::U>::iterator itr_u=list_u.begin();
		typename std::list<typename Robot::U>::const_iterator itr_ud=list_ud.begin();

		for(;itr_ud!=list_ud.end();)
			*itr_u++=*itr_ud++;

//		std::cout<<index<<std::endl;
	}
//	else
//	{
//		std::cout<<"Failture"<<std::endl;
//	}
	return J1;
}
Пример #10
0
template <typename Robot> double SAC<Robot>::sac_ctrl(typename std::list<typename Robot::U> & list_u, typename Robot::U umin, typename Robot::U umax)
{
	double J1=J0;

	typename std::list<typename Robot::U>::const_iterator itr_u1=list_u1.cbegin();
	typename std::list<typename Robot::State>::const_iterator itr_state=list_states.cbegin();
	typename std::list<typename Robot::coState>::const_iterator itr_costate=list_costates.cbegin();

	double Ti=params.Tc+params.Ts;

	params.alpha*=J0;

	double t;

	for(t=0; t<params.Tcal-1e-10; t+=sim.dt, itr_u1++, itr_state++, itr_costate++)
		list_u2.push_back(*itr_u1);

	for(; t<Ti-1e-10; t+=sim.dt, itr_u1++, itr_state++, itr_costate++)
	{
		Eigen::Matrix<double,Robot::M,Robot::N> h=Robot::h(sys,*itr_state);
		Eigen::Matrix<double,Robot::N,1> gamma=h.transpose()*(itr_costate->costate);
		Eigen::Matrix<double,Robot::N,Robot::N> lamada=gamma*gamma.transpose();

		Eigen::Matrix<double,Robot::N,1> u=(lamada+params.R).ldlt().solve(lamada*(*itr_u1)+gamma*params.alpha);

		for(int i=0;i<Robot::N;i++)
			if(u(i)<umin(i))
				u(i)=umin(i);
			else
				if(u(i)>umax(i))
					u(i)=umax(i);

		list_u2.push_back(u);
	}

	int index=-1;

	double Tc=params.Tc;

	for(int n=0; n<params.kmax;n++, Tc/=2.0)
	{
		double t;
		typename std::list<typename Robot::Ref>::const_iterator itr_par;
		typename std::list<typename Robot::U>::const_iterator itr_u;

		double Tm=params.Tcal+Tc; 

		sim.init(list_states.front());
		
		itr_u=list_u2.begin();
	
		for(t=0; t<Tm-1e-10; t+=sim.dt, itr_u++)
			sim.update(*itr_u);
	
		itr_u=list_u1.begin();
		
		std::advance(itr_u, int(Tm/sim.dt+0.5));

		for(; t<params.Tp-1e-10; t+=sim.dt, itr_u++)
			sim.update(*itr_u);

		double Jn=J(sim.get_states());
	
		if (Jn<J1)
		{
			J1=Jn;
			index=n;
		}
	}

	if(list_u.size()<int(params.Tp/sim.dt))
		list_u=list_u1;

	if(index>=0)
	{
		double Tm=params.Tcal+params.Tc/pow(2.0,index);
		typename std::list<typename Robot::U>::iterator itr_u=list_u.begin();
		typename std::list<typename Robot::U>::const_iterator itr_u1=list_u1.begin();
		typename std::list<typename Robot::U>::const_iterator itr_u2=list_u2.begin();

		double t=0;
		for(; t<Tm-1e-10; t+=sim.dt, itr_u1++)
			*itr_u++=*itr_u2++;

		for(; t<params.Tp-1e-10; t+=sim.dt)
			*itr_u++=*itr_u1++;
	}

	return J1;
}