diff --git a/bag/generate_trajectory_csv.bash b/bag/generate_trajectory_csv.bash
index a5c8f600f0bcfca4bf33bc1f6d7ebea41a000623..67bdbf5b774a88bfc2020b29564064fb5ce394bb 100644
--- a/bag/generate_trajectory_csv.bash
+++ b/bag/generate_trajectory_csv.bash
@@ -1,4 +1,17 @@
#!/bin/bash
-rosbag filter trajectory_recording.bag trajectory_recording_filtered.bag "t.to_sec() >=1620124021"
-rostopic echo -b trajectory_recording_filtered.bag /wolf_ros_node/trajectory -p > trajectory.csv
-python3 transpose_csv.py
+BAG="ana_lab_2"
+t0=1620123952.21
+tend=1620123952.21
+
+#roslaunch wolf_demo_imu2d imu2d_demo.launch bag:=$BAG\_filtered_notf test:=0 speed:=1 suffix:="_GT"
+#rosbag filter trajectory_recording_GT.bag trajectory_recording_GT_tmp.bag "t.to_sec >= $tend"
+#rostopic echo -b trajectory_recording_GT_tmp.bag /wolf_ros_node/trajectory -p > trajectory_$BAG\_exp_$i.csv
+rm trajectory_recording_GT_tmp.bag
+#python3 transpose_csv.py trajectory_$BAG\_exp_$i.csv trajectory_$BAG\_exp_$i\_transp.csv
+for i in 10 20 30 40 50
+do
+ filtertime=$t0+$i+30.0-1.0
+ roslaunch wolf_demo_imu2d imu2d_demo.launch bag:=$BAG\_shortened_$i test:=5 speed:=1 suffix:="_$i"
+ rostopic echo -b trajectory_recording_$i.bag /wolf_ros_node/pose_pose_with_cov -p > trajectory_$BAG\_exp_$i.csv
+ python3 transpose_csv.py trajectory_$BAG\_exp_$i.csv trajectory_$BAG\_exp_$i\_transp.csv
+done
diff --git a/bag/shorten_topic.bash b/bag/shorten_topic.bash
index 61c8184d583cdf161fcf0ae7f9593e14477da853..7657ccbee8d1abd243035241aeef2ae303f28854 100644
--- a/bag/shorten_topic.bash
+++ b/bag/shorten_topic.bash
@@ -1,3 +1,12 @@
#!/bin/bash
BAG="ana_lab_2"
-rosbag filter $BAG\_filtered_notf.bag $BAG\_shortened.bag "topic != '/ana/sensors/scan' or (topic == '/ana/sensors/scan' and t.to_sec() <= 1620124012)"
+tstartstr=$(rosbag info $BAG.bag | grep start)
+t0=$(echo $tstr | cut -d "(" -f2 | cut -d ")" -f1 | bc -l)
+
+for i in 10 20 30 40 50
+do
+ cuttime=$(($t0+$i))
+ echo $cuttime
+ endtime=$(($cuttime+30.0))
+ rosbag filter $BAG\_filtered_notf.bag $BAG\_shortened\_$i.bag "(topic != '/ana/sensors/scan' and t.to_sec() <=$endtime) or (topic == '/ana/sensors/scan' and t.to_sec() <= $cuttime)"
+done
diff --git a/bag/transpose_csv.py b/bag/transpose_csv.py
index d28863c2498208047632b021035c7063fcdfa618..75296bd35fed68785e46deef9391c9b8cb24a4e3 100644
--- a/bag/transpose_csv.py
+++ b/bag/transpose_csv.py
@@ -1,2 +1,11 @@
import pandas as pd
-pd.read_csv('trajectory.csv', header=None).T.to_csv('trajectory_transp.csv', header=False, index=False)
+import sys
+def transpose(in_bag, out_bag):
+ pd.read_csv(in_bag, header=None).T.to_csv(out_bag, header=False, index=False)
+
+if __name__ == '__main__':
+ in_bag = sys.argv[1]
+ out_bag = sys.argv[2]
+ transpose(in_bag, out_bag)
+ print("Done")
+
diff --git a/launch/imu2d_demo.launch b/launch/imu2d_demo.launch
index 7a30284c079b603fdaec2807ae3a248ae7111fbc..0b074634a0211c11e57855b9d99d2315aa844eb9 100644
--- a/launch/imu2d_demo.launch
+++ b/launch/imu2d_demo.launch
@@ -7,13 +7,14 @@
<arg name="gdb" default="false" />
<arg name="test" default="1" />
<arg name="bag" default="test_no_map" />
+ <arg name="suffix" default="" />
<arg name="launch_pref" default="" unless="$(eval profiling or gdb)"/>
<arg name="launch_pref" value="valgrind --tool=callgrind --callgrind-out-file='callgrind.wolf.%p'" if="$(arg profiling)" />
<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="rosbag" type="record" name="recorder" args='record -O $(find wolf_demo_imu2d)/bag/trajectory_recording.bag "/wolf_ros_node/trajectory" "/wolf_ros_node/graph_trajectory" "/wolf_ros_node/pose_pos_array"'/>
+ <node pkg="rosbag" type="record" name="recorder" args='record -O $(find wolf_demo_imu2d)/bag/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov"'/>
<node pkg="tf"
type="static_transform_publisher"
@@ -38,7 +39,8 @@
<node pkg="rosbag"
type="play"
name="player"
- args="-s $(arg sec) -r $(arg speed) -k --clock $(find wolf_demo_imu2d)/bag/$(arg bag).bag"/>
+ required="true"
+ args="-s $(arg sec) -r $(arg speed) --clock $(find wolf_demo_imu2d)/bag/$(arg bag).bag"/>
<node type="rviz"
name="rviz"
diff --git a/yaml/imu2d_test5.yaml b/yaml/imu2d_test5.yaml
index d2168404f832598c1b65f4efa8ed2e47a42fb4b6..37f81ab22d132723a3b9a4b207aee755b7d953f1 100644
--- a/yaml/imu2d_test5.yaml
+++ b/yaml/imu2d_test5.yaml
@@ -62,13 +62,13 @@ config:
apply_loss_function: true
keyframe_vote:
voting_active: false
- recent_frames_ignored: 3
+ recent_frames_ignored: 10
frames_ignored_after_loop: 0
max_error_threshold: 0.02
min_points_percent: 40
- max_loops: 3
+ max_loops: 1
max_candidates: 5
- max_attempts: 15
+ max_attempts: 5
candidate_generation: "random" # 'random' or 'tree'
icp:
follow: "csm.yaml"
diff --git a/yaml/solver.yaml b/yaml/solver.yaml
index ef04fd43fffb6b5b6ede533e948e125d365f212d..cb52fa3d63983c71bcc06e5bc4c4f6b1daa6d28a 100644
--- a/yaml/solver.yaml
+++ b/yaml/solver.yaml
@@ -1,5 +1,5 @@
minimizer: LEVENBERG_MARQUARDT
-max_num_iterations: 20
+max_num_iterations: 100
verbose: 0
period: 0.0
n_threads: 2
@@ -8,6 +8,6 @@ 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
+function_tolerance: 1e-9
+gradient_tolerance: 1e-10
use_nonmonotonic_steps: false
diff --git a/yaml/test_imu_params_microstrain.yaml b/yaml/test_imu_params_microstrain.yaml
index 05b747ba2880c81b3bbe7232eb74815675a7ee3c..8ff559405ab5152c537da910a4d9abfac552dc6f 100644
--- a/yaml/test_imu_params_microstrain.yaml
+++ b/yaml/test_imu_params_microstrain.yaml
@@ -3,8 +3,8 @@ extrinsic:
pose: [0,0,0]
a_noise: 0.9 #0.9
w_noise: 0.01 #0.01
-ab_initial_stdev: 0.05 # m/s2 - initial bias
-wb_initial_stdev: 0.01 # rad/sec - initial bias
-ab_rate_stdev: 0.00001 # m/s2/sqrt(s)
-wb_rate_stdev: 0.00001 # rad/s/sqrt(s)
+ab_initial_stdev: 0.5 # m/s2 - initial bias
+wb_initial_stdev: 0.1 # rad/sec - initial bias
+ab_rate_stdev: 0.001 # m/s2/sqrt(s)
+wb_rate_stdev: 0.001 # rad/s/sqrt(s)
orthogonal_gravity: true
diff --git a/yaml/test_imu_subscriber_microstrain.yaml b/yaml/test_imu_subscriber_microstrain.yaml
index 7c747b9d2dc4907c1128084f2929cc34a8c10542..1bf1e8c462c3917a25c3351d3749473cc930d7d2 100644
--- a/yaml/test_imu_subscriber_microstrain.yaml
+++ b/yaml/test_imu_subscriber_microstrain.yaml
@@ -9,5 +9,5 @@ in_degrees: false
#all
topic_enable: "/helena/sensors/bno055_imu/imu/topic_enable"
static_init_duration: 3
-lowpass_filter: true
+lowpass_filter: false
lowpass_cutoff_freq: 5