diff --git a/src/processor_IMU.cpp b/src/processor_IMU.cpp
index 0341a5ee48dfc763f00d2fb7a2ece8a86f2f520f..14ab30164adb44619dc294c5a8e69533f07a0810 100644
--- a/src/processor_IMU.cpp
+++ b/src/processor_IMU.cpp
@@ -3,13 +3,13 @@
namespace wolf {
-ProcessorIMU::ProcessorIMU(ProcessorParamsIMUPtr _params) :
+ProcessorIMU::ProcessorIMU(const ProcessorParamsIMU& _params) :
ProcessorMotion("IMU", 10, 10, 9, 6, 0.01, 6),
- max_time_span_ (_params ? _params ->max_time_span : 1.0 ),
- max_buff_length_(_params ? _params ->max_buff_length : 10000 ),
- dist_traveled_ (_params ? _params ->dist_traveled : 1.0 ),
- angle_turned_ (_params ? _params ->angle_turned : 0.2 ),
- voting_active_ (_params ? _params ->voting_active : false )
+ max_time_span_ (_params.max_time_span ),
+ max_buff_length_(_params.max_buff_length ),
+ dist_traveled_ (_params.dist_traveled ),
+ angle_turned_ (_params.angle_turned ),
+ voting_active_ (_params.voting_active )
{
// Set constant parts of Jacobians
jacobian_delta_preint_.setIdentity(9,9); // dDp'/dDp, dDv'/dDv, all zeros
@@ -24,12 +24,22 @@ ProcessorIMU::~ProcessorIMU()
ProcessorBasePtr ProcessorIMU::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
{
- // cast inputs to the correct type
- std::shared_ptr<ProcessorParamsIMU> prc_imu_params = std::static_pointer_cast<ProcessorParamsIMU>(_params);
+ if (_params)
+ {
+ // cast inputs to the correct type
+ std::shared_ptr<ProcessorParamsIMU> prc_imu_params = std::static_pointer_cast<ProcessorParamsIMU>(_params);
- ProcessorIMUPtr prc_ptr = std::make_shared<ProcessorIMU>(prc_imu_params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
+ ProcessorIMUPtr prc_ptr = std::make_shared<ProcessorIMU>(*prc_imu_params);
+ prc_ptr->setName(_unique_name);
+ return prc_ptr;
+ }
+ else
+ {
+ ProcessorIMUPtr prc_ptr = std::make_shared<ProcessorIMU>();
+
+ prc_ptr->setName(_unique_name);
+ return prc_ptr;
+ }
}
bool ProcessorIMU::voteForKeyFrame()
diff --git a/src/processor_IMU.h b/src/processor_IMU.h
index da50c60943812a412f6057516611ec5edc00f7f6..1dab8cd154439c10e7a261dbfa94471b83103818 100644
--- a/src/processor_IMU.h
+++ b/src/processor_IMU.h
@@ -12,23 +12,23 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsIMU);
struct ProcessorParamsIMU : public ProcessorParamsBase
{
- Scalar max_time_span;
- Size max_buff_length;
- Scalar dist_traveled;
- Scalar angle_turned;
- bool voting_active; //IMU will not vote for key Frames to be created
-
-
- ProcessorParamsIMU() :
- max_time_span(0.5),
- max_buff_length(10),
- dist_traveled(5),
- angle_turned(.5),
- voting_active(false)
- {
- type = "IMU";
- name = "";
- }
+ Scalar max_time_span = 0.5;
+ Size max_buff_length = 10;
+ Scalar dist_traveled = 5;
+ Scalar angle_turned = 0.5;
+ bool voting_active = false; //IMU will not vote for key Frames to be created
+
+
+// ProcessorParamsIMU() :
+// max_time_span(0.5),
+// max_buff_length(10),
+// dist_traveled(5),
+// angle_turned(.5),
+// voting_active(false)
+// {
+// type = "IMU";
+// name = "";
+// }
};
WOLF_PTR_TYPEDEFS(ProcessorIMU);
@@ -36,7 +36,7 @@ WOLF_PTR_TYPEDEFS(ProcessorIMU);
//class
class ProcessorIMU : public ProcessorMotion{
public:
- ProcessorIMU(ProcessorParamsIMUPtr _params = nullptr);
+ ProcessorIMU(const ProcessorParamsIMU& _params = ProcessorParamsIMU());
virtual ~ProcessorIMU();
protected:
diff --git a/src/test/gtest_processor_IMU.cpp b/src/test/gtest_processor_IMU.cpp
index b6035ee527755c6e39e007c9b228abac15749077..cf0a563391970fc136001cfc3ff28b5d316db47c 100644
--- a/src/test/gtest_processor_IMU.cpp
+++ b/src/test/gtest_processor_IMU.cpp
@@ -85,19 +85,20 @@ TEST(ProcessorIMU_constructors, ALL)
//constructor without any argument
ProcessorIMUPtr prc0 = std::make_shared<ProcessorIMU>();
- ASSERT_EQ(prc0->getMaxTimeSpan(), 1.0);
- ASSERT_EQ(prc0->getMaxBuffLength(), 10000);
- ASSERT_EQ(prc0->getDistTraveled(), 1.0);
- ASSERT_EQ(prc0->getAngleTurned(), 0.2);
+ ProcessorParamsIMU params_default;
+ ASSERT_EQ(prc0->getMaxTimeSpan(), params_default.max_time_span);
+ ASSERT_EQ(prc0->getMaxBuffLength(), params_default.max_buff_length);
+ ASSERT_EQ(prc0->getDistTraveled(), params_default.dist_traveled);
+ ASSERT_EQ(prc0->getAngleTurned(), params_default.angle_turned);
//constructor with ProcessorIMUParamsPtr argument only
ProcessorParamsIMUPtr param_ptr = std::make_shared<ProcessorParamsIMU>();
- param_ptr->max_time_span = 2.0;
+ param_ptr->max_time_span = 2.0;
param_ptr->max_buff_length = 20000;
- param_ptr->dist_traveled = 2.0;
- param_ptr->angle_turned = 2.0;
+ param_ptr->dist_traveled = 2.0;
+ param_ptr->angle_turned = 2.0;
- ProcessorIMUPtr prc1 = std::make_shared<ProcessorIMU>(param_ptr);
+ ProcessorIMUPtr prc1 = std::make_shared<ProcessorIMU>(*param_ptr);
ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span);
ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length);
ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled);
@@ -109,17 +110,17 @@ TEST(ProcessorIMU_constructors, ALL)
Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), 1.0);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), 10000);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), 1.0);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), 0.2);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), params_default.max_time_span);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), params_default.max_buff_length);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), params_default.dist_traveled);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), params_default.angle_turned);
//Factory constructor with yaml
processor_ptr = problem->installProcessor("IMU", "Sec IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), 2.0);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), 2.0);
ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), 20000);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), 2.0);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), 0.2);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), 2.0);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), 0.2);
}
TEST(ProcessorIMU, voteForKeyFrame)