From c2d12b0b4a9cbf5eafb01d22ac5a4e141c042531 Mon Sep 17 00:00:00 2001 From: feixh Date: Thu, 29 Aug 2019 13:37:29 -0700 Subject: [PATCH] got the density right; trajectory is not smooth --- cfg/phab.json | 88 ++++++++++++++------------------------ node/launch/xivo.xml | 2 +- scripts/interpolate_imu.py | 4 +- 3 files changed, 36 insertions(+), 58 deletions(-) diff --git a/cfg/phab.json b/cfg/phab.json index effbc91a..19c2aae3 100644 --- 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 index 14938482..37c6ba43 100644 --- a/node/launch/xivo.xml +++ b/node/launch/xivo.xml @@ -12,7 +12,7 @@ - + diff --git a/scripts/interpolate_imu.py b/scripts/interpolate_imu.py index 2f976811..ce356adc 100644 --- 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!') \ No newline at end of file + raise ValueError('empty list of raw measurement!')