[Positioning] Add RADAR positioning algorithm

Add classes RADAR and CartographyAlgorithm, which is the super-class of
SS map based algorithms.

Measurement: Add functions:
- add_ss_list(), to add several SS in one operation.
- merge(), to merge a Measurement into another.
- ss_square_distance() (from old commented code), to compute the
  distance to a SS value or another Measurement.

posexcept: Add exception cannot_merge, used by Measurement::merge().

ReferencePoint:
- Add ss_square_distance() and functions used by it:
  complete_with_dummy_measurements(), compute_ss_square_distance() and
  get_all_measurements_sorted(). Use of sorted values (map instead of
  unordered_map) is useless, should be fixed soon.
- Delete old commented code: getPowerForAp(), addMeasurement() and old
  getSsSquareDistance().

Stock: Add function closest_reference_point().

owlps-positioning/Makefile

@@ -54,6 +54,7 @@ OBJ_LIST = \
 	result.o \
 	minmax.o \
 	interlinknetworks.o \
+	radar.o \
 	realposition.o \
 	configuration.o \
 	userinterface.o \
@@ -72,6 +73,7 @@ OBJ_LIST = \
 OBJ_NOTEST_LIST = \
 	posexcept.o \
 	multilaterationalgorithm.o \
+	cartographyalgorithm.o \
 	inputmedium.o
 INTERFACES_LIST = \
 	inputlogmedium.hh \
@@ -111,6 +113,8 @@ $(OBJ_DIR)/%.o: $(SRC_DIR)/%.cc $(SRC_DIR)/%.hh
 	$(LD) $(LDFLAGS) -o $@ $^
 
 # Dependencies
+$(OBJ_DIR)/owlps-positioning.o: \
+	$(OBJ_DIR)/posexcept.o
 $(OBJ_DIR)/owlps-positioning.o: \
 	$(OBJ_DIR)/userinterface.o \
 	$(OBJ_DIR)/inputdatareader.o \
@@ -128,7 +132,9 @@ $(OBJ_DIR)/topologyreadercsv.o: \
 	$(OBJ_DIR)/waypoint.o \
 	$(OBJ_DIR)/stock.o
 $(OBJ_DIR)/referencepoint.o: \
-	$(OBJ_DIR)/point3d.o
+	$(OBJ_DIR)/point3d.o \
+	$(OBJ_DIR)/measurement.o \
+	$(OBJ_DIR)/stock.o
 $(OBJ_DIR)/waypoint.o: \
 	$(OBJ_DIR)/point3d.o \
 	$(OBJ_DIR)/building.o
@@ -143,7 +149,8 @@ $(OBJ_DIR)/accesspoint.o: \
 $(OBJ_DIR)/mobile.o: \
 	$(OBJ_DIR)/wifidevice.o
 $(OBJ_DIR)/measurement.o: \
-	$(OBJ_DIR)/accesspoint.o
+	$(OBJ_DIR)/accesspoint.o \
+	$(OBJ_DIR)/posexcept.o
 $(OBJ_DIR)/request.o: \
 	$(OBJ_DIR)/timestamp.o \
 	$(OBJ_DIR)/measurement.o
@@ -194,6 +201,8 @@ $(OBJ_DIR)/minmax.o: \
 	$(OBJ_DIR)/accesspoint.o
 $(OBJ_DIR)/interlinknetworks.o: \
 	$(OBJ_DIR)/multilaterationalgorithm.o
+$(OBJ_DIR)/radar.o: \
+	$(OBJ_DIR)/cartographyalgorithm.o
 $(OBJ_DIR)/realposition.o: \
 	$(SRC_DIR)/positioningalgorithm.hh \
 	$(OBJ_DIR)/result.o \
@@ -203,6 +212,8 @@ $(OBJ_DIR)/positioning.o: \
 	$(OBJ_DIR)/inputdatareader.o \
 	$(OBJ_DIR)/input.o \
 	$(OBJ_DIR)/realposition.o \
+	$(OBJ_DIR)/radar.o \
+	$(OBJ_DIR)/interlinknetworks.o \
 	$(OBJ_DIR)/output.o \
 	$(OBJ_DIR)/configuration.o \
 	$(OBJ_DIR)/posexcept.o

