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;
}
void MainWindow::updateGL(){
if(ui->plot_ax->graph(0)->data()->isEmpty()){
return;
}
if(glpos<65){
glpos+=6;
}
int pos=sampleNumber-65+glpos;
if(pos<0){
pos+=3000;
}
float x=ui->plot_ax->graph(0)->data()->value(pos).value;
float y=ui->plot_ay->graph(0)->data()->value(pos).value;
float z=ui->plot_az->graph(0)->data()->value(pos).value;
float length=sqrt(x*x+y*y+z*z);
calculateAngles(x/length,y/length,z/length);
ui->glwidget->setRotation(pitch,roll);
ui->glwidget->updateGL();
}
/**
* @brief Accelerometer thread
* @param arguments
* @retval None
*/
void Thread_Accelerometer(void const *argument){
while(1){
osSignalWait (data_ready_flag,osWaitForever);
osSignalClear(tid_Thread_Accelerometer,data_ready_flag);
calculateAngles();
}
}
void RobotControl::run()
{
while (isRunning())
{
if (hasTrajectory == true)
{
Time startTime = Clock::universal_time();
writeData(calculateAngles(getTrajectory()));
BOOST_LOG_TRIVIAL(info) << readData();
std::cout << "Tijd tot de response: " << (Clock::universal_time() - startTime).total_milliseconds() << "ms" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
//hitBall(getTrajectory());
hasTrajectory = false;
}
}
}
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;
}
void sample_layout_1::calculateAnglesAndPrint(int label, float O, float A, float *theta, float thetaDegrees){
calculateAngles(O, A, *theta, thetaDegrees);
printAngles(label, O, A, *theta, thetaDegrees);
}
