double NodeMatcher::scorePair(long nid1, long nid2)
{
shared_ptr<const Node> n1 = _map->getNode(nid1);
shared_ptr<const Node> n2 = _map->getNode(nid2);
const set<long>& wids1 = _map->getIndex().getNodeToWayMap()->at(nid1);
const set<long>& wids2 = _map->getIndex().getNodeToWayMap()->at(nid2);
double acc = 0;
for (set<long>::const_iterator it = wids1.begin(); it != wids1.end(); ++it)
{
acc = max(acc, _map->getWay(*it)->getCircularError());
}
for (set<long>::const_iterator it = wids2.begin(); it != wids2.end(); ++it)
{
acc = max(acc, _map->getWay(*it)->getCircularError());
}
vector<Radians> theta1 = calculateAngles(_map.get(), nid1, wids1);
vector<Radians> theta2 = calculateAngles(_map.get(), nid2, wids2);
int s1 = theta1.size();
int s2 = theta2.size();
if (s1 < 3 || s2 < 3)
{
return 0.0;
}
double d = n1->toCoordinate().distance(n2->toCoordinate());
/// @todo this isnt right Talk to mike
double distanceScore = 1 - (Normal::phi(d, acc * 1.5) - 0.5) * 2.0;
if (theta1.size() < theta2.size())
{
swap(theta1, theta2);
}
double thetaScore;
// this is very unsual and will slow things down.
if (theta1.size() > 6 && theta2.size() > 6)
{
LOG_WARN("7 intersections at one spot? Odd. Giving it a high angleScore.");
LOG_VAR(nid1);
LOG_VAR(nid2);
LOG_VAR(wids1);
LOG_VAR(wids2);
thetaScore = 1.0;
}
else
{
vector<bool> exclude(theta1.size(), false);
thetaScore = _calculateAngleScore(theta1, theta2, exclude, 0);
}
// simple stupid heuristic. Replace w/ some cosine fanciness later.
int diff = abs((int)s1 - (int)s2);
return (min(s1, s2) - diff) * thetaScore * distanceScore;
}
double NodeMatcher::_calculateAngleScore(const vector<Radians>& theta1,
const vector<Radians>& theta2, vector<bool>& exclude, size_t depth, bool debug)
{
assert(theta1.size() <= theta2.size());
if (depth == theta1.size())
{
return 1.0;
}
double max = 0;
for (int j = 0; j < (int)theta2.size(); j++)
{
if (exclude[j] == false)
{
Radians m = WayHeading::deltaMagnitude(theta1[depth], theta2[j]);
double r = 0;
if (m < M_PI / 2.0)
{
r = pow(cos(WayHeading::deltaMagnitude(theta1[depth], theta2[j])), _strictness);
}
exclude[j] = true;
double v = r * _calculateAngleScore(theta1, theta2, exclude, depth + 1, debug);
exclude[j] = false;
if (v > max)
{
max = v;
}
}
}
return max;
}
double NodeMatcher::_calculateAngleScore(const vector<Radians>& theta1,
const vector<Radians>& theta2, vector<bool>& exclude, size_t depth)
{
if (depth == theta1.size())
{
return 1.0;
}
//LOG_INFO("depth: " << depth << " exclude: " << exclude);
double max = 0;
for (int j = 0; j < (int)theta2.size(); j++)
{
if (exclude[j] == false)
{
double r = pow(cos(WayHeading::deltaMagnitude(theta1[depth], theta2[j])), _strictness);
exclude[j] = true;
double v = r * _calculateAngleScore(theta1, theta2, exclude, depth + 1);
exclude[j] = false;
if (v > max)
{
max = v;
}
}
}
return max;
}
double NodeMatcher::scorePair(long nid1, long nid2)
{
ConstNodePtr n1 = _map->getNode(nid1);
ConstNodePtr n2 = _map->getNode(nid2);
const set<long>& wids1 = _map->getIndex().getNodeToWayMap()->at(nid1);
const set<long>& wids2 = _map->getIndex().getNodeToWayMap()->at(nid2);
double acc = 0;
for (set<long>::const_iterator it = wids1.begin(); it != wids1.end(); ++it)
{
acc = max(acc, _map->getWay(*it)->getCircularError());
}
for (set<long>::const_iterator it = wids2.begin(); it != wids2.end(); ++it)
{
acc = max(acc, _map->getWay(*it)->getCircularError());
}
vector<Radians> theta1 = calculateAngles(_map.get(), nid1, wids1, _delta);
vector<Radians> theta2 = calculateAngles(_map.get(), nid2, wids2, _delta);
int s1 = theta1.size();
int s2 = theta2.size();
if (s1 < 3 || s2 < 3)
{
return 0.0;
}
double d = n1->toCoordinate().distance(n2->toCoordinate());
// TODO: this isnt right; Talk to mike
double distanceScore = 1 - (Normal::phi(d, acc * 1.5) - 0.5) * 2.0;
LOG_VART(nid1);
LOG_VART(nid2);
LOG_VART(distanceScore);
LOG_VART(acc);
LOG_VART(d);
LOG_VART(Normal::phi(d, acc * 1.5));
LOG_VART(Normal::phi(d, acc / 2.0));
if (theta1.size() < theta2.size())
{
swap(theta1, theta2);
}
double thetaScore;
// this is very unsual and will slow things down.
if (theta1.size() > 6 && theta2.size() > 6)
{
if (logWarnCount < Log::getWarnMessageLimit())
{
LOG_WARN("Greater than seven intersections at one spot? Odd. Giving it a high angleScore.");
}
else if (logWarnCount == Log::getWarnMessageLimit())
{
LOG_WARN(className() << ": " << Log::LOG_WARN_LIMIT_REACHED_MESSAGE);
}
logWarnCount++;
LOG_VART(nid1);
LOG_VART(nid2);
LOG_VART(wids1);
LOG_VART(wids2);
thetaScore = 1.0;
}
else
{
if (theta2.size() < theta1.size())
{
vector<bool> exclude(theta2.size(), false);
thetaScore = _calculateAngleScore(theta2, theta1, exclude, 0);
}
else
{
vector<bool> exclude(theta1.size(), false);
thetaScore = _calculateAngleScore(theta1, theta2, exclude, 0);
}
}
// simple stupid heuristic. Replace w/ some cosine fanciness later.
int diff = abs((int)s1 - (int)s2);
double result = (min(s1, s2) - diff) * thetaScore * distanceScore;
LOG_VART(result);
return result;
}
