2010-03-04 17:51:44 +01:00
|
|
|
#include "positioning.hh"
|
|
|
|
#include "realposition.hh"
|
2010-06-10 15:57:03 +02:00
|
|
|
#include "fbcm.hh"
|
2010-06-14 17:15:48 +02:00
|
|
|
#include "frbhmbasic.hh"
|
2010-04-16 09:32:12 +02:00
|
|
|
#include "interlinknetworks.hh"
|
2010-06-10 15:57:03 +02:00
|
|
|
#include "radar.hh"
|
2011-04-05 11:52:06 +02:00
|
|
|
#include "resultlist.hh"
|
2010-03-04 17:51:44 +01:00
|
|
|
#include "configuration.hh"
|
|
|
|
#include "posexcept.hh"
|
2010-06-10 15:57:03 +02:00
|
|
|
#include "stock.hh"
|
2010-03-04 17:51:44 +01:00
|
|
|
|
2011-03-29 16:00:37 +02:00
|
|
|
#include <owlps.h>
|
|
|
|
|
2011-07-25 16:50:35 +02:00
|
|
|
#include <iostream>
|
2011-11-22 15:14:39 +01:00
|
|
|
#include <boost/tr1/unordered_set.hpp>
|
2011-07-25 16:50:35 +02:00
|
|
|
|
2010-03-04 17:51:44 +01:00
|
|
|
using namespace std ;
|
2011-11-22 15:14:39 +01:00
|
|
|
using std::tr1::unordered_set ;
|
2010-03-04 17:51:44 +01:00
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* *** Constructors *** */
|
|
|
|
|
|
|
|
|
|
|
|
Positioning::Positioning()
|
|
|
|
{
|
|
|
|
initialise_algorithms() ;
|
|
|
|
loop() ;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
Positioning::~Positioning()
|
|
|
|
{
|
|
|
|
for (vector<PositioningAlgorithm*>::const_iterator i =
|
|
|
|
algorithms.begin() ; i != algorithms.end() ; ++i)
|
|
|
|
delete *i ;
|
|
|
|
algorithms.clear() ;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* *** Operations *** */
|
|
|
|
|
|
|
|
|
|
|
|
void Positioning::initialise_algorithms()
|
|
|
|
{
|
|
|
|
if (! Configuration::is_configured("positioning.algorithm"))
|
2010-04-20 15:59:27 +02:00
|
|
|
throw missing_configuration(
|
|
|
|
"No positioning algorithm specified in configuration!") ;
|
2010-03-04 17:51:44 +01:00
|
|
|
|
|
|
|
const vector<string> &algo_names =
|
|
|
|
Configuration::string_vector_value("positioning.algorithm") ;
|
|
|
|
|
2011-11-22 15:14:39 +01:00
|
|
|
// We will store the names of the stored algorithms in a set, to
|
|
|
|
// avoid multiple occurrences of the same algorithm:
|
|
|
|
unordered_set<string> stored_algos ;
|
|
|
|
|
2010-03-04 17:51:44 +01:00
|
|
|
for (vector<string>::const_iterator i = algo_names.begin() ;
|
|
|
|
i != algo_names.end() ; ++i)
|
|
|
|
{
|
|
|
|
if (*i == "Real")
|
2011-11-22 15:14:39 +01:00
|
|
|
{
|
|
|
|
pair<unordered_set<string>::iterator, bool> stored =
|
|
|
|
stored_algos.insert("Real") ;
|
|
|
|
if (stored.second) // Check if there was no previous instance
|
|
|
|
algorithms.insert(algorithms.begin(), new RealPosition) ;
|
|
|
|
/* Note: in order to compute the errors of the other
|
|
|
|
* algorithms, Real must be the first for each request,
|
|
|
|
* so we add it at the begining. */
|
|
|
|
}
|
2010-03-04 17:51:44 +01:00
|
|
|
|
2010-06-10 15:57:03 +02:00
|
|
|
else if (*i == "FBCM")
|
|
|
|
{
|
2011-11-22 15:14:39 +01:00
|
|
|
pair<unordered_set<string>::iterator, bool> stored =
|
|
|
|
stored_algos.insert("FBCM") ;
|
|
|
|
if (stored.second) // Check if there was no previous instance
|
|
|
|
{
|
|
|
|
/* Generate the Friis indexes only if the autocalibration
|
|
|
|
* is not activated (if it is, Friis indexes will be
|
|
|
|
* regenerated each time FBCM wants to computes a position)
|
|
|
|
*/
|
|
|
|
if (! Configuration::
|
|
|
|
bool_value("positioning.generate-reference-points"))
|
|
|
|
Stock::update_all_friis_indexes() ;
|
|
|
|
algorithms.push_back(new FBCM) ;
|
|
|
|
}
|
2010-06-10 15:57:03 +02:00
|
|
|
}
|
|
|
|
|
2010-06-14 17:15:48 +02:00
|
|
|
else if (*i == "FRBHMBasic")
|
2011-11-22 15:14:39 +01:00
|
|
|
{
|
|
|
|
pair<unordered_set<string>::iterator, bool> stored =
|
|
|
|
stored_algos.insert("FRBHMBasic") ;
|
|
|
|
if (stored.second) // Check if there was no previous instance
|
|
|
|
algorithms.push_back(new FRBHMBasic) ;
|
|
|
|
// TODO: Pre-compute all per-ReferencePoint friis indexes?
|
|
|
|
}
|
2010-06-14 17:15:48 +02:00
|
|
|
|
2010-04-16 09:32:12 +02:00
|
|
|
else if (*i == "InterlinkNetworks")
|
2011-11-22 15:14:39 +01:00
|
|
|
{
|
|
|
|
pair<unordered_set<string>::iterator, bool> stored =
|
|
|
|
stored_algos.insert("InterlinkNetworks") ;
|
|
|
|
if (stored.second) // Check if there was no previous instance
|
|
|
|
algorithms.push_back(new InterlinkNetworks) ;
|
|
|
|
}
|
2010-04-16 09:32:12 +02:00
|
|
|
|
2010-06-02 12:37:43 +02:00
|
|
|
else if (*i == "RADAR")
|
2011-11-22 15:14:39 +01:00
|
|
|
{
|
|
|
|
pair<unordered_set<string>::iterator, bool> stored =
|
|
|
|
stored_algos.insert("RADAR") ;
|
|
|
|
if (stored.second) // Check if there was no previous instance
|
|
|
|
algorithms.push_back(new RADAR) ;
|
|
|
|
}
|
2010-06-02 12:37:43 +02:00
|
|
|
|
2010-03-04 17:51:44 +01:00
|
|
|
else
|
2010-04-20 15:59:27 +02:00
|
|
|
throw bad_configuration(
|
|
|
|
"The specified positioning_algorithm « "+ *i +
|
|
|
|
" » is unknown!") ;
|
2010-03-04 17:51:44 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void Positioning::loop()
|
|
|
|
{
|
|
|
|
vector<PositioningAlgorithm*>::const_iterator algo ;
|
|
|
|
|
2011-03-29 16:00:37 +02:00
|
|
|
while (! input.eof() && owl_run)
|
2010-03-04 17:51:44 +01:00
|
|
|
{
|
|
|
|
const Request &request = input.get_next_request() ;
|
2010-04-16 09:32:12 +02:00
|
|
|
if (! request)
|
|
|
|
continue ;
|
|
|
|
|
2011-05-03 15:33:12 +02:00
|
|
|
Point3D real_position ;
|
|
|
|
bool compute_error = false ;
|
2011-04-05 11:52:06 +02:00
|
|
|
ResultList results(&request) ;
|
2010-03-04 17:51:44 +01:00
|
|
|
for (algo = algorithms.begin() ; algo != algorithms.end() ;
|
|
|
|
++algo)
|
2011-05-03 15:33:12 +02:00
|
|
|
{
|
2011-06-29 17:33:44 +02:00
|
|
|
Result res ;
|
|
|
|
|
|
|
|
try
|
|
|
|
{
|
|
|
|
res = (*algo)->compute(request) ;
|
|
|
|
}
|
|
|
|
catch (exception &e)
|
|
|
|
{
|
|
|
|
cerr
|
|
|
|
<< "Cannot compute with algorithm "
|
|
|
|
<< (*algo)->get_name() << ": « "
|
|
|
|
<< e.what() << " »\n" ;
|
|
|
|
continue ;
|
|
|
|
}
|
|
|
|
|
2011-05-03 15:33:12 +02:00
|
|
|
if (compute_error)
|
|
|
|
res.compute_error(real_position) ;
|
|
|
|
else if ((*algo)->get_name() == "Real")
|
|
|
|
{
|
|
|
|
compute_error = true ;
|
|
|
|
real_position = res.get_position() ;
|
|
|
|
}
|
2011-06-29 17:33:44 +02:00
|
|
|
|
2011-05-03 15:33:12 +02:00
|
|
|
results.add(res) ;
|
|
|
|
}
|
|
|
|
|
2011-04-05 11:52:06 +02:00
|
|
|
output.write(results) ;
|
2010-03-04 17:51:44 +01:00
|
|
|
}
|
|
|
|
}
|