669 lines
19 KiB
C++
669 lines
19 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 <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 ;
|
|
}
|
|
|
|
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 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)
|
|
{
|
|
Point3D current_point(x,y,z) ;
|
|
Autocalibration ac(current_point) ;
|
|
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 ;
|
|
}
|
|
}
|