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; }
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; }
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_ ); } } }
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; }
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; }
/** * @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; }
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); } } } }
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; }
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; }
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; }