diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..85c6b68e9eaccb40a1f8ba91b3a77074f1734fe8 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +bag/* diff --git a/launch/imu2d_demo.launch b/launch/imu2d_demo.launch index 7d5a13654a7adbe5cc9118419e018df7f1f44b17..ce4520c3193fd942f72ca6b47be0c90e98f2668f 100644 --- a/launch/imu2d_demo.launch +++ b/launch/imu2d_demo.launch @@ -12,7 +12,7 @@ <arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" /> <!-- using "clock" option to use the simulated time. Rosbag launch as the first node --> - <node pkg="rostopic" type="rostopic" name="" args='pub /helena/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/> + <node pkg="rostopic" type="rostopic" name="" args='pub /ana/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/> <node pkg="tf" type="static_transform_publisher" diff --git a/rviz/imu2d_demo.rviz b/rviz/imu2d_demo.rviz index 947506c5c96f62226c02d7cbea8512bb08725351..921254e521956da2cbd5c00ed0a0425943f34de3 100644 --- a/rviz/imu2d_demo.rviz +++ b/rviz/imu2d_demo.rviz @@ -11,7 +11,6 @@ Panels: - /LaserScan1 - /Landmarks1 - /Factors1 - - /Factors1/Status1 - /Factors1/Namespaces1 - /Trajectory1 - /Trajectory1/Namespaces1 @@ -345,7 +344,7 @@ Visualization Manager: Enabled: true History Length: 1 Name: Imu - Topic: /helena/sensors/bno055_imu/imu_remapped + Topic: /helena/sensors/imu/data Unreliable: false Value: true Enabled: true diff --git a/yaml/csm.yaml b/yaml/csm.yaml index 59f62f628e15cb484f40d3b0ce50b404dfdd317d..2dd8f8680123cbf977c7bd7defcb0c65dd1d33f5 100644 --- a/yaml/csm.yaml +++ b/yaml/csm.yaml @@ -1,35 +1,45 @@ -max_iterations: 50 -max_correspondence_dist: 10 -use_corr_tricks: 1 -outliers_maxPerc: 0.9 -use_point_to_line_distance: 1 -outliers_adaptive_order: 0.7 -outliers_adaptive_mult: 1.5 -do_compute_covariance: 1 - -max_angular_correction_deg: 10 -max_linear_correction: 1 -epsilon_xy: 0 -epsilon_theta: 0 -sigma: 0.2 -restart: 0 -restart_threshold_mean_error: 0 -restart_dt: 0 -restart_dtheta: 0 -clustering_threshold: 0 -orientation_neighbourhood: 0 -do_alpha_test: 0 -do_alpha_test_thresholdDeg: 0 -do_visibility_test: 0 -outliers_remove_doubles: 0 -debug_verify_tricks: 0 -gpm_theta_bin_size_deg: 0 -gpm_extend_range_deg: 0 -gpm_interval: 0 -min_reading: 0.023 -max_reading: 60 -use_ml_weights: 0 -use_sigma_weights: 0 - - -cov_factor: 1 +verbose: false # prints debug messages + +# ALGORITHM OPTIONS +use_point_to_line_distance: true # use PlICP (true) or use vanilla ICP (false). +max_angular_correction_deg: 180 # Maximum angular displacement between scans (deg) +max_linear_correction: 10 # Maximum translation between scans (m) + +max_correspondence_dist: 0.5 # Maximum distance for a correspondence to be valid +use_corr_tricks: true # Use smart tricks for finding correspondences. Only influences speed; not convergence. +debug_verify_tricks: false # Checks that find_correspondences_tricks give the right answer + +# STOP CRITERIA +max_iterations: 50 # maximum iterations +epsilon_xy: 1e-4 # distance change +epsilon_theta: 1e-3 # angle change + +# RESTART +restart: false # Restart algorithm +restart_threshold_mean_error: 0 # Threshold for restarting +restart_dt: 0 # Displacement for restarting +restart_dtheta: 0 # Displacement for restarting + +# DISCARDING POINTS/CORRESPONDENCES +min_reading: 0.023 # discard rays outside of this interval +max_reading: 60 # discard rays outside of this interval +outliers_maxPerc: 0.9 # Percentage of correspondences to consider: if 0.9, always discard the top 10% of correspondences with more error +outliers_adaptive_order: 0.7 # Parameters describing a simple adaptive algorithm for discarding +outliers_adaptive_mult: 1.5 # Parameters describing a simple adaptive algorithm for discarding +outliers_remove_doubles: false # Do not allow two different correspondences to share a point +do_visibility_test: false # If initial guess, visibility test can discard points that are not visible +do_alpha_test: false # Discard correspondences based on the angles +do_alpha_test_thresholdDeg: 10 # + +# POINT ORIENTATION +clustering_threshold: 0.5 # Max-distance clustering for point orientation +orientation_neighbourhood: 4 # Number of neighbour rays used to estimate the orientation + +# WEIGHTS +use_ml_weights: false # weight the impact of each correspondence. This works fabolously if the first scan has no noise. +use_sigma_weights: false # If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 +sigma: 0.2 # Noise of the scan + +# COVARIANCE +do_compute_covariance: true +cov_factor: 5 # Factor multiplying the cov output of csm diff --git a/yaml/csm_inactive.yaml b/yaml/csm_inactive.yaml index b99171d815291adbd3d320cc2e67bd53a128fd85..b5336a90b5dad444b2bfd9d946582ab8d987cfc1 100644 --- a/yaml/csm_inactive.yaml +++ b/yaml/csm_inactive.yaml @@ -1,35 +1,47 @@ -max_iterations: 50 -max_correspondence_dist: 10 -use_corr_tricks: 1 -outliers_maxPerc: 0.9 -use_point_to_line_distance: 1 -outliers_adaptive_order: 0.7 -outliers_adaptive_mult: 1.5 -do_compute_covariance: 1 - -max_angular_correction_deg: 10 -max_linear_correction: 1 -epsilon_xy: 0 -epsilon_theta: 0 -sigma: 0.2 -restart: 0 -restart_threshold_mean_error: 0 -restart_dt: 0 -restart_dtheta: 0 -clustering_threshold: 0 -orientation_neighbourhood: 0 -do_alpha_test: 0 -do_alpha_test_thresholdDeg: 0 -do_visibility_test: 0 -outliers_remove_doubles: 0 -debug_verify_tricks: 0 -gpm_theta_bin_size_deg: 0 -gpm_extend_range_deg: 0 -gpm_interval: 0 -min_reading: 0.023 -max_reading: 60 -use_ml_weights: 0 -use_sigma_weights: 0 +verbose: false # prints debug messages +# ALGORITHM OPTIONS +use_point_to_line_distance: true # use PlICP (true) or use vanilla ICP (false). +max_angular_correction_deg: 180 # Maximum angular displacement between scans (deg) +max_linear_correction: 10 # Maximum translation between scans (m) + +max_correspondence_dist: 0.5 # Maximum distance for a correspondence to be valid +use_corr_tricks: true # Use smart tricks for finding correspondences. Only influences speed; not convergence. +debug_verify_tricks: false # Checks that find_correspondences_tricks give the right answer + +# STOP CRITERIA +max_iterations: 50 # maximum iterations +epsilon_xy: 1e-4 # distance change +epsilon_theta: 1e-3 # angle change + +# RESTART +restart: false # Restart algorithm +restart_threshold_mean_error: 0 # Threshold for restarting +restart_dt: 0 # Displacement for restarting +restart_dtheta: 0 # Displacement for restarting + +# DISCARDING POINTS/CORRESPONDENCES +min_reading: 0.023 # discard rays outside of this interval +max_reading: 60 # discard rays outside of this interval +outliers_maxPerc: 0.9 # Percentage of correspondences to consider: if 0.9, always discard the top 10% of correspondences with more error +outliers_adaptive_order: 0.7 # Parameters describing a simple adaptive algorithm for discarding +outliers_adaptive_mult: 1.5 # Parameters describing a simple adaptive algorithm for discarding +outliers_remove_doubles: false # Do not allow two different correspondences to share a point +do_visibility_test: false # If initial guess, visibility test can discard points that are not visible +do_alpha_test: false # Discard correspondences based on the angles +do_alpha_test_thresholdDeg: 10 # + +# POINT ORIENTATION +clustering_threshold: 0.5 # Max-distance clustering for point orientation +orientation_neighbourhood: 4 # Number of neighbour rays used to estimate the orientation + +# WEIGHTS +use_ml_weights: false # weight the impact of each correspondence. This works fabolously if the first scan has no noise. +use_sigma_weights: false # If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 +sigma: 0.2 # Noise of the scan + +# COVARIANCE +do_compute_covariance: true +cov_factor: 5 # Factor multiplying the cov output of csm cov_factor: 9999999999999999999999 diff --git a/yaml/csm_old.yaml b/yaml/csm_old.yaml new file mode 100644 index 0000000000000000000000000000000000000000..59f62f628e15cb484f40d3b0ce50b404dfdd317d --- /dev/null +++ b/yaml/csm_old.yaml @@ -0,0 +1,35 @@ +max_iterations: 50 +max_correspondence_dist: 10 +use_corr_tricks: 1 +outliers_maxPerc: 0.9 +use_point_to_line_distance: 1 +outliers_adaptive_order: 0.7 +outliers_adaptive_mult: 1.5 +do_compute_covariance: 1 + +max_angular_correction_deg: 10 +max_linear_correction: 1 +epsilon_xy: 0 +epsilon_theta: 0 +sigma: 0.2 +restart: 0 +restart_threshold_mean_error: 0 +restart_dt: 0 +restart_dtheta: 0 +clustering_threshold: 0 +orientation_neighbourhood: 0 +do_alpha_test: 0 +do_alpha_test_thresholdDeg: 0 +do_visibility_test: 0 +outliers_remove_doubles: 0 +debug_verify_tricks: 0 +gpm_theta_bin_size_deg: 0 +gpm_extend_range_deg: 0 +gpm_interval: 0 +min_reading: 0.023 +max_reading: 60 +use_ml_weights: 0 +use_sigma_weights: 0 + + +cov_factor: 1 diff --git a/yaml/imu2d_test1.yaml b/yaml/imu2d_test1.yaml index b674e10644b8d88ce3176dfb00c4efa2f711f664..549158408686f6e793fa8505e7fbffe4aa1da6d9 100644 --- a/yaml/imu2d_test1.yaml +++ b/yaml/imu2d_test1.yaml @@ -26,18 +26,8 @@ config: plugin: "core" solver: - max_num_iterations: 10 - verbose: 0 - period: 0 - update_immediately: false - n_threads: 2 - compute_cov: false - function_tolerance: 1e-6 - gradient_tolerance: 1e-6 - minimizer: "LEVENBERG_MARQUARDT" - use_nonmonotonic_steps: False - max_consecutive_nonmonotonic_steps: 10 - + follow: "solver.yaml" + sensors: - type: "SensorOdom2d" @@ -135,4 +125,3 @@ config: p_free_th: 0.2 p_obst_th: 0.9 discard_max_range: true - diff --git a/yaml/imu2d_test4.yaml b/yaml/imu2d_test4.yaml index e9dd597b83e9c1cc4bfe09d68018bc5ce9f2e9cb..a338b354e4b6a3b66f4b64752f5f4bc2f5891900 100644 --- a/yaml/imu2d_test4.yaml +++ b/yaml/imu2d_test4.yaml @@ -2,8 +2,12 @@ config: debug: profiling: true profiling_file: "~/wolf_demo_profiling_imu2d_test4.txt" - print_problem: false + print_problem: true print_period: 5 + print_depth: 4 + print_constr_by: false + print_metric: true + print_state_blocks: true problem: tree_manager: type: "none" @@ -16,14 +20,13 @@ config: O: [0] V: [0,0] time_tolerance: 0.1 + node_rate: 100 + map: + type: "MapBase" + plugin: "core" solver: - max_num_iterations: 100 - verbose: 0 - period: 0 - update_immediately: false - n_threads: 2 - compute_cov: false + follow: "solver.yaml" sensors: - @@ -31,11 +34,11 @@ config: name: "scanner_front_left" plugin: "laser" follow: "test_laser_params.yaml" - - - type: "SensorImu2d" - name: "bno" - plugin: "imu" - follow: "test_imu_params.yaml" + # - + # type: "SensorImu2d" + # name: "bno" + # plugin: "imu" + # follow: "test_imu_params.yaml" - type: "SensorImu2d" name: "microstrain" @@ -66,7 +69,7 @@ config: - package: "wolf_ros_laser" type: "SubscriberLaser2d" - topic: "/ana/sensors/scan" + topic: "/helena/sensors/scan" sensor_name: "scanner_front_left" load_params_from_msg: true - @@ -76,7 +79,7 @@ config: #topic: "/ana/sensors/bno055_imu/imu" #sensor_name: "bno" #microstrain - topic: "/ana/imu/data" + topic: "/helena/sensors/imu/data" sensor_name: "microstrain" follow: "test_imu_subscriber.yaml" diff --git a/yaml/solver.yaml b/yaml/solver.yaml new file mode 100644 index 0000000000000000000000000000000000000000..ef04fd43fffb6b5b6ede533e948e125d365f212d --- /dev/null +++ b/yaml/solver.yaml @@ -0,0 +1,13 @@ +minimizer: LEVENBERG_MARQUARDT +max_num_iterations: 20 +verbose: 0 +period: 0.0 +n_threads: 2 +interrupt_on_problem_change: false +min_num_iterations: 5 #if update immediately +compute_cov: false +cov_enum: 3 # if compute_cov +cov_period: 1 # if compute_cov +function_tolerance: 1e-8 +gradient_tolerance: 1e-9 +use_nonmonotonic_steps: false diff --git a/yaml/test_imu_subscriber.yaml b/yaml/test_imu_subscriber.yaml index cd15bf7b922f91f46823d93ae914defd97487bdc..8aa2403f6c288b9256abe8cd33a86f22b090d6f1 100644 --- a/yaml/test_imu_subscriber.yaml +++ b/yaml/test_imu_subscriber.yaml @@ -1,20 +1,20 @@ -#bno: -dt: 0.016 -imu_x_axis: 3 -imu_y_axis: -2 -imu_z_axis: 1 -cov_source: "sensor" -in_degrees: true -##microstrain: -#dt: 0.01 -#imu_x_axis: 1 +##bno: +#dt: 0.016 +#imu_x_axis: 3 #imu_y_axis: -2 -#imu_z_axis: -3 +#imu_z_axis: 1 #cov_source: "sensor" -#in_degrees: false +#in_degrees: true +#microstrain: +dt: 0.01 +imu_x_axis: 1 +imu_y_axis: -2 +imu_z_axis: -3 +cov_source: "sensor" +in_degrees: false #all topic_enable: "/ana/sensors/bno055_imu/imu/topic_enable" -static_init_duration: 7 +static_init_duration: 1 lowpass_filter: true lowpass_cutoff_freq: 5 diff --git a/yaml/test_laser_processor.yaml b/yaml/test_laser_processor.yaml index d2aaceeee6da6c882b2965f3a7ef20989d812cf8..ae63e5c449a9c38b02edfae415b5aab12d67e98b 100644 --- a/yaml/test_laser_processor.yaml +++ b/yaml/test_laser_processor.yaml @@ -7,7 +7,8 @@ keyframe_vote: min_error: 999 max_points: 0 max_new_features: 15 -follow: "csm.yaml" +icp: + follow: "csm.yaml" time_tolerance: 0.05 #Half the sample time state_getter: true state_priority: 10 diff --git a/yaml/test_laser_processor_inactive.yaml b/yaml/test_laser_processor_inactive.yaml index a584d2cde4e2f8852b231a822d6160c2a08add4b..38d70089dc29746f0347874fac97ebf5f0bccde7 100644 --- a/yaml/test_laser_processor_inactive.yaml +++ b/yaml/test_laser_processor_inactive.yaml @@ -7,7 +7,8 @@ keyframe_vote: min_error: 999 max_points: 0 max_new_features: 15 -follow: "csm_inactive.yaml" +icp: + follow: "csm_inactive.yaml" time_tolerance: 0.05 #Half the sample time state_getter: true state_priority: 10