Wiselib
wiselib.testing/algorithms/localization/distance_based/math/localization_triangulation.h
Go to the documentation of this file.
00001 /***************************************************************************
00002  ** This file is part of the generic algorithm library Wiselib.           **
00003  ** Copyright (C) 2008,2009 by the Wisebed (www.wisebed.eu) project.      **
00004  **                                                                       **
00005  ** The Wiselib is free software: you can redistribute it and/or modify   **
00006  ** it under the terms of the GNU Lesser General Public License as        **
00007  ** published by the Free Software Foundation, either version 3 of the    **
00008  ** License, or (at your option) any later version.                       **
00009  **                                                                       **
00010  ** The Wiselib is distributed in the hope that it will be useful,        **
00011  ** but WITHOUT ANY WARRANTY; without even the implied warranty of        **
00012  ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         **
00013  ** GNU Lesser General Public License for more details.                   **
00014  **                                                                       **
00015  ** You should have received a copy of the GNU Lesser General Public      **
00016  ** License along with the Wiselib.                                       **
00017  ** If not, see <http://www.gnu.org/licenses/>.                           **
00018  ***************************************************************************/
00019 #ifndef __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_MATH_TRIANGULATION_H
00020 #define __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_MATH_TRIANGULATION_H
00021 
00022 #include "algorithms/localization/distance_based/math/vec.h"
00023 #include "algorithms/localization/distance_based/math/localization_simple_matrix.h"
00024 #include "algorithms/localization/distance_based/neighborhood/localization_neighborhood.h"
00025 #include "algorithms/localization/distance_based/util/localization_defutils.h"
00026 #include "util/pstl/algorithm.h"
00027 #include "util/pstl/pair.h"
00028 #include <float.h>
00029 
00030 //#include <isense/platforms/jennic/jennic_os.h>
00031 
00032 // TODO: Remove macros!!!
00033 #define SQR(n) (n*n)
00034 
00035 namespace wiselib
00036 {
00037 
00038    enum LaterationType
00039    {
00040       lat_anchors,  
00041       lat_confident 
00042    };
00043 
00055    template<typename OsModel_P,
00056             typename NeighborInfoList,
00057             typename Arithmatic_P>
00058    bool est_pos_min_max( const NeighborInfoList&, Vec<Arithmatic_P>& );
00076    template<typename OsModel_P,
00077             typename NeighborInfoList,
00078             typename Arithmatic_P>
00079    bool est_pos_lateration(
00080          const NeighborInfoList&,
00081          Vec<Arithmatic_P>&,
00082          const LaterationType&,
00083          bool );
00099    template<typename OsModel_P,
00100             typename NeighborInfoList,
00101             typename Arithmatic_P>
00102    bool check_residue(
00103          const NeighborInfoList&,
00104          const Vec<Arithmatic_P>&,
00105          const LaterationType&,
00106          Arithmatic_P );
00124    template<typename OsModel_P, typename DistancePair_P, typename Arithmatic_P>
00125    DistancePair_P trilateration_distance(
00126          Arithmatic_P, Arithmatic_P, Arithmatic_P, Arithmatic_P, Arithmatic_P );
00137    template<typename OsModel_P,
00138             typename Neighborhood,
00139             typename NeighborInfoList,
00140             typename Arithmatic_P>
00141    void collect_neighbors(
00142          Neighborhood&,
00143          const LaterationType&,
00144          NeighborInfoList& );
00145    // -----------------------------------------------------------------------
00146    // -----------------------------------------------------------------------
00147    // -----------------------------------------------------------------------
00148    template<typename OsModel_P,
00149             typename NeighborInfoList,
00150             typename Arithmatic_P>
00151    bool
00152    est_pos_min_max( const NeighborInfoList& neighbors, Vec<Arithmatic_P>& pos )
00153    {
00154       typedef typename NeighborInfoList::iterator NeighborInfoListIterator;
00155 
00156       if ( neighbors.empty() ) return false;
00157 
00158       wiselib::pair<Vec<Arithmatic_P>, Vec<Arithmatic_P> > intersection;
00159       NeighborInfoListIterator it = neighbors.begin();
00160 
00161       intersection.first = Vec<Arithmatic_P>(
00162          (*it)->pos().x() - (*it)->distance(),
00163          (*it)->pos().y() - (*it)->distance() );
00164       intersection.second = Vec<Arithmatic_P>(
00165          (*it)->pos().x() + (*it)->distance(),
00166          (*it)->pos().y() + (*it)->distance() );
00167 
00168       for ( ++it; it != neighbors.end(); ++it )
00169       {
00170          intersection.first = Vec<Arithmatic_P>(
00171             wiselib::max( intersection.first.x(),
00172                   (*it)->pos().x() - (*it)->distance() ),
00173             wiselib::max( intersection.first.y(),
00174                   (*it)->pos().y() - (*it)->distance() ) );
00175          intersection.second = Vec<Arithmatic_P>(
00176             wiselib::min( intersection.second.x(),
00177                   (*it)->pos().x() + (*it)->distance() ),
00178             wiselib::min( intersection.second.y(),
00179                   (*it)->pos().y() + (*it)->distance() ) );
00180       }
00181 
00182       pos = Vec<Arithmatic_P>(
00183          ( intersection.first.x() + intersection.second.x() ) / 2,
00184          ( intersection.first.y() + intersection.second.y() ) / 2 );
00185 
00186       return true;
00187    }
00188    // ----------------------------------------------------------------------
00189    template<typename OsModel_P,
00190             typename NeighborInfoList,
00191             typename Arithmatic_P>
00192    bool
00193    est_pos_lateration(
00194          const NeighborInfoList& neighbors,
00195          Vec<Arithmatic_P>& pos,
00196          const LaterationType& lat_type,
00197          bool use_pos )
00198    {
00199 
00200 
00201       typedef typename NeighborInfoList::iterator NeighborInfoListIterator;
00202 
00203 
00204       int nbr_size = neighbors.size();
00205       if ( nbr_size < 3 ) return false;
00206 
00207 
00208 
00209       typedef SimpleMatrix<OsModel_P, Arithmatic_P> Matrix;
00210       Matrix m_a;
00211       Matrix m_b;
00212       Matrix m_x;
00213 
00214 
00215 
00216       NeighborInfoListIterator it = neighbors.begin();
00217 
00218 
00219 
00220 
00221       Arithmatic_P x_1, y_1, d_1;
00222       if ( use_pos )
00223       {
00224          m_a = Matrix( nbr_size, 2 );
00225          m_b = Matrix( nbr_size, 1 );
00226          x_1 = pos.x();
00227          y_1 = pos.y();
00228          d_1 = 0;
00229       }
00230       else
00231       {
00232          m_a = Matrix( nbr_size - 1, 2 );
00233          m_b = Matrix( nbr_size - 1, 1 );
00234          x_1 = (*it)->pos().x();
00235          y_1 = (*it)->pos().y();
00236          d_1 = (*it)->distance();
00237          ++it;
00238       }
00239 
00240      /* if(isense::JennicOs::jennic_os->id()==0x9999)
00241                        isense::JennicOs::jennic_os->debug("befgin it %f %f %f",x_1,y_1,d_1);*/
00242 
00243       int row = 0;
00244       for ( ; it != neighbors.end(); ++it )
00245       {
00246          Arithmatic_P confidence = (*it)->confidence();
00247 
00248              /*  if(isense::JennicOs::jennic_os->id()==0x9999)
00249                  isense::JennicOs::jennic_os->debug("it %f %f %f %f",(*it)->pos().x(),(*it)->pos().y(),(*it)->distance(),(*it)->confidence());*/
00250 
00251          if ( lat_type == lat_anchors ) confidence = 1;
00252 
00253 
00254          m_a(row,0) = 2 * ( (*it)->pos().x() - x_1 ) * confidence;
00255          m_a(row,1) = 2 * ( (*it)->pos().y() - y_1 ) * confidence;
00256 
00257        m_b(row,0) =
00258             ( SQR( (*it)->pos().x() ) - SQR( x_1 )
00259                + SQR( (*it)->pos().y() ) - SQR( y_1 )
00260                + SQR( d_1 )
00261                - SQR( (*it)->distance() ) )
00262             * confidence;
00263 //    std::cout << "distanz von Knoten"<<  <<"gesch�tzt: " << (*
00264 /*     m_b(row,0) = 
00265           ( SQR( (*it)->pos().x() -  x_1 )
00266                + SQR( (*it)->pos().y() - y_1 )
00267                + SQR( d_1 )
00268                - SQR( (*it)->distance() ) ) *  0.5;
00269 */         row++;
00270       }
00271 
00272 
00273 //
00274 //     if (
00275 //            (( m_a.transposed() * m_a ).det() < 0.0001 )&&
00276 //            (( m_a.transposed() * m_a ).det() > -0.0001 )
00277 //       ){
00278 //         /*if(isense::JennicOs::jennic_os->id()==0x9999)
00279 //            isense::JennicOs::jennic_os->debug("before second transport");*/
00280 //         return false;
00281 //      }else
00282 //      {
00283 //        /* if(isense::JennicOs::jennic_os->id()==0x9999)
00284 //                  isense::JennicOs::jennic_os->debug("det %f",( m_a.transposed() * m_a ).det());*/
00285 //        // return true;
00286 //      }
00287 
00288 
00289 
00290 Matrix tmp = m_a.transposed();
00291 tmp*=m_a;
00292 
00293 Arithmatic_P det = tmp.det();
00294 
00295 
00296 
00297 
00298 if( (det < 0.0001 )&&(det > -0.0001 ))
00299 {
00300    return false;
00301 }
00302 
00303 
00304 
00305 
00306 m_x = tmp.inverse();
00307 
00308 
00309 
00310 tmp = m_a.transposed();
00311 
00312 
00313 
00314 
00315 
00316 
00317 tmp*=m_b;
00318 
00319 
00320 
00321 
00322 m_x*=tmp;
00323 
00324 
00325 
00326      // solve Ax = b
00327  /*     m_x = ( m_a.transposed() * m_a ).inverse()
00328          * ( m_a.transposed() * m_b );*/
00329 
00330 /*      if(isense::JennicOs::jennic_os->id()==0x9999)
00331         isense::JennicOs::jennic_os->debug("after second transport");
00332       return true;*/
00333 
00334       pos = Vec<Arithmatic_P>( m_x(0,0), m_x(1,0));
00335 
00336       return true;
00337    }
00338    // ----------------------------------------------------------------------
00339    template<typename OsModel_P,
00340             typename NeighborInfoList,
00341             typename Arithmatic_P>
00342    bool
00343    check_residue(
00344          const NeighborInfoList& neighbors,
00345          const Vec<Arithmatic_P>& est_pos,
00346          const LaterationType& lat_type,
00347          Arithmatic_P comm_range )
00348    {
00349       typedef typename NeighborInfoList::iterator NeighborInfoListIterator;
00350 
00351       int nbr_size = neighbors.size();
00352       if ( nbr_size == 0 ) return false;
00353 
00354       Arithmatic_P residue = 0;
00355       Arithmatic_P conf_sum = 0;
00356 
00357       for ( NeighborInfoListIterator
00358                it = neighbors.begin();
00359                it != neighbors.end();
00360                ++it )
00361       {
00362          Arithmatic_P confidence = (*it)->confidence();
00363          if ( lat_type == lat_anchors ) confidence = 1;
00364 
00365          conf_sum += confidence;
00366          residue +=
00367             ( sqrt(
00368                   SQR( est_pos.x() - (*it)->pos().x() ) +
00369                   SQR( est_pos.y() - (*it)->pos().y() ) )
00370                - (*it)->distance() )
00371             * confidence;
00372       }
00373 
00374       residue /= conf_sum;
00375 
00376       if ( fabs( residue ) > comm_range )
00377          return false;
00378       else
00379          return true;
00380    }
00381    // ----------------------------------------------------------------------
00382    template<typename OsModel_P, typename DistancePair_P, typename Arithmatic_P>
00383    DistancePair_P
00384    trilateration_distance(
00385          Arithmatic_P s_n1,
00386          Arithmatic_P s_n2,
00387          Arithmatic_P n1_n2,
00388          Arithmatic_P n1_anchor,
00389          Arithmatic_P n2_anchor )
00390    {
00391       typedef DistancePair_P DistancePair;
00392       typedef Arithmatic_P Arithmatic;
00393 
00394       // trilaterate triangle self-n1-n2
00395       Arithmatic_P self_x = ( SQR(s_n1) - SQR(s_n2) + SQR(n1_n2) ) / ( 2*n1_n2 );
00396       Arithmatic_P tmp = SQR(s_n1) - SQR(self_x);
00397       if ( tmp < -0.1 )
00398          return DistancePair( -1, -1 );
00399 
00400       Arithmatic_P self_y = sqrt( fabs(tmp) );
00401 
00402       // trilaterate triangle anchor-n1-n2
00403       Arithmatic_P anchor_x = ( SQR(n1_anchor) - SQR(n2_anchor) + SQR(n1_n2) ) / ( 2*n1_n2 );
00404       tmp = SQR(n1_anchor) - SQR(anchor_x);
00405       if ( tmp < -0.1 )
00406          return DistancePair( -1, -1 );
00407       Arithmatic_P anchor_y = sqrt( fabs(tmp) );
00408 
00409       // distance between self and anchor
00410       Arithmatic_P d_x = self_x - anchor_x;
00411       Arithmatic_P d_y1 = self_y - anchor_y;
00412       Arithmatic_P d_y2 = self_y + anchor_y;
00413 
00414       Arithmatic_P dist1 = sqrt( SQR(d_x) + SQR(d_y1) );
00415       Arithmatic_P dist2 = sqrt( SQR(d_x) + SQR(d_y2) );
00416 
00417       return DistancePair( dist1, dist2 );
00418    }
00419    // ----------------------------------------------------------------------
00420    template<typename OsModel_P,
00421             typename Neighborhood,
00422             typename NeighborInfoList,
00423             typename Arithmatic_P>
00424    void
00425    collect_neighbors(
00426          Neighborhood& nbrhood,
00427          const LaterationType& lat_type,
00428          NeighborInfoList& nbrs )
00429    {
00430       typedef typename Neighborhood::NeighborhoodIterator NeighborhoodIterator;
00431 
00432       for ( NeighborhoodIterator
00433                it = nbrhood.begin_neighborhood();
00434                it != nbrhood.end_neighborhood();
00435                ++it )
00436          if ( ( lat_type == lat_anchors && it->second.is_anchor() && it->second.is_valid() ) ||
00437                ( lat_type == lat_confident && it->second.is_confident() ) )
00438             nbrs.push_back( &it->second );
00439    }
00440 
00441 }// namespace wiselib
00442 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines