work on scripts

bag/generate_trajectory_csv.bash

 #!/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

bag/shorten_topic.bash

 #!/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

bag/transpose_csv.py

 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")
+

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"

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"

yaml/solver.yaml

 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

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

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