diff options
Diffstat (limited to 'src/main.cpp')
| -rw-r--r-- | src/main.cpp | 19 |
1 files changed, 17 insertions, 2 deletions
diff --git a/src/main.cpp b/src/main.cpp index c4b2c4c..fb86f52 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -41,6 +41,13 @@ double ransac_li_threshold = 1.8e-6; double ransac_hi_threshold = 0.25; double covbias = 2e-5; +double angbiasx; +double angbiasy; +double angbiasz; +double accbiasx; +double accbiasy; +double accbiasz; + using std::cout; using std::endl; using std::cerr; @@ -216,7 +223,7 @@ imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp Vector3d acc(msg.linear_acceleration.y,msg.linear_acceleration.x,-msg.linear_acceleration.z); Vector3d ang(msg.angular_velocity.y,msg.angular_velocity.x,-msg.angular_velocity.z); // WARNING: This is the bias for 1112-1 - Vector3d ang_bias(ANGBIASX, ANGBIASY, ANGBIASZ); + Vector3d ang_bias(angbiasx, angbiasy, angbiasz); ang -= ang_bias; double dtf = dt.secs+dt.nsecs*1e-9; @@ -445,7 +452,7 @@ main(int argc, char **argv) attitude_t att; // bias in FRD coordinates Vector3d acc_bias; - acc_bias << ACCBIASX, ACCBIASY, ACCBIASZ; + acc_bias << accbiasx, accbiasy, accbiasz; mu.accelerometer_bias(acc_bias); timestamp dt{0,0}; timestamp t_old{0,0}; @@ -469,6 +476,7 @@ main(int argc, char **argv) // 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); @@ -482,6 +490,13 @@ main(int argc, char **argv) fscanf(pfin,"%lf",&ransac_li_threshold); fscanf(pfin,"%lf",&ransac_hi_threshold); fscanf(pfin,"%lf",&covbias); + */ + fscanf(pfin,"%lf",&angbiasx); + fscanf(pfin,"%lf",&angbiasy); + fscanf(pfin,"%lf",&angbiasz); + fscanf(pfin,"%lf",&accbiasx); + fscanf(pfin,"%lf",&accbiasy); + fscanf(pfin,"%lf",&accbiasz); fclose(pfin); // Read sensors from file int i=0; |