New method Problem::perturb()

include/core/problem/problem.h

@@ -271,6 +271,9 @@ class Problem : public std::enable_shared_from_this<Problem>
         // Zero state provider
         Eigen::VectorXd zeroState ( ) const;
         bool priorIsSet() const;
+        // Perturb state
+        void            perturb(double amplitude = 0.01);
+
 
         // Map branch -----------------------------------------
         MapBasePtr getMap() const;

include/core/sensor/sensor_base.h

@@ -181,6 +181,9 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         StateBlockPtr getStateBlockDynamic(const char _key) const { return getStateBlockDynamic(std::string(1,_key)); }
         StateBlockPtr getStateBlockDynamic(const char _key, const TimeStamp& _ts) const { return getStateBlockDynamic(std::string(1,_key), _ts); }
 
+        // Declare a state block as dynamic or static (with _dymanic = false)
+        void setStateBlockDynamic(const std::string& _key, bool _dynamic = true);
+
         bool isStateBlockDynamic(const std::string& _key) const;
         bool isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const;
         bool isStateBlockInCapture(const std::string& _key, CaptureBasePtr& _cap) const;
@@ -197,7 +200,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
     protected:
         void removeStateBlocks();
         virtual void registerNewStateBlocks() const;
-        void setStateBlockDynamic(const std::string& _key, bool _dynamic = true);
 
     public:
 

src/capture/capture_base.cpp

@@ -140,7 +140,7 @@ const std::string& CaptureBase::getStructure() const
 
 StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
 {
-    if (getSensor() and getSensor()->getStateBlock(_key))
+    if (getSensor() and getSensor()->hasStateBlock(_key))
     {
         if (getSensor()->isStateBlockDynamic(_key))
             return HasStateBlocks::getStateBlock(_key);

src/problem/problem.cpp

@@ -1386,6 +1386,26 @@ std::string Problem::printToString(int depth, bool constr_by, bool metric, bool
     result << endl;
     return result.str();
 }
+
+void Problem::perturb(double amplitude)
+{
+    // Sensors
+    for (auto& S : getHardware()->getSensorList())
+        S->perturb(amplitude);
+
+    // Frames and Captures
+    for (auto& F : getTrajectory()->getFrameList())
+    {
+        F->perturb(amplitude);
+        for (auto& C : F->getCaptureList())
+            C->perturb(amplitude);
+    }
+
+    // Landmarks
+    for (auto& L : getMap()->getLandmarkList())
+        L->perturb(amplitude);
+}
+
 bool Problem::check(int verbose_level) const
 {
     using std::cout;

test/gtest_problem.cpp

@@ -16,6 +16,11 @@
 #include "dummy/processor_tracker_feature_dummy.h"
 #include "core/solver/solver_manager.h"
 
+#include "core/sensor/sensor_diff_drive.h"
+#include "core/processor/processor_diff_drive.h"
+#include "core/capture/capture_diff_drive.h"
+#include "core/state_block/state_quaternion.h"
+
 #include <iostream>
 
 using namespace wolf;
@@ -317,6 +322,97 @@ TEST(Problem, Covariances)
 
 }
 
+TEST(Problem, perturb)
+{
+    auto problem = Problem::create("PO", 2);
+
+    // make a sensor first
+    auto intr = std::make_shared<ParamsSensorDiffDrive>();
+    intr->radius_left                   = 1.0;
+    intr->radius_right                  = 1.0;
+    intr->wheel_separation              = 1.0;
+    intr->ticks_per_wheel_revolution    = 100;
+    Vector3d extr(0,0,0);
+    auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
+    sensor->setStateBlockDynamic("I", true);
+
+    Vector3d pose; pose << 0,0,0;
+
+    for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
+    {
+        for ( int i = 0; i < 2; i++)
+        {
+            auto F = problem->emplaceFrame(KEY, pose, t);
+            if (i==0) F->fix();
+
+            for (int j = 0; j< 2 ; j++)
+            {
+                auto sb     = std::make_shared<StateBlock>(Vector3d(1,1,1));
+                auto cap    = CaptureBase::emplace<CaptureBase>(F, "CaptureBase", t, sensor, nullptr, nullptr, sb);
+            }
+        }
+    }
+
+    for (int l = 0; l < 2; l++)
+    {
+        auto sb1    = std::make_shared<StateBlock>(Vector2d(1,2));
+        auto sb2    = std::make_shared<StateBlock>(Vector1d(3));
+        auto L      = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", sb1, sb2);
+        if (l==0) L->fix();
+    }
+
+    /// PRINT, PERTURB, AND PRINT AGAIN
+    problem->print(2,0,1,1);
+
+    problem->perturb(0.2);
+
+    problem->print(2,0,1,1);
+
+
+    //// CHECK ALL STATE BLOCKS ///
+
+    double prec = 1e-2;
+
+    for (auto S : problem->getHardware()->getSensorList())
+    {
+        ASSERT_TRUE (S->getP()->getState().isApprox(Vector2d(0,0), prec) );
+        ASSERT_TRUE (S->getO()->getState().isApprox(Vector1d(0), prec) );
+        ASSERT_FALSE(S->getIntrinsic()->getState().isApprox(Vector3d(1,1,1), prec) );
+    }
+
+    for (auto F : problem->getTrajectory()->getFrameList())
+    {
+        if (F->isFixed()) // fixed
+        {
+            ASSERT_TRUE (F->getP()->getState().isApprox(Vector2d(0,0), prec) );
+            ASSERT_TRUE (F->getO()->getState().isApprox(Vector1d(0), prec) );
+        }
+        else
+        {
+            ASSERT_FALSE(F->getP()->getState().isApprox(Vector2d(0,0), prec) );
+            ASSERT_FALSE(F->getO()->getState().isApprox(Vector1d(0), prec) );
+        }
+        for (auto C : F->getCaptureList())
+        {
+            // all are estimated
+            ASSERT_FALSE(C->getSensorIntrinsic()->getState().isApprox(Vector3d(1,1,1), prec) );
+        }
+    }
+    for (auto L : problem->getMap()->getLandmarkList())
+    {
+        if ( L->isFixed() ) // fixed
+        {
+            ASSERT_TRUE (L->getP()->getState().isApprox(Vector2d(1,2), prec) );
+            ASSERT_TRUE (L->getO()->getState().isApprox(Vector1d(3), prec) );
+        }
+        else
+        {
+            ASSERT_FALSE(L->getP()->getState().isApprox(Vector2d(1,2), prec) );
+            ASSERT_FALSE(L->getO()->getState().isApprox(Vector1d(3), prec) );
+        }
+    }
+}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);