New method StateBlock::perturb()

include/core/state_block/state_block.h

@@ -142,6 +142,10 @@ public:
          **/
         void resetLocalParamUpdated();
 
+        /** \brief perturb state
+         */
+        void perturb(double amplitude = 0.1);
+
         /** \brief Add this state_block to the problem
          **/
         //void addToProblem(ProblemPtr _problem_ptr);

src/state_block/state_block.cpp

@@ -36,4 +36,22 @@ void StateBlock::setFixed(bool _fixed)
 //    _problem_ptr->removeStateBlock(shared_from_this());
 //}
 
+void StateBlock::perturb(double amplitude)
+{
+    using namespace Eigen;
+    VectorXd perturbation(VectorXd::Random(getLocalSize()) * amplitude);
+    if (local_param_ptr_ == nullptr)
+        state_ += perturbation;
+    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;
+    }
+}
+
 }

test/CMakeLists.txt

@@ -137,6 +137,10 @@ target_link_libraries(gtest_shared_from_this ${PROJECT_NAME})
 wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
 target_link_libraries(gtest_solver_manager ${PROJECT_NAME})
 
+# StateBlock class test
+wolf_add_gtest(gtest_state_block gtest_state_block.cpp)
+target_link_libraries(gtest_state_block ${PROJECT_NAME})
+
 # TimeStamp class test
 wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
 target_link_libraries(gtest_time_stamp ${PROJECT_NAME})

test/gtest_state_block.cpp

+/*
+ * gtest_state_block.cpp
+ *
+ *  Created on: Mar 31, 2020
+ *      Author: jsola
+ */
+
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+
+#include <iostream>
+
+
+using namespace Eigen;
+using namespace std;
+using namespace wolf;
+
+TEST(StateBlock, block_perturb)
+{
+    Vector3d x(10,20,30);
+    StateBlock sb(x);
+
+    sb.perturb(0.5);
+
+    WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+    ASSERT_NE(x.norm(), sb.getState().norm());
+    ASSERT_MATRIX_APPROX(x , sb.getState() , 0.5 * 4); // 4-sigma test...
+}
+
+TEST(StateBlock, angle_perturb)
+{
+    Vector1d x(1.0);
+    StateAngle sb(x(0));
+
+    sb.perturb(0.1);
+
+    WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+    ASSERT_NE(x.norm(), sb.getState().norm());
+    ASSERT_MATRIX_APPROX(x , sb.getState() , 0.1 * 4); // 4-sigma test...
+}
+
+TEST(StateBlock, quaternion_perturb)
+{
+    Vector4d x(0.5,0.5,0.5,0.5);
+    StateQuaternion sb(x);
+
+    sb.perturb(0.01);
+
+    WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+    ASSERT_LT((sb.getState().transpose() * x).norm() , 1.0); // quaternions are not parallel ==> not equal
+    ASSERT_MATRIX_APPROX(x , sb.getState() , 0.01 * 4); // 4-sigma test...
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+
+
+