[Toon-members] tag/tag constantvelocity.h kalmanfilter.h measu...

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Gerhard Reitmayr <gerhard>      06/06/05 15:19:30

Modified files:
        tag            : constantvelocity.h kalmanfilter.h 
                         measurements.h 

Log message:
        various fixes to make kalman filter stuff actually compile :)

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/tag/constantvelocity.h?cvsroot=toon&r1=1.1&r2=1.2
http://cvs.savannah.gnu.org/viewcvs/tag/tag/kalmanfilter.h?cvsroot=toon&r1=1.3&r2=1.4
http://cvs.savannah.gnu.org/viewcvs/tag/tag/measurements.h?cvsroot=toon&r1=1.2&r2=1.3

Patches:
Index: constantvelocity.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/constantvelocity.h,v
retrieving revision 1.1
retrieving revision 1.2
diff -u -b -r1.1 -r1.2
--- constantvelocity.h  31 May 2006 16:46:20 -0000      1.1
+++ constantvelocity.h  5 Jun 2006 15:19:30 -0000       1.2
@@ -22,27 +22,28 @@
     }
 
     inline void reset(void){
-        position = SE3();
-        Zero(angularVelocity);
-        Zero(velocity);
-        Identity(covariance);
+        pose = TooN::SE3();
+        TooN::Zero(angularVelocity);
+        TooN::Zero(velocity);
+        TooN::Identity(covariance);
     }
 
     inline void resetVelocity(void){
-        Zero(angularVelocity);
-        Zero(velocity);
+        TooN::Zero(angularVelocity);
+        TooN::Zero(velocity);
     }
 
     static const int STATE_DIMENSION = 12;
-    SE3 pose;
-    Vector<3> angularVelocity;
-    Vector<3> velocity;
-    Matrix<STATE_DIMENSION> covariance;
+    TooN::SE3 pose;
+    TooN::Vector<3> angularVelocity;
+    TooN::Vector<3> velocity;
+    TooN::Matrix<STATE_DIMENSION> covariance;
 };
 
 /// operator to print out instances of State in a useable manner.
 /// @ingroup constantvelocitygroup
-inline std::ostream & operator<< (std::ostream & os , const State & st){
+template <class O>
+inline O & operator<< (O & os , const State & st){
     os << st.pose.ln() << st.velocity << st.angularVelocity << 
st.pose.inverse().get_translation();
     return os;
 }
@@ -52,40 +53,40 @@
 /// @ingroup constantvelocitygroup
 class Model {
 public:
-    Vector<State::STATE_DIMENSION> sigma;
+    TooN::Vector<State::STATE_DIMENSION> sigma;
     // dampening of velocity
     double damp;
 
     Model(void){
-        Zero(sigma);
+        TooN::Zero(sigma);
         damp = 1;
     }
 
     // Jacobian has pos, rot, vel, angularVel in this order
-    Matrix<State::STATE_DIMENSION> getJacobian(const State & state, double dt){
-            Matrix<State::STATE_DIMENSION> result;
-            Identity(result);
-            Identity(result.slice<0,6,6,6>(), dt);
+    TooN::Matrix<State::STATE_DIMENSION> getJacobian(const State & state, 
double dt){
+            TooN::Matrix<State::STATE_DIMENSION> result;
+            TooN::Identity(result);
+            TooN::Identity(result.slice<0,6,6,6>(), dt);
             return result;
     }
 
     void updateState( State & state, const double dt ){
         // full velocity vector
-        Vector<6> vel;
+        TooN::Vector<6> vel;
         vel.slice<0,3>() = state.velocity;
         vel.slice<3,3>() = state.angularVelocity;
 
         // update translational components
-        state.pose = SE3::exp(vel * dt) * state.pose;
+        state.pose = TooN::SE3::exp(vel * dt) * state.pose;
         // dampen velocitys
         double attenuation = pow(damp, dt);
         state.velocity *= attenuation;
         state.angularVelocity *= attenuation;
     }
 
-    Matrix<State::STATE_DIMENSION> getNoiseCovariance( double dt ){
-        Matrix<State::STATE_DIMENSION> result;
-        Zero(result);
+    TooN::Matrix<State::STATE_DIMENSION> getNoiseCovariance( double dt ){
+        TooN::Matrix<State::STATE_DIMENSION> result;
+        TooN::Zero(result);
         double dt2 = dt * dt * 0.5;
         double dt3 = dt * dt * dt * 0.3333333333333;
         for(unsigned int i = 0; i < 6; i++){
@@ -96,8 +97,8 @@
         return result;
     }
 
-    void updateFromMeasurement( State & state, const 
Vector<State::STATE_DIMENSION> & innovation ){
-        state.pose = SE3::exp(innovation.slice<0,6>()) * state.pose;
+    void updateFromMeasurement( State & state, const 
TooN::Vector<State::STATE_DIMENSION> & innovation ){
+        state.pose = TooN::SE3::exp(innovation.slice<0,6>()) * state.pose;
         state.velocity = state.velocity + innovation.slice<6,3>();
         state.angularVelocity = state.angularVelocity + 
innovation.slice<9,3>();
     }

Index: kalmanfilter.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/kalmanfilter.h,v
retrieving revision 1.3
retrieving revision 1.4
diff -u -b -r1.3 -r1.4
--- kalmanfilter.h      31 May 2006 16:46:20 -0000      1.3
+++ kalmanfilter.h      5 Jun 2006 15:19:30 -0000       1.4
@@ -82,28 +82,28 @@
 public:
 
     KalmanFilter(){
-        Toon::Identity(identity);
+        TooN::Identity(identity);
     }
 
     /// predicts the state by applying the process model over the time 
interval dt
     /// @param[in] dt time interval
     void predict(double dt){
-        Toon::Matrix<State::STATE_DIMENSION> A = model.getJacobian( state, dt 
);
+        TooN::Matrix<State::STATE_DIMENSION> A = model.getJacobian( state, dt 
);
         model.updateState( state, dt );
         state.covariance = A * state.covariance * A.T() + 
model.getNoiseCovariance( dt );
-        Toon::Symmetrize(state.covariance);
+        TooN::Symmetrize(state.covariance);
     }
 
     /// incorporates a measurement
     /// @param[in] m the measurement to add to the filter state
     template<class Measurement> void filter(Measurement & m){
-        Toon::Matrix<Measurement::M_DIMENSION,State::STATE_DIMENSION> H = 
m.getMeasurementJacobian( state );
-        Toon::Matrix<Measurement::M_DIMENSION> R = m.getMeasurementCovariance( 
state );
-        Toon::Matrix<Measurement::M_DIMENSION> I = H * state.covariance * 
H.T() + R;
+        TooN::Matrix<Measurement::M_DIMENSION,State::STATE_DIMENSION> H = 
m.getMeasurementJacobian( state );
+        TooN::Matrix<Measurement::M_DIMENSION> R = m.getMeasurementCovariance( 
state );
+        TooN::Matrix<Measurement::M_DIMENSION> I = H * state.covariance * 
H.T() + R;
         TooN::LU<Measurement::M_DIMENSION> lu(I);
-        Toon::Matrix<State::STATE_DIMENSION, Measurement::M_DIMENSION> K = 
state.covariance * H.T() * lu.get_inverse();
-        Toon::Vector<Measurement::M_DIMENSION> innovation = m.getInnovation( 
state );
-        Toon::Vector<State::STATE_DIMENSION> stateInnovation = K * innovation;
+        TooN::Matrix<State::STATE_DIMENSION, Measurement::M_DIMENSION> K = 
state.covariance * H.T() * lu.get_inverse();
+        TooN::Vector<Measurement::M_DIMENSION> innovation = m.getInnovation( 
state );
+        TooN::Vector<State::STATE_DIMENSION> stateInnovation = K * innovation;
         model.updateFromMeasurement( state, stateInnovation );
         state.covariance = (identity - K * H) * state.covariance;
         Symmetrize( state.covariance );

Index: measurements.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/measurements.h,v
retrieving revision 1.2
retrieving revision 1.3
diff -u -b -r1.2 -r1.3
--- measurements.h      4 Jun 2006 12:08:49 -0000       1.2
+++ measurements.h      5 Jun 2006 15:19:30 -0000       1.3
@@ -38,7 +38,7 @@
         return covariance;
     }
 
-    inline TooN::Vector<M_DIMENSION> & getMeasurement( const State & state ){
+    inline TooN::Vector<M_DIMENSION> & getInnovation( const State & state ){
         return measurement;
     }
 
@@ -59,7 +59,7 @@
     TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> jacobian;
     TooN::SE3 measurement;
 
-    AbsolutePose(void){
+    WorldPose(void){
         TooN::Identity(covariance);
         TooN::Zero(jacobian);
         TooN::Identity(jacobian.template slice<0,0,6,6>());
@@ -74,7 +74,7 @@
         return covariance;
     }
 
-    inline TooN::Vector<M_DIMENSION> & getMeasurement( const State & state ){
+    inline TooN::Vector<M_DIMENSION> & getInnovation( const State & state ){
         return (measurement * state.pose.inverse()).ln();
     }
 };
@@ -107,7 +107,7 @@
         return covariance;
     }
 
-    inline TooN::Vector<M_DIMENSION> & getMeasurement( const State & state ){
+    inline TooN::Vector<M_DIMENSION> & getInnovation( const State & state ){
         return (state.pose * position - state.pose.get_translation());
     }
 
@@ -150,7 +150,7 @@
         return covariance;
     }
 
-    inline TooN::Vector<M_DIMENSION> getMeasurement( const State & state ){
+    inline TooN::Vector<M_DIMENSION> getInnovation( const State & state ){
         // angular velocity
         /// @todo think about the -gyro and either document or remove
         return  (-gyro - state.angularVelocity);
@@ -186,9 +186,9 @@
         TooN::Zero(result);
         // direction jacobian
         TooN::Vector<M_DIMENSION> local = state.pose.get_rotation() * 
reference;
-        result.template slice<0,3,3,1>() = SO3::generator_field(0, local 
).as_col();
-        result.template slice<0,4,3,1>() = SO3::generator_field(1, local 
).as_col();
-        result.template slice<0,5,3,1>() = SO3::generator_field(2, local 
).as_col();
+        result.template slice<0,3,3,1>() = TooN::SO3::generator_field(0, local 
).as_col();
+        result.template slice<0,4,3,1>() = TooN::SO3::generator_field(1, local 
).as_col();
+        result.template slice<0,5,3,1>() = TooN::SO3::generator_field(2, local 
).as_col();
         return result;
     }
 
@@ -196,7 +196,7 @@
         return covariance;
     }
 
-    inline TooN::Vector<M_DIMENSION> & getMeasurement( const State & state ){
+    inline TooN::Vector<M_DIMENSION> & getInnovation( const State & state ){
         return (measurement - (state.pose.get_rotation() * reference));
     }