void ncc_tracker_t::track( float ref_frame, float search_frame)
{
Imath::V2f offset = pimpl_->track();
Imath::V2f pos = track_pos( ref_frame);
set_track_pos( pos + offset, search_frame);
set_confidence( pimpl_->confidence());
}
// ----------------------------------------------------------------------
void
LocalizationIterLaterationModule::
work( void )
throw()
{
if ( state_ == il_finished )
return;
// do initial stuff;
// if anchor, send 'init-message' and set state to 'finished';
// if unknown, check whether the node is sound or not. if sound,
// send messages, if not, clear estimated position.
if ( state_ == il_init )
{
if ( owner().is_anchor() )
{
send( new LocalizationIterLaterationMessage( observer().confidence() ) );
state_ = il_finished;
}
else
{
sound_ = neighborhood().is_sound();
if ( sound_ && node().has_est_position() )
{
send( new LocalizationIterLaterationMessage( observer().confidence() ) );
send( new LocalizationIterLaterationSoundMessage() );
}
else
{
set_confidence( 0 );
node_w().clear_est_position();
}
state_ = il_work;
}
}
if ( state_ == il_work )
iter_lateration_step();
}
// ----------------------------------------------------------------------
void
LocalizationIterLaterationModule::
iter_lateration_step( void )
throw()
{
if ( state_ == il_finished || !sound_ )
return;
if ( iteration_cnt_ >= iteration_limit_ )
state_ = il_finished;
++iteration_cnt_;
neighborhood_w().reassign_twins( twin_measure_ * observer().comm_range() );
if ( neighborhood().confident_neighbor_cnt() < min_confident_nbrs_ )
return;
Vec est_pos;
NeighborInfoList neighbors;
collect_neighbors( neighborhood(), lat_confident, neighbors );
// try to update position. if lateration fails, confidence is set to 0,
// else position is updated and confidence is set to average of all
// neighbor confidences
if ( est_pos_lateration( neighbors, est_pos, lat_confident, false ) &&
est_pos_lateration( neighbors, est_pos, lat_confident, true ) )
{
set_confidence( neighborhood().avg_neighbor_confidence() );
// check validity of new position. if check fails, there is a chance
// of 'res_acceptance_', which is normally set to 0.1, to accept
// the position anyway. this is done to avoid being trapped in a
// local minimum. moreover, if the bad position is accepted, the
// confidence is reduced by 50%.
if ( !check_residue( neighbors, est_pos, lat_confident, observer().comm_range() ) )
{
if ( res_acceptance_ > uniform_random_0i_1i() )
set_confidence( observer().confidence() / 2 );
else
{
if ( observer().confidence() == 0 ) return;
set_confidence( 0 );
node_w().clear_est_position();
send( new LocalizationIterLaterationMessage( observer().confidence() ) );
return;
}
}
// check, whether the new position is very close to the old one or
// not. if so, refinement is finished.
if ( node().has_est_position() )
{
double pos_diff = euclidean_distance( est_pos, node().est_position() );
if ( pos_diff < abort_pos_update_ * observer().comm_range() )
state_ = il_finished;
}
node_w().set_est_position( est_pos );
}
else
{
if ( observer().confidence() == 0 ) return;
set_confidence( 0 );
node_w().clear_est_position();
}
send( new LocalizationIterLaterationMessage( observer().confidence() ) );
}
