Add #if statements to call methods properly.

1 files changed, 61 insertions, 15 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 393ffa2..5253425 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -189,16 +189,17 @@ State::partialUpdate( const MatrixXd &h, const MatrixXd &S,
         const std::vector<measurement_t> &z, const Quaterniond &q )
 #endif
 {
-#if STATESIZE==13
-    Quaterniond q = body->qhat();
-#endif
     MatrixXd K;
     // P^T is implied here since P is symmetric
     K = S.fullPivLu().solve(h*P).transpose();
 
     // Compute the innovation or error
     Matrix<double,Dynamic,1> y;
+#if STATESIZE==13
+    y = innovation(z);
+#else
     y = innovation(z,q);
+#endif
     
     // Get the update
     Matrix<double,Dynamic,1> dx;
@@ -230,11 +231,12 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S,
         const std::vector<measurement_t> &z, const Quaterniond &q)
 #endif
 {
-#if STATESIZE==13
-    Quaterniond q = body->qhat();
-#endif
     MatrixXd K;
+#if STATESIZE==13
+    K = partialUpdate(h,S,z);
+#else
     K = partialUpdate(h,S,z,q);
+#endif
 
 
     // Compute P_k|k
@@ -314,9 +316,6 @@ State::handle_measurements ( const std::vector<measurement_t> &z)
 State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterniond &q)
 #endif
 {
-#if STATESIZE==13
-    Quaterniond q = body->qhat();
-#endif
     std::vector<measurement_t> featuresToAdd;
     std::vector<measurement_t> featuresToUpdate;
     double zmeas = body->ned()[2];
@@ -328,6 +327,9 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
             Feature *ft;
             ft = featureById(i->id);
 #ifdef INLIERTEST
+#if STATESIZE==13
+            Quaterniond q = body->qhat();
+#endif
             if (ft->isInlier(*i, Pxx(), Pxy(i->id), Pyy(i->id), body->ned(), q, INLIER_THRESHOLD)) 
 #endif
             {
@@ -359,16 +361,32 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
     }
     if (featuresToUpdate.size()>1) {
 #ifdef  DORANSAC
+#if STATESIZE==13
+        ransacUpdate(featuresToUpdate);
+#else
         ransacUpdate(featuresToUpdate,q);
+#endif
 #else      /* -----  not DORANSAC  ----- */
         MatrixXd h;
+#if STATESIZE==13
+        h = H(featuresToUpdate);
+#else
         h = H(q,featuresToUpdate);
+#endif
         MatrixXd S;
         S = h*P*h.transpose() + R(featuresToUpdate);
+#if STATESIZE==13
+        kalmanUpdate(h,S,featuresToUpdate);
+#else
         kalmanUpdate(h,S,featuresToUpdate,q);
+#endif
 #endif     /* -----  not DORANSAC  ----- */
     }
+#if STATESIZE==13
+    addFeatures( featuresToAdd, zmeas);
+#else
     addFeatures( featuresToAdd, q, zmeas);
+#endif
 
     return ;
 }		/* -----  end of method State::feature_update  ----- */
@@ -444,8 +462,9 @@ State::Pyy ( int id )
 
 void
 #if STATESIZE==13
-#else
 State::addFeatures ( std::vector<measurement_t> &F, double z)
+#else
+State::addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z);
 #endif
 {
 #if STATESIZE==13
@@ -572,12 +591,13 @@ State::Pkk1 ( const Vector3d &ang, const double dt)
 State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt)
 #endif
 {
-#if STATESIZE==13
-    Quaterniond q = body->qhat();
-#endif
     MatrixXd f,fullQ;
     fullQ = Q(dt);
+#if STATESIZE==13
+    f = F ( ang, dt );
+#else
     f = F ( q, ang, dt );
+#endif
     P = f*P*f.transpose()+fullQ;
     // Enforce symmetry
     P = 0.5*(P+P.transpose());
@@ -593,10 +613,12 @@ State::motionModel ( const Vector3d &acc, const Vector3d &ang,
 #endif
 {
 #if STATESIZE==13
-    Quaterniond q = body->qhat();
-#endif
+    Pkk1(ang,dt);
+    body->motionModel(acc,ang,dt);
+#else
     Pkk1(ang,q,dt);
     body->motionModel(acc,ang,q,dt);
+#endif
 
     Vector3d vel;
     vel = body->vel();
@@ -862,10 +884,18 @@ State::ransacUpdate ( std::vector<measurement_t> &z, const Quaterniond &q )
         std::vector<measurement_t> Z;
         Z.push_back(zi);
         MatrixXd h;
+#if STATESIZE==13
+        h = H(Z);
+#else
         h = H(q,Z);
+#endif
         MatrixXd S;
         S = h*P*h.transpose() + R(Z);
+#if STATESIZE==13
+        partialUpdate(h,S,Z);
+#else
         partialUpdate(h,S,Z,q);
+#endif
 
         for (auto j=z.begin(); j!=z.end(); ++j) {
             if (j->id==zi.id) continue;
@@ -892,10 +922,18 @@ State::ransacUpdate ( std::vector<measurement_t> &z, const Quaterniond &q )
 
     // Perform a full update using the inliers.
     MatrixXd h;
+#if STATESIZE==13
+    h = H(inliers);
+#else
     h = H(q,inliers);
+#endif
     MatrixXd S;
     S = h*P*h.transpose() + R(inliers);
+#if STATESIZE==13
+    kalmanUpdate(h, S, inliers);
+#else
     kalmanUpdate(h, S, inliers, q);
+#endif
 
     // High innovation update
     std::vector<measurement_t> hi_inliers;
@@ -917,9 +955,17 @@ State::ransacUpdate ( std::vector<measurement_t> &z, const Quaterniond &q )
         }
     }
 
+#if STATESIZE==13
+    h = H(hi_inliers);
+#else
     h = H(q,hi_inliers);
+#endif
     S = h*P*h.transpose() + R(hi_inliers);
+#if STATESIZE==13
+    kalmanUpdate(h, S, hi_inliers);
+#else
     kalmanUpdate(h, S, hi_inliers, q);
+#endif
 
     /*
     cout << "Z: " << z.size()