diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6cedf3a4a9752de28b14ebde80a0a32b385448b3..88f24f03d87d969fcec144f9923fea9917d4fcf3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -215,6 +215,7 @@ SET(HDRS_STATE_BLOCK
include/${PROJECT_NAME}/state_block/local_parametrization_quaternion.h
include/${PROJECT_NAME}/state_block/state_angle.h
include/${PROJECT_NAME}/state_block/state_block.h
+ include/${PROJECT_NAME}/state_block/state_block_derived.h
include/${PROJECT_NAME}/state_block/state_composite.h
include/${PROJECT_NAME}/state_block/state_homogeneous_3d.h
include/${PROJECT_NAME}/state_block/state_quaternion.h
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 9dfb7cae3e62adac06662e9414c75764045148e2..6aacf018eca716505f258ee5c7ac861654bd0f74 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -86,7 +86,11 @@ class Problem : public std::enable_shared_from_this<Problem>
StateStructure frame_structure_;
PriorOptionsPtr prior_options_;
- private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
+ std::atomic_bool transformed_;
+ VectorComposite transformation_;
+ mutable std::mutex mut_transform_;
+
+ private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
void setup();
@@ -364,6 +368,10 @@ class Problem : public std::enable_shared_from_this<Problem>
// All branches -------------------------------------------
// perturb states
void perturb(double amplitude = 0.01);
+ void transform(const VectorComposite& _transformation);
+ bool isTransformed() const;
+ void resetTransformed();
+ VectorComposite getTransformation() const;
// Covariances
void clearCovariance();
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 7c0cf9b2c776be5808c853c54c5d5c3f0cca5ff7..7f32daba5f16e446b061eb4e1402ab9db44ff0d2 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -167,12 +167,13 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
RUNNING_WITH_KF_AFTER_ORIGIN
} ProcessingStep ;
- protected:
+ protected:
ParamsProcessorMotionPtr params_motion_;
- ProcessingStep processing_step_; ///< State machine controlling the processing step
+ ProcessingStep processing_step_; ///< State machine controlling the processing step
+ bool bootstrapping_; ///< processor is bootstrapping
- // This is the main public interface
- public:
+ // This is the main public interface
+ public:
ProcessorMotion(const std::string& _type,
std::string _state_structure,
int _dim,
@@ -289,6 +290,11 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
*/
virtual void preProcess(){ };
+ /**
+ * @brief Bootstrap the processor with some initialization steps
+ */
+ virtual void bootstrap(){};
+
/** Post-process
*
* This is called by process() after finishing the processing algorithm.
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index ac82297f5e36b5130151bd1b0464a110264b5fe7..90ba34708a5ffc71a3c8b2f38f14d0a039c76c32 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -40,6 +40,8 @@ namespace wolf
class HasStateBlocks
{
+ friend Problem;
+
public:
HasStateBlocks();
HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {}
@@ -100,11 +102,13 @@ class HasStateBlocks
unsigned int getLocalSize(const StateStructure& _structure="") const;
// Perturb state
- void perturb(double amplitude = 0.01);
+ void perturb(double amplitude = 0.01);
- protected:
- void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
+ protected:
+ // transform state
+ void transform(const VectorComposite& _transformation);
+ void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
private:
StateStructure structure_;
diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h
index 3cc093665df69b32ed10d2d49921fdacedea7871..7e97022b4618dbdbd6896dcfc4261c936fd76cb8 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.h
@@ -38,15 +38,17 @@ namespace wolf
class StateAngle : public StateBlock
{
public:
- StateAngle(double _angle = 0.0, bool _fixed = false);
+ StateAngle(double _angle = 0.0, bool _fixed = false, bool _transformable = true);
~StateAngle() override;
+ virtual void transform(const VectorComposite& _transformation) override;
+
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
-inline StateAngle::StateAngle(double _angle, bool _fixed) :
- StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>())
+inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable) :
+ StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
{
state_(0) = pi2pi(_angle);
}
@@ -62,6 +64,11 @@ inline StateBlockPtr StateAngle::create (const Eigen::VectorXd& _state, bool _fi
return std::make_shared<StateAngle>(_state(0), _fixed);
}
+inline void StateAngle::transform(const VectorComposite& _transformation)
+{
+ if (isTransformable()) setState(_transformation.at('O') + getState()); // 2D rotation is a sum of angles
+}
+
} /* namespace wolf */
#endif /* STATE_ANGLE_H_ */
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 8f4b0468b8d9fdc519e11137a8dc491330ec3ac7..2572cc9b64a729f36496ac403fb18700846dc43c 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -31,6 +31,7 @@ class LocalParametrizationBase;
//Wolf includes
#include "core/common/wolf.h"
+#include "core/state_block/state_composite.h"
//std includes
#include <iostream>
@@ -65,6 +66,8 @@ public:
std::atomic_bool fix_updated_; ///< Flag to indicate whether the status has been updated or not
std::atomic_bool local_param_updated_; ///< Flag to indicate whether the local_parameterization has been updated or not
+ bool transformable_; ///< Flag to indicate if the state block can be transformed to another reference frame
+
public:
/** \brief Constructor from size
*
@@ -72,7 +75,7 @@ public:
* \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
* \param _local_param_ptr pointer to the local parametrization for the block
*/
- StateBlock(const SizeEigen _size, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr);
+ StateBlock(const SizeEigen _size, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true);
/** \brief Constructor from vector
*
@@ -80,7 +83,7 @@ public:
* \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
* \param _local_param_ptr pointer to the local parametrization for the block
**/
- StateBlock(const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr);
+ StateBlock(const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true);
///< Explicitly not copyable/movable
StateBlock(const StateBlock& o) = delete;
@@ -177,6 +180,11 @@ public:
*/
void perturb(double amplitude = 0.1);
+ bool isTransformable() const {
+ return transformable_;
+ };
+
+ virtual void transform(const VectorComposite& _transformation) { };
void plus(const Eigen::VectorXd& _dv);
@@ -186,6 +194,7 @@ public:
};
+
} // namespace wolf
// IMPLEMENTATION
@@ -195,31 +204,30 @@ public:
namespace wolf {
-inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
-// notifications_{Notification::ADD},
+inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
fixed_(_fixed),
state_size_(_state.size()),
state_(_state),
local_param_ptr_(_local_param_ptr),
state_updated_(false),
fix_updated_(false),
- local_param_updated_(false)
+ local_param_updated_(false),
+ transformable_(_transformable)
{
// std::cout << "constructed +sb" << std::endl;
}
-inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
-// notifications_{Notification::ADD},
+inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
fixed_(_fixed),
state_size_(_size),
state_(Eigen::VectorXd::Zero(_size)),
local_param_ptr_(_local_param_ptr),
state_updated_(false),
fix_updated_(false),
- local_param_updated_(false)
+ local_param_updated_(false),
+ transformable_(_transformable)
{
- //
-// std::cout << "constructed +sb" << std::endl;
+ // std::cout << "constructed +sb" << std::endl;
}
inline StateBlock::~StateBlock()
diff --git a/include/core/state_block/state_block_derived.h b/include/core/state_block/state_block_derived.h
new file mode 100644
index 0000000000000000000000000000000000000000..541b79a19453f34cedf13343de1caee2d2fca74b
--- /dev/null
+++ b/include/core/state_block/state_block_derived.h
@@ -0,0 +1,154 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef STATE_BLOCK_DERIVED_H_
+#define STATE_BLOCK_DERIVED_H_
+
+#include "core/state_block/state_block.h"
+
+namespace wolf
+{
+
+/**
+ * @brief State block for general parameters
+ *
+ * This state block cannot be transformed with a geometric frame transformation.
+ *
+ * This state block cannot have a local parametrization
+ *
+ * @tparam size the size of the state block's vector
+ */
+template <size_t size>
+class StateParams : public StateBlock
+{
+ public:
+ StateParams(const Eigen::VectorXd& _state, bool _fixed = false) : StateBlock(_state, _fixed, nullptr, false) {}
+ void transform(const VectorComposite& _transformation) override
+ {
+ // non transformable! --> do nothing
+ }
+};
+
+typedef StateParams<1> StateParams1;
+typedef StateParams<2> StateParams2;
+typedef StateParams<3> StateParams3;
+typedef StateParams<4> StateParams4;
+typedef StateParams<5> StateParams5;
+typedef StateParams<6> StateParams6;
+typedef StateParams<7> StateParams7;
+typedef StateParams<8> StateParams8;
+typedef StateParams<9> StateParams9;
+typedef StateParams<10> StateParams10;
+
+/**
+ * @brief State block for general 2D points and positions
+ *
+ * This state block can be transformed with a geometric frame transformation.
+ * This is controlled at construction time by parameter _transformable,
+ * or later via setTransformable(bool).
+ *
+ * This state block cannot have a local parametrization
+ */
+class StatePoint2d : public StateBlock
+{
+ public:
+ StatePoint2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
+ : StateBlock(_state, _fixed, nullptr, _transformable)
+ {
+ }
+ void transform(const VectorComposite& _transformation) override
+ {
+ if (transformable_) setState(_transformation.at('P') + Rotation2Dd(_transformation.at('O')(0)) * getState());
+ }
+};
+
+/**
+ * @brief State block for general 2D vectors
+ *
+ * This state block can be transformed with a geometric frame transformation.
+ * This is controlled at construction time by parameter _transformable,
+ * or later via setTransformable(bool).
+ * Being a vector, the geometric frame transformation will rotate the vector.
+ *
+ * This state block cannot have a local parametrization
+ */
+class StateVector2d : public StateBlock
+{
+ public:
+ StateVector2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
+ : StateBlock(_state, _fixed, nullptr, _transformable)
+ {
+ }
+ void transform(const VectorComposite& _transformation) override
+ {
+ if (transformable_) setState(Rotation2Dd(_transformation.at('O')(0)) * getState());
+ }
+};
+
+/**
+ * @brief State block for general 3D points and positions
+ *
+ * This state block can be transformed with a geometric frame transformation.
+ * This is controlled at construction time by parameter _transformable,
+ * or later via setTransformable(bool).
+ *
+ * This state block cannot have a local parametrization
+ */
+class StatePoint3d : public StateBlock
+{
+ public:
+ StatePoint3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
+ : StateBlock(_state, _fixed, nullptr, _transformable)
+ {
+ }
+ void transform(const VectorComposite& _transformation) override
+ {
+ if (transformable_)
+ setState(_transformation.at('P') + Quaterniond(_transformation.at('O').data()) * getState());
+ }
+};
+
+/**
+ * @brief State block for general 3D vectors
+ *
+ * This state block can be transformed with a geometric frame transformation.
+ * This is controlled at construction time by parameter _transformable,
+ * or later via setTransformable(bool).
+ * Being a vector, the geometric frame transformation will rotate the vector.
+ *
+ * This state block cannot have a local parametrization
+ */
+class StateVector3d : public StateBlock
+{
+ public:
+ StateVector3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
+ : StateBlock(_state, _fixed, nullptr, _transformable)
+ {
+ }
+ void transform(const VectorComposite& _transformation) override
+ {
+ if (transformable_) setState(Quaterniond(_transformation.at('O').data()) * getState());
+ }
+};
+
+} // namespace wolf
+
+#endif // STATE_BLOCK_DERIVED_H_
\ No newline at end of file
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index e3569b5b4ed30159e0593de789b3b7a6c3644387..dcfa71155597334de8ff4a3daedbb760314f1856 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -246,7 +246,7 @@ class MatrixComposite : public std::unordered_map < char, std::unordered_map < c
* S["V"]["V"] S["V"]["W"]
* S["W"]["V"] S["W"]["W"]
*/
- MatrixComposite propagate(const MatrixComposite & _Cov); // This performs Jac * this * Jac.tr
+ MatrixComposite propagate(const MatrixComposite & _Cov); // This performs this * _Cov * this.tr
/**
* \brief Matrix-vector product
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index 6ee8a85c28725805ff5a28fb2149950f7cb8e132..8af6059aa6badcfc91665307e92df8fd9379694b 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -37,25 +37,27 @@ namespace wolf {
class StateHomogeneous3d : public StateBlock
{
public:
- StateHomogeneous3d(bool _fixed = false);
- StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false);
+ StateHomogeneous3d(bool _fixed = false, bool _transformable = true);
+ StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false, bool _transformable = true);
~StateHomogeneous3d() override;
void setIdentity(bool _notify = true) override;
Eigen::VectorXd identity() const override;
+ virtual void transform(const VectorComposite& _transformation) override;
+
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
-inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed) :
- StateBlock(_state, _fixed)
+inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed, bool _transformable) :
+ StateBlock(_state, _fixed, nullptr, _transformable)
{
assert(_state.size() == 4 && "Homogeneous 3d must be size 4.");
local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
}
-inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed) :
- StateBlock(4, _fixed)
+inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed, bool _transformable) :
+ StateBlock(4, _fixed, nullptr, _transformable)
{
local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
state_ << 0, 0, 0, 1; // Set origin
@@ -77,6 +79,12 @@ inline Eigen::VectorXd StateHomogeneous3d::identity() const
return Eigen::Quaterniond::Identity().coeffs();
}
+inline void StateHomogeneous3d::transform(const VectorComposite& _transformation)
+{
+ if (transformable_)
+ setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs()); // TODO is this correct?????? probably not!!!
+}
+
inline StateBlockPtr StateHomogeneous3d::create (const Eigen::VectorXd& _state, bool _fixed)
{
MatrixSizeCheck<4,1>::check(_state);
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index f22772908c411fba8a096fb764af40fe6b39316e..b73279f3f7347cf994f23ba8bde4d03e3db75b34 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -37,27 +37,29 @@ namespace wolf {
class StateQuaternion : public StateBlock
{
public:
- StateQuaternion(bool _fixed = false);
- StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false);
- StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false);
+ StateQuaternion(bool _fixed = false, bool _transformable = true);
+ StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true);
+ StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false, bool _transformable = true);
~StateQuaternion() override;
void setIdentity(bool _notify = true) override;
Eigen::VectorXd identity() const override;
+ virtual void transform(const VectorComposite& _transformation) override;
+
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
-inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) :
- StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
+inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed, bool _transformable) :
+ StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
{
// TODO: remove these lines after issue #381 is addressed
assert(isValid(1e-5) && "Quaternion unit norm is worse than 1e-5 tolerance!");
state_.normalize();
}
-inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed) :
- StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
+inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed, bool _transformable) :
+ StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
{
assert(state_.size() == 4 && "The quaternion state vector must be of size 4");
@@ -66,8 +68,8 @@ inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fix
state_.normalize();
}
-inline StateQuaternion::StateQuaternion(bool _fixed) :
- StateBlock(4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
+inline StateQuaternion::StateQuaternion(bool _fixed, bool _transformable) :
+ StateBlock(4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
{
state_ = Eigen::Quaterniond::Identity().coeffs();
//
@@ -89,6 +91,12 @@ inline Eigen::VectorXd StateQuaternion::identity() const
return Eigen::Quaterniond::Identity().coeffs();
}
+inline void StateQuaternion::transform(const VectorComposite& _transformation)
+{
+ if (transformable_)
+ setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs());
+}
+
inline StateBlockPtr StateQuaternion::create (const Eigen::VectorXd& _state, bool _fixed)
{
MatrixSizeCheck<4,1>::check(_state);
diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h
index e5283ccd5b5f669e000fda1904245c99e58955ee..0cde0badc595350bacf33f466abf21ede7b6a67f 100644
--- a/include/core/utils/params_server.h
+++ b/include/core/utils/params_server.h
@@ -51,6 +51,8 @@ public:
void addParams(std::map<std::string, std::string> _params);
+ bool hasParam(std::string _key) const;
+
// template<typename T>
// T getParam(std::string key, std::string def_value) const {
// if(params_.find(key) != params_.end()){
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 0fc42a15dbf90088f389d87cf98705a5947ca047..7a4cb9ea7c682cf346c52b2f3e8e3dba90582b1a 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -44,7 +44,8 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr
map_ptr_(_map),
motion_provider_map_(),
frame_structure_(_frame_structure),
- prior_options_(std::make_shared<PriorOptions>())
+ prior_options_(std::make_shared<PriorOptions>()),
+ transformed_(false)
{
dim_ = _dim;
if (_frame_structure == "PO" and _dim == 2)
@@ -1255,4 +1256,42 @@ void Problem::perturb(double amplitude)
L->perturb(amplitude);
}
+void Problem::transform(const VectorComposite& _transformation)
+{
+ std::lock_guard<std::mutex> lock_transform (mut_transform_); // Protect especially from SolverManager::update()
+
+ transformation_ = _transformation;
+ transformed_.store(true);
+
+ // Sensors
+ for (auto S : getHardware()->getSensorList())
+ S->transform(_transformation);
+
+ // Frames and Captures
+ for (auto F : getTrajectory()->getFrameMap())
+ {
+ F.second->transform(_transformation);
+ for (auto& C : F.second->getCaptureList())
+ C->transform(_transformation);
+ }
+
+ // Landmarks
+ for (auto L : getMap()->getLandmarkList())
+ L->transform(_transformation);
+}
+
+bool Problem::isTransformed() const
+{
+ return transformed_.load();
+}
+void Problem::resetTransformed()
+{
+ transformed_.store(false);
+}
+VectorComposite Problem::getTransformation() const
+{
+ std::lock_guard<std::mutex> lock(mut_transform_);
+ return transformation_;
+}
+
} // namespace wolf
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 063c4e082a68e563249ffb46c447ab681530a178..682b02f654cf396452c4b646163a69bb284c219d 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -47,6 +47,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
MotionProvider(_state_structure, _params_motion),
params_motion_(_params_motion),
processing_step_(RUNNING_WITHOUT_KF),
+ bootstrapping_(false),
x_size_(_state_size),
data_size_(_data_size),
delta_size_(_delta_size),
@@ -231,8 +232,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Done at this place because setPrior() needs
integrateOneStep();
+ // perform bootstrap steps (usually only IMU requires this)
+ if (bootstrapping_) bootstrap();
- switch(processing_step_)
+ switch (processing_step_)
{
case RUNNING_WITH_KF_BEFORE_ORIGIN :
{
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 4620caea4e169aa3ef64d233e6883fdea53bed02..f5c4e6aa21c79b2e2d3b0875fdefb8c0108cb51a 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -37,7 +37,7 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
SensorBase("SensorDiffDrive",
std::make_shared<StateBlock>(_extrinsics.head(2), true),
std::make_shared<StateAngle>(_extrinsics(2), true),
- std::make_shared<StateBlock>(3, false), 2),
+ std::make_shared<StateBlock>(3, false, nullptr, false), 2),
params_diff_drive_(_intrinsics)
{
radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index c9d192a536151f0340a7fd1f8f9f5e88acf54c26..a9d3ee25ad0cc1e5d7c2a8991c221835fbcd4334 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -57,6 +57,9 @@ SolverManager::~SolverManager()
void SolverManager::update()
{
+ // lock mutex to prevent Problem::transform() during this update()
+ std::lock_guard<std::mutex> lock_transform (wolf_problem_->mut_transform_);
+
// Consume notification maps
std::map<StateBlockPtr,Notification> sb_notification_map;
std::map<FactorBasePtr,Notification> fac_notification_map;
@@ -147,8 +150,9 @@ void SolverManager::update()
state_ptr->resetFixUpdated();
state_ptr->resetLocalParamUpdated();
}
+ wolf_problem_->resetTransformed();
- #ifdef _WOLF_DEBUG
+#ifdef _WOLF_DEBUG
assert(check("after update()"));
#endif
}
@@ -188,7 +192,15 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
{
// Avoid usuless copies
if (!stateblock_statevector.first->isFixed())
- stateblock_statevector.first->setState(stateblock_statevector.second, false); // false = do not raise the flag state_updated_
+ {
+ stateblock_statevector.first->setState(stateblock_statevector.second,
+ false); // false = do not raise the flag state_updated_
+
+ // Transform the whole set of state blocks if necessary
+ // (this may happen when Problem::transform() has been called during the execution of this solve())
+ if (wolf_problem_->isTransformed())
+ stateblock_statevector.first->transform(wolf_problem_->getTransformation());
+ }
}
auto duration_state = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_state);
acc_duration_state_ += duration_state;
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 047469dc899a8973b7bdf07df318b2079c155ef9..91523db7c2ea51f7f47e680cfda5cac7cadacedf 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -76,6 +76,13 @@ void HasStateBlocks::perturb(double amplitude)
}
}
+void HasStateBlocks::transform(const VectorComposite& _transformation)
+{
+ for (auto& pair_key_sb : state_block_map_)
+ pair_key_sb.second->transform(_transformation);
+}
+
+
void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
if (_metric && _state_blocks)
diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp
index ce4a79743d24302907e85117b812458ed4de6c83..df5cd7677ddc448918c4c5aec0c74fddef0073e2 100644
--- a/src/utils/params_server.cpp
+++ b/src/utils/params_server.cpp
@@ -47,3 +47,8 @@ void ParamsServer::addParams(std::map<std::string, std::string> _params)
{
params_.insert(_params.begin(), _params.end());
}
+
+bool ParamsServer::hasParam(std::string _key) const
+{
+ return (params_.count(_key) != 0);
+}
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index bdc5e696b1e4a012abdee9fc93b16bd647b85172..7ec49fdee46c118d8d7907e17a43f5a2c598e661 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -813,6 +813,6 @@ int main(int argc, char **argv)
*/
testing::InitGoogleTest(&argc, argv);
- ::testing::GTEST_FLAG(filter) = "Conversions.R2q_norm_of_q";
+ // ::testing::GTEST_FLAG(filter) = "Conversions.R2q_norm_of_q";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
index 9bce46a4856e123124e3b62aed99c4e2a09a7b01..e1696bcc92f15f5f87a1aeed24fbed554e8cdbab 100644
--- a/test/gtest_state_block.cpp
+++ b/test/gtest_state_block.cpp
@@ -28,21 +28,21 @@
#include "core/utils/utils_gtest.h"
-
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/state_angle.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_composite.h"
#include <iostream>
-
using namespace Eigen;
using namespace std;
using namespace wolf;
TEST(StateBlock, block_perturb)
{
- Vector3d x(10,20,30);
+ Vector3d x(10, 20, 30);
StateBlock sb(x);
sb.perturb(0.5);
@@ -50,12 +50,12 @@ TEST(StateBlock, block_perturb)
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...
+ ASSERT_MATRIX_APPROX(x, sb.getState(), 0.5 * 4); // 4-sigma test...
}
TEST(StateBlock, angle_perturb)
{
- Vector1d x(1.0);
+ Vector1d x(1.0);
StateAngle sb(x(0));
sb.perturb(0.1);
@@ -63,28 +63,174 @@ TEST(StateBlock, angle_perturb)
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...
+ 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);
+ 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...
+ 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)
+TEST(StatePoint3d, transformable)
+{
+ Vector3d x(1, 2, 3);
+
+ // default is transformable
+ StatePoint3d sb_tr(x);
+ ASSERT_TRUE(sb_tr.isTransformable());
+
+ // not fixed, non transformable
+ StatePoint3d sb_ntr(x, false, false);
+ ASSERT_FALSE(sb_ntr.isTransformable());
+
+ // not fixed, transformable
+ StatePoint3d sb_tr2(x, false, true);
+ ASSERT_TRUE(sb_tr2.isTransformable());
+}
+
+TEST(StatePoint3d, transform_identity)
+{
+ Vector3d x0(1, 2, 3);
+ StatePoint3d sb(x0);
+
+ Vector7d xtrf;
+ xtrf << 0, 0, 0, 0, 0, 0, 1;
+ VectorComposite trf(xtrf, "PO", {3, 4});
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x0, 1e-15);
+}
+
+TEST(StatePoint3d, transform_translation_4x5y6z)
+{
+ Vector3d x0(1, 2, 3);
+ StatePoint3d sb(x0);
+
+ Vector7d xtrf;
+ xtrf << 4, 5, 6, 0, 0, 0, 1;
+ VectorComposite trf(xtrf, "PO", {3, 4});
+
+ Vector3d x(5, 7, 9); // x0 translated (4,5,6)
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StatePoint3d, transform_rotation_90x)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ Vector3d x0(1, 2, 3);
+ StatePoint3d sb(x0);
+
+ Vector7d xtrf;
+ xtrf << 0, 0, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+ VectorComposite trf(xtrf, "PO", {3, 4});
+
+ Vector3d x(1, -3, 2); // x0 rotated (90,0,0)
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
}
+TEST(StatePoint3d, transform_translation_1y_rotation_90x)
+{
+ Vector3d x0(1, 2, 3);
+ StatePoint3d sb(x0);
+
+ Vector7d xtrf;
+ xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+ VectorComposite trf(xtrf, "PO", {3, 4});
+
+ Vector3d x(1, -2, 2); // x0 translated (0,1,0) and rotated (90,0,0)
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StatePoint3d, non_transformable_translation_rotation)
+{
+ Vector3d x0(1, 2, 3);
+ StatePoint3d sb(x0, false, false); // non transformable
+ Vector7d xtrf;
+ xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+ VectorComposite trf(xtrf, "PO", {3, 4});
+ Vector3d x(1, 2, 3); // x0 NOT translated (0,1,0) and NOT rotated (90,0,0)
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StateVector3d, transform_translation_1y_rotation_90x)
+{
+ Vector3d x0(1, 2, 3);
+ StateVector3d sb(x0);
+
+ Vector7d xtrf;
+ xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+ VectorComposite trf(xtrf, "PO", {3, 4});
+
+ Vector3d x(1, -3, 2); // x0 NOT translated (0,1,0) and rotated (90,0,0)
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StatePoint2d, transform_translation_1y_rotation_90)
+{
+ Vector2d x0(1, 2);
+ StatePoint2d sb(x0);
+
+ Vector3d xtrf;
+ xtrf << 0, 1, M_PI / 2;
+ VectorComposite trf(xtrf, "PO", {2, 1});
+
+ Vector2d x(-2, 2); // x0 translated (0,1) and rotated 90 deg
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StateVector2d, transform_translation_1y_rotation_90)
+{
+ Vector2d x0(1, 2);
+ StateVector2d sb(x0);
+
+ Vector3d xtrf;
+ xtrf << 0, 1, M_PI / 2;
+ VectorComposite trf(xtrf, "PO", {2, 1});
+
+ Vector2d x(-2, 1); // x0 NON translated (0,1) and rotated 90 deg
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StateParams4, transform_translation_1y_rotation_90x)
+{
+ Vector4d x0(1, 2, 3, 4);
+ StateParams<4> sb(x0);
+
+ Vector7d xtrf;
+ xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+ VectorComposite trf(xtrf, "PO", {3, 4});
+
+ Vector4d x(1, 2, 3, 4); // x0 NOT translated (0,1,0) and NOT rotated (90,0,0)
+
+ sb.transform(trf);
+ ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}