diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 3238fb8ece01ff1f48e4cfcd5b403760a5bb4c2f..9b8f979fc57b93658cb2caebb951ef375e7cfafa 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -147,7 +147,10 @@ public:
void resetLocalParamUpdated();
virtual void setIdentity(bool _notify = true);
- virtual void setZero (bool _notify = true);
+ void setZero (bool _notify = true);
+
+ virtual Eigen::VectorXd identity() const;
+ Eigen::VectorXd zero() const;
/** \brief perturb state
*/
@@ -299,6 +302,14 @@ inline void StateBlock::setZero(bool _notify)
setIdentity(_notify);
}
+inline Eigen::VectorXd StateBlock::identity() const
+{
+ return Eigen::VectorXd::Zero(state_size_);
+}
+inline Eigen::VectorXd StateBlock::zero() const
+{
+ return identity();
+}
}// namespace wolf
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index 267a6e1b23caddfa5f8f210510e6cde2a0d3b99b..b533eb686740ee2db8c8643fb4356c71d96d39c3 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -29,25 +29,12 @@ typedef StateBlockMap::const_iterator StateBlockMapCIter;
class StateBlockComposite
{
public:
- StateBlockComposite() = default;
- virtual ~StateBlockComposite() = default;
+ StateBlockComposite() = default;
+ ~StateBlockComposite() = default;
- const StateBlockMap& getStateBlockMap() const;
+ const StateBlockMap& getStateBlockMap() const;
- // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
- virtual void fix();
- virtual void unfix();
- virtual bool isFixed() const;
-
- virtual unsigned int remove(const std::string& _sb_type);
- bool has(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
- bool has(const StateBlockPtr& _sb) const;
- StateBlockPtr at(const std::string& _sb_type) const;
- void set(const std::string _sb_type, const StateBlockPtr& _sb);
- bool key(const StateBlockPtr& _sb, std::string& _key) const;
- StateBlockMapCIter find(const StateBlockPtr& _sb) const;
-
- virtual StateBlockPtr add(const std::string& _sb_type, const StateBlockPtr& _sb);
+ StateBlockPtr add(const std::string& _sb_type, const StateBlockPtr& _sb);
// Emplace derived state blocks (angle, quaternion, etc).
template<typename SB, typename ... Args>
@@ -57,12 +44,32 @@ class StateBlockComposite
template<typename ... Args>
inline StateBlockPtr emplace(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor);
- // Perturb state with noise
- void perturb(double amplitude = 0.01);
+ unsigned int remove(const std::string& _sb_type);
+ bool has(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
+ bool has(const StateBlockPtr& _sb) const;
+ StateBlockPtr at(const std::string& _sb_type) const;
+ void set(const std::string _sb_type, const StateBlockPtr& _sb);
+ bool key(const StateBlockPtr& _sb, std::string& _key) const;
+ std::string key(const StateBlockPtr& _sb) const;
+ StateBlockMapCIter find(const StateBlockPtr& _sb) const;
+
+ // identity and zero (they are the same with different names)
+ void setIdentity(bool _notify = true);
+ void setZero(bool _notify = true);
+ VectorComposite identity() const;
+ VectorComposite zero() const;
// Plus operator
void plus(const VectorComposite& _dx);
+ // Perturb state with random noise
+ void perturb(double amplitude = 0.01);
+
+ // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
+ void fix();
+ void unfix();
+ bool isFixed() const;
+
// Get composite of state vectors (not blocks)
VectorComposite getVectorComposite() const;
bool getVectorComposite(VectorComposite& _state) const;
@@ -81,8 +88,8 @@ class MatrixComposite
std::unordered_map < string, unsigned int> size_rows_, size_cols_;
public:
- MatrixComposite() = default;
- virtual ~MatrixComposite() = default;
+ MatrixComposite() = default;
+ ~MatrixComposite() = default;
unsigned int count(const std::string &_row,
const std::string &_col) const;
@@ -108,7 +115,7 @@ class MatrixComposite
const MatrixXd& operator ()(const std::string &_row,
const std::string &_col) const;
- VectorComposite operator *(const VectorComposite &_second) const;
+ VectorComposite operator *(const VectorComposite &_second) const;
MatrixComposite operator *(const MatrixComposite &_second) const;
friend std::ostream& operator << (std::ostream& _os, const MatrixComposite& _M);
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index 547e849005666a97fe36e6761b95dca94cf06182..7a8e233b74816b347afa240183fad49dbed6141b 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -20,6 +20,8 @@ class StateHomogeneous3d : public StateBlock
StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false);
virtual ~StateHomogeneous3d();
virtual void setIdentity(bool _notify = true) override;
+ virtual Eigen::VectorXd identity() const override;
+
};
inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed) :
@@ -46,7 +48,10 @@ inline void StateHomogeneous3d::setIdentity(bool _notify)
setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
}
-
+inline Eigen::VectorXd StateHomogeneous3d::identity() const
+{
+ return Eigen::Quaterniond::Identity().coeffs();
+}
} // namespace wolf
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index a7f460b81324c44fa7d319a88e3569a3f2e796b6..3e9c2c09ae0c03b690dbfba0551855f636e6aa38 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -21,6 +21,7 @@ class StateQuaternion : public StateBlock
StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false);
virtual ~StateQuaternion();
virtual void setIdentity(bool _notify = true) override;
+ virtual Eigen::VectorXd identity() const override;
};
inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) :
@@ -51,6 +52,11 @@ inline void StateQuaternion::setIdentity(bool _notify)
setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
}
+inline Eigen::VectorXd StateQuaternion::identity() const
+{
+ return Eigen::Quaterniond::Identity().coeffs();
+}
+
} // namespace wolf
#endif /* SRC_STATE_QUATERNION_H_ */
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index 6fd4cdded28dbfe9fcc7b0e42d976678b25a975a..b11d911fd3e32f273fd3d2dfb30fd72b68df78ee 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -155,10 +155,16 @@ const StateBlockMap& StateBlockComposite::getStateBlockMap() const
void StateBlockComposite::fix()
{
+ for (auto pair_key_sbp : state_block_map_)
+ if (pair_key_sbp.second != nullptr)
+ pair_key_sbp.second->fix();
}
void StateBlockComposite::unfix()
{
+ for (auto pair_key_sbp : state_block_map_)
+ if (pair_key_sbp.second != nullptr)
+ pair_key_sbp.second->unfix();
}
bool StateBlockComposite::isFixed() const
@@ -222,6 +228,19 @@ bool StateBlockComposite::key(const StateBlockPtr &_sb, std::string &_key) const
}
}
+std::string StateBlockComposite::key(const StateBlockPtr& _sb) const
+{
+ const auto& it = this->find(_sb);
+
+ bool found = (it != state_block_map_.end());
+
+ if (found)
+ return it->first;
+ else
+ return "";
+}
+
+
StateBlockMapCIter StateBlockComposite::find(const StateBlockPtr &_sb) const
{
const auto& it = std::find_if(state_block_map_.begin(),
@@ -290,4 +309,31 @@ void StateBlockComposite::setVectorComposite(const VectorComposite &_state)
}
}
+void StateBlockComposite::setIdentity(bool _notify)
+{
+ for (const auto& pair_key_sb : getStateBlockMap())
+ {
+ pair_key_sb.second->setIdentity(_notify);
+ }
+}
+
+void StateBlockComposite::setZero(bool _notify)
+{
+ setIdentity(_notify);
+}
+
+VectorComposite StateBlockComposite::identity() const
+{
+ VectorComposite x;
+ for (const auto& pair_key_sb : getStateBlockMap())
+ x.emplace(pair_key_sb.first, pair_key_sb.second->identity());
+ return x;
+}
+
+VectorComposite StateBlockComposite::zero() const
+{
+ return identity();
+}
+
} // namespace wolf
+
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
index d814d0918fab6f1f15b63ae5051ddbc571f7a1a4..ce6897eaa0c0ec175605883a7b49eb696aaaaf73 100644
--- a/test/gtest_state_composite.cpp
+++ b/test/gtest_state_composite.cpp
@@ -83,7 +83,7 @@ TEST_F(StateBlockCompositeInit, set)
ASSERT_EQ(states.at("X"), nullptr);
}
-TEST_F(StateBlockCompositeInit, key)
+TEST_F(StateBlockCompositeInit, key_ref)
{
std::string key;
ASSERT_TRUE(states.key(sbp, key));
@@ -93,6 +93,15 @@ TEST_F(StateBlockCompositeInit, key)
ASSERT_EQ(key, "");
}
+TEST_F(StateBlockCompositeInit, key_return)
+{
+ // existing key
+ ASSERT_EQ(states.key(sbp), "P");
+
+ // non existing key returns empty string
+ ASSERT_EQ(states.key(sbx), "");
+}
+
TEST_F(StateBlockCompositeInit, find)
{
auto it = states.find(sbp);
@@ -109,6 +118,17 @@ TEST_F(StateBlockCompositeInit, add)
ASSERT_EQ(states.at("X"), sbx);
}
+TEST_F(StateBlockCompositeInit, remove)
+{
+ // remove existing block
+ states.remove("V");
+ ASSERT_EQ(states.getStateBlockMap().size(), 2);
+
+ // remove non existing block -- no effect
+ states.remove("X");
+ ASSERT_EQ(states.getStateBlockMap().size(), 2);
+}
+
TEST_F(StateBlockCompositeInit, perturb)
{
ASSERT_TRUE(states.at("P")->getState().isApprox(sbp->getState(), 1e-3));
@@ -123,6 +143,64 @@ TEST_F(StateBlockCompositeInit, perturb)
ASSERT_FALSE(states.at("Q")->getState().isApprox(Vector4d(.5,.5,.5,.5), 1e-3));
}
+TEST_F(StateBlockCompositeInit, setIdentity)
+{
+ ASSERT_TRUE(states.at("P")->getState().isApprox(sbp->getState(), 1e-3));
+ ASSERT_TRUE(states.at("V")->getState().isApprox(sbv->getState(), 1e-3));
+ ASSERT_TRUE(states.at("Q")->getState().isApprox(sbq->getState(), 1e-3));
+
+ states.setIdentity();
+
+ // values have moved wrt original
+ ASSERT_TRUE(states.at("P")->getState().isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(states.at("V")->getState().isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(states.at("Q")->getState().isApprox(Vector4d(0,0,0,1), 1e-10));
+}
+
+TEST_F(StateBlockCompositeInit, identity)
+{
+ VectorComposite v = states.identity();
+
+ ASSERT_TRUE(v.at("P").isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(v.at("V").isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(v.at("Q").isApprox(Vector4d(0,0,0,1), 1e-10));
+}
+
+TEST_F(StateBlockCompositeInit, fix)
+{
+ states.fix();
+
+ ASSERT_TRUE(states.at("P")->isFixed());
+ ASSERT_TRUE(states.at("V")->isFixed());
+ ASSERT_TRUE(states.at("Q")->isFixed());
+}
+
+TEST_F(StateBlockCompositeInit, unfix)
+{
+ states.fix();
+
+ ASSERT_TRUE(states.at("P")->isFixed());
+ ASSERT_TRUE(states.at("V")->isFixed());
+ ASSERT_TRUE(states.at("Q")->isFixed());
+
+ states.unfix();
+
+ ASSERT_FALSE(states.at("P")->isFixed());
+ ASSERT_FALSE(states.at("V")->isFixed());
+ ASSERT_FALSE(states.at("Q")->isFixed());
+}
+
+TEST_F(StateBlockCompositeInit, isFixed)
+{
+ states.fix();
+
+ ASSERT_TRUE(states.isFixed());
+
+ states.at("P")->unfix();
+
+ ASSERT_FALSE(states.isFixed());
+}
+