|
Wiselib
|
wiselib.testing/algorithms/localization/distance_based/modules/distance/localization_euclidean_module.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_LOCALIZATION_EUCLIEDEAN_MODULE_H 00020 #define __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_LOCALIZATION_EUCLIEDEAN_MODULE_H 00021 00022 #include "algorithms/localization/distance_based/modules/localization_module.h" 00023 #include "algorithms/localization/distance_based/modules/distance/localization_euclidean_messages.h" 00024 #include "algorithms/localization/distance_based/util/localization_defutils.h" 00025 #include "algorithms/localization/distance_based/math/localization_triangulation.h" 00026 #include "algorithms/localization/distance_based/math/localization_statistic.h" 00027 #include "config_testing.h" 00028 00029 00030 namespace wiselib 00031 { 00032 00035 const char* EUCL_COL_CHECK_STD[] = { "lax", "strict", "one" }; 00036 const char* EUCL_COL_CHECK_NV[] = { "lax", "strict", "one" }; 00037 const char* EUCL_COL_CHECK_CN[] = { "lax", "strict", "one" }; 00038 const char* EUCL_ALGO[] = { "normal", "opt" }; 00039 const char* EUCL_VOTE[] = { "nv", "cn", "nvcn", "cnnv" }; 00041 00042 00044 00064 template<typename OsModel_P, 00065 typename Radio_P, 00066 typename Clock_P, 00067 typename Distance_P, 00068 typename Debug_P, 00069 typename SharedData_P, 00070 typename DistancePair_P> 00071 class LocalizationEuclideanModule 00072 : public LocalizationModule<OsModel_P, Radio_P, SharedData_P> 00073 { 00074 00075 public: 00076 typedef OsModel_P OsModel; 00077 typedef Radio_P Radio; 00078 typedef Clock_P Clock; 00079 typedef Distance_P Distance; 00080 typedef Debug_P Debug; 00081 typedef SharedData_P SharedData; 00082 typedef DistancePair_P DistancePair; 00083 00084 typedef LocalizationSumDistModule<OsModel, Radio, Clock, Distance, Debug, SharedData> self_type; 00085 typedef LocalizationModule<OsModel, Radio, SharedData> base_type; 00086 00087 typedef typename Radio::size_t size_t; 00088 typedef typename Radio::node_id_t node_id_t; 00089 typedef typename Radio::block_data_t block_data_t; 00090 00091 typedef typename Clock_P::time_t time_t; 00092 00093 typedef typename SharedData::DistanceMap DistanceMap; 00094 typedef typename SharedData::NodeList NodeList; 00095 typedef typename NodeList::iterator NodeListIterator; 00096 00097 typedef LocalizationEuclideanInitMessage<OsModel, Radio> EuclideanInitMessage; 00098 typedef LocalizationEuclideanAnchorMessage<OsModel, Radio> EuclideanAnchorMessage; 00099 typedef LocalizationEuclideanNeighborMessage<OsModel, Radio, DistanceMap> EuclideanNeighborMessage; 00100 00101 typedef LocalizationStatistic<OsModel> Statistics; 00102 00103 typedef typename SharedData::Neighborhood::NeighborhoodIterator NeighborhoodIterator; 00104 00105 enum EuclideanCollinearCheckStd 00106 { 00107 eu_cc_std_lax, 00108 eu_cc_std_strict, 00109 eu_cc_std_none 00110 }; 00111 00112 enum EuclideanCollinearCheckNV 00113 { 00114 eu_cc_nv_lax, 00115 eu_cc_nv_strict, 00116 eu_cc_nv_none 00117 }; 00118 00119 enum EuclideanCollinearCheckCN 00120 { 00121 eu_cc_cn_lax, 00122 eu_cc_cn_strict, 00123 eu_cc_cn_none 00124 }; 00125 00126 enum EuclideanAlgo 00127 { 00128 eu_algo_normal, 00129 eu_algo_opt 00130 }; 00131 00132 enum EuclideanVote 00133 { 00134 eu_vote_nv, 00135 eu_vote_cn, 00136 eu_vote_nvcn, 00137 eu_vote_cnnv 00138 }; 00139 00143 LocalizationEuclideanModule(); 00145 ~LocalizationEuclideanModule(); 00147 00148 00151 00155 void receive( node_id_t from, size_t len, block_data_t *data ); 00161 void work( void ); 00163 00164 00167 00170 bool finished( void ); 00172 00173 void rollback( void ); 00174 00175 00176 void init( Radio& radio, Clock& clock, Debug& debug, SharedData& shared_data, Distance& distance ) { 00177 radio_ = &radio; 00178 clock_ = &clock; 00179 debug_ = &debug; 00180 this->set_shared_data( shared_data ); 00181 distance_ = &distance; 00182 } 00183 00184 protected: 00185 00188 00193 bool process_euclidean_init_message( node_id_t from, size_t len, block_data_t *data ); 00199 void broadcast_neighborhood( void ); 00205 bool process_euclidean_neighbor_message( node_id_t from, size_t len, block_data_t *data ); 00213 bool process_euclidean_anchor_message( node_id_t from, size_t len, block_data_t *data ); 00219 void execute_euclidean( node_id_t anchor ); 00221 00222 00225 00236 double find_anchor_distance( node_id_t anchor ); 00248 double find_anchor_distance_opt( node_id_t anchor ); 00260 NodeList find_unique_neighbor_neighbors( node_id_t anchor, node_id_t n1, node_id_t n2 ); 00272 NodeList find_common_neighbor_neighbors( node_id_t anchor, node_id_t n1, node_id_t n2 ); 00287 NodeList find_common_neighbor_neighbors_opt( node_id_t, node_id_t, node_id_t, double& ); 00319 double neighbor_vote( 00320 node_id_t, node_id_t, node_id_t, 00321 DistancePair&, NodeList& ); 00339 double common_neighbor( 00340 node_id_t, node_id_t, node_id_t, 00341 DistancePair&, NodeList& ); 00343 00344 00347 00350 void set_collinear_check_std( EuclideanCollinearCheckStd check ) 00351 { cc_std_ = check; } 00352 void set_collinear_check_nv( EuclideanCollinearCheckNV check ) 00353 { cc_nv_ = check; } 00354 void set_collinear_check_cn( EuclideanCollinearCheckCN check ) 00355 { cc_cn_ = check; } 00356 void set_euclidean_algo( EuclideanAlgo algo ) 00357 { algo_ = algo; } 00358 void set_euclidean_algo( EuclideanVote vote ) 00359 { vote_ = vote; } 00361 00362 00363 private: 00364 00365 enum MessagesIds 00366 { 00367 EUCLIDEAN_INIT_MESSAGE = 203, 00368 EUCLIDEAN_ANCHOR_MESSAGE = 204, 00369 EUCLIDEAN_NEIGHBOR_MESSAGE = 205 00370 }; 00371 00372 enum EuclideanState 00373 { 00374 eu_init, 00375 eu_wait, 00376 eu_broadcast, 00377 eu_work, 00378 eu_finished 00379 }; 00380 00381 EuclideanState state_; 00382 EuclideanCollinearCheckStd cc_std_; 00383 EuclideanCollinearCheckNV cc_nv_; 00384 EuclideanCollinearCheckCN cc_cn_; 00385 EuclideanAlgo algo_; 00386 EuclideanVote vote_; 00387 00388 time_t last_useful_msg_; 00389 double col_measure_; 00390 00391 Radio* radio_; 00392 Clock* clock_; 00393 Debug* debug_; 00394 Distance* distance_; 00395 }; 00396 // ---------------------------------------------------------------------- 00397 // ---------------------------------------------------------------------- 00398 // ---------------------------------------------------------------------- 00399 template<typename OsModel_P, 00400 typename Radio_P, 00401 typename Clock_P, 00402 typename Distance_P, 00403 typename Debug_P, 00404 typename SharedData_P, 00405 typename DistancePair_P> 00406 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 00407 LocalizationEuclideanModule() 00408 : state_ ( eu_init ), 00409 cc_std_ ( eu_cc_std_lax ), 00410 cc_nv_ ( eu_cc_nv_lax ), 00411 cc_cn_ ( eu_cc_cn_strict ), 00412 algo_ ( eu_algo_opt ), 00413 vote_ ( eu_vote_nvcn ), 00414 last_useful_msg_ ( 0 ), 00415 col_measure_ ( 0 ) 00416 {} 00417 // ---------------------------------------------------------------------- 00418 template<typename OsModel_P, 00419 typename Radio_P, 00420 typename Clock_P, 00421 typename Distance_P, 00422 typename Debug_P, 00423 typename SharedData_P, 00424 typename DistancePair_P> 00425 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 00426 ~LocalizationEuclideanModule() 00427 {} 00428 // ---------------------------------------------------------------------- 00429 template<typename OsModel_P, 00430 typename Radio_P, 00431 typename Clock_P, 00432 typename Distance_P, 00433 typename Debug_P, 00434 typename SharedData_P, 00435 typename DistancePair_P> 00436 void 00437 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 00438 receive( node_id_t from, size_t len, block_data_t *data ) 00439 { 00440 switch ( data[0] ) 00441 { 00442 case EUCLIDEAN_INIT_MESSAGE: 00443 process_euclidean_init_message( from, len, data ); 00444 break; 00445 case EUCLIDEAN_ANCHOR_MESSAGE: 00446 process_euclidean_anchor_message( from, len, data ); 00447 break; 00448 case EUCLIDEAN_NEIGHBOR_MESSAGE: 00449 process_euclidean_neighbor_message( from, len, data ); 00450 break; 00451 } 00452 } 00453 // ---------------------------------------------------------------------- 00454 template<typename OsModel_P, 00455 typename Radio_P, 00456 typename Clock_P, 00457 typename Distance_P, 00458 typename Debug_P, 00459 typename SharedData_P, 00460 typename DistancePair_P> 00461 void 00462 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 00463 work( void ) 00464 { 00465 // send initial messages 00466 if ( state_ == eu_init ) 00467 { 00468 if ( this->shared_data().is_anchor() ) 00469 { 00470 EuclideanInitMessage message; 00471 message.set_msg_id( EUCLIDEAN_INIT_MESSAGE ); 00472 message.set_anchor( true ); 00473 message.set_source_position( this->shared_data().position() ); 00474 radio_->send( Radio::BROADCAST_ADDRESS, 00475 message.buffer_size(), (block_data_t*)&message ); 00476 } 00477 else 00478 { 00479 EuclideanInitMessage message; 00480 message.set_msg_id( EUCLIDEAN_INIT_MESSAGE ); 00481 message.set_anchor( false ); 00482 radio_->send( Radio::BROADCAST_ADDRESS, 00483 message.buffer_size(), (block_data_t*)&message ); 00484 } 00485 00486 state_ = eu_wait; 00487 } 00488 00489 // after idle-time passed, initial messages of neighbors had already been 00490 // received and state is set to 'broadcast'. 00491 if ( state_ == eu_wait && 00492 clock_->time() - last_useful_msg_ > 00493 this->shared_data().idle_time() ) 00494 state_ = eu_broadcast; 00495 00496 // broadcast collected information 00497 if ( state_ == eu_broadcast ) 00498 broadcast_neighborhood(); 00499 } 00500 // ---------------------------------------------------------------------- 00501 template<typename OsModel_P, 00502 typename Radio_P, 00503 typename Clock_P, 00504 typename Distance_P, 00505 typename Debug_P, 00506 typename SharedData_P, 00507 typename DistancePair_P> 00508 bool 00509 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 00510 finished( void ) 00511 { 00512 return (state_ == eu_finished); 00513 } 00514 // ---------------------------------------------------------------------- 00515 template<typename OsModel_P, 00516 typename Radio_P, 00517 typename Clock_P, 00518 typename Distance_P, 00519 typename Debug_P, 00520 typename SharedData_P, 00521 typename DistancePair_P> 00522 void 00523 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 00524 rollback( void ) 00525 { 00526 state_ = eu_init; 00527 last_useful_msg_ = clock_->time(); 00528 col_measure_ = 0; 00529 } 00530 // ---------------------------------------------------------------------- 00531 template<typename OsModel_P, 00532 typename Radio_P, 00533 typename Clock_P, 00534 typename Distance_P, 00535 typename Debug_P, 00536 typename SharedData_P, 00537 typename DistancePair_P> 00538 bool 00539 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 00540 process_euclidean_init_message( node_id_t from, size_t len, block_data_t* data ) 00541 { 00542 EuclideanInitMessage* msg = (EuclideanInitMessage*)data; 00543 Vec source_pos = msg->source_position(); 00544 double distance = distance_->distance( from ); 00545 if ( distance == UNKNOWN_DISTANCE ) 00546 return false; 00547 00548 last_useful_msg_ = clock_->time(); 00549 00550 // add info of received message to own neighborhood 00551 if ( msg->anchor() ) 00552 this->neighborhood().update_anchor( from, source_pos, distance ); 00553 else 00554 this->neighborhood().update_neighbor( from, distance ); 00555 00556 //BugFix: One-hop to anchor 00557 if ( this->neighborhood().valid_anchor_cnt() >= (int)this->shared_data().floodlimit() ) 00558 state_ = eu_finished; 00559 00560 return true; 00561 } 00562 // ---------------------------------------------------------------------- 00563 template<typename OsModel_P, 00564 typename Radio_P, 00565 typename Clock_P, 00566 typename Distance_P, 00567 typename Debug_P, 00568 typename SharedData_P, 00569 typename DistancePair_P> 00570 void 00571 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 00572 broadcast_neighborhood( void ) 00573 { 00574 // send info about own neighborhood 00575 EuclideanNeighborMessage message; 00576 message.set_msg_id( EUCLIDEAN_NEIGHBOR_MESSAGE ); 00577 // FIXME: isn't there a better way of doing this? problem here is 00578 // without having const, and passing dmaps per value, not 00579 // reference, where even copy constructor is called! 00580 DistanceMap dmap = this->neighborhood().neighbor_distance_map(); 00581 message.set_neighbors( dmap ); 00582 radio_->send( Radio::BROADCAST_ADDRESS, message.buffer_size(), (block_data_t*)&message ); 00583 state_ = eu_work; 00584 } 00585 // ---------------------------------------------------------------------- 00586 template<typename OsModel_P, 00587 typename Radio_P, 00588 typename Clock_P, 00589 typename Distance_P, 00590 typename Debug_P, 00591 typename SharedData_P, 00592 typename DistancePair_P> 00593 bool 00594 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 00595 process_euclidean_neighbor_message( node_id_t from, size_t len, block_data_t* data ) 00596 { 00597 EuclideanNeighborMessage* msg = (EuclideanNeighborMessage*)data; 00598 00599 // set neighborhood of received node 00600 // FIXME: isn't there a better way of doing this? problem here is 00601 // without having const, and passing dmaps per value, not 00602 // reference, where even copy constructor is called! 00603 DistanceMap dmap = msg->neighbors(); 00604 this->neighborhood().update_nneighbors( from, dmap ); 00605 00606 // if source is valid anchor, send anchor-message 00607 NeighborhoodIterator it = this->neighborhood().find( from ); 00608 if ( it == this->neighborhood().end_neighborhood() ) 00609 return true; 00610 00611 if ( it->second->is_anchor() && it->second->is_valid() ) 00612 { 00613 EuclideanAnchorMessage message; 00614 message.set_msg_id( EUCLIDEAN_ANCHOR_MESSAGE ); 00615 message.set_anchor( it->second->node() ); 00616 message.set_anchor_position( it->second->pos() ); 00617 message.set_distance( it->second->distance() ); 00618 radio_->send( Radio::BROADCAST_ADDRESS, 00619 message.buffer_size(), (block_data_t*)&message ); 00620 } 00621 00622 return true; 00623 } 00624 // ---------------------------------------------------------------------- 00625 template<typename OsModel_P, 00626 typename Radio_P, 00627 typename Clock_P, 00628 typename Distance_P, 00629 typename Debug_P, 00630 typename SharedData_P, 00631 typename DistancePair_P> 00632 bool 00633 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 00634 process_euclidean_anchor_message( node_id_t from, size_t len, block_data_t* data ) 00635 { 00636 if ( state_ == eu_finished ) 00637 return true; 00638 00639 EuclideanAnchorMessage* msg = (EuclideanAnchorMessage*)data; 00640 00641 node_id_t anchor = msg->anchor(); 00642 Vec anchor_pos = msg->anchor_position(); 00643 00644 // if anchor receives message about another anchor, the real distance 00645 // is calculated and sent as new anchor-message 00646 if ( this->shared_data().is_anchor() ) 00647 { 00648 if ( this->neighborhood().find( anchor ) == this->neighborhood().end_neighborhood() ) 00649 this->neighborhood().update_anchor( anchor, anchor_pos ); 00650 else 00651 return true; 00652 00653 double distance = euclidean_distance( anchor_pos, this->shared_data().position() ); 00654 EuclideanAnchorMessage message; 00655 message.set_msg_id( EUCLIDEAN_ANCHOR_MESSAGE ); 00656 message.set_anchor( anchor ); 00657 message.set_anchor_position( anchor_pos ); 00658 message.set_distance( distance ); 00659 radio_->send( Radio::BROADCAST_ADDRESS, 00660 message.buffer_size(), (block_data_t*)&message ); 00661 00662 return true; 00663 } 00664 00665 this->neighborhood().update_anchor( anchor, anchor_pos ); 00666 this->neighborhood().update_nneighbor( anchor, from, msg->distance() ); 00667 00668 execute_euclidean( anchor ); 00669 00670 return true; 00671 } 00672 // ---------------------------------------------------------------------- 00673 template<typename OsModel_P, 00674 typename Radio_P, 00675 typename Clock_P, 00676 typename Distance_P, 00677 typename Debug_P, 00678 typename SharedData_P, 00679 typename DistancePair_P> 00680 void 00681 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 00682 execute_euclidean( node_id_t anchor ) 00683 { 00684 NeighborhoodIterator it = this->neighborhood().find( anchor ); 00685 //In case that we have not yet estimated a distance to this anchor 00686 if ( !it->second->is_valid() ) 00687 { 00688 double distance = -1; 00689 00690 switch ( algo_ ) 00691 { 00692 case eu_algo_normal: 00693 distance = find_anchor_distance( it->second->node() ); 00694 break; 00695 00696 case eu_algo_opt: 00697 distance = find_anchor_distance_opt( it->second->node() ); 00698 break; 00699 } 00700 00701 if ( distance == -1 ) 00702 { 00703 #ifdef LOCALIZATION_DISTANCEBASED_EUCLIDEAN_DEBUG 00704 debug_->debug( "Distance to anchor %d is -1\n", anchor ); 00705 #endif 00706 return; 00707 } 00708 else 00709 { 00710 #ifdef LOCALIZATION_DISTANCEBASED_EUCLIDEAN_DEBUG 00711 debug_->debug( "Distance to anchor %d is %f\n", anchor, distance ); 00712 #endif 00713 } 00714 00715 it->second->set_distance( distance ); 00716 EuclideanAnchorMessage message; 00717 message.set_msg_id( EUCLIDEAN_ANCHOR_MESSAGE ); 00718 message.set_anchor( it->second->node() ); 00719 message.set_anchor_position( it->second->pos() ); 00720 message.set_distance( it->second->distance() ); 00721 radio_->send( Radio::BROADCAST_ADDRESS, 00722 message.buffer_size(), (block_data_t*)&message ); 00723 00724 // if floodlimit reached, finished 00725 if ( this->neighborhood().valid_anchor_cnt() >= (int)this->shared_data().floodlimit() ) 00726 state_ = eu_finished; 00727 00728 } 00729 } 00730 // ---------------------------------------------------------------------- 00731 template<typename OsModel_P, 00732 typename Radio_P, 00733 typename Clock_P, 00734 typename Distance_P, 00735 typename Debug_P, 00736 typename SharedData_P, 00737 typename DistancePair_P> 00738 double 00739 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 00740 find_anchor_distance( node_id_t anchor ) 00741 { 00742 double distance = -1; 00743 bool leave = false; 00744 00745 for ( NeighborhoodIterator it1 = this->neighborhood().begin_neighborhood(); it1 != this->neighborhood().end_neighborhood(); ++it1 ) 00746 { 00747 for ( NeighborhoodIterator it2 = it1; it2 != this->neighborhood().end_neighborhood(); ++it2 ) 00748 { 00749 if ( it1 == it2 ) 00750 continue; 00751 00752 // check, that all needed distances exist 00753 if ( !this->neighborhood().has_valid_neighbor( it1->first ) || 00754 !this->neighborhood().has_valid_neighbor( it2->first ) || 00755 !this->neighborhood().has_valid_nneighbor( it1->first, it2->first ) || 00756 !this->neighborhood().has_valid_nneighbor( it1->first, anchor ) || 00757 !this->neighborhood().has_valid_nneighbor( it2->first, anchor ) ) 00758 continue; 00759 00760 double self_n1 = this->neighborhood().neighbor_distance( it1->first ); 00761 double self_n2 = this->neighborhood().neighbor_distance( it2->first ); 00762 double n1_n2 = this->neighborhood().nneighbor_distance( it1->first, it2->first ); 00763 double n1_anchor = this->neighborhood().nneighbor_distance( it1->first, anchor ); 00764 double n2_anchor = this->neighborhood().nneighbor_distance( it2->first, anchor ); 00765 00766 // check collinearity 00767 switch ( cc_std_ ) 00768 { 00769 case eu_cc_std_strict: 00770 if ( is_collinear( self_n1, self_n2, n1_n2, col_measure_ ) || 00771 is_collinear( n1_anchor, n2_anchor, n1_n2, col_measure_ ) ) 00772 continue; 00773 break; 00774 00775 case eu_cc_std_lax: 00776 if ( is_collinear( self_n1, self_n2, n1_n2, col_measure_ ) && 00777 is_collinear( n1_anchor, n2_anchor, n1_n2, col_measure_ ) ) 00778 continue; 00779 break; 00780 00781 case eu_cc_std_none: 00782 ; 00783 } 00784 00785 DistancePair dp = trilateration_distance<OsModel, DistancePair>( self_n1, self_n2, n1_n2, n1_anchor, n2_anchor ); 00786 if ( dp.first == -1 ) 00787 continue; 00788 00789 NodeList nl_nv = find_unique_neighbor_neighbors( anchor, it1->first, it2->first ); 00790 NodeList nl_cn = find_common_neighbor_neighbors( anchor, it1->first, it2->first ); 00791 double dist_nv = neighbor_vote( anchor, it1->first, it2->first, dp, nl_nv ); 00792 double dist_cn = common_neighbor( anchor, it1->first, it2->first, dp, nl_cn ); 00793 00794 if ( dist_nv == -1 && dist_cn == -1 ) 00795 continue; 00796 00797 switch ( vote_) 00798 { 00799 case eu_vote_nv: 00800 { 00801 if ( dist_nv != -1 ) distance = dist_nv; 00802 else continue; 00803 00804 break; 00805 } 00806 case eu_vote_cn: 00807 { 00808 if ( dist_cn != -1 ) distance = dist_cn; 00809 else continue; 00810 00811 break; 00812 } 00813 case eu_vote_nvcn: 00814 { 00815 if ( dist_cn != -1 ) distance = dist_cn; 00816 if ( dist_nv != -1 ) distance = dist_nv; 00817 else continue; 00818 00819 break; 00820 } 00821 case eu_vote_cnnv: 00822 { 00823 if ( dist_nv != -1 ) distance = dist_nv; 00824 if ( dist_cn != -1 ) distance = dist_cn; 00825 else continue; 00826 00827 break; 00828 } 00829 }// switch vote_ 00830 00831 leave = true; 00832 break; 00833 }// for it2 00834 00835 if ( leave ) break; 00836 }// for it1 00837 00838 return distance; 00839 } 00840 // ---------------------------------------------------------------------- 00841 template<typename OsModel_P, 00842 typename Radio_P, 00843 typename Clock_P, 00844 typename Distance_P, 00845 typename Debug_P, 00846 typename SharedData_P, 00847 typename DistancePair_P> 00848 double 00849 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 00850 find_anchor_distance_opt( node_id_t anchor ) 00851 { 00852 double distance = -1; 00853 double best_nv = -1; 00854 double best_cn = -1; 00855 double max_measure_nv = DBL_MIN; 00856 double max_measure_cn = DBL_MIN; 00857 00858 for ( NeighborhoodIterator 00859 it1 = this->neighborhood().begin_neighborhood(); 00860 it1 != this->neighborhood().end_neighborhood(); 00861 ++it1 ) 00862 { 00863 for ( NeighborhoodIterator 00864 it2 = it1; 00865 it2 != this->neighborhood().end_neighborhood(); 00866 ++it2 ) 00867 { 00868 if ( it1 == it2 ) 00869 continue; 00870 00871 // check, that all needed distance estimations to neighboring nodes exist 00872 if ( !this->neighborhood().has_valid_neighbor( it1->first ) || 00873 !this->neighborhood().has_valid_neighbor( it2->first ) || 00874 !this->neighborhood().has_valid_nneighbor( it1->first, it2->first ) || 00875 !this->neighborhood().has_valid_nneighbor( it1->first, anchor ) || 00876 !this->neighborhood().has_valid_nneighbor( it2->first, anchor ) ) 00877 continue; 00878 00879 double self_n1 = this->neighborhood().neighbor_distance( it1->first ); 00880 double self_n2 = this->neighborhood().neighbor_distance( it2->first ); 00881 double n1_anchor = this->neighborhood().nneighbor_distance( it1->first, anchor ); 00882 double n2_anchor = this->neighborhood().nneighbor_distance( it2->first, anchor ); 00883 double n1_n2 = this->neighborhood().nneighbor_distance( it1->first, it2->first ); 00884 00885 // check collinearity 00886 switch ( cc_std_ ) 00887 { 00888 case eu_cc_std_strict: 00889 if ( is_collinear( self_n1, self_n2, n1_n2, col_measure_ ) || 00890 is_collinear( n1_anchor, n2_anchor, n1_n2, col_measure_ ) ) 00891 continue; 00892 break; 00893 00894 case eu_cc_std_lax: 00895 if ( is_collinear( self_n1, self_n2, n1_n2, col_measure_ ) && 00896 is_collinear( n1_anchor, n2_anchor, n1_n2, col_measure_ ) ) 00897 continue; 00898 break; 00899 00900 case eu_cc_std_none: 00901 ; 00902 } 00903 00904 DistancePair dp = trilateration_distance<OsModel, DistancePair>( self_n1, self_n2, n1_n2, n1_anchor, n2_anchor ); 00905 00906 if ( dp.first > -1.00000000001 && dp.first < -0.999999999999 ) 00907 { 00908 continue; 00909 } 00910 00911 double measure; 00912 NodeList nl = find_common_neighbor_neighbors_opt( anchor, it1->first, it2->first, measure ); 00913 00914 double dist_nv = neighbor_vote( anchor, it1->first, it2->first, dp, nl ); 00915 double dist_cn = common_neighbor( anchor, it1->first, it2->first, dp, nl ); 00916 00917 if ( dist_cn > -1.000000001 && dist_cn < -0.9999999999 && max_measure_cn < measure && measure > 0 ) 00918 { 00919 max_measure_cn = measure; 00920 best_cn = dist_cn; 00921 } 00922 00923 if ( dist_nv > -1.000000001 && dist_nv < -0.9999999999 && max_measure_nv < measure && measure > 0 ) 00924 { 00925 max_measure_nv = measure; 00926 best_nv = dist_nv; 00927 } 00928 }// for it2 00929 }// for it1 00930 00931 switch ( vote_) 00932 { 00933 case eu_vote_nv: 00934 { 00935 if ( best_nv > -1.000000001 && best_nv < -0.9999999999 ) distance = best_nv; 00936 break; 00937 } 00938 case eu_vote_cn: 00939 { 00940 if ( best_cn > -1.000000001 && best_cn < -0.9999999999 ) distance = best_cn; 00941 break; 00942 } 00943 case eu_vote_nvcn: 00944 { 00945 if ( best_cn > -1.000000001 && best_cn < -0.9999999999 ) distance = best_cn; 00946 if (best_nv > -1.000000001 && best_nv < -0.9999999999 ) distance = best_nv; 00947 break; 00948 } 00949 case eu_vote_cnnv: 00950 { 00951 if ( best_nv > -1.0000000001 && best_nv < -0.9999999999 -1 ) distance = best_nv; 00952 if ( best_cn > -1.0000000001 && best_cn < -0.9999999999 ) distance = best_cn; 00953 break; 00954 } 00955 }// switch vote_ 00956 00957 return distance; 00958 } 00959 // ---------------------------------------------------------------------- 00960 template<typename OsModel_P, 00961 typename Radio_P, 00962 typename Clock_P, 00963 typename Distance_P, 00964 typename Debug_P, 00965 typename SharedData_P, 00966 typename DistancePair_P> 00967 typename LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>::NodeList 00968 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 00969 find_unique_neighbor_neighbors( node_id_t anchor, node_id_t n1, node_id_t n2 ) 00970 { 00971 NodeList temp; 00972 00973 for ( NeighborhoodIterator 00974 it = this->neighborhood().begin_neighborhood(); 00975 it != this->neighborhood().end_neighborhood(); 00976 ++it ) 00977 { 00978 // node is not n1 or n2, and connected to self and anchor 00979 if ( it->first == n1 || it->first == n2 || it->first == anchor || 00980 !this->neighborhood().has_valid_neighbor( it->first ) || 00981 !this->neighborhood().has_valid_nneighbor( it->first, anchor ) ) 00982 continue; 00983 00984 // node is connected to n1 and n2 00985 if ( !this->neighborhood().has_valid_nneighbor( n1, it->first ) && 00986 !this->neighborhood().has_valid_nneighbor( n2, it->first ) ) 00987 continue; 00988 00989 temp.push_back( it->first ); 00990 } 00991 00992 return temp; 00993 } 00994 // ---------------------------------------------------------------------- 00995 template<typename OsModel_P, 00996 typename Radio_P, 00997 typename Clock_P, 00998 typename Distance_P, 00999 typename Debug_P, 01000 typename SharedData_P, 01001 typename DistancePair_P> 01002 typename LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>::NodeList 01003 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 01004 find_common_neighbor_neighbors( node_id_t anchor, node_id_t n1, node_id_t n2 ) 01005 { 01006 NodeList temp; 01007 01008 for ( NeighborhoodIterator 01009 it = this->neighborhood().begin_neighborhood(); 01010 it != this->neighborhood().end_neighborhood(); 01011 ++it ) 01012 { 01013 // node is not n1 or n2, and connected to self and anchor 01014 if ( it->first == n1 || it->first == n2 || it->first == anchor || 01015 !this->neighborhood().has_valid_neighbor( it->first ) || 01016 !this->neighborhood().has_valid_nneighbor( it->first, anchor ) ) 01017 continue; 01018 01019 // node is connected to n1 and n2 01020 if ( !this->neighborhood().has_valid_nneighbor( n1, it->first ) || 01021 !this->neighborhood().has_valid_nneighbor( n2, it->first ) ) 01022 continue; 01023 01024 temp.push_back( it->first ); 01025 } 01026 01027 return temp; 01028 } 01029 // ---------------------------------------------------------------------- 01030 template<typename OsModel_P, 01031 typename Radio_P, 01032 typename Clock_P, 01033 typename Distance_P, 01034 typename Debug_P, 01035 typename SharedData_P, 01036 typename DistancePair_P> 01037 typename LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>::NodeList 01038 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 01039 find_common_neighbor_neighbors_opt( node_id_t anchor, node_id_t n1,node_id_t n2, double& col_measure ) 01040 { 01041 NodeList temp; 01042 col_measure = DBL_MIN; 01043 01044 for ( NeighborhoodIterator 01045 it = this->neighborhood().begin_neighborhood(); 01046 it != this->neighborhood().end_neighborhood(); 01047 ++it ) 01048 { 01049 // node is not n1 or n2, and connected to self and anchor 01050 if ( it->first == n1 || it->first == n2 || it->first == anchor || 01051 !this->neighborhood().has_valid_neighbor( it->first ) || 01052 !this->neighborhood().has_valid_nneighbor( it->first, anchor ) ) 01053 continue; 01054 01055 // node is connected to n1 and n2 01056 if ( !this->neighborhood().has_valid_nneighbor( n1, it->first ) || 01057 !this->neighborhood().has_valid_nneighbor( n2, it->first ) ) 01058 continue; 01059 01060 double n1_n2 = this->neighborhood().nneighbor_distance( n1, n2 ); 01061 double n1_n3 = this->neighborhood().nneighbor_distance( n1, it->first ); 01062 double n2_n3 = this->neighborhood().nneighbor_distance( n2, it->first ); 01063 01064 double tmp_measure = collinear_measure( n1_n2, n1_n3, n2_n3 ); 01065 01066 if ( tmp_measure > col_measure ) 01067 { 01068 col_measure = tmp_measure; 01069 01070 temp.clear(); 01071 temp.push_back( it->first ); 01072 } 01073 } 01074 01075 return temp; 01076 } 01077 // ---------------------------------------------------------------------- 01078 template<typename OsModel_P, 01079 typename Radio_P, 01080 typename Clock_P, 01081 typename Distance_P, 01082 typename Debug_P, 01083 typename SharedData_P, 01084 typename DistancePair_P> 01085 double 01086 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 01087 neighbor_vote( node_id_t anchor, node_id_t n1, node_id_t n2, DistancePair& dp1, NodeList& nl ) 01088 { 01089 if ( nl.empty() ) 01090 { 01091 return -1; 01092 } 01093 01094 // temporary workaround. update to 2D/3D should follow soon, so euclidean 01095 // will exclusively work with NodeLists. 01096 NodeList nl_refs; 01097 nl_refs.push_back( n1 ); 01098 nl_refs.push_back( n2 ); 01099 01100 Statistics stat1, stat2; 01101 stat1 += dp1.first; 01102 stat2 += dp1.second; 01103 01104 for ( NodeListIterator it = nl.begin(); it != nl.end(); ++it ) 01105 { 01106 node_id_t n3 = *it; 01107 01108 for ( NodeListIterator 01109 it_refs = nl_refs.begin(); 01110 it_refs != nl_refs.end(); 01111 ++it_refs ) 01112 { 01113 01114 node_id_t n1_2 = *it_refs; 01115 if ( !this->neighborhood().has_nneighbor( n1_2, n3 ) ) 01116 continue; 01117 01118 double self_n12 = this->neighborhood().neighbor_distance( n1_2 ); 01119 double self_n3 = this->neighborhood().neighbor_distance( n3 ); 01120 double n12_n3 = this->neighborhood().nneighbor_distance( n1_2, n3 ); 01121 double n12_anchor = this->neighborhood().nneighbor_distance( n1_2, anchor ); 01122 double n3_anchor = this->neighborhood().nneighbor_distance( n3, anchor ); 01123 01124 // check collinearity 01125 switch ( cc_nv_ ) 01126 { 01127 case eu_cc_nv_strict: 01128 if ( is_collinear( self_n12, self_n3, n12_n3, col_measure_ ) || 01129 is_collinear( n12_n3, n12_anchor, n3_anchor, col_measure_ ) ) 01130 continue; 01131 break; 01132 01133 case eu_cc_nv_lax: 01134 if ( is_collinear( self_n12, self_n3, n12_n3, col_measure_ ) && 01135 is_collinear( n12_n3, n12_anchor, n3_anchor, col_measure_ ) ) 01136 continue; 01137 break; 01138 01139 case eu_cc_nv_none: 01140 ; 01141 } 01142 01143 DistancePair dp2 = trilateration_distance<OsModel, DistancePair>( self_n12, self_n3, n12_n3, n12_anchor, n3_anchor ); 01144 if ( dp2.first > -1.00000000001 && dp2.first < -0.999999999999) 01145 continue; 01146 01147 enum ChooseDist { d11, d12, d21, d22 } ch_dist = d11; 01148 double dev_sel = fabs( stat1.mean() - dp2.first ); 01149 01150 if ( fabs( stat1.mean() - dp2.second ) < dev_sel ) 01151 { 01152 dev_sel = fabs( stat1.mean() - dp2.second ); 01153 ch_dist = d12; 01154 } 01155 if ( fabs( stat2.mean() - dp2.first ) < dev_sel ) 01156 { 01157 dev_sel = fabs( stat2.mean() - dp2.first ); 01158 ch_dist = d21; 01159 } 01160 if ( fabs( stat2.mean() - dp2.second ) < dev_sel ) 01161 { 01162 dev_sel = fabs( stat2.mean() - dp2.second ); 01163 ch_dist = d22; 01164 } 01165 01166 switch ( ch_dist ) 01167 { 01168 case d11: 01169 case d22: 01170 { 01171 stat1 += dp2.first; 01172 stat2 += dp2.second; 01173 continue; 01174 } 01175 case d12: 01176 case d21: 01177 { 01178 stat1 += dp2.second; 01179 stat2 += dp2.first; 01180 continue; 01181 } 01182 }// switch 01183 }// for NodeList refs 01184 }// for NodeList nl 01185 01186 #ifdef LOCALIZATION_DISTANCEBASED_EUCLIDEAN_DEBUG 01187 // double real = euclidean_distance( anchor.real_position(), node().real_position() ); 01188 // DEBUG( owner().logger(), 01189 // "Stats: " 01190 // << std::endl 01191 // << stat1 << ";;; " << stat2 01192 // << std::endl 01193 // << "Real: " << real << ": " 01194 // << std::endl 01195 // << stat1.mean() << "; " << stat1.std_dev() << " <= " << stat1.mean()*0.05 01196 // << " | " 01197 // << stat2.mean() << "; " << stat2.std_dev() << " <= " << stat2.mean()*0.05 01198 // << std::endl ); 01199 #endif 01200 01201 if ( stat1.std_dev() > stat1.mean()*0.05 && stat2.std_dev() > stat2.mean()*0.05 ) 01202 return -1; 01203 01204 if ( stat1.std_dev()*3 < stat2.std_dev() ) 01205 return stat1.mean(); 01206 else if ( stat2.std_dev()*3 < stat1.std_dev() ) 01207 return stat2.mean(); 01208 01209 return -1; 01210 } 01211 // ---------------------------------------------------------------------- 01212 template<typename OsModel_P, 01213 typename Radio_P, 01214 typename Clock_P, 01215 typename Distance_P, 01216 typename Debug_P, 01217 typename SharedData_P, 01218 typename DistancePair_P> 01219 double 01220 LocalizationEuclideanModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P, DistancePair_P>:: 01221 common_neighbor( node_id_t anchor, node_id_t n1, node_id_t n2, DistancePair& dp, NodeList& nl ) 01222 { 01223 if ( nl.empty() ) return -1; 01224 01225 for ( NodeListIterator 01226 it = nl.begin(); 01227 it != nl.end(); 01228 ++it ) 01229 { 01230 node_id_t n3 = *it; 01231 01232 double self_n1 = this->neighborhood().neighbor_distance( n1 ); 01233 double self_n2 = this->neighborhood().neighbor_distance( n2 ); 01234 double self_n3 = this->neighborhood().neighbor_distance( n3 ); 01235 double n1_n2, n1_anchor, n2_anchor, n3_n1, n3_n2; 01236 if ( this->neighborhood().has_valid_nneighbor( n1, n2 ) && 01237 this->neighborhood().has_valid_nneighbor( anchor, n1 ) && 01238 this->neighborhood().has_valid_nneighbor( anchor, n2 ) && 01239 this->neighborhood().has_valid_nneighbor( n3, n1 ) && 01240 this->neighborhood().has_valid_nneighbor( n3, n2 ) ) 01241 { 01242 n1_n2 = this->neighborhood().nneighbor_distance( n1, n2 ); 01243 n1_anchor = this->neighborhood().nneighbor_distance( anchor, n1 ); 01244 n2_anchor = this->neighborhood().nneighbor_distance( anchor, n2 ); 01245 n3_n1 = this->neighborhood().nneighbor_distance( n3, n1 ); 01246 n3_n2 = this->neighborhood().nneighbor_distance( n3, n2 ); 01247 } 01248 else 01249 continue; 01250 01251 // check collinearity 01252 switch ( cc_cn_ ) 01253 { 01254 case eu_cc_cn_strict: 01255 if ( is_collinear( self_n1, self_n2, n1_n2, col_measure_ ) || 01256 is_collinear( n1_n2, n3_n1, n3_n2, col_measure_ ) || 01257 is_collinear( n1_n2, n1_anchor, n2_anchor, col_measure_ ) ) 01258 continue; 01259 break; 01260 01261 case eu_cc_cn_lax: 01262 if ( is_collinear( self_n1, self_n2, n1_n2, col_measure_ ) && 01263 is_collinear( n1_n2, n3_n1, n3_n2, col_measure_ ) && 01264 is_collinear( n1_n2, n1_anchor, n2_anchor, col_measure_ ) ) 01265 continue; 01266 break; 01267 01268 case eu_cc_cn_none: 01269 ; 01270 } 01271 01272 // distancepair self-n3 01273 DistancePair dp_sn3 = trilateration_distance<OsModel, DistancePair>( 01274 self_n1, self_n2, n1_n2, n3_n1, n3_n2 ); 01275 if ( dp_sn3.first == -1 ) 01276 continue; 01277 01278 int side_n3 = 0; 01279 if ( fabs( self_n3 - dp_sn3.first ) < fabs( self_n3 - dp_sn3.second ) ) 01280 side_n3 = 1; 01281 else 01282 side_n3 = -1; 01283 01284 // distancepair n3-anchor 01285 DistancePair dp_n3a = trilateration_distance<OsModel, DistancePair>( 01286 n3_n1, n3_n2, n1_n2, n1_anchor, n2_anchor ); 01287 if ( dp_n3a.first == -1 ) 01288 continue; 01289 01290 int side_a = 0; 01291 double n3_a = this->neighborhood().find( n3 )->second->neighbor_distance( anchor ); 01292 if ( fabs( n3_a - dp_n3a.first ) < fabs( n3_a - dp_n3a.second ) ) 01293 side_a = 1; 01294 else 01295 side_a = -1; 01296 01297 #ifdef LOCALIZATION_DISTANCEBASED_EUCLIDEAN_DEBUG 01298 // double real_dist = euclidean_distance( node().real_position(), anchor.real_position() ); 01299 // DEBUG( owner().logger(), 01300 // real_dist << ": " << dp.first << "; " << dp.second 01301 // << " :: " << side_n3 * side_a 01302 // << std::endl 01303 // << self_n3 << ": " << dp_sn3.first << "; " << dp_sn3.second 01304 // << std::endl 01305 // << n3_a << ": " << dp_n3a.first << "; " << dp_n3a.second 01306 // << std::endl ); 01307 #endif 01308 01309 if ( side_n3 == 1 && side_a == 1 ) 01310 return dp.first; 01311 else if ( side_n3 * side_a == -1 || ( side_n3 == -1 && side_a == -1 ) ) 01312 return dp.second; 01313 }// for 01314 01315 return -1; 01316 } 01317 01318 }// namespace wiselib 01319 #endif
