Skip to content

Commit

Permalink
add manual initial coordinates for slamprior
Browse files Browse the repository at this point in the history
  • Loading branch information
@brytsknguyen
brytsknguyen committed Aug 22, 2024
1 parent 26cdb74 commit df226af
Show file tree
Hide file tree
Showing 6 changed files with 136 additions and 112 deletions.
4 changes: 2 additions & 2 deletions config/mcdviral_atv.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ reassoc_rate: 0
use_ceres: 0

# Downsampling grid size, chose -1 for no downsampling
leaf_size: 0.05 # Downsample scale of input pointcloud, also the scale of ufomap leaf
leaf_size: 0.1 # Downsample scale of input pointcloud, also the scale of ufomap leaf
assoc_spacing: 0.4
surfel_map_depth: 16
surfel_min_point: 5
Expand Down Expand Up @@ -136,7 +136,7 @@ regularize_imu: 0
imu_init_time: 0.5
lite_redeskew: 1
max_outer_iters: 3
max_lidar_factor: 4000 # Maximum number of lidar factors
max_lidar_factor: 8000 # Maximum number of lidar factors
dj_thres: 0.0
dx_thres: 0.5

Expand Down
19 changes: 9 additions & 10 deletions launch/mcdviral.rviz
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,9 @@ Panels:
- /Optimization1
- /Optimization1/OptOdom1/Shape1
- /Optimization1/PredictOdom1/Shape1
- /Map1
- /Map1/KfCloud1
Splitter Ratio: 0.5614035129547119
Tree Height: 441
Tree Height: 410
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expand All @@ -28,7 +27,7 @@ Panels:
- Class: rviz/Time
Name: Time
SyncMode: 0
SyncSource: KfPose
SyncSource: PriorMap
Preferences:
PromptSaveOnExit: true
Toolbars:
Expand Down Expand Up @@ -535,7 +534,7 @@ Visualization Manager:
Marker Topic: /loop_marker
Name: LoopPrior
Namespaces:
loop_marker: true
{}
Queue Size: 1
Value: true
- Alpha: 1
Expand Down Expand Up @@ -916,25 +915,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/XYOrbit
Distance: 14.295909881591797
Distance: 19.14592742919922
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.09421633183956146
Y: 2.3248889446258545
Z: 1.912194875330897e-06
X: 5.948090553283691
Y: -2.457622528076172
Z: -1.6128524293890223e-07
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: -0.01460192259401083
Pitch: 0.46039843559265137
Target Frame: body
Yaw: 4.708576679229736
Yaw: 5.943588733673096
Saved: ~
Window Geometry:
Displays:
Expand Down
2 changes: 1 addition & 1 deletion launch/run_mcdviral.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@
<node pkg="slict" required="true" type="slict_sensorsync" name="slict_sensorsync" respawn="false" output="log"/>

<!-- Run the relocalization -->
<node if="$(eval use_prior_map > 0)" pkg="slict" required="true" type="slict_relocalization" name="slict_relocalization" respawn="false" output="screen">
<node if="$(eval use_prior_map > 0 and descriptor_path != '')" pkg="slict" required="true" type="slict_relocalization" name="slict_relocalization" respawn="false" output="screen">
<param name="relocalization/descriptor_file_path" type="string" value="$(arg descriptor_path)"/>
</node>

Expand Down
92 changes: 46 additions & 46 deletions launch/run_mcdviral_slamprior.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
<!-- <arg name="data_path" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/PublishedSequences"/> -->
<arg name="data_path" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/PublishedSequencesUnzipped"/>

