void match(unsigned int position)
{
// cairo_set_line_cap(cairoOut, CAIRO_LINE_CAP_ROUND);
m_sensorReference.seek(position);
cairo_matrix_t m1;
cairo_get_matrix(cairoOut, &m1);
cairo_identity_matrix(cairoOut);
cairo_set_source_surface(cairoOut, cairo_get_target(cairoMap), 0., 0.);
cairo_paint(cairoOut);
cairo_set_matrix(cairoOut, &m1);
// cairo_set_line_width(cairoOut, 1./(2.*scaleFactor));
std::vector<InterestPoint *> pointsLocal(m_pointsReference[position].size());
const LaserReading* lreadReference = dynamic_cast<const LaserReading*>(m_sensorReference.current());
for(unsigned int j = 0; j < m_pointsReference[position].size(); j++){
InterestPoint * point = new InterestPoint(*m_pointsReference[position][j]);
point->setPosition(lreadReference->getLaserPose().ominus(point->getPosition()));
pointsLocal[j] = point;
}
for(unsigned int i = 0; i < m_pointsReference.size(); i++){
if(i == position) {
continue;
}
OrientedPoint2D transform;
std::vector< std::pair<InterestPoint*, InterestPoint* > > correspondences;
double result = m_ransac->matchSets(m_pointsReference[i], pointsLocal, transform, correspondences);
if(correspondences.size() >= corresp) {
cairo_matrix_t m;
cairo_get_matrix(cairoOut, &m);
cairo_translate(cairoOut, transform.x, transform.y);
cairo_rotate(cairoOut, transform.theta);
cairo_set_source_rgba(cairoOut, 1., 0., 0., 1. - result/(acceptanceSigma * acceptanceSigma * 5.99 * double(pointsLocal.size())));
cairo_move_to(cairoOut, 0., -0.3);
cairo_line_to(cairoOut, 0.6, 0.);
cairo_line_to(cairoOut, 0., 0.3);
cairo_close_path(cairoOut);
cairo_fill(cairoOut);
cairo_set_matrix(cairoOut, &m);
}
}
cairo_matrix_t m;
cairo_get_matrix(cairoOut, &m);
cairo_translate(cairoOut, lreadReference->getLaserPose().x, lreadReference->getLaserPose().y);
cairo_rotate(cairoOut, lreadReference->getLaserPose().theta);
cairo_set_source_rgba(cairoOut, 0., 0., 1., 1.);
cairo_move_to(cairoOut, 0., -0.3);
cairo_line_to(cairoOut, 0.6, 0.);
cairo_line_to(cairoOut, 0., 0.3);
cairo_close_path(cairoOut);
cairo_stroke(cairoOut);
cairo_set_matrix(cairoOut, &m);
// cairo_show_page(cairoOut);
}
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;
}
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;
}
void match(unsigned int position)
{
m_sensorReference.seek(position);
std::vector<InterestPoint *> pointsLocal(m_pointsReference[position].size());
const LaserReading* lreadReference = dynamic_cast<const LaserReading*>(m_sensorReference.current());
for(unsigned int j = 0; j < m_pointsReference[position].size(); j++){
InterestPoint * point = new InterestPoint(*m_pointsReference[position][j]);
point->setPosition(lreadReference->getLaserPose().ominus(point->getPosition()));
pointsLocal[j] = point;
}
unsigned int inliers[m_pointsReference.size()];
double results[m_pointsReference.size()];
double linearErrors[m_pointsReference.size()];
double angularErrors[m_pointsReference.size()];
struct timeval start, end, diff, sum;
for(unsigned int i = 0; i < m_pointsReference.size(); i++){
if(fabs(double(i) - double(position)) < m_localSkip) {
results[i] = 1e17;
inliers[i] = 0;
linearErrors[i] = 1e17;
angularErrors[i] = 1e17;
continue;
}
OrientedPoint2D transform;
std::vector< std::pair<InterestPoint*, InterestPoint* > > correspondences;
gettimeofday(&start,NULL);
// std::cout << m_pointsReference[i].size() << " vs. " << pointsLocal.size() << std::endl;
results[i] = m_ransac->matchSets(m_pointsReference[i], pointsLocal, transform, correspondences);
gettimeofday(&end,NULL);
timersub(&end, &start, &diff);
timeradd(&ransacTime, &diff, &sum);
ransacTime = sum;
inliers[i] = correspondences.size();
OrientedPoint2D delta = m_posesReference[position] - transform;
linearErrors[i] = correspondences.size() ? delta * delta : 1e17;
angularErrors[i] = correspondences.size() ? delta.theta * delta.theta : 1e17;
}
for(unsigned int c = 0; c < 8; c++){
unsigned int maxCorres = 0;
double maxResult = 1e17;
double linError = 1e17, angError = 1e17;
double linErrorC = 1e17, angErrorC = 1e17;
double linErrorR = 1e17, angErrorR = 1e17;
bool valid = false;
for(unsigned int i = 0; i < m_pointsReference.size(); i++){
if(linError + angError > linearErrors[i] + angularErrors[i]) {
linError = linearErrors[i];
angError = angularErrors[i];
}
if(maxCorres < inliers[i]){
linErrorC = linearErrors[i];
angErrorC = angularErrors[i];
maxCorres = inliers[i];
}
if(maxResult > results[i]){
linErrorR = linearErrors[i];
angErrorR = angularErrors[i];
maxResult = results[i];
}
valid = valid || inliers[i] >= corresp[c];
}
if(valid){
m_match[c] += (linError <= (linErrorTh * linErrorTh) && angError <= (angErrorTh * angErrorTh) );
m_matchC[c] += (linErrorC <= (linErrorTh * linErrorTh) && angErrorC <= (angErrorTh * angErrorTh) );
m_matchR[c] += (linErrorR <= (linErrorTh * linErrorTh) && angErrorR <= (angErrorTh * angErrorTh) );
m_error[c] += sqrt(linError + angError);
m_errorC[c] += sqrt(linErrorC + angErrorC);
m_errorR[c] += sqrt(linErrorR + angErrorR);
m_valid[c]++;
}
}
}