[Positioner] Add AutocalibrationMesh (refactored)
Refactor pieces of Autocalibration into the new class AutocalibrationMesh, in order to allow the addition of new autocalibration sub-classes.
This commit is contained in:
parent
b436526d40
commit
2ac9cc2c1d
|
@ -44,6 +44,7 @@ set(OWLPS_POSITIONER_CLASSES_SRC_FILES
|
||||||
capturepointsreadercsv.cc
|
capturepointsreadercsv.cc
|
||||||
area.cc
|
area.cc
|
||||||
autocalibration.cc
|
autocalibration.cc
|
||||||
|
autocalibrationmesh.cc
|
||||||
building.cc
|
building.cc
|
||||||
calibrationrequest.cc
|
calibrationrequest.cc
|
||||||
cartographyalgorithm.cc
|
cartographyalgorithm.cc
|
||||||
|
|
|
@ -14,15 +14,12 @@
|
||||||
|
|
||||||
#include "autocalibration.hh"
|
#include "autocalibration.hh"
|
||||||
|
|
||||||
#include "capturepoint.hh"
|
|
||||||
#include "stock.hh"
|
#include "stock.hh"
|
||||||
#include "configuration.hh"
|
#include "configuration.hh"
|
||||||
#include "posexcept.hh"
|
#include "posexcept.hh"
|
||||||
|
|
||||||
#include <map>
|
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <sstream>
|
|
||||||
|
|
||||||
using namespace std ;
|
using namespace std ;
|
||||||
|
|
||||||
|
@ -36,8 +33,9 @@ uint32_t Autocalibration::nb_virtual_mobiles = 0 ;
|
||||||
|
|
||||||
/* *** Operations *** */
|
/* *** Operations *** */
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* The reference point P we want to generate will end-up containing
|
* The reference point P we want to generate will end up containing
|
||||||
* as many calibration requests as the total number of CPs.
|
* as many calibration requests as the total number of CPs.
|
||||||
* For each of these requests, we consider a transmitter TRX (which is
|
* For each of these requests, we consider a transmitter TRX (which is
|
||||||
* a virtual mobile located in P) and a receiver RX (the current CP).
|
* a virtual mobile located in P) and a receiver RX (the current CP).
|
||||||
|
@ -68,7 +66,10 @@ void Autocalibration::generate_reference_point()
|
||||||
|
|
||||||
/* Generate the SS(s) for each CP */
|
/* Generate the SS(s) for each CP */
|
||||||
for (rx = Stock::cps.begin() ; rx != Stock::cps.end() ; ++rx)
|
for (rx = Stock::cps.begin() ; rx != Stock::cps.end() ; ++rx)
|
||||||
generate_ss() ;
|
{
|
||||||
|
if (init_ss_generation())
|
||||||
|
generate_ss() ;
|
||||||
|
}
|
||||||
assert(! measurements.empty()) ;
|
assert(! measurements.empty()) ;
|
||||||
|
|
||||||
/* Create the virtual mobile */
|
/* Create the virtual mobile */
|
||||||
|
@ -86,7 +87,11 @@ void Autocalibration::generate_reference_point()
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void Autocalibration::generate_ss()
|
/**
|
||||||
|
* @returns `true` if the SS can be generated.
|
||||||
|
* @returns `false` in case of error.
|
||||||
|
*/
|
||||||
|
bool Autocalibration::init_ss_generation()
|
||||||
{
|
{
|
||||||
/* Update the mobile's attributes */
|
/* Update the mobile's attributes */
|
||||||
vmob_gain += rx->second.get_antenna_gain() / Stock::cps.size() ;
|
vmob_gain += rx->second.get_antenna_gain() / Stock::cps.size() ;
|
||||||
|
@ -96,153 +101,16 @@ void Autocalibration::generate_ss()
|
||||||
* to generate */
|
* to generate */
|
||||||
const Point3D &rx_coord = rx->second.get_coordinates() ;
|
const Point3D &rx_coord = rx->second.get_coordinates() ;
|
||||||
if (rx_coord.get_z() != point.get_z())
|
if (rx_coord.get_z() != point.get_z())
|
||||||
return ;
|
return false ;
|
||||||
|
|
||||||
/* Compute the origin angle P-RX-O */
|
return true ;
|
||||||
Point3D origin(rx_coord) ;
|
|
||||||
origin.set_x(rx_coord.get_x() + 10) ;
|
|
||||||
origin_angle = rx_coord.angle_2d(point, origin) ;
|
|
||||||
if (point.get_y() > rx_coord.get_y())
|
|
||||||
origin_angle = -origin_angle ;
|
|
||||||
|
|
||||||
/* Choose the nearest CP(s) in angle */
|
|
||||||
sort_reference_cps() ;
|
|
||||||
// We need at least one reference CP:
|
|
||||||
if (sorted_negative_angles.empty() && sorted_positive_angles.empty())
|
|
||||||
throw autocalibration_error("Not enough CPs in coverage!") ;
|
|
||||||
|
|
||||||
/* Compute the angle weight of the reference CPs */
|
|
||||||
weight_cps() ;
|
|
||||||
|
|
||||||
/* Compute the SS that would be received from a mobile at a
|
|
||||||
* distance of RX-P, in the direction of each reference CP */
|
|
||||||
compute_ss() ;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* The reference (transmitter) CPs are first filtered (floor, coverage),
|
* This function is a simple wrapper around compute_single_packet_ss()
|
||||||
* then sorted according to their angles with RX and P.
|
* and compute_multi_packet_ss(), depending on the configuration.
|
||||||
*/
|
*/
|
||||||
void Autocalibration::sort_reference_cps()
|
|
||||||
{
|
|
||||||
const Point3D &rx_coord = rx->second.get_coordinates() ;
|
|
||||||
|
|
||||||
sorted_negative_angles.clear() ;
|
|
||||||
sorted_positive_angles.clear() ;
|
|
||||||
|
|
||||||
for (unordered_map<string, CapturePoint>::const_iterator
|
|
||||||
ref = Stock::cps.begin() ; ref != Stock::cps.end() ; ++ref)
|
|
||||||
{
|
|
||||||
if (ref == rx)
|
|
||||||
continue ;
|
|
||||||
|
|
||||||
/* Skip the CP if it is not at the same floor than the
|
|
||||||
* receiver CP */
|
|
||||||
const Point3D &ref_coord = ref->second.get_coordinates() ;
|
|
||||||
if (ref_coord.get_z() != rx_coord.get_z())
|
|
||||||
continue ;
|
|
||||||
|
|
||||||
/* Skip the CP if it is not in coverage with the receiver CP */
|
|
||||||
float coverage =
|
|
||||||
rx->second.received_calibration_from_cp(ref->second) ;
|
|
||||||
if (coverage < 1) // Less than 1% coverage is ridiculous!
|
|
||||||
continue ;
|
|
||||||
|
|
||||||
/* Angle P-RX-REF */
|
|
||||||
double angle = rx_coord.angle_2d(point, ref_coord) ;
|
|
||||||
|
|
||||||
/* Weight in the CPs' list according to coverage and angle */
|
|
||||||
double weight = angle / coverage ;
|
|
||||||
/* Note: this weight is used only to sort the CPs, it has nothing
|
|
||||||
to do with the angle weight (see weight_cps()) used to compute
|
|
||||||
the SSs. */
|
|
||||||
|
|
||||||
/* Create the list entry */
|
|
||||||
pair<double,
|
|
||||||
unordered_map<string, CapturePoint>::const_iterator>
|
|
||||||
angle_cp(angle, ref) ;
|
|
||||||
pair<double, pair<double,
|
|
||||||
unordered_map<string, CapturePoint>::const_iterator> >
|
|
||||||
weight_angle_cp(weight, angle_cp) ;
|
|
||||||
|
|
||||||
/* Rotate the CP's coordinates to know the angle direction */
|
|
||||||
Point3D ref_coord_r(ref_coord) ;
|
|
||||||
ref_coord_r.rotate_2d(rx_coord, origin_angle) ;
|
|
||||||
// Insert the CP in the right list, according to the direction:
|
|
||||||
if (ref_coord_r.get_y() < rx_coord.get_y())
|
|
||||||
sorted_negative_angles.insert(weight_angle_cp) ;
|
|
||||||
else
|
|
||||||
sorted_positive_angles.insert(weight_angle_cp) ;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void Autocalibration::weight_cps()
|
|
||||||
{
|
|
||||||
/* Retrieve the reference CPs & angles */
|
|
||||||
ref_cps.clear() ;
|
|
||||||
map<double, pair<double,
|
|
||||||
unordered_map<string, CapturePoint>::const_iterator> >
|
|
||||||
::const_iterator s ;
|
|
||||||
s = sorted_negative_angles.begin() ;
|
|
||||||
if (s != sorted_negative_angles.end())
|
|
||||||
init_cp(s) ;
|
|
||||||
s = sorted_positive_angles.begin() ;
|
|
||||||
if (s != sorted_positive_angles.end())
|
|
||||||
init_cp(s) ;
|
|
||||||
|
|
||||||
/* Weight the CPs */
|
|
||||||
if (ref_cps.size() == 1)
|
|
||||||
ref_cps[0].weight = 100 ;
|
|
||||||
else if (ref_cps.size() == 2)
|
|
||||||
weight_2_cps() ;
|
|
||||||
else
|
|
||||||
{
|
|
||||||
ostringstream oss ;
|
|
||||||
oss << "We should not be here... error when sorting the reference"
|
|
||||||
<< " CPs? (" << ref_cps.size() << " CPs sorted)" ;
|
|
||||||
throw autocalibration_error(oss.str()) ;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void Autocalibration::init_cp(
|
|
||||||
map<double, pair<double, unordered_map<
|
|
||||||
string, CapturePoint>::const_iterator> >::const_iterator s)
|
|
||||||
{
|
|
||||||
struct cp ref ;
|
|
||||||
ref.cp = &s->second.second->second ;
|
|
||||||
ref.angle = s->second.first ; // Angle P-REF-RX
|
|
||||||
ref_cps.push_back(ref) ;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void Autocalibration::weight_2_cps()
|
|
||||||
{
|
|
||||||
assert(ref_cps.size() > 1) ;
|
|
||||||
|
|
||||||
/* Compute the angle REF1-RX-REF2 */
|
|
||||||
double reference_angle = ref_cps[0].angle + ref_cps[1].angle ;
|
|
||||||
/* Note: the reference angle must be the sum of the two angles
|
|
||||||
P-RX-REF1 and P-RX-REF2 instead of simply REF1-RX-REF2, to allow
|
|
||||||
the angle to be within [0;360[° instead of [0;180[°. This is needed
|
|
||||||
if P-RX-REF1 or P-RX-REF2 are greater than 90°, otherwise the
|
|
||||||
weights would be wrong. */
|
|
||||||
|
|
||||||
/* Weight the two CPs */
|
|
||||||
if (reference_angle == 0)
|
|
||||||
ref_cps[0].weight = ref_cps[1].weight = 50 ;
|
|
||||||
else
|
|
||||||
{
|
|
||||||
ref_cps[0].weight = ref_cps[0].angle * 100 / reference_angle ;
|
|
||||||
if (ref_cps[0].weight > 100)
|
|
||||||
ref_cps[0].weight = 100 ;
|
|
||||||
ref_cps[1].weight = 100 - ref_cps[0].weight ;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void Autocalibration::compute_ss()
|
void Autocalibration::compute_ss()
|
||||||
{
|
{
|
||||||
if (Configuration::
|
if (Configuration::
|
||||||
|
@ -251,120 +119,3 @@ void Autocalibration::compute_ss()
|
||||||
else
|
else
|
||||||
compute_single_packet_ss(0) ;
|
compute_single_packet_ss(0) ;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void Autocalibration::compute_multi_packet_ss()
|
|
||||||
{
|
|
||||||
const cp &ref1 = ref_cps[0] ;
|
|
||||||
const ReferencePoint &ref1_rp =
|
|
||||||
Stock::get_reference_point(ref1.cp->get_coordinates()) ;
|
|
||||||
const vector<CalibrationRequest*> &ref1_cr =
|
|
||||||
ref1_rp.get_requests() ;
|
|
||||||
|
|
||||||
/* Search for the first measurement made by RX in ref1_rp */
|
|
||||||
const Measurement *rx_measurement = NULL ;
|
|
||||||
vector<CalibrationRequest*>::const_iterator cr ;
|
|
||||||
const string &rx_mac = rx->second.get_mac_addr() ;
|
|
||||||
for (cr = ref1_cr.begin() ; cr != ref1_cr.end() ; ++cr)
|
|
||||||
{
|
|
||||||
rx_measurement = (*cr)->get_measurement(rx_mac) ;
|
|
||||||
if (rx_measurement)
|
|
||||||
break ;
|
|
||||||
}
|
|
||||||
|
|
||||||
// The selected reference CPs are in coverage of RX, therefore
|
|
||||||
// we should always find a measurement of RX in ref1_rp
|
|
||||||
assert(rx_measurement) ;
|
|
||||||
|
|
||||||
/* Generate the SS for each packet */
|
|
||||||
bool packet_generated = false ;
|
|
||||||
for (pkt_id_t pkt_id = 1 ; pkt_id <= (*cr)->get_nb_packets() ;
|
|
||||||
++pkt_id)
|
|
||||||
if (compute_single_packet_ss(pkt_id))
|
|
||||||
packet_generated = true ;
|
|
||||||
|
|
||||||
if (! packet_generated)
|
|
||||||
{
|
|
||||||
if (Configuration::is_configured("verbose"))
|
|
||||||
cerr << "No packet has been generated for RX CP " << rx_mac
|
|
||||||
<< "... failing back to single packet request. Calibration"
|
|
||||||
<< " request involved : " << **cr << "\n" ;
|
|
||||||
compute_single_packet_ss(0) ;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @param pkt_id The number of the packet to create. Must be 0 if all
|
|
||||||
* the generated reference points contain only one packet.
|
|
||||||
*/
|
|
||||||
bool Autocalibration::compute_single_packet_ss(pkt_id_t pkt_id)
|
|
||||||
{
|
|
||||||
/* Distance RX-P */
|
|
||||||
const Point3D &rx_coord = rx->second.get_coordinates() ;
|
|
||||||
float point_dst = rx_coord.distance(point) ;
|
|
||||||
|
|
||||||
/* Compute the consolidated SS */
|
|
||||||
double rx_ss = 0 ;
|
|
||||||
for (unsigned int i = 0 ; i < ref_cps.size() ; ++i)
|
|
||||||
{
|
|
||||||
double tmp_ss = compute_weighted_ss(i, pkt_id, point_dst) ;
|
|
||||||
if (tmp_ss > 0) // error
|
|
||||||
return false ;
|
|
||||||
rx_ss += tmp_ss ;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Add the SS to the measurements' list */
|
|
||||||
const string &rx_mac = rx->second.get_mac_addr() ;
|
|
||||||
// 0 is not a valid packet ID, so we use 1 if we generate only one
|
|
||||||
// packet per reference point:
|
|
||||||
if (pkt_id == 0)
|
|
||||||
pkt_id = 1 ;
|
|
||||||
// The Measurement measurements[rx_mac] is created if needed:
|
|
||||||
measurements[rx_mac].add_ss(pkt_id, rx_ss) ;
|
|
||||||
// Useful only if the measurement was just created, but it is not
|
|
||||||
// worth a test:
|
|
||||||
measurements[rx_mac].set_cp(&rx->second) ;
|
|
||||||
|
|
||||||
return true ;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @param cp_idx The index of the CP in #ref_cps.
|
|
||||||
* @param pkt_id The number of the packet to generate, or 0 if we
|
|
||||||
* generate only one packet per reference point.
|
|
||||||
* @param point_dst The distance RX-P.
|
|
||||||
*
|
|
||||||
* @returns The weighted SS value in dBm, or 1 in case of error.
|
|
||||||
*/
|
|
||||||
double Autocalibration::compute_weighted_ss(
|
|
||||||
unsigned int cp_idx, pkt_id_t pkt_id, float point_dst)
|
|
||||||
{
|
|
||||||
const cp &ref = ref_cps[cp_idx] ;
|
|
||||||
const ReferencePoint &rp =
|
|
||||||
Stock::get_reference_point(ref.cp->get_coordinates()) ;
|
|
||||||
|
|
||||||
/* Friis index */
|
|
||||||
const string &rx_mac = rx->second.get_mac_addr() ;
|
|
||||||
float friis_idx ;
|
|
||||||
if (pkt_id == 0)
|
|
||||||
friis_idx = rp.friis_index_for_cp(rx_mac) ;
|
|
||||||
else
|
|
||||||
{
|
|
||||||
friis_idx = rp.friis_index_for_cp(rx_mac, pkt_id) ;
|
|
||||||
if (friis_idx == 0)
|
|
||||||
return 1 ; // The packet pkt_id does not exist in rp
|
|
||||||
}
|
|
||||||
|
|
||||||
/* SS */
|
|
||||||
// Constant term
|
|
||||||
double common_ss = rx->second.friis_constant_term() ;
|
|
||||||
double ss =
|
|
||||||
common_ss +
|
|
||||||
ref.cp->get_trx_power() +
|
|
||||||
ref.cp->get_antenna_gain() -
|
|
||||||
10 * friis_idx * log10(point_dst) ;
|
|
||||||
|
|
||||||
return ss * ref.weight / 100 ;
|
|
||||||
}
|
|
||||||
|
|
|
@ -15,81 +15,45 @@
|
||||||
#ifndef _OWLPS_POSITIONING_AUTOCALIBRATION_HH_
|
#ifndef _OWLPS_POSITIONING_AUTOCALIBRATION_HH_
|
||||||
#define _OWLPS_POSITIONING_AUTOCALIBRATION_HH_
|
#define _OWLPS_POSITIONING_AUTOCALIBRATION_HH_
|
||||||
|
|
||||||
class CapturePoint ;
|
|
||||||
class Point3D ;
|
class Point3D ;
|
||||||
|
|
||||||
#include "measurement.hh"
|
#include "measurement.hh"
|
||||||
|
|
||||||
#include <vector>
|
/// Base class for the autocalibration methods
|
||||||
#include <unordered_map>
|
|
||||||
|
|
||||||
/// Generates reference points from autocalibration measurements
|
|
||||||
class Autocalibration
|
class Autocalibration
|
||||||
{
|
{
|
||||||
private:
|
private:
|
||||||
struct cp
|
|
||||||
{
|
|
||||||
const CapturePoint *cp ;
|
|
||||||
float weight ; // Weight of the CP in percents
|
|
||||||
float angle ;
|
|
||||||
} ;
|
|
||||||
|
|
||||||
/// Current CP to generate a SS for
|
|
||||||
std::unordered_map<std::string, CapturePoint>::const_iterator rx ;
|
|
||||||
/// Angle P-RX-O, O being the origin of the trigonometric circle
|
|
||||||
float origin_angle ;
|
|
||||||
/// Selected transmitter CPs
|
|
||||||
std::vector<cp> ref_cps ;
|
|
||||||
/// Angles of the transmitter CPs (before M on the trigonometric
|
|
||||||
/// circle)
|
|
||||||
/**
|
|
||||||
* Note that the angles are stored in absolute value.
|
|
||||||
*/
|
|
||||||
std::multimap<double,
|
|
||||||
std::pair<double,
|
|
||||||
std::unordered_map<
|
|
||||||
std::string, CapturePoint>::const_iterator> >
|
|
||||||
sorted_negative_angles ;
|
|
||||||
/// Angles of the transmitter CPs (after M on the trigonometric circle)
|
|
||||||
std::multimap<double,
|
|
||||||
std::pair<double,
|
|
||||||
std::unordered_map<
|
|
||||||
std::string, CapturePoint>::const_iterator> >
|
|
||||||
sorted_positive_angles ;
|
|
||||||
/// Characteristics of the virtual mobile
|
|
||||||
double vmob_gain, vmob_pow ;
|
|
||||||
/// Generated measurements' list
|
|
||||||
std::unordered_map<std::string, Measurement> measurements ;
|
|
||||||
|
|
||||||
/// Initialises a struct cp with the values from `s`
|
|
||||||
void init_cp(
|
|
||||||
std::map<double, std::pair<double, std::unordered_map<
|
|
||||||
std::string, CapturePoint>::const_iterator> >::const_iterator s) ;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
/// Number of generated "virtual" mobiles
|
/// Number of generated "virtual" mobiles
|
||||||
static uint32_t nb_virtual_mobiles ;
|
static uint32_t nb_virtual_mobiles ;
|
||||||
|
|
||||||
|
/// Characteristics of the virtual mobile
|
||||||
|
double vmob_gain, vmob_pow ;
|
||||||
|
|
||||||
|
/// Work to do before actually generating a SS with generate_ss()
|
||||||
|
bool init_ss_generation(void) ;
|
||||||
|
|
||||||
|
protected:
|
||||||
/// The coordinates of the reference point to generate
|
/// The coordinates of the reference point to generate
|
||||||
const Point3D &point ;
|
const Point3D &point ;
|
||||||
|
|
||||||
|
/// Current CP to generate a SS for
|
||||||
|
std::unordered_map<std::string, CapturePoint>::const_iterator rx ;
|
||||||
|
|
||||||
|
/// Generated measurements' list
|
||||||
|
std::unordered_map<std::string, Measurement> measurements ;
|
||||||
|
|
||||||
/// Generates the SSs for all the CPs
|
/// Generates the SSs for all the CPs
|
||||||
void generate_ss(void) ;
|
/**
|
||||||
/// Sorts the reference CPs for a receiver CP
|
* Once the preliminary work done, this function should call
|
||||||
void sort_reference_cps(void) ;
|
* compute_ss() to actually compute the signal strength.
|
||||||
/// Computes the weight of the selected CP(s)
|
*/
|
||||||
void weight_cps(void) ;
|
virtual void generate_ss(void) = 0 ;
|
||||||
/// Weights two CPs according to their angles
|
|
||||||
void weight_2_cps(void) ;
|
|
||||||
/// Computes the SS of the virtual mobile
|
/// Computes the SS of the virtual mobile
|
||||||
void compute_ss(void) ;
|
void compute_ss(void) ;
|
||||||
/// Computes a SS with several packets in it
|
/// Computes a SS with several packets in it
|
||||||
void compute_multi_packet_ss(void) ;
|
virtual void compute_multi_packet_ss(void) = 0 ;
|
||||||
/// Computes a SS with only one packet in it
|
/// Computes a SS with only one packet in it
|
||||||
bool compute_single_packet_ss(pkt_id_t pkt_id) ;
|
virtual bool compute_single_packet_ss(pkt_id_t pkt_id) = 0 ;
|
||||||
/// Computes a SS according to the weight of the CP
|
|
||||||
double compute_weighted_ss(
|
|
||||||
unsigned int cp_idx, pkt_id_t pkt_id, float point_dst) ;
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
Autocalibration(const Point3D &_point): point(_point) {}
|
Autocalibration(const Point3D &_point): point(_point) {}
|
||||||
|
@ -99,4 +63,3 @@ public:
|
||||||
} ;
|
} ;
|
||||||
|
|
||||||
#endif // _OWLPS_POSITIONING_AUTOCALIBRATION_HH_
|
#endif // _OWLPS_POSITIONING_AUTOCALIBRATION_HH_
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1,297 @@
|
||||||
|
/*
|
||||||
|
* 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
|
||||||
|
* http://code.lm7.fr/p/owlps/source/tree/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 "autocalibrationmesh.hh"
|
||||||
|
|
||||||
|
#include "point3d.hh"
|
||||||
|
#include "capturepoint.hh"
|
||||||
|
#include "stock.hh"
|
||||||
|
#include "configuration.hh"
|
||||||
|
#include "posexcept.hh"
|
||||||
|
|
||||||
|
#include <map>
|
||||||
|
#include <string>
|
||||||
|
#include <iostream>
|
||||||
|
#include <sstream>
|
||||||
|
|
||||||
|
using namespace std ;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/* *** Operations *** */
|
||||||
|
|
||||||
|
|
||||||
|
void AutocalibrationMesh::generate_ss()
|
||||||
|
{
|
||||||
|
/* Compute the origin angle P-RX-O */
|
||||||
|
const Point3D &rx_coord = rx->second.get_coordinates() ;
|
||||||
|
Point3D origin(rx_coord) ;
|
||||||
|
origin.set_x(rx_coord.get_x() + 10) ;
|
||||||
|
origin_angle = rx_coord.angle_2d(point, origin) ;
|
||||||
|
if (point.get_y() > rx_coord.get_y())
|
||||||
|
origin_angle = -origin_angle ;
|
||||||
|
|
||||||
|
/* Choose the nearest CP(s) in angle */
|
||||||
|
sort_reference_cps() ;
|
||||||
|
// We need at least one reference CP:
|
||||||
|
if (sorted_negative_angles.empty() && sorted_positive_angles.empty())
|
||||||
|
throw autocalibration_error("Not enough CPs in coverage!") ;
|
||||||
|
|
||||||
|
/* Compute the angle weight of the reference CPs */
|
||||||
|
weight_cps() ;
|
||||||
|
|
||||||
|
/* Compute the SS that would be received from a mobile at a
|
||||||
|
* distance of RX-P, in the direction of each reference CP */
|
||||||
|
compute_ss() ;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* The reference (transmitter) CPs are first filtered (floor, coverage),
|
||||||
|
* then sorted according to their angles with RX and P.
|
||||||
|
*/
|
||||||
|
void AutocalibrationMesh::sort_reference_cps()
|
||||||
|
{
|
||||||
|
const Point3D &rx_coord = rx->second.get_coordinates() ;
|
||||||
|
|
||||||
|
sorted_negative_angles.clear() ;
|
||||||
|
sorted_positive_angles.clear() ;
|
||||||
|
|
||||||
|
for (unordered_map<string, CapturePoint>::const_iterator
|
||||||
|
ref = Stock::cps.begin() ; ref != Stock::cps.end() ; ++ref)
|
||||||
|
{
|
||||||
|
if (ref == rx)
|
||||||
|
continue ;
|
||||||
|
|
||||||
|
/* Skip the CP if it is not at the same floor than the
|
||||||
|
* receiver CP */
|
||||||
|
const Point3D &ref_coord = ref->second.get_coordinates() ;
|
||||||
|
if (ref_coord.get_z() != rx_coord.get_z())
|
||||||
|
continue ;
|
||||||
|
|
||||||
|
/* Skip the CP if it is not in coverage with the receiver CP */
|
||||||
|
float coverage =
|
||||||
|
rx->second.received_calibration_from_cp(ref->second) ;
|
||||||
|
if (coverage < 1) // Less than 1% coverage is ridiculous!
|
||||||
|
continue ;
|
||||||
|
|
||||||
|
/* Angle P-RX-REF */
|
||||||
|
double angle = rx_coord.angle_2d(point, ref_coord) ;
|
||||||
|
|
||||||
|
/* Weight in the CPs' list according to coverage and angle */
|
||||||
|
double weight = angle / coverage ;
|
||||||
|
/* Note: this weight is used only to sort the CPs, it has nothing
|
||||||
|
to do with the angle weight (see weight_cps()) used to compute
|
||||||
|
the SSs. */
|
||||||
|
|
||||||
|
/* Create the list entry */
|
||||||
|
pair<double,
|
||||||
|
unordered_map<string, CapturePoint>::const_iterator>
|
||||||
|
angle_cp(angle, ref) ;
|
||||||
|
pair<double, pair<double,
|
||||||
|
unordered_map<string, CapturePoint>::const_iterator> >
|
||||||
|
weight_angle_cp(weight, angle_cp) ;
|
||||||
|
|
||||||
|
/* Rotate the CP's coordinates to know the angle direction */
|
||||||
|
Point3D ref_coord_r(ref_coord) ;
|
||||||
|
ref_coord_r.rotate_2d(rx_coord, origin_angle) ;
|
||||||
|
// Insert the CP in the right list, according to the direction:
|
||||||
|
if (ref_coord_r.get_y() < rx_coord.get_y())
|
||||||
|
sorted_negative_angles.insert(weight_angle_cp) ;
|
||||||
|
else
|
||||||
|
sorted_positive_angles.insert(weight_angle_cp) ;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void AutocalibrationMesh::weight_cps()
|
||||||
|
{
|
||||||
|
/* Retrieve the reference CPs & angles */
|
||||||
|
ref_cps.clear() ;
|
||||||
|
map<double, pair<double,
|
||||||
|
unordered_map<string, CapturePoint>::const_iterator> >
|
||||||
|
::const_iterator s ;
|
||||||
|
s = sorted_negative_angles.begin() ;
|
||||||
|
if (s != sorted_negative_angles.end())
|
||||||
|
init_cp(s) ;
|
||||||
|
s = sorted_positive_angles.begin() ;
|
||||||
|
if (s != sorted_positive_angles.end())
|
||||||
|
init_cp(s) ;
|
||||||
|
|
||||||
|
/* Weight the CPs */
|
||||||
|
if (ref_cps.size() == 1)
|
||||||
|
ref_cps[0].weight = 100 ;
|
||||||
|
else if (ref_cps.size() == 2)
|
||||||
|
weight_2_cps() ;
|
||||||
|
else
|
||||||
|
{
|
||||||
|
ostringstream oss ;
|
||||||
|
oss << "We should not be here... error when sorting the reference"
|
||||||
|
<< " CPs? (" << ref_cps.size() << " CPs sorted)" ;
|
||||||
|
throw autocalibration_error(oss.str()) ;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void AutocalibrationMesh::init_cp(
|
||||||
|
map<double, pair<double, unordered_map<
|
||||||
|
string, CapturePoint>::const_iterator> >::const_iterator s)
|
||||||
|
{
|
||||||
|
struct cp ref ;
|
||||||
|
ref.cp = &s->second.second->second ;
|
||||||
|
ref.angle = s->second.first ; // Angle P-REF-RX
|
||||||
|
ref_cps.push_back(ref) ;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void AutocalibrationMesh::weight_2_cps()
|
||||||
|
{
|
||||||
|
assert(ref_cps.size() > 1) ;
|
||||||
|
|
||||||
|
/* Compute the angle REF1-RX-REF2 */
|
||||||
|
double reference_angle = ref_cps[0].angle + ref_cps[1].angle ;
|
||||||
|
/* Note: the reference angle must be the sum of the two angles
|
||||||
|
P-RX-REF1 and P-RX-REF2 instead of simply REF1-RX-REF2, to allow
|
||||||
|
the angle to be within [0;360[° instead of [0;180[°. This is needed
|
||||||
|
if P-RX-REF1 or P-RX-REF2 are greater than 90°, otherwise the
|
||||||
|
weights would be wrong. */
|
||||||
|
|
||||||
|
/* Weight the two CPs */
|
||||||
|
if (reference_angle == 0)
|
||||||
|
ref_cps[0].weight = ref_cps[1].weight = 50 ;
|
||||||
|
else
|
||||||
|
{
|
||||||
|
ref_cps[0].weight = ref_cps[0].angle * 100 / reference_angle ;
|
||||||
|
if (ref_cps[0].weight > 100)
|
||||||
|
ref_cps[0].weight = 100 ;
|
||||||
|
ref_cps[1].weight = 100 - ref_cps[0].weight ;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void AutocalibrationMesh::compute_multi_packet_ss()
|
||||||
|
{
|
||||||
|
const cp &ref1 = ref_cps[0] ;
|
||||||
|
const ReferencePoint &ref1_rp =
|
||||||
|
Stock::get_reference_point(ref1.cp->get_coordinates()) ;
|
||||||
|
const vector<CalibrationRequest*> &ref1_cr =
|
||||||
|
ref1_rp.get_requests() ;
|
||||||
|
|
||||||
|
/* Search for the first measurement made by RX in ref1_rp */
|
||||||
|
const Measurement *rx_measurement = NULL ;
|
||||||
|
vector<CalibrationRequest*>::const_iterator cr ;
|
||||||
|
const string &rx_mac = rx->second.get_mac_addr() ;
|
||||||
|
for (cr = ref1_cr.begin() ; cr != ref1_cr.end() ; ++cr)
|
||||||
|
{
|
||||||
|
rx_measurement = (*cr)->get_measurement(rx_mac) ;
|
||||||
|
if (rx_measurement)
|
||||||
|
break ;
|
||||||
|
}
|
||||||
|
|
||||||
|
// The selected reference CPs are in coverage of RX, therefore
|
||||||
|
// we should always find a measurement of RX in ref1_rp
|
||||||
|
assert(rx_measurement) ;
|
||||||
|
|
||||||
|
/* Generate the SS for each packet */
|
||||||
|
bool packet_generated = false ;
|
||||||
|
for (pkt_id_t pkt_id = 1 ; pkt_id <= (*cr)->get_nb_packets() ;
|
||||||
|
++pkt_id)
|
||||||
|
if (compute_single_packet_ss(pkt_id))
|
||||||
|
packet_generated = true ;
|
||||||
|
|
||||||
|
if (! packet_generated)
|
||||||
|
{
|
||||||
|
if (Configuration::is_configured("verbose"))
|
||||||
|
cerr << "No packet has been generated for RX CP " << rx_mac
|
||||||
|
<< "... failing back to single packet request. Calibration"
|
||||||
|
<< " request involved : " << **cr << "\n" ;
|
||||||
|
compute_single_packet_ss(0) ;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @param pkt_id The number of the packet to create. Must be 0 if all
|
||||||
|
* the generated reference points contain only one packet.
|
||||||
|
*/
|
||||||
|
bool AutocalibrationMesh::compute_single_packet_ss(pkt_id_t pkt_id)
|
||||||
|
{
|
||||||
|
/* Distance RX-P */
|
||||||
|
const Point3D &rx_coord = rx->second.get_coordinates() ;
|
||||||
|
float point_dst = rx_coord.distance(point) ;
|
||||||
|
|
||||||
|
/* Compute the consolidated SS */
|
||||||
|
double rx_ss = 0 ;
|
||||||
|
for (unsigned int i = 0 ; i < ref_cps.size() ; ++i)
|
||||||
|
{
|
||||||
|
double tmp_ss = compute_weighted_ss(i, pkt_id, point_dst) ;
|
||||||
|
if (tmp_ss > 0) // error
|
||||||
|
return false ;
|
||||||
|
rx_ss += tmp_ss ;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Add the SS to the measurements' list */
|
||||||
|
const string &rx_mac = rx->second.get_mac_addr() ;
|
||||||
|
// 0 is not a valid packet ID, so we use 1 if we generate only one
|
||||||
|
// packet per reference point:
|
||||||
|
if (pkt_id == 0)
|
||||||
|
pkt_id = 1 ;
|
||||||
|
// The Measurement measurements[rx_mac] is created if needed:
|
||||||
|
measurements[rx_mac].add_ss(pkt_id, rx_ss) ;
|
||||||
|
// Useful only if the measurement was just created, but it is not
|
||||||
|
// worth a test:
|
||||||
|
measurements[rx_mac].set_cp(&rx->second) ;
|
||||||
|
|
||||||
|
return true ;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @param cp_idx The index of the CP in #ref_cps.
|
||||||
|
* @param pkt_id The number of the packet to generate, or 0 if we
|
||||||
|
* generate only one packet per reference point.
|
||||||
|
* @param point_dst The distance RX-P.
|
||||||
|
*
|
||||||
|
* @returns The weighted SS value in dBm, or 1 in case of error.
|
||||||
|
*/
|
||||||
|
double AutocalibrationMesh::compute_weighted_ss(
|
||||||
|
unsigned int cp_idx, pkt_id_t pkt_id, float point_dst)
|
||||||
|
{
|
||||||
|
const cp &ref = ref_cps[cp_idx] ;
|
||||||
|
const ReferencePoint &rp =
|
||||||
|
Stock::get_reference_point(ref.cp->get_coordinates()) ;
|
||||||
|
|
||||||
|
/* Friis index */
|
||||||
|
const string &rx_mac = rx->second.get_mac_addr() ;
|
||||||
|
float friis_idx ;
|
||||||
|
if (pkt_id == 0)
|
||||||
|
friis_idx = rp.friis_index_for_cp(rx_mac) ;
|
||||||
|
else
|
||||||
|
{
|
||||||
|
friis_idx = rp.friis_index_for_cp(rx_mac, pkt_id) ;
|
||||||
|
if (friis_idx == 0)
|
||||||
|
return 1 ; // The packet pkt_id does not exist in rp
|
||||||
|
}
|
||||||
|
|
||||||
|
/* SS */
|
||||||
|
// Constant term
|
||||||
|
double common_ss = rx->second.friis_constant_term() ;
|
||||||
|
double ss =
|
||||||
|
common_ss +
|
||||||
|
ref.cp->get_trx_power() +
|
||||||
|
ref.cp->get_antenna_gain() -
|
||||||
|
10 * friis_idx * log10(point_dst) ;
|
||||||
|
|
||||||
|
return ss * ref.weight / 100 ;
|
||||||
|
}
|
|
@ -0,0 +1,83 @@
|
||||||
|
/*
|
||||||
|
* 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
|
||||||
|
* http://code.lm7.fr/p/owlps/source/tree/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.
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef _OWLPS_POSITIONING_AUTOCALIBRATIONMESH_HH_
|
||||||
|
#define _OWLPS_POSITIONING_AUTOCALIBRATIONMESH_HH_
|
||||||
|
|
||||||
|
class CapturePoint ;
|
||||||
|
class Point3D ;
|
||||||
|
|
||||||
|
#include "autocalibration.hh"
|
||||||
|
|
||||||
|
#include <vector>
|
||||||
|
#include <unordered_map>
|
||||||
|
|
||||||
|
/// Generates reference points in a mesh
|
||||||
|
class AutocalibrationMesh: public Autocalibration
|
||||||
|
{
|
||||||
|
protected:
|
||||||
|
struct cp
|
||||||
|
{
|
||||||
|
const CapturePoint *cp ;
|
||||||
|
float weight ; // Weight of the CP in percents
|
||||||
|
float angle ;
|
||||||
|
} ;
|
||||||
|
|
||||||
|
/// Angle P-RX-O, O being the origin of the trigonometric circle
|
||||||
|
float origin_angle ;
|
||||||
|
/// Selected transmitter CPs
|
||||||
|
std::vector<cp> ref_cps ;
|
||||||
|
/// Angles of the transmitter CPs (before M on the trigonometric
|
||||||
|
/// circle)
|
||||||
|
/**
|
||||||
|
* Note that the angles are stored in absolute value.
|
||||||
|
*/
|
||||||
|
std::multimap<double,
|
||||||
|
std::pair<double,
|
||||||
|
std::unordered_map<
|
||||||
|
std::string, CapturePoint>::const_iterator> >
|
||||||
|
sorted_negative_angles ;
|
||||||
|
/// Angles of the transmitter CPs (after M on the trigonometric circle)
|
||||||
|
std::multimap<double,
|
||||||
|
std::pair<double,
|
||||||
|
std::unordered_map<
|
||||||
|
std::string, CapturePoint>::const_iterator> >
|
||||||
|
sorted_positive_angles ;
|
||||||
|
|
||||||
|
/// Initialises a struct cp with the values from `s`
|
||||||
|
void init_cp(
|
||||||
|
std::map<double, std::pair<double, std::unordered_map<
|
||||||
|
std::string, CapturePoint>::const_iterator> >::const_iterator s) ;
|
||||||
|
|
||||||
|
/// Generates the SSs for all the CPs
|
||||||
|
void generate_ss(void) ;
|
||||||
|
/// Sorts the reference CPs for a receiver CP
|
||||||
|
void sort_reference_cps(void) ;
|
||||||
|
/// Computes the weight of the selected CP(s)
|
||||||
|
void weight_cps(void) ;
|
||||||
|
/// Weights two CPs according to their angles
|
||||||
|
void weight_2_cps(void) ;
|
||||||
|
/// Computes a SS with several packets in it
|
||||||
|
void compute_multi_packet_ss(void) ;
|
||||||
|
/// Computes a SS with only one packet in it
|
||||||
|
bool compute_single_packet_ss(pkt_id_t pkt_id) ;
|
||||||
|
/// Computes a SS according to the weight of the CP
|
||||||
|
double compute_weighted_ss(
|
||||||
|
unsigned int cp_idx, pkt_id_t pkt_id, float point_dst) ;
|
||||||
|
|
||||||
|
public:
|
||||||
|
AutocalibrationMesh(const Point3D &_point): Autocalibration(_point) {}
|
||||||
|
} ;
|
||||||
|
|
||||||
|
#endif // _OWLPS_POSITIONING_AUTOCALIBRATIONMESH_HH_
|
|
@ -17,6 +17,7 @@
|
||||||
#include "posexcept.hh"
|
#include "posexcept.hh"
|
||||||
#include "area.hh"
|
#include "area.hh"
|
||||||
#include "csvstringreader.hh"
|
#include "csvstringreader.hh"
|
||||||
|
#include "autocalibrationmesh.hh"
|
||||||
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
||||||
|
@ -503,7 +504,7 @@ void Stock::generate_reference_points_list()
|
||||||
*/
|
*/
|
||||||
void Stock::generate_reference_point(const Point3D &coord)
|
void Stock::generate_reference_point(const Point3D &coord)
|
||||||
{
|
{
|
||||||
Autocalibration ac(coord) ;
|
AutocalibrationMesh ac(coord) ;
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
ac.generate_reference_point() ;
|
ac.generate_reference_point() ;
|
||||||
|
|
|
@ -15,7 +15,6 @@
|
||||||
#ifndef _OWLPS_POSITIONING_STOCK_HH_
|
#ifndef _OWLPS_POSITIONING_STOCK_HH_
|
||||||
#define _OWLPS_POSITIONING_STOCK_HH_
|
#define _OWLPS_POSITIONING_STOCK_HH_
|
||||||
|
|
||||||
#include "autocalibration.hh"
|
|
||||||
#include "building.hh"
|
#include "building.hh"
|
||||||
#include "waypoint.hh"
|
#include "waypoint.hh"
|
||||||
#include "mobile.hh"
|
#include "mobile.hh"
|
||||||
|
@ -30,6 +29,7 @@
|
||||||
class Stock
|
class Stock
|
||||||
{
|
{
|
||||||
friend class Autocalibration ;
|
friend class Autocalibration ;
|
||||||
|
friend class AutocalibrationMesh ;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/// List of known Building
|
/// List of known Building
|
||||||
|
|
Loading…
Reference in New Issue