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

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Gerhard Reitmayr <gerhard>      06/12/14 14:00:46

Modified files:
        tag            : constantvelocity.h kalmanfilter.h 

Log message:
        optimizations through returning references instead of temporary return 
values and using reference variables in the filter

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

Patches:
Index: constantvelocity.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/constantvelocity.h,v
retrieving revision 1.2
retrieving revision 1.3
diff -u -b -r1.2 -r1.3
--- constantvelocity.h  5 Jun 2006 15:19:30 -0000       1.2
+++ constantvelocity.h  14 Dec 2006 14:00:46 -0000      1.3
@@ -17,7 +17,7 @@
 /// @ingroup constantvelocitygroup
 class State {
 public:
-    State(void){
+    inline State(void){
         reset();
     }
 
@@ -54,23 +54,30 @@
 class Model {
 public:
     TooN::Vector<State::STATE_DIMENSION> sigma;
+    TooN::Matrix<State::STATE_DIMENSION> jacobian;
+    TooN::Matrix<State::STATE_DIMENSION> noise;
     // dampening of velocity
     double damp;
 
     Model(void){
         TooN::Zero(sigma);
         damp = 1;
+        TooN::Identity(jacobian);
+        TooN::Zero(noise);
     }
 
     // Jacobian has pos, rot, vel, angularVel in this order
-    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;
+    inline const TooN::Matrix<State::STATE_DIMENSION> & getJacobian(const 
State & state, const double dt) {
+            jacobian(0,6) = dt;
+            jacobian(1,7) = dt;
+            jacobian(2,8) = dt;
+            jacobian(3,9) = dt;
+            jacobian(4,10) = dt;
+            jacobian(5,11) = dt;
+            return jacobian;
     }
 
-    void updateState( State & state, const double dt ){
+    inline void updateState( State & state, const double dt ){
         // full velocity vector
         TooN::Vector<6> vel;
         vel.slice<0,3>() = state.velocity;
@@ -84,20 +91,18 @@
         state.angularVelocity *= attenuation;
     }
 
-    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;
+    inline const TooN::Matrix<State::STATE_DIMENSION> & getNoiseCovariance( 
const double dt ){
+        const double dt2 = dt * dt * 0.5;
+        const double dt3 = dt * dt * dt * 0.3333333333333;
         for(unsigned int i = 0; i < 6; i++){
-            result(i,i) = dt * sigma[i] + dt3 * sigma[i+6];
-            result(i+6,i) = result(i,i+6) = dt2 * sigma[i+6];
-            result(i+6, i+6) = dt * sigma[i+6];
+            noise(i,i) = dt * sigma[i] + dt3 * sigma[i+6];
+            noise(i+6,i) = noise(i,i+6) = dt2 * sigma[i+6];
+            noise(i+6, i+6) = dt * sigma[i+6];
         }
-        return result;
+        return noise;
     }
 
-    void updateFromMeasurement( State & state, const 
TooN::Vector<State::STATE_DIMENSION> & innovation ){
+    inline 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.7
retrieving revision 1.8
diff -u -b -r1.7 -r1.8
--- kalmanfilter.h      5 Dec 2006 17:43:10 -0000       1.7
+++ kalmanfilter.h      14 Dec 2006 14:00:46 -0000      1.8
@@ -24,11 +24,11 @@
 class Model {
 public:
     // return process model jacobian for a given state and time delta 
(typically A)
-    TooN::Matrix<State::STATE_DIMENSION> getJacobian(const State & state, 
double dt);
+    const TooN::Matrix<State::STATE_DIMENSION> & getJacobian(const State & 
state, double dt);
     // update the state for a given time delta (not all states are actually x 
= Ax, therefore this is a function)
     void updateState( State & state, const double dt );
     // return process noise matrix for given time delta (typically Q)
-    TooN::Matrix<State::STATE_DIMENSION> getNoiseCovariance( double dt );
+    const TooN::Matrix<State::STATE_DIMENSION> & getNoiseCovariance( double dt 
);
     // update the state from an innovation. the innovation was computed by the 
filter based on measurement etc.
     void updateFromMeasurement( State & state, const 
TooN::Vector<State::STATE_DIMENSION> & innovation );
 };
@@ -41,11 +41,11 @@
 public:
     static const int M_DIMENSION =  ??;  // dimension of measurement
     // return measurement jacobian, from state -> measurement
-    Matrix<M_DIMENSION,State::STATE_DIMENSION> getMeasurementJacobian( const 
State & state );
+    const Matrix<M_DIMENSION,State::STATE_DIMENSION> & getMeasurementJacobian( 
const State & state );
     // return measurement noise covariance
-    Matrix<M_DIMENSION> getMeasurementCovariance( const State & state );
+    const Matrix<M_DIMENSION> & getMeasurementCovariance( const State & state 
);
     // return the innovation, the difference between actual measurement and 
the measurement prediction based on the state
-    Vector<M_DIMENSION> getInnovation( const State & state );
+    const Vector<M_DIMENSION> & getInnovation( const State & state );
 };
 @endcode
 All of the member functions take the state as parameters, because the returned 
values are typically
@@ -71,6 +71,9 @@
     filter.filter(m);
 }
 @endcode
+
+Note, that all the return values from the various classes are const 
references. This avoids any unnecessary copying of data.
+You can also return types that may be stored in const references, such as 
non-const references and return values.
 */
 
 /**
@@ -91,21 +94,20 @@
     /// 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 
);
         model.updateState( state, dt );
-        state.covariance = TooN::transformCovariance(A, state.covariance) + 
model.getNoiseCovariance( dt );
+        state.covariance = TooN::transformCovariance(model.getJacobian( state, 
dt ), state.covariance) + model.getNoiseCovariance( dt );
         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 );
+        const TooN::Matrix<Measurement::M_DIMENSION,State::STATE_DIMENSION> & 
H = m.getMeasurementJacobian( state );
+        const TooN::Matrix<Measurement::M_DIMENSION> & R = 
m.getMeasurementCovariance( state );
         TooN::Matrix<Measurement::M_DIMENSION> I = 
TooN::transformCovariance(H, state.covariance) + 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 );
+        const 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;