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