New methods plus() for state blocks and other

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;

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

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);
+    }
+}
+
+
 }

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);
+}

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);
     }
 }
 

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);