From 3b092975bc2ca9d7bc8d42f326cf1bf1513a5d45 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 8 Apr 2020 13:00:29 +0200
Subject: [PATCH] New methods plus() for state blocks and other
---
.../state_block/local_parametrization_base.h | 12 ++--
include/core/state_block/state_block.h | 8 +--
src/state_block/has_state_blocks.cpp | 12 ++++
.../local_parametrization_base.cpp | 11 +++
src/state_block/state_block.cpp | 27 +++-----
test/gtest_state_block.cpp | 67 +++++++++++++++++++
6 files changed, 107 insertions(+), 30 deletions(-)
diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h
index 32599d9dc..14f2ce38f 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 ab3bb7147..99e15bd20 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 59270a335..7bdd6b488 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 f46a93f9c..aee3f7041 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 783a56a5e..94f2dbfb8 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 19d9e3cf6..d2a1398a8 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);
--
GitLab