diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index f38d3f7e573e60fd3e44e50e6aa40fd1e2254dac..60783d1d35882fb4e7ef117267df8390dfeaf894 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -48,9 +48,6 @@ inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, b
std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(),
_transformable)
{
- // TODO: remove these lines after issue #381 is addressed
- if (not isValid(1e-5)) throw std::runtime_error("Quaternion unit norm is worse than 1e-5 tolerance!");
-
state_.normalize();
}
@@ -63,9 +60,6 @@ inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fix
{
if (state_.size() != 4) throw std::runtime_error("The quaternion state vector must be of size 4!");
- // TODO: remove these lines after issue #381 is addressed
- if (not isValid(1e-5)) throw std::runtime_error("Quaternion unit norm is worse than 1e-5 tolerance!");
-
state_.normalize();
}
@@ -77,12 +71,10 @@ inline StateQuaternion::StateQuaternion(bool _fixed, bool _transformable)
_transformable)
{
state_ = Eigen::Quaterniond::Identity().coeffs();
- //
}
inline StateQuaternion::~StateQuaternion()
{
- // The local_param_ptr_ pointer is already removed by the base class
}
inline void StateQuaternion::setIdentity(bool _notify)
diff --git a/include/core/utils/string_utils.h b/include/core/utils/string_utils.h
index 4c67b2514f6f22f9a09901cdfb94d50fcb19f581..b35d62f37aa3b7559a3f45091e96644051756013 100644
--- a/include/core/utils/string_utils.h
+++ b/include/core/utils/string_utils.h
@@ -42,5 +42,18 @@ inline std::string dateTimeNow()
std::string date_time(time_char);
return date_time;
}
-
} // namespace wolf
+
+namespace std
+{
+template <typename Derived>
+std::string to_string(const Eigen::DenseBase<Derived>& mat)
+{
+ std::stringstream ss;
+ if (mat.cols() == 1)
+ ss << mat.transpose();
+ else
+ ss << std::endl << mat;
+ return ss.str();
+}
+} // namespace std
\ No newline at end of file
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 266f8e27e074631342a0dd437fb47376b87a18e5..ccce31267c599bc3cdd1d769a28ee2e1a7cb156a 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -116,20 +116,16 @@ bool CaptureBase::hasChildren() const
FactorBasePtr CaptureBase::emplaceDriftFactor(CaptureBasePtr _capture_origin, char _key, bool _apply_loss_function)
{
- // Check that the sensor exists
- if (not getSensor())
+ // Checks
+ if (not getSensor()) // sensor exists
throw std::runtime_error("Attempting to call emplaceDriftFactor to a capture with no associated sensor.");
- // Check that the states exist
- if (not getStateBlock(_key) or not _capture_origin->getStateBlock(_key))
+ if (not getStateBlock(_key) or not _capture_origin->getStateBlock(_key)) // states exist
throw std::runtime_error("Attempting to call emplaceDriftFactor to a nullptr state.");
- // Check that the state is dynamic
- if (not getSensor()->isStateBlockDynamic(_key))
+ if (not getSensor()->isStateBlockDynamic(_key)) // state is dynamic
throw std::runtime_error("Attempting to call emplaceDriftFactor to a non-dynamic state.");
- // Check that sensor has drift specified
- if (not getSensor()->hasDrift(_key))
+ if (not getSensor()->hasDrift(_key)) // drift specified
throw std::runtime_error("Attempting to call emplaceDriftFactor to a state without drift specified.");
- // Check that _capture_oriding is different than this
- if (_capture_origin == shared_from_this_capture())
+ if (_capture_origin == shared_from_this_capture()) //_capture_origin is different than this
throw std::runtime_error("Attempting to call emplaceDriftFactor to the same capture.");
// Feature drift
diff --git a/src/common/node_state_blocks.cpp b/src/common/node_state_blocks.cpp
index fbd923fc6c17e6e33fdedd0d913b6c05ae3aae87..b14904b48ce010eef73dd79124eb7d1188a56d6c 100644
--- a/src/common/node_state_blocks.cpp
+++ b/src/common/node_state_blocks.cpp
@@ -19,6 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/common/node_state_blocks.h"
+#include "core/utils/string_utils.h"
#include "core/state_block/factory_state_block.h"
#include "core/math/covariance.h"
#include "core/factor/factor_block_absolute.h"
@@ -104,6 +105,11 @@ StateBlockPtr NodeStateBlocks::emplaceStateBlock(const char _key,
// create state block
auto sb = FactoryStateBlock::create(_type, _vector, _fixed);
+ // if local parameterization, check is valid (normalized for quaternions)
+ if (sb->hasLocalParametrization() and not sb->isValid(1e-4))
+ throw std::runtime_error(std::string("NodeStateBlocks::emplaceStateBlock: State values provided for key '") + _key +
+ "' of type '" + sb->getType() + "' are not valid according the local parameterization (tolerance: 1e-4): " + std::to_string(_vector));
+
// store state block
state_blocks_.emplace(_key, sb);
@@ -134,8 +140,7 @@ void NodeStateBlocks::emplaceFactorStateBlock(const char& _key,
if (_start_idx != 0)
throw std::runtime_error(
"NodeStateBlocks::emplaceFactorStateBlock: Prior factor for a segment of a state with local size "
- "different "
- "from size not allowed");
+ "different from size not allowed");
// meas size
if (_x.size() != _sb->getSize())
throw std::runtime_error("NodeStateBlocks::emplaceFactorStateBlock: Bad measurement size");
@@ -161,16 +166,6 @@ void NodeStateBlocks::emplaceFactorStateBlock(const char& _key,
std::string("NodeStateBlocks::emplaceFactorStateBlock: There already is a factor prior for this key ") +
_key + ", factor " + std::to_string(factor_prior_map_.at(_key)->id()));
- // SET STATE
- if (_x.size() == _sb->getSize())
- _sb->setState(_x);
- else
- {
- VectorXd new_x = _sb->getState();
- new_x.segment(_start_idx, _x.size()) = _x;
- _sb->setState(new_x);
- }
-
// EMPLACE FACTOR
bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr);
if (is_quaternion)
diff --git a/test/gtest_node_state_blocks.cpp b/test/gtest_node_state_blocks.cpp
index 35c627f242131c7eb62f29df96d55c023d77c2c4..242599afcd7ccb89bd333bdc91c0066191ee1ea0 100644
--- a/test/gtest_node_state_blocks.cpp
+++ b/test/gtest_node_state_blocks.cpp
@@ -127,7 +127,7 @@ TEST(NodeStateBlocksTest, constructor_type_vector_prior)
ASSERT_FALSE(N->isLandmark());
ASSERT_FALSE(N->isSensor());
- // apply priors (fix and initial guess priors wouldn't be necessary to put here)
+ // emplace priors
N->emplacePriors();
checkNode(N, 'P', p_state, true, MatrixXd(0, 0));