Add compile time flags.

7 files changed, 40 insertions, 12 deletions

diff --git a/src/body.cpp b/src/body.cpp
index 1e1f57d..1b07361 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -17,6 +17,19 @@
  */
 #include "body.h"
 
+void
+Body::clamp()
+{
+#ifdef DOCLAMP
+    // Constrain the height
+    if (X[2]<MAXHEIGHT) {
+        X[2]=MAXHEIGHT;
+    } else if (X[2]>MINHEIGHT) {
+        X[2]=MINHEIGHT;
+    }
+#endif     /* -----  DOCLAMP  ----- */
+}
+
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  Body
@@ -28,12 +41,7 @@ void
 Body::dx ( const Matrix<double,9,1> &del )
 {
     X += del;
-    // Constrain the height
-    if (X[2]<-1.5) {
-        X[2]=-1.;
-    } else if (X[2]>-0.3) {
-        X[2]=-0.3;
-    }
+    clamp();
     return ;
 }		/* -----  end of method Body::dx  ----- */
 
@@ -195,8 +203,7 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond
     b.segment<3>(3) = acc-bias+Rbw.transpose()*gravity_world;
 
     X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt;
-    if (X[2]>-0.3) X[2]=-0.3;
-    if (X[2]<-1.3) X[2]=-1.3;
+    clamp();
     return ;
 }		/* -----  end of method Body::motionModel  ----- */
 
diff --git a/src/body.h b/src/body.h
index 90e1998..7bbd50d 100644
--- a/src/body.h
+++ b/src/body.h
@@ -6,6 +6,11 @@
 #define R_HEIGHT 2.5e-3            /* measurement noise of height measurement */
 #define IMU_NOISE 800e-6            /* IMU process noise */
 #define IMU_RANDOMWALK 50e-6      /* Bias process noise */
+//#define DOCLAMP
+#define MAXHEIGHT -1.5 /* Notion of max and min is reversed due to z pointing down */
+#define MINHEIGHT -0.3            /*  */
+
+
 using Eigen::Matrix;
 using Eigen::Vector3d;
 using Eigen::Quaterniond;
@@ -30,6 +35,7 @@ class Body
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias( const Vector3d &b);
         void asVector(const Matrix<double,9,1> &m);
+        void clamp();
         void pos( const UTM &utm);
         void dx( const Matrix<double,9,1> &del);
         void vel(const Matrix<double,3,1> &v);
diff --git a/src/camera.h b/src/camera.h
index 599fa23..7dd83e7 100644
--- a/src/camera.h
+++ b/src/camera.h
@@ -5,7 +5,8 @@
 #include <yaml-cpp/yaml.h>
 
 #define BINNING 0.5 // set the binning factor
-#define YAWCORRECT 7.5
+//#define YAWCORRECT 7.5
+#define YAWCORRECT 0.0
 using Eigen::Matrix;
 using Eigen::Vector4d;
 using Eigen::Vector3d;
diff --git a/src/main.cpp b/src/main.cpp
index 246f73d..1de5b29 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -31,8 +31,12 @@ double
 aboveWater(const Quaterniond &q)
 {
     Vector3d tip;
+#ifdef  DOHEIGHT
     tip << CANOECENTER*3.43, 0, CANOEHEIGHT;
     tip = q._transformVector(tip);
+#else      /* -----  not DOHEIGHT  ----- */
+    tip[2] = CANOEHEIGHT;
+#endif     /* -----  not DOHEIGHT  ----- */
     return tip[2];
 }
 
diff --git a/src/main.h b/src/main.h
index 46fb2f6..8235b98 100644
--- a/src/main.h
+++ b/src/main.h
@@ -37,6 +37,7 @@
 #define CANOECENTER 0.65            /* center of gravity of canoe  */
 #define CANOEHEIGHT -0.55
 #define DOWNSAMPLE 1            /*  */
+#define DOHEIGHT            /*  */
 
 #if  ROS_PUBLISH
 #include <ros/ros.h>
diff --git a/src/state.cpp b/src/state.cpp
index a9a55ae..472ee66 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -290,7 +290,9 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
         } else if (exists(i->id)) {
             Feature *ft;
             ft = featureById(i->id);
+#ifdef INLIERTEST
             if (ft->isInlier(*i, Pxx(), Pxy(i->id), Pyy(i->id), body->ned(), q, INLIER_THRESHOLD)) 
+#endif
             {
                 featuresToUpdate.push_back(*i);
             } 
@@ -319,13 +321,15 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
         }
     }
     if (featuresToUpdate.size()>1) {
+#ifdef  DORANSAC
         ransacUpdate(featuresToUpdate,q);
-    } else if (featuresToUpdate.size()>1) {
+#else      /* -----  not DORANSAC  ----- */
         MatrixXd h;
         h = H(q,featuresToUpdate);
         MatrixXd S;
         S = h*P*h.transpose() + R(featuresToUpdate);
         kalmanUpdate(h,S,featuresToUpdate,q);
+#endif     /* -----  not DORANSAC  ----- */
     }
     addFeatures( featuresToAdd, q, zmeas);
 
@@ -405,8 +409,11 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double
         // Create feature
         Feature *f;
         if (i->z_type==REFLECTION) {
+#ifdef  INITDEPTH
             f = new Feature(i->id, i->source, i->reflection, pos, q, z);
-            //f = new Feature(i->id, i->source, pos, q);
+#else      /* -----  not INITDEPTH  ----- */
+            f = new Feature(i->id, i->source, pos, q);
+#endif     /* -----  not INITDEPTH  ----- */
         } else {
             f = new Feature(i->id, i->source, pos, q);
         }
diff --git a/src/state.h b/src/state.h
index 2383706..19e807b 100644
--- a/src/state.h
+++ b/src/state.h
@@ -13,7 +13,9 @@
 #define MAXFEATURES 50
 #define COVBIAS 2e-5
 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
-#define DORANSAC            /*  */
+//#define DORANSAC            /*  */
+//#define INITDEPTH            /*  */
+//#define INLIERTEST            /*  */
 
 using Eigen::Dynamic;
 using Eigen::Matrix;