From 482dc360b38f04b14b2b668426db3cb325bbc28f Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 16 Feb 2021 09:02:55 +0100
Subject: [PATCH] compiling
---
src/sensor/sensor_base.cpp | 36 ++----------------------------------
1 file changed, 2 insertions(+), 34 deletions(-)
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index f5e091572..a9f0ecf1d 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -1,5 +1,6 @@
#include "core/sensor/sensor_base.h"
#include "core/state_block/state_block.h"
+#include "core/state_block/state_angle.h"
#include "core/state_block/state_quaternion.h"
#include "core/factor/factor_block_absolute.h"
#include "core/factor/factor_quaternion_absolute.h"
@@ -30,7 +31,7 @@ SensorBase::SensorBase(const std::string& _type,
(key != 'O' ?
std::make_shared<StateBlock>(sb_param.state,sb_param.prior_mode == "fix") :
(_params->dim == 2 ?
- std::static_pointer_cast<StateBlock>(std::make_shared<StateAngle>(sb_param.state, sb_param.prior_mode == "fix")) :
+ std::static_pointer_cast<StateBlock>(std::make_shared<StateAngle>(sb_param.state(0), sb_param.prior_mode == "fix")) :
std::static_pointer_cast<StateBlock>(std::make_shared<StateQuaternion>(sb_param.state,sb_param.prior_mode == "fix")) ) ),
sb_param.dynamic);
@@ -39,39 +40,6 @@ SensorBase::SensorBase(const std::string& _type,
sb_param.state,
sb_param.sigma.asDiagonal());
}
-
- /*// P
- addStateBlock('P',
- std::make_shared<StateBlock>(_params->state_blocks.at('P').state,
- _params->state_blocks.at('P').prior_mode == "fix"),
- _params->state_blocks.at('P').dynamic);
-
- if (_params->state_blocks.at('P').prior_mode == "factor")
- addPriorP(_params->state_blocks.at('P').state, _params->state_blocks.at('P').sigma.asDiagonal());
-
- // O
- addStateBlock('O',
- (_params->dim == 2 ?
- std::make_shared<StateAngle>(_params->state_blocks.at('O').state,
- _params->state_blocks.at('O').prior_mode == "fix") :
- std::make_shared<StateQuaternion>(_params->state_blocks.at('O').state,
- _params->state_blocks.at('O').prior_mode == "fix") ),
- _params->o_dynamic);
-
- if (_params->state_blocks.at('O').prior_mode == "factor")
- addPriorO(_params->p_state, _params->p_cov);
-
- // I
- if (_intrinsics_size != 0)
- {
- addStateBlock('I',
- std::make_shared<StateBlock>(_params->state_blocks.at('I').state,
- _params->state_blocks.at('I').prior_mode == "fix"),
- _params->state_blocks.at('I').dynamic);
-
- if (_params->state_blocks.at('I').prior_mode == "factor")
- addPriorI(_params->state_blocks.at('I').state, _params->state_blocks.at('I').sigma.asDiagonal());
- }*/
}
SensorBase::SensorBase(const std::string& _type,
--
GitLab