diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 004ba29b696bab7d8d3298ef702e08eb06d2c738..4f521c1155b33c1ab5afbf3aab67a3b38312dfd5 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/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 0f7d13f99c2af92d8779c11a7e705bb4ef94f99f..ef55d163be4764393c4e1b08ac0003ae67b1d223 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -68,6 +68,10 @@ class HasStateBlocks
unsigned int getSize() const;
unsigned int getLocalSize() const;
+ // Perturb state
+ void perturb(double amplitude = 0.01);
+
+
private:
std::string structure_;
std::unordered_map<std::string, StateBlockPtr> state_block_map_;
@@ -251,6 +255,5 @@ inline unsigned int HasStateBlocks::getLocalSize() const
return size;
}
-
} // namespace wolf
#endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index b68b4ad7dd482f071faf04339656879635232dd2..33aa6fcb4b221ea1dc23f24b21f4a0fb12cbfc0d 100644
--- a/include/core/state_block/state_block.h
+++ b/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);
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 a5576fb3c0fc1524b6721405c69ed5c2ce22ec8c..855236ece07f90a1570d293ac5c9333ee445658c 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/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 8b11f1cb3981206700b4ce54ad5529df54ae6d29..83cf44b26bdfcad7e415633ef6d8fe783564a75c 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -44,5 +44,14 @@ void HasStateBlocks::removeStateBlocks(ProblemPtr _problem)
}
}
+void HasStateBlocks::perturb(double amplitude)
+{
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ auto& sb = pair_key_sb.second;
+ if (!sb->isFixed())
+ sb->perturb(amplitude);
+ }
+}
}
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index f582ae97787b4a3df8d1e98177385427580a90c0..783a56a5e8cb52fe0ae79e2ad14ca4b50976a48c 100644
--- a/src/state_block/state_block.cpp
+++ b/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;
+ }
+}
+
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 10a1f9ab0d5d3b43699b098ef0d7f2bf072990fb..d32d5c6216c857ad703c34aa1a10f4b29d6c4dc0 100644
--- a/test/CMakeLists.txt
+++ b/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})
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 40e354ddfef4e98746fc7d3bd5c6e0ca983a3d93..e0ba9a42a17936b62ec329c1fb022052d5ae6c30 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;
@@ -318,6 +323,96 @@ 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;
+
+ int i = 0;
+ for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
+ {
+ 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);
+ }
+ i++;
+ }
+
+ 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);
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..19d9e3cf63623ef275ee5f8fc8111f4e88ece4d1
--- /dev/null
+++ b/test/gtest_state_block.cpp
@@ -0,0 +1,69 @@
+/*
+ * 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();
+}
+
+
+
+