<arg name="bag_file" default="$(arg data_path)/ntu_day_01/*.bag"/>
<!-- <arg name="bag_file" default="$(arg data_path)/ntu_day_01/*.bag"/> -->
<!-- <arg name="bag_file" default="$(arg data_path)/ntu_day_02/*.bag"/> -->
<!-- <arg name="bag_file" default="$(arg data_path)/ntu_day_10/*.bag"/> -->
<!-- <arg name="bag_file" default="$(arg data_path)/ntu_night_04/*.bag"/> -->
Expand Down Expand Up @@ -54,58 +54,58 @@
<!-- <arg name="bag_file" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/UnpublishedSequences/ntu_night_12/*.bag"/> -->

<!-- Manual initialization for reloc -->
<group if="$(eval 'ntu_day_01' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 0.0, 0.0, 0.0, -0, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_02' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 61.99, 119.58, 7.69, -134, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_03' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 62.82, 119.51, 7.70, 39, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_04' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 55.52, 110.70, 7.72, -40, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_05' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 59.18, 116.06, 7.72, 42, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_06' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 48.67, 109.16, 7.64, -28, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_07' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [-27.11, -1.57, 8.73, -8, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_08' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 260.65, -74.57, 0.664, 180, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_09' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 71.90, 57.99, 7.67, 80, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_10' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 39.49, 23.48, 6.54, 36, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_01' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 59.69, 108.43, 7.82, -36, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_02' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 55.78, 108.37, 7.78, -32, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_03' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 143.28, 36.80, 8.97, -136, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_04' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 244.20, -99.86, 5.97, -32, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_05' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 85.37, 73.99, 7.77, -132, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_06' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 46.02, 21.03, 6.6, -135, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_07' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 55.97, 112.70, 7.75, -36, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_08' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 260.65, -74.57, 0.664, 180, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_09' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 234.26, -41.31, 6.69, -107, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_10' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 194.55, -216.91, -3.69, 176, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_11' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 15.34, -197.79, -4.99, 124, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_12' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 60.77, -45.23, 2.2, -139, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_13' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 81.38, -18.45, 3.43, 42, 0, 0] </rosparam> </group>

<group if="$(eval 'kth_day_06' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 64.41, 66.48, 38.50, 144, 0, 0] </rosparam> </group>
<group if="$(eval 'kth_day_09' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 70.40, 63.12, 38.30, -26, 0, 0] </rosparam> </group>
<group if="$(eval 'kth_day_10' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 69.13, 63.57, 38.38, 145, 0, 0] </rosparam> </group>
<group if="$(eval 'kth_night_01' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 68.84, -64.10, 38.43, 138, 0, 0] </rosparam> </group>
<group if="$(eval 'kth_night_04' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 71.47, 63.56, 38.37, -52, 0, 0] </rosparam> </group>
<group if="$(eval 'kth_night_05' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 43.81, - 131.66, 29.42, 76, 0, 0] </rosparam> </group>

<group if="$(eval 'tuhh_day_02' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 45.73, 447.10, 14.69, -157, 0, 0] </rosparam> </group>
<group if="$(eval 'tuhh_day_03' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 43.62, 446.53, 14.59, -162, 0, 0] </rosparam> </group>
<group if="$(eval 'tuhh_day_04' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 35.15, 114.94, -1.3, -130, 0, 0] </rosparam> </group>
<group if="$(eval 'tuhh_night_07' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 43.87, 447.10, 14.59, -138, 0, 0] </rosparam> </group>
<group if="$(eval 'tuhh_night_08' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 42.93, 446.72, 14.63, -143, 0, 0] </rosparam> </group>
<group if="$(eval 'tuhh_night_09' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 32.47, 111.61, -1.42, -149, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_01' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 0.0, 0.0, 0.0, 0, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_02' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 1.61, 17.60, 0.36, -92, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_03' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 2.29, 18.08, 0.41, 80, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_04' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 2.59, 6.64, 0.31, 1, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_05' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 1.82, 13.04, 0.32, 83, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_06' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [-1.56, 0.94, 0.12, 14, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_07' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 15.01, -132.45, 1.54, 34, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_08' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 54.15, -74.52, -0.45, 89, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_09' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 49.79, -22.19, 0.61, 121, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_day_10' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 48.21, -68.97, -0.65, 77, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_01' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 7.24, 7.69, 0.47, 5, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_02' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 5.63, 6.10, 0.33, 3.6, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_03' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 117.57, 8.95, 3.17, -95, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_04' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 283.09, -27.83, 6.25, 8.65, -2, -1] </rosparam> </group>
<group if="$(eval 'ntu_night_05' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 49.30, -1.27, 0.82, -91, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_06' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 54.74, -66.52, -0.47, -94, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_07' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 1.64, 8.36, 0.33, 5, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_08' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 186.8, 9.48, 3.61, 176, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_09' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 237.44, 9.77, 5.12, -66, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_10' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 322.44, -148.63, -0.88, -142, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_11' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 175.00, -252.22, -5.32, 165, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_12' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 109.12, -107.19, -3.77, -98, 0, 0] </rosparam> </group>
<group if="$(eval 'ntu_night_13' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 107.19, -73.28, -2.93, 83, 0, 0] </rosparam> </group>

<!-- <group if="$(eval 'kth_day_06' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 64.41, 66.48, 38.50, 144, 0, 0] </rosparam> </group> -->
<!-- <group if="$(eval 'kth_day_09' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 70.40, 63.12, 38.30, -26, 0, 0] </rosparam> </group> -->
<!-- <group if="$(eval 'kth_day_10' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 69.13, 63.57, 38.38, 145, 0, 0] </rosparam> </group> -->
<!-- <group if="$(eval 'kth_night_01' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 68.84, -64.10, 38.43, 138, 0, 0] </rosparam> </group> -->
<!-- <group if="$(eval 'kth_night_04' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 71.47, 63.56, 38.37, -52, 0, 0] </rosparam> </group> -->
<!-- <group if="$(eval 'kth_night_05' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 43.81, - 131.66, 29.42, 76, 0, 0] </rosparam> </group> -->

<!-- <group if="$(eval 'tuhh_day_02' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 45.73, 447.10, 14.69, -157, 0, 0] </rosparam> </group> -->
<!-- <group if="$(eval 'tuhh_day_03' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 43.62, 446.53, 14.59, -162, 0, 0] </rosparam> </group> -->
<!-- <group if="$(eval 'tuhh_day_04' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 35.15, 114.94, -1.3, -130, 0, 0] </rosparam> </group> -->
<!-- <group if="$(eval 'tuhh_night_07' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 43.87, 447.10, 14.59, -138, 0, 0] </rosparam> </group> -->
<!-- <group if="$(eval 'tuhh_night_08' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 42.93, 446.72, 14.63, -143, 0, 0] </rosparam> </group> -->
<!-- <group if="$(eval 'tuhh_night_09' in bag_file)"> <rosparam param="/tf_Lprior_L0_init"> [ 32.47, 111.61, -1.42, -149, 0, 0] </rosparam> </group> -->

<!-- Chose the config file based on the sequence names -->
<arg name="config_file" value="$(find slict)/config/mcdviral_atv.yaml" if="$(eval 'ntu_' in bag_file)" />
<arg name="config_file" value="$(find slict)/config/mcdviral_hhs.yaml" if="$(eval 'kth_' in bag_file)" />
<arg name="config_file" value="$(find slict)/config/mcdviral_hhs.yaml" if="$(eval 'tuhh_' in bag_file)" />
<!-- <arg name="config_file" value="$(find slict)/config/mcdviral_hhs.yaml" if="$(eval 'kth_' in bag_file)" /> -->
<!-- <arg name="config_file" value="$(find slict)/config/mcdviral_hhs.yaml" if="$(eval 'tuhh_' in bag_file)" /> -->

<!-- Priormap file -->
<arg name="prior_map_dir" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/PriorMapPublish/NTU" if="$(eval 'ntu_' in bag_file)" />
<arg name="prior_map_dir" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/PriorMapPublish/KTH" if="$(eval 'kth_' in bag_file)" />
<arg name="prior_map_dir" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/PriorMapPublish/TUHH" if="$(eval 'tuhh_' in bag_file)" />
<arg name="prior_map_dir" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/PriorMapSLAM/NTU/ntu_day_01_exp" if="$(eval 'ntu_' in bag_file)" />
<!-- <arg name="prior_map_dir" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/PriorMapPublish/KTH" if="$(eval 'kth_' in bag_file)" /> -->
<!-- <arg name="prior_map_dir" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/PriorMapPublish/TUHH" if="$(eval 'tuhh_' in bag_file)" /> -->

<!-- STD descriptor for relocalization -->
<arg name="descriptor_path" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/Reloc/ntu_day_01/saved_descriptors/" if="$(eval 'ntu_' in bag_file)" />
<arg name="descriptor_path" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/Reloc/kth_day_06/saved_descriptors/" if="$(eval 'kth_' in bag_file)" />
<arg name="descriptor_path" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/Reloc/tuhh_day_03/saved_descriptors/" if="$(eval 'tuhh_' in bag_file)" />
<arg name="descriptor_path" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/PriorMapSLAM/NTU/ntu_day_01_exp/saved_descriptors/" if="$(eval 'ntu_' in bag_file)" />
<!-- <arg name="descriptor_path" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/Reloc/kth_day_06/saved_descriptors/" if="$(eval 'kth_' in bag_file)" /> -->
<!-- <arg name="descriptor_path" default="/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/Reloc/tuhh_day_03/saved_descriptors/" if="$(eval 'tuhh_' in bag_file)" /> -->

<!-- Load the param -->
<rosparam file="$(arg config_file)" command="load"/>
Expand Down
53 changes: 0 additions & 53 deletions scripts/quat2eul.ipynb
View file

This file was deleted.

Loading

0 comments on commit df226af

Please sign in to comment.