Wiselib
|
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