[Positioner] Add AutocalibrationLine
New autocalibration technique to generate points in line (use case: underground tunnels). The handling of the positioning.generate-reference-points has changed to be able to use several autocalibration methods simultaneously.
This commit is contained in:
parent
b100513265
commit
1c34eaee1e
2
TODO.t2t
2
TODO.t2t
|
@ -298,6 +298,8 @@ Work to do in OwlPS
|
|||
- Classes without tests or with mock test files (possibly untestable or
|
||||
not worth testing, to be checked):
|
||||
- Autocalibration
|
||||
- AutocalibrationLine
|
||||
- AutocalibrationMesh
|
||||
- CapturePointsReaderCSV
|
||||
- CartographyAlgorithm
|
||||
- CSVStringReader
|
||||
|
|
|
@ -44,6 +44,7 @@ set(OWLPS_POSITIONER_CLASSES_SRC_FILES
|
|||
capturepointsreadercsv.cc
|
||||
area.cc
|
||||
autocalibration.cc
|
||||
autocalibrationline.cc
|
||||
autocalibrationmesh.cc
|
||||
building.cc
|
||||
calibrationrequest.cc
|
||||
|
|
|
@ -42,7 +42,7 @@ protected:
|
|||
/// Generated measurements' list
|
||||
std::unordered_map<std::string, Measurement> measurements ;
|
||||
|
||||
/// Generates the SSs for all the CPs
|
||||
/// Generates the SSs for the current CP (#rx)
|
||||
/**
|
||||
* Once the preliminary work done, this function should call
|
||||
* compute_ss() to actually compute the signal strength.
|
||||
|
|
|
@ -0,0 +1,254 @@
|
|||
/*
|
||||
* 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 "autocalibrationline.hh"
|
||||
|
||||
#include "point3d.hh"
|
||||
#include "capturepoint.hh"
|
||||
#include "stock.hh"
|
||||
#include "configuration.hh"
|
||||
#include "posexcept.hh"
|
||||
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
|
||||
using namespace std ;
|
||||
|
||||
|
||||
|
||||
/* *** Constructors *** */
|
||||
|
||||
|
||||
/**
|
||||
* @param _cp1 The first capture point (receiver) to use the measurements
|
||||
* of to generate the reference points; it cannot be NULL.
|
||||
* @param _cp2 The second receiver capture point (optional).
|
||||
*/
|
||||
AutocalibrationLine::AutocalibrationLine(
|
||||
const Point3D &_point,
|
||||
const CapturePoint *const _cp1, const CapturePoint *const _cp2):
|
||||
Autocalibration(_point), cp1(_cp1), cp2(_cp2),
|
||||
trx_cp(NULL), trx_rp(NULL)
|
||||
{
|
||||
assert(cp1) ;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* *** Operations *** */
|
||||
|
||||
|
||||
void AutocalibrationLine::generate_ss()
|
||||
{
|
||||
/* With AutocalibrationLine, we want to generate measurements only
|
||||
* for the CP(s) we want to use. */
|
||||
assert(&rx->second) ;
|
||||
if (&rx->second != cp1 && &rx->second != cp2)
|
||||
return ;
|
||||
|
||||
/* Choose the reference CP: try the other extremity first */
|
||||
if (&rx->second == cp2) // rx is cp2
|
||||
trx_cp = cp1 ;
|
||||
else if (cp2) // rx is cp1 and cp2 is defined
|
||||
trx_cp = cp2 ;
|
||||
else // rx is cp1 and cp2 is not defined
|
||||
/* Choose another reference CP */
|
||||
sort_reference_cps() ;
|
||||
|
||||
/* Get the ReferencePoint corresponding to trx_cp */
|
||||
try
|
||||
{
|
||||
trx_rp = &Stock::get_reference_point(trx_cp->get_coordinates()) ;
|
||||
}
|
||||
catch (element_not_found e)
|
||||
{
|
||||
ostringstream oss ;
|
||||
oss << "No measurements from transmitter CP "
|
||||
<< trx_cp->get_mac_addr()
|
||||
<< ": cannot generate RP " << point
|
||||
<< " (receiver CP: " << rx->second.get_mac_addr() << ")." ;
|
||||
throw autocalibration_error(oss.str()) ;
|
||||
}
|
||||
assert(trx_rp) ;
|
||||
|
||||
compute_ss() ;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* The reference (transmitter) CPs are first filtered (floor, coverage),
|
||||
* then sorted according to the signal strength RX receives from them.
|
||||
*/
|
||||
void AutocalibrationLine::sort_reference_cps()
|
||||
{
|
||||
// Transmitter capture points, sorted by coverage with rx
|
||||
std::map<float, const CapturePoint*> sorted_ref_cps ;
|
||||
|
||||
const Point3D &rx_coord = rx->second.get_coordinates() ;
|
||||
|
||||
for (auto 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 ;
|
||||
|
||||
/* Store the CP according to the SS rx receives from it */
|
||||
const ReferencePoint &ref_rp =
|
||||
Stock::get_reference_point(ref->second.get_coordinates()) ;
|
||||
const vector<CalibrationRequest*> &ref_cr = ref_rp.get_requests() ;
|
||||
/* Search for the first measurement made by RX in ref_rp */
|
||||
const Measurement *rx_measurement = NULL ;
|
||||
vector<CalibrationRequest*>::const_iterator cr ;
|
||||
const string &rx_mac = rx->second.get_mac_addr() ;
|
||||
for (cr = ref_cr.begin() ; cr != ref_cr.end() ; ++cr)
|
||||
{
|
||||
rx_measurement = (*cr)->get_measurement(rx_mac) ;
|
||||
if (rx_measurement)
|
||||
break ;
|
||||
}
|
||||
// We skip CPs in bad coverage, so we should have a measurement
|
||||
assert(rx_measurement) ;
|
||||
float avg_ss = rx_measurement->get_average_dbm() ;
|
||||
sorted_ref_cps[avg_ss] = &ref->second ;
|
||||
}
|
||||
|
||||
if (sorted_ref_cps.empty())
|
||||
{
|
||||
ostringstream oss ;
|
||||
oss << "No CPs in coverage with receiver CP "
|
||||
<< rx->second.get_mac_addr() << " to generate RP "
|
||||
<< point << "." ;
|
||||
throw autocalibration_error(oss.str()) ;
|
||||
}
|
||||
|
||||
// Select the last CP from sorted_ref_cps (the CP with the highest SS)
|
||||
trx_cp = sorted_ref_cps.rbegin()->second ;
|
||||
}
|
||||
|
||||
|
||||
void AutocalibrationLine::compute_multi_packet_ss()
|
||||
{
|
||||
const vector<CalibrationRequest*> &ref_cr = trx_rp->get_requests() ;
|
||||
|
||||
/* Search for the first measurement made by RX in trx_rp */
|
||||
const Measurement *rx_measurement = NULL ;
|
||||
vector<CalibrationRequest*>::const_iterator cr ;
|
||||
const string &rx_mac = rx->second.get_mac_addr() ;
|
||||
for (cr = ref_cr.begin() ; cr != ref_cr.end() ; ++cr)
|
||||
{
|
||||
rx_measurement = (*cr)->get_measurement(rx_mac) ;
|
||||
if (rx_measurement)
|
||||
break ;
|
||||
}
|
||||
|
||||
/* The selected reference CP is supposed to be in coverage of RX,
|
||||
* but it is possible that it is not, in case we use cp1 and cp2
|
||||
* and no calibration request was transmitted in both directions
|
||||
* yet. */
|
||||
if (! rx_measurement)
|
||||
{
|
||||
ostringstream oss ;
|
||||
oss << "Transmitter CP " << trx_cp->get_mac_addr()
|
||||
<< " is not in coverage with receiver CP "
|
||||
<< rx->second.get_mac_addr() << " to generate RP "
|
||||
<< point << "." ;
|
||||
throw autocalibration_error(oss.str()) ;
|
||||
}
|
||||
|
||||
/* 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 AutocalibrationLine::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 SS */
|
||||
double rx_ss = compute_single_ss(pkt_id, point_dst) ;
|
||||
if (rx_ss > 127) // error
|
||||
return false ;
|
||||
|
||||
/* 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 ;
|
||||
}
|
||||
|
||||
|
||||
double AutocalibrationLine::
|
||||
compute_single_ss(pkt_id_t pkt_id, float point_dst)
|
||||
{
|
||||
/* Friis index */
|
||||
const string &rx_mac = rx->second.get_mac_addr() ;
|
||||
float friis_idx ;
|
||||
if (pkt_id == 0)
|
||||
friis_idx = trx_rp->friis_index_for_cp(rx_mac) ;
|
||||
else
|
||||
{
|
||||
friis_idx = trx_rp->friis_index_for_cp(rx_mac, pkt_id) ;
|
||||
if (friis_idx == 0)
|
||||
return 128 ; // The packet pkt_id does not exist in trx_rp
|
||||
}
|
||||
|
||||
/* SS */
|
||||
// Constant term
|
||||
double common_ss = rx->second.friis_constant_term() ;
|
||||
double ss =
|
||||
common_ss +
|
||||
trx_cp->get_trx_power() +
|
||||
trx_cp->get_antenna_gain() -
|
||||
10 * friis_idx * log10(point_dst) ;
|
||||
|
||||
return ss ;
|
||||
}
|
|
@ -0,0 +1,55 @@
|
|||
/*
|
||||
* 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_AUTOCALIBRATIONLINE_HH_
|
||||
#define _OWLPS_POSITIONING_AUTOCALIBRATIONLINE_HH_
|
||||
|
||||
class CapturePoint ;
|
||||
class Point3D ;
|
||||
class ReferencePoint ;
|
||||
|
||||
#include "autocalibration.hh"
|
||||
|
||||
#include <map>
|
||||
|
||||
/// Generates reference points following a path
|
||||
class AutocalibrationLine: public Autocalibration
|
||||
{
|
||||
protected:
|
||||
/// Capture points to use for the generation
|
||||
const CapturePoint *cp1, *cp2 ;
|
||||
|
||||
/// Reference (transmitter) capture point
|
||||
const CapturePoint *trx_cp ;
|
||||
/// True reference point corresponding to #trx_cp
|
||||
const ReferencePoint *trx_rp ;
|
||||
|
||||
/// Generates the SSs for the current CP
|
||||
void generate_ss(void) ;
|
||||
/// Sorts the reference CPs for a receiver CP
|
||||
void sort_reference_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) ;
|
||||
/// Actually computes a SS
|
||||
double compute_single_ss(pkt_id_t pkt_id, float point_dst) ;
|
||||
|
||||
public:
|
||||
AutocalibrationLine(const Point3D &_point,
|
||||
const CapturePoint *const _cp1,
|
||||
const CapturePoint *const _cp2 = NULL) ;
|
||||
} ;
|
||||
|
||||
#endif // _OWLPS_POSITIONING_AUTOCALIBRATIONLINE_HH_
|
|
@ -95,8 +95,8 @@ mobile-csv-file = /usr/local/etc/owlps/mobiles.csv
|
|||
|
||||
# Coordinates of the deployment area.
|
||||
# This is used to delimit the area in which reference points are
|
||||
# generated (when generate-reference-points is set to "mesh" or "both"),
|
||||
# and also by the MinMax trilateration method.
|
||||
# generated (when generate-reference-points includes "mesh"), and
|
||||
# also by the MinMax trilateration method.
|
||||
# Since MinMax is currently the only trilateration method implemented
|
||||
# in OwlPS, you should define these parameters if you use any of the
|
||||
# trilateration-based algorithms (InterlinkNetworks, FBCM, FRBHM).
|
||||
|
@ -136,16 +136,28 @@ mobile-csv-file = /usr/local/etc/owlps/mobiles.csv
|
|||
|
||||
# Generate reference points from the (auto)calibration requests
|
||||
# received.
|
||||
# Possible values
|
||||
# - false (default): do not generate reference points.
|
||||
# This option can be set to any combination of the following values,
|
||||
# separated by any characters of your choice (or even none):
|
||||
# - false (default): do not generate reference points; this is
|
||||
# equivalent to an empty string and if at least one of the next
|
||||
# values are present, the presence of the "false" string in the
|
||||
# option's value will be ignored.
|
||||
# - mesh: generate reference points according to the regular meshing
|
||||
# defined by the options area-start/stop and
|
||||
# generated-meshing-grain-*.
|
||||
# - line: generate reference points according to the line defined by
|
||||
# the option generated-line-path and generated-line-step.
|
||||
# - list: generate only the reference points defined by the option
|
||||
# generated-points-list.
|
||||
# - both: generate both a regular meshing and the points from the
|
||||
# list.
|
||||
# Any other or extra characters are ignored, and the string is
|
||||
# equivalent to "false" if it doesn't contain any valid value.
|
||||
# Default value:
|
||||
#generate-reference-points = false
|
||||
# Example of valid value to activate both meshing and list generation:
|
||||
#generate-reference-points = mesh+list
|
||||
#generate-reference-points = list,mesh
|
||||
#generate-reference-points = meshlist
|
||||
#generate-reference-points = falselistmesh
|
||||
|
||||
# With this option disabled, each generated reference point contains
|
||||
# a single packet in a single calibration request, computed from the
|
||||
|
@ -156,9 +168,9 @@ mobile-csv-file = /usr/local/etc/owlps/mobiles.csv
|
|||
# instead. Default is enabled.
|
||||
#generate-multi-packet-reference-points = true
|
||||
|
||||
# When generate-reference-points is set to "list" or "both", the
|
||||
# reference points are generated with the specified distance (in metres)
|
||||
# between one another, in the X and Y axis.
|
||||
# When generate-reference-points includes "list", the reference points
|
||||
# are generated with the specified distance (in metres) between one
|
||||
# another, in the X and Y axis.
|
||||
#generated-meshing-grain-x = 0.5
|
||||
#generated-meshing-grain-y = 0.5
|
||||
# The Z option is currently a floor number instead of a vertical
|
||||
|
@ -166,11 +178,29 @@ mobile-csv-file = /usr/local/etc/owlps/mobiles.csv
|
|||
# deploying across non-contiguous floors, which is unlikely.
|
||||
#generated-meshing-grain-z = 1
|
||||
|
||||
# When generate-reference-points is set to "list" or "both", the list of
|
||||
# reference points declared here is be generated. In the string, the
|
||||
# values are separated by semicolons, and each group of coordinates can
|
||||
# optionnaly be surrounded by parenthesis. The two following examples,
|
||||
# which declare the points (1;2;1) and (4;2.5;1), are equivalent:
|
||||
# When generate-reference-points includes "line", the reference points
|
||||
# are generated along to the path defined by this option. They are
|
||||
# generated in a straight line between the given points. At least one of
|
||||
# the points provided must be the coordinates of a capture point, and
|
||||
# the path defined here should "link" (include) all the capture points
|
||||
# that are going to be used for this kind of autocalibration.
|
||||
# In the string, the values are separated by semicolons, and each group
|
||||
# of coordinates can optionnaly be surrounded by parenthesis. The two
|
||||
# following examples, which declare a path following the points (1;1;1),
|
||||
# (10;1;1) and (10;10;1), are equivalent:
|
||||
#generated-line-path = (1;1;1);(10;1;1);(10;10;1)
|
||||
#generated-line-path = 1;1;1;10;1;1;10;10;1
|
||||
|
||||
# When generate-reference-points includes "line", the reference points
|
||||
# are generated with this (approximate) distance (in metres) between one
|
||||
# another.
|
||||
#generated-line-step = 0.5
|
||||
|
||||
# When generate-reference-points includes "list", the list of reference
|
||||
# points declared here is be generated. In the string, the values are
|
||||
# separated by semicolons, and each group of coordinates can optionnaly
|
||||
# be surrounded by parenthesis. The two following examples, which
|
||||
# declare the points (1;2;1) and (4;2.5;1), are equivalent:
|
||||
#generated-points-list = (1;2;1);(4;2.5;1)
|
||||
#generated-points-list = 1;2;1;4;2.5;1
|
||||
|
||||
|
|
|
@ -18,6 +18,7 @@
|
|||
#include "area.hh"
|
||||
#include "csvstringreader.hh"
|
||||
#include "autocalibrationmesh.hh"
|
||||
#include "autocalibrationline.hh"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
|
@ -435,22 +436,17 @@ void Stock::regenerate_reference_points()
|
|||
|
||||
delete_non_cp_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") ;
|
||||
assert(! ac_mode.empty()) ;
|
||||
|
||||
const string &ac_mode =
|
||||
Configuration::string_value("positioning.generate-reference-points") ;
|
||||
|
||||
if (ac_mode == "mesh" || ac_mode == "both")
|
||||
if (ac_mode.find("mesh") != string::npos)
|
||||
generate_reference_points_mesh() ;
|
||||
|
||||
if (ac_mode == "list" || ac_mode == "both")
|
||||
if (ac_mode.find("line") != string::npos)
|
||||
generate_reference_points_line() ;
|
||||
|
||||
if (ac_mode.find("list") != string::npos)
|
||||
generate_reference_points_list() ;
|
||||
}
|
||||
|
||||
|
@ -463,6 +459,15 @@ void Stock::generate_reference_points_mesh()
|
|||
"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 =
|
||||
|
@ -476,7 +481,157 @@ void Stock::generate_reference_points_mesh()
|
|||
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)) ;
|
||||
{
|
||||
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 = NULL ;
|
||||
|
||||
// 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 = NULL ;
|
||||
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 = NULL ;
|
||||
}
|
||||
|
||||
// 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) ;
|
||||
}
|
||||
|
||||
|
||||
|
@ -487,6 +642,15 @@ void Stock::generate_reference_points_list()
|
|||
"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") ;
|
||||
|
||||
|
@ -494,16 +658,18 @@ void Stock::generate_reference_points_list()
|
|||
|
||||
Point3D rp ;
|
||||
while (rp_list_parser.read_point3d(rp))
|
||||
generate_reference_point(rp) ;
|
||||
{
|
||||
AutocalibrationMesh ac(rp) ;
|
||||
generate_reference_point(ac) ;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @param coord The coordinates of the reference point to generate.
|
||||
* @param ac The pre-initialised Autocalibration to use.
|
||||
*/
|
||||
void Stock::generate_reference_point(const Point3D &coord)
|
||||
void Stock::generate_reference_point(Autocalibration &ac)
|
||||
{
|
||||
AutocalibrationMesh ac(coord) ;
|
||||
try
|
||||
{
|
||||
ac.generate_reference_point() ;
|
||||
|
|
|
@ -15,6 +15,8 @@
|
|||
#ifndef _OWLPS_POSITIONING_STOCK_HH_
|
||||
#define _OWLPS_POSITIONING_STOCK_HH_
|
||||
|
||||
class Autocalibration ;
|
||||
|
||||
#include "building.hh"
|
||||
#include "waypoint.hh"
|
||||
#include "mobile.hh"
|
||||
|
@ -30,6 +32,8 @@ class Stock
|
|||
{
|
||||
friend class Autocalibration ;
|
||||
friend class AutocalibrationMesh ;
|
||||
friend class AutocalibrationLine ;
|
||||
|
||||
|
||||
private:
|
||||
/// List of known Building
|
||||
|
@ -63,10 +67,17 @@ private:
|
|||
//@{
|
||||
/// Generates reference points according to a meshing
|
||||
static void generate_reference_points_mesh(void) ;
|
||||
/// Generates reference points according to a path
|
||||
static void generate_reference_points_line(void) ;
|
||||
/// Generates reference points between `start` and `end`
|
||||
static void generate_interpolated_reference_points(
|
||||
const Point3D &start, const Point3D &end,
|
||||
const CapturePoint *const cp1,
|
||||
const CapturePoint *const cp2 = NULL) ;
|
||||
/// Generates reference points according to a list
|
||||
static void generate_reference_points_list(void) ;
|
||||
/// Generates a single reference point
|
||||
static void generate_reference_point(const Point3D &coord) ;
|
||||
static void generate_reference_point(Autocalibration &ac) ;
|
||||
//@}
|
||||
|
||||
|
||||
|
|
|
@ -255,9 +255,12 @@ void UserInterface::fill_positioning_options()
|
|||
"Generate reference points from the (auto)calibration requests"
|
||||
" received. Can be 'false', 'mesh' to generate reference points"
|
||||
" regularly according to the meshing grain specified and the"
|
||||
" positioning area defined, 'list' to generate reference points"
|
||||
" from a list (cf. option generated-points-list) or 'both' to"
|
||||
" generate points both from the list and following the meshing.")
|
||||
" positioning area defined, 'line' to generate reference points"
|
||||
" regularly according to a straight path (cf. options"
|
||||
" generated-line-path and generated-line-step), 'list' to generate"
|
||||
" reference points from a list (cf. option generated-points-list),"
|
||||
" or a combination of the three previous options (separated by any"
|
||||
" character) to generate points in several ways.")
|
||||
("positioning.generate-multi-packet-reference-points",
|
||||
po::value<bool>()->default_value(true),
|
||||
"Generate several packets per reference point by trying to match the"
|
||||
|
@ -265,22 +268,33 @@ void UserInterface::fill_positioning_options()
|
|||
" requests are averaged and only one packet is generated.")
|
||||
("positioning.generated-meshing-grain-x",
|
||||
po::value<float>()->default_value(DEFAULT_MESHING_GRAIN),
|
||||
"When generating reference points, this distance (in meters) will"
|
||||
" separate each point to the next in X.")
|
||||
"When generate-reference-points includes 'mesh', this distance (in"
|
||||
" metres) will separate each point to the next in X.")
|
||||
("positioning.generated-meshing-grain-y",
|
||||
po::value<float>()->default_value(DEFAULT_MESHING_GRAIN),
|
||||
"When generating reference points, this distance (in meters) will"
|
||||
" separate each point to the next in Y.")
|
||||
"When generate-reference-points includes 'mesh', this distance (in"
|
||||
" metres) will separate each point to the next in Y.")
|
||||
("positioning.generated-meshing-grain-z",
|
||||
po::value<float>()->default_value(DEFAULT_Z_MESHING_GRAIN),
|
||||
"When generating reference points, this parameter represents the"
|
||||
" number of the floor. Currently, each increment of Y is a new"
|
||||
" floor (full 3-D coordinates are not supported yet).")
|
||||
"When generate-reference-points includes 'mesh', this parameter"
|
||||
" represents the number of the floor. Currently, each increment of Y"
|
||||
" is a new floor (full 3-D coordinates are not supported yet).")
|
||||
("positioning.generated-line-path",
|
||||
po::value<string>(),
|
||||
"Path of reference points to generate, if generate-reference-points"
|
||||
" includes 'line'; the reference points are generated in straight"
|
||||
" lines between the given points, and one of the points must be the"
|
||||
" coordinates of a CP (string format: \"(X1;Y1;Z1);(X2;Y2;Z2);...\""
|
||||
" or \"X1;Y1;Z1;X2;Y2;Z2;...\").")
|
||||
("positioning.generated-line-step",
|
||||
po::value<float>()->default_value(DEFAULT_MESHING_GRAIN),
|
||||
"When generate-reference-points includes 'line', two generated points"
|
||||
" will be separated approximately by this distance (in metres).")
|
||||
("positioning.generated-points-list",
|
||||
po::value<string>(),
|
||||
"List of reference points to generate, if generate-reference-points"
|
||||
" is set to 'list' or 'both' (string format:"
|
||||
" \"(X1;Y1;Z1);(X2;Y2;Z2);...\" or \"X1;Y1;Z1;X2;Y2;Z2;...\").")
|
||||
" includes 'list' (string format: \"(X1;Y1;Z1);(X2;Y2;Z2);...\" or"
|
||||
" \"X1;Y1;Z1;X2;Y2;Z2;...\").")
|
||||
("positioning.accept-new-calibration-requests",
|
||||
po::value<bool>()->default_value(false),
|
||||
"Add the calibration requests received during the run-time to"
|
||||
|
|
Loading…
Reference in New Issue