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);
}
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);
}
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);
}