From 178067c7aff26cd9aabdd84698620adfaed5ba3b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 11 Apr 2019 10:29:15 +0200
Subject: [PATCH] Addressing issue #158
---
src/frame_base.cpp | 9 ++-------
src/landmark/landmark_base.cpp | 9 ++-------
src/problem.cpp | 6 ------
3 files changed, 4 insertions(+), 20 deletions(-)
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index ccd51dc7f..71e486711 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -177,11 +177,7 @@ void FrameBase::setState(const Eigen::VectorXs& _state)
Eigen::VectorXs FrameBase::getState() const
{
- SizeEigen size = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- size += sb->getSize();
- Eigen::VectorXs state(size);
+ Eigen::VectorXs state;
getState(state);
@@ -195,10 +191,9 @@ void FrameBase::getState(Eigen::VectorXs& _state) const
if (sb)
size += sb->getSize();
- assert(_state.size() == size && "Wrong state vector size");
+ _state = Eigen::VectorXs(size);
SizeEigen index = 0;
-
for (StateBlockPtr sb : state_block_vec_)
if (sb)
{
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index d1fb63043..8ee7568ab 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -122,11 +122,7 @@ void LandmarkBase::removeStateBlocks()
Eigen::VectorXs LandmarkBase::getState() const
{
- SizeEigen size = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- size += sb->getSize();
- Eigen::VectorXs state(size);
+ Eigen::VectorXs state;
getState(state);
@@ -140,10 +136,9 @@ void LandmarkBase::getState(Eigen::VectorXs& _state) const
if (sb)
size += sb->getSize();
- assert(_state.size() == size && "Wrong state vector size");
+ _state = Eigen::VectorXs(size);
SizeEigen index = 0;
-
for (StateBlockPtr sb : state_block_vec_)
if (sb)
{
diff --git a/src/problem.cpp b/src/problem.cpp
index b9cd0a1e6..c95a725a5 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -249,8 +249,6 @@ Eigen::VectorXs Problem::getCurrentState()
void Problem::getCurrentState(Eigen::VectorXs& state)
{
- assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size");
-
if (processor_motion_ptr_ != nullptr)
processor_motion_ptr_->getCurrentState(state);
else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
@@ -261,8 +259,6 @@ void Problem::getCurrentState(Eigen::VectorXs& state)
void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
{
- assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size");
-
if (processor_motion_ptr_ != nullptr)
{
processor_motion_ptr_->getCurrentState(state);
@@ -279,8 +275,6 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state)
{
- assert(state.size() == getFrameStructureSize() && "Problem::getStateAtTimeStamp: bad state size");
-
// try to get the state from processor_motion if any, otherwise...
if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, state))
{
--
GitLab