|
Wiselib
|
wiselib.testing/algorithms/localization/distance_based/neighborhood/localization_local_coordinate_system.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_NEIGHBORHOOD_LCS_H 00020 #define __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_NEIGHBORHOOD_LCS_H 00021 00022 #include "algorithms/localization/distance_based/math/vec.h" 00023 #include "algorithms/localization/distance_based/math/localization_lcs_helpers.h" 00024 #include "algorithms/localization/distance_based/math/localization_general_math.h" 00025 #include "algorithms/localization/distance_based/math/localization_triangulation.h" 00026 #include "algorithms/localization/distance_based/util/localization_defutils.h" 00027 #include "algorithms/localization/distance_based/neighborhood/localization_neighborhood.h" 00028 00029 00030 namespace wiselib 00031 { 00032 00034 00041 template<typename OsModel_P, 00042 typename node_id_t_P, 00043 typename Neighborhood_P, 00044 typename LocationMap_P, 00045 typename Arithmatic_P > 00046 class LocalizationLocalCoordinateSystem 00047 { 00048 00049 public: 00050 typedef OsModel_P OsModel; 00051 typedef node_id_t_P node_id_t; 00052 typedef Neighborhood_P Neighborhood; 00053 typedef LocationMap_P LocationMap; 00054 typedef Arithmatic_P Arithmatic; 00055 typedef typename LocationMap::iterator LocationMapIterator; 00056 00057 typedef LocalizationLocalCoordinateSystem<OsModel, node_id_t, Neighborhood, LocationMap, Arithmatic> self_type; 00058 00060 struct CorrectionData 00061 { 00062 Arithmatic correction_angle; 00063 bool mirror; 00064 Vec<Arithmatic_P> pos; 00065 }; 00066 00070 LocalizationLocalCoordinateSystem(); 00072 LocalizationLocalCoordinateSystem( const self_type& ); 00074 ~LocalizationLocalCoordinateSystem(); 00076 00079 00081 void update_basic_nodes( node_id_t, node_id_t, Neighborhood& ); 00084 void update_node( node_id_t ); 00087 void set_src_node( node_id_t ); 00090 void set_position( Vec<Arithmatic_P>& ); 00092 00095 00097 Arithmatic epsilon( void ); 00100 void set_epsilon( Arithmatic ); 00103 bool is_valid( void ); 00107 int size( void ); 00110 node_id_t src_node( void ); 00115 bool has_neighbor( node_id_t ); 00118 const Vec<Arithmatic_P>& node_position( node_id_t ); 00121 Vec<Arithmatic_P>& src_position( void ); 00123 00126 00128 void perform_correction( CorrectionData& ); 00131 void rotate( Arithmatic ); 00134 void mirror_x( void ); 00137 void mirror_y( void ); 00145 bool correct_lcs( self_type&, CorrectionData& ); 00152 bool correct_lcs_to_real_ncs( CorrectionData& ); 00154 00157 00159 void clear( void ); 00161 00162 private: 00163 00164 node_id_t p_, q_, src_node_; 00165 Neighborhood* neighborhood_; 00166 Vec<Arithmatic_P> src_pos_; 00167 00168 LocationMap locations_; 00169 00170 bool valid_; 00171 00172 Arithmatic epsilon_; 00173 }; 00174 // ---------------------------------------------------------------------- 00175 // ---------------------------------------------------------------------- 00176 // ---------------------------------------------------------------------- 00177 template<typename OsModel_P, 00178 typename node_id_t_P, 00179 typename Neighborhood_P, 00180 typename LocationMap_P, 00181 typename Arithmatic_P> 00182 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00183 LocalizationLocalCoordinateSystem() 00184 : src_pos_ ( Vec<Arithmatic_P>( 0.0, 0.0, 0.0 ) ), 00185 valid_ ( false ), 00186 epsilon_ ( 0.0001 ) 00187 {} 00188 // ---------------------------------------------------------------------- 00189 template<typename OsModel_P, 00190 typename node_id_t_P, 00191 typename Neighborhood_P, 00192 typename LocationMap_P, 00193 typename Arithmatic_P> 00194 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00195 LocalizationLocalCoordinateSystem( const LocalizationLocalCoordinateSystem& llcs ) 00196 : p_ ( llcs.p_ ), 00197 q_ ( llcs.q_ ), 00198 src_node_ ( llcs.src_node_ ), 00199 neighborhood_ ( llcs.neighborhood_ ), 00200 src_pos_ ( llcs.src_pos_ ), 00201 locations_ ( llcs.locations_ ), 00202 valid_ ( llcs.valid_ ), 00203 epsilon_ ( llcs.epsilon_ ) 00204 {} 00205 // ---------------------------------------------------------------------- 00206 template<typename OsModel_P, 00207 typename node_id_t_P, 00208 typename Neighborhood_P, 00209 typename LocationMap_P, 00210 typename Arithmatic_P> 00211 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00212 ~LocalizationLocalCoordinateSystem() 00213 {} 00214 // ---------------------------------------------------------------------- 00215 template<typename OsModel_P, 00216 typename node_id_t_P, 00217 typename Neighborhood_P, 00218 typename LocationMap_P, 00219 typename Arithmatic_P> 00220 void 00221 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00222 update_basic_nodes( node_id_t p, node_id_t q, Neighborhood& nbrh ) 00223 { 00224 p_ = p; 00225 q_ = q; 00226 neighborhood_ = &nbrh; 00227 00228 Arithmatic src_p = neighborhood_->neighbor_distance( p_ ); 00229 Arithmatic src_q = neighborhood_->neighbor_distance( q_ ); 00230 Arithmatic p_q = neighborhood_->nneighbor_distance( p_, q_ ); 00231 00232 Vec<Arithmatic_P> pos_p, pos_q; 00233 00234 if ( !compute_basis_coords( src_p, src_q, p_q, pos_p, pos_q ) ) 00235 { 00236 if ( !valid_ ) 00237 clear(); 00238 return; 00239 } 00240 00241 locations_[p_] = pos_p; 00242 locations_[q_] = pos_q; 00243 00244 valid_ = true; 00245 } 00246 // ---------------------------------------------------------------------- 00247 template<typename OsModel_P, 00248 typename node_id_t_P, 00249 typename Neighborhood_P, 00250 typename LocationMap_P, 00251 typename Arithmatic_P> 00252 void 00253 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00254 update_node( node_id_t j ) 00255 { 00256 if ( !valid_ ) 00257 return; 00258 00259 Arithmatic src_p = neighborhood_->neighbor_distance( p_ ); 00260 Arithmatic src_q = neighborhood_->neighbor_distance( q_ ); 00261 Arithmatic src_j = neighborhood_->neighbor_distance( j ); 00262 Arithmatic p_j = neighborhood_->nneighbor_distance( p_, j ); 00263 Arithmatic q_j = neighborhood_->nneighbor_distance( q_, j ); 00264 Arithmatic p_q = neighborhood_->nneighbor_distance( p_, q_ ); 00265 00266 Vec<Arithmatic_P> pos_j; 00267 00268 if ( !compute_rel_coord_triangulisation( src_p, src_q, src_j, p_j, q_j, p_q, pos_j, epsilon_ ) ) 00269 return; 00270 00271 locations_[j] = pos_j; 00272 } 00273 // ---------------------------------------------------------------------- 00274 template<typename OsModel_P, 00275 typename node_id_t_P, 00276 typename Neighborhood_P, 00277 typename LocationMap_P, 00278 typename Arithmatic_P> 00279 void 00280 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00281 set_src_node( node_id_t src_node ) 00282 { 00283 src_node_ = src_node; 00284 } 00285 // ---------------------------------------------------------------------- 00286 template<typename OsModel_P, 00287 typename node_id_t_P, 00288 typename Neighborhood_P, 00289 typename LocationMap_P, 00290 typename Arithmatic_P> 00291 void 00292 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00293 set_position( Vec<Arithmatic_P>& new_src_pos ) 00294 { 00295 for ( LocationMapIterator 00296 it = locations_.begin(); 00297 it != locations_.end(); 00298 ++it ) 00299 it->second += new_src_pos - src_pos_; 00300 00301 src_pos_ = new_src_pos; 00302 } 00303 // ---------------------------------------------------------------------- 00304 template<typename OsModel_P, 00305 typename node_id_t_P, 00306 typename Neighborhood_P, 00307 typename LocationMap_P, 00308 typename Arithmatic_P> 00309 typename LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>::Arithmatic 00310 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00311 epsilon( void ) 00312 { 00313 return epsilon_; 00314 } 00315 // ---------------------------------------------------------------------- 00316 template<typename OsModel_P, 00317 typename node_id_t_P, 00318 typename Neighborhood_P, 00319 typename LocationMap_P, 00320 typename Arithmatic_P> 00321 void 00322 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00323 set_epsilon( Arithmatic epsilon ) 00324 { 00325 epsilon_ = epsilon; 00326 } 00327 // ---------------------------------------------------------------------- 00328 template<typename OsModel_P, 00329 typename node_id_t_P, 00330 typename Neighborhood_P, 00331 typename LocationMap_P, 00332 typename Arithmatic_P> 00333 bool 00334 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00335 is_valid( void ) 00336 { 00337 return valid_; 00338 } 00339 // ---------------------------------------------------------------------- 00340 template<typename OsModel_P, 00341 typename node_id_t_P, 00342 typename Neighborhood_P, 00343 typename LocationMap_P, 00344 typename Arithmatic_P> 00345 int 00346 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00347 size( void ) 00348 { 00349 return locations_.size(); 00350 } 00351 // ---------------------------------------------------------------------- 00352 template<typename OsModel_P, 00353 typename node_id_t_P, 00354 typename Neighborhood_P, 00355 typename LocationMap_P, 00356 typename Arithmatic_P> 00357 typename LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>::node_id_t 00358 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00359 src_node( void ) 00360 { 00361 return src_node_; 00362 } 00363 // ---------------------------------------------------------------------- 00364 template<typename OsModel_P, 00365 typename node_id_t_P, 00366 typename Neighborhood_P, 00367 typename LocationMap_P, 00368 typename Arithmatic_P> 00369 bool 00370 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00371 has_neighbor( node_id_t node ) 00372 { 00373 return ( locations_.find( node ) != locations_.end() ); 00374 } 00375 // ---------------------------------------------------------------------- 00376 template<typename OsModel_P, 00377 typename node_id_t_P, 00378 typename Neighborhood_P, 00379 typename LocationMap_P, 00380 typename Arithmatic_P> 00381 const Vec<Arithmatic_P>& 00382 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00383 node_position( node_id_t node ) 00384 { 00385 if ( locations_.find( node ) != locations_.end() ) 00386 return locations_.find( node )->second; 00387 00388 return UNKNOWN_POSITION; 00389 } 00390 // ---------------------------------------------------------------------- 00391 template<typename OsModel_P, 00392 typename node_id_t_P, 00393 typename Neighborhood_P, 00394 typename LocationMap_P, 00395 typename Arithmatic_P> 00396 Vec<Arithmatic_P>& 00397 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00398 src_position( void ) 00399 { 00400 return src_pos_; 00401 } 00402 // ---------------------------------------------------------------------- 00403 template<typename OsModel_P, 00404 typename node_id_t_P, 00405 typename Neighborhood_P, 00406 typename LocationMap_P, 00407 typename Arithmatic_P> 00408 void 00409 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00410 perform_correction( CorrectionData& cd ) 00411 { 00412 rotate( cd.correction_angle ); 00413 if ( cd.mirror ) 00414 mirror_x(); 00415 set_position( cd.pos ); 00416 } 00417 // ---------------------------------------------------------------------- 00418 template<typename OsModel_P, 00419 typename node_id_t_P, 00420 typename Neighborhood_P, 00421 typename LocationMap_P, 00422 typename Arithmatic_P> 00423 void 00424 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00425 rotate( Arithmatic angle ) 00426 { 00427 for ( LocationMapIterator 00428 it = locations_.begin(); 00429 it != locations_.end(); 00430 ++it ) 00431 rotate_2D( angle, it->second, src_pos_ ); 00432 } 00433 // ---------------------------------------------------------------------- 00434 template<typename OsModel_P, 00435 typename node_id_t_P, 00436 typename Neighborhood_P, 00437 typename LocationMap_P, 00438 typename Arithmatic_P> 00439 void 00440 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00441 mirror_x( void ) 00442 { 00443 for ( LocationMapIterator 00444 it = locations_.begin(); 00445 it != locations_.end(); 00446 ++it ) 00447 it->second = Vec<Arithmatic_P>( -it->second.x(), it->second.y(), it->second.z() ); 00448 } 00449 // ---------------------------------------------------------------------- 00450 template<typename OsModel_P, 00451 typename node_id_t_P, 00452 typename Neighborhood_P, 00453 typename LocationMap_P, 00454 typename Arithmatic_P> 00455 void 00456 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00457 mirror_y( void ) 00458 { 00459 for ( LocationMapIterator 00460 it = locations_.begin(); 00461 it != locations_.end(); 00462 ++it ) 00463 it->second = Vec<Arithmatic_P>( it->second.x(), -it->second.y(), it->second.z() ); 00464 } 00465 // ---------------------------------------------------------------------- 00466 template<typename OsModel_P, 00467 typename node_id_t_P, 00468 typename Neighborhood_P, 00469 typename LocationMap_P, 00470 typename Arithmatic_P> 00471 bool 00472 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00473 correct_lcs( LocalizationLocalCoordinateSystem& lcs, CorrectionData& cd ) 00474 { 00475 if ( !is_valid() || !lcs.is_valid() || 00476 lcs.node_position( src_node() ) == UNKNOWN_POSITION || 00477 node_position( lcs.src_node() ) == UNKNOWN_POSITION ) 00478 return false; 00479 00480 for ( LocationMapIterator 00481 it = locations_.begin(); 00482 it != locations_.end(); 00483 ++it ) 00484 if ( lcs.has_neighbor( it->first ) ) 00485 { 00486 int bas_node = lcs.src_node(); 00487 int ref_node = it->first; 00488 int me = src_node(); 00489 00490 if ( lcs.node_position( ref_node ) == UNKNOWN_POSITION || 00491 node_position( ref_node ) == UNKNOWN_POSITION || 00492 bas_node == ref_node || me == ref_node ) 00493 continue; 00494 00495 Arithmatic angle_bas_me = angle_vec( lcs.node_position( me ) - lcs.src_position() ); 00496 Arithmatic angle_bas_ref = angle_vec( lcs.node_position( ref_node ) - lcs.src_position() ); 00497 Arithmatic angle_me_bas = angle_vec( node_position( bas_node ) - src_position() ); 00498 Arithmatic angle_me_ref = angle_vec( node_position( ref_node ) - src_position() ); 00499 00500 Arithmatic me_bas = angle_me_ref - angle_me_bas; 00501 Arithmatic bas_me = angle_bas_ref - angle_bas_me; 00502 00503 me_bas = normalize_angle( me_bas ); 00504 bas_me = normalize_angle( bas_me ); 00505 00506 if ( ( bas_me < M_PI && me_bas < M_PI ) || 00507 ( bas_me > M_PI && me_bas > M_PI ) ) 00508 { 00509 Arithmatic correction_angle = angle_bas_me + angle_me_bas; 00510 cd.correction_angle = -correction_angle; 00511 cd.mirror = true; 00512 } 00513 else // if ( ( bas_me < M_PI && me_bas > M_PI ) || 00514 // ( bas_me > M_PI && me_bas < M_PI ) ) 00515 { 00516 Arithmatic correction_angle = angle_bas_me - angle_me_bas + M_PI; 00517 cd.correction_angle = correction_angle; 00518 cd.mirror = false; 00519 } 00520 00521 cd.correction_angle = normalize_angle( cd.correction_angle ); 00522 cd.pos = lcs.node_position( src_node() ); 00523 00524 return true; 00525 } 00526 00527 return false; 00528 } 00529 // ---------------------------------------------------------------------- 00530 template<typename OsModel_P, 00531 typename node_id_t_P, 00532 typename Neighborhood_P, 00533 typename LocationMap_P, 00534 typename Arithmatic_P> 00535 bool 00536 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00537 correct_lcs_to_real_ncs( CorrectionData& cd ) 00538 { 00539 if ( !is_valid() ) return false; 00540 00541 // TODO: This does only work for Shawn - but is quite useful when debugging 00542 // (visualizing) the algorithm. however, somehow there is a possibility 00543 // needed to get the real positions of other nodes in the network :/ 00544 // 00545 // int bas_node = q_; 00546 // int ref_node = p_; 00547 // int me = src_node(); 00548 // 00549 // Arithmatic angle_bas_me = angle_vec( me->real_position() - bas_node->real_position() ); 00550 // Arithmatic angle_bas_ref = angle_vec( ref_node->real_position() - bas_node->real_position() ); 00551 // Arithmatic angle_me_bas = angle_vec( node_position( bas_node ) - src_position() ); 00552 // Arithmatic angle_me_ref = angle_vec( node_position( ref_node ) - src_position() ); 00553 // 00554 // Arithmatic me_bas = angle_me_ref - angle_me_bas; 00555 // Arithmatic bas_me = angle_bas_ref - angle_bas_me; 00556 // 00557 // me_bas = normalize_angle( me_bas ); 00558 // bas_me = normalize_angle( bas_me ); 00559 // 00560 // if ( ( bas_me < M_PI && me_bas < M_PI ) || 00561 // ( bas_me > M_PI && me_bas > M_PI ) ) 00562 // { 00563 // Arithmatic correction_angle = angle_bas_me + angle_me_bas; 00564 // cd.correction_angle = -correction_angle; 00565 // cd.mirror = true; 00566 // } 00567 // else // if ( ( bas_me < M_PI && me_bas > M_PI ) || 00568 // // ( bas_me > M_PI && me_bas < M_PI ) ) 00569 // { 00570 // Arithmatic correction_angle = angle_bas_me - angle_me_bas + M_PI; 00571 // cd.correction_angle = correction_angle; 00572 // cd.mirror = false; 00573 // } 00574 // 00575 // cd.correction_angle = normalize_angle( cd.correction_angle ); 00576 // cd.pos = me->real_position(); 00577 00578 return true; 00579 } 00580 // ---------------------------------------------------------------------- 00581 template<typename OsModel_P, 00582 typename node_id_t_P, 00583 typename Neighborhood_P, 00584 typename LocationMap_P, 00585 typename Arithmatic_P> 00586 void 00587 LocalizationLocalCoordinateSystem<OsModel_P, node_id_t_P, Neighborhood_P, LocationMap_P, Arithmatic_P>:: 00588 clear( void ) 00589 { 00590 p_ = NULL; 00591 q_ = NULL; 00592 neighborhood_ = NULL; 00593 00594 src_pos_ = Vec<Arithmatic_P>( 0.0, 0.0, 0.0 ); 00595 locations_.clear(); 00596 valid_ = false; 00597 } 00598 00599 }// namespace wiselib 00600 #endif