owlps-positioning/TODO

@@ -39,8 +39,6 @@
 - ReferencePoint
    ° La liste des requêtes devrait être un unordered_set (et pas un
    vector), pour garantir l'unicité des entrées.
-   ° Faire quelque chose pour le code commenté (idem dans
-   Measurement).
 
 - MinMax
    ° Différencier le pas pour X, Y et Z ?

owlps-positioning/src/cartographyalgorithm.cc

@@ -0,0 +1,9 @@
+#include "cartographyalgorithm.hh"
+
+
+
+Result CartographyAlgorithm::compute(const Request &request)
+{
+  Point3D position(select_point(request)) ;
+  return Result(&request, position) ;
+}

owlps-positioning/src/cartographyalgorithm.hh

@@ -0,0 +1,20 @@
+#ifndef _OWLPS_POSITIONING_CARTOGRAPHYALGORITHM_HH_
+#define _OWLPS_POSITIONING_CARTOGRAPHYALGORITHM_HH_
+
+#include "positioningalgorithm.hh"
+
+/// Super-class of SS cartography-based positioning algorithms
+class CartographyAlgorithm: public PositioningAlgorithm
+{
+public:
+  CartographyAlgorithm(void) {}
+  virtual ~CartographyAlgorithm(void) {}
+
+  /** @name Operations */
+  //@{
+  Result compute(const Request &request) ;
+  virtual Point3D select_point(const Request &request) = 0 ;
+  //@}
+} ;
+
+#endif // _OWLPS_POSITIONING_CARTOGRAPHYALGORITHM_HH_

owlps-positioning/src/measurement.cc

@@ -1,5 +1,5 @@
 #include "measurement.hh"
-
+#include "posexcept.hh"
 
 using namespace std ;
 
@@ -61,6 +61,28 @@ void Measurement::set_ss_list(const std::vector<int> &_ss_list)
 }
 
 
+void Measurement::add_ss_list(const std::vector<int> &_ss_list)
+{
+  ss_list.insert(ss_list.end(), _ss_list.begin(), _ss_list.end()) ;
+  update_average_ss() ;
+}
+
+
+/**
+ * Merge consists of adding the SS values of #source to #ss_list. It is
+ * possible only if the #ap of the two Measurement are identical.
+ * @throw cannot_merge if the AP of the two Measurement are different.
+ */
+void Measurement::merge(const Measurement &source)
+{
+  if (ap != source.ap)
+    throw cannot_merge(
+      "error when trying to merge measurements, APs are different") ;
+
+  add_ss_list(source.ss_list) ;
+}
+
+
 /**
  * - #ap is not deleted, only initialised to NULL.
  * - #ss_list is cleared.

owlps-positioning/src/measurement.hh

@@ -42,18 +42,29 @@ public:
   AccessPoint* get_ap() const ;
   const std::vector<int>& get_ss_list() const ;
   double get_average_ss() const ;
-  //float get_ss_square_distance(const float &ss) const ;
   //@}
 
   /** @name Write accessors */
   //@{
   void set_ap(const AccessPoint *_ap) ;
+  void set_ss_list(const std::vector<int> &_ss_list) ;
   /// Adds a signal strength to #ss_list
   void add_ss(const int &ss_dbm) ;
-  void set_ss_list(const std::vector<int> &_ss_list) ;
+  /// Adds several signal strengths to #ss_list
+  void add_ss_list(const std::vector<int> &_ss_list) ;
+  /// Merges a given Measurement into the current Measurement
+  void merge(const Measurement &source) ;
   void clear(void) ;
   //@}
 
+  /** @name Operations */
+  //@{
+  /// Computes the distance to another Measurement in SS space
+  float ss_square_distance(const Measurement &source) const ;
+  /// Computes the distance to another SS value
+  float ss_square_distance(const float &ss) const ;
+  //@}
+
   /** @name Operators */
   //@{
   const Measurement& operator=(const Measurement &m) ;
@@ -89,12 +100,6 @@ inline double Measurement::get_average_ss() const
 }
 
 
