diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index 0e312c52c58d6805a1b0710965913216dc42d1bd..1a65deef5b4745367aa9568f41fe99d97cdd74c9 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -98,15 +98,6 @@ class CaptureMotion : public CaptureBase
MatrixType getDeltaPreintCov() const;
MatrixType getDeltaPreintCov(const TimeStamp& _ts) const;
-// // Composite API forms
-// void setCalibrationPreintComposite(const VectorComposite& _calib_preint);
-// const VectorComposite& getCalibrationPreintComposite() const;
-// const VectorComposite& getDeltaPreintComposite () const;
-// const VectorComposite& getDeltaPreintComposite (const TimeStamp& _ts) const;
-// const MatrixComposite& getJacobianCalibComposite () const;
-// const MatrixComposite& getJacobianCalibComposite (const TimeStamp& _ts) const;
-// const MotionBufferComposite& getBufferComposite() const;
-
// Origin frame and capture
CaptureBasePtr getOriginCapture();
CaptureBasePtr getOriginCapture() const;
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index e4fe0f4fa7b68fdf0fd7736592fc17277ac4e7ab..14867387c0d0d579cd3d35370846f201ad1ca4e1 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -28,9 +28,45 @@ typedef StateBlockMap::const_iterator StateBlockMapCIter;
class VectorComposite : public std::unordered_map < std::string, Eigen::VectorXd >
{
public:
+ VectorComposite() {};
+ VectorComposite(const VectorComposite & v) : unordered_map<string, VectorXd>(v){};
+ /**
+ * \brief Construct from Eigen::VectorXd and structure
+ *
+ * Usage:
+ *
+ * VectorXd vec_eigen(1,2,3,4,5);
+ *
+ * VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
+ *
+ * result:
+ *
+ * vec_comp["a"].transpose(); // = (1,2);
+ * vec_comp["b"].transpose(); // = (3,4,5);
+ */
+ VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
+
unsigned int size(const StateStructure& _structure) const;
+
Eigen::VectorXd vector(const StateStructure& _structure) const;
+ /**
+ * \brief set from Eigen::VectorXd and structure
+ *
+ * Usage:
+ *
+ * Eigen::VectorXd vec_eigen;
+ * wolf::VectorComposite vec_comp;
+ *
+ * vec_comp.set( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
+ *
+ * result:
+ *
+ * vec_comp["a"].transpose(); // = (1,2);
+ * vec_comp["b"].transpose(); // = (3,4,5);
+ */
+ void set(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
+
friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x);
friend wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index 82957aede0f3fdbcf18d00e8ec76e6c9b20e7fb4..d0618799c899fb294cddc60eb4b8671e7345a72f 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -8,12 +8,28 @@ namespace wolf{
////// VECTOR COMPOSITE //////////
+VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
+{
+ int index = 0;
+ auto size_it = _sizes.begin();
+ for ( const auto& ckey : _structure)
+ {
+ const auto& key = string(1,ckey);
+ const auto& size = *size_it;
+
+ (*this)[key] = _v.segment(index,size);
+
+ index += size;
+ size_it ++;
+ }
+}
+
unsigned int VectorComposite::size(const StateStructure &_structure) const
{
unsigned int size = 0;
for (const auto& ckey : _structure)
{
- std::string key(1,ckey);
+ std::string key(1,ckey); // ckey is char
const VectorXd& v = this->at(key);
size += v.size();
}
@@ -27,7 +43,7 @@ Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const
unsigned int size = 0;
for (const auto& ckey : _structure)
{
- std::string key(1,ckey);
+ std::string key(1,ckey); // ckey is char
vp.push_back(&(this->at(key)));
size += vp.back()->size();
}
@@ -79,6 +95,22 @@ wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::Ve
return xpy;
}
+void VectorComposite::set (const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
+{
+ int index = 0;
+ auto size_it = _sizes.begin();
+ for ( const auto& ckey : _structure)
+ {
+ const auto& key = string(1,ckey); // ckey is char
+ const auto& size = *size_it;
+
+ (*this)[key] = _v.segment(index,size);
+
+ index += size;
+ size_it ++;
+ }
+}
+
wolf::VectorComposite operator -(const wolf::VectorComposite &_x)
{
wolf::VectorComposite m;
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
index 24385c46901a94a607097a9918ea44f0d11e6502..af20924b2ae47d00ce3d5811bcb70a54a8ed92b8 100644
--- a/test/gtest_state_composite.cpp
+++ b/test/gtest_state_composite.cpp
@@ -13,14 +13,6 @@
using namespace wolf;
using namespace std;
-/*
-// You may use this to make some methods of Foo public
-WOLF_PTR_TYPEDEFS(FooPublic);
-class FooPublic : public Foo
-{
- // You may use this to make some methods of Foo public
-}
-*/
class StateBlockCompositeInit : public testing::Test
{
@@ -214,9 +206,43 @@ TEST_F(StateBlockCompositeInit, isFixed)
}
+TEST(VectorComposite, constructor_empty)
+{
+ VectorComposite v;
+ ASSERT_TRUE(v.empty());
+}
+TEST(VectorComposite, constructor_copy)
+{
+ VectorComposite u;
+ u.emplace("a", Vector2d(1,2));
+ u.emplace("b", Vector3d(3,4,5));
+
+ VectorComposite v(u);
+ ASSERT_FALSE(v.empty());
+
+ ASSERT_MATRIX_APPROX(u["a"], v["a"], 1e-20);
+ ASSERT_MATRIX_APPROX(u["b"], v["b"], 1e-20);
+}
+TEST(VectorComposite, constructor_from_list)
+{
+ VectorComposite v(Vector4d(1,2,3,4), "ab", {3,1});
+
+ ASSERT_MATRIX_APPROX(v.at("a"), Vector3d(1,2,3), 1e-20);
+ ASSERT_MATRIX_APPROX(v.at("b"), Vector1d(4), 1e-20);
+}
+
+TEST(VectorComposite, set)
+{
+ VectorComposite v;
+
+ v.set(Vector4d(1,2,3,4), "ab", {3,1});
+
+ ASSERT_MATRIX_APPROX(v.at("a"), Vector3d(1,2,3), 1e-20);
+ ASSERT_MATRIX_APPROX(v.at("b"), Vector1d(4), 1e-20);
+}
TEST(VectorComposite, operatorStream)
{