void BoxClass::InitializeIds()
{
TRACET(BoxClass);
size_t nr = 0;
for (typename listtype::iterator i = boxList.begin();
i != boxList.end();
i++) {
/* currently, the user provides the numeric ids, so we don't need to change them */
//(*i)->set_routefile_id(nr);
nr = (*i)->get_routefile_id();
routeListType & list = (*i)->routes;
for (routeListType::iterator j = list.begin();
j != list.end();
j++) {
//thistype * ptr = NULL;
(*j)->SetBoxId(nr);
}
// nr++;
}
return;
#if 0
for (typename listtype::iterator i = deviceList.begin();
i != deviceList.end();
i++) {
(*i)->set_routexfile_id(nr);
routeListType & list = (*i)->routes;
for (routeListType::iterator j = list.begin();
j != list.end();
j++) {
thistype * ptr = NULL;
(*j)->SetDeviceId(nr, ptr);
}
nr++;
}
TRACET(thistype);
#endif
}
void groupBeacons() {
STARTCHRONO
unsigned int num_beacons = beaconContainer->getNumBeacons();
unsigned int usedBeacons = 0;
std::vector<Beacon *> beacons;
bool first;
TRACET(DEBUG, DGB, "\ngroupBeacons()-----------\n");
// For each beacon found
for (unsigned int currentBeaconIndex = 0; currentBeaconIndex < num_beacons; currentBeaconIndex++) {
Beacon& currentBeacon = beaconContainer->getBeacon(currentBeaconIndex);
// If the beacon is already grouped skip it!
if (currentBeacon.used())
continue;
currentBeacon.used(true);
TRACET(DEBUG, DGB, "Using beacon %d, closest beacons: ", currentBeacon.getId());
// Clear the indexed beacon group
beacons.clear();
// Put the first beacon index in the indexed beacon group
beacons.push_back(¤tBeacon);
// Unset test flag for the beacons
for (unsigned int i = currentBeaconIndex + 1; i < num_beacons; i++)
beaconContainer->getBeacon(i).tested(false);
// Search closest beacons
unsigned int notUsedBeacons = beaconContainer->getNumBeacons() - usedBeacons;
unsigned int numToTest = notUsedBeacons < MAX_WAYPOINT_BEACONS ? notUsedBeacons : MAX_WAYPOINT_BEACONS;
for (unsigned int i = 1; i < numToTest; i++) {
float minDistance = INFINITY;
Beacon * closestBeacon = NULL;
// Search closest beacon
for (unsigned int j = currentBeaconIndex + 1; j < num_beacons; j++) {
Beacon & beacon = beaconContainer->getBeacon(j);
if (beacon.tested() || beacon.used())
continue;
float distance = (currentBeacon - beacon).abs();
if (distance < minDistance) {
minDistance = distance;
closestBeacon = &beacon;
}
}
if (closestBeacon == NULL) {
TRACET(DEBUG, DGB, "CRITICAL ERROR: Closest Point not found!\n");
return;
}
// Set the closest beacon as tested and put it into the beacon group
closestBeacon->tested(true);
beacons.push_back(closestBeacon);
TRACET(DEBUG, DGB, "%d ", closestBeacon->getId());
}
// Search minimum beacon distance
float minDistance = INFINITY;
for (unsigned int i = 0; i < numToTest; i++) {
Beacon & iBeacon = *(beacons[i]);
for (unsigned int j = i + 1; j < numToTest; j++) {
Beacon & jBeacon = *(beacons[j]);
float distance = (iBeacon - jBeacon).abs();
if (distance < minDistance)
minDistance = distance;
}
}
bool discardCurrentBeacon = false;
for (unsigned int i = 1; i < numToTest; i++) {
float distance = (*(beacons[0]) - *(beacons[i])).abs();
TRACET(DEBUG, DGB, "\n Distance %d -> %d: %f (min: %f) (%f, %f, %f)", beacons[0]->getId(), beacons[i]->getId(), distance, minDistance,\
beacons[i]->x, beacons[i]->y, beacons[i]->z);
if (distance / minDistance > DISTANCE_FACTOR) {
if (i == 1)
discardCurrentBeacon = true;
TRACET(DEBUG, DGB, " Beacon %d is too far\n", beacons[i]->getId());
// Clean far beacons
beacons.resize(i);
break;
}
}
if (discardCurrentBeacon || beacons.size() < 3) {
TRACET(DEBUG, DGB, " Discarding current beacon\n");
beacons[0]->discard();
beacons[0]->used(true);
usedBeacons++;
}
else {
// EUREKA!!! beacon group found, generate beaconGroup and set the beacons in the group as used
TRACET(DEBUG, DGB, " Beacon group found (%d beacons)\n", int(beacons.size()));
BeaconGroup & beaconGroup = beaconContainer->getCleanBeaconGroup();
beaconGroup.appendBeacons(beacons);
beaconGroup.computeBeaconGroup();
usedBeacons += beacons.size();
}
}
TRACET(DEBUG, DGB, "-%d us---------------------------------------\n", GETCLICK);
}
// Search waypoints that fits each beacon group and if so obtain the position and orientation
void findWaypoints() {
STARTCHRONO
TRACET(DEBUG, DFW, "\nfindWaypoints()------------------------\n");
// Unset positioned flags in the waypoints
for (unsigned int waypointIndex = 0; waypointIndex < nominalContainer.getNumBeaconGroups(); waypointIndex++) {
BeaconGroup & waypoint = nominalContainer.getBeaconGroup(waypointIndex);
waypoint.setPositioned(false);
}
// For each beacon group found
for (int beaconGroupIndex = 0; beaconGroupIndex < beaconContainer->getNumBeaconGroups(); beaconGroupIndex++) {
BeaconGroup & beaconGroup = beaconContainer->getBeaconGroup(beaconGroupIndex);
unsigned int beaconNumber = beaconGroup.getNumBeacons();
unsigned int angleScore = 101;
unsigned int distanceScore = 101;
unsigned int bestScore = 201;
unsigned int bestFirstBeaconIndex;
BeaconGroup *bestWaypoint = NULL;
// For each waypoint
for (unsigned int waypointIndex = 0; waypointIndex < nominalContainer.getNumBeaconGroups(); waypointIndex++) {
BeaconGroup & waypoint = nominalContainer.getBeaconGroup(waypointIndex);
TRACET(DEBUG, DFW, " Comparing beaconGroup %d to waypoint %d\n", beaconGroup.getId(), waypoint.getId());
// Check beacon number
if (beaconNumber != waypoint.getNumBeacons()) {
TRACET(DEBUG, DFW, " Beacon number missmatch, skipping waypoint\n");
continue;
}
// Obtain scale
float scale = beaconGroup.getMeanDistance() / waypoint.getMeanDistance();
Beacon& nominalBeacon = waypoint.getBeacon(0);
// For each beacon found
for (unsigned int firstBeaconIndex=0; firstBeaconIndex < beaconNumber; firstBeaconIndex++) {
Beacon& beacon = beaconGroup.getBeacon(firstBeaconIndex);
float distance = beacon.distance() / scale;
TRACET(DEBUG, DFW, " Using beacon %d as first beacon\n", beacon.getId());
// Check distance
float error = fabs(nominalBeacon.distance() - distance);
if (error < DISTANCE_MARGIN) {
distanceScore = error * 100 / DISTANCE_MARGIN;
TRACET(DEBUG, DFW, " Distance test passed, error: %f, score: %d\n", error, distanceScore);
TRACET(DEBUG, DFW, " Angle test in progress\n");
// Check angles
float maxAngleError = 0;
bool found = true;
for (unsigned int i=0; i < beaconNumber; i++) {
Beacon& cBeacon = beaconGroup.getBeacon((firstBeaconIndex + i) % beaconNumber);
Beacon& cNominalBeacon = waypoint.getBeacon(i);
TRACET(DEBUG, DFW, " Testing beacon %d ", cBeacon.getId());
float nominalAngle = cNominalBeacon.theta();
float angle = cBeacon.theta();
float angleError = fabs(nominalAngle - angle);
if (angleError > ANGLE_MARGIN) {
// Angle test negative, search next beacon that fits the nominal distance
found = false;
TRACET(DEBUG, DFW, "Angle missmatch (nominal: %f, actual: %f)\n", nominalAngle, angle);
break;
}
else {
if (angleError > maxAngleError)
maxAngleError = angleError;
TRACET(DEBUG, DFW, "Angle matched (nominal: %f, actual: %f)\n", nominalAngle, angle);
}
}
if (found) {
// EUREKA!!! the beacon group fits this waypoint!
angleScore = maxAngleError * 100 / ANGLE_MARGIN;
unsigned int score = angleScore + distanceScore;
if (score < bestScore) {
bestScore = score;
bestWaypoint = &waypoint;
bestFirstBeaconIndex = firstBeaconIndex;
}
TRACET(DEBUG, DFW, " All angles matched, the beacon group %d fits to the waypoint id %d, score: %d, angleScore: %d, distanceScore: %d\n",\
beaconGroup.getId(), waypoint.getId(), score, angleScore, distanceScore);
break;
}
else {
TRACET(DEBUG, DFW, " At least one angle missmatch has been found, choosing another first beacon if available\n\n");
}
}
else {
TRACET(DEBUG, DFW, " Distance test failed, error: %f\n", error);
}
}
}
if (bestWaypoint != NULL) {
// Compute position and heading using the best waypoint
Beacon& nominalBeacon = bestWaypoint->getBeacon(0);
Beacon& beacon = beaconGroup.getBeacon(bestFirstBeaconIndex);
// Obtain the heading
float heading = beacon.phi() - nominalBeacon.phi();
if (heading < 0)
heading += 360;
// Obtain the position
Vector3<float> position;
Vector3<float> beaconPosition = beacon;
// Height
position.z = bestWaypoint->getMeanDistance() / beaconGroup.getMeanDistance();
beaconPosition.rotate(-heading / RAD2DEG);
// Project p to the ground
beaconPosition.x = beaconPosition.x * position.z;
beaconPosition.y = beaconPosition.y * position.z;
position.x = nominalBeacon.x - beaconPosition.x;
position.y = nominalBeacon.y - beaconPosition.y;
// Load data to the beacon group
beaconGroup.setWaypoint(*bestWaypoint);
beaconGroup.setPosition(position);
beaconGroup.setHeading(heading);
// Update position of the waypoint in the image
bestWaypoint->setImageCentroid(beaconGroup.getImageCentroid());
bestWaypoint->setPosition(position);
bestWaypoint->setHeading(heading);
TRACET(DEBUG, DFW, " POSITION FOUND: position: (%f,\t%f,\t%f)\tyaw: %f\n\n", position.x, position.y, position.z, heading);
}
}
TRACET(DEBUG, DFW, "-%d us---------------------------------------\n", GETCLICK);
}
