Skip to content
Snippets Groups Projects
Commit dd670363 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

setState() and getState() use a simplified design

Tested with gtest and working
parent ee38493d
No related branches found
No related tags found
No related merge requests found
...@@ -131,36 +131,25 @@ void FrameBase::setState(const Eigen::VectorXs& _st) ...@@ -131,36 +131,25 @@ void FrameBase::setState(const Eigen::VectorXs& _st)
st_check += (state_block_vec_[i]==nullptr ? 0 : state_block_vec_[i]->getSize()); st_check += (state_block_vec_[i]==nullptr ? 0 : state_block_vec_[i]->getSize());
} }
/*assert(_st.size() == ((getPPtr()==nullptr ? 0 : getPPtr()->getSize()) +
(getOPtr()==nullptr ? 0 : getOPtr()->getSize()) +
(getVPtr()==nullptr ? 0 : getVPtr()->getSize())) &&
"In FrameBase::setState wrong state size");*/
assert(_st.size() == st_check && "In FrameBase::setState wrong state size"); assert(_st.size() == st_check && "In FrameBase::setState wrong state size");
unsigned int index = 0; unsigned int index = 0;
if (getPPtr()!=nullptr)
{ for (StateBlockPtr sb : state_block_vec_)
getPPtr()->setVector(_st.head(getPPtr()->getSize())); if (sb)
index += getPPtr()->getSize(); {
} sb->setVector(_st.segment(index, sb->getSize()));
if (getOPtr()!=nullptr) index += sb->getSize();
{ }
getOPtr()->setVector(_st.segment(index, getOPtr()->getSize()));
index += getOPtr()->getSize();
}
if (getVPtr()!=nullptr)
{
getVPtr()->setVector(_st.segment(index, getVPtr()->getSize()));
// index += getVPtr()->getSize();
}
} }
Eigen::VectorXs FrameBase::getState() const Eigen::VectorXs FrameBase::getState() const
{ {
Eigen::VectorXs state((getPPtr()==nullptr ? 0 : getPPtr()->getSize()) + Size sz = 0;
(getOPtr()==nullptr ? 0 : getOPtr()->getSize()) + for (auto sb : state_block_vec_)
(getVPtr()==nullptr ? 0 : getVPtr()->getSize())); if (sb)
sz += sb->getSize();
Eigen::VectorXs state(sz);
getState(state); getState(state);
...@@ -169,26 +158,21 @@ Eigen::VectorXs FrameBase::getState() const ...@@ -169,26 +158,21 @@ Eigen::VectorXs FrameBase::getState() const
void FrameBase::getState(Eigen::VectorXs& state) const void FrameBase::getState(Eigen::VectorXs& state) const
{ {
assert(state.size() == ((getPPtr()==nullptr ? 0 : getPPtr()->getSize()) + Size sz = 0;
(getOPtr()==nullptr ? 0 : getOPtr()->getSize()) + for (auto sb : state_block_vec_)
(getVPtr()==nullptr ? 0 : getVPtr()->getSize()))); if (sb)
sz += sb->getSize();
assert(state.size() == sz && "Wrong state vector size");
unsigned int index = 0; unsigned int index = 0;
if (getPPtr()!=nullptr)
{ for (auto sb : state_block_vec_)
state.head(getPPtr()->getSize()) = getPPtr()->getVector(); if (sb)
index += getPPtr()->getSize(); {
} state.segment(index,sb->getSize()) = sb->getVector();
if (getOPtr()!=nullptr) index += sb->getSize();
{ }
state.segment(index, getOPtr()->getSize()) = getOPtr()->getVector();
index += getOPtr()->getSize();
}
if (getVPtr()!=nullptr)
{
state.segment(index, getVPtr()->getSize()) = getVPtr()->getVector();
// index += getVPtr()->getSize();
}
} }
FrameBasePtr FrameBase::getPreviousFrame() const FrameBasePtr FrameBase::getPreviousFrame() const
......
...@@ -111,6 +111,41 @@ TEST(FrameBase, LinksToTree) ...@@ -111,6 +111,41 @@ TEST(FrameBase, LinksToTree)
ASSERT_TRUE(F1->getCaptureList().empty()); ASSERT_TRUE(F1->getCaptureList().empty());
} }
#include "state_quaternion.h"
TEST(FrameBase, StateBlockVectors)
{
// Create PQV_3D state blocks
StateBlockPtr sbp = make_shared<StateBlock>(3);
StateBlockPtr sbv = make_shared<StateBlock>(3);
StateQuaternionPtr sbq = make_shared<StateQuaternion>();
// Create frame
FrameBase F(1, sbp, sbq, sbv);
// Give values to vectors and vector blocks
VectorXs x(10), x1(10), x2(10);
VectorXs p(3), v(3), q(4);
p << 1,2,3;
v << 9,8,7;
q << .5, -.5, .5, -.5;
x << p, q, v;
// Set the state, check that state blocks hold the current states
F.setState(x);
ASSERT_TRUE((p - F.getPPtr()->getVector()).isMuchSmallerThan(1, Constants::EPS_SMALL));
ASSERT_TRUE((q - F.getOPtr()->getVector()).isMuchSmallerThan(1, Constants::EPS_SMALL));
ASSERT_TRUE((v - F.getVPtr()->getVector()).isMuchSmallerThan(1, Constants::EPS_SMALL));
// Get the state, form 1 by reference
F.getState(x1);
ASSERT_TRUE((x1 - x).isMuchSmallerThan(1, Constants::EPS_SMALL));
// get the state, form 2 by return value
x2 = F.getState();
ASSERT_TRUE((x2 - x).isMuchSmallerThan(1, Constants::EPS_SMALL));
}
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment