add feature noise params to cli

5 files changed, 19 insertions, 5 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index 57effcc..43fd834 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -152,7 +152,7 @@ Feature::Q ( const double dt )
 {
     Matrix<double,3,3> Q;
     Q = Matrix<double,3,3>::Identity();
-    Q *= 0.5*dt*dt*FEATURE_NOISE*FEATURE_NOISE;//*1e-1;
+    Q *= 0.5*dt*dt*feature_noise*feature_noise;//*1e-1;
     return Q;
 }		/* -----  end of method Feature::q  ----- */
 
diff --git a/src/feature.h b/src/feature.h
index e3ec2b8..d582250 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -4,8 +4,8 @@
 #include <Eigen/Dense>
 #include <iostream>
 
-#include "camera.h"
 #include "types.h"
+#include "camera.h"
 
 #define FEATURE_NOISE  3.5           /* Feature process noise */
 #define VIEW_NOISE 6e-3             /*  */
diff --git a/src/main.cpp b/src/main.cpp
index 57960dd..c1d2a27 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -23,6 +23,7 @@
 bool seenutm=false;
 bool seenpva=false;
 bool seencov=false;
+double feature_noise,view_noise,initial_view_noise,reflection_view_noise;
 using std::cout;
 using std::endl;
 using std::cerr;
@@ -418,8 +419,8 @@ update_dt(const timestamp t, timestamp *t_old)
 int 
 main(int argc, char **argv)
 {
-    if (argc!=2) {
-        fprintf(stderr, "Usage: %s camera\n", argv[0]);
+    if (argc!=3) {
+        fprintf(stderr, "Usage: %s camera params\n", argv[0]);
         exit(1);
     }
     State mu;
@@ -448,6 +449,14 @@ main(int argc, char **argv)
     printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
 #endif
 #endif     /* -----  not ROS_PUBLISH  ----- */
+    // read params file
+    FILE *pfin;
+    pfin = fopen(argv[2], "r");
+    fscanf(pfin,"%lf",&feature_noise);
+    fscanf(pfin,"%lf",&view_noise);
+    fscanf(pfin,"%lf",&initial_view_noise);
+    fscanf(pfin,"%lf",&reflection_view_noise);
+    fclose(pfin);
     // Read sensors from file
     int i=0;
     char line[MAXLINE];
diff --git a/src/main.h b/src/main.h
index 7b8ebac..0210aab 100644
--- a/src/main.h
+++ b/src/main.h
@@ -9,11 +9,11 @@
 #include <iostream>
 #include <vector>
 
+#include "types.h"
 #include "body.h"
 #include "camera.h"
 #include "ourerr.hpp"
 #include "state.h"
-#include "types.h"
 #include "vision.h"
 //#define ROS_PUBLISH          /* Uncomment to publish ROS node */
 //#define ROS_SUBSCRIBE            /* Uncomment to subscribe to ROS */
diff --git a/src/types.h b/src/types.h
index 52f8228..e7fa82f 100644
--- a/src/types.h
+++ b/src/types.h
@@ -16,6 +16,11 @@ using Eigen::Vector2d;
 using Eigen::Vector3d;
 using Eigen::Vector4d;
 
+extern double feature_noise;
+extern double view_noise;
+extern double initial_view_noise;
+extern double reflection_view_noise;
+
 typedef Eigen::Matrix<double,2,1,Eigen::DontAlign> UVector2d;
 // A struct for storing measurements.
 typedef enum {BOTH,REFLECTION,MONO,HEIGHT} measurement_type;