mcy
/
owlps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
owlps/owlps-positioning/src/stock.cc

741 lines
21 KiB
C++
Raw Blame History

/*
* This file is part of the Owl Positioning System (OwlPS).
* OwlPS is a project of the University of Franche-Comté
* (Université de Franche-Comté), France.
*/
#include "stock.hh"
#include "configuration.hh"
#include "posexcept.hh"
#include "area.hh"
#include <iostream>
using namespace std ;
using std::tr1::unordered_map ;
using std::tr1::unordered_set ;
/* *** Attribute definitions *** */
unordered_map<string, Building> Stock::buildings ;
unordered_map<Point3D, Waypoint> Stock::waypoints ;
unordered_map<string, Mobile> Stock::mobiles ;
uint32_t Stock::nb_virtual_mobiles = 0 ;
unordered_map<string, AccessPoint> Stock::aps ;
unordered_set<ReferencePoint,
boost::hash<ReferencePoint>,
reference_point_equal_to> Stock::reference_points ;
unordered_set<CalibrationRequest> Stock::calibration_requests ;
void Stock::clear()
{
buildings.clear() ;
// Waypoints are normally deleted in Building::~Building(), so we must
// not clear waypoints before buildings, but we clear it *after*, just
// in case.
waypoints.clear() ;
mobiles.clear() ;
aps.clear() ;
reference_points.clear() ;
calibration_requests.clear() ;
}
/* *** Building operations *** */
/**
* The name of the Building is initialised.
*/
const Building& Stock::find_create_building(const string &name)
{
unordered_map<string, Building>::const_iterator i =
buildings.find(name) ;
if (i != buildings.end())
return i->second ;
Building &building = buildings[name] ;
building.set_name(name) ;
return building ;
}
/**
* @param name The name of the Building to search for.
* It must be a valid name, as no check is performed.
* @return A const reference to the Building.
* @throw element_not_found is thrown if the Building corresponding
* to \em name does not exist.
*/
const Building& Stock::get_building(const string &name)
{
unordered_map<string, Building>::const_iterator i =
buildings.find(name) ;
if (i != buildings.end())
return i->second ;
throw element_not_found("No Building with name \"" + name + "\"!") ;
}
/**
* @returns A pointer to the first Area in which \em point was found.
* @returns NULL if \em point does not belong to any Area.
*/
const Area* Stock::in_which_area_is(const Point3D &point)
{
for (unordered_map<string, Building>::const_iterator
b = buildings.begin() ; b != buildings.end() ; ++b)
{
const unordered_map<string, Area*> &areas = b->second.get_areas() ;
for (unordered_map<string, Area*>::const_iterator
a = areas.begin() ; a != areas.end() ; ++a)
if (a->second->contains_point(point))
return a->second ;
}
return NULL ;
}
/* *** Waypoint operations *** */
const Waypoint& Stock::
find_create_waypoint(const Point3D &point)
{
unordered_map<Point3D, Waypoint>::const_iterator i =
waypoints.find(point) ;
if (i != waypoints.end())
return i->second ;
Waypoint &waypoint = waypoints[point] ;
waypoint.set_coordinates(point) ;
return waypoint ;
}
void Stock::waypoint_remove_building(const Waypoint &point,
const Building *building)
{
unordered_map<Point3D, Waypoint>::iterator i =
waypoints.find(point) ;
Waypoint *waypoint = const_cast<Waypoint*>(&i->second) ;
waypoint->remove_building(building) ;
// If the Waypoint is not linked to any Building any more, we
// delete it
if (waypoint->get_1st_building() == NULL)
waypoints.erase(i) ;
}
/* *** Mobile operations *** */
/**
* @param mac The MAC address of the Mobile to search for.
* It must be a valid MAC address, as no check is performed.
* @return \em true if a mobile with this MAC address exists.
* @return \em false if this MAC address was not found.
*/
bool Stock::mobile_exists(const std::string &mac)
{
unordered_map<string, Mobile>::const_iterator i = mobiles.find(mac) ;
return i != mobiles.end() ;
}
/**
* @param mac The MAC address of the Mobile to search for.
* It must be a valid MAC address, as no check is performed.
* @return A const reference to the Mobile.
* @throw element_not_found is thrown if the Mobile corresponding
* to \em mac does not exist.
*/
const Mobile& Stock::get_mobile(const string &mac)
{
unordered_map<string, Mobile>::const_iterator i = mobiles.find(mac) ;
if (i != mobiles.end())
return i->second ;
throw element_not_found("No Mobile with MAC address \"" +
mac + "\"!") ;
}
/**
* If created, the MAC address of the Mobile is initialised.
*/
const Mobile& Stock::find_create_mobile(const string &mac)
{
unordered_map<string, Mobile>::const_iterator i = mobiles.find(mac) ;
if (i != mobiles.end())
return i->second ;
Mobile &mobile = mobiles[mac] ;
mobile.set_mac_addr(mac) ;
return mobile ;
}
/**
* If the Mobile already exists, it is replaced by the \em source.
*/
const Mobile& Stock::find_create_mobile(const Mobile &source)
{
const string &mac = source.get_mac_addr() ;
unordered_map<string, Mobile>::const_iterator i = mobiles.find(mac) ;
if (i != mobiles.end())
return i->second ;
mobiles[mac] = source ;
return mobiles[mac] ;
}
/* *** AccessPoint operations *** */
/**
* @param mac The MAC address of the AccessPoint to search for.
* It must be a valid MAC address, as no check is performed.
* @return \em true if an AP with this MAC address exists.
* @return \em false if this MAC address was not found.
*/
bool Stock::ap_exists(const std::string &mac)
{
unordered_map<string, AccessPoint>::const_iterator i = aps.find(mac) ;
return i != aps.end() ;
}
/**
* @param mac The MAC address of the AccessPoint to search for.
* It must be a valid MAC address, as no check is performed.
* @return A const reference to the AccessPoint.
* @throw element_not_found is thrown if the AccessPoint corresponding
* to \em mac does not exist.
*/
const AccessPoint& Stock::get_ap(const string &mac)
{
unordered_map<string, AccessPoint>::const_iterator i = aps.find(mac) ;
if (i != aps.end())
return i->second ;
throw element_not_found("No AccessPoint with MAC address \"" +
mac + "\"!") ;
}
/**
* The MAC address of the AccessPoint is initialised.
*/
const AccessPoint& Stock::find_create_ap(const string &mac)
{
unordered_map<string, AccessPoint>::const_iterator i = aps.find(mac) ;
if (i != aps.end())
return i->second ;
AccessPoint &ap = aps[mac] ;
ap.set_mac_addr(mac) ;
if (Configuration::is_configured("verbose"))
cerr
<< "New AP \"" << mac
<< "\" (total: " << nb_aps() << " APs).\n" ;
return ap ;
}
/**
* If the AccessPoint already exists, it is replaced by the \em source.
*/
const AccessPoint& Stock::find_create_ap(const AccessPoint &source)
{
const string &mac = source.get_mac_addr() ;
unordered_map<string, AccessPoint>::const_iterator i = aps.find(mac) ;
if (i != aps.end())
return i->second ;
aps[mac] = source ;
return aps[mac] ;
}
void Stock::update_all_friis_indexes()
{
for (unordered_map<string, AccessPoint>::iterator ap = aps.begin() ;
ap != aps.end() ; ++ap)
{
string ap_mac = ap->second.get_mac_addr() ;
double friis_idx_sum = 0 ;
int nb_friis_idx = 0 ;
// Compute main general term, independant from scans
double const_term = ap->second.friis_constant_term() ;
/*
* Compute indexes for each ReferencePoint. The Friis index for an
* AP is the average of all indexes in all ReferencePoint.
*/
for (unordered_set<ReferencePoint>::const_iterator rp =
reference_points.begin() ; rp != reference_points.end() ;
++rp)
{
int nb_idx = 0 ;
float ap_friis_idx_sum =
rp->friis_indexes_for_ap(ap->second, const_term, nb_idx) ;
if (nb_idx != 0)
{
friis_idx_sum += ap_friis_idx_sum ;
nb_friis_idx += nb_idx ;
}
}
if (nb_friis_idx > 0)
ap->second.set_friis_index(friis_idx_sum / nb_friis_idx) ;
}
}
double Stock::ap_matrix_get_ss(const std::string &mac_transmitter,
const std::string &mac_receiver)
{
const AccessPoint &receiver = get_ap(mac_receiver) ;
const ReferencePoint &rp =
get_reference_point(receiver.get_coordinates()) ;
return rp.average_measurements(mac_transmitter) ;
}
/**
* @returns \em true if \em coord are the coordinates of an existing
* AP, \em false if not.
*/
bool Stock::is_ap_coordinate(const Point3D &coord)
{
for (unordered_map<string, AccessPoint>::const_iterator ap =
aps.begin() ; ap != aps.end() ; ++ap)
if (ap->second.get_coordinates() == coord)
return true ;
return false ;
}
/* *** ReferencePoint operations *** */
bool Stock::reference_point_exists(const ReferencePoint &point)
{
unordered_set<ReferencePoint>::const_iterator i =
reference_points.find(point) ;
return i != reference_points.end() ;
}
/**
* @param point Coordinates of the wanted ReferencePoint.
* @return The ReferencePoint at the given coordinates, if found.
* @throw element_not_found is thrown if no ReferencePoint exists at the
* given coordinates.
*/
const ReferencePoint& Stock::
get_reference_point(const ReferencePoint &point)
{
unordered_set<ReferencePoint>::const_iterator i =
reference_points.find(point) ;
if (i != reference_points.end())
return *i ;
throw element_not_found("No ReferencePoint with coordinates " +
static_cast<string>(point) + "!") ;
}
const ReferencePoint& Stock::
find_create_reference_point(const ReferencePoint &point)
{
// unordered_set::insert() do all the job: see the documentation at
// http://boost.org/doc/libs/1_42_0/doc/html/boost/unordered_set.html
pair<unordered_set<ReferencePoint>::iterator, bool> ret =
reference_points.insert(point) ;
return *ret.first ;
}
const ReferencePoint& Stock::
closest_reference_point(const Request &request)
{
if (reference_points.empty())
throw element_not_found(
"Cannot search for the closest reference point: reference points'"
" list is empty!") ;
bool ignore_aps = Configuration::bool_value(
"positioning.nss.ignore-ap-reference-points") ;
unordered_set<ReferencePoint>::const_iterator i =
reference_points.begin() ;
if (ignore_aps)
{
// Fast-forward to the next non-AP reference point
while (i != reference_points.end() && is_ap_coordinate(*i))
++i ;
// No non-AP reference point was found, we are forced to consider
// the AP reference points
if (i == reference_points.end())
ignore_aps = false ;
}
float distance = i->ss_square_distance(request) ;
unordered_set<ReferencePoint>::const_iterator closest = i ;
for (++i ; i != reference_points.end() ; ++i)
{
if (ignore_aps && is_ap_coordinate(*i))
continue ;
float tmp_distance = i->ss_square_distance(request) ;
if (tmp_distance < distance)
{
distance = tmp_distance ;
closest = i ;
}
}
return *closest ;
}
void Stock::regenerate_reference_points()
{
assert(! aps.empty()) ;
assert(! reference_points.empty()) ;
delete_non_ap_calibration_requests() ;
if (reference_points.size() < 3)
{
if (Configuration::is_configured("verbose"))
cerr
<< reference_points.size() << " true reference points is"
<< " not enough to generate reference points.\n" ;
return ;
}
/* Generate RPs */
if (! Configuration::is_configured("positioning.area-start") ||
! Configuration::is_configured("positioning.area-stop"))
throw missing_configuration(
"You must define the deployment area in order to generate"
" reference points.") ;
Point3D start(
Configuration::string_value("positioning.area-start")) ;
Point3D stop(
Configuration::string_value("positioning.area-stop")) ;
float step_x =
Configuration::float_value("positioning.generated-meshing-grain-x") ;
float step_y =
Configuration::float_value("positioning.generated-meshing-grain-y") ;
float z = 1 ; // FIXME
for (float x = start.get_x() ; x <= stop.get_x() ; x += step_x)
for (float y = start.get_y() ; y <= stop.get_y() ; y += step_y)
//for (float z = start.get_z() ; z <= stop.get_z() ; z += step_z)
{
Point3D current_point(x,y,z) ;
generate_reference_point(current_point) ;
}
}
/**
* The reference point P we want to generate will end-up containing
* as many calibration requests as the total number of APs.
* For each of these requests, we consider a transmitter TRX (which is
* a virtual mobile located in P) and a receiver RX (the current AP).
* To generate the signal strength (SS) received by RX from TRX,
* we first select two reference APs REF1 and REF2. Then, we average
* the real measurements REF1->RX and REF2->RX, weighting in function
* of the angles REF-RX-TRX and the distance from RX to TRX to compute
* the SS.
* Hope this is clear enough…
*/
void Stock::generate_reference_point(const Point3D &point)
{
// If the point is the coordinates of an existing AP, we don't
// need to generate it
if (is_ap_coordinate(point))
return ;
/* Get/create the reference point */
const ReferencePoint &current_rp =
find_create_reference_point(point) ;
/* Create the timestamp */
Timestamp time_sent ;
time_sent.now() ;
/* Create the measurement list */
unordered_map<string, Measurement> measurements ;
/* Prepare the virtual mobile */
string vmob_mac(PosUtil::int_to_mac(nb_virtual_mobiles++)) ;
// The gain and trx power of the mobile will be the average of
// those of all the known APs (could be better, probably)
double vmob_gain = 0 ;
double vmob_pow = 0 ;
for (unordered_map<string, AccessPoint>::const_iterator
rx = aps.begin() ; rx != aps.end() ; ++rx)
{
/* Update the mobile's attributes */
vmob_gain += rx->second.get_antenna_gain() / aps.size() ;
vmob_pow += rx->second.get_trx_power() / aps.size() ;
/* Choose the 2 nearest APs in angle */
const Point3D &rx_coord =
rx->second.get_coordinates() ;
multimap<double,
unordered_map<string, AccessPoint>::const_iterator>
sorted_angles ;
for (unordered_map<string, AccessPoint>::const_iterator
ref = aps.begin() ; ref != aps.end() ; ++ref)
{
if (ref == rx)
continue ;
const Point3D &ref_coord =
ref->second.get_coordinates() ;
// Skip the AP if the associated reference point does
// not exist yet:
if (! reference_point_exists(ref_coord))
continue ;
double angle =
rx_coord.angle(point, ref_coord) ;
pair<double,
unordered_map<string, AccessPoint>::const_iterator>
angle_ap(angle, ref) ;
sorted_angles.insert(angle_ap) ;
}
assert(sorted_angles.size() > 1) ;
/* Compute the angle weight of the 2 reference APs */
map<double,
unordered_map<string, AccessPoint>::const_iterator>
::const_iterator s = sorted_angles.begin() ;
// Angle REF1-RX-P
double ref1_angle = s->first ;
const AccessPoint &ref1 = s->second->second ;
++s ;
const AccessPoint &ref2 = s->second->second ;
// Angle REF1-RX-REF2
const Point3D &ref1_coord = ref1.get_coordinates() ;
const Point3D &ref2_coord = ref2.get_coordinates() ;
double reference_angle =
rx_coord.angle(ref1_coord, ref2_coord) ;
double ref1_percent, ref2_percent ;
if (reference_angle == 0)
ref1_percent = ref2_percent = 50 ;
else
{
ref1_percent = ref1_angle * 100 / reference_angle ;
if (ref1_percent > 100)
ref1_percent = 100 ;
ref2_percent = 100 - ref1_percent ;
}
/* Compute the SS that would be received from a mobile at
* distance of RX-P, in the direction of each reference AP */
// Distance RX-P
float point_dst =
rx_coord.distance(point) ;
// Constant term
double common_ss = rx->second.friis_constant_term() ;
// SS for REF1
const ReferencePoint &ref1_rp =
get_reference_point(ref1_coord) ;
double friis_index =
ref1_rp.friis_index_for_ap(rx->second.get_mac_addr()) ;
double ref1_ss =
common_ss +
ref1.get_trx_power() +
ref1.get_antenna_gain() -
10 * friis_index * log10(point_dst) ;
// SS for REF2
const ReferencePoint &ref2_rp =
get_reference_point(ref2_coord) ;
friis_index =
ref2_rp.friis_index_for_ap(rx->second.get_mac_addr()) ;
double ref2_ss =
common_ss +
ref2.get_trx_power() +
ref2.get_antenna_gain() -
10 * friis_index * log10(point_dst) ;
/* Compute the SS */
ref1_ss = ref1_ss * ref1_percent / 100 ;
ref2_ss = ref2_ss * ref2_percent / 100 ;
double rx_ss = ref1_ss + ref2_ss ;
/* Create the measurement, add it to the list */
Measurement m(&rx->second) ;
m.add_ss(rx_ss) ;
measurements[rx->second.get_mac_addr()] = m ;
}
/* Create the virtual mobile */
Mobile vmob("", vmob_mac, vmob_gain, vmob_pow) ;
const Mobile &mobile = find_create_mobile(vmob) ;
/* Create the calibration request */
CalibrationRequest cr(OWL_REQUEST_GENERATED) ;
cr.set_time_sent(time_sent) ;
cr.set_mobile(&mobile) ;
cr.set_measurements(measurements) ;
cr.set_reference_point(&current_rp) ;
store_calibration_request(cr) ;
}
/* *** CalibrationRequest operations *** */
/**
* @returns The CalibrationRequest created or found.
*/
const CalibrationRequest& Stock::
store_calibration_request(const CalibrationRequest &request)
{
const CalibrationRequest &calibration_request =
find_create_calibration_request(request) ;
calibration_request.reference_point_backward_link() ;
return calibration_request ;
}
void Stock::
delete_calibration_request(const CalibrationRequest &request)
{
#ifndef NDEBUG
unordered_set<CalibrationRequest>::const_iterator found =
calibration_requests.find(request) ;
if (found == calibration_requests.end())
{
cerr
<< "Error! Cannot delete an unstored CalibrationRequest: "
<< request << endl ;
return ;
}
#endif // NDEBUG
calibration_requests.erase(request) ;
}
const CalibrationRequest& Stock::
find_create_calibration_request(const CalibrationRequest &request)
{
pair<unordered_set<CalibrationRequest>::iterator, bool> ret =
calibration_requests.insert(request) ;
return *ret.first ;
}
const CalibrationRequest& Stock::
closest_calibration_request(const Request &request)
{
if (calibration_requests.empty())
throw element_not_found(
"Cannot search for the closest calibration request: calibration"
" requests' list is empty!") ;
bool ignore_aps = Configuration::bool_value(
"positioning.nss.ignore-ap-reference-points") ;
unordered_set<CalibrationRequest>::const_iterator i =
calibration_requests.begin() ;
if (ignore_aps)
{
// Fast-forward to the next non-AP reference point
while (i != calibration_requests.end() &&
is_ap_coordinate(*i->get_reference_point()))
++i ;
// No non-AP reference point was found, we are forced to consider
// the AP reference points
if (i == calibration_requests.end())
{
i = calibration_requests.begin() ;
ignore_aps = false ;
}
}
float distance = i->ss_square_distance(request) ;
unordered_set<CalibrationRequest>::const_iterator closest = i ;
for (++i ; i != calibration_requests.end() ; ++i)
{
if (ignore_aps && is_ap_coordinate(*i->get_reference_point()))
continue ;
float tmp_distance = i->ss_square_distance(request) ;
if (tmp_distance < distance)
{
distance = tmp_distance ;
closest = i ;
}
}
return *closest ;
}
void Stock::delete_non_ap_calibration_requests()
{
unordered_set<ReferencePoint>::iterator rp = reference_points.begin() ;
while (rp != reference_points.end())
{
ReferencePoint rp_copy(*rp) ;
if (rp_copy.delete_generated_requests()) // rp_copy was modified
{
rp = reference_points.erase(rp) ;
// Reinsert the modified copy if it still contains at least
// one CalibrationRequest:
if (! rp_copy.get_requests().empty())
reference_points.insert(rp_copy) ;
/* __Note on the above insert__
* From the boost::unordered documentation:
* "insert is only allowed to invalidate iterators when the
* insertion causes the load factor to be greater than or
* equal to the maximum load factor."
* In our case, we always remove an element prior to insert,
* so the load factor will never change; therefore insert()
* should be safe here.
*/
}
else // rp_copy was not modified
++rp ;
}
}
Reference in New Issue View Git Blame Copy Permalink