set ransac thresh and covbias

4 files changed, 15 insertions, 5 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index 43fd834..95a5366 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -1099,7 +1099,7 @@ Feature::isRansacInlier(const measurement_t &z, const Vector3d &pos,
     y = zi-hi;
     Y = y.transpose()*y;
 
-    return (Y<RANSAC_LI_THRESHOLD) ? true : false;
+    return (Y<ransac_li_threshold) ? true : false;
 }
 
 Vector3d
diff --git a/src/main.cpp b/src/main.cpp
index 85ccf55..e64c0c8 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -34,6 +34,11 @@ double acc_std = 0.002835;
 double ang_std = 0.008019;
 double acc_bias_std = 0.00891;
 
+double canoecenter = 0.62;
+double canoeheight = -0.46;
+
+double ransac_li_threshold, ransac_hi_threshold,covbias;
+
 using std::cout;
 using std::endl;
 using std::cerr;
@@ -462,8 +467,9 @@ main(int argc, char **argv)
     // read params file
     FILE *pfin;
     pfin = fopen(argv[2], "r");
-    fscanf(pfin,"%lf",&canoecenter);
-    fscanf(pfin,"%lf",&canoeheight);
+    fscanf(pfin,"%lf",&ransac_li_threshold);
+    fscanf(pfin,"%lf",&ransac_hi_threshold);
+    fscanf(pfin,"%lf",&covbias);
     fclose(pfin);
     // Read sensors from file
     int i=0;
diff --git a/src/state.cpp b/src/state.cpp
index cbf0e15..8b1ff3f 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -21,7 +21,7 @@ State::State ( )
 {
     body = new Body;
     P = Matrix<double,Dynamic,Dynamic>::Zero(STATESIZE,STATESIZE);
-    P.block<3,3>(6,6) = COVBIAS*Matrix3d::Identity();
+    P.block<3,3>(6,6) = covbias*Matrix3d::Identity();
     return ;
 }		/* -----  end of method State::State  ----- */
 
@@ -988,7 +988,7 @@ State::ransacUpdate ( vector<measurement_t> &z, const Quaterniond &q )
         }
         if (!found) {
             Feature *ft = featureById(i->id);
-            if (ft->isInlier(*i, Pxx(), Pxy(i->id), Pyy(i->id), body->ned(), q, RANSAC_HI_THRESHOLD)) {
+            if (ft->isInlier(*i, Pxx(), Pxy(i->id), Pyy(i->id), body->ned(), q, ransac_hi_threshold)) {
                 hi_inliers.push_back(*i);
             } else {
                 removeFeature(i->id, true);
diff --git a/src/types.h b/src/types.h
index b9a5560..fac708c 100644
--- a/src/types.h
+++ b/src/types.h
@@ -29,6 +29,10 @@ extern double acc_bias_std;
 extern double canoecenter;
 extern double canoeheight;
 
+extern double ransac_li_threshold;
+extern double ransac_hi_threshold;
+extern double covbias;
+
 typedef Eigen::Matrix<double,2,1,Eigen::DontAlign> UVector2d;
 // A struct for storing measurements.
 typedef enum {BOTH,REFLECTION,MONO,HEIGHT} measurement_type;