owlps/owlps-positioner/src/stock.cc

708 lines
20 KiB
C++

/*
* This file is part of the Owl Positioning System (OwlPS).
* OwlPS is a project of the University of Franche-Comte
* (Université de Franche-Comté), France.
*
* Copyright © Université de Franche-Comté 2007-2012.
*
* Corresponding author: Matteo Cypriani <mcy@lm7.fr>
*
***********************************************************************
*
* This software is governed by the CeCILL license under French law and
* abiding by the rules of distribution of free software. You can use,
* modify and/or redistribute the software under the terms of the CeCILL
* license as circulated by CEA, CNRS and INRIA at the following URL:
* http://www.cecill.info
*
* As a counterpart to the access to the source code and rights to copy,
* modify and redistribute granted by the license, users are provided
* only with a limited warranty and the software's authors, the holder
* of the economic rights, and the successive licensors have only
* limited liability.
*
* In this respect, the user's attention is drawn to the risks
* associated with loading, using, modifying and/or developing or
* reproducing the software by the user in light of its specific status
* of free software, that may mean that it is complicated to manipulate,
* and that also therefore means that it is reserved for developers and
* experienced professionals having in-depth computer knowledge. Users
* are therefore encouraged to load and test the software's suitability
* as regards their requirements in conditions enabling the security of
* their systems and/or data to be ensured and, more generally, to use
* and operate it in the same conditions as regards security.
*
* The fact that you are presently reading this means that you have had
* knowledge of the CeCILL license and that you accept its terms.
*
***********************************************************************
*/
#include "stock.hh"
#include "configuration.hh"
#include "posexcept.hh"
#include "area.hh"
#include "csvstringreader.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 ;
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 similarity = i->similarity(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_similarity = i->similarity(request) ;
if (tmp_similarity < similarity)
{
similarity = tmp_similarity ;
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 ;
}
const string &ac_mode =
Configuration::string_value("positioning.generate-reference-points") ;
if (ac_mode == "mesh" || ac_mode == "both")
generate_reference_points_mesh() ;
if (ac_mode == "list" || ac_mode == "both")
generate_reference_points_list() ;
}
void Stock::generate_reference_points_mesh()
{
if (! (Configuration::is_configured("positioning.area-start") &&
Configuration::is_configured("positioning.area-stop")))
throw missing_configuration(
"In order to generate a meshing of reference points, you must"
" define the deployment area.") ;
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 step_z =
Configuration::float_value("positioning.generated-meshing-grain-z") ;
/* Generate RPs */
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)
generate_reference_point(Point3D(x,y,z)) ;
}
void Stock::generate_reference_points_list()
{
if (! Configuration::is_configured("positioning.generated-points-list"))
throw missing_configuration(
"In order to generate a list of reference points, you must"
" define the list.") ;
const string &rp_list_csv =
Configuration::string_value("positioning.generated-points-list") ;
CSVStringReader rp_list_parser(rp_list_csv) ;
Point3D rp ;
while (rp_list_parser.read_point3d(rp))
generate_reference_point(rp) ;
}
/**
* @arg coord The coordinates of the reference point to generate.
*/
void Stock::generate_reference_point(const Point3D &coord)
{
Autocalibration ac(coord) ;
try
{
ac.generate_reference_point() ;
}
catch (autocalibration_error &e)
{
if (Configuration::is_configured("verbose"))
cerr << e.what() << '\n' ;
}
}
/* *** 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) ;
}
/**
* @param timeout The maximum age of the calibration requests (seconds).
*/
void Stock::delete_calibration_requests_older_than(int timeout)
{
assert(timeout > 0) ;
unordered_set<CalibrationRequest>::iterator cr =
calibration_requests.begin() ;
while (cr != calibration_requests.end())
{
Timestamp request_time ;
if (Configuration::bool_value("replay"))
request_time = cr->get_time_sent() ;
else
request_time = cr->get_time_received() ;
Timestamp elapsed(request_time.elapsed()) ;
uint64_t elapsed_sec = static_cast<uint64_t>(elapsed) / 1000 ;
if (elapsed_sec >= static_cast<unsigned int>(timeout))
{
ReferencePoint *rp = cr->get_reference_point() ;
if (rp)
{
rp->delete_request(&*cr) ;
if (rp->get_requests().empty())
reference_points.erase(*rp) ;
}
cr = calibration_requests.erase(cr) ;
}
else
++cr ;
}
}
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 similarity = i->similarity(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 ;
assert(! i->get_measurements().empty()) ;
float tmp_similarity = i->similarity(request) ;
if (tmp_similarity < similarity)
{
similarity = tmp_similarity ;
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 ;
}
}