From 011a1f0448e836285e36edbabc38afee4b4ac224 Mon Sep 17 00:00:00 2001 From: Xiaohan Fei Date: Mon, 19 Aug 2019 22:22:41 -0700 Subject: [PATCH] clean up --- cfg/atan.json | 10 - cfg/equidistant.json | 13 - cfg/estimator_latex.json | 122 ------ cfg/estimator_scenenet.json | 154 -------- cfg/estimator_sim.json | 77 ---- cfg/lyapunov_baseline.json | 156 -------- cfg/pointgrey_xsens.json | 228 ----------- cfg/pointgrey_xsens_viewer.json | 28 -- cfg/simulation.json | 8 - cfg/simulator.json | 60 --- cfg/tuning.json | 155 -------- cfg/viewer_scenenet.json | 27 -- experimental/CMakeLists.txt | 14 - experimental/async_dataloader.cpp | 195 ---------- experimental/camera_factory.h | 438 ---------------------- experimental/inproc_communication_zmq.cpp | 76 ---- experimental/test_camera_factory.cpp | 17 - format_all.sh => misc/format_all.sh | 0 print_stats.sh => misc/print_stats.sh | 0 run_all.sh => misc/run_all.sh | 0 requirements.txt | 1 - src/app/simulation.cpp | 131 ------- src/simulator.cpp | 398 -------------------- src/simulator.h | 82 ---- 24 files changed, 2390 deletions(-) delete mode 100644 cfg/atan.json delete mode 100644 cfg/equidistant.json delete mode 100644 cfg/estimator_latex.json delete mode 100644 cfg/estimator_scenenet.json delete mode 100644 cfg/estimator_sim.json delete mode 100644 cfg/lyapunov_baseline.json delete mode 100644 cfg/pointgrey_xsens.json delete mode 100644 cfg/pointgrey_xsens_viewer.json delete mode 100644 cfg/simulation.json delete mode 100644 cfg/simulator.json delete mode 100644 cfg/tuning.json delete mode 100644 cfg/viewer_scenenet.json delete mode 100644 experimental/CMakeLists.txt delete mode 100644 experimental/async_dataloader.cpp delete mode 100644 experimental/camera_factory.h delete mode 100644 experimental/inproc_communication_zmq.cpp delete mode 100644 experimental/test_camera_factory.cpp rename format_all.sh => misc/format_all.sh (100%) rename print_stats.sh => misc/print_stats.sh (100%) rename run_all.sh => misc/run_all.sh (100%) delete mode 100644 src/app/simulation.cpp delete mode 100644 src/simulator.cpp delete mode 100644 src/simulator.h diff --git a/cfg/atan.json b/cfg/atan.json deleted file mode 100644 index c094d8b5..00000000 --- a/cfg/atan.json +++ /dev/null @@ -1,10 +0,0 @@ -{ -"model": "atan", -"rows": 320, -"cols": 480, -"fx": 0.425881, -"fy": 0.639873, -"cx": 0.504153, -"cy": 0.525719, -"w": 0.857591 -} diff --git a/cfg/equidistant.json b/cfg/equidistant.json deleted file mode 100644 index 7d8ed7a6..00000000 --- a/cfg/equidistant.json +++ /dev/null @@ -1,13 +0,0 @@ -{ -"model": "equidistant", -"rows": 512, -"cols": 512, -"fx": 190.97847715128717, -"fy": 190.9733070521226, -"cx": 254.93170605935475, -"cy": 256.8974428996504, -"k0123": [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182], -"max_iter": 15, -"comment": "512-cam0" -} - diff --git a/cfg/estimator_latex.json b/cfg/estimator_latex.json deleted file mode 100644 index e2c56900..00000000 --- a/cfg/estimator_latex.json +++ /dev/null @@ -1,122 +0,0 @@ -{ - // This particular configuration is used to generate the results in - // the latex manuscript. - "simulation": false, - "update_method": "ekf", - "gravity": [0, 0, -9.8], - // Initial State - "X" : { - "W" : [0, 0, 0], - "T" : [0, 0, 0], - "V" : [0, 0, 0], - "bg" : [0, 0, 0], - "ba" : [0, 0, 0], - "Wbc" : [[-0.99952504, 0.00750192, -0.02989013], - [0.02961534, -0.03439736, -0.99896935], - [-0.00852233, -0.99938008, 0.03415885]], - "Tbc": [0.04557484, -0.0711618, -0.04468125], - "Wg" : [0, 0] - }, - - "P" : { - "W" : 0.0001, - "T" : 0.001, - "V" : 0.5, - "bg" : 0.0001, - "ba" : 0.001, - "Wbc" : 0.0001, - "Tbc" : 0.001, - "Wg" : 0.01 - }, - - "Qmodel" : { - "W" : 0.000, - "T" : 0.0, - "V" : 0.0, - "wb" : 0.0000, - "ab" : 0.000, - "Wbc" : 0.000, - "Tbc" : 0.0, - "Wg" : 0.000 - }, - - "Qimu": { - "gyro": 1e-3, - "gyro_bias": 1e-4, - "accel":1e-2, - "accel_bias": 1e-1 - }, - - // initial std on feature state - "initial_z": 2.5, // meter - "initial_std_x": 3.5, // pixel - "initial_std_y": 3.5, // pixel - "initial_std_z": 1.0, // meter - - // std of visuale measurement, in pixels - "visual_meas_std": 1.5, - "subfilter_visual_meas_std": 3.5, - "max_depth": 5.0, - "min_depth": 0.05, - "max_depth_init_steps": 5, - "depth_opt_iters": 5, - "depth_opt_eps": 0.001, - "depth_opt_damping": 1e-3, - "inn_thresh": 64, - "MH_thresh": 6, - - "imu_calib": { - "Cas": [1, 1, 1], - "Car": [[1, 0, 0], - [0, 1, 0], - [0, 0, 1]], - "Cgs": [1, 1, 1], - "Cgr": [[1, 0, 0], - [0, 1, 0], - [0, 0, 1]] - }, - "imu_init_counter": 20, - "RK4": true, - - "camera_cfg": { - "model": "equidistant", - "rows": 512, - "cols": 512, - "fx": 190.97847715128717, - "fy": 190.9733070521226, - "cx": 254.93170605935475, - "cy": 256.8974428996504, - "k0123": [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182], - "max_iter": 15, - "comment": "512-cam0" - }, - - "tracker_cfg": { - "mask_size": 15, - "num_features_min": 90, - "num_features_max": 120, - "normalize": true, - - "OpticalFlow": { - "win_size": 15, - "max_level": 5, - "max_iter": 15, - "eps": 1e-4, - "threshold": 20 - }, - - // somehow, only BRISK and FAST work well with the generic FeatureDetector interface (Version 2.4.9) - "DetectorType": "FAST", - - "FAST": { - "threshold": 20, - "nonmaxSuppression": true - }, - - "GridAdapter": { - "grid_rows": 4, - "grid_cols": 4 - } - } - -} diff --git a/cfg/estimator_scenenet.json b/cfg/estimator_scenenet.json deleted file mode 100644 index 5efe4954..00000000 --- a/cfg/estimator_scenenet.json +++ /dev/null @@ -1,154 +0,0 @@ -{ - // verbose - "simulation": false, - "print_timer": false, - - // 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_MH_gating": true, - "use_1pt_RANSAC": false, - "use_compression": true, // measurement compression - "compression_trigger_ratio": 1.5, - - "PrinceDormand": { - "control_stepsize": false, - "tolerance": 1e-15, - "attempts": 12, - "min_scale_factor": 0.125, - "max_scale_factor": 4.0, - "stepsize": 0.002 - }, - - "RK4": { - "stepsize": 0.002 - }, - - // memory - "max_features": 256, - "max_groups": 256, // max groups should increase as allowed group lifetime increases - - "gravity": [0, 0, -9.8], - "timing": false, - - // Initial State - "X" : { - "W" : [0, 0, 0], - "T" : [0, 0, 0], - "V" : [0, 0, 0], - "bg" : [0, 0, 0], - "ba" : [0, 0, 0], - "Wbc" : [[1, 0, 0], - [0, 1, 0], - [0, 0, 1]], - "Tbc": [0, 0, 0], - "Wg" : [0, 0] - }, - - "P" : { - "W" : 0.001, - "T" : 0.01, - "V" : 0.5, - "bg" : 0.000, - "ba" : 0.00, - "Wbc" : 0.00001, - "Tbc" : 0.0001, - "Wg" : 0.01 - }, - - "Qmodel" : { - "W" : 0.000, - "T" : 0.0, - "V" : 0.0, - "wb" : 0.0000, - "ab" : 0.000, - "Wbc" : 0.000, - "Tbc" : 0.0, - "Wg" : 0.000 - }, - - // // 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 - }, - - // initial std on feature state - "initial_z": 5.5, // meter - "initial_std_x": 0.5, // pixel - "initial_std_y": 0.5, // pixel - "initial_std_z": 2.0, // meter - - // std of visuale measurement, in pixels - "visual_meas_std": 1.5, - "subfilter_visual_meas_std": 1.5, - "max_depth": 10.0, - "min_depth": 0.05, - "max_depth_init_steps": 5, - - "use_depth_opt": true, - "depth_opt_iters": 5, - "depth_opt_eps": 0.001, - "depth_opt_damping": 1e-3, - - "feature_P0_damping": 10.0, - "inn_thresh": 64, - "MH_thresh": 8.991, - - "imu_calib": { - "Cas": [1, 1, 1], - "Car": [[1, 0, 0], - [0, 1, 0], - [0, 0, 1]], - "Cgs": [1, 1, 1], - "Cgr": [[1, 0, 0], - [0, 1, 0], - [0, 0, 1]] - }, - "imu_init_counter": 5, - - "camera_cfg": { - "model": "pinhole", - "rows": 240, - "cols": 320, - "fx": 277.12812921, - "fy": 289.70562748, - "cx": 160, - "cy": 120, - "comment": "pinhole" - }, - - "tracker_cfg": { - "use_prediction": true, - "mask_size": 15, - "num_features_min": 60, - "num_features_max": 90, - "normalize": false, - - "OpticalFlow": { - "win_size": 7, - "max_level": 5, - "max_iter": 15, - "eps": 1e-4, - "threshold": 20 - }, - - // somehow, only BRISK and FAST work well with the generic FeatureDetector interface (Version 2.4.9) - "DetectorType": "FAST", - - "FAST": { - "threshold": 20, - "nonmaxSuppression": true - }, - - "GridAdapter": { - "grid_rows": 4, - "grid_cols": 4 - } - } - -} diff --git a/cfg/estimator_sim.json b/cfg/estimator_sim.json deleted file mode 100644 index c4473710..00000000 --- a/cfg/estimator_sim.json +++ /dev/null @@ -1,77 +0,0 @@ -{ - "simulation": true, - "gravity": [0, 0, -9.8], - // Initial State - "X" : { - "W" : [0, 0, 0], - "T" : [0, 0, 0], - "V" : [0, 0, 0], - "bg" : [0, 0, 0], - "ba" : [0, 0, 0], - "Wbc" : [[-0.99952504, 0.00750192, -0.02989013], - [0.02961534, -0.03439736, -0.99896935], - [-0.00852233, -0.99938008, 0.03415885]], - "Tbc": [0.04557484, -0.0711618, -0.04468125], - "Wg" : [0, 0] - }, - - "P" : { - "W" : 0.0000, - "T" : 0.000, - "V" : 0.0, - "bg" : 0.005, - "ba" : 0.05, - "Wbc" : 0.005, - "Tbc" : 0.05, - "Wg" : 0.0 - }, - - "Qmodel" : { - "W" : 0.000, - "T" : 0.0, - "V" : 0.0, - "wb" : 0.0000, - "ab" : 0.000, - "Wbc" : 0.000, - "Tbc" : 0.0, - "Wg" : 0.000 - }, - - "Qimu": { - "gyro": 3e-3, - "accel":3e-2 - }, - - // initial std on feature state - "initial_z": 2.0, // meter - "initial_std_x": 0.1, // pixel - "initial_std_y": 0.1, // pixel - "initial_std_z": 1.0, // meter - - // std of visuale measurement, in pixels - "visual_meas_std": 1.5, - "subfilter_visual_meas_std": 1.5, - "max_depth": 5.0, - "min_depth": 0.05, - "max_depth_init_steps": 2, - "depth_opt_iters": 10, - "depth_opt_eps": 0.001, - "depth_opt_damping": 1e-4, - "inn_thresh": 20, - - - "imu_calib": { - "Cas": [1, 1, 1], - "Car": [[1, 0, 0], - [0, 1, 0], - [0, 0, 1]], - "Cgs": [1, 1, 1], - "Cgr": [[1, 0, 0], - [0, 1, 0], - [0, 0, 1]] - }, - "imu_init_counter": 10, - - "camera_cfg": "../cfg/equidistant.json", // atan - "tracker_cfg": "../cfg/tracker.json" -} diff --git a/cfg/lyapunov_baseline.json b/cfg/lyapunov_baseline.json deleted file mode 100644 index da685893..00000000 --- a/cfg/lyapunov_baseline.json +++ /dev/null @@ -1,156 +0,0 @@ -{ - // verbose - "simulation": false, - "print_timer": false, - - // algorithmic-level knobs - "update_method": "ekf", - "integration_method": "RK4", // RK4, ForwardEuler - "RK4_stepsize": 0.002, - "use_OOS": false, // update with Out-Of-State features - "use_depth_opt": true, // depth optimization - "use_MH_gating": true, - "use_1pt_RANSAC": false, - "use_compression": true, // measurement compression - "compression_trigger_ratio": 1.5, - "use_discrete_covariance_propagation": true, - - // memory - "max_features": 256, - "max_groups": 256, // max groups should increase as allowed group lifetime increases - "max_group_lifetime": 120, - - // gravity constant - "gravity": [0, 0, -9.8], - // Initial State - "X" : { - "W" : [0, 0, 0], - "T" : [0, 0, 0], - "V" : [0, 0, 0], - "bg" : [0, 0, 0], - "ba" : [0, 0, 0], - "Wbc" : [[-0.99952504, 0.00750192, -0.02989013], - [0.02961534, -0.03439736, -0.99896935], - [-0.00852233, -0.99938008, 0.03415885]], - "Tbc": [0.04557484, -0.0711618, -0.04468125], - "Wg" : [0, 0, 0] - }, - - "P" : { - "W" : 0.0001, - "T" : 0.001, - "V" : 0.5, - "bg" : 0.0001, - "ba" : 0.001, - "Wbc" : 0.00001, - "Tbc" : 0.001, - "Wg" : 0.01 - }, - - "Qmodel" : { - "W" : 0.000, - "T" : 0.0, - "V" : 0.0, - "wb" : 0.0000, - "ab" : 0.000, - "Wbc" : 0.000, - "Tbc" : 0.0, - "Wg" : 0.000 - }, - - // original block without numerical integration of Lyapunov - "Qimu": { - "gyro": 0.5e-3, - "gyro_bias": 1e-4, - "accel":1e-2, - "accel_bias": 1e-1 - }, - - // initial std on feature state - "initial_z": 2.5, // meter - "initial_std_x": 0.5, // pixel - "initial_std_y": 0.5, // pixel - "initial_std_z": 2.0, // meter - - // std of visuale measurement, in pixels - "visual_meas_std": 1.5, - "oos_meas_std": 3.5, - "subfilter_visual_meas_std": 3.5, - "max_depth": 5.0, - "min_depth": 0.05, - - - // depth sub-filter setting - "subfilter_ready_steps": 5, - "subfilter_MH_thresh": 8.991, - - "depth_opt_iters": 5, - "depth_opt_eps": 0.001, - "depth_opt_damping": 1e-3, - - "feature_P0_damping": 1000.0, - - "imu_calib": { - "Cas": [1, 1, 1], - "Car": [[1, 0, 0], - [0, 1, 0], - [0, 0, 1]], - "Cgs": [1, 1, 1], - "Cgr": [[1, 0, 0], - [0, 1, 0], - [0, 0, 1]] - }, - "imu_init_counter": 20, - - "camera_cfg": { - "model": "equidistant", - "rows": 512, - "cols": 512, - "fx": 190.97847715128717, - "fy": 190.9733070521226, - "cx": 254.93170605935475, - "cy": 256.8974428996504, - "k0123": [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182], - "max_iter": 25, - "comment": "512-cam0" - }, - - "min_inliers": 5, // minimum number of inlier measurements - - "MH_thresh": 8.991, // 8.991 - "MH_adjust_factor": 1.5, - - "1pt_RANSAC_thresh": 1.5, - "1pt_RANSAC_prob": 0.95, - "1pt_RANSAC_Chi2": 5.89, - - "tracker_cfg": { - "use_prediction": true, - "mask_size": 15, - "num_features_min": 60, - "num_features_max": 90, - "normalize": true, - - "OpticalFlow": { - "win_size": 15, - "max_level": 5, - "max_iter": 15, - "eps": 1e-4, - "threshold": 20 - }, - - // somehow, only BRISK and FAST work well with the generic FeatureDetector interface (Version 2.4.9) - "DetectorType": "FAST", - - "FAST": { - "threshold": 20, - "nonmaxSuppression": true - }, - - "GridAdapter": { - "grid_rows": 4, - "grid_cols": 4 - } - } - -} diff --git a/cfg/pointgrey_xsens.json b/cfg/pointgrey_xsens.json deleted file mode 100644 index 5131dc03..00000000 --- a/cfg/pointgrey_xsens.json +++ /dev/null @@ -1,228 +0,0 @@ -{ - // verbose - "simulation": false, - "print_timer": false, - "use_canvas": true, - - // 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_MH_gating": true, - "use_1pt_RANSAC": false, - "use_compression": true, // measurement compression - "compression_trigger_ratio": 1.5, - "max_group_lifetime": 6, - - "PrinceDormand": { - "control_stepsize": false, - "tolerance": 1e-15, - "attempts": 12, - "min_scale_factor": 0.125, - "max_scale_factor": 4.0, - "stepsize": 0.002 - }, - - "RK4": { - "stepsize": 0.002 - }, - - // memory - "memory": { - "max_features": 256, - "max_groups": 256 // max groups should increase as allowed group lifetime increases - }, - - - // gravity constant - "gravity": [0, 0, -9.8], - // Initial State - "X" : { - "W" : [0, 0, 0], - "T" : [0, 0, 0], - "V" : [0, 0, 0], - - "bg" : [-0.05163, 0.0108186, 0.0188156], - "ba" : [-0.00342173, -0.00861755, 0.0218768], - "Wbc" : [[4.01e-02, -4.24e-02, 9.98e-01], - [-9.99e-01, -2.05e-03, 4.00e-02], - [3.50e-04, -9.99e-01, -4.25e-02]], - "Tbc": [0.0737, -0.00977, -0.1088], - "Wg" : [0, 0, 0] - }, - - "P" : { - "W" : 0.0001, - "T" : 0.001, - "V" : 0.5, - "bg" : 0.001, - "ba" : 0.01, - "Wbc" : 0.001, - "Tbc" : 0.01, - "Wg" : 0.01 - }, - - "Qmodel" : { - "W" : 0.000, - "T" : 0.0, - "V" : 0.0, - "wb" : 0.0000, - "ab" : 0.000, - "Wbc" : 0.000, - "Tbc" : 0.0, - "Wg" : 0.000 - }, - - "Qimu": { - "gyro": 24.0e-4, - "gyro_bias": 6.6e-5, - "accel": 4.2e-2, - "accel_bias": 25.8e-4 - }, - - - // initial std on feature state - "initial_z": 3.5, // meter - "initial_std_x": 1.5, // pixel - "initial_std_y": 1.5, // pixel - "initial_std_z": 2.0, // meter - - // std of visuale measurement, in pixels - "visual_meas_std": 3.5, - "subfilter_visual_meas_std": 3.5, - "oos_meas_std": 3.5, - "max_depth": 8.0, - "min_depth": 0.05, - - - // depth sub-filter setting - "subfilter": { - "ready_steps": 5, - "MH_thresh": 8.991 - }, - - "depth_opt": { - "max_iters": 5, - "eps": 1e-5, - "damping_factor": 1e-3, - "adjust_factor": 5.0, - "max_res_norm": 5.0 // maximal norm of per observation residuals - }, - - "feature_P0_damping": 10.0, - - "imu_calib": { - "Cas": [1.00056, 1.00062, 1.00136], - "Car": [[1, -0.000914275, -0.000414882], - [0, 1, -0.0000350183], - [0, 0, 1]], - "Cgs": [0.999823, 1.00412, 0.987355], - "Cgr": [[1, -0.00224194, 0.0120628], - [0.00306442, 1, -0.000998032], - [0.00285738, -0.0154485, 1]] - }, - "imu_init_counter": 20, - - "camera_cfg": { - "model": "atan", - "rows": 500, - "cols": 960, - "fx": 0.561859, - "fy": 1.081848, - "cx": 0.491896, - "cy": 0.5151548, - "w": 0.709402, - "comment": "intrinsics converted from relative to absolute values" - }, - - "min_inliers": 5, // minimum number of inlier measurements - - "MH_thresh": 5.991, // 8.991 - "MH_adjust_factor": 1.5, - - "1pt_RANSAC_thresh": 1.5, - "1pt_RANSAC_prob": 0.95, - "1pt_RANSAC_Chi2": 5.89, - - "tracker_cfg": { - "use_prediction": true, - "mask_size": 15, - "margin": 16, // image boundary to mask out - "num_features_min": 60, - "num_features_max": 90, - "max_pixel_displacement": 128, - "normalize": false, - - "KLT": { - "win_size": 15, - "max_level": 5, - "max_iter": 15, - "eps": 1e-2 - }, - - "extract_descriptor": false, - "descriptor_distance_thresh": -1, // -1 to disable descriptor check - "default_descriptor": "BRIEF", - - // "detector": "BRISK", - "detector": "FAST", - // "detector": "ORB", - // "detector": "AGAST", - // "detector": "GFTT", - - "FAST": { - // https://docs.opencv.org/3.4/df/d74/classcv_1_1FastFeatureDetector.html - "threshold": 5, - "nonmaxSuppression": true - }, - - "BRISK": { - // https://docs.opencv.org/3.4/de/dbf/classcv_1_1BRISK.html - "thresh": 25, - "octaves": 3, - "patternScale": 1.0 - }, - - "ORB": { - // https://docs.opencv.org/3.3.0/db/d95/classcv_1_1ORB.html - "nfeatures": 500, - "scaleFactor": 1.2, - "nlevels": 4, - "edgeThreshold": 31, - "firstLevel": 0, - "WTA_K": 2, - "patchSize": 31, - "fastThreshold": 20 - }, - - "AGAST": { - // https://docs.opencv.org/3.4/d7/d19/classcv_1_1AgastFeatureDetector.html - "threshold": 10, - "nonmaxSuppression": true - }, - - "GFTT": { - // Good Feature To Track - // https://docs.opencv.org/3.3.0/df/d21/classcv_1_1GFTTDetector.html - "maxCorners": 1000, - "qualityLevel": 0.01, - "minDistance": 1.0, - "blockSize": 3, - "useHarrisDetector": false, - "k": 0.04 - }, - - "BRIEF": { - "bytes": 64, - "use_orientation": false - }, - - "FREAK": { - "orientationNormalized": true, - "scaleNormalized": true, - "patternScale": 22.0, - "nOctaves": 4 - } - } - -} diff --git a/cfg/pointgrey_xsens_viewer.json b/cfg/pointgrey_xsens_viewer.json deleted file mode 100644 index b1f63b58..00000000 --- a/cfg/pointgrey_xsens_viewer.json +++ /dev/null @@ -1,28 +0,0 @@ -{ - "window": { - "height": 1000, - "width": 960 - }, - - "viewport": { - "height": 500, - "width": 960, - "fx": 800, - "fy": 800, - "cx": 480, - "cy": 250, - "znear": 0.1, - "zfar": 10 - }, - - "bg_color": { - "r": 0.0, - "g": 0.0, - "b": 0.0, - "a": 1.0 - }, - "grid_size": 20, - "draw_trace_as_dots": false - -} - diff --git a/cfg/simulation.json b/cfg/simulation.json deleted file mode 100644 index 3d5d6d73..00000000 --- a/cfg/simulation.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "simulator_cfg": "../cfg/simulator.json", - "estimator_cfg": "../cfg/estimator_sim.json", - - "add_noise_to_init_state": true, - "enable_update": true, - "visualize": false -} diff --git a/cfg/simulator.json b/cfg/simulator.json deleted file mode 100644 index f16b2e74..00000000 --- a/cfg/simulator.json +++ /dev/null @@ -1,60 +0,0 @@ -{ - // sensor setups - // total simulation time, in seconds - "total_time": 10, - "imu_hz": 200, - "camera_hz": 30, - "num_pts_to_track": 120, - "min_num_pts_to_track": 90, - "motion_type": 2, - "init_static_period": 1, - - "fixed_seed": true, - "seed": 0, - - // measurement noise - "gyro_noise_std": 1e-3, - "accel_noise_std": 1e-2, - "tracking_noise_std": 0.1, // in pixels - "max_track_lifetime": 10, - - // initial standard deviation added to states - "init_noise_std": { - "W": 0.0, - "T": 0.0, - "V": 0.0, - "bg": 0.001, - "ba": 0.01, - "Wbc": 0.001, - "Tbc": 0.01, - "Wg": 0.0 - }, - - "X" : { - "W" : [0, 0, 0], - "T" : [0, 0, 0], - "V" : [0, 0, 0], - "wb" : [0, 0, 0], - "ab" : [0, 0, 0], - "Wbc" : [3.1319, 0.00905, 0.00800], - "Tbc": [-0.0162997, -0.00375004, -0.0383651], - "Wg" : [0, 0], - "to" : 0.0 - }, - - // camera latency - "delay_camera_time": 0.00, - - // gravity - "gravity_magnitude": 9.8, - - // camera calibration - // "camera_cfg": "../cfg/atan.json", - "camera_cfg": "../cfg/equidistant.json", - - "z_near": 0.1, - "z_far": 5.0, - "visualize_tracks": false, - "track_drop_probability": 0.005, - "mask_size": 15.0 -} diff --git a/cfg/tuning.json b/cfg/tuning.json deleted file mode 100644 index adeb3d6c..00000000 --- a/cfg/tuning.json +++ /dev/null @@ -1,155 +0,0 @@ -{ - // verbose - "simulation": false, - "print_timer": false, - - // algorithmic-level knobs - "update_method": "ekf", - "integration_method": "RK4", // RK4, ForwardEuler - "RK4_stepsize": -1, //0.002, - "use_OOS": false, // update with Out-Of-State features - "use_depth_opt": true, // depth optimization - "use_MH_gating": true, - "use_1pt_RANSAC": false, - "use_compression": true, // measurement compression - "compression_trigger_ratio": 1.5, - - // memory - "max_features": 256, - "max_groups": 256, // max groups should increase as allowed group lifetime increases - "max_group_lifetime": 120, - - // gravity constant - "gravity": [0, 0, -9.8], - // Initial State - "X" : { - "W" : [0, 0, 0], - "T" : [0, 0, 0], - "V" : [0, 0, 0], - "bg" : [0, 0, 0], - "ba" : [0, 0, 0], - "Wbc" : [[-0.99952504, 0.00750192, -0.02989013], - [0.02961534, -0.03439736, -0.99896935], - [-0.00852233, -0.99938008, 0.03415885]], - "Tbc": [0.04557484, -0.0711618, -0.04468125], - "Wg" : [0, 0, 0] - }, - - "P" : { - "W" : 0.0001, - "T" : 0.001, - "V" : 0.5, - "bg" : 0.001, - "ba" : 0.001, - "Wbc" : 0.00001, - "Tbc" : 0.001, - "Wg" : 0.01 - }, - - "Qmodel" : { - "W" : 0.000, - "T" : 0.0, - "V" : 0.0, - "wb" : 0.0000, - "ab" : 0.000, - "Wbc" : 0.000, - "Tbc" : 0.0, - "Wg" : 0.000 - }, - - // original block without numerical integration of Lyapunov - "Qimu": { - "gyro": 0.5e-3, - "gyro_bias": 1e-4, - "accel":1e-2, - "accel_bias": 1e-1 - }, - - // initial std on feature state - "initial_z": 2.5, // meter - "initial_std_x": 0.5, // pixel - "initial_std_y": 0.5, // pixel - "initial_std_z": 2.0, // meter - - // std of visuale measurement, in pixels - "visual_meas_std": 1.5, - "oos_meas_std": 3.5, - "subfilter_visual_meas_std": 3.5, - "max_depth": 5.0, - "min_depth": 0.05, - - - // depth sub-filter setting - "subfilter_ready_steps": 5, - "subfilter_MH_thresh": 8.991, - - "depth_opt_iters": 5, - "depth_opt_eps": 0.001, - "depth_opt_damping": 1e-3, - - "feature_P0_damping": 1000.0, - - "imu_calib": { - "Cas": [1, 1, 1], - "Car": [[1, 0, 0], - [0, 1, 0], - [0, 0, 1]], - "Cgs": [1, 1, 1], - "Cgr": [[1, 0, 0], - [0, 1, 0], - [0, 0, 1]] - }, - "imu_init_counter": 20, - - "camera_cfg": { - "model": "equidistant", - "rows": 512, - "cols": 512, - "fx": 190.97847715128717, - "fy": 190.9733070521226, - "cx": 254.93170605935475, - "cy": 256.8974428996504, - "k0123": [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182], - "max_iter": 25, - "comment": "512-cam0" - }, - - "min_inliers": 5, // minimum number of inlier measurements - - "MH_thresh": 8.991, // 8.991 - "MH_adjust_factor": 1.5, - - "1pt_RANSAC_thresh": 1.5, - "1pt_RANSAC_prob": 0.95, - "1pt_RANSAC_Chi2": 5.89, - - "tracker_cfg": { - "use_prediction": true, - "mask_size": 15, - "num_features_min": 60, - "num_features_max": 90, - "normalize": true, - - "OpticalFlow": { - "win_size": 15, - "max_level": 5, - "max_iter": 15, - "eps": 1e-4, - "threshold": 20 - }, - - // somehow, only BRISK and FAST work well with the generic FeatureDetector interface (Version 2.4.9) - "DetectorType": "FAST", - - "FAST": { - "threshold": 20, - "nonmaxSuppression": true - }, - - "GridAdapter": { - "grid_rows": 4, - "grid_cols": 4 - } - } - -} diff --git a/cfg/viewer_scenenet.json b/cfg/viewer_scenenet.json deleted file mode 100644 index ea5d42c3..00000000 --- a/cfg/viewer_scenenet.json +++ /dev/null @@ -1,27 +0,0 @@ -{ - "window": { - "height": 240, - "width": 640 - }, - - "viewport": { - "width": 320, - "height": 240, - "fx": 400, - "fy": 400, - "cx": 160, - "cy": 120, - "znear": 0.1, - "zfar": 10 - }, - - "bg_color": { - "r": 0.0, - "g": 0.0, - "b": 0.0, - "a": 1.0 - }, - "grid_size": 20 - -} - diff --git a/experimental/CMakeLists.txt b/experimental/CMakeLists.txt deleted file mode 100644 index 5b87d8bc..00000000 --- a/experimental/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -# TODO (xfei): fill in details -################################################################################ -# EXPERIMENTAL -################################################################################ - -# add_executable(inproc_zmq experimental/inproc_communication_zmq.cpp) -# target_link_libraries(inproc_zmq zmq pthread glog) -# -# add_executable(async_dataloader experimental/async_dataloader.cpp) -# target_link_libraries(async_dataloader zmq pthread glog msckf) - -add_executable(test_camera_factory camera_factory.h test_camera_factory.cpp) - - diff --git a/experimental/async_dataloader.cpp b/experimental/async_dataloader.cpp deleted file mode 100644 index dfc68442..00000000 --- a/experimental/async_dataloader.cpp +++ /dev/null @@ -1,195 +0,0 @@ -// Asynchronous dataloader. -// Data acquisition (loader or multiple sensors) lives in its own node (might be multiple nodes), and dispatches messages to the estimator node. This file implements a dataloader node. -// Author: Xiaohan Fei -#include - -#include "zmq.hpp" -#include "glog/logging.h" - -#include "opencv2/core/core.hpp" -#include "opencv2/highgui/highgui.hpp" - -#include "tumvi.h" -#include "helpers.h" - -static std::string tumvi_root = "/home/feixh/Data/tumvi/exported/euroc/512_16/"; -static std::string euroc_root = "/home/feixh/Data/euroc/"; - - -namespace feh { -namespace node { - -// packet for communication -struct Packet { - Packet(): type_{UNINITIALIZED}, image_ptr_{nullptr} {} - - Packet(uint64_t ts, - ftype rx, ftype ry, ftype rz, - ftype tx, ftype ty, ftype tz): - type_{IMU}, ts_{ts}, - rx_{rx}, ry_{ry}, rz_{rz}, - tx_{tx}, ty_{ty}, tz_{tz}, - image_ptr_{nullptr} - {} - - Packet(uint64_t ts, - cv::Mat *ptr): - type_{IMAGE_PTR}, ts_{ts}, - image_ptr_{ptr} - {} - - enum: uint8_t { - UNINITIALIZED = 0, - IMAGE_PTR = 1, - IMU = 2 - } type_; - - uint64_t ts_; - ftype rx_, ry_, rz_, tx_, ty_, tz_; - cv::Mat *image_ptr_; -}; - - -class DataServer { -public: - DataServer(zmq::context_t &context, const std::string &port): - pub_{context, ZMQ_PUB}, - loader_{nullptr} - { - try { - pub_.bind(port); - LOG(INFO) << "Bound to port: " << port; - } catch (const std::exception &e) { - LOG(FATAL) << "Failed to bind to port: " << port; - } - } - - void Initialize(const std::string dataset, - const std::string root, - const std::string seq, - int cam_id) { - std::string image_dir, imu_dir, gt_dir; - std::tie(image_dir, imu_dir, gt_dir) = GetDirs(dataset, root, seq, cam_id); - loader_ = std::unique_ptr(new TUMVILoader{image_dir, imu_dir}); - LOG(INFO) << "#entries=" << loader_->size() << std::endl; - } - - void run() { - if (!loader_) { - LOG(FATAL) << "Empty loader!"; - } - - for (int i = 0; i < loader_->size(); ++i) { - msg::Generic entry = loader_->Get(i); - - // serialize and send the actual message - Packet packet; - if (entry.type_ == msg::Generic::IMAGE) { - cv::Mat *image_ptr = new cv::Mat(cv::imread(entry.image_path_)); - packet = std::move(Packet{entry.ts_, image_ptr}); - } else if (entry.type_ == msg::Generic::IMU) { - packet = std::move(Packet{entry.ts_, - entry.gyro_[0], entry.gyro_[1], entry.gyro_[2], - entry.accel_[0], entry.accel_[1], entry.accel_[2]}); - } - - zmq::message_t msg(sizeof(packet)); - memcpy(msg.data(), &packet, sizeof(packet)); - pub_.send(msg); - LOG(INFO) << "Sent " << i+1 << " messages"; - } - } - -private: - zmq::socket_t pub_; - std::unique_ptr loader_; -}; - -class MessageEchoClient { -public: - - MessageEchoClient(zmq::context_t &context, const std::string &port): - sub_{context, ZMQ_SUB}, - msg_counter_{0} - { - try { - sub_.connect(port); - LOG(INFO) << "Connected to port: " << port; - } catch (const std::exception &e) { - LOG(FATAL) << "Failed to connect to port: " << port; - } - sub_.setsockopt(ZMQ_SUBSCRIBE, "", 0); - // static_cast required due to zmq bug in C++11 - items_.push_back({static_cast(sub_), 0, ZMQ_POLLIN, 0}); - } - - void run() { - while (true) { - zmq::message_t rx_msg; - zmq::poll(&items_[0], 1, -1); // http://api.zeromq.org/2-1:zmq-poll - if (items_[0].revents & ZMQ_POLLIN) { // revents == received events, make sure the revents is ZMQ_POLLIN (read) - sub_.recv(&rx_msg); - - // receive message - Packet packet; - memcpy(&packet, rx_msg.data(), rx_msg.size()); - if (packet.type_ == Packet::IMU) { - std::cout << packet.ts_ << " " << packet.rx_ << " " << packet.ry_ << " " << packet.rz_ << " " - << packet.tx_ << " " << packet.ty_ << " " << packet.tz_ << std::endl; - } else if (packet.type_ == Packet::IMAGE_PTR) { - if (packet.image_ptr_) { - cv::Mat image{packet.image_ptr_->clone()}; - delete packet.image_ptr_; - - cv::imshow("disp", image); - char ckey = cv::waitKey(10); - } else { - LOG(FATAL) << "Empty image pointer"; - } - } else { - LOG(FATAL) << "UN-INITIALIZED PACKET"; - } - - ++msg_counter_; - LOG(INFO) << "Received " << msg_counter_ << " messages"; - } - } - } - - zmq::socket_t sub_; - std::vector items_; - int msg_counter_; - // TODO (xfei): implement a thread-safe message buffer -}; - -} // namespace node - -} // namespace feh - - -// some constants -static const std::string dataset{"tumvi"}; -static const std::string root{"/home/feixh/Data/tumvi/exported/euroc/512_16/"}; -static const std::string seq{"room1"}; -static const int cam_id{0}; - -int main() -{ - zmq::context_t context(0); - std::string port("inproc://7007"); - - // NOTE: launch subscriber first - feh::node::MessageEchoClient client(context, port); - std::thread sub_thread(&feh::node::MessageEchoClient::run, &client); - - feh::node::DataServer loader(context, port); - loader.Initialize(dataset, root, seq, cam_id); - std::thread pub_thread(&feh::node::DataServer::run, &loader); - - // join - pub_thread.join(); - sub_thread.join(); - - - -} diff --git a/experimental/camera_factory.h b/experimental/camera_factory.h deleted file mode 100644 index d903e6ce..00000000 --- a/experimental/camera_factory.h +++ /dev/null @@ -1,438 +0,0 @@ -/* Curiously Recurring Template Pattern (CRTP) implementation - * of camera models. - * Problem is now we have multiple different base camera classes: - * Each base camera type with a specific derived camera type as - * the template parameter is a different type. - */ -#pragma once -#include -#include -#include "Eigen/Core" - -// TODO (xfei): separate model and factory -#include "json/json.h" -#include "utils.h" - -namespace feh { - -template -class BaseCamera_CRTP { -public: - BaseCamera_CRTP(int rows, int cols, T fx, T fy, T cx, T cy): - rows_{rows}, cols_{cols}, - fx_{fx}, fy_{fy}, cx_{cx}, cy_{cy} - {} - BaseCamera_CRTP& operator=(const BaseCamera_CRTP &) = delete; - BaseCamera_CRTP(const BaseCamera_CRTP &) = delete; - ~BaseCamera_CRTP() { - } - - template - Eigen::Matrix - Project(const Eigen::MatrixBase &xc, - Eigen::Matrix *jac=nullptr) const { - return static_cast(this)->Project(xc, jac); - } - - template - Eigen::Matrix - UnProject(const Eigen::MatrixBase &xp, - Eigen::Matrix *jac=nullptr) const { - return static_cast(this)->UnProject(xp, jac); - } - - virtual void Print(std::ostream &out) const = 0; - - int rows() const { return rows_; } - int cols() const { return cols_; } - T cx() const { return cx_; } - T cy() const { return cy_; } - T fx() const { return fx_; } - T fy() const { return fy_; } - -protected: - int rows_, cols_; - T fx_, fy_, cx_, cy_; -}; - -template -class ATANCamera: public BaseCamera_CRTP> { -public: - ATANCamera(int rows, int cols, T fx, T fy, T cx, T cy, T w): - BaseCamera_CRTP>{rows, cols, fx, fy, cx, cy}, - w_(w), invw_(1.0 / w), tanw2term_(2.0*std::tan(w * 0.5)) - {} - - template - Eigen::Matrix - Project(const Eigen::MatrixBase &xc, - Eigen::Matrix *jac=nullptr) const - { - std::cout << "atan project" << std::endl; - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1); - using f_t = typename Derived::Scalar; - - Eigen::Matrix xp; - - f_t R = xc.norm(); - f_t f; - if (R < 0.0001 || w_ == 0) { - f = 1; - } else { - f = invw_ * atan(tanw2term_ * R) / R; - } - - //Project through distortion model - xp(0) = fx_ * f * xc(0) + cx_; - xp(1) = fy_ * f * xc(1) + cy_; - - if (jac != nullptr) { - // compute jacobians - if (R < 0.0001 || w_ == 0) { - (*jac)(0, 0) = fx_; - (*jac)(1, 1) = fy_; - } else { - // FIXME: optimize computation - f_t df_dx, df_dy, df_dR; - f_t a = tanw2term_ * R; - df_dR = invw_ * ( 1./(1+a*a)*a - std::atan(a) ) / R / R; - df_dx = df_dR * xc(0) / R; - df_dy = df_dR * xc(1) / R; - - *jac << fx_ * f + fx_ * xc(0) * df_dx, fx_ * xc(0) * df_dy, - fy_ * xc(1) * df_dx, fy_ * f + fy_ * xc(1) * df_dy; - } - } - return xp; - } - - template - Eigen::Matrix - UnProject(const Eigen::MatrixBase &xp, - Eigen::Matrix *jac=nullptr) const - { - std::cout << "atan un-project" << std::endl; - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1); - - using f_t = typename Derived::Scalar; - Eigen::Matrix xc; - - Eigen::Matrix tmp((xp(0) - cx_) / fx_, (xp(1) - cy_) / fy_); - f_t R = tmp.norm(); - f_t RR{w_ == 0 ? R : std::tan(R * w_) / tanw2term_}; - f_t f{R > 0.01 ? RR / R : 1.0}; - xc = f * tmp; - - if (jac != nullptr) { - if (f == 1) { - (*jac)(0, 0) = 1.0 / fx_; - (*jac)(1, 1) = 1.0 / fy_; - } else { - f_t df_dR; - f_t a = std::tan(R * w_); - df_dR = 1.0 / tanw2term_ * (((1+a*a)*w_*R - a) / R / R); - - f_t df_dx, df_dy; - df_dx = df_dR * tmp(0) / R / fx_; - df_dy = df_dR * tmp(1) / R / fy_; - - (*jac) << tmp(0) * df_dx + f / fx_, tmp(0) * df_dy, - tmp(1) * df_dx, tmp(1) * df_dy + f / fy_; - } - } - return xc; - } - - void Print(std::ostream &out) const - { - out << "ATAN Camera" << std::endl - << "[rows, cols]=" << rows_ << "," << cols_ << "]" << std::endl - << "[fx, fy, cx, cy, w]=[" - << fx_ << "," << fy_ << "," << cx_ << "," << cy_ << "," << w_ << "]" << std::endl; - } - -private: - using BaseCamera_CRTP>::rows_; - using BaseCamera_CRTP>::cols_; - using BaseCamera_CRTP>::fx_; - using BaseCamera_CRTP>::fy_; - using BaseCamera_CRTP>::cx_; - using BaseCamera_CRTP>::cy_; - - T w_, invw_, tanw2term_; -}; - - - // static - // ATANCamera* Create(const Json::Value &cfg) - // { - // auto cam_model = cfg["model"].asString(); - // int rows = cfg["rows"].asInt(); - // int cols = cfg["cols"].asInt(); - // T fx = cfg["fx"].asDouble(); - // T fy = cfg["fy"].asDouble(); - // T cx = cfg["cx"].asDouble(); - // T cy = cfg["cy"].asDouble(); - // T w = cfg["w"].asDouble(); - // auto cam = new ATANCamera( - // rows, cols, - // cols * fx, rows * fy, cols * cx, rows * cy, w); - // return cam; - // } - - -/* -template -class EquidistantCamera: public BaseCamera_CRTP { -public: - EquidistantCamera( int rows, int cols, - T fx, T fy, T cx, T cy, - T k0, T k1, T k2, T k3, - int max_iter=15): - BaseCamera_CRTP{rows, cols, fx, fy, cx, cy}, - k0_{k0}, k1_{k1}, k2_{k2}, k3_{k3}, - max_iter_{max_iter} {} - - - template - Eigen::Matrix Project( - const Eigen::MatrixBase &xc, - Eigen::Matrix *jac=nullptr) const - { - std::cout << "equidistant project" << std::endl; - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1); - using f_t = typename Derived::Scalar; - Eigen::Matrix xp; - - f_t xy_norm2 = xc.squaredNorm(); - f_t xy_norm= sqrt( xy_norm2 ); - f_t xyz_norm2 = xy_norm2 + 1; - - f_t th = atan2( xy_norm, 1.0 ); - - f_t phi = atan2( xc[1], xc[0] ); - - f_t th2 = th * th; - f_t th3 = th2 * th; - f_t th4 = th3 * th; - f_t th5 = th3 * th2; - f_t th6 = th5 * th; - f_t th7 = th5 * th2; - f_t th8 = th7 * th; - f_t th9 = th7 * th2; - f_t r = th + k0_ * th3 + k1_ * th5 + k2_ * th7 + k3_ * th9; - - f_t cos_phi = cos( phi ); - f_t sin_phi = sin( phi ); - - f_t u = fx_ * r * cos_phi + cx_; - f_t v = fy_ * r * sin_phi + cy_; - - // fill in xp - xp[0] = u; - xp[1] = v; - - - if (jac != nullptr) { - f_t dphi_dx = -xc[1] / xy_norm2; - f_t dphi_dy = xc[0] / xy_norm2; - - f_t dth_dx = xc[0] / xyz_norm2 / xy_norm; - f_t dth_dy = xc[1] / xyz_norm2 / xy_norm; - - f_t dr_dth = 1 + k0_ * 3 * th2 + k1_ * 5 * th4 + k2_ * 7 * th6 + k3_ * 9 * th8; - - f_t du_dx = fx_ * cos_phi * dr_dth * dth_dx - fx_ * r * sin_phi * dphi_dx; - f_t du_dy = fx_ * cos_phi * dr_dth * dth_dy - fx_ * r * sin_phi * dphi_dy; - - f_t dv_dx = fy_ * sin_phi * dr_dth * dth_dx + fy_ * r * cos_phi * dphi_dx; - f_t dv_dy = fy_ * sin_phi * dr_dth * dth_dy + fy_ * r * cos_phi * dphi_dy; - - // fill in jacobians - (*jac) << du_dx, du_dy, dv_dx, dv_dy; - } - return xp; - } - - template - Eigen::Matrix - UnProject(const Eigen::MatrixBase &xp, - Eigen::Matrix *jac=nullptr) const - { - std::cout << "equidistant un-project" << std::endl; - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1); - using f_t = typename Derived::Scalar; - using Vec2 = Eigen::Matrix; - Vec2 xc; - - f_t xn = xp[0] - cx_; - f_t yn = xp[1] - cy_; - - f_t b(fx_ * yn), a(fy_ * xn); - f_t phi = atan2( b, a ); - f_t cos_phi = cos( phi ); - f_t sin_phi = sin( phi ); - - f_t rth = xn / ( fx_ * cos_phi ); - - f_t th = rth; - // solve th: - // th + k0*th**3 + k1*th**5 + k2*th**7 + k3*th**9 = rth - - f_t th2, th3, th4, th6, x0, x1; - for ( int i = 0; i < max_iter_; i++ ) { - // f = (th + k0*th**3 + k1*th**5 + k2*th**7 + k3*th**9 - rth)^2 - th2 = th * th; - th3 = th2 * th; - th4 = th2 * th2; - th6 = th4 * th2; - x0 = k0_ * th3 + k1_ * th4 * th + k2_ * th6 * th + k3_ * th6 * th3 - rth + th; - x1 = 3 * k0_ * th2 + 5 * k1_ * th4 + 7 * k2_ * th6 + 9 * k3_ * th6 * th2 + 1; - f_t d = 2 * x0 * x1; - f_t d2 = 4 * th * x0 * ( 3 * k0_ + 10 * k1_ * th2 + 21 * k2_ * th4 + 36 * k3_ * th6 ) + 2 * x1 * x1; - f_t delta = d / d2; - th -= delta; - } - f_t tan_th = tan( th ); - xc[0] = tan_th * cos_phi; - xc[1] = tan_th * sin_phi; - - if (jac != nullptr) { - f_t a2b2 = a*a+b*b; - Vec2 dphi_dxy( -b / a2b2 * fy_, a / a2b2 * fx_); - Vec2 dcosphi_dxy(-sin_phi * dphi_dxy); - Vec2 dsinphi_dxy(cos_phi * dphi_dxy); - Vec2 drth_dxy(cos_phi-xn*dcosphi_dxy[0], -xn*dcosphi_dxy[1]); - drth_dxy /= (fx_*cos_phi*cos_phi); - - f_t cos_th(cos(th)); - Vec2 dtanth_dxy(drth_dxy / x1 / (cos_th*cos_th)); - Vec2 doutx_dxy(cos_phi * dtanth_dxy + tan_th * dcosphi_dxy); - Vec2 douty_dxy(sin_phi * dtanth_dxy + tan_th * dsinphi_dxy); - (*jac) << doutx_dxy[0], doutx_dxy[1], - douty_dxy[0], douty_dxy[1]; - } - return xc; - } - - void Print(std::ostream &out) - { - out << "Equidistant Camera" << std::endl - << "[rows, cols]=" << rows_ << "," << cols_ << "]" << std::endl - << "[fx, fy, cx, cy]=[" - << fx_ << "," << fy_ << "," << cx_ << "," << cy_ << "]" << std::endl - << "[k0, k1, k2, k3]=[" - << k0_ << "," << k1_ << "," << k2_ << "," << k3_ << "]" << std::endl; - } - -private: - using BaseCamera_CRTP::rows_; - using BaseCamera_CRTP::cols_; - using BaseCamera_CRTP::fx_; - using BaseCamera_CRTP::fy_; - using BaseCamera_CRTP::cx_; - using BaseCamera_CRTP::cy_; - - T k0_, k1_, k2_, k3_; - int max_iter_; -}; - - // static - // EquidistantCamera* Create(const Json::Value &cfg) - // { - // int rows = cfg["rows"].asInt(); - // int cols = cfg["cols"].asInt(); - // T fx = cfg["fx"].asDouble(); - // T fy = cfg["fy"].asDouble(); - // T cx = cfg["cx"].asDouble(); - // T cy = cfg["cy"].asDouble(); - // auto k0123 = GetVectorFromJson(cfg, "k0123"); - // int max_iter = cfg["max_iter"].asInt(); - - // auto cam = new EquidistantCamera( - // rows, cols, - // fx, fy, cx, cy, - // k0123[0], k0123[1], k0123[2], k0123[3], - // max_iter); - // return cam; - // } - -template -class RadialTangentialCamera: public BaseCamera_CRTP { -public: - RadialTangentialCamera(int rows, int cols, - T fx, T fy, T cx, T cy, - T r0, T r1, - T k0, T k1, T k2, - int max_iter=15): - BaseCamera_CRTP{rows, cols, fx, fy, cx, cy}, - r0_{r0}, r1_{r1}, - k0_{k0}, k1_{k1}, k2_{k2}, - max_iter_{max_iter} {} - - template - Eigen::Matrix Project( - const Eigen::MatrixBase &xc, - Eigen::Matrix *jac=nullptr) const - { - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1); - using f_t = typename Derived::Scalar; - using Vec2 = Eigen::Matrix; - Vec2 xp; - return xp; - } - - template - Eigen::Matrix - UnProject(const Eigen::MatrixBase &xp, - Eigen::Matrix *jac=nullptr) const - { - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1); - using f_t = typename Derived::Scalar; - using Vec2 = Eigen::Matrix; - Vec2 xc; - // TODO (xfei): implement - return xc; - } - - void Print(std::ostream &out) - { - // TODO (xfei): implement - } - -private: - using BaseCamera_CRTP::rows_; - using BaseCamera_CRTP::cols_; - using BaseCamera_CRTP::fx_; - using BaseCamera_CRTP::fy_; - using BaseCamera_CRTP::cx_; - using BaseCamera_CRTP::cy_; - - T r0_, r1_; - T k0_, k1_, k2_; - int max_iter_; -}; - - -template -bool OutOfView(T x, T y, const Cam &cam) -{ - float margin(0.01f); - return x < cam.cols()*margin - || x >= cam.cols()*(1-margin) - || y < cam.rows()*margin - || y >= cam.rows()*(1-margin); -} - -template -bool OutOfView(const Eigen::MatrixBase &xp, const Cam &cam) -{ - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1); - return OutOfView(xp(0), xp(1), cam); -} - - -*/ - -} // feh - diff --git a/experimental/inproc_communication_zmq.cpp b/experimental/inproc_communication_zmq.cpp deleted file mode 100644 index a287604a..00000000 --- a/experimental/inproc_communication_zmq.cpp +++ /dev/null @@ -1,76 +0,0 @@ -// Getting familar with ZeroMQ library. -// Author: Xiaohan Fei -// Reference: -// http://zguide.zeromq.org/page:all#sockets-and-patterns -// Example: -// https://ogbe.net/blog/zmq_helloworld.html -// http://zguide.zeromq.org/cpp:mspoller -#include -#include -#include -#include -#include - -#include "zmq.hpp" -#include "glog/logging.h" - -class Node { -public: - - Node(zmq::context_t &context, const std::string &port): - sub_{context, ZMQ_SUB}, - msg_counter_{0} - { - try { - sub_.connect(port); - LOG(INFO) << "Connected to port: " << port; - } catch (const std::exception &e) { - LOG(FATAL) << "Failed to connect to port: " << port; - } - sub_.setsockopt(ZMQ_SUBSCRIBE, "", 0); - // static_cast required due to zmq bug in C++11 - items_.push_back({static_cast(sub_), 0, ZMQ_POLLIN, 0}); - } - - void run(int max_msg) { - while (true) { - zmq::message_t rx_msg; - zmq::poll(&items_[0], 1, -1); // http://api.zeromq.org/2-1:zmq-poll - if (items_[0].revents & ZMQ_POLLIN) { // revents == received events, make sure the revents is ZMQ_POLLIN (read) - sub_.recv(&rx_msg); - std::string rx_str; - rx_str.assign(static_cast(rx_msg.data()), rx_msg.size()); // assign ? - std::cout << "Received: " << rx_str << std::endl; - if (++msg_counter_ == max_msg) { - break; - } - } - } - } - - zmq::socket_t sub_; - std::vector items_; - int msg_counter_; -}; - -int main() -{ - zmq::context_t context(0); - zmq::socket_t publisher(context, ZMQ_PUB); - std::string port("inproc://5566"); - publisher.bind(port); - - Node node(context, port); - - size_t nmsg = 10; - std::thread subs_thread(&Node::run, &node, nmsg); - - for (size_t i = 0; i < nmsg; ++i) { - std::string content("Hello " + std::to_string(i)); - zmq::message_t msg(content.size()); - std::copy(content.c_str(), content.c_str() + content.size(), static_cast(msg.data())); - publisher.send(msg); - } - - subs_thread.join(); -} diff --git a/experimental/test_camera_factory.cpp b/experimental/test_camera_factory.cpp deleted file mode 100644 index 7b4cb590..00000000 --- a/experimental/test_camera_factory.cpp +++ /dev/null @@ -1,17 +0,0 @@ -#include "camera_factory.h" -#include "json/json.h" -#include "Eigen/Dense" - -using namespace feh; -using namespace std; - -int main() -{ - ATANCamera *cam = new ATANCamera(480, 640, 400, 400, 320, 240, 1.0); - Eigen::Vector2f xc(100, 100); - std::cout << "rows=" << cam->rows() << "; cols=" << cam->cols() << std::endl; - cam->Project(xc); - - delete cam; -} - diff --git a/format_all.sh b/misc/format_all.sh similarity index 100% rename from format_all.sh rename to misc/format_all.sh diff --git a/print_stats.sh b/misc/print_stats.sh similarity index 100% rename from print_stats.sh rename to misc/print_stats.sh diff --git a/run_all.sh b/misc/run_all.sh similarity index 100% rename from run_all.sh rename to misc/run_all.sh diff --git a/requirements.txt b/requirements.txt index 876c287a..3965ced8 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,5 +1,4 @@ transforms3d -mayavi numpy glog diff --git a/src/app/simulation.cpp b/src/app/simulation.cpp deleted file mode 100644 index ba125502..00000000 --- a/src/app/simulation.cpp +++ /dev/null @@ -1,131 +0,0 @@ -#include -#include -#include - -#include "absl/strings/str_format.h" -#include "glog/logging.h" -#include "opencv2/highgui/highgui.hpp" -#include "json/json.h" - -#include "core.h" -#include "estimator.h" -#include "simulator.h" - -using namespace feh; - -// \brief: Clone state from simulator to estimator for initialization in -// simulation. -void CloneState(std::shared_ptr est, std::shared_ptr sim, - bool add_noise = false) { - // clone nominal state - est->X_ = sim->X_; - // clone imu calibration - est->Cas_ = sim->Cas_; - est->Car_ = sim->Car_; - est->Cgs_ = sim->Cgs_; - est->Cgr_ = sim->Cgr_; - // clone gravity - est->g_ = sim->g_; - - // now add some noise to initial state - if (add_noise) { - auto n = sim->cfg_["init_noise_std"]; - est->X_.Rsb = - SO3::exp(est->X_.Rsb.log() + RandomVector<3>(0., n["W"].asDouble())); - est->X_.Tsb += RandomVector<3>(0., n["T"].asDouble()); - est->X_.Vsb += RandomVector<3>(0., n["V"].asDouble()); - est->X_.bg += RandomVector<3>(0., n["bg"].asDouble()); - est->X_.ba += RandomVector<3>(0., n["ba"].asDouble()); - est->X_.Rbc = - SO3::exp(est->X_.Rbc.log() + RandomVector<3>(0., n["Wbc"].asDouble())); - est->X_.Tbc += RandomVector<3>(0., n["Tbc"].asDouble()); - Vec3 Wg = est->X_.Rg.log(); - Wg.head<2>() += RandomVector<2>(0., n["Wg"].asDouble()); - Wg(2) = 0; - est->X_.Rg = SO3::exp(Wg); - } - - auto dX = est->X_ - sim->X_; - LOG(INFO) << "Estimator state initialized from simulator."; - LOG(INFO) << absl::StrFormat("||dT||=%0.2f, ||dR||=%0.2f\n", dX.Tsb.norm(), - dX.Rsb.log().norm()); -} - -//############################################################################ -int main(int argc, char *argv[]) { - google::InitGoogleLogging(argv[0]); - - std::string cfg_path; - if (argc < 2) { - cfg_path = "../cfg/simulation.json"; - LOG(INFO) << "Usage: ./simulate [CONFIGURATION]" << std::endl; - LOG(INFO) << "CONFIGURATION not specified; use default configuration @ " - << cfg_path << std::endl; - } else { - cfg_path = argv[1]; - } - - auto cfg = LoadJson(cfg_path); - - auto est = std::make_shared(cfg["estimator_cfg"].asString()); - auto sim = std::make_shared(cfg["simulator_cfg"].asString()); - // duplicate the initial state ... - // otherwise, this won't work since the initial gravity rotation is not right - // and we are not integrating gravity properly - CloneState(est, sim, cfg.get("add_noise_to_init_state", false).asBool()); - - msg::IMU imu_msg; - msg::Track track_msg; - - imu_msg.initialized_ = false; - track_msg.initialized_ = false; - - bool enable_update = cfg.get("enable_update", true).asBool(); - - cv::Mat disp; - ftype path_length(0); - int counter(0); - State last_X; - while (sim->GetMeas(imu_msg, track_msg)) { - if (imu_msg.initialized_) { - est->InertialMeas(imu_msg.ts_, imu_msg.gyro_, imu_msg.accel_); - if (!enable_update) { - // pretend vision module has initialized - est->vision_initialized_ = true; - est->curr_time_ = imu_msg.ts_; - } - } - - if (track_msg.initialized_) { - if (enable_update) { - est->ProcessTracks_EKF(track_msg.ts_, track_msg.features_); - } - disp = sim->VisualizeTracks(); - } - auto dX = est->X_ - sim->X_; - VLOG(0) << absl::StrFormat("||dT||=%0.6f, ||dR||=%0.6f\n", dX.Tsb.norm(), - dX.Rsb.log().norm()); - VLOG(0) << absl::StrFormat("||dTbc||=%0.6f, ||dRbc||=%0.6f\n", - dX.Tbc.norm(), dX.Rbc.log().norm()); - if (counter != 0) { - path_length += (sim->X_ - last_X).Tsb.norm(); - VLOG(0) << absl::StrFormat("T.drift=%0.2f%% of %0.2f meters\n", - dX.Tsb.norm() / path_length * 100, - path_length); - } - std::cout << path_length << " " << dX.Tsb.norm() << std::endl; - last_X = sim->X_; - ++counter; - - // reset messages - imu_msg.initialized_ = false; - track_msg.initialized_ = false; - - if (cfg.get("visualize", false).asBool()) { - cv::imshow("simulation", disp); - char ckey = cv::waitKey(5); - if (ckey == 'q') - break; - } - } -} diff --git a/src/simulator.cpp b/src/simulator.cpp deleted file mode 100644 index d2d41982..00000000 --- a/src/simulator.cpp +++ /dev/null @@ -1,398 +0,0 @@ - -#include -#include -#include -#include -#include - -#include "glog/logging.h" -#include "opencv2/imgproc/imgproc.hpp" - -#include "simulator.h" -#include "visualize.h" - -namespace feh { - -Simulator::Simulator(const std::string &cfg_path) { - LOG(INFO) << "Initializing simulator... " << std::endl; - cfg_ = LoadJson(cfg_path); - total_time_ = cfg_["total_time"].asDouble(); // seconds - imu_hz_ = cfg_["imu_hz"].asDouble(); - cam_hz_ = cfg_["camera_hz"].asDouble(); - total_count_ = static_cast(ceil(imu_hz_ * total_time_)); - counter_ = 0; - curr_sim_time_ = 0; - frame_counter_ = 0; - motion_type_ = 0; // easiest motion type - init_period_ = cfg_["init_static_period"].asDouble(); - - num_pts_to_track_ = cfg_["num_pts_to_track"].asUInt(); - min_num_pts_to_track_ = - cfg_["min_num_pts_to_track"].asUInt(); // when number of tracks is below - // this threshold, add more tracks - - // Measurement Noises - gyro_noise_std_ = cfg_["gyro_noise_std"].asDouble(); - accel_noise_std_ = cfg_["accel_noise_std"].asDouble(); - tracking_noise_std_ = cfg_["tracking_noise_std"].asDouble(); - max_track_lifetime_ = cfg_["max_track_lifetime"].asInt(); - - // camera sampling rate - cam_sample_rate_ = imu_hz_ / cam_hz_; - - // State generation noises - // model_noise_da_ = 0.005; // for motion_type ? - // model_noise_dw_ = 0.005; - - // initialize camera model - auto cam_cfg = cfg_["camera_cfg"].isString() - ? LoadJson(cfg_["camera_cfg"].asString()) - : cfg_["camera_cfg"]; - cam_ = new Camera(cam_cfg); - LOG(INFO) << "camera initialized" << std::endl; - - mask_ = cv::Mat(cam_->rows(), cam_->cols(), CV_8UC1); - z_near_ = cfg_.get("z_nar", 0.05).asDouble(); - z_far_ = cfg_.get("z_far", 10.0).asDouble(); - - z_dist_ = std::uniform_real_distribution(z_near_, z_far_); - x_dist_ = std::uniform_real_distribution(0, cam_->cols()); - y_dist_ = std::uniform_real_distribution(0, cam_->rows()); - - drop_prob_ = cfg_["track_drop_probability"].asDouble(); - mask_size_ = cfg_["mask_size"].asDouble() / 2.0; - motion_type_ = cfg_["motion_type"].asInt(); - delay_camera_time_ = cfg_["delay_camera_time"].asDouble(); - - // measurement noise distributions - if (!cfg_["fixed_seed"].asBool()) { - long seed = std::chrono::system_clock::now().time_since_epoch().count(); - LOG(INFO) << "\nseed=" << seed << std::endl; - gen_ = std::shared_ptr(new std::knuth_b(seed)); - } else { - long seed = cfg_["seed"].asInt64(); - LOG(INFO) << "\nuse saved seed=" << seed << std::endl; - gen_ = std::shared_ptr(new std::knuth_b(seed)); - } - - // Construct simulated trajectories - translations_.clear(); - velocities_.clear(); - accelerations_.clear(); - GenerateTrajectory(); - - // Initialize simulated state - auto X = cfg_["X"]; - X_.Rsb = SO3::exp(GetVectorFromJson(X, "W")); - X_.Tsb = GetVectorFromJson(X, "T"); - X_.Vsb = GetVectorFromJson(X, "V"); - X_.bg = GetVectorFromJson(X, "bg"); - X_.ba = GetVectorFromJson(X, "ba"); - X_.Rbc = SO3::exp(GetVectorFromJson(X, "Wbc")); - X_.Tbc = GetVectorFromJson(X, "Tbc"); - Vec3 Wg; - Wg.head<2>() = GetVectorFromJson(X, "Wg"); - X_.Rg = SO3::exp(Wg); - - // Initialize grav Vector WARNING: Make sure filter has the same one - g_ << 0, 0, -9.8; - // Initialize imu calibration - Cas_.setIdentity(); - Car_.setIdentity(); - Cgs_.setIdentity(); - Cgr_.setIdentity(); - LOG(INFO) << "Simulator initialized"; -} - -bool Simulator::GetMeas(msg::IMU &imu_msg, msg::Track &track_msg) { - ftype dt = 1.0 / imu_hz_; - imu_msg.ts_ = curr_sim_time_; - - // both accel and gyro below are measured in spatial frame - Vec3 accel = accelerations_[counter_]; - Vec3 gyro = rotational_velocities_[counter_]; - SO3 Rsb = X_.Rsb; - - // fix the the gravity mis-alignment (not really used here, since Rg=I) - // and transfer to body frame - SO3 Rg; - accel = Rsb.inv() * (accel - X_.Rg * g_); // in body frame - - accel += X_.ba; - imu_msg.accel_ = accel; - gyro += X_.bg; - imu_msg.gyro_ = gyro; - - // Propagate the true state - Vec3 gyro_calib = Cgr_ * Cgs_ * (gyro - X_.bg); - Vec3 accel_calib = Car_ * Cas_ * (accel - X_.ba); - X_.Rsb = Rsb * SO3::exp(gyro_calib * dt); - X_.Tsb += X_.Vsb * dt; - X_.Vsb += (Rsb * accel_calib + X_.Rg * g_) * dt; - - // Add measurement noise - imu_msg.accel_ += RandomVector<3>(0, accel_noise_std_, gen_); - imu_msg.gyro_ += RandomVector<3>(0, gyro_noise_std_, gen_); - imu_msg.initialized_ = true; - - // Generate visual measurement - if (counter_ % cam_sample_rate_ == 0) { - // Pull out the measurement for pose - - const auto &Rsb = X_.Rsb; - const auto &Rbc = X_.Rbc; - - const auto &Tsb = X_.Tsb; - const auto &Vsb = X_.Vsb; - const auto &Tbc = X_.Tbc; - - // camera to spatial frame transformation - SO3 Rsc = Rsb * Rbc; - Vec3 Tsc = Rsb * Tbc + Tsb; - - SO3 Rcs = Rsc.inv(); - Vec3 Tcs = -(Rcs * Tsc); - - // generate random points in camera frame (always in the front of the - // camera) - // and bring them to the spatial frame - - // remove invalid tracks - auto it = std::remove_if(pts_.begin(), pts_.end(), [](FeaturePtr f) { - return f->status_ == FeatureStatus::REJECTED_BY_TRACKER || - f->status_ == FeatureStatus::REJECTED_BY_FILTER || - f->status_ == FeatureStatus::DROPPED; - }); - pts_.erase(it, pts_.end()); - - // clear mask - mask_.setTo(cv::Scalar(1)); - std::vector pts(pts_.begin(), pts_.end()); - std::sort(pts.begin(), pts.end(), [](FeaturePtr f1, FeaturePtr f2) -> bool { - return f1->score_ > f2->score_; - }); - pts_.resize(pts.size()); - std::copy(pts.begin(), pts.end(), pts_.begin()); - - // "track" the features - uint32_t valid_tracks = 0; - int total_drop = 0; - for (auto f : pts_) { - Vec3 Xs = f->sim_.Xs; - Vec3 Xc = Rcs * Xs + Tcs; - if (Xc(2) <= z_near_ || Xc(2) > z_far_) { - f->track_.status_ = TrackStatus::REJECTED; - continue; - } - Vec2 xc = Xc.head<2>() / Xc(2); - - Vec2 xp = cam_->Project(xc); - xp += RandomVector<2>(0.0, tracking_noise_std_, gen_); - bool track_ok(true); - - if (OutOfView(xp, *cam_)) { - f->track_.status_ = TrackStatus::DROPPED; - track_ok = false; - ++total_drop; - } else { - if (std::uniform_real_distribution(0, 1)(*gen_) < drop_prob_) { - f->track_.status_ = TrackStatus::DROPPED; - track_ok = false; - ++total_drop; - } else if (f->track_.size() > f->sim_.lifetime) { - // drop tracks exceeding max lifetime with high probability - f->track_.status_ = TrackStatus::DROPPED; - track_ok = false; - ++total_drop; - } - } - - if (track_ok) { - if (!mask_.at(int(xp(1)), int(xp(0)))) { - f->track_.status_ = TrackStatus::REJECTED; - track_ok = false; - } else { - auto x(xp(0)), y(xp(1)); - cv::rectangle(mask_, cv::Point2d(x - mask_size_, y - mask_size_), - cv::Point2d(x + mask_size_, y + mask_size_), - cv::Scalar(0), -1); - } - - ++valid_tracks; - f->UpdateTrack(xp); - f->track_.status_ = TrackStatus::TRACKED; - } - } - VLOG(0) << "Total drop=" << total_drop << std::endl; - // only add more tracks when number of valid tracks is below a threshold - for (; valid_tracks < min_num_pts_to_track_; ++valid_tracks) { - ftype z = z_dist_(*gen_); - Vec2 xp(x_dist_(*gen_), y_dist_(*gen_)); - while (OutOfView(xp, *cam_) || !mask_.at((int)xp[1], (int)xp[0])) { - xp << x_dist_(*gen_), y_dist_(*gen_); - } - // mask out the occupied region - auto x(xp(0)), y(xp(1)); - cv::rectangle(mask_, cv::Point2d(x - mask_size_, y - mask_size_), - cv::Point2d(x + mask_size_, y + mask_size_), cv::Scalar(0), - -1); - Vec2 xc = cam_->UnProject(xp); - auto xp_meas = xp + RandomVector<2>(0.0, tracking_noise_std_, gen_); - - FeaturePtr f = std::make_shared(xp_meas(0), xp_meas(1)); - Vec3 Xs = Rsc * Vec3{xc(0) * z, xc(1) * z, z} + Tsc; - // set ground truth - f->sim_.Xs = Xs; - f->sim_.xp = xp; - f->sim_.xc = xc; - f->sim_.z = z; - f->sim_.lifetime = - std::uniform_int_distribution(2, max_track_lifetime_)(*gen_); - - // simulate score of a feature detector - f->score_ = std::uniform_real_distribution(0, 1)(*gen_); - - pts_.emplace_back(f); - } - - track_msg.features_.resize(pts_.size()); - std::copy(pts_.begin(), pts_.end(), track_msg.features_.begin()); - track_msg.initialized_ = true; - track_msg.ts_ = imu_msg.ts_; - - LOG(INFO) << "simulated features: " << pts_.size(); - - // debug - int mean_track_lifetime = - std::accumulate(pts_.begin(), pts_.end(), 0, [](int x, FeaturePtr f) { - return x + f->track_.size(); - }); - mean_track_lifetime /= pts_.size(); - LOG(INFO) << "mean track life=" << mean_track_lifetime; - } - - ++counter_; - curr_sim_time_ += dt; - return counter_ < total_count_; -} - -SO3 fixor(const Vec3 &A, const Vec3 &B) { - Vec3 x(1, 0, 0); - Vec3 y = A - B; - Vec3 z = y / y.norm(); - - y = z.cross(x); - y /= y.norm(); - // now y is a unit vector perpendicular to x-z plane - x = y.cross(z); - x /= x.norm(); - // now x is a unit vector perpendicular to y-z plane - - // overall, direction of z is fixed to (B->A) - // and the other directions are constructed properly - Mat3 R; - R.setZero(); - R.block<3, 1>(0, 0) = x; - R.block<3, 1>(0, 1) = y; - R.block<3, 1>(0, 2) = z; - return SO3{R}; -} - -void Simulator::GenerateTrajectory() { - LOG(INFO) << "Generating simulated trajectory type " << motion_type_ - << std::endl; - ftype delta_t = 1.0 / imu_hz_; // TODO: make clock for sim work with differing - // rates of imu and cam - ftype time = 0; - - // parameters controling the synthetic data - Vec3 rate, mult; - - // random frequencies and multipliers - rate = RandomVector<3>(0., 1., gen_); - mult = RandomVector<3>(0., 1., gen_); - - // saved set - rate << 15, 30, 15; - rate /= 180.0; - mult << 2, 3, 4; - Vec3 init_slope(mult / (init_period_ * init_period_)); - - // generate the trajectory - for (int i = 0; i < total_count_ + 2; ++i) { - ftype u = time * time / (init_period_ + time); - // ftype u = time * std::sqrt(time) / (10 + time); - // ftype u = time; - Vec3 radius(mult); - if (time < init_period_) { - radius -= init_slope * (time - init_period_) * (time - init_period_); - } - if (motion_type_ == 0) { - translations_.emplace_back(radius[0] * std::cos((M_PI * rate[0] * u)) * - std::sin((M_PI * rate[0]) * u), - radius[1] * std::cos((M_PI * rate[1] * u)) * - std::cos((M_PI * rate[1]) * u), - radius[2] * std::sin((M_PI * rate[2]) * u)); - } else if (motion_type_ == 1) { - translations_.emplace_back(radius[0] * std::sin((M_PI * rate[0]) * u), - radius[1] * std::sin((M_PI * rate[1]) * u), - radius[2] * std::cos((M_PI * rate[2]) * u)); - } else if (motion_type_ == 2) { - // most challenging - translations_.emplace_back(radius[0] * std::sin((M_PI * rate[0]) * u), - radius[1] * std::sin((M_PI * rate[1]) * u), - radius[2] * - (1 - std::cos((M_PI * rate[2]) * u))); - } else if (motion_type_ == 3) { - translations_.emplace_back(5 * std::cos(M_PI * rate[0] * u), - 5 * std::sin(M_PI * rate[1] * u), 0); - } else if (motion_type_ == 4) { - // stationary - translations_.emplace_back(0, 0, 0); - } else { - LOG(FATAL) << "only motion type 0, 1, 2, 3 are supported"; - } - time += delta_t; - } - LOG(INFO) << "translation generated"; - - // differentiate - for (int i = 1; i < total_count_ + 2; ++i) - velocities_.emplace_back((translations_[i] - translations_[i - 1]) / - delta_t); - LOG(INFO) << "velocity generated"; - - for (int i = 1; i < total_count_ + 1; ++i) - accelerations_.emplace_back((velocities_[i] - velocities_[i - 1]) / - delta_t); - accelerations_.push_back(accelerations_[total_count_ - 1]); - LOG(INFO) << "acceleration generated"; - - Vec3 look_at; // point to look at - // look_at = RandomVector<3>(1, 2, gen_); - look_at = Vec3(0, 0, 5); - - // get rotational velocities - for (int i = 1; i < total_count_ + 2; ++i) { - SO3 Ra, Rb; - if (motion_type_ <= 2) { - Rb = fixor(translations_[i], look_at); - Ra = fixor(translations_[i - 1], look_at); - } else if (motion_type_ == 3) { - Rb = fixor(translations_[i], translations_[i] + Vec3(0, 0, 1.0)); - Ra = fixor(translations_[i - 1], translations_[i - 1] + Vec3(0, 0, 1.0)); - } else if (motion_type_ == 4) { - // do nothing - } - auto rot_vel = SO3::log(Ra.inv() * Rb) / delta_t; - rotational_velocities_.push_back(rot_vel); - } - LOG(INFO) << "orientation generated" << std::endl; -} - -cv::Mat Simulator::VisualizeTracks() { - cv::Mat disp(cam_->rows(), cam_->cols(), CV_8UC3); - disp.setTo(cv::Scalar(0, 0, 0)); - return feh::VisualizeTracks(disp, pts_); -} -} diff --git a/src/simulator.h b/src/simulator.h deleted file mode 100644 index e60eb0aa..00000000 --- a/src/simulator.h +++ /dev/null @@ -1,82 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "core.h" -#include "estimator.h" -#include "feature.h" - -#undef SIMULATE_VISUAL_MEAS - -namespace feh { - -struct Simulator { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - Simulator(const std::string &cfg_path); - - /// \brief get and pass measurements to motion integtration of simulator - /// \param imu_dat: imu measurement with Gaussian noise - bool GetMeas(msg::IMU &imu_msg, msg::Track &track_msg); - /// \brief internal simulation generation functions - void GenerateTrajectory(); - cv::Mat VisualizeTracks(); - - Json::Value cfg_; - - //////////////////////////////////////////////////////////////////////////////// - // Clone the following to initialize estimator in simulation - //////////////////////////////////////////////////////////////////////////////// - State X_; - Eigen::Matrix err_; // error state - Mat3 Cas_, Car_; - Mat3 Cgs_, Cgr_; - Vec3 g_; - //////////////////////////////////////////////////////////////////////////////// - - std::list pts_; - Camera *cam_; - - cv::Mat mask_; - uint32_t num_pts_to_track_; - uint32_t min_num_pts_to_track_; // when number of tracks is below this - // threshold, add more tracks - ftype z_near_, z_far_; // near and far plane - std::uniform_real_distribution x_dist_, y_dist_, z_dist_; - - ftype drop_prob_; - ftype mask_size_; - int motion_type_; - ftype init_period_; - - // sampling rates and duration - int64_t total_time_, curr_sim_time_; // seconds - ftype imu_hz_, cam_hz_; - int counter_, total_count_, cam_sample_rate_; - int frame_counter_; - - ftype delay_camera_time_; - - // generation noises - ftype accel_noise_std_, gyro_noise_std_; - ftype tracking_noise_std_; - int max_track_lifetime_; - - ftype model_noise_da_, model_noise_dw_; - - // trajectory generation - std::vector translations_; - std::vector velocities_; - std::vector accelerations_; - std::vector rotational_velocities_; - - // measurement noise generators - std::shared_ptr gen_; -}; - -using SimulatorPtr = std::shared_ptr; - -} // feh