diff --git a/demos/processor_odom_3D.yaml b/demos/processor_odom_3D.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a0205b5fa6285d62085e86b12c875d0632787b19
--- /dev/null
+++ b/demos/processor_odom_3D.yaml
@@ -0,0 +1,13 @@
+type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
+
+time_tolerance: 0.01 # seconds
+
+voting_active: false
+voting_aux_active: false
+max_time_span: 0.2 # seconds
+max_buff_length: 10 # motion deltas
+dist_traveled: 0.5 # meters
+angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+unmeasured_perturbation_std: 0.001
\ No newline at end of file
diff --git a/demos/sensor_odom_3D.yaml b/demos/sensor_odom_3D.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d0c1f0b0e4d025090170cbbd85ea3ae4cfc9f62b
--- /dev/null
+++ b/demos/sensor_odom_3D.yaml
@@ -0,0 +1,8 @@
+type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
+
+k_disp_to_disp: 0.02 # m^2 / m
+k_disp_to_rot: 0.02 # rad^2 / m
+k_rot_to_rot: 0.01 # rad^2 / rad
+min_disp_var: 0.01 # m^2
+min_rot_var: 0.01 # rad^2
diff --git a/include/IMU/sensor/sensor_IMU.h b/include/IMU/sensor/sensor_IMU.h
index 06d8f9bffab322f5916ad68f8fbdb346da02602f..e1afc10e2f9ac49044457bfbf44a9c685425f89b 100644
--- a/include/IMU/sensor/sensor_IMU.h
+++ b/include/IMU/sensor/sensor_IMU.h
@@ -10,8 +10,6 @@ namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsIMU);
-//TODO : bias_ptr defined as intrinsics StateBlock in constructor (see SensorBase) but here we also have another intrinsics
-// This is confusing.
struct IntrinsicsIMU : public IntrinsicsBase
{
@@ -35,12 +33,12 @@ struct IntrinsicsIMU : public IntrinsicsBase
IntrinsicsIMU(std::string _unique_name, const ParamsServer& _server):
IntrinsicsBase(_unique_name, _server)
{
- w_noise = _server.getParam<Scalar>(_unique_name + "/w_noise", "0.001");
- a_noise = _server.getParam<Scalar>(_unique_name + "/a_noise", "0.004");
- ab_initial_stdev = _server.getParam<Scalar>(_unique_name + "/ab_initial_stdev", "0.01");
- wb_initial_stdev = _server.getParam<Scalar>(_unique_name + "/wb_initial_stdev", "0.01");
- ab_rate_stdev = _server.getParam<Scalar>(_unique_name + "/ab_rate_stdev", "0.00001");
- wb_rate_stdev = _server.getParam<Scalar>(_unique_name + "/wb_rate_stdev", "0.00001");
+ w_noise = _server.getParam<Scalar>(_unique_name + "/w_noise");
+ a_noise = _server.getParam<Scalar>(_unique_name + "/a_noise");
+ ab_initial_stdev = _server.getParam<Scalar>(_unique_name + "/ab_initial_stdev");
+ wb_initial_stdev = _server.getParam<Scalar>(_unique_name + "/wb_initial_stdev");
+ ab_rate_stdev = _server.getParam<Scalar>(_unique_name + "/ab_rate_stdev");
+ wb_rate_stdev = _server.getParam<Scalar>(_unique_name + "/wb_rate_stdev");
}
std::string print()
{
@@ -64,10 +62,10 @@ class SensorIMU : public SensorBase
Scalar w_noise; //Rate Noise Spectral Density (same for all the axis) in deg/sec/ sqrt(Hz)
//This is a trial to factor how much can the bias change in 1 sec at most
- Scalar ab_initial_stdev; //accelerometer micro_g/sec
+ Scalar ab_initial_stdev; //accelerometer m/sec^s
Scalar wb_initial_stdev; //gyroscope rad/sec
- Scalar ab_rate_stdev; //accelerometer micro_g/sec
- Scalar wb_rate_stdev; //gyroscope rad/sec
+ Scalar ab_rate_stdev; //accelerometer m/sec^2 / sqrt(sec)
+ Scalar wb_rate_stdev; //gyroscope rad/sec / sqrt(sec)
public:
diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp
index f6044cf0037b1d0fa3644fab899cda2aea354f2d..448437cab67a9b6300b186e38f3b94e2f02ab415 100644
--- a/test/gtest_IMU.cpp
+++ b/test/gtest_IMU.cpp
@@ -8,16 +8,17 @@
//Wolf
#include "IMU/processor/processor_IMU.h"
#include "IMU/sensor/sensor_IMU.h"
-#include "core/common/wolf.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/processor/processor_odom_3D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-#include <core/utils/utils_gtest.h>
-#include "core/utils/logging.h"
#include "IMU/internal/config.h"
+#include <core/common/wolf.h>
+#include <core/sensor/sensor_odom_3D.h>
+#include <core/processor/processor_odom_3D.h>
+#include <core/ceres_wrapper/ceres_manager.h>
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+
// make my life easier
using namespace Eigen;
using namespace wolf;
@@ -604,17 +605,12 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
SensorBasePtr sensor = problem->installSensor ("ODOM 3D", "Odometer", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml" );
ProcessorBasePtr processor = problem->installProcessor("ODOM 3D", "Odometer", "Odometer" , wolf_root + "/demos/processor_odom_3D.yaml");
+
sensor_odo = static_pointer_cast<SensorOdom3D>(sensor);
processor_odo = static_pointer_cast<ProcessorOdom3D>(processor);
-
processor_odo->setTimeTolerance(dt/2);
-
- // prevent this processor from voting by setting high thresholds :
- processor_odo->setAngleTurned(999);
- processor_odo->setDistTraveled(999);
- processor_odo->setMaxBuffLength(999);
- processor_odo->setMaxTimeSpan(999);
+ processor_odo->setVotingActive(false);
}
virtual void integrateAll() override
@@ -1507,7 +1503,7 @@ int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
// ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU.*";
- // ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU_ODO.*";
+ ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU_ODO.*";
// ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
return RUN_ALL_TESTS();