diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h
index 32599d9dc72a556c06f9ade7df6339f38cc7c8cf..14f2ce38f1dd55b6571884710eb7ed7b7640f985 100644
--- a/include/core/state_block/local_parametrization_base.h
+++ b/include/core/state_block/local_parametrization_base.h
@@ -12,8 +12,6 @@
namespace wolf {
-//WOLF_PTR_TYPEDEFS(LocalParametrizationBase);
-
class LocalParametrizationBase{
protected:
unsigned int global_size_;
@@ -22,13 +20,17 @@ class LocalParametrizationBase{
LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size);
virtual ~LocalParametrizationBase();
+ bool plus(const Eigen::VectorXd& _x,
+ const Eigen::VectorXd& _delta,
+ Eigen::VectorXd& _x_plus_delta) const;
virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x,
Eigen::Map<const Eigen::VectorXd>& _delta,
- Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x, Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const = 0;
+ Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0;
+ virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x,
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const = 0;
virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
Eigen::Map<const Eigen::VectorXd>& _x2,
- Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
+ Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
unsigned int getLocalSize() const;
unsigned int getGlobalSize() const;
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index ab3bb7147f3c81f57a3436216412274394acc04a..99e15bd20351c1dcb61f4d373a842b7ea492d5d6 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -150,13 +150,7 @@ public:
*/
void perturb(double amplitude = 0.1);
- /** \brief Add this state_block to the problem
- **/
- //void addToProblem(ProblemPtr _problem_ptr);
-
- /** \brief Remove this state_block from the problem
- **/
- //void removeFromProblem(ProblemPtr _problem_ptr);
+ void plus(const Eigen::VectorXd& _dv);
};
} // namespace wolf
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 59270a335c9a75bb362e23614e3fb886dc4e880d..7bdd6b48842ed91257b308d82ae836d4ab2cee82 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -78,4 +78,16 @@ void HasStateBlocks::perturb(double amplitude)
}
}
+void HasStateBlocks::plus(const VectorComposite& _dx)
+{
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ auto& sb = pair_key_sb.second;
+ auto& dv = _dx.at(pair_key_sb.first);
+
+ sb->plus(dv);
+ }
+}
+
+
}
diff --git a/src/state_block/local_parametrization_base.cpp b/src/state_block/local_parametrization_base.cpp
index f46a93f9c488d0b899e94df156f3faff04a63554..aee3f704126bf4ba81e9658b3c337ded088c78a5 100644
--- a/src/state_block/local_parametrization_base.cpp
+++ b/src/state_block/local_parametrization_base.cpp
@@ -22,3 +22,14 @@ unsigned int LocalParametrizationBase::getGlobalSize() const
}
} // namespace wolf
+
+bool wolf::LocalParametrizationBase::plus(const Eigen::VectorXd &_x,
+ const Eigen::VectorXd &_delta,
+ Eigen::VectorXd &_x_plus_delta) const
+{
+ Eigen::Map<const Eigen::VectorXd> x(_x.data(), _x.size());
+ Eigen::Map<const Eigen::VectorXd> delta(_delta.data(), _delta.size());
+ Eigen::Map<Eigen::VectorXd> x_plus_delta(_x_plus_delta.data(), _x_plus_delta.size());
+
+ return plus(x, delta, x_plus_delta);
+}
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index 783a56a5e8cb52fe0ae79e2ad14ca4b50976a48c..94f2dbfb8d92cdad7ec1aa2de7bb8197facfeb94 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -26,31 +26,22 @@ void StateBlock::setFixed(bool _fixed)
fixed_.store(_fixed);
}
-//void StateBlock::addToProblem(ProblemPtr _problem_ptr)
-//{
-// _problem_ptr->addStateBlock(shared_from_this());
-//}
-//
-//void StateBlock::removeFromProblem(ProblemPtr _problem_ptr)
-//{
-// _problem_ptr->removeStateBlock(shared_from_this());
-//}
-
void StateBlock::perturb(double amplitude)
{
using namespace Eigen;
VectorXd perturbation(VectorXd::Random(getLocalSize()) * amplitude);
+ this->plus(perturbation);
+}
+
+void StateBlock::plus(const Eigen::VectorXd &_dv)
+{
if (local_param_ptr_ == nullptr)
- state_ += perturbation;
+ setState(getState() + _dv);
else
{
- VectorXd state_perturbed(getSize());
- // Note: LocalParametrizationBase::plus() works with Eigen::Map only. Build all necessary maps:
- Map<const VectorXd> state_map(state_.data(), getSize());
- Map<const VectorXd> perturbation_map(perturbation.data(), getLocalSize());
- Map<VectorXd> state_perturbed_map(state_perturbed.data(), getSize());
- local_param_ptr_->plus(state_map, perturbation_map, state_perturbed_map);
- state_ = state_perturbed;
+ Eigen::VectorXd state(getSize());
+ local_param_ptr_->plus(getState(), _dv, state);
+ setState(state);
}
}
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
index 19d9e3cf63623ef275ee5f8fc8111f4e88ece4d1..d2a1398a8e21daf05603f7810b031f98528b009e 100644
--- a/test/gtest_state_block.cpp
+++ b/test/gtest_state_block.cpp
@@ -19,6 +19,73 @@ using namespace Eigen;
using namespace std;
using namespace wolf;
+TEST(StateBlock, plus)
+{
+ Vector3d x(10,20,30);
+ StateBlock sb(x);
+
+ Vector3d dx(1,2,3);
+
+ sb.plus(dx);
+
+ WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+ ASSERT_MATRIX_APPROX(x + dx , sb.getState() , 1e-20);
+}
+
+TEST(StateBlock, angle_plus)
+{
+ StateAngle sb(1.0);
+
+ Vector1d dx(0.1);
+
+ sb.plus(dx);
+
+ WOLF_INFO("original ", 1.0, ", perturbed = ", sb.getState().transpose());
+
+ ASSERT_MATRIX_APPROX(Vector1d(1.0) + dx , sb.getState() , 1e-20);
+}
+
+TEST(StateBlock, angle_plus_past_pi)
+{
+ StateAngle sb(3.14);
+
+ Vector1d dx(0.1);
+
+ sb.plus(dx);
+
+ WOLF_INFO("original ", 3.14, ", perturbed = ", sb.getState().transpose());
+
+ ASSERT_MATRIX_APPROX(Vector1d(3.14 - 2.0*M_PI) + dx , sb.getState() , 1e-20);
+}
+
+TEST(StateBlock, quaternion_plus_norm)
+{
+ Vector4d x(0.5,0.5,0.5,0.5);
+ StateQuaternion sb(x);
+ Vector3d dx(.1,.2,.3);
+
+ sb.plus(dx);
+
+ WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+ ASSERT_LT((sb.getState().transpose() * x).norm() , 1.0); // quaternions are not parallel ==> not equal
+}
+
+TEST(StateBlock, quaternion_plus_trivial)
+{
+ Vector4d x(0,0,0,1); // identity (0,0,0,1)
+ StateQuaternion sb(x);
+ Vector3d dx(M_PI,0,0); // half turn over X axis --> result quaternion is (1,0,0,0)
+
+ sb.plus(dx);
+
+ WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+ ASSERT_MATRIX_APPROX(sb.getState(), Vector4d(1,0,0,0), 1e-10);
+}
+
+
TEST(StateBlock, block_perturb)
{
Vector3d x(10,20,30);