Changeset 35:708c45e8708d in rrlib_data_fusion


Ignore:
Timestamp:
11.05.2020 13:59:45 (12 months ago)
Author:
Tobias Föhst <foehst@…>
Branch:
14.08
Children:
36:f7bf0a06d5dc, 37:b821255060a6
Phase:
public
Message:

Fixes weighted average fusion

File:
1 edited

Legend:

Unmodified
Added
Removed
  • tWeightedAverage.h

    r32 r35  
    104104    double accumulated_weights = 0; 
    105105 
    106     std::function<double(const TChannel<TSample> &)> weight_function = [](const TChannel<TSample> &channel) 
    107     { 
    108       return 1.0; 
    109     }; 
    110106    for (auto it = channels.begin(); it != channels.end(); ++it) 
    111107    { 
    112       if (it->GetKey() != 0.0) 
    113       { 
    114         weight_function = [](const TChannel<TSample> &channel) 
    115         { 
    116           return channel.GetKey(); 
    117         }; 
    118         break; 
    119       } 
    120     } 
    121  
    122     for (auto it = channels.begin(); it != channels.end(); ++it) 
    123     { 
    124       *accumulated += it->GetSample() * weight_function(*it); 
    125       accumulated_weights += weight_function(*it); 
    126     } 
    127     return *accumulated * (1.0 / accumulated_weights); 
     108      *accumulated += it->GetSample() * it->GetKey(); 
     109      accumulated_weights += it->GetKey(); 
     110    } 
     111    return accumulated_weights ? *accumulated * (1.0 / accumulated_weights) : 0; 
    128112  } 
    129113 
     
    164148    double accumulated_weights = 0; 
    165149 
    166     std::function<double(const TChannel<tAngle> &)> weight_function = [](const TChannel<tAngle> &channel) 
    167     { 
    168       return 1.0; 
    169     }; 
    170     for (auto it = channels.begin(); it != channels.end(); ++it) 
    171     { 
    172       if (it->GetKey() != 0.0) 
    173       { 
    174         weight_function = [](const TChannel<tAngle> &channel) 
    175         { 
    176           return channel.GetKey(); 
    177         }; 
    178         break; 
    179       } 
    180     } 
    181  
    182150    for (typename std::vector<TChannel<tAngle>>::const_iterator it = channels.begin(); it != channels.end(); ++it) 
    183151    { 
    184       accumulated_value += static_cast<double>(it->GetSample()) * weight_function(*it); 
    185       accumulated_weights += weight_function(*it); 
    186     } 
    187     return tAngle(accumulated_value / accumulated_weights); 
     152      accumulated_value += static_cast<double>(it->GetSample()) * it->GetKey(); 
     153      accumulated_weights += it->GetKey(); 
     154    } 
     155    return tAngle(accumulated_weights ? accumulated_value / accumulated_weights : 0); 
    188156  } 
    189157 
     
    221189    double accumulated_weights = 0; 
    222190 
    223     std::function<double(const TChannel<math::tPose2D> &)> weight_function = [](const TChannel<math::tPose2D> &channel) 
    224     { 
    225       return 1.0; 
    226     }; 
    227     for (auto it = channels.begin(); it != channels.end(); ++it) 
    228     { 
    229       if (it->GetKey() != 0.0) 
    230       { 
    231         weight_function = [](const TChannel<math::tPose2D> &channel) 
    232         { 
    233           return channel.GetKey(); 
    234         }; 
    235         break; 
    236       } 
    237     } 
    238  
    239191    for (typename std::vector<TChannel<math::tPose2D>>::const_iterator it = channels.begin(); it != channels.end(); ++it) 
    240192    { 
    241       accumulated_position += it->GetSample().Position() * weight_function(*it); 
    242       accumulated_yaw += it->GetSample().Yaw() * weight_function(*it); 
    243       accumulated_weights += weight_function(*it); 
    244     } 
    245     double factor = 1.0 / accumulated_weights; 
     193      accumulated_position += it->GetSample().Position() * it->GetKey(); 
     194      accumulated_yaw += it->GetSample().Yaw() * it->GetKey(); 
     195      accumulated_weights += it->GetKey(); 
     196    } 
     197    double factor = accumulated_weights ? 1.0 / accumulated_weights : 0; 
    246198    return math::tPose2D(accumulated_position * factor, math::tAngleRad(accumulated_yaw * factor)); 
    247199  } 
     
    282234    double accumulated_weights = 0; 
    283235 
    284     std::function<double(const TChannel<math::tPose3D> &)> weight_function = [](const TChannel<math::tPose3D> &channel) 
    285     { 
    286       return 1.0; 
    287     }; 
    288     for (auto it = channels.begin(); it != channels.end(); ++it) 
    289     { 
    290       if (it->GetKey() != 0.0) 
    291       { 
    292         weight_function = [](const TChannel<math::tPose3D> &channel) 
    293         { 
    294           return channel.GetKey(); 
    295         }; 
    296         break; 
    297       } 
    298     } 
    299  
    300236    for (typename std::vector<TChannel<math::tPose3D>>::const_iterator it = channels.begin(); it != channels.end(); ++it) 
    301237    { 
    302       accumulated_position += it->GetSample().Position() * weight_function(*it); 
    303       accumulated_roll += it->GetSample().Roll() * weight_function(*it); 
    304       accumulated_pitch += it->GetSample().Pitch() * weight_function(*it); 
    305       accumulated_yaw += it->GetSample().Yaw() * weight_function(*it); 
    306       accumulated_weights += weight_function(*it); 
    307     } 
    308     double factor = 1.0 / accumulated_weights; 
     238      accumulated_position += it->GetSample().Position() * it->GetKey(); 
     239      accumulated_roll += it->GetSample().Roll() * it->GetKey(); 
     240      accumulated_pitch += it->GetSample().Pitch() * it->GetKey(); 
     241      accumulated_yaw += it->GetSample().Yaw() * it->GetKey(); 
     242      accumulated_weights += it->GetKey(); 
     243    } 
     244    double factor = accumulated_weights ? 1.0 / accumulated_weights : 0; 
    309245    return math::tPose3D(accumulated_position * factor, math::tAngleRad(accumulated_roll * factor), math::tAngleRad(accumulated_pitch * factor), math::tAngleRad(accumulated_yaw * factor)); 
    310246  } 
Note: See TracChangeset for help on using the changeset viewer.