initial commit
This commit is contained in:
351
ov_init/launch/display.rviz
Normal file
351
ov_init/launch/display.rviz
Normal file
@@ -0,0 +1,351 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Path VIO1
|
||||
- /Path GT1
|
||||
Splitter Ratio: 0.6011396050453186
|
||||
Tree Height: 730
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ACTIVE Features
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 112; 112; 115
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 80
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 120
|
||||
Frames:
|
||||
All Enabled: true
|
||||
Marker Alpha: 1
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: false
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
- Class: rviz/Image
|
||||
Enabled: false
|
||||
Image Topic: /ov_msckf/trackhist
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image Tracks
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: compressed
|
||||
Unreliable: false
|
||||
Value: false
|
||||
- Class: rviz/Image
|
||||
Enabled: false
|
||||
Image Topic: /ov_msckf/loop_depth_colored
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Current Depths
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: compressed
|
||||
Unreliable: false
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 0; 255; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.10000000149011612
|
||||
Line Style: Billboards
|
||||
Line Width: 0.029999999329447746
|
||||
Name: Path VIO
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: Axes
|
||||
Queue Size: 10
|
||||
Radius: 0.009999999776482582
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /ov_msckf/pathimu
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 0; 255; 255
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.10000000149011612
|
||||
Line Style: Billboards
|
||||
Line Width: 0.029999999329447746
|
||||
Name: Path GT
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: Axes
|
||||
Queue Size: 10
|
||||
Radius: 0.009999999776482582
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /ov_msckf/pathgt
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 0.800000011920929
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 252; 175; 62
|
||||
Color Transformer: FlatColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: MSCKF Points
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 8
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /ov_msckf/points_msckf
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 0; 0
|
||||
Color Transformer: FlatColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: SLAM Points
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 8
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /ov_msckf/points_slam
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 117; 80; 123
|
||||
Color Transformer: FlatColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: ARUCO Points
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 15
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /ov_msckf/points_aruco
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: FlatColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: ACTIVE Features
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 6
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /ov_msckf/loop_feats
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 127
|
||||
Color Transformer: FlatColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: SIM Points
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 6
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /ov_msckf/points_sim
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 44; 44; 44
|
||||
Default Light: true
|
||||
Fixed Frame: global
|
||||
Frame Rate: 60
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Theta std deviation: 0.2617993950843811
|
||||
Topic: /initialpose
|
||||
X std deviation: 0.5
|
||||
Y std deviation: 0.5
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 10
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Field of View: 0.7853981852531433
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: false
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.7853981852531433
|
||||
Target Frame: <Fixed Frame>
|
||||
Yaw: 0.7853981852531433
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Current Depths:
|
||||
collapsed: false
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 842
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: true
|
||||
Image Tracks:
|
||||
collapsed: false
|
||||
QMainWindow State: 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
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: true
|
||||
Width: 1525
|
||||
X: 2314
|
||||
Y: 1161
|
||||
52
ov_init/launch/dynamic.launch
Normal file
52
ov_init/launch/dynamic.launch
Normal file
@@ -0,0 +1,52 @@
|
||||
<launch>
|
||||
|
||||
|
||||
<!-- ================================================================ -->
|
||||
<!-- ================================================================ -->
|
||||
|
||||
<!-- what config we are going to run (should match folder name) -->
|
||||
<arg name="verbosity" default="DEBUG" /> <!-- ALL, DEBUG, INFO, WARNING, ERROR, SILENT -->
|
||||
<arg name="config" default="rpng_sim" />
|
||||
<arg name="config_path" default="$(find ov_init)/../config/$(arg config)/estimator_config.yaml" />
|
||||
|
||||
<arg name="seed" default="3" />
|
||||
<arg name="freq_cam" default="10" />
|
||||
<arg name="freq_imu" default="400" />
|
||||
<arg name="dataset" default="tum_corridor1_512_16_okvis.txt" /> <!-- udel_gore, udel_gore_zupt, tum_corridor1_512_16_okvis, udel_arl, udel_neighborhood, euroc_V1_01_easy -->
|
||||
|
||||
<!-- if we should perturb the initial state values (i.e. calibration) -->
|
||||
<arg name="sim_do_perturbation" default="true" />
|
||||
|
||||
<!-- if we should optimize the calibration -->
|
||||
<arg name="mle_opt_calib" default="false" />
|
||||
|
||||
|
||||
<!-- ================================================================ -->
|
||||
<!-- ================================================================ -->
|
||||
|
||||
|
||||
<!-- MASTER NODE! -->
|
||||
<!-- <node name="test_dynamic_init" pkg="ov_init" type="test_dynamic_init" output="screen" clear_params="true" required="true" launch-prefix="gdb -ex run --args">-->
|
||||
<node name="test_dynamic_init" pkg="ov_init" type="test_dynamic_init" output="screen" clear_params="true" required="true">
|
||||
|
||||
<param name="sim_traj_path" type="string" value="$(find ov_data)/sim/$(arg dataset)" />
|
||||
<param name="sim_seed_state_init" type="int" value="0" />
|
||||
<param name="sim_seed_measurements" type="int" value="$(arg seed)" />
|
||||
<param name="sim_seed_preturb" type="int" value="$(arg seed)" />
|
||||
<param name="sim_freq_cam" type="int" value="$(arg freq_cam)" />
|
||||
<param name="sim_freq_imu" type="int" value="$(arg freq_imu)" />
|
||||
<param name="sim_do_perturbation" type="bool" value="$(arg sim_do_perturbation)" />
|
||||
|
||||
<!-- master configuration object -->
|
||||
<param name="verbosity" type="string" value="$(arg verbosity)" />
|
||||
<param name="config_path" type="string" value="$(arg config_path)" />
|
||||
|
||||
<!-- world/filter parameters -->
|
||||
<param name="max_cameras" type="int" value="1" />
|
||||
<param name="init_dyn_mle_opt_calib" type="bool" value="$(arg mle_opt_calib)" />
|
||||
|
||||
</node>
|
||||
|
||||
|
||||
|
||||
</launch>
|
||||
53
ov_init/launch/mle.launch
Normal file
53
ov_init/launch/mle.launch
Normal file
@@ -0,0 +1,53 @@
|
||||
<launch>
|
||||
|
||||
|
||||
<!-- ================================================================ -->
|
||||
<!-- ================================================================ -->
|
||||
|
||||
<!-- what config we are going to run (should match folder name) -->
|
||||
<arg name="verbosity" default="DEBUG" /> <!-- ALL, DEBUG, INFO, WARNING, ERROR, SILENT -->
|
||||
<arg name="config" default="rpng_sim" />
|
||||
<arg name="config_path" default="$(find ov_init)/../config/$(arg config)/estimator_config.yaml" />
|
||||
|
||||
<arg name="seed" default="3" />
|
||||
<arg name="freq_cam" default="10" />
|
||||
<arg name="freq_imu" default="400" />
|
||||
<arg name="dataset" default="euroc_V1_01_easy.txt" /> <!-- udel_gore, udel_gore_zupt, tum_corridor1_512_16_okvis, udel_arl, udel_neighborhood, euroc_V1_01_easy -->
|
||||
|
||||
<!-- if we should perturb the initial state values (i.e. calibration) -->
|
||||
<arg name="sim_do_perturbation" default="false" />
|
||||
|
||||
<!-- if we should optimize the calibration -->
|
||||
<arg name="mle_opt_calib" default="false" />
|
||||
|
||||
|
||||
<!-- ================================================================ -->
|
||||
<!-- ================================================================ -->
|
||||
|
||||
|
||||
<!-- MASTER NODE! -->
|
||||
<node name="test_dynamic_mle" pkg="ov_init" type="test_dynamic_mle" output="screen" clear_params="true" required="true">
|
||||
<!-- <node name="test_dynamic_mle" pkg="ov_init" type="test_dynamic_mle" output="screen" clear_params="true" required="true" launch-prefix="gdb -ex run --args">-->
|
||||
|
||||
<param name="sim_traj_path" type="string" value="$(find ov_data)/sim/$(arg dataset)" />
|
||||
<param name="sim_seed_state_init" type="int" value="0" />
|
||||
<param name="sim_seed_measurements" type="int" value="$(arg seed)" />
|
||||
<param name="sim_seed_preturb" type="int" value="$(arg seed)" />
|
||||
<param name="sim_freq_cam" type="int" value="$(arg freq_cam)" />
|
||||
<param name="sim_freq_imu" type="int" value="$(arg freq_imu)" />
|
||||
<param name="sim_do_perturbation" type="bool" value="$(arg sim_do_perturbation)" />
|
||||
|
||||
<!-- master configuration object -->
|
||||
<param name="verbosity" type="string" value="$(arg verbosity)" />
|
||||
<param name="config_path" type="string" value="$(arg config_path)" />
|
||||
|
||||
<!-- world/filter parameters -->
|
||||
<param name="max_cameras" type="int" value="1" />
|
||||
<param name="init_dyn_mle_opt_calib" type="bool" value="$(arg mle_opt_calib)" />
|
||||
<param name="init_max_features" type="int" value="30" />
|
||||
|
||||
</node>
|
||||
|
||||
|
||||
|
||||
</launch>
|
||||
Reference in New Issue
Block a user