-// inline float Measurement::get_ss_square_distance(const float &ss) const
-// {
-//   return ((ss - average_ss) * (ss - average_ss)) ;
-// }
-
-
 
 /* *** Write accessors *** */
 
@@ -106,6 +111,23 @@ inline void Measurement::set_ap(const AccessPoint *_ap)
 
 
 
+/* *** Operations *** */
+
+
+inline float Measurement::
+ss_square_distance(const Measurement &source) const
+{
+  return ss_square_distance(source.average_ss) ;
+}
+
+
+inline float Measurement::ss_square_distance(const float &ss) const
+{
+  return ((ss - average_ss) * (ss - average_ss)) ;
+}
+
+
+
 /* *** Operators *** */
 
 

owlps-positioning/src/posexcept.cc

@@ -9,6 +9,11 @@ using namespace std ;
 /* *** Miscellaneous exceptions *** */
 
 
+cannot_merge::
+cannot_merge(const string &_explanation) throw():
+  posexcept(_explanation) {}
+
+
 element_not_found::
 element_not_found(const string &_explanation) throw():
   posexcept("Element not found in collection: " + _explanation) {}

owlps-positioning/src/posexcept.hh

@@ -29,6 +29,13 @@ public:
 /* *** Miscellaneous exceptions *** */
 
 
+class cannot_merge: public posexcept
+{
+public:
+  cannot_merge(const std::string &_explanation) throw() ;
+} ;
+
+
 class element_not_found: public posexcept
 {
 public:

owlps-positioning/src/positioning.cc

@@ -1,5 +1,6 @@
 #include "positioning.hh"
 #include "realposition.hh"
+#include "radar.hh"
 #include "interlinknetworks.hh"
 #include "configuration.hh"
 #include "posexcept.hh"
@@ -49,6 +50,9 @@ void Positioning::initialise_algorithms()
       else if (*i == "InterlinkNetworks")
         algorithms.push_back(new InterlinkNetworks) ;
 
+      else if (*i == "RADAR")
+        algorithms.push_back(new RADAR) ;
+
       else
         throw bad_configuration(
           "The specified positioning_algorithm « "+ *i +

owlps-positioning/src/radar.cc

@@ -0,0 +1,9 @@
+#include "radar.hh"
+#include "stock.hh"
+
+
+
+Point3D RADAR::select_point(const Request &request)
+{
+  return Stock::closest_reference_point(request) ;
+}

owlps-positioning/src/radar.hh

@@ -0,0 +1,22 @@
+#ifndef _OWLPS_POSITIONING_RADAR_HH_
+#define _OWLPS_POSITIONING_RADAR_HH_
+
+#include "cartographyalgorithm.hh"
+
+/// Computes a position selecting the 1-closest point in SS cartography
+/**
+ * This class is named after the positioning system conceived by Paramvir
+ * Bahl and Venkata N. Padmanabhan in their paper "RADAR: An In-Building
+ * RF-Based User Location and Tracking System" (2000). For more details,
+ * see: http://citeseer.ist.psu.edu/bahl00radar.html
+ */
+class RADAR: public CartographyAlgorithm
+{
+public:
+  RADAR(void) {}
+  ~RADAR(void) {}
+
+  Point3D select_point(const Request &request) ;
+} ;
+
+#endif // _OWLPS_POSITIONING_RADAR_HH_

owlps-positioning/src/referencepoint.cc

@@ -1,7 +1,10 @@
 #include "referencepoint.hh"
-
+#include "measurement.hh"
+#include "calibrationrequest.hh"
+#include "stock.hh"
 
 using namespace std ;
+using std::tr1::unordered_map ;
 
 
 
@@ -19,110 +22,113 @@ ReferencePoint::~ReferencePoint()
 
 
 
-// float ReferencePoint::getSsSquareDistance(const vector<CalibrationMeasurement> &m) const
-// {
-//   unsigned int i, j;
-//   float ret = 0;
-//   bool found;
-//   vector<CalibrationMeasurement> ref_m = measurements;
-//   vector<CalibrationMeasurement> test_m = m;
-//   CalibrationMeasurement new_meas;
-
-//   new_meas.addSsValue(-95);
-
-//   /* Complete measurement vector with unexisting ap (from ref point) */
-//   for (i = 0 ; i < ref_m.size() ; ++i)
-//     {
-//       found = false;
-//       for (j = 0 ; j < test_m.size() && !found ; ++j)
-//         if (test_m[j].getMacAddr() == ref_m[i].getMacAddr())
-//           found = true;
-//       if (!found)
-//         {
-//           new_meas.setMacAddr(measurements[i].getMacAddr());
-//           test_m.push_back(new_meas);
-//         }
-//     }
-
-//   /* Now, complete ref. point meas. */
-//   for (i = 0 ; i < test_m.size() ; ++i)
-//     {
-//       found = false;
-//       for (j = 0 ; j < ref_m.size() && !found ; ++j)
-//         if (test_m[i].getMacAddr() == ref_m[j].getMacAddr())
-//           found = true;
-//       if (!found)
-//         {
-//           new_meas.setMacAddr(test_m[i].getMacAddr());
-//           ref_m.push_back(new_meas);
-//         }
-//     }
-
-//   /* Now, compute SS distance */
-//   for (i = 0 ; i < test_m.size() ; ++i)
-//     {
-//       j = 0;
-//       found = false;
-//       while ((j < ref_m.size())&&(found == false))
-//         {
-//           if (ref_m[j].getMacAddr() == test_m[i].getMacAddr())
-//             {
-//               found = true;
-//               ret += ref_m[j].getSsSquareDistance(test_m[i].getAverage());
-//             }
-//           ++j;
-//         }
-//     }
-
-//   ref_m.clear();
-//   test_m.clear();
-
-//   return ret;
-// }
+/* *** Accessors *** */
 
 
-// void ReferencePoint::addMeasurement(const string &mac_a, const int &value)
-// {
-//   unsigned int i;
-//   CalibrationMeasurement m;
-//   bool inserted = false;
+map<string, Measurement> ReferencePoint::
+get_all_measurements_sorted() const
+{
+  map<string, Measurement> all ;
 
-//   for (i = 0 ; i < measurements.size() ; ++i)
-//     if (measurements[i].getMacAddr() == mac_a)
-//       {
-//         measurements[i].addSsValue(value);
-//         inserted = true;
-//         break;
-//       }
-//   if (inserted == false)
-//     {
-//       m.setMacAddr(mac_a);
-//       m.addSsValue(value);
-//       measurements.push_back(m);
-//     }
-// }
+  for (vector<CalibrationRequest*>::const_iterator i = requests.begin() ;
+       i != requests.end() ; ++i)
+    {
+      unordered_map<string, Measurement> measurements =
+        (*i)->get_measurements() ;
+      for (unordered_map<string, Measurement>::const_iterator j =
+             measurements.begin() ; j != measurements.end() ; ++j)
+        if (! all.insert(*j).second)
+          all[j->first].merge(j->second) ;
+    }
+
+  return all ;
+}
 
 
-// bool ReferencePoint::getPowerForAp(const string &ap_mac, float *p) const
-// {
-//   unsigned int i;
-//   string str = ap_mac;
-//   string macLowerCase;
 
-//   //Pour convertir les majuscules en miniscules
-//   const int length = str.length();
-//   for (int j=0; j < length; ++j)
-//     str[j] = tolower(str[j]);
+/* *** Operations *** */
 
-//   for (i = 0 ; i < measurements.size() ; ++i)
-//     if (measurements[i].getMacAddr() == str)
-//       {
-//         *p = measurements[i].getAverage();
-//         return true;
-//       }
 
-//   return false;
-// }
+float ReferencePoint::ss_square_distance(const Request &source) const
+{
+  assert(! requests.empty()) ;
+
+  const unordered_map<string, Measurement> &request_measurements =
+    source.get_measurements() ;
+  map<string, Measurement> request_measurements_sorted(
+    request_measurements.begin(),
+    request_measurements.end()) ;
+
+  map<string, Measurement> all_measurements_sorted =
+    get_all_measurements_sorted() ;
+
+  complete_with_dummy_measurements(all_measurements_sorted,
+                                   request_measurements_sorted) ;
+
+  return compute_ss_square_distance(all_measurements_sorted,
+                                    request_measurements_sorted) ;
+}
+
+
+/**
+ * APs that have not captured a Request must not have too much weight in
+ * the computation. Thus, in the measurements lists we compare, we add
+ * missing APs with a very low SS value.
+ * Both lists can be completed, depending of the APs they contain.
+ */
+void ReferencePoint::complete_with_dummy_measurements(
+  map<string, Measurement> &measurements1,
+  map<string, Measurement> &measurements2) const
+{
+  assert(! measurements1.empty()) ;
+  assert(! measurements2.empty()) ;
+
+  Measurement dummy ;
+  dummy.add_ss(-98) ; // FIXME: should be the smallest possible value
+
+  for (map<string, Measurement>::const_iterator i =
+         measurements1.begin() ; i != measurements1.end() ; ++i)
+    if (measurements2.find(i->first) == measurements2.end())
+      {
+        dummy.set_ap(&Stock::get_ap(i->first)) ;
+        measurements2[i->first] = dummy ;
+      }
+
+  for (map<string, Measurement>::const_iterator i =
+         measurements2.begin() ; i != measurements2.end() ; ++i)
+    if (measurements1.find(i->first) == measurements1.end())
+      {
+        dummy.set_ap(&Stock::get_ap(i->first)) ;
+        measurements1[i->first] = dummy ;
+      }
+}
+
+
+/**
+ * Both lists must have the same size: you should call
+ * complete_with_dummy_measurements() before
+ * compute_ss_square_distance().
+ */
+float ReferencePoint::compute_ss_square_distance(
+  map<string, Measurement> &measurements1,
+  map<string, Measurement> &measurements2) const
+{
+  assert(measurements1.size() == measurements2.size()) ;
+
+  float distance = 0 ;
+
+  for (map<string, Measurement>::const_iterator i1 =
+         measurements1.begin() ; i1 != measurements1.end() ; ++i1)
+    {
+      map<string, Measurement>::const_iterator i2 =
+        measurements2.find(i1->first) ;
+      assert(i2 != measurements2.end()) ;
+      distance += i1->second.ss_square_distance(
+                    i2->second) ;
+    }
+
+  return distance ;
+}
 
 
 

owlps-positioning/src/referencepoint.hh

@@ -2,10 +2,13 @@
 #define _OWLPS_POSITIONING_REFERENCEPOINT_HH_
 
 class CalibrationRequest ;
+class Request ;
+class Measurement ;
 
 #include "point3d.hh"
 
 #include <vector>
+#include <map>
 #include <ostream>
 
 /// Represents a reference point in 3-D space
@@ -17,9 +20,22 @@ protected:
       are stored, not values. */
   std::vector<CalibrationRequest*> requests ;
 
+  /** @name Operations */
+  //@{
+  /// Mutually complete two Measurement lists with missing APs
+  void complete_with_dummy_measurements(
+    std::map<std::string, Measurement> &measurements1,
+    std::map<std::string, Measurement> &measurements2) const ;
+  /// Compute the distance between two Measurement lists
+  float compute_ss_square_distance(
+    std::map<std::string, Measurement> &measurements1,
+    std::map<std::string, Measurement> &measurements2) const ;
+  //@}
+
 public:
-  ReferencePoint(const float &_x = 0, const float &_y = 0, const float &_z = 0):
-    Point3D(_x, _y, _z) {}
+  ReferencePoint(const float &_x = 0,
+                 const float &_y = 0,
+                 const float &_z = 0): Point3D(_x, _y, _z) {}
 
   ReferencePoint(const Point3D &p): Point3D(p) {}
 
@@ -31,14 +47,20 @@ public:
   /** @name Read accessors */
   //@{
   const std::vector<CalibrationRequest*>& get_requests(void) const ;
+  std::map<std::string, Measurement>
+  get_all_measurements_sorted(void) const ;
   //@}
 
   /** @name Write accessors */
   //@{
   /// Adds a Request to the \link #requests request list\endlink
   void add_request(const CalibrationRequest *cm) ;
-  //  float get_ss_square_distance(const vector<CalibrationRequest> &m) const ;
-  //  bool get_power_for_ap(const string &ap_mac, float *p) const ;
+  //@}
+
+  /** @name Operations */
+  //@{
+  /// Compute the distance between the ReferencePoint and a Request
+  float ss_square_distance(const Request &source) const ;
   //@}
 
   /** @name Operators */

owlps-positioning/src/stock.cc

@@ -164,6 +164,32 @@ find_create_reference_point(const ReferencePoint &point)
 }
 
 
+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 point list is empty!") ;
+
+  unordered_set<ReferencePoint>::const_iterator i =
+    reference_points.begin() ;
+  float distance = i->ss_square_distance(request) ;
+  unordered_set<ReferencePoint>::const_iterator closest = i ;
+
+  for (++i ; i != reference_points.end() ; ++i)
+    {
+      float tmp_distance = i->ss_square_distance(request) ;
+      if (tmp_distance < distance)
+        {
+          distance = tmp_distance ;
+          closest = i ;
+        }
+    }
+
+  return *closest ;
+}
+
+
 const CalibrationRequest& Stock::
 find_create_calibration_request(const CalibrationRequest &request)
 {

owlps-positioning/src/stock.hh

@@ -68,6 +68,10 @@ public:
   /// Look for a ReferencePoint and add it if it does not exist
   static const ReferencePoint&
   find_create_reference_point(const ReferencePoint &point) ;
+  /// \brief Look for the closest ReferencePoint (in the signal strength
+  /// space) to a given Request
+  static const ReferencePoint&
+  closest_reference_point(const Request &request) ;
 
   /// Look for a CalibrationRequest and add it if it does not exist
   static const CalibrationRequest&

owlps-positioning/src/userinterface.cc

@@ -172,7 +172,7 @@ void UserInterface::fill_positioning_options()
   ("positioning.algorithm,a", po::value< vector<string> >()->composing(),
    "Algorithms used to compute positions. You can specify \
 this option more than once (but at least once). Allowed: Real, \
-InterlinkNetworks.")
+InterlinkNetworks, RADAR.")
   ;
 
   file_options->add(options) ;

owlps-positioning/tests/measurement_test.hh

@@ -77,6 +77,27 @@ public:
   }
 
 
+  void test_ss_square_distance(void)
+  {
+    float ss = -78 ;
+
+    Measurement measurement1, measurement2 ;
+    measurement1.add_ss(ss) ;
+    measurement2.add_ss(ss) ;
+    TS_ASSERT_EQUALS(0, measurement1.ss_square_distance(measurement2)) ;
+    TS_ASSERT_EQUALS(0, measurement1.ss_square_distance(ss)) ;
+
+    /* Distance computation:
+     * (-42 - (-78))^2 == 1296
+     */
+    ss = -42 ;
+    measurement2.clear() ;
+    measurement2.add_ss(ss) ;
+    TS_ASSERT_EQUALS(1296, measurement1.ss_square_distance(measurement2)) ;
+    TS_ASSERT_EQUALS(1296, measurement1.ss_square_distance(ss)) ;
+  }
+
+
   void test_operators(void)
   {
     // ==

owlps-positioning/tests/radar_test.hh

@@ -0,0 +1,24 @@
+#include <cxxtest/TestSuite.h>
+
+#include "radar.hh"
+
+class RADAR_test: public CxxTest::TestSuite
+{
+public:
+
+  void test_select_point(void)
+  {
+    CartographyAlgorithm *algo = new RADAR() ;
+
+    /*
+     * TODO: implement the test.
+     * I was told to accelerate the development, so I leave unneeded unit
+     * tests for later. This tests *is* actually needed, but I did some
+     * result comparison between the old version of OWLPS Positioning and
+     * the current one, and it seems acceptably close.
+     */
+
+    delete algo ;
+  }
+
+} ;

owlps-positioning/tests/referencepoint_test.hh

@@ -58,6 +58,33 @@ public:
   }
 
 
+  void test_ss_square_distance(void)
+  {
+    AccessPoint ap1(Point3D(1,2,3), "aa:bb:cc:dd:ee:ff") ;
+    CalibrationRequest calibrationrequest1 ;
+    Measurement measurement1(&ap1) ;
+    measurement1.add_ss(-78) ;
+    std::tr1::unordered_map<std::string, Measurement> measurements ;
+    measurements["aa:bb:cc:dd:ee:ff"] = measurement1 ;
+    calibrationrequest1.set_measurements(measurements) ;
+    ReferencePoint referencepoint1 ;
+    referencepoint1.add_request(&calibrationrequest1) ;
+    TS_ASSERT_EQUALS(0, referencepoint1.ss_square_distance(
+                       calibrationrequest1)) ;
+
+    /* Distance computation:
+     * (-42 - (-78))^2 == 1296
+     */
+    CalibrationRequest calibrationrequest2 ;
+    measurement1.clear() ;
+    measurement1.add_ss(-42) ;
+    measurements["aa:bb:cc:dd:ee:ff"] = measurement1 ;
+    calibrationrequest2.set_measurements(measurements) ;
+    TS_ASSERT_EQUALS(1296, referencepoint1.ss_square_distance(
+                       calibrationrequest2)) ;
+  }
+
+
   void test_operators(void)
   {
     // ==

owlps-positioning/tests/stock_test.hh

@@ -118,6 +118,64 @@ public:
     TS_ASSERT_EQUALS(&referencepoint1_ref1, &referencepoint1_ref2) ;
   }
 
+  void test_closest_reference_point(void)
+  {
+    const AccessPoint &ap1 = Stock::find_create_ap("aa:bb:cc:dd:ee:ff") ;
+    CalibrationRequest calibrationrequest1 ;
+    Measurement measurement1(&ap1) ;
+    measurement1.add_ss(-23) ;
+    std::tr1::unordered_map<std::string, Measurement> measurements ;
+    measurements["aa:bb:cc:dd:ee:ff"] = measurement1 ;
+    calibrationrequest1.set_measurements(measurements) ;
+    TS_ASSERT_THROWS(Stock::closest_reference_point(calibrationrequest1),
+                     element_not_found) ;
+
+    ReferencePoint referencepoint1(3,5,7) ;
+    referencepoint1.add_request(&calibrationrequest1) ;
+    const ReferencePoint &referencepoint1_ref =
+      Stock::find_create_reference_point(referencepoint1) ;
+    TS_ASSERT_EQUALS(
+      &Stock::closest_reference_point(calibrationrequest1),
+      &referencepoint1_ref) ;
+
+    CalibrationRequest calibrationrequest2 ;
+    Measurement measurement2(&ap1) ;
+    measurement2.add_ss(-78) ;
+    measurements.clear() ;
+    measurements["aa:bb:cc:dd:ee:ff"] = measurement2 ;
+    calibrationrequest2.set_measurements(measurements) ;
+    ReferencePoint referencepoint2(88,99,111) ;
+    referencepoint2.add_request(&calibrationrequest2) ;
+    const ReferencePoint &referencepoint2_ref =
+      Stock::find_create_reference_point(referencepoint2) ;
+    TS_ASSERT_EQUALS(
+      &Stock::closest_reference_point(calibrationrequest1),
+      &referencepoint1_ref) ;
+    TS_ASSERT_EQUALS(
+      &Stock::closest_reference_point(calibrationrequest2),
+      &referencepoint2_ref) ;
+
+    Request request3 ;
+    Measurement measurement3(&ap1) ;
+    measurement3.add_ss(-75) ;
+    measurements.clear() ;
+    measurements["aa:bb:cc:dd:ee:ff"] = measurement3 ;
+    request3.set_measurements(measurements) ;
+    TS_ASSERT_EQUALS(
+      &Stock::closest_reference_point(request3),
+      &referencepoint2_ref) ;
+
+    Request request4 ;
+    Measurement measurement4(&ap1) ;
+    measurement4.add_ss(-8) ;
+    measurements.clear() ;
+    measurements["aa:bb:cc:dd:ee:ff"] = measurement4 ;
+    request4.set_measurements(measurements) ;
+    TS_ASSERT_EQUALS(
+      &Stock::closest_reference_point(request4),
+      &referencepoint1_ref) ;
+  }
+
   void test_calibration_requests(void)
   {
     CalibrationRequest calibrationrequest1 ;