860 lines
23 KiB
C++
860 lines
23 KiB
C++
/*
|
|
* This file is part of the Owl Positioning System (OwlPS) project.
|
|
* It is subject to the copyright notice and license terms in the
|
|
* COPYRIGHT.t2t file found in the top-level directory of this
|
|
* distribution and at
|
|
* https://code.lm7.fr/mcy/owlps/src/master/COPYRIGHT.t2t
|
|
* No part of the OwlPS Project, including this file, may be copied,
|
|
* modified, propagated, or distributed except according to the terms
|
|
* contained in the COPYRIGHT.t2t file; the COPYRIGHT.t2t file must be
|
|
* distributed along with this file, either separately or by replacing
|
|
* this notice by the COPYRIGHT.t2t file's contents.
|
|
*/
|
|
|
|
|
|
#include "stock.hh"
|
|
#include "configuration.hh"
|
|
#include "posexcept.hh"
|
|
#include "area.hh"
|
|
#include "csvstringreader.hh"
|
|
#include "autocalibrationmesh.hh"
|
|
#include "autocalibrationline.hh"
|
|
|
|
#include <iostream>
|
|
#include <cmath>
|
|
|
|
using namespace std ;
|
|
|
|
|
|
|
|
/* *** Attribute definitions *** */
|
|
|
|
unordered_map<string, Building> Stock::buildings ;
|
|
|
|
unordered_map<Point3D, Waypoint> Stock::waypoints ;
|
|
|
|
unordered_map<string, Mobile> Stock::mobiles ;
|
|
|
|
unordered_map<string, CapturePoint> Stock::cps ;
|
|
|
|
unordered_set<ReferencePoint> 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() ;
|
|
cps.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)
|
|
{
|
|
auto 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.
|
|
* @returns A const reference to the Building.
|
|
* @throw element_not_found is thrown if the Building corresponding
|
|
* to `name` does not exist.
|
|
*/
|
|
const Building& Stock::get_building(const string &name)
|
|
{
|
|
auto 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 `point` was found.
|
|
* @returns nullptr if `point` does not belong to any Area.
|
|
*/
|
|
const Area* Stock::in_which_area_is(const Point3D &point)
|
|
{
|
|
for (auto b = buildings.begin() ; b != buildings.end() ; ++b)
|
|
{
|
|
const unordered_map<string, const Area*> &areas =
|
|
b->second.get_areas() ;
|
|
for (auto a = areas.begin() ; a != areas.end() ; ++a)
|
|
if (a->second->contains_point(point))
|
|
return a->second ;
|
|
}
|
|
|
|
return nullptr ;
|
|
}
|
|
|
|
|
|
|
|
/* *** Waypoint operations *** */
|
|
|
|
|
|
const Waypoint& Stock::
|
|
find_create_waypoint(const Point3D &point)
|
|
{
|
|
auto 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,
|
|
Building *const building)
|
|
{
|
|
auto 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())
|
|
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.
|
|
* @returns `true` if a mobile with this MAC address exists.
|
|
* @returns `false` if this MAC address was not found.
|
|
*/
|
|
bool Stock::mobile_exists(const std::string &mac)
|
|
{
|
|
auto 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.
|
|
* @returns A const reference to the Mobile.
|
|
* @throw element_not_found is thrown if the Mobile corresponding
|
|
* to `mac` does not exist.
|
|
*/
|
|
const Mobile& Stock::get_mobile(const string &mac)
|
|
{
|
|
auto 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)
|
|
{
|
|
auto 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 `source.`
|
|
*/
|
|
const Mobile& Stock::find_create_mobile(const Mobile &source)
|
|
{
|
|
const string &mac = source.get_mac_addr() ;
|
|
auto i = mobiles.find(mac) ;
|
|
if (i != mobiles.end())
|
|
return i->second ;
|
|
|
|
mobiles[mac] = source ;
|
|
return mobiles[mac] ;
|
|
}
|
|
|
|
|
|
|
|
/* *** CapturePoint operations *** */
|
|
|
|
|
|
/**
|
|
* @param mac The MAC address of the CapturePoint to search for.
|
|
* It must be a valid MAC address, as no check is performed.
|
|
* @returns `true` if a CP with this MAC address exists.
|
|
* @returns `false` if this MAC address was not found.
|
|
*/
|
|
bool Stock::cp_exists(const std::string &mac)
|
|
{
|
|
auto i = cps.find(mac) ;
|
|
return i != cps.end() ;
|
|
}
|
|
|
|
|
|
/**
|
|
* @param mac The MAC address of the CapturePoint to search for.
|
|
* It must be a valid MAC address, as no check is performed.
|
|
* @returns A const reference to the CapturePoint.
|
|
* @throw element_not_found is thrown if the CapturePoint corresponding
|
|
* to `mac` does not exist.
|
|
*/
|
|
const CapturePoint& Stock::get_cp(const string &mac)
|
|
{
|
|
auto i = cps.find(mac) ;
|
|
if (i != cps.end())
|
|
return i->second ;
|
|
throw element_not_found("No CapturePoint with MAC address \"" +
|
|
mac + "\"!") ;
|
|
}
|
|
|
|
|
|
/**
|
|
* The MAC address of the CapturePoint is initialised.
|
|
*/
|
|
const CapturePoint& Stock::find_create_cp(const string &mac)
|
|
{
|
|
auto i = cps.find(mac) ;
|
|
if (i != cps.end())
|
|
return i->second ;
|
|
|
|
CapturePoint &cp = cps[mac] ;
|
|
cp.set_mac_addr(mac) ;
|
|
|
|
if (Configuration::is_configured("verbose"))
|
|
cerr
|
|
<< "New CP \"" << mac
|
|
<< "\" (total: " << nb_cps() << " CPs).\n" ;
|
|
|
|
return cp ;
|
|
}
|
|
|
|
|
|
/**
|
|
* If the CapturePoint already exists, it is replaced by `source.`
|
|
*/
|
|
const CapturePoint& Stock::find_create_cp(const CapturePoint &source)
|
|
{
|
|
const string &mac = source.get_mac_addr() ;
|
|
auto i = cps.find(mac) ;
|
|
if (i != cps.end())
|
|
return i->second ;
|
|
|
|
cps[mac] = source ;
|
|
return cps[mac] ;
|
|
}
|
|
|
|
|
|
void Stock::update_all_friis_indexes()
|
|
{
|
|
for (auto cp = cps.begin() ; cp != cps.end() ; ++cp)
|
|
{
|
|
double friis_idx_sum = 0 ;
|
|
int nb_friis_idx = 0 ;
|
|
|
|
// Compute main general term, independant from scans
|
|
double const_term = cp->second.friis_constant_term() ;
|
|
|
|
/*
|
|
* Compute indexes for each ReferencePoint. The Friis index for an
|
|
* CP is the average of all indexes in all ReferencePoint.
|
|
*/
|
|
for (auto rp = reference_points.begin() ;
|
|
rp != reference_points.end() ; ++rp)
|
|
{
|
|
int nb_idx = 0 ;
|
|
float cp_friis_idx_sum =
|
|
rp->friis_indexes_for_cp(cp->second, const_term, nb_idx) ;
|
|
if (nb_idx != 0)
|
|
{
|
|
friis_idx_sum += cp_friis_idx_sum ;
|
|
nb_friis_idx += nb_idx ;
|
|
}
|
|
}
|
|
|
|
if (nb_friis_idx > 0)
|
|
cp->second.set_friis_index(friis_idx_sum / nb_friis_idx) ;
|
|
}
|
|
}
|
|
|
|
|
|
double Stock::cp_matrix_get_ss(const std::string &mac_transmitter,
|
|
const std::string &mac_receiver)
|
|
{
|
|
const CapturePoint &receiver = get_cp(mac_receiver) ;
|
|
const ReferencePoint &rp =
|
|
get_reference_point(receiver.get_coordinates()) ;
|
|
|
|
return rp.average_measurements(mac_transmitter) ;
|
|
}
|
|
|
|
|
|
/**
|
|
* @returns A const pointer to the CP `coord` is the coordinates of.
|
|
* @returns nullptr if `coord` is not the coordinates of any CP.
|
|
*/
|
|
const CapturePoint* Stock::is_cp_coordinate(const Point3D &coord)
|
|
{
|
|
for (auto cp = cps.begin() ; cp != cps.end() ; ++cp)
|
|
if (cp->second.get_coordinates() == coord)
|
|
return &cp->second ;
|
|
|
|
return nullptr ;
|
|
}
|
|
|
|
|
|
/**
|
|
* @returns The distance from `location` to the closest CP.
|
|
* @returns -1 if there are no CPs in stock.
|
|
*/
|
|
float Stock::distance_from_closest_cp(const Point3D &location)
|
|
{
|
|
auto cp = cps.begin() ;
|
|
if (cp == cps.end())
|
|
return -1 ;
|
|
|
|
float min_dist = location.square_distance(cp->second.get_coordinates()) ;
|
|
++cp ;
|
|
|
|
while (cp != cps.end())
|
|
{
|
|
float tmp_dist =
|
|
location.square_distance(cp->second.get_coordinates()) ;
|
|
if (tmp_dist < min_dist)
|
|
min_dist = tmp_dist ;
|
|
++cp ;
|
|
}
|
|
|
|
return sqrtf(min_dist) ;
|
|
}
|
|
|
|
|
|
|
|
/* *** ReferencePoint operations *** */
|
|
|
|
|
|
bool Stock::reference_point_exists(const ReferencePoint &point)
|
|
{
|
|
auto i = reference_points.find(point) ;
|
|
return i != reference_points.end() ;
|
|
}
|
|
|
|
|
|
/**
|
|
* @param point Coordinates of the wanted ReferencePoint.
|
|
* @returns 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 Point3D &point)
|
|
{
|
|
/* Note that we can look up a ReferencePoint in the unordered_set
|
|
* #reference_points using only its coordinates (Point3D) only because
|
|
* we defined `hash` and `equal_to` to take into consideration only
|
|
* the coordinates of the ReferencePoint (see referencepoint.hh).
|
|
*/
|
|
auto i = reference_points.find(ReferencePoint(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://en.cppreference.com/w/cpp/container/unordered_set/insert
|
|
auto 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_cps = Configuration::bool_value(
|
|
"positioning.nss.ignore-cp-reference-points") ;
|
|
auto i = reference_points.begin() ;
|
|
|
|
if (ignore_cps)
|
|
{
|
|
// Fast-forward to the next non-CP reference point
|
|
while (i != reference_points.end() && is_cp_coordinate(*i))
|
|
++i ;
|
|
|
|
// No non-CP reference point was found, we are forced to consider
|
|
// the CP reference points
|
|
if (i == reference_points.end())
|
|
ignore_cps = false ;
|
|
}
|
|
|
|
float similarity = i->similarity(request) ;
|
|
auto closest = i ;
|
|
|
|
for (++i ; i != reference_points.end() ; ++i)
|
|
{
|
|
if (ignore_cps && is_cp_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(Configuration::autocalibration_enabled()) ;
|
|
assert(! cps.empty()) ;
|
|
assert(! reference_points.empty()) ;
|
|
|
|
delete_non_cp_calibration_requests() ;
|
|
|
|
const string &ac_mode = Configuration::string_value(
|
|
"positioning.generate-reference-points") ;
|
|
assert(! ac_mode.empty()) ;
|
|
|
|
if (ac_mode.find("mesh") != string::npos)
|
|
generate_reference_points_mesh() ;
|
|
|
|
if (ac_mode.find("line") != string::npos)
|
|
generate_reference_points_line() ;
|
|
|
|
if (ac_mode.find("list") != string::npos)
|
|
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.") ;
|
|
|
|
if (reference_points.size() < 3)
|
|
{
|
|
if (Configuration::is_configured("verbose"))
|
|
cerr << reference_points.size() << " true reference points is"
|
|
<< " not enough to generate a meshing of reference"
|
|
<< " points.\n" ;
|
|
return ;
|
|
}
|
|
|
|
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)
|
|
{
|
|
AutocalibrationMesh ac(Point3D(x,y,z)) ;
|
|
generate_reference_point(ac) ;
|
|
}
|
|
}
|
|
|
|
|
|
void Stock::generate_reference_points_line()
|
|
{
|
|
if (! Configuration::is_configured("positioning.generated-line-path"))
|
|
throw missing_configuration(
|
|
"In order to generate a line of reference points, you must"
|
|
" define a path.") ;
|
|
|
|
if (reference_points.empty())
|
|
{
|
|
if (Configuration::is_configured("verbose"))
|
|
cerr << "At least one true reference points is necessary to"
|
|
<< " generate a path of reference points.\n" ;
|
|
return ;
|
|
}
|
|
|
|
const string &path_csv =
|
|
Configuration::string_value("positioning.generated-line-path") ;
|
|
CSVStringReader path_parser(path_csv) ;
|
|
Point3D p ;
|
|
vector<Point3D> read_points ;
|
|
const CapturePoint *cp1 = nullptr ;
|
|
|
|
// Read points until the first CP coordinate
|
|
while (path_parser.read_point3d(p))
|
|
{
|
|
cp1 = Stock::is_cp_coordinate(p) ;
|
|
if (cp1)
|
|
break ;
|
|
read_points.push_back(p) ;
|
|
}
|
|
|
|
// We reached the end of the points' list without finding the
|
|
// coordinates of a CP
|
|
if (! cp1)
|
|
{
|
|
cerr << "Warning! positioning.generated-line-path doesn't contain"
|
|
<< "any point that is the coordinates of capture point!"
|
|
<< endl ;
|
|
return ;
|
|
}
|
|
|
|
// Generate the first RPs (between the begining and cp1)
|
|
if (! read_points.empty())
|
|
{
|
|
auto first = read_points.begin() ;
|
|
for (auto end = first + 1 ; end != read_points.end() ; ++end)
|
|
{
|
|
generate_interpolated_reference_points(*first, *end, cp1) ;
|
|
first = end ;
|
|
}
|
|
generate_interpolated_reference_points(
|
|
*first, cp1->get_coordinates(), cp1) ;
|
|
read_points.clear() ;
|
|
}
|
|
|
|
// Read the next points and generate the RPs
|
|
const CapturePoint *cp2 = nullptr ;
|
|
while (path_parser.read_point3d(p))
|
|
{
|
|
cp2 = Stock::is_cp_coordinate(p) ;
|
|
if (! cp2)
|
|
{
|
|
read_points.push_back(p) ;
|
|
continue ;
|
|
}
|
|
|
|
// We don't have any intermediate points between the two CPs
|
|
if (read_points.empty())
|
|
generate_interpolated_reference_points(
|
|
cp1->get_coordinates(), cp2->get_coordinates(), cp1, cp2) ;
|
|
|
|
// Generate the RPs between cp1, the intermediate points, and cp2
|
|
else
|
|
{
|
|
auto first = read_points.begin() ;
|
|
generate_interpolated_reference_points(
|
|
cp1->get_coordinates(), *first, cp1, cp2) ;
|
|
for (auto end = first + 1 ; end != read_points.end() ; ++end)
|
|
{
|
|
generate_interpolated_reference_points(*first, *end,
|
|
cp1, cp2) ;
|
|
first = end ;
|
|
}
|
|
generate_interpolated_reference_points(
|
|
*first, cp2->get_coordinates(), cp1, cp2) ;
|
|
read_points.clear() ;
|
|
}
|
|
|
|
cp1 = cp2 ;
|
|
cp2 = nullptr ;
|
|
}
|
|
|
|
// Generate the last RPs (after the last RP)
|
|
if (read_points.empty())
|
|
return ;
|
|
auto first = read_points.begin() ;
|
|
generate_interpolated_reference_points(
|
|
cp1->get_coordinates(), *first, cp1) ;
|
|
for (auto end = first + 1 ; end != read_points.end() ; ++end)
|
|
{
|
|
generate_interpolated_reference_points(*first, *end, cp1) ;
|
|
first = end ;
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* This function interpolates the coordinates between `start` and `end`
|
|
* and generates all the corresponding reference points (the two
|
|
* extremities are generated as well).
|
|
*
|
|
* @param start The first point of the line.
|
|
* @param end The last point of the line.
|
|
* @param cp1 The first capture point to use to generate the reference
|
|
* points.
|
|
* @param cp2 The second capture point to use (optional).
|
|
*/
|
|
void Stock::generate_interpolated_reference_points(
|
|
const Point3D &start, const Point3D &end,
|
|
const CapturePoint *const cp1, const CapturePoint *const cp2)
|
|
{
|
|
assert(cp1) ;
|
|
|
|
// Generate the first RP
|
|
AutocalibrationLine ac_start(start, cp1, cp2) ;
|
|
generate_reference_point(ac_start) ;
|
|
|
|
/* Compute the intermediate coordinates */
|
|
float step_hint =
|
|
Configuration::float_value("positioning.generated-line-step") ;
|
|
vector<Point3D> interpolated ; // List of intermediate points
|
|
// Generate the intermediate coordinates
|
|
start.interpolate(end, step_hint, interpolated) ;
|
|
|
|
// Generate the corresponding reference points
|
|
for (auto i = interpolated.begin() ; i != interpolated.end() ; ++i)
|
|
{
|
|
AutocalibrationLine ac(*i, cp1, cp2) ;
|
|
generate_reference_point(ac) ;
|
|
}
|
|
|
|
// Generate the last RP
|
|
AutocalibrationLine ac_end(end, cp1, cp2) ;
|
|
generate_reference_point(ac_end) ;
|
|
}
|
|
|
|
|
|
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.") ;
|
|
|
|
if (reference_points.size() < 3)
|
|
{
|
|
if (Configuration::is_configured("verbose"))
|
|
cerr << reference_points.size() << " true reference points is"
|
|
<< " not enough to generate a meshing of reference"
|
|
<< " points.\n" ;
|
|
return ;
|
|
}
|
|
|
|
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))
|
|
{
|
|
AutocalibrationMesh ac(rp) ;
|
|
generate_reference_point(ac) ;
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* @param ac The pre-initialised Autocalibration to use.
|
|
*/
|
|
void Stock::generate_reference_point(Autocalibration &ac)
|
|
{
|
|
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
|
|
auto 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(const int timeout)
|
|
{
|
|
assert(timeout > 0) ;
|
|
|
|
auto 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)
|
|
{
|
|
auto 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_cps = Configuration::bool_value(
|
|
"positioning.nss.ignore-cp-reference-points") ;
|
|
auto i = calibration_requests.begin() ;
|
|
|
|
if (ignore_cps)
|
|
{
|
|
// Fast-forward to the next non-CP reference point
|
|
while (i != calibration_requests.end() &&
|
|
is_cp_coordinate(*i->get_reference_point()))
|
|
++i ;
|
|
|
|
// No non-CP reference point was found, we are forced to consider
|
|
// the CP reference points
|
|
if (i == calibration_requests.end())
|
|
{
|
|
i = calibration_requests.begin() ;
|
|
ignore_cps = false ;
|
|
}
|
|
}
|
|
|
|
float similarity = i->similarity(request) ;
|
|
auto closest = i ;
|
|
|
|
for (++i ; i != calibration_requests.end() ; ++i)
|
|
{
|
|
if (ignore_cps && is_cp_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_cp_calibration_requests()
|
|
{
|
|
auto 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 ;
|
|
}
|
|
}
|