|
Wiselib
|
wiselib.testing/algorithms/localization/distance_based/neighborhood/localization_neighborhood.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_NEIGHBORHOOD_H 00020 #define __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_NEIGHBORHOOD_NEIGHBORHOOD_H 00021 00022 #include "algorithms/localization/distance_based/neighborhood/localization_neighbor_info.h" 00023 #include "algorithms/localization/distance_based/math/localization_statistic.h" 00024 #include "algorithms/localization/distance_based/math/vec.h" 00025 #include "algorithms/localization/distance_based/util/localization_defutils.h" 00026 #include "util/pstl/map_static_vector.h" 00027 00028 00029 namespace wiselib 00030 { 00031 00033 00059 template<typename OsModel_P, 00060 typename node_id_t_P, 00061 typename NeighborInfo_P, 00062 typename NeighborInfoMap_P, 00063 typename Arithmatic_P > 00064 class LocalizationNeighborhood 00065 { 00066 00067 public: 00068 typedef OsModel_P OsModel; 00069 typedef node_id_t_P node_id_t; 00070 00071 typedef NeighborInfo_P NeighborInfo; 00072 typedef typename NeighborInfo::NodeSet NodeSet; 00073 typedef typename NeighborInfo::DistanceMap DistanceMap; 00074 00075 typedef NeighborInfoMap_P NeighborInfoMap; 00076 typedef typename NeighborInfoMap::iterator NeighborhoodIterator; 00077 00078 typedef Arithmatic_P Arithmatic; 00079 00083 LocalizationNeighborhood(); 00085 ~LocalizationNeighborhood(); 00087 00088 00091 00095 void set_source( node_id_t ); 00098 node_id_t source( void ); 00100 00101 00104 00112 void update_anchor( node_id_t, Vec<Arithmatic>, Arithmatic = UNKNOWN_DISTANCE ); 00119 void update_neighbor( node_id_t, Arithmatic ); 00129 void update_nneighbor( node_id_t, node_id_t, Arithmatic ); 00138 void update_nneighbors( node_id_t, DistanceMap& ); 00140 00141 00144 00151 void set_ref_node( node_id_t, node_id_t ); 00164 void update_ref_node( node_id_t, node_id_t ); 00172 template<typename NodeList_P> 00173 void update_ref_nodes( node_id_t node, const NodeList_P& nl ) 00174 { 00175 NeighborhoodIterator it = find_w( node ); 00176 if ( it == end_neighborhood() ) 00177 return; 00178 00179 it->second->clear_ref_nodes(); 00180 it->second->ref_nodes_w().insert( nl.begin(), nl.end() ); 00181 } 00184 NodeSet ref_nodes( void ); 00187 void add_sound( node_id_t ); 00195 bool is_sound( void ); 00197 00198 00201 00205 bool has_anchor( node_id_t ); 00211 bool has_neighbor( node_id_t ); 00218 bool has_nneighbor( node_id_t, node_id_t ); 00228 bool has_valid_neighbor( node_id_t ); 00240 bool has_valid_nneighbor( node_id_t, node_id_t ); 00242 00243 00246 00250 Arithmatic neighbor_distance( node_id_t ); 00260 Arithmatic nneighbor_distance( node_id_t, node_id_t ); 00263 DistanceMap neighbor_distance_map( void ); 00265 00266 00269 00275 int valid_anchor_cnt( void ); 00282 int anchor_cnt(); 00290 int confident_neighbor_cnt( void ); 00299 Arithmatic avg_neighbor_confidence( void ); 00301 00302 00305 00314 void reassign_twins( Arithmatic ); 00316 00317 00320 00322 inline NeighborhoodIterator begin_neighborhood( void ) 00323 { return neighborhood_.begin(); } 00326 inline NeighborhoodIterator end_neighborhood( void ) 00327 { return neighborhood_.end(); } 00330 inline size_t size( void ) 00331 { return neighborhood_.size(); } 00336 inline NeighborhoodIterator find( node_id_t node ) 00337 { return neighborhood_.find( node ); } 00339 00340 inline NeighborInfoMap get_neighborhood(void ) 00341 { return neighborhood_; } 00342 00343 private: 00344 NeighborInfoMap neighborhood_; 00345 NodeSet sounds_; 00346 00347 int source_; 00348 }; 00349 // ---------------------------------------------------------------------- 00350 // ---------------------------------------------------------------------- 00351 // ---------------------------------------------------------------------- 00352 template<typename OsModel_P, 00353 typename node_id_t_P, 00354 typename NeighborInfo_P, 00355 typename NeighborInfoMap_P, 00356 typename Arithmatic_P> 00357 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P,Arithmatic_P>:: 00358 LocalizationNeighborhood() 00359 {} 00360 // ---------------------------------------------------------------------- 00361 template<typename OsModel_P, 00362 typename node_id_t_P, 00363 typename NeighborInfo_P, 00364 typename NeighborInfoMap_P, 00365 typename Arithmatic_P 00366 > 00367 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P,Arithmatic_P>:: 00368 ~LocalizationNeighborhood() 00369 {} 00370 // ---------------------------------------------------------------------- 00371 template<typename OsModel_P, 00372 typename node_id_t_P, 00373 typename NeighborInfo_P, 00374 typename NeighborInfoMap_P, 00375 typename Arithmatic_P> 00376 void 00377 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P,Arithmatic_P>:: 00378 set_source( node_id_t source ) 00379 { 00380 source_ = source; 00381 } 00382 // ---------------------------------------------------------------------- 00383 template<typename OsModel_P, 00384 typename node_id_t_P, 00385 typename NeighborInfo_P, 00386 typename NeighborInfoMap_P, 00387 typename Arithmatic_P> 00388 node_id_t_P 00389 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00390 source( void ) 00391 { 00392 return source_; 00393 } 00394 // ---------------------------------------------------------------------- 00395 template<typename OsModel_P, 00396 typename node_id_t_P, 00397 typename NeighborInfo_P, 00398 typename NeighborInfoMap_P, 00399 typename Arithmatic_P> 00400 void 00401 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00402 update_anchor( node_id_t node, Vec<Arithmatic> pos, Arithmatic distance ) 00403 { 00404 NeighborhoodIterator it = find( node ); 00405 if ( it != end_neighborhood() ) 00406 { 00407 it->second.set_pos( pos ); 00408 if ( distance != UNKNOWN_DISTANCE ) 00409 it->second.set_distance( distance ); 00410 it->second.set_anchor( true ); 00411 return; 00412 } 00413 00414 // TODO: NO NEW!!! 00415 // NeighborInfo *nih = new NeighborInfo( node, true ); 00416 // nih->set_pos( pos ); 00417 // if ( distance != UNKNOWN_DISTANCE ) 00418 // nih->set_distance( distance ); 00419 // neighborhood_[node] = nih; 00420 00421 // NeighborInfo nih2 = NeighborInfo( node, true ); 00422 // temp_[node] = nih2; 00423 // neighborhood_[node] = &(temp_[index_]); 00424 // index_++; 00425 00426 neighborhood_[node].set_node(node); 00427 neighborhood_[node].set_anchor(true); 00428 neighborhood_[node].set_pos( pos ); 00429 if ( distance != UNKNOWN_DISTANCE ) 00430 neighborhood_[node].set_distance( distance ); 00431 } 00432 // ---------------------------------------------------------------------- 00433 template<typename OsModel_P, 00434 typename node_id_t_P, 00435 typename NeighborInfo_P, 00436 typename NeighborInfoMap_P, 00437 typename Arithmatic_P> 00438 void 00439 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00440 update_neighbor( node_id_t node, Arithmatic distance ) 00441 { 00442 NeighborhoodIterator it = find( node ); 00443 if ( it != end_neighborhood() ) 00444 { 00445 it->second->set_distance( distance ); 00446 return; 00447 } 00448 00449 // TODO: NO NEW!!! 00450 // NeighborInfo *nih = new NeighborInfo( node, false ); 00451 // nih->set_distance( distance ); 00452 // neighborhood_[node] = nih; 00453 00454 // NeighborInfo nih2 = NeighborInfo( node, false ); 00455 // temp_[node] = nih2; 00456 // neighborhood_[node] = &(temp_[index_]); 00457 // index_++; 00458 00459 neighborhood_[node].set_node(node); 00460 neighborhood_[node].set_anchor(false); 00461 neighborhood_[node].set_distance( distance ); 00462 } 00463 // ---------------------------------------------------------------------- 00464 template<typename OsModel_P, 00465 typename node_id_t_P, 00466 typename NeighborInfo_P, 00467 typename NeighborInfoMap_P, 00468 typename Arithmatic_P> 00469 void 00470 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00471 update_nneighbor( node_id_t node, node_id_t neighbor, Arithmatic distance ) 00472 { 00473 if ( node == neighbor ) 00474 return; 00475 00476 NeighborhoodIterator it; 00477 00478 if ( ( it = find( node ) ) != end_neighborhood() ) 00479 it->second->update_neighbor( neighbor, distance ); 00480 00481 if ( ( it = find( neighbor ) ) != end_neighborhood() ) 00482 it->second->update_neighbor( node, distance ); 00483 } 00484 // ---------------------------------------------------------------------- 00485 template<typename OsModel_P, 00486 typename node_id_t_P, 00487 typename NeighborInfo_P, 00488 typename NeighborInfoMap_P, 00489 typename Arithmatic_P> 00490 void 00491 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00492 update_nneighbors( node_id_t node, DistanceMap& dm ) 00493 { 00494 NeighborhoodIterator it = find( node ); 00495 if ( it == end_neighborhood() ) 00496 return; 00497 00498 it->second->update_neighbors( dm ); 00499 } 00500 // ---------------------------------------------------------------------- 00501 template<typename OsModel_P, 00502 typename node_id_t_P, 00503 typename NeighborInfo_P, 00504 typename NeighborInfoMap_P, 00505 typename Arithmatic_P> 00506 void 00507 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P,Arithmatic_P>:: 00508 set_ref_node( node_id_t node, node_id_t ref ) 00509 { 00510 NeighborhoodIterator it = find( node ); 00511 if ( it == end_neighborhood() ) 00512 return; 00513 00514 it->second.clear_ref_nodes(); 00515 it->second.add_ref_node( ref ); 00516 } 00517 // ---------------------------------------------------------------------- 00518 template<typename OsModel_P, 00519 typename node_id_t_P, 00520 typename NeighborInfo_P, 00521 typename NeighborInfoMap_P, 00522 typename Arithmatic_P> 00523 void 00524 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P,Arithmatic_P>:: 00525 update_ref_node( node_id_t node, node_id_t ref ) 00526 { 00527 NeighborhoodIterator it = find( node ); 00528 if ( it == end_neighborhood() ) 00529 return; 00530 00531 //FIXME: check, if 'set of ref-nodes' is getting better/greater by adding the node. 00532 // -> Too much computing??? 00533 it->second.clear_ref_nodes(); 00534 it->second.add_ref_node( ref ); 00535 } 00536 // ---------------------------------------------------------------------- 00537 template<typename OsModel_P, 00538 typename node_id_t_P, 00539 typename NeighborInfo_P, 00540 typename NeighborInfoMap_P, 00541 typename Arithmatic_P> 00542 typename LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P,Arithmatic_P>::NodeSet 00543 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P,Arithmatic_P>:: 00544 ref_nodes( void ) 00545 { 00546 NodeSet temp; 00547 00548 for ( NeighborhoodIterator 00549 it = begin_neighborhood(); 00550 it != end_neighborhood(); 00551 ++it ) 00552 if ( it->second->is_anchor() && it->second->is_valid() ) 00553 { 00554 NodeSet references = it->second->ref_nodes(); 00555 for ( typename NodeSet::iterator 00556 refit = references.begin(); 00557 refit != references.end(); 00558 ++refit ) 00559 temp.insert( *refit ); 00560 } 00561 00562 return temp; 00563 } 00564 // ---------------------------------------------------------------------- 00565 template<typename OsModel_P, 00566 typename node_id_t_P, 00567 typename NeighborInfo_P, 00568 typename NeighborInfoMap_P, 00569 typename Arithmatic_P> 00570 void 00571 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P,Arithmatic_P>:: 00572 add_sound( node_id_t node ) 00573 { 00574 sounds_.insert( node ); 00575 } 00576 // ---------------------------------------------------------------------- 00577 template<typename OsModel_P, 00578 typename node_id_t_P, 00579 typename NeighborInfo_P, 00580 typename NeighborInfoMap_P, 00581 typename Arithmatic_P> 00582 bool 00583 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00584 is_sound( void ) 00585 { 00586 NodeSet temp = ref_nodes(); 00587 00588 for ( typename NodeSet::iterator 00589 sit = sounds_.begin(); 00590 sit != sounds_.end(); 00591 ++sit ) 00592 temp.insert( *sit ); 00593 00594 // if there is no info about ref-nodes of the anchors, ignore this 00595 // check return true anyway 00596 if ( anchor_cnt() > 0 && temp.size() == 0 ) 00597 return true; 00598 00599 //TODO: Dimension anpassen !!!! 00600 return (int)temp.size() >= 3; 00601 } 00602 // ---------------------------------------------------------------------- 00603 template<typename OsModel_P, 00604 typename node_id_t_P, 00605 typename NeighborInfo_P, 00606 typename NeighborInfoMap_P, 00607 typename Arithmatic_P> 00608 bool 00609 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00610 has_anchor( node_id_t node ) 00611 { 00612 NeighborhoodIterator it = neighborhood_.find( node ); 00613 00614 return ( it != end_neighborhood() && it->second->is_anchor() ); 00615 } 00616 // ---------------------------------------------------------------------- 00617 template<typename OsModel_P, 00618 typename node_id_t_P, 00619 typename NeighborInfo_P, 00620 typename NeighborInfoMap_P, 00621 typename Arithmatic_P> 00622 bool 00623 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00624 has_neighbor( node_id_t node ) 00625 { 00626 return ( neighborhood_.find( node ) != end_neighborhood() ); 00627 } 00628 // ---------------------------------------------------------------------- 00629 template<typename OsModel_P, 00630 typename node_id_t_P, 00631 typename NeighborInfo_P, 00632 typename NeighborInfoMap_P, 00633 typename Arithmatic_P> 00634 bool 00635 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00636 has_nneighbor( node_id_t node, node_id_t neighbor ) 00637 { 00638 if ( node == neighbor ) 00639 return false; 00640 00641 NeighborhoodIterator it = neighborhood_.find( node ); 00642 if ( it == end_neighborhood() ) 00643 return false; 00644 00645 return it->second->has_neighbor( neighbor ); 00646 } 00647 // ---------------------------------------------------------------------- 00648 template<typename OsModel_P, 00649 typename node_id_t_P, 00650 typename NeighborInfo_P, 00651 typename NeighborInfoMap_P, 00652 typename Arithmatic_P> 00653 bool 00654 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00655 has_valid_neighbor( node_id_t node ) 00656 { 00657 NeighborhoodIterator it = neighborhood_.find( node ); 00658 if ( it == end_neighborhood() ) return false; 00659 00660 return ( it->second->has_distance() ); 00661 } 00662 // ---------------------------------------------------------------------- 00663 template<typename OsModel_P, 00664 typename node_id_t_P, 00665 typename NeighborInfo_P, 00666 typename NeighborInfoMap_P, 00667 typename Arithmatic_P> 00668 bool 00669 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00670 has_valid_nneighbor( node_id_t node, node_id_t neighbor ) 00671 { 00672 if ( node == neighbor ) 00673 return false; 00674 00675 NeighborhoodIterator it = neighborhood_.find( node ); 00676 if ( it == end_neighborhood() ) return false; 00677 00678 return ( it->second->has_neighbor( neighbor ) && 00679 it->second->has_neighbor_distance( neighbor ) ); 00680 } 00681 // ---------------------------------------------------------------------- 00682 template<typename OsModel_P, 00683 typename node_id_t_P, 00684 typename NeighborInfo_P, 00685 typename NeighborInfoMap_P, 00686 typename Arithmatic_P> 00687 typename LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>::Arithmatic 00688 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00689 neighbor_distance( node_id_t node ) 00690 { 00691 NeighborhoodIterator it = neighborhood_.find( node ); 00692 if ( it == end_neighborhood() ) 00693 return UNKNOWN_DISTANCE; 00694 00695 return ( it->second->distance() ); 00696 } 00697 // ---------------------------------------------------------------------- 00698 template<typename OsModel_P, 00699 typename node_id_t_P, 00700 typename NeighborInfo_P, 00701 typename NeighborInfoMap_P, 00702 typename Arithmatic_P> 00703 typename LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>::Arithmatic 00704 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00705 nneighbor_distance( node_id_t node, node_id_t neighbor ) 00706 { 00707 if ( node == neighbor ) 00708 return 0; 00709 00710 NeighborhoodIterator it1 = neighborhood_.find( node ); 00711 NeighborhoodIterator it2 = neighborhood_.find( neighbor ); 00712 00713 Arithmatic sum = 0.0; 00714 00715 if ( it1 != end_neighborhood() && 00716 it1->second->has_neighbor_distance( neighbor ) ) 00717 sum += it1->second->neighbor_distance( neighbor ); 00718 else 00719 return UNKNOWN_DISTANCE; 00720 00721 if ( it2 != end_neighborhood() && 00722 it2->second->has_neighbor_distance( node ) ) 00723 sum += it2->second->neighbor_distance( node ); 00724 else 00725 return UNKNOWN_DISTANCE; 00726 00727 return sum / 2; 00728 #if 0 00729 if ( node == neighbor ) 00730 return 0; 00731 00732 NeighborhoodIterator it1 = neighborhood_.find( node ); 00733 NeighborhoodIterator it2 = neighborhood_.find( neighbor ); 00734 00735 LocalizationStatistic<OsModel> stat; 00736 00737 if ( it1 != end_neighborhood() && 00738 it1->second->has_neighbor_distance( neighbor ) ) 00739 stat += it1->second->neighbor_distance( neighbor ); 00740 00741 if ( it2 != end_neighborhood() && 00742 it2->second->has_neighbor_distance( node ) ) 00743 stat += it2->second->neighbor_distance( node ); 00744 00745 if ( stat.size() == 0 ) 00746 return UNKNOWN_DISTANCE; 00747 00748 return ( stat.mean() ); 00749 #endif 00750 return 0; 00751 } 00752 // ---------------------------------------------------------------------- 00753 template<typename OsModel_P, 00754 typename node_id_t_P, 00755 typename NeighborInfo_P, 00756 typename NeighborInfoMap_P, 00757 typename Arithmatic_P> 00758 typename LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>::DistanceMap 00759 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00760 neighbor_distance_map( void ) 00761 { 00762 DistanceMap temp; 00763 00764 for ( NeighborhoodIterator 00765 it = begin_neighborhood(); 00766 it != end_neighborhood(); 00767 ++it ) 00768 temp[it->second->node()] = it->second->distance(); 00769 00770 return temp; 00771 } 00772 // ---------------------------------------------------------------------- 00773 template<typename OsModel_P, 00774 typename node_id_t_P, 00775 typename NeighborInfo_P, 00776 typename NeighborInfoMap_P, 00777 typename Arithmatic_P> 00778 int 00779 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00780 valid_anchor_cnt( void ) 00781 { 00782 int cnt = 0; 00783 00784 for ( NeighborhoodIterator 00785 it = begin_neighborhood(); 00786 it != end_neighborhood(); 00787 ++it ) 00788 if ( it->second->is_anchor() && it->second->is_valid() ) 00789 ++cnt; 00790 00791 return cnt; 00792 } 00793 // ---------------------------------------------------------------------- 00794 template<typename OsModel_P, 00795 typename node_id_t_P, 00796 typename NeighborInfo_P, 00797 typename NeighborInfoMap_P, 00798 typename Arithmatic_P> 00799 int 00800 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00801 anchor_cnt() 00802 { 00803 int cnt = 0; 00804 00805 for ( NeighborhoodIterator 00806 it = begin_neighborhood(); 00807 it != end_neighborhood(); 00808 ++it ) 00809 if ( it->second.is_anchor() ) 00810 ++cnt; 00811 00812 return cnt; 00813 } 00814 // ---------------------------------------------------------------------- 00815 template<typename OsModel_P, 00816 typename node_id_t_P, 00817 typename NeighborInfo_P, 00818 typename NeighborInfoMap_P, 00819 typename Arithmatic_P> 00820 int 00821 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00822 confident_neighbor_cnt( void ) 00823 { 00824 int cnt = 0; 00825 00826 for ( NeighborhoodIterator 00827 it = begin_neighborhood(); 00828 it != end_neighborhood(); 00829 ++it ) 00830 if ( it->second->is_confident() ) 00831 ++cnt; 00832 00833 return cnt; 00834 } 00835 // ---------------------------------------------------------------------- 00836 template<typename OsModel_P, 00837 typename node_id_t_P, 00838 typename NeighborInfo_P, 00839 typename NeighborInfoMap_P, 00840 typename Arithmatic_P> 00841 typename LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>::Arithmatic 00842 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00843 avg_neighbor_confidence( void ) 00844 { 00845 Arithmatic conf = 0; 00846 int count = 0; 00847 00848 for ( NeighborhoodIterator 00849 it = begin_neighborhood(); 00850 it != end_neighborhood(); 00851 ++it ) 00852 if ( it->second->is_confident() ) 00853 { 00854 ++count; 00855 conf += it->second->confidence(); 00856 } 00857 00858 return ( conf / count ); 00859 } 00860 // ---------------------------------------------------------------------- 00861 template<typename OsModel_P, 00862 typename node_id_t_P, 00863 typename NeighborInfo_P, 00864 typename NeighborInfoMap_P, 00865 typename Arithmatic_P> 00866 void 00867 LocalizationNeighborhood<OsModel_P, node_id_t_P, NeighborInfo_P, NeighborInfoMap_P, Arithmatic_P>:: 00868 reassign_twins( Arithmatic twin_measure ) 00869 { 00870 // main task of this cycle is to unset all twins in the neighborhood. 00871 // additionally it is checked, whether the neighbor is a twin of the 00872 // source node or not. 00873 for ( NeighborhoodIterator 00874 it = begin_neighborhood(); 00875 it != end_neighborhood(); 00876 ++it ) 00877 // TODO!! 00878 // if ( euclidean_distance( 00879 // source().est_position(), it->second->node().est_position() ) 00880 // <= twin_measure ) 00881 // it->second->set_twin( true ); 00882 // else 00883 // it->second->set_twin( false ); 00884 00885 // this cycle sorts out all neighbors that are twins to each other, so 00886 // that after this check there is only one neighbor on one position. 00887 for ( NeighborhoodIterator 00888 it1 = begin_neighborhood(); 00889 it1 != end_neighborhood(); 00890 ++it1 ) 00891 for ( NeighborhoodIterator 00892 it2 = it1; 00893 it2 != end_neighborhood(); 00894 ++it2 ) 00895 { 00896 if ( it1 == it2 ) 00897 continue; 00898 00899 if ( !it1->second->has_pos() || !it2->second->has_pos() ) 00900 continue; 00901 00902 if ( euclidean_distance( it1->second->pos(), it2->second->pos() ) 00903 <= twin_measure ) 00904 it2->second->set_twin( true ); 00905 } 00906 } 00907 00908 }// namespace wiselib 00909 #endif
