00001 00059 /* 00060 * dof_variance.h 00061 * 00062 * Created on: 27.05.2012 00063 * Author: josh 00064 */ 00065 00066 #ifndef DOF_VARIANCE_H_ 00067 #define DOF_VARIANCE_H_ 00068 00069 #include "euler.h" 00070 #include <ros/time.h> 00071 00073 namespace DOF6 00074 { 00075 00079 template <typename _SOURCE1, typename _SOURCE2> 00080 class DOF6_Source 00081 { 00082 public: 00083 typedef _SOURCE1 SOURCE1; 00084 typedef _SOURCE2 SOURCE2; 00085 typedef typename SOURCE1::TYPE TYPE; 00086 typedef Eigen::Matrix<TYPE,3,1> Vector; 00087 typedef boost::shared_ptr<SOURCE1> Source1_Ptr; 00088 typedef boost::shared_ptr<SOURCE2> Source2_Ptr; 00089 00090 private: 00091 Source1_Ptr src1_; 00092 Source2_Ptr src2_; 00093 00094 public: 00095 00096 DOF6_Source(Source1_Ptr src1, Source2_Ptr src2) 00097 :src1_(src1),src2_(src2) 00098 { 00099 ROS_ASSERT(src1_); 00100 ROS_ASSERT(src2_); 00101 } 00102 00103 DOF6_Source() 00104 :src1_(new SOURCE1),src2_(new SOURCE2) 00105 { 00106 } 00107 00108 void deepCopy(const DOF6_Source &o) 00109 { 00110 src1_->deepCopy(*o.src1_); 00111 src2_->deepCopy(*o.src2_); 00112 } 00113 00114 Source1_Ptr getSource1() {return src1_;} 00115 Source2_Ptr getSource2() {return src2_;} 00116 00117 TYPE getRotationVariance() const 00118 { 00119 const TYPE C1 = src1_->getRotationVariance(); 00120 const TYPE C2 = src2_->getRotationVariance(); 00121 return C1 - C1*C1/(C1+C2); 00122 } 00123 00124 TYPE getTranslationVariance() const 00125 { 00126 const TYPE C1 = src1_->getTranslationVariance(); 00127 const TYPE C2 = src2_->getTranslationVariance(); 00128 return C1 - C1*C1/(C1+C2); 00129 } 00130 00131 EulerAngles<TYPE> getRotation() const 00132 { 00133 const EulerAngles<TYPE> X1 = src1_->getRotation(); 00134 const EulerAngles<TYPE> X2 = src2_->getRotation(); 00135 const TYPE C1 = src1_->getRotationVariance(); 00136 const TYPE C2 = src2_->getRotationVariance(); 00137 return X1 + C1/(C1+C2) * (X2-X1); 00138 } 00139 00140 Vector getTranslation() const 00141 { 00142 const Vector X1 = src1_->getTranslation(); 00143 const Vector X2 = src2_->getTranslation(); 00144 const TYPE C1 = src1_->getRotationVariance(); 00145 const TYPE C2 = src2_->getRotationVariance(); 00146 return X1 + C1/(C1+C2) * (X2-X1); 00147 } 00148 00149 void reset() { 00150 src1_->reset(); 00151 src2_->reset(); 00152 } 00153 00154 void setTime(const double time_in_sec) 00155 { 00156 src1_->setTime(time_in_sec); 00157 src2_->setTime(time_in_sec); 00158 } 00159 00160 }; 00161 00162 } 00163 00164 00165 00166 #endif /* DOF_VARIANCE_H_ */