Changeset 14:124f25d79a76 in rrlib_data_fusion
 Timestamp:
 03.07.2012 13:27:23 (7 years ago)
 Branch:
 default
 Phase:
 public
 Files:

 2 edited
Legend:
 Unmodified
 Added
 Removed

tAverage.h
r12 r14 43 43 // 44 44 #ifdef _LIB_RRLIB_MATH_PRESENT_ 45 #include "rrlib/math/tAngle.h" 45 46 #include "rrlib/math/tPose2D.h" 46 47 #include "rrlib/math/tPose3D.h" … … 118 119 #ifdef _LIB_RRLIB_MATH_PRESENT_ 119 120 121 template <typename TElement, typename TUnitPolicy, typename TSignedPolicy, template <typename> class TChannel> 122 class tAverage<math::tAngle<TElement, TUnitPolicy, TSignedPolicy>, TChannel> : public tDataFusion<math::tAngle<TElement, TUnitPolicy, TSignedPolicy>, TChannel> 123 { 124 125 typedef math::tAngle<TElement, TUnitPolicy, TSignedPolicy> tAngle; 126 127 // 128 // Private fields and methods 129 // 130 private: 131 132 virtual const char *GetLogDescription() const 133 { 134 return "tWeightedAverage<math::tAngle<...>>"; 135 } 136 137 virtual const bool HasValidState() const 138 { 139 return true; 140 } 141 142 virtual const tAngle CalculateFusedValue(const std::vector<TChannel<tAngle>> &channels) 143 { 144 double accumulated_value = 0; 145 for (typename std::vector<TChannel<tAngle>>::const_iterator it = channels.begin(); it != channels.end(); ++it) 146 { 147 accumulated_value += static_cast<double>(it>GetSample()); 148 } 149 return tAngle(accumulated_value / channels.size()); 150 } 151 152 virtual void ResetStateImplementation() 153 {} 154 155 virtual void EnterNextTimestepImplementation() 156 {} 157 158 }; 159 120 160 template <template <typename> class TChannel> 121 161 class tAverage<math::tPose2D, TChannel> : public tDataFusion<math::tPose2D, TChannel> 
tWeightedAverage.h
r12 r14 43 43 // 44 44 #ifdef _LIB_RRLIB_MATH_PRESENT_ 45 #include "rrlib/math/tAngle.h" 45 46 #include "rrlib/math/tPose2D.h" 46 47 #include "rrlib/math/tPose3D.h" … … 120 121 #ifdef _LIB_RRLIB_MATH_PRESENT_ 121 122 123 template <typename TElement, typename TUnitPolicy, typename TSignedPolicy, template <typename> class TChannel> 124 class tWeightedAverage<math::tAngle<TElement, TUnitPolicy, TSignedPolicy>, TChannel> : public tDataFusion<math::tAngle<TElement, TUnitPolicy, TSignedPolicy>, TChannel> 125 { 126 127 typedef math::tAngle<TElement, TUnitPolicy, TSignedPolicy> tAngle; 128 129 // 130 // Private fields and methods 131 // 132 private: 133 134 virtual const char *GetLogDescription() const 135 { 136 return "tWeightedAverage<math::tAngle<...>>"; 137 } 138 139 virtual const bool HasValidState() const 140 { 141 return true; 142 } 143 144 virtual const tAngle CalculateFusedValue(const std::vector<TChannel<tAngle>> &channels) 145 { 146 double accumulated_value = 0; 147 double accumulated_weights = 0; 148 for (typename std::vector<TChannel<tAngle>>::const_iterator it = channels.begin(); it != channels.end(); ++it) 149 { 150 accumulated_value += static_cast<double>(it>GetSample()) * it>GetKey(); 151 accumulated_weights += it>GetKey(); 152 } 153 return tAngle(accumulated_value / accumulated_weights); 154 } 155 156 virtual void ResetStateImplementation() 157 {} 158 159 virtual void EnterNextTimestepImplementation() 160 {} 161 162 }; 163 122 164 template <template <typename> class TChannel> 123 165 class tWeightedAverage<math::tPose2D, TChannel> : public tDataFusion<math::tPose2D, TChannel>
Note: See TracChangeset
for help on using the changeset viewer.