diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 9d306d5983883edea43f8712f07484d2086df065..f147738f20d34703ec0850fc1430892284e0137b 100644
--- a/include/core/problem/problem.h
+++ b/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;
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 676a83bc5c9ade574e1b10b2a21a6bf2740cd3cd..1c099cb9be4827705bd43d229714ea1dabf24ed0 100644
--- a/include/core/sensor/sensor_base.h
+++ b/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:
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index fbb54972b09b78f0a0e6a6e66701396a481d9cfc..54e5d2a3cdedb301a7580e691cad9543df3c24c2 100644
--- a/src/capture/capture_base.cpp
+++ b/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);
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 482fdf08bf67cc4f26929b840871ae044ceefa42..d075d8162f862407c10d73c6f2d6c067d39e90b6 100644
--- a/src/problem/problem.cpp
+++ b/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;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 17f39d09f7ae18275245856e1ad66dcaa3d33bfd..f1c7424d93ade98b8f836d23262aad75474d9ad6 100644
--- a/test/gtest_problem.cpp
+++ b/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);