Пример #1
0
const float NaoPose::getHomLength(const ublas::vector<float> &vec) {
    float sum = 0.0f;
    for (ublas::vector<float>::const_iterator i = vec.begin(); i != vec.end()
            - 1; ++i) {
        sum += *i * *i;
    }
    return sqrt(sum);
}
Пример #2
0
Serialization SDPSeriationGen::Impl::readout_connected(ublas::vector<double>& x,const AdjMat::AdjMatT& adj)
{
	unsigned int n = x.size();

	// tricky: make sure x > 0 at all times.
	x += ublas::scalar_vector<double>(n, 1 - (*min_element(x.begin(),x.end())));

	Serialization::RankT ranks(n);
	std::vector<bool> done(n,false);

	// find highest component of x
	ublas::vector<double>::iterator it = BEST_ELEM(x);
	int idx = std::distance(x.begin(),it);


	L("Determine Actual Path through Graph.\n");
	for(unsigned int i=0;i<n;i++){
		// mark as visited
		ranks[i] = idx;
		done[idx] = true;

		// make sure we do not visit again
		*it = 0.0; 

		// [m,i] = max(x.*A(:,i));
		ublas::vector<double> adjcol = ublas::column(adj,idx);
		for(unsigned int j=0;j<adjcol.size();j++)
			if(adjcol[j]>0.00001) adjcol[j]=1;
		ublas::vector<double> tmp = ublas::element_prod(x,adjcol);
		it = BEST_ELEM(tmp);

		if( *it < 0.000000001 && i<n-1)
		{
			// if *it small, then either x[it] visited or adj(old_idx,idx) not connected
			// --> we reached a dead end, find next best start point
			it  = BEST_ELEM(x);
			idx = std::distance(x.begin(),it);
		}else{
			idx = std::distance(tmp.begin(),it);
			// point it in x, not tmp:
			it  = x.begin();
			it += idx;
		}


	}
	return Serialization(ranks);
}
Пример #3
0
Serialization SDPSeriationGen::Impl::readout_plain(ublas::vector<double>& x,const AdjMat::AdjMatT& adj)
{
	unsigned int n = x.size();

	// tricky: make sure x > 0 at all times.
	x += ublas::scalar_vector<double>(n, 1 - (*min_element(x.begin(),x.end())));

	Serialization::RankT ranks(n);
	std::vector<bool> done(n,false);

	// find highest component of x
	ublas::vector<double>::iterator it = BEST_ELEM(x);
	int idx = std::distance(x.begin(),it);

	L("Determine Actual Path through Graph.\n");
	for(unsigned int i=0;i<n;i++){
		ranks[i] = idx;
		done[idx] = true;
		*it = 0.0; 
		it = BEST_ELEM(x);
		idx = std::distance(x.begin(),it);
	}
	return Serialization(ranks);
}