Imu2d static initialization

include/imu/factor/factor_imu.h

@@ -4,6 +4,7 @@
 //Wolf includes
 #include "imu/feature/feature_imu.h"
 #include "imu/sensor/sensor_imu.h"
+#include "imu/processor/processor_imu.h"
 #include "core/factor/factor_autodiff.h"
 #include "core/math/rotations.h"
 

test/CMakeLists.txt

@@ -37,6 +37,12 @@ target_link_libraries(gtest_feature_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
 wolf_add_gtest(gtest_factor_imu2d gtest_factor_imu2d.cpp)
 target_link_libraries(gtest_factor_imu2d ${PLUGIN_NAME} ${wolf_LIBRARY})
 
+wolf_add_gtest(gtest_imu_static_init gtest_imu_static_init.cpp)
+target_link_libraries(gtest_imu_static_init ${PLUGIN_NAME} ${wolf_LIBRARY})
+
+wolf_add_gtest(gtest_imu2d_static_init gtest_imu2d_static_init.cpp)
+target_link_libraries(gtest_imu2d_static_init ${PLUGIN_NAME} ${wolf_LIBRARY})
+
 wolf_add_gtest(gtest_imu_mocap_fusion gtest_imu_mocap_fusion.cpp)
 target_link_libraries(gtest_imu_mocap_fusion ${PLUGIN_NAME} ${wolf_LIBRARY})
 

test/gtest_processor_motion_intrinsics_update.cpp

@@ -155,7 +155,7 @@ TEST_F(ProcessorImuTest, test1)
  *  SET TO TRUE  TO PRODUCE CSV FILE
  *  SET TO FALSE TO STOP PRODUCING CSV FILE
  */
-#define WRITE_CSV_FILE  false
+#define WRITE_CSV_FILE  true
 
 TEST_F(ProcessorImuTest, getState)
 {

test/yaml/imu2d_static_init.yaml

+type: "ProcessorImu2d"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+time_tolerance: 0.0025         # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.0001
+
+keyframe_vote:
+    voting_active:      true
+    voting_aux_active:  false
+    max_time_span:      0.9999   # seconds
+    max_buff_length:    1000000000   # motion deltas
+    dist_traveled:      100000000000   # meters
+    angle_turned:       10000000000   # radians (1 rad approx 57 deg, approx 60 deg)

test/yaml/imu_static_init.yaml

+type: "ProcessorImu"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+time_tolerance: 0.0025         # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.0001
+
+keyframe_vote:
+    voting_active:      true
+    voting_aux_active:  false
+    max_time_span:      0.9999   # seconds
+    max_buff_length:    1000000000   # motion deltas
+    dist_traveled:      100000000000   # meters
+    angle_turned:       10000000000   # radians (1 rad approx 57 deg, approx 60 deg)

test/yaml/sensor_imu.yaml

+type: "SensorImu"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+motion_variances: 
+    a_noise:                0.053     # standard deviation of Acceleration noise (same for all the axis) in m/s2
+    w_noise:                0.0011    # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+    ab_initial_stdev:       0.800     # m/s2    - initial bias 
+    wb_initial_stdev:       0.350     # rad/sec - initial bias 
+    ab_rate_stdev:          0.1       # m/s2/sqrt(s)           
+    wb_rate_stdev:          0.0400    # rad/s/sqrt(s)

test/yaml/sensor_imu2d_static_init.yaml

+type: "SensorImu2d"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+motion_variances: 
+    a_noise:                0.053     # standard deviation of Acceleration noise (same for all the axis) in m/s2
+    w_noise:                0.0011    # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+    ab_initial_stdev:       0.800     # m/s2    - initial bias 
+    wb_initial_stdev:       0.350     # rad/sec - initial bias 
+    ab_rate_stdev:          0.1       # m/s2/sqrt(s)           
+    wb_rate_stdev:          0.0400    # rad/s/sqrt(s)

test/yaml/sensor_imu_static_init.yaml

+type: "SensorImu"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+motion_variances: 
+    a_noise:                0.001     # standard deviation of Acceleration noise (same for all the axis) in m/s2
+    w_noise:                0.001    # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+    ab_initial_stdev:       0.001     # m/s2    - initial bias 
+    wb_initial_stdev:       0.001     # rad/sec - initial bias 
+    ab_rate_stdev:          0.001       # m/s2/sqrt(s)           
+    wb_rate_stdev:          0.001    # rad/s/sqrt(s)