got the density right; trajectory is not smooth

tekrajchhetri · Aug 29, 2019 · c2d12b0 · c2d12b0
1 parent d93857b
commit c2d12b0
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 58 deletions.
diff --git a/cfg/phab.json b/cfg/phab.json
@@ -3,7 +3,7 @@
   "simulation": false,
   "print_timing": true,
   "use_canvas": true,
-  "use_debug_view": false,  // draw rejected & dropped features on canvas
+  "use_debug_view": true,  // draw rejected & dropped features on canvas
   "async_run": false, // turn this off in benchmarking
 
   // visualization (tracker view) option
@@ -13,7 +13,7 @@
   // algorithmic-level knobs
   "integration_method": "PrinceDormand", // "PrinceDormand", "RK4", //, Fehlberg
   "use_OOS": false, // update with Out-Of-State features
-  "use_depth_opt": true,  // depth optimization
+  "use_depth_opt": false,  // depth optimization
   "use_MH_gating": true,
   "use_1pt_RANSAC": false,   
   "use_compression": true,  // measurement compression
@@ -27,7 +27,7 @@
     "attempts": 12,
     "min_scale_factor": 0.125,
     "max_scale_factor": 4.0,
-    "stepsize": 0.002
+    "stepsize": -1
   },
 
   "RK4": {
@@ -42,7 +42,7 @@
 
 
   // gravity constant
-  "gravity": [0, 0, -9.8],
+  "gravity": [0, 0, -9.796],
   // Initial State
   "X" : {
     "W"   : [0, 0, 0],
@@ -52,11 +52,12 @@
     "bg"  : [8.93594e-05, -0.000113499, -4.95253e-05],
     "ba"  : [0.106957, 0.0822465, -0.092585],
 
-    "Wbc" : [-2.21, -2.21, 0],
-    // Wbc in tangent space: [-0.0418, -2.172, 2.248]
-    "Tbc": [0, 0, 0],
+    "Wbc" : [1.94, 1.97, 0],
+    "Tbc": [-0.02, 0.08, -0.03],
+    // "Tbc": [0, 0, 0],
+
     "Wg"  : [0, 0, 0],
-    "td"  : 0
+    "td"  : -0.001
   },
 
   "P" : {
@@ -65,13 +66,13 @@
     "V"   : 0.5,
     "bg"  : 1e-4,
     "ba"  : 1e-3,
-    "Wbc" : 3.1,
+    "Wbc" : 0.1,
     "Tbc" : 0.01,
-    "Wg"  : 3.01,
-    "td"  : 1e-5, // 1ms
-    "Cg"  : 1e-5,
-    "Ca"  : 1e-5,
-    "FC"  : 50,
+    "Wg"  : 0.01,
+    "td"  : 3e-5, // 1ms
+    "Cg"  : 1e-3,
+    "Ca"  : 1e-3,
+    "FC"  : 10, //
     "distortion": 1e-3
   },
 
@@ -86,56 +87,33 @@
     "Wg"  : 0
   },
 
-  // // exact densities provided by TUM-VI dataset
-  // // https://vision.in.tum.de/data/datasets/visual-inertial-dataset
-  // "Qimu": {
-  //   "gyro": 8.0e-5,
-  //   "gyro_bias": 2.2e-6,
-  //   "accel":1.4e-3,
-  //   "accel_bias": 8.6e-5
-  // },
-
-  // // inflated by sqrt(3) times 
+  // Exact densities provided by TUM-VI dataset
+  // This is for tango yellowstone, not the phab phone
   // "Qimu": {
-  //   "gyro": 14.0e-5,
-  //   "gyro_bias": 3.8e-6,
-  //   "accel": 2.4e-3,
-  //   "accel_bias": 14.9e-5
+  //   "gyro": 0.00015,
+  //   "gyro_bias": 0.0004,
+  //   "accel":0.003,
+  //   "accel_bias": 0.000005
   // },
 
-  // // doubled 
-  // "Qimu": {
-  //   "gyro": 16.0e-5,
-  //   "gyro_bias": 4.4e-6,
-  //   "accel":2.8e-3,
-  //   "accel_bias": 17.2e-5
-  // },
-
-  // inflated by 3 times
   "Qimu": {
-    "gyro": 24.0e-5,
-    "gyro_bias": 6.6e-6,
-    "accel": 4.2e-3,
-    "accel_bias": 25.8e-5
+    // NOTE: this should be sqrt of the density
+    // since internally, this will be squared
+    "gyro": 0.023,
+    "gyro_bias": 0.0034,
+    "accel":0.095,
+    "accel_bias": 0.0039
   },
 
-  // // inflated by 10 times
-  // "Qimu": {
-  //   "gyro": 8.0e-4,
-  //   "gyro_bias": 2.2e-5,
-  //   "accel":1.4e-2,
-  //   "accel_bias": 8.6e-4
-  // },
-
 
   // initial std on feature state
   "initial_z": 2.5, // meter
-  "initial_std_x": 1.0, // pixel
-  "initial_std_y": 1.0, // pixel
+  "initial_std_x": 1.5, // pixel
+  "initial_std_y": 1.5, // pixel
   "initial_std_z": 1.0,  // meter
 
   // std of visuale measurement, in pixels
-  "visual_meas_std":  1.5,
+  "visual_meas_std":  3.5,
   "outlier_thresh": 1.1,
   "oos_meas_std": 3.5,
   "max_depth": 5.0,
@@ -146,7 +124,7 @@
   "subfilter": {
     "visual_meas_std":  3.5,
     "ready_steps": 2,
-    "MH_thresh": 8.991
+    "MH_thresh": 18.991
   },
 
   // pre-subfilter triangulation options
@@ -157,7 +135,7 @@
   },
 
   "depth_opt": {
-    "two_view": false,
+    "two_view": true,
     "use_hessian": true,
     "max_iters": 5,
     "eps": 1e-3,
@@ -206,7 +184,7 @@
 
   "min_inliers": 5, // minimum number of inlier measurements
 
-  "MH_thresh": 5.991, // 8.991
+  "MH_thresh": 8.991, // 8.991
   "MH_adjust_factor": 1.15,
 
   "1pt_RANSAC_thresh": 1.5,

diff --git a/node/launch/xivo.xml b/node/launch/xivo.xml
@@ -12,7 +12,7 @@
     <param name="viewer_config_path" value="$(find xivo)/../cfg/phab_viewer.json"/>
     <param name="estimator_queue_size" value="1000" />
     <param name="image_topic"   value="/cam0/image_raw" />
-    <param name="imu_topic"   value="/imu0_calib" />
+    <param name="imu_topic"   value="/imu0" />
     <param name="viewer_type" value="pangolin" />
     <!-- <param name="viewer_type" value="ros" /> -->
   </node>

diff --git a/scripts/interpolate_imu.py b/scripts/interpolate_imu.py
@@ -94,7 +94,7 @@ def find_next_accel(curr, data):
         prev_gyro = (ts, gyro)
       else:
         # both fields are non-empty, forward
-        prev_gyro, prev_accel = gyro, accel
+        prev_gyro, prev_accel = (ts, gyro), (ts, accel)
         interp.append((ts, gyro, accel))
 
     with open(output, 'w') as ofid:
@@ -109,4 +109,4 @@ def find_next_accel(curr, data):
       plt.show()
 
   else:
-    raise ValueError('empty list of raw measurement!')
+    raise ValueError('empty list of raw measurement!')