InterestPoint::InterestPoint(const InterestPoint& _point):
m_position(_point.getPosition()),
m_scale(_point.getScale()),
m_scaleLevel(_point.getScaleLevel())
{
if(_point.getDescriptor())
m_descriptor = _point.getDescriptor()->clone();
else
m_descriptor = 0;
}
void writeBoW(std::ofstream& out){
std::string bar(50, ' ');
bar[0] = '#';
unsigned int progress = 0;
struct timeval start, end;
gettimeofday(&start, NULL);
for(unsigned int i = 0; i < m_pointsReference.size(); i++){
unsigned int currentProgress = (i*100)/(m_pointsReference.size() - 1);
if (progress < currentProgress){
progress = currentProgress;
bar[progress/2] = '#';
std::cout << "\rDescribing scans [" << bar << "] " << progress << "%" << std::flush;
}
std::multimap<double,WordResult> signature;
for(unsigned int j = 0; j < m_pointsReference[i].size(); j++){
InterestPoint * point = m_pointsReference[i][j];
OrientedPoint2D localpose = m_posesReference[i].ominus(point->getPosition());
double angle = atan2(localpose.y, localpose.x);
unsigned int bestWord = 0;
double bestMatch = 0.;
std::vector<double> descriptor;
std::vector<double> weights;
point->getDescriptor()->getWeightedFlatDescription(descriptor, weights);
HistogramFeatureWord word(descriptor, NULL, weights);
for(unsigned int w = 0; w < histogramVocabulary.size(); w++) {
double score = histogramVocabulary[w].sim(&word);
if(score > bestMatch) {
bestMatch = score;
bestWord = w;
}
}
WordResult best; best.pose = localpose; best.word = bestWord;
signature.insert(std::make_pair(angle,best));
}
out << m_pointsReference[i].size();
for(std::multimap<double,WordResult>::const_iterator it = signature.begin(); it != signature.end(); it++){
out << " " << it->second.word << " " << it->second.pose.x << " " << it->second.pose.y;
}
out <<std::endl;
}
gettimeofday(&end,NULL);
timersub(&end,&start,&vocabularyTime);
std::cout << " done." << std::endl;
}
InterestPointRos toRos (const InterestPoint& pt)
{
InterestPointRos m;
m.pose.x = pt.getPosition().x;
m.pose.y = pt.getPosition().y;
m.pose.theta = pt.getPosition().theta;
m.support_points.reserve(pt.getSupport().size());
BOOST_FOREACH (const Point2D& p, pt.getSupport())
m.support_points.push_back(toPoint(p));
m.scale = pt.getScale();
m.scale_level = pt.getScaleLevel();
m.descriptor = toRos(pt.getDescriptor());
return m;
}