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

Add setIdentity() and setZero() to state_block

parent 44c1b250
No related branches found
No related tags found
2 merge requests!358WIP: Resolve "Complete state vector new data structure?",!343WIP: Resolve "Complete state vector new data structure?"
...@@ -146,6 +146,9 @@ public: ...@@ -146,6 +146,9 @@ public:
**/ **/
void resetLocalParamUpdated(); void resetLocalParamUpdated();
virtual void setIdentity(bool _notify = true);
virtual void setZero (bool _notify = true);
/** \brief perturb state /** \brief perturb state
*/ */
void perturb(double amplitude = 0.1); void perturb(double amplitude = 0.1);
...@@ -286,6 +289,17 @@ inline double* StateBlock::getStateData() ...@@ -286,6 +289,17 @@ inline double* StateBlock::getStateData()
return state_.data(); return state_.data();
} }
inline void StateBlock::setIdentity(bool _notify)
{
setState( VectorXd::Zero(state_size_), _notify );
}
inline void StateBlock::setZero(bool _notify)
{
setIdentity(_notify);
}
}// namespace wolf }// namespace wolf
#endif #endif
...@@ -19,6 +19,7 @@ class StateHomogeneous3d : public StateBlock ...@@ -19,6 +19,7 @@ class StateHomogeneous3d : public StateBlock
StateHomogeneous3d(bool _fixed = false); StateHomogeneous3d(bool _fixed = false);
StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false); StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false);
virtual ~StateHomogeneous3d(); virtual ~StateHomogeneous3d();
virtual void setIdentity(bool _notify = true) override;
}; };
inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed) : inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed) :
...@@ -40,6 +41,13 @@ inline StateHomogeneous3d::~StateHomogeneous3d() ...@@ -40,6 +41,13 @@ inline StateHomogeneous3d::~StateHomogeneous3d()
// The local_param_ptr_ pointer is already removed by the base class // The local_param_ptr_ pointer is already removed by the base class
} }
inline void StateHomogeneous3d::setIdentity(bool _notify)
{
setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
}
} // namespace wolf } // namespace wolf
#endif /* SRC_STATE_HOMOGENEOUS_3d_H_ */ #endif /* SRC_STATE_HOMOGENEOUS_3d_H_ */
...@@ -20,6 +20,7 @@ class StateQuaternion : public StateBlock ...@@ -20,6 +20,7 @@ class StateQuaternion : public StateBlock
StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false); StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false);
StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false); StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false);
virtual ~StateQuaternion(); virtual ~StateQuaternion();
virtual void setIdentity(bool _notify = true) override;
}; };
inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) : inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) :
...@@ -45,6 +46,11 @@ inline StateQuaternion::~StateQuaternion() ...@@ -45,6 +46,11 @@ inline StateQuaternion::~StateQuaternion()
// The local_param_ptr_ pointer is already removed by the base class // The local_param_ptr_ pointer is already removed by the base class
} }
inline void StateQuaternion::setIdentity(bool _notify)
{
setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
}
} // namespace wolf } // namespace wolf
#endif /* SRC_STATE_QUATERNION_H_ */ #endif /* SRC_STATE_QUATERNION_H_ */
...@@ -19,6 +19,69 @@ using namespace Eigen; ...@@ -19,6 +19,69 @@ using namespace Eigen;
using namespace std; using namespace std;
using namespace wolf; using namespace wolf;
TEST(StateBlock, identity)
{
Vector3d x(10,20,30);
StateBlock sb(x);
sb.setIdentity();
ASSERT_MATRIX_APPROX(sb.getState(), Vector3d::Zero() , 1e-20);
}
TEST(StateAngle, identity)
{
StateAngle sb(1.0);
sb.setIdentity();
ASSERT_MATRIX_APPROX(Vector1d::Zero() , sb.getState() , 1e-20);
}
TEST(StateQuaternion, identity)
{
Vector4d x(0.5,0.5,0.5,0.5);
StateQuaternion sb(x);
sb.setIdentity();
Vector4d q1(0 ,0,0,1);
ASSERT_MATRIX_APPROX(q1 , sb.getState() , 1e-20);
}
TEST(StateBlock, zero)
{
Vector3d x(10,20,30);
StateBlock sb(x);
sb.setZero();
ASSERT_MATRIX_APPROX(sb.getState(), Vector3d::Zero() , 1e-20);
}
TEST(StateAngle, zero)
{
StateAngle sb(1.0);
sb.setZero();
ASSERT_MATRIX_APPROX(Vector1d::Zero() , sb.getState() , 1e-20);
}
TEST(StateQuaternion, zero)
{
Vector4d x(0.5,0.5,0.5,0.5);
StateQuaternion sb(x);
sb.setZero();
Vector4d q1(0 ,0,0,1);
ASSERT_MATRIX_APPROX(q1 , sb.getState() , 1e-20);
}
TEST(StateBlock, plus) TEST(StateBlock, plus)
{ {
Vector3d x(10,20,30); Vector3d x(10,20,30);
...@@ -33,7 +96,7 @@ TEST(StateBlock, plus) ...@@ -33,7 +96,7 @@ TEST(StateBlock, plus)
ASSERT_MATRIX_APPROX(x + dx , sb.getState() , 1e-20); ASSERT_MATRIX_APPROX(x + dx , sb.getState() , 1e-20);
} }
TEST(StateBlock, angle_plus) TEST(StateAngle, plus)
{ {
StateAngle sb(1.0); StateAngle sb(1.0);
...@@ -46,7 +109,7 @@ TEST(StateBlock, angle_plus) ...@@ -46,7 +109,7 @@ TEST(StateBlock, angle_plus)
ASSERT_MATRIX_APPROX(Vector1d(1.0) + dx , sb.getState() , 1e-20); ASSERT_MATRIX_APPROX(Vector1d(1.0) + dx , sb.getState() , 1e-20);
} }
TEST(StateBlock, angle_plus_past_pi) TEST(StateAngle, plus_past_pi)
{ {
StateAngle sb(3.14); StateAngle sb(3.14);
...@@ -59,7 +122,7 @@ TEST(StateBlock, angle_plus_past_pi) ...@@ -59,7 +122,7 @@ TEST(StateBlock, angle_plus_past_pi)
ASSERT_MATRIX_APPROX(Vector1d(3.14 - 2.0*M_PI) + dx , sb.getState() , 1e-20); ASSERT_MATRIX_APPROX(Vector1d(3.14 - 2.0*M_PI) + dx , sb.getState() , 1e-20);
} }
TEST(StateBlock, quaternion_plus_norm) TEST(StateQuaternion, plus_norm)
{ {
Vector4d x(0.5,0.5,0.5,0.5); Vector4d x(0.5,0.5,0.5,0.5);
StateQuaternion sb(x); StateQuaternion sb(x);
...@@ -72,7 +135,7 @@ TEST(StateBlock, quaternion_plus_norm) ...@@ -72,7 +135,7 @@ TEST(StateBlock, quaternion_plus_norm)
ASSERT_LT((sb.getState().transpose() * x).norm() , 1.0); // quaternions are not parallel ==> not equal ASSERT_LT((sb.getState().transpose() * x).norm() , 1.0); // quaternions are not parallel ==> not equal
} }
TEST(StateBlock, quaternion_plus_trivial) TEST(StateQuaternion, plus_trivial)
{ {
Vector4d x(0,0,0,1); // identity (0,0,0,1) Vector4d x(0,0,0,1); // identity (0,0,0,1)
StateQuaternion sb(x); StateQuaternion sb(x);
...@@ -86,7 +149,7 @@ TEST(StateBlock, quaternion_plus_trivial) ...@@ -86,7 +149,7 @@ TEST(StateBlock, quaternion_plus_trivial)
} }
TEST(StateBlock, block_perturb) TEST(StateBlock, perturb)
{ {
Vector3d x(10,20,30); Vector3d x(10,20,30);
StateBlock sb(x); StateBlock sb(x);
...@@ -99,7 +162,7 @@ TEST(StateBlock, block_perturb) ...@@ -99,7 +162,7 @@ TEST(StateBlock, block_perturb)
ASSERT_MATRIX_APPROX(x , sb.getState() , 0.5 * 4); // 4-sigma test... ASSERT_MATRIX_APPROX(x , sb.getState() , 0.5 * 4); // 4-sigma test...
} }
TEST(StateBlock, angle_perturb) TEST(StateAngle, perturb)
{ {
Vector1d x(1.0); Vector1d x(1.0);
StateAngle sb(x(0)); StateAngle sb(x(0));
...@@ -112,7 +175,7 @@ TEST(StateBlock, angle_perturb) ...@@ -112,7 +175,7 @@ TEST(StateBlock, angle_perturb)
ASSERT_MATRIX_APPROX(x , sb.getState() , 0.1 * 4); // 4-sigma test... ASSERT_MATRIX_APPROX(x , sb.getState() , 0.1 * 4); // 4-sigma test...
} }
TEST(StateBlock, quaternion_perturb) TEST(StateQuaternion, perturb)
{ {
Vector4d x(0.5,0.5,0.5,0.5); Vector4d x(0.5,0.5,0.5,0.5);
StateQuaternion sb(x); StateQuaternion sb(x);
......
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