diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index c10374ae82ae1a8f495e6adc3bea5b7f167b548b..324b83297d987561c4a119e699ac44c2d0eb4ec5 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -83,8 +83,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
// State blocks
const StateStructure& getStructure() const;
- StateBlockPtr getStateBlock(const std::string& _key) const;
- StateBlockPtr getStateBlock(const char _key) const { return getStateBlock(std::string(1, _key)); }
+ StateBlockPtr getStateBlock(const char& _key) const;
StateBlockPtr getSensorP() const;
StateBlockPtr getSensorO() const;
StateBlockPtr getSensorIntrinsic() const;
@@ -138,17 +137,17 @@ std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... al
inline StateBlockPtr CaptureBase::getSensorP() const
{
- return getStateBlock("P");
+ return getStateBlock('P');
}
inline StateBlockPtr CaptureBase::getSensorO() const
{
- return getStateBlock("O");
+ return getStateBlock('O');
}
inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
{
- return getStateBlock("I");
+ return getStateBlock('I');
}
inline unsigned int CaptureBase::id() const
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 91a94d274b36d0023a0ba267583f4b67726d8665..a28e13e1e73c089f717a43dcf23ed8d2cc486fe3 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -99,7 +99,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
// State blocks ----------------------------------------------------
public:
using HasStateBlocks::addStateBlock;
- StateBlockPtr addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb);
+ StateBlockPtr addStateBlock(const char& _sb_type, const StateBlockPtr& _sb);
StateBlockPtr getV() const;
protected:
@@ -211,7 +211,7 @@ inline TimeStamp FrameBase::getTimeStamp() const
inline StateBlockPtr FrameBase::getV() const
{
- return getStateBlock("V");
+ return getStateBlock('V');
}
inline TrajectoryBasePtr FrameBase::getTrajectory() const
@@ -234,7 +234,7 @@ inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
return constrained_by_list_;
}
-inline StateBlockPtr FrameBase::addStateBlock(const std::string& _sb_type,
+inline StateBlockPtr FrameBase::addStateBlock(const char& _sb_type,
const StateBlockPtr& _sb)
{
if (isKeyOrAux())
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index 4ac32d2ded2923f212937cfa88a0994fb0dca5b4..a051a63e3989f4f0363fc96a8ed462fd1b407cea 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -69,8 +69,8 @@ inline Eigen::Matrix2d V_helper(const T theta)
inline VectorComposite identity()
{
VectorComposite v;
- v["P"] = Vector2d::Zero();
- v["O"] = Vector1d::Zero();
+ v['P'] = Vector2d::Zero();
+ v['O'] = Vector1d::Zero();
return v;
}
@@ -138,12 +138,12 @@ inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D
inline void exp(const VectorComposite& _tau, VectorComposite& _delta)
{
// [1] eq. 156
- const auto &p = _tau.at("P");
- const auto &theta = pi2pi(_tau.at("O")(0));
+ const auto &p = _tau.at('P');
+ const auto &theta = pi2pi(_tau.at('O')(0));
Matrix2d V = V_helper(theta);
- _delta["P"] = V * p;
- _delta["O"] = Matrix1d(theta);
+ _delta['P'] = V * p;
+ _delta['O'] = Matrix1d(theta);
}
inline VectorComposite exp(const VectorComposite& tau)
@@ -158,22 +158,22 @@ inline VectorComposite exp(const VectorComposite& tau)
inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComposite& _J_delta_tau)
{
// [1] eq. 156
- const auto &p = _tau.at("P");
- const auto &theta = pi2pi(_tau.at("O")(0));
+ const auto &p = _tau.at('P');
+ const auto &theta = pi2pi(_tau.at('O')(0));
Matrix2d V = V_helper(theta);
- _delta["P"] = V * p;
- _delta["O"] = Matrix1d(theta);
+ _delta['P'] = V * p;
+ _delta['O'] = Matrix1d(theta);
// Jacobian follows the composite definition in [1] eq. 89, with jacobian blocks:
// J_Vp_p = V: see V_helper, eq. 158
// J_Vp_theta: see fcn helper
// J_theta_theta = 1; eq. 126
_J_delta_tau.clear();
- _J_delta_tau.emplace("P", "P", V);
- _J_delta_tau.emplace("P", "O", J_Vp_theta(p, theta));
- _J_delta_tau.emplace("O", "P", RowVector2d(0.0, 0.0));
- _J_delta_tau.emplace("O", "O", Matrix1d(1));
+ _J_delta_tau.emplace('P', 'P', V);
+ _J_delta_tau.emplace('P', 'O', J_Vp_theta(p, theta));
+ _J_delta_tau.emplace('O', 'P', RowVector2d(0.0, 0.0));
+ _J_delta_tau.emplace('O', 'O', Matrix1d(1));
}
template<class D1, class D2, class D3>
@@ -247,18 +247,18 @@ inline void compose(const VectorComposite& _x1,
const VectorComposite& _x2,
VectorComposite& _c)
{
- const auto& p1 = _x1.at("P");
- const auto& a1 = _x1.at("O")(0); // angle
+ const auto& p1 = _x1.at('P');
+ const auto& a1 = _x1.at('O')(0); // angle
const auto& R1 = SO2::exp(a1);
- const auto& p2 = _x2.at("P");
- const auto& a2 = _x2.at("O")(0); // angle
+ const auto& p2 = _x2.at('P');
+ const auto& a2 = _x2.at('O')(0); // angle
// tc = t1 + R1 t2
- _c["P"] = p1 + R1 * p2;
+ _c['P'] = p1 + R1 * p2;
// Rc = R1 R2 --> theta = theta1 + theta2
- _c["O"] = Matrix1d( pi2pi(a1 + a2) ) ;
+ _c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
}
inline void compose(const VectorComposite& _x1,
@@ -267,12 +267,12 @@ inline void compose(const VectorComposite& _x1,
MatrixComposite& _J_c_x1,
MatrixComposite& _J_c_x2)
{
- const auto& p1 = _x1.at("P");
- const auto& a1 = _x1.at("O")(0); // angle
+ const auto& p1 = _x1.at('P');
+ const auto& a1 = _x1.at('O')(0); // angle
Matrix2d R1 = SO2::exp(a1); // need temporary
- const auto& p2 = _x2.at("P");
- const auto& a2 = _x2.at("O")(0); // angle
+ const auto& p2 = _x2.at('P');
+ const auto& a2 = _x2.at('O')(0); // angle
/* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
*
@@ -302,25 +302,25 @@ inline void compose(const VectorComposite& _x1,
*/
_J_c_x1.clear();
- _J_c_x1.emplace("P", "P", Matrix2d::Identity());
- _J_c_x1.emplace("P", "O", MatrixXd(R1 * skewed(p2)) );
- _J_c_x1.emplace("O", "P", RowVector2d(0,0));
- _J_c_x1.emplace("O", "O", Matrix1d(1));
+ _J_c_x1.emplace('P', 'P', Matrix2d::Identity());
+ _J_c_x1.emplace('P', 'O', MatrixXd(R1 * skewed(p2)) );
+ _J_c_x1.emplace('O', 'P', RowVector2d(0,0));
+ _J_c_x1.emplace('O', 'O', Matrix1d(1));
_J_c_x2.clear();
- _J_c_x2.emplace("P", "P", R1);
- _J_c_x2.emplace("P", "O", Vector2d(0,0));
- _J_c_x2.emplace("O", "P", RowVector2d(0,0));
- _J_c_x2.emplace("O", "O", Matrix1d(1));
+ _J_c_x2.emplace('P', 'P', R1);
+ _J_c_x2.emplace('P', 'O', Vector2d(0,0));
+ _J_c_x2.emplace('O', 'P', RowVector2d(0,0));
+ _J_c_x2.emplace('O', 'O', Matrix1d(1));
// Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1' or 'x2'.
// tc = t1 + R1 t2
- _c["P"] = p1 + R1 * p2;
+ _c['P'] = p1 + R1 * p2;
// Rc = R1 R2 --> theta = theta1 + theta2
- _c["O"] = Matrix1d( pi2pi(a1 + a2) ) ;
+ _c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
}
@@ -366,15 +366,15 @@ inline Matrix<typename D1::Scalar, 3, 1> plus(const MatrixBase<D1>& d1,
inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
{
- plus(x.at("P"), x.at("O"), tau.at("P"), tau.at("O"), res.at("P"), res.at("O"));
+ plus(x.at('P'), x.at('O'), tau.at('P'), tau.at('O'), res.at('P'), res.at('O'));
}
inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
{
VectorComposite res;
- res["P"] = Vector2d();
- res["O"] = Vector1d();
+ res['P'] = Vector2d();
+ res['O'] = Vector1d();
plus(x, tau, res);
diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index 4b000889c59320e8c7e819725c415dd19308d077..79db282a20d91648b2f965d1b057dbb3b92cf324 100644
--- a/include/core/math/SE3.h
+++ b/include/core/math/SE3.h
@@ -67,8 +67,8 @@ inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q)
inline void identity(VectorComposite& v)
{
v.clear();
- v["P"] = Vector3d::Zero();
- v["O"] = Quaterniond::Identity().coeffs();
+ v['P'] = Vector3d::Zero();
+ v['O'] = Quaterniond::Identity().coeffs();
}
template<typename T = double>
@@ -207,7 +207,7 @@ inline void compose(const MatrixBase<D1>& d1,
inline void compose(const VectorComposite& x1, const VectorComposite& x2, VectorComposite& c)
{
- compose(x1.at("P"), x1.at("O"), x2.at("P"), x2.at("O"), c["P"], c["O"]);
+ compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']);
}
inline void compose(const VectorComposite& x1,
@@ -218,20 +218,20 @@ inline void compose(const VectorComposite& x1,
{
// Some useful temporaries
- const auto R1 = q2R(x1.at("O")); //q1.matrix(); // make temporary
- const auto& R2 = q2R(x2.at("O")); //q2.matrix(); // do not make temporary, only reference
+ const auto R1 = q2R(x1.at('O')); //q1.matrix(); // make temporary
+ const auto& R2 = q2R(x2.at('O')); //q2.matrix(); // do not make temporary, only reference
// Jac wrt first delta
- J_c_x1.emplace("P", "P", Matrix3d::Identity() ) ;
- J_c_x1.emplace("P", "O", - R1 * skew(x2.at("P")) ) ;
- J_c_x1.emplace("O", "P", Matrix3d::Zero() ) ;
- J_c_x1.emplace("O", "O", R2.transpose() ) ;
+ J_c_x1.emplace('P', 'P', Matrix3d::Identity() ) ;
+ J_c_x1.emplace('P', 'O', - R1 * skew(x2.at('P')) ) ;
+ J_c_x1.emplace('O', 'P', Matrix3d::Zero() ) ;
+ J_c_x1.emplace('O', 'O', R2.transpose() ) ;
// Jac wrt second delta
- J_c_x2.emplace("P", "P", R1 );
- J_c_x2.emplace("P", "O", Matrix3d::Zero() );
- J_c_x2.emplace("O", "P", Matrix3d::Zero() );
- J_c_x2.emplace("O", "O", Matrix3d::Identity() );
+ J_c_x2.emplace('P', 'P', R1 );
+ J_c_x2.emplace('P', 'O', Matrix3d::Zero() );
+ J_c_x2.emplace('O', 'P', Matrix3d::Zero() );
+ J_c_x2.emplace('O', 'O', Matrix3d::Identity() );
// compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the same variable
compose(x1, x2, c);
@@ -325,15 +325,15 @@ inline Matrix<typename Derived::Scalar, 7, 1> exp(const MatrixBase<Derived>& d_i
inline VectorComposite exp(const VectorComposite& tau)
{
- const auto& p = tau.at("P");
- const auto& theta = tau.at("O");
+ const auto& p = tau.at('P');
+ const auto& theta = tau.at('O');
Matrix3d V = jac_SO3_left(theta); // see [1] eqs. (174) and (145)
VectorComposite res;
- res["P"] = V * p ;
- res["O"] = exp_q(theta).coeffs() ;
+ res['P'] = V * p ;
+ res['O'] = exp_q(theta).coeffs() ;
return res;
}
@@ -392,15 +392,15 @@ inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
{
- plus(x.at("P"), x.at("O"), tau.at("P"), tau.at("O"), res.at("P"), res.at("O"));
+ plus(x.at('P'), x.at('O'), tau.at('P'), tau.at('O'), res.at('P'), res.at('O'));
}
inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
{
VectorComposite res;
- res["P"] = Vector3d();
- res["O"] = Vector4d();
+ res['P'] = Vector3d();
+ res['O'] = Vector4d();
plus(x, tau, res);
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index e50b2f55764cbc2a5570a4192ed8c15f560804fe..6fb9eb40f5923a5ca3526e64bc8abaa3da2397df 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -71,12 +71,12 @@ class ProcessorDiffDrive : public ProcessorOdom2d
inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBasePtr _capture) const
{
- return _capture->getStateBlock("I")->getState();
+ return _capture->getStateBlock('I')->getState();
}
inline void ProcessorDiffDrive::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
{
- _capture->getStateBlock("I")->setState(_calibration);
+ _capture->getStateBlock('I')->setState(_calibration);
}
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index d59daef943bffdc01e835453963f4dac8c7ad7df..b1572a010a6594473b51aceb949f5b21c26e8eb6 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -96,9 +96,9 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
unsigned int sensor_id_; // sensor ID
- std::map<std::string, bool> state_block_dynamic_; // mark dynamic state blocks
+ std::map<char, bool> state_block_dynamic_; // mark dynamic state blocks
- std::map<std::string, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
+ std::map<char, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
protected:
Eigen::VectorXd noise_std_; // std of sensor noise
@@ -174,21 +174,18 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
// State blocks
using HasStateBlocks::addStateBlock;
- StateBlockPtr addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false);
- StateBlockPtr addStateBlock(const char _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false) { return this->addStateBlock(std::string(1, _key), _sb_ptr, _dynamic); }
- StateBlockPtr getStateBlockDynamic(const std::string& _key) const;
- StateBlockPtr getStateBlockDynamic(const std::string& _key, const TimeStamp& _ts) const;
- 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); }
+ StateBlockPtr addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false);
+ StateBlockPtr getStateBlockDynamic(const char& _key) const;
+ StateBlockPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const;
// Declare a state block as dynamic or static (with _dymanic = false)
- void setStateBlockDynamic(const std::string& _key, bool _dynamic = true);
+ void setStateBlockDynamic(const char& _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;
- bool isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts) const;
- bool isStateBlockInCapture(const std::string& _key) const;
+ bool isStateBlockDynamic(const char& _key) const;
+ bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const;
+ bool isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) const;
+ bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts) const;
+ bool isStateBlockInCapture(const char& _key) const;
StateBlockPtr getP(const TimeStamp _ts) const;
StateBlockPtr getO(const TimeStamp _ts) const;
@@ -218,7 +215,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
* \param _size state segment size (-1: whole state) (not used in quaternions)
*
**/
- void addPriorParameter(const std::string& _key,
+ void addPriorParameter(const char& _key,
const Eigen::VectorXd& _x,
const Eigen::MatrixXd& _cov,
unsigned int _start_idx = 0,
@@ -296,7 +293,7 @@ inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
return processor_list_;
}
-inline StateBlockPtr SensorBase::addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic)
+inline StateBlockPtr SensorBase::addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic)
{
assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
HasStateBlocks::addStateBlock(_key, _sb_ptr, getProblem());
@@ -304,13 +301,13 @@ inline StateBlockPtr SensorBase::addStateBlock(const std::string& _key, const St
return _sb_ptr;
}
-inline void SensorBase::setStateBlockDynamic(const std::string& _key, bool _dynamic)
+inline void SensorBase::setStateBlockDynamic(const char& _key, bool _dynamic)
{
assert(hasStateBlock(_key) and "setting dynamic an state block of a key that doesn't exist in the sensor. It is required anyway.");
state_block_dynamic_[_key] = _dynamic;
}
-inline bool SensorBase::isStateBlockDynamic(const std::string& _key) const
+inline bool SensorBase::isStateBlockDynamic(const char& _key) const
{
assert(state_block_dynamic_.count(_key) != 0);
return state_block_dynamic_.at(_key);
@@ -338,17 +335,17 @@ inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr)
inline void SensorBase::addPriorP(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
{
- addPriorParameter("P", _x, _cov, _start_idx, _size);
+ addPriorParameter('P', _x, _cov, _start_idx, _size);
}
inline void SensorBase::addPriorO(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov)
{
- addPriorParameter("O", _x, _cov);
+ addPriorParameter('O', _x, _cov);
}
inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
{
- addPriorParameter("I", _x, _cov);
+ addPriorParameter('I', _x, _cov);
}
inline SizeEigen SensorBase::getCalibSize() const
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
index efa0db6d76410f4437180b3bc9f2c844c2b878d5..4e1028bf954a7c2ac71e4d133a097700a6150283 100644
--- a/include/core/state_block/factory_state_block.h
+++ b/include/core/state_block/factory_state_block.h
@@ -116,7 +116,7 @@ inline std::string FactoryStateBlock::getClass() const
}
template<>
-inline StateBlockPtr FactoryStateBlock::create(const std::string& _type, const Eigen::VectorXd& _state, bool _fixed)
+inline StateBlockPtr FactoryStateBlock::create(const string& _type, const Eigen::VectorXd& _state, bool _fixed)
{
typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type);
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index a2d6e1e767da2bb800e5f83146a596ea8e35535a..5e1250ff066611a40af092e937324e840e58ae0e 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -16,10 +16,6 @@
namespace wolf
{
-/// State of nodes containing several state blocks
-//typedef std::unordered_map<std::string, Eigen::VectorXd> State;
-//typedef std::string StateStructure;
-
class HasStateBlocks
{
@@ -29,10 +25,10 @@ class HasStateBlocks
virtual ~HasStateBlocks();
const StateStructure& getStructure() const { return structure_; }
- void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; }
- bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
+ void appendToStructure(const char& _frame_type){ structure_ += _frame_type; }
+ bool isInStructure(const char& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
- const std::unordered_map<std::string, StateBlockPtr>& getStateBlockMap() const;
+ const std::unordered_map<char, StateBlockPtr>& getStateBlockMap() const;
std::vector<StateBlockPtr> getStateBlockVec() const;
// Some typical shortcuts -- not all should be coded here, see notes below.
@@ -44,27 +40,22 @@ class HasStateBlocks
virtual void unfix();
virtual bool isFixed() const;
- virtual StateBlockPtr addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem);
- virtual StateBlockPtr addStateBlock(const char _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem) { return addStateBlock(std::string(1,_sb_type), _sb, _problem); }
- virtual unsigned int removeStateBlock(const std::string& _sb_type);
- virtual unsigned int removeStateBlock(const char _sb_type);
- bool hasStateBlock(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
- bool hasStateBlock(const char _sb_type) const { return hasStateBlock(std::string(1, _sb_type)); }
+ virtual StateBlockPtr addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem);
+ virtual unsigned int removeStateBlock(const char& _sb_type);
+ bool hasStateBlock(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
bool hasStateBlock(const StateBlockPtr& _sb) const;
- StateBlockPtr getStateBlock(const std::string& _sb_type) const;
- StateBlockPtr getStateBlock(const char _sb_type) const { return getStateBlock(std::string(1,_sb_type)); }
- bool setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb);
- bool setStateBlock(const char _sb_type, const StateBlockPtr& _sb) { return setStateBlock(std::string(1, _sb_type), _sb); }
- bool stateBlockKey(const StateBlockPtr& _sb, std::string& _key) const;
- std::unordered_map<std::string, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb) const;
+ StateBlockPtr getStateBlock(const char& _sb_type) const;
+ bool setStateBlock(const char _sb_type, const StateBlockPtr& _sb);
+ bool stateBlockKey(const StateBlockPtr& _sb, char& _key) const;
+ std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb) const;
// Emplace derived state blocks (angle, quaternion, etc).
template<typename SB, typename ... Args>
- std::shared_ptr<SB> emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
+ std::shared_ptr<SB> emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
// Emplace base state blocks.
template<typename ... Args>
- StateBlockPtr emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
+ StateBlockPtr emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
// Register/remove state blocks to/from wolf::Problem
void registerNewStateBlocks(ProblemPtr _problem) const;
@@ -87,7 +78,7 @@ class HasStateBlocks
private:
StateStructure structure_;
- std::unordered_map<std::string, StateBlockPtr> state_block_map_;
+ std::unordered_map<char, StateBlockPtr> state_block_map_;
};
@@ -112,7 +103,7 @@ inline HasStateBlocks::~HasStateBlocks()
//
}
-inline const std::unordered_map<std::string, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const
+inline const std::unordered_map<char, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const
{
return state_block_map_;
}
@@ -127,18 +118,18 @@ inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() const
return sbv;
}
-inline unsigned int HasStateBlocks::removeStateBlock(const char _sb_type)
-{
- return removeStateBlock(std::string(1, _sb_type));
-}
+//inline unsigned int HasStateBlocks::removeStateBlock(const char _sb_type)
+//{
+// return removeStateBlock(std::string(1, _sb_type));
+//}
-inline unsigned int HasStateBlocks::removeStateBlock(const std::string& _sb_type)
+inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_type)
{
return state_block_map_.erase(_sb_type);
}
template<typename SB, typename ... Args>
-inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
+inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
{
assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
@@ -149,7 +140,7 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string&
}
template<typename ... Args>
-inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor)
+inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor)
{
assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
@@ -159,7 +150,7 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_ty
return sb;
}
-inline bool HasStateBlocks::setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb)
+inline bool HasStateBlocks::setStateBlock(const char _sb_type, const StateBlockPtr& _sb)
{
assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
assert ( state_block_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
@@ -167,7 +158,7 @@ inline bool HasStateBlocks::setStateBlock(const std::string _sb_type, const Stat
return true; // success
}
-inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const std::string& _sb_type) const
+inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type) const
{
auto it = state_block_map_.find(_sb_type);
if (it != state_block_map_.end())
@@ -178,12 +169,12 @@ inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const std::string& _sb_
inline wolf::StateBlockPtr HasStateBlocks::getP() const
{
- return getStateBlock("P");
+ return getStateBlock('P');
}
inline wolf::StateBlockPtr HasStateBlocks::getO() const
{
- return getStateBlock("O");
+ return getStateBlock('O');
}
inline void HasStateBlocks::fix()
@@ -238,9 +229,8 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateS
assert(_state.size() == size && "In FrameBase::setState wrong state size");
unsigned int index = 0;
- for (const char ckey : structure)
+ for (const char key : structure)
{
- const auto& key = string(1,ckey);
const auto& sb = getStateBlock(key);
assert (sb && "Stateblock key not in the structure");
@@ -257,9 +247,8 @@ inline void HasStateBlocks::setState (const Eigen::VectorXd& _state,
{
SizeEigen index = 0;
auto size_it = _sizes.begin();
- for (const auto& ckey : _structure)
+ for (const auto& key : _structure)
{
- const auto& key = string(1,ckey);
const auto& sb = getStateBlock(key);
assert (sb && "Stateblock key not in the structure");
assert(*size_it == sb->getSize() && "State block size mismatch");
@@ -274,9 +263,8 @@ inline void HasStateBlocks::setState (const Eigen::VectorXd& _state,
inline void HasStateBlocks::setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify)
{
auto vec_it = _vectors.begin();
- for (const auto ckey : _structure)
+ for (const auto key : _structure)
{
- const auto key = string(1,ckey);
const auto& sb = getStateBlock(key);
assert (sb && "Stateblock key not in the structure");
assert(vec_it->size() == sb->getSize() && "State block size mismatch");
@@ -317,10 +305,8 @@ inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure
VectorComposite state;
- for (const auto ckey : structure)
+ for (const auto key : structure)
{
- const auto& key = string(1,ckey); // ckey is char
-
auto state_it = state_block_map_.find(key);
if (state_it != state_block_map_.end())
@@ -328,10 +314,6 @@ inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure
state.emplace(key, state_it->second->getState());
}
-// for (auto& pair_key_sb : state_block_map_)
-// {
-// state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
-// }
return state;
}
@@ -352,11 +334,11 @@ inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) co
return size;
}
-inline std::unordered_map<std::string, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
+inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
{
const auto& it = std::find_if(state_block_map_.begin(),
state_block_map_.end(),
- [_sb](const std::pair<std::string, StateBlockPtr>& pair)
+ [_sb](const std::pair<char, StateBlockPtr>& pair)
{
return pair.second == _sb;
}
@@ -372,7 +354,7 @@ inline bool HasStateBlocks::hasStateBlock(const StateBlockPtr &_sb) const
return it != state_block_map_.end();
}
-inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string& _key) const
+inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, char& _key) const
{
const auto& it = this->find(_sb);
@@ -385,7 +367,7 @@ inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string&
}
else
{
- _key = "";
+ _key = '0'; // '0' = not found
return false;
}
}
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index 0d3cafcc80c602a4de1bf59b9ce7c2ddd9123c9a..c7a125ac0d96b1d1b0799ab9e983bccf76bf36db 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -22,16 +22,16 @@ using std::string;
using namespace Eigen;
typedef std::string StateStructure;
-typedef std::unordered_map < std::string, StateBlockPtr > StateBlockMap;
+typedef std::unordered_map < char, StateBlockPtr > StateBlockMap;
typedef StateBlockMap::const_iterator StateBlockMapCIter;
-class VectorComposite : public std::unordered_map < std::string, Eigen::VectorXd >
+class VectorComposite : public std::unordered_map < char, Eigen::VectorXd >
{
public:
VectorComposite() {};
VectorComposite(const StateStructure& _s);
VectorComposite(const StateStructure& _s, const std::list<int>& _sizes);
- VectorComposite(const VectorComposite & v) : unordered_map<string, VectorXd>(v){};
+ VectorComposite(const VectorComposite & v) : unordered_map<char, VectorXd>(v){};
/**
* \brief Construct from Eigen::VectorXd and structure
*
@@ -76,17 +76,17 @@ class VectorComposite : public std::unordered_map < std::string, Eigen::VectorXd
};
-class MatrixComposite : public std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > >
+class MatrixComposite : public std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >
{
private:
- std::unordered_map < string, unsigned int> size_rows_, size_cols_;
+ std::unordered_map < char, unsigned int> size_rows_, size_cols_;
unsigned int rows() const;
unsigned int cols() const;
void sizeAndIndices(const StateStructure & _struct_rows,
const StateStructure & _struct_cols,
- std::unordered_map < string, unsigned int>& _indices_rows,
- std::unordered_map < string, unsigned int>& _indices_cols,
+ std::unordered_map < char, unsigned int>& _indices_rows,
+ std::unordered_map < char, unsigned int>& _indices_cols,
unsigned int& _rows,
unsigned int& _cols) const;
@@ -129,25 +129,25 @@ class MatrixComposite : public std::unordered_map < std::string, std::unordered_
static MatrixComposite identity(const StateStructure& _structure,
const std::list<int>& _sizes);
- unsigned int count(const std::string &_row,
- const std::string &_col) const;
+ unsigned int count(const char &_row,
+ const char &_col) const;
- bool emplace(const std::string &_row,
- const std::string &_col,
+ bool emplace(const char &_row,
+ const char &_col,
const Eigen::MatrixXd &_mat_blk);
// throw error if queried block is not present
- bool at(const std::string &_row,
- const std::string &_col,
+ bool at(const char &_row,
+ const char &_col,
Eigen::MatrixXd &_mat_blk) const;
- const MatrixXd& at(const std::string &_row,
- const std::string &_col) const;
- MatrixXd& at(const std::string &_row,
- const std::string &_col);
+ const MatrixXd& at(const char &_row,
+ const char &_col) const;
+ MatrixXd& at(const char &_row,
+ const char &_col);
// make some shadowed API available
- using std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > >::at;
- using std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > >::count;
+ using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::at;
+ using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::count;
MatrixXd matrix(const StateStructure & _struct_rows,
@@ -266,25 +266,25 @@ class StateBlockComposite
const StateBlockMap& getStateBlockMap() const;
- StateBlockPtr add(const std::string& _sb_type, const StateBlockPtr& _sb);
+ StateBlockPtr add(const char& _sb_type, const StateBlockPtr& _sb);
// Emplace derived state blocks (angle, quaternion, etc).
template<typename SB, typename ... Args>
- std::shared_ptr<SB> emplace(const std::string& _sb_type, Args&&... _args_of_derived_state_block_constructor);
+ std::shared_ptr<SB> emplace(const char& _sb_type, Args&&... _args_of_derived_state_block_constructor);
// Emplace base state blocks.
template<typename ... Args>
- inline StateBlockPtr emplace(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor);
+ inline StateBlockPtr emplace(const char& _sb_type, Args&&... _args_of_base_state_block_constructor);
- unsigned int remove(const std::string& _sb_type);
- bool has(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
+ unsigned int remove(const char& _sb_type);
+ bool has(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
bool has(const StateBlockPtr& _sb) const;
- StateBlockPtr at(const std::string& _sb_type) const;
- void set(const std::string& _sb_type, const StateBlockPtr& _sb);
- void set(const std::string& _sb_type, const VectorXd& _vec);
+ StateBlockPtr at(const char& _sb_type) const;
+ void set(const char& _sb_type, const StateBlockPtr& _sb);
+ void set(const char& _sb_type, const VectorXd& _vec);
void setVectors(const StateStructure& _structure, const std::list<VectorXd> &_vectors);
- bool key(const StateBlockPtr& _sb, std::string& _key) const;
- std::string key(const StateBlockPtr& _sb) const;
+ bool key(const StateBlockPtr& _sb, char& _key) const;
+ char key(const StateBlockPtr& _sb) const;
StateBlockMapCIter find(const StateBlockPtr& _sb) const;
// identity and zero (they are the same with different names)
@@ -321,7 +321,7 @@ class StateBlockComposite
//////////// IMPLEMENTATION ////////////
-inline unsigned int MatrixComposite::count(const std::string &_row, const std::string &_col) const
+inline unsigned int MatrixComposite::count(const char &_row, const char &_col) const
{
const auto& rit = this->find(_row);
@@ -334,7 +334,7 @@ inline unsigned int MatrixComposite::count(const std::string &_row, const std::s
template<typename SB, typename ... Args>
-inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const std::string &_sb_type,
+inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const char &_sb_type,
Args &&... _args_of_derived_state_block_constructor)
{
assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
@@ -346,7 +346,7 @@ inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const std::string
}
template<typename ... Args>
-inline StateBlockPtr wolf::StateBlockComposite::emplace(const std::string &_sb_type,
+inline StateBlockPtr wolf::StateBlockComposite::emplace(const char &_sb_type,
Args &&... _args_of_base_state_block_constructor)
{
assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
@@ -358,7 +358,7 @@ inline StateBlockPtr wolf::StateBlockComposite::emplace(const std::string &_sb_t
}
inline MatrixComposite::MatrixComposite (const MatrixComposite& m)
- : unordered_map<string, unordered_map<string, MatrixXd> >(m), size_rows_(m.size_rows_), size_cols_(m.size_cols_)
+ : unordered_map<char, unordered_map<char, MatrixXd> >(m), size_rows_(m.size_rows_), size_cols_(m.size_cols_)
{
}
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index 8c8e34935d728a505537643fa4c51bf001787bb6..76e710f75d4aeb31589e27aa819304ec0b8d9cec 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -94,6 +94,16 @@ struct converter<bool>{
}
};
template<>
+struct converter<char>{
+ static char convert(std::string val){
+ //Here we should check that val.length() == 1 and get val[0] into a char variable
+ if(val.length() == 1) return val.at(0);
+ else throw std::runtime_error("Invalid converstion to char. String too long. String provided: " + val);
+ throw std::runtime_error("Invalid char conversion. String provided: " + val);
+ }
+};
+ //// TYPES ----> ToSTRING
+template<>
struct converter<std::string>{
static std::string convert(std::string val){
return val;
@@ -109,6 +119,9 @@ struct converter<std::string>{
static std::string convert(double val){
return std::to_string(val);
}
+ static std::string convert(char val){
+ return std::to_string(val);
+ }
template<typename A>
static std::string convert(utils::list<A> val){
std::string result = "";
@@ -171,17 +184,17 @@ struct converter<std::string>{
static std::string convert(VectorComposite val){
//TODO
// throw std::runtime_error("TODO! We still haven't got around to implement convert for VectorComposite to String :(");
- auto helper = std::unordered_map<StateStructure, Eigen::VectorXd>();
+ auto helper = std::unordered_map<char, Eigen::VectorXd>();
for(const auto& pair: val)
- helper.insert(std::pair<StateStructure, Eigen::VectorXd>(pair.first, pair.second));
+ helper.insert(std::pair<char, Eigen::VectorXd>(pair.first, pair.second));
return converter<std::string>::convert(helper);
}
static std::string convert(MatrixComposite val){
//TODO
// throw std::runtime_error("TODO! We still haven't got around to implement convert for MatrixComposite to String :(");
- auto helper = std::unordered_map< StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>();
+ auto helper = std::unordered_map< char, std::unordered_map<char, Eigen::MatrixXd>>();
for(const auto& pair: val)
- helper.insert(std::pair<StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>(pair.first, pair.second));
+ helper.insert(std::pair<char, std::unordered_map<char, Eigen::MatrixXd>>(pair.first, pair.second));
return converter<std::string>::convert(helper);
}
};
@@ -305,13 +318,13 @@ struct converter<std::unordered_map<A,B>>{
template<>
struct converter<VectorComposite>{
static VectorComposite convert(std::string val){
- auto unordered_map = converter<std::unordered_map<StateStructure, Eigen::VectorXd>>::convert(val);
+ auto unordered_map = converter<std::unordered_map<char, Eigen::VectorXd>>::convert(val);
// VectorComposite result = VectorComposite(unordered_map);
// return result;
auto helper = VectorComposite();
for(auto const& it: unordered_map)
{
- helper.insert(std::pair<StateStructure, Eigen::VectorXd>(it.first, it.second));
+ helper.insert(std::pair<char, Eigen::VectorXd>(it.first, it.second));
}
return helper;
}
@@ -319,14 +332,14 @@ struct converter<VectorComposite>{
template<>
struct converter<MatrixComposite>{
static MatrixComposite convert(std::string val){
- auto unordered_map = converter<std::unordered_map<StateStructure,
- std::unordered_map<StateStructure, Eigen::MatrixXd>>>::convert(val);
+ auto unordered_map = converter<std::unordered_map<char,
+ std::unordered_map<char, Eigen::MatrixXd>>>::convert(val);
// VectorComposite result = VectorComposite(unordered_map);
// return result;
auto helper = MatrixComposite();
for(auto const& it: unordered_map)
{
- helper.insert(std::pair<StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>(it.first, it.second));
+ helper.insert(std::pair<char, std::unordered_map<char, Eigen::MatrixXd>>(it.first, it.second));
}
return helper;
}
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 880af68b86919dbab088f7f702f07ac41feefb98..7e7f627e525cf5a0a080dafdb3ce1372a85c671b 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -23,26 +23,26 @@ CaptureBase::CaptureBase(const std::string& _type,
assert(time_stamp_.ok() && "Creating a capture with an invalid timestamp!");
if (_sensor_ptr)
{
- if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic("P"))
+ if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic('P'))
{
assert(_p_ptr && "Pointer to dynamic position params is null!");
- addStateBlock("P", _p_ptr, nullptr);
+ addStateBlock('P', _p_ptr, nullptr);
}
else
assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static or doesn't exist");
- if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic("O"))
+ if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic('O'))
{
assert(_o_ptr && "Pointer to dynamic orientation params is null!");
- addStateBlock("O", _o_ptr, nullptr);
+ addStateBlock('O', _o_ptr, nullptr);
}
else
assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist");
- if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic("I"))
+ if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic('I'))
{
assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
- addStateBlock("I", _intr_ptr, nullptr);
+ addStateBlock('I', _intr_ptr, nullptr);
}
else
assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist");
@@ -157,7 +157,7 @@ const StateStructure& CaptureBase::getStructure() const
return HasStateBlocks::getStructure();
}
-StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
+StateBlockPtr CaptureBase::getStateBlock(const char& _key) const
{
if (getSensor() and getSensor()->hasStateBlock(_key))
{
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 4147b98ca8dcd67d76fb732f11cbbee85cea9df8..a8f56d1a20302504ae0acad701a9fce7c86e91d9 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -28,7 +28,7 @@ FrameBase::FrameBase(const FrameType & _tp,
const auto& key = pair_key_vec.first;
const auto& vec = pair_key_vec.second;
- StateBlockPtr sb = FactoryStateBlock::create(key, vec, false); // false = non fixed
+ StateBlockPtr sb = FactoryStateBlock::create(string(1,key), vec, false); // false = non fixed
addStateBlock(key, sb);
}
@@ -49,12 +49,11 @@ FrameBase::FrameBase(const FrameType & _tp,
assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!");
auto vec_it = _vectors.begin();
- for (const auto ckey : _frame_structure)
+ for (const auto key : _frame_structure)
{
- const auto& key = string(1,ckey);
const auto& vec = *vec_it;
- StateBlockPtr sb = FactoryStateBlock::create(key, vec, false); // false = non fixed
+ StateBlockPtr sb = FactoryStateBlock::create(string(1,key), vec, false); // false = non fixed
addStateBlock(key, sb);
@@ -77,15 +76,15 @@ FrameBase::FrameBase(const FrameType & _tp,
{
if (_p_ptr)
{
- addStateBlock("P", _p_ptr);
+ addStateBlock('P', _p_ptr);
}
if (_o_ptr)
{
- addStateBlock("O", _o_ptr);
+ addStateBlock('O', _o_ptr);
}
if (_v_ptr)
{
- addStateBlock("V", _v_ptr);
+ addStateBlock('V', _v_ptr);
}
}
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 0fdd18775a2ea50c2ca6ee51ffaeaff9db7c85fa..af2d01b68e40c33b129f799d93fe3d133459671b 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -21,11 +21,11 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
{
if (_p_ptr)
{
- addStateBlock("P", _p_ptr, nullptr);
+ addStateBlock('P', _p_ptr, nullptr);
}
if (_o_ptr)
{
- addStateBlock("O", _o_ptr, nullptr);
+ addStateBlock('O', _o_ptr, nullptr);
}
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index b75b2eb0a6f961db7b9ceb217296f5cbf0f03530..9e84a825a30ffacf0ffb2e66fbb3670697901cd9 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -334,11 +334,9 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
VectorComposite state;
SizeEigen index = 0;
SizeEigen size = 0;
- for (const auto& ckey : _frame_structure)
+ for (const auto& key : _frame_structure)
{
- const auto& key = string(1,ckey); // ckey is char
-
- if (key == "O")
+ if (key == 'O')
{
if (_dim == 2) size = 1;
else size = 4;
@@ -471,9 +469,8 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
else if (prior_options_ and prior_options_->mode != "nothing")
state_last = prior_options_->state;
- for (const auto& ckey : structure)
+ for (const auto& key : structure)
{
- const auto& key = string(1,ckey);
if (state.count(key) == 0)
{
auto state_last_it = state_last.find(key);
@@ -482,7 +479,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
state.emplace(key, state_last_it->second);
else
- state.emplace(key, stateZero(key).at(key));
+ state.emplace(key, stateZero(string(1,key)).at(key));
}
}
@@ -521,9 +518,8 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
else if (prior_options_ and prior_options_->mode != "nothing")
state_last = prior_options_->state;
- for (const auto& ckey : structure)
+ for (const auto& key : structure)
{
- const auto& key = string(1,ckey);
if (state.count(key) == 0)
{
auto state_last_it = state_last.find(key);
@@ -532,7 +528,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
state.emplace(key, state_last_it->second);
else
- state.emplace(key, stateZero(key).at(key));
+ state.emplace(key, stateZero(string(1,key)).at(key));
}
}
@@ -551,9 +547,9 @@ const StateStructure& Problem::getFrameStructure() const
void Problem::appendToStructure(const StateStructure& _structure)
{
- for (const auto& ckey : _structure)
- if (frame_structure_.find(ckey) == std::string::npos) // now key not found in problem structure -> append!
- frame_structure_ += ckey;
+ for (const auto& key : _structure)
+ if (frame_structure_.find(key) == std::string::npos) // now key not found in problem structure -> append!
+ frame_structure_ += key;
}
void Problem::setTreeManager(TreeManagerBasePtr _gm)
@@ -589,11 +585,10 @@ VectorComposite Problem::stateZero ( const StateStructure& _structure ) const
StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
VectorComposite state;
- for (const auto& ckey : structure)
+ for (const auto& key : structure)
{
- const auto& key = string(1,ckey);
VectorXd vec;
- if (key == "O")
+ if (key == 'O')
{
if (dim_ == 2) vec = VectorXd::Zero(1);
else if (dim_ == 3) vec = Quaterniond::Identity().coeffs();
@@ -1067,7 +1062,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
assert(sb->getLocalSize() == covar_blk.rows() && "prior_options cov. wrong size");
// feature
- auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "Prior " + key, state_blk, covar_blk);
+ auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "Prior " + string(1,key), state_blk, covar_blk);
// factor
if (sb->hasLocalParametrization())
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 284090e238f3c75327424cb76f3946e277e23bdf..2da7c330b59ffce0c7eceb11515e4f992a9eb3d9 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -93,8 +93,8 @@ void ProcessorOdom2d::statePlusDelta(const VectorComposite& _x,
assert(_delta.size() == delta_size_ && "Wrong _x_plus_delta vector size");
// This is just a frame composition in 2d
- _x_plus_delta["P"] = _x.at("P") + Eigen::Rotation2Dd(_x.at("O")(0)).matrix() * _delta.head<2>();
- _x_plus_delta["O"] = Vector1d(pi2pi(_x.at("O")(0) + _delta(2)));
+ _x_plus_delta['P'] = _x.at('P') + Eigen::Rotation2Dd(_x.at('O')(0)).matrix() * _delta.head<2>();
+ _x_plus_delta['O'] = Vector1d(pi2pi(_x.at('O')(0) + _delta(2)));
}
bool ProcessorOdom2d::voteForKeyFrame() const
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 5b01e98b3ad8fbbe6f31bedc4f25b1a9cfe76747..d6f7e77a9562e8762f9ab99c9ac525b8fe592c54 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -147,13 +147,13 @@ void ProcessorOdom3d::statePlusDelta(const VectorComposite& _x,
{
assert(_delta.size() == delta_size_ && "Wrong _delta vector size");
- Eigen::Map<const Eigen::Vector3d> p (_x.at("P").data() );
- Eigen::Map<const Eigen::Quaterniond> q (_x.at("O").data() );
+ Eigen::Map<const Eigen::Vector3d> p (_x.at('P').data() );
+ Eigen::Map<const Eigen::Quaterniond> q (_x.at('O').data() );
Eigen::Map<const Eigen::Vector3d> dp (_delta.data() );
Eigen::Map<const Eigen::Quaterniond> dq (_delta.data() + 3 );
- _x_plus_delta["P"] = p + q * dp;
- _x_plus_delta["O"] = (q * dq).coeffs();
+ _x_plus_delta['P'] = p + q * dp;
+ _x_plus_delta['O'] = (q * dq).coeffs();
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 7d15881a6aea62a9a01591ac5bede38d1b3c6342..b32f02f3dca55c584567b8d84e220a2f2604114b 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -33,13 +33,13 @@ SensorBase::SensorBase(const std::string& _type,
noise_cov_.setZero();
if (_p_ptr)
- addStateBlock("P", _p_ptr, _p_dyn);
+ addStateBlock('P', _p_ptr, _p_dyn);
if (_o_ptr)
- addStateBlock("O", _o_ptr, _o_dyn);
+ addStateBlock('O', _o_ptr, _o_dyn);
if (_intr_ptr)
- addStateBlock("I", _intr_ptr, _intr_dyn);
+ addStateBlock('I', _intr_ptr, _intr_dyn);
updateCalibSize();
}
@@ -63,13 +63,13 @@ SensorBase::SensorBase(const std::string& _type,
setNoiseStd(_noise_std);
if (_p_ptr)
- addStateBlock("P", _p_ptr, _p_dyn);
+ addStateBlock('P', _p_ptr, _p_dyn);
if (_o_ptr)
- addStateBlock("O", _o_ptr, _o_dyn);
+ addStateBlock('O', _o_ptr, _o_dyn);
if (_intr_ptr)
- addStateBlock("I", _intr_ptr, _intr_dyn);
+ addStateBlock('I', _intr_ptr, _intr_dyn);
updateCalibSize();
}
@@ -82,9 +82,8 @@ SensorBase::~SensorBase()
void SensorBase::removeStateBlocks()
{
- for (const auto& _key : getStructure())
+ for (const auto& key : getStructure())
{
- auto key = std::string(1,_key);
auto sbp = getStateBlock(key);
if (sbp != nullptr)
{
@@ -121,10 +120,9 @@ void SensorBase::unfixExtrinsics()
void SensorBase::fixIntrinsics()
{
- for (const auto& _key : getStructure())
+ for (const auto& key : getStructure())
{
- auto key = std::string(1,_key);
- if (key != "P" and key != "O") // exclude extrinsics
+ if (key != 'P' and key != 'O') // exclude extrinsics
{
auto sbp = getStateBlockDynamic(key);
if (sbp != nullptr)
@@ -136,10 +134,9 @@ void SensorBase::fixIntrinsics()
void SensorBase::unfixIntrinsics()
{
- for (const auto& _key : getStructure())
+ for (const auto& key : getStructure())
{
- auto key = std::string(1,_key);
- if (key != "P" and key != "O") // exclude extrinsics
+ if (key != 'P' and key != 'O') // exclude extrinsics
{
auto sbp = getStateBlockDynamic(key);
if (sbp != nullptr)
@@ -149,7 +146,7 @@ void SensorBase::unfixIntrinsics()
updateCalibSize();
}
-void SensorBase::addPriorParameter(const std::string& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
+void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
{
assert(!isStateBlockInCapture(_key) && "SensorBase::addPriorParameter only allowed for static parameters");
@@ -287,32 +284,32 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts) const
StateBlockPtr SensorBase::getP(const TimeStamp _ts) const
{
- return getStateBlockDynamic("P", _ts);
+ return getStateBlockDynamic('P', _ts);
}
StateBlockPtr SensorBase::getO(const TimeStamp _ts) const
{
- return getStateBlockDynamic("O", _ts);
+ return getStateBlockDynamic('O', _ts);
}
StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts) const
{
- return getStateBlockDynamic("I", _ts);
+ return getStateBlockDynamic('I', _ts);
}
StateBlockPtr SensorBase::getP() const
{
- return getStateBlockDynamic("P");
+ return getStateBlockDynamic('P');
}
StateBlockPtr SensorBase::getO() const
{
- return getStateBlockDynamic("O");
+ return getStateBlockDynamic('O');
}
StateBlockPtr SensorBase::getIntrinsic() const
{
- return getStateBlockDynamic("I");
+ return getStateBlockDynamic('I');
}
SizeEigen SensorBase::computeCalibSize() const
@@ -382,7 +379,7 @@ void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr)
processor_list_.remove(_proc_ptr);
}
-StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key) const
+StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key) const
{
CaptureBasePtr cap;
@@ -392,7 +389,7 @@ StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key) const
return getStateBlock(_key);
}
-StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key, const TimeStamp& _ts) const
+StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const
{
CaptureBasePtr cap;
@@ -402,7 +399,7 @@ StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key, const Ti
return getStateBlock(_key);
}
-bool SensorBase::isStateBlockInCapture(const std::string& _key, CaptureBasePtr& _cap) const
+bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) const
{
if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
{
@@ -415,7 +412,7 @@ bool SensorBase::isStateBlockInCapture(const std::string& _key, CaptureBasePtr&
return false;
}
-bool SensorBase::isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const
+bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const
{
if (isStateBlockDynamic(_key))
{
@@ -427,14 +424,14 @@ bool SensorBase::isStateBlockInCapture(const std::string& _key, const TimeStamp&
return false;
}
-bool SensorBase::isStateBlockInCapture(const std::string& _key) const
+bool SensorBase::isStateBlockInCapture(const char& _key) const
{
CaptureBasePtr cap;
return isStateBlockInCapture(_key, cap);
}
-bool SensorBase::isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts) const
+bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts) const
{
CaptureBasePtr cap;
@@ -477,9 +474,8 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
if (_metric && _state_blocks)
{
_stream << _tabs << " " << "sb: ";
- for (auto& _key : getStructure())
+ for (auto& key : getStructure())
{
- auto key = std::string(1,_key);
auto sb = getStateBlockDynamic(key);
_stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
}
@@ -488,9 +484,8 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
else if (_metric)
{
_stream << _tabs << " " << "( ";
- for (auto& _key : getStructure())
+ for (auto& key : getStructure())
{
- auto key = std::string(1,_key);
auto sb = getStateBlockDynamic(key);
_stream << sb->getState().transpose() << " ";
}
@@ -499,9 +494,8 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
else if (_state_blocks)
{
_stream << _tabs << " " << "sb: ";
- for (auto& _key : getStructure())
+ for (auto& key : getStructure())
{
- auto key = std::string(1,_key);
auto sb = getStateBlockDynamic(key);
_stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
}
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 83cf44b26bdfcad7e415633ef6d8fe783564a75c..5855fda6b33931665fb9240b715a7436ac1ed034 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -4,7 +4,7 @@
namespace wolf
{
-StateBlockPtr HasStateBlocks::addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
+StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
{
assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
state_block_map_.emplace(_sb_type, _sb);
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index 90b7f47c6b5ffa2a28374b5673827d325224430f..0f4efab6f1f093d8f1425d0855a453c6370f1c93 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -11,9 +11,8 @@ namespace wolf{
VectorComposite::VectorComposite(const StateStructure& _structure, const std::list<int>& _sizes)
{
auto size_it = _sizes.begin();
- for ( const auto& ckey : _structure)
+ for ( const auto& key : _structure)
{
- const auto& key = string(1,ckey);
const auto& size = *size_it;
this->emplace(key, VectorXd(size).setZero());
@@ -26,9 +25,8 @@ VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _stru
{
int index = 0;
auto size_it = _sizes.begin();
- for ( const auto& ckey : _structure)
+ for ( const auto& key : _structure)
{
- const auto& key = string(1,ckey);
const auto& size = *size_it;
(*this)[key] = _v.segment(index,size);
@@ -40,9 +38,8 @@ VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _stru
VectorComposite::VectorComposite (const StateStructure& _s)
{
- for (const auto& ckey : _s)
+ for (const auto& key : _s)
{
- const auto& key = string(1,ckey); // ckey is char
this->emplace(key,VectorXd(0));
}
}
@@ -53,9 +50,8 @@ VectorComposite::VectorComposite (const StateStructure& _structure, const std::l
auto vector_it = _vectors.begin();
- for (const auto& ckey : _structure)
+ for (const auto& key : _structure)
{
- string key(1,ckey);
this->emplace(key, *vector_it);
vector_it++;
}
@@ -67,9 +63,8 @@ Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const
// traverse once with unordered_map access
std::vector<const VectorXd*> vp;
unsigned int size = 0;
- for (const auto& ckey : _structure)
+ for (const auto& key : _structure)
{
- std::string key(1,ckey); // ckey is char
vp.push_back(&(this->at(key)));
size += vp.back()->size();
}
@@ -125,9 +120,8 @@ void VectorComposite::set (const VectorXd& _v, const StateStructure& _structure,
{
int index = 0;
auto size_it = _sizes.begin();
- for ( const auto& ckey : _structure)
+ for ( const auto& key : _structure)
{
- const auto& key = string(1,ckey); // ckey is char
const auto& size = *size_it;
(*this)[key] = _v.segment(index,size);
@@ -156,7 +150,7 @@ wolf::VectorComposite operator -(const wolf::VectorComposite &_x)
////// MATRIX COMPOSITE //////////
-bool MatrixComposite::emplace(const std::string &_row, const std::string &_col, const Eigen::MatrixXd &_mat_blk)
+bool MatrixComposite::emplace(const char &_row, const char &_col, const Eigen::MatrixXd &_mat_blk)
{
// check rows
if (size_rows_.count(_row) == 0)
@@ -175,7 +169,7 @@ bool MatrixComposite::emplace(const std::string &_row, const std::string &_col,
return true;
}
-bool MatrixComposite::at(const std::string &_row, const std::string &_col, Eigen::MatrixXd &_mat_blk) const
+bool MatrixComposite::at(const char &_row, const char &_col, Eigen::MatrixXd &_mat_blk) const
{
const auto &row_it = this->find(_row);
if(row_it != this->end())
@@ -190,7 +184,7 @@ bool MatrixComposite::at(const std::string &_row, const std::string &_col, Eigen
return true;
}
-Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string &_col)
+Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col)
{
const auto &row_it = this->find(_row);
assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
@@ -201,7 +195,7 @@ Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string
return col_it->second;
}
-const Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string &_col) const
+const Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col) const
{
const auto &row_it = this->find(_row);
assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
@@ -287,7 +281,7 @@ MatrixComposite MatrixComposite::operator - (void) const
for (const auto& pair_rkey_row : *this)
{
- m.unordered_map<string,unordered_map<string, MatrixXd>>::emplace(pair_rkey_row.first, unordered_map<string, MatrixXd>());
+ m.unordered_map<char,unordered_map<char, MatrixXd>>::emplace(pair_rkey_row.first, unordered_map<char, MatrixXd>());
for (const auto& pair_ckey_blk : pair_rkey_row.second)
{
m[pair_rkey_row.first].emplace(pair_ckey_blk.first, - pair_ckey_blk.second);
@@ -449,8 +443,8 @@ MatrixComposite operator * (double scalar_left, const MatrixComposite& M)
MatrixXd MatrixComposite::matrix(const StateStructure &_struct_rows, const StateStructure &_struct_cols) const
{
- std::unordered_map < string, unsigned int> indices_rows;
- std::unordered_map < string, unsigned int> indices_cols;
+ std::unordered_map < char, unsigned int> indices_rows;
+ std::unordered_map < char, unsigned int> indices_cols;
unsigned int rows, cols;
sizeAndIndices(_struct_rows, _struct_cols, indices_rows, indices_cols, rows, cols);
@@ -498,22 +492,20 @@ unsigned int MatrixComposite::cols() const
void MatrixComposite::sizeAndIndices(const StateStructure &_struct_rows,
const StateStructure &_struct_cols,
- std::unordered_map<string, unsigned int> &_indices_rows,
- std::unordered_map<string, unsigned int> &_indices_cols,
+ std::unordered_map<char, unsigned int> &_indices_rows,
+ std::unordered_map<char, unsigned int> &_indices_cols,
unsigned int &_rows,
unsigned int &_cols) const
{
_rows = 0;
_cols = 0;
- for (const auto& crow : _struct_rows)
+ for (const auto& row : _struct_rows)
{
- string row = string(1,crow); // crow is char
_indices_rows[row] = _rows;
_rows += size_rows_.at(row);
}
- for (const auto& ccol : _struct_cols)
+ for (const auto& col : _struct_cols)
{
- string col = string(1,ccol); // ccol is char
_indices_cols[col] = _cols;
_cols += size_cols_.at(col);
}
@@ -521,16 +513,9 @@ void MatrixComposite::sizeAndIndices(const StateStructure &_struct_rows,
MatrixComposite::MatrixComposite (const StateStructure& _row_structure, const StateStructure& _col_structure)
{
- for (const auto& rkey_char : _row_structure)
- {
- string rkey(1,rkey_char); // key was char
- for (const auto& ckey_char : _col_structure)
- {
- string ckey(1,ckey_char); // key was char
-
+ for (const auto& rkey : _row_structure)
+ for (const auto& ckey : _col_structure)
this->emplace(rkey, ckey, MatrixXd(0,0));
- }
- }
}
MatrixComposite::MatrixComposite (const StateStructure& _row_structure,
@@ -542,15 +527,11 @@ MatrixComposite::MatrixComposite (const StateStructure& _row_structure,
assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
auto rsize_it = _row_sizes.begin();
- for (const auto& rkey_char : _row_structure)
+ for (const auto& rkey : _row_structure)
{
- string rkey(1,rkey_char); // key was char
-
auto csize_it = _col_sizes.begin();
- for (const auto& ckey_char : _col_structure)
+ for (const auto& ckey : _col_structure)
{
- string ckey(1,ckey_char); // key was char
-
this->emplace(rkey, ckey, MatrixXd(*rsize_it, *csize_it)); // caution: blocks non initialized.
csize_it ++;
@@ -570,21 +551,17 @@ MatrixComposite::MatrixComposite (const MatrixXd& _m,
SizeEigen rindex = 0, cindex;
auto rsize_it = _row_sizes.begin();
- for (const auto& rkey_char : _row_structure)
+ for (const auto& rkey : _row_structure)
{
assert(_m.rows() >= rindex + *rsize_it && "Provided matrix has insufficient number of rows");
- string rkey(1,rkey_char); // key was char
-
cindex = 0;
auto csize_it = _col_sizes.begin();
- for (const auto& ckey_char : _col_structure)
+ for (const auto& ckey : _col_structure)
{
assert(_m.cols() >= cindex + *csize_it && "Provided matrix has insufficient number of columns");
- string ckey(1,ckey_char); // key was char
-
this->emplace(rkey, ckey, _m.block(rindex, cindex, *rsize_it, *csize_it));
cindex += *csize_it;
@@ -609,15 +586,11 @@ MatrixComposite MatrixComposite::zero (const StateStructure& _row_structure, con
assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
auto rsize_it = _row_sizes.begin();
- for (const auto& rkey_char : _row_structure)
+ for (const auto& rkey : _row_structure)
{
- string rkey(1,rkey_char); // key was char
-
auto csize_it = _col_sizes.begin();
- for (const auto& ckey_char : _col_structure)
+ for (const auto& ckey : _col_structure)
{
- string ckey(1,ckey_char); // key was char
-
m.emplace(rkey, ckey, MatrixXd::Zero(*rsize_it, *csize_it));
csize_it ++;
@@ -639,7 +612,7 @@ MatrixComposite MatrixComposite::identity (const StateStructure& _structure, con
while (rkey_it != _structure.end())
{
- const auto& rkey = string(1,*rkey_it);
+ const auto& rkey = *rkey_it;
const auto rsize = *rsize_it;
m.emplace(rkey, rkey, MatrixXd::Identity(rsize,rsize)); // diagonal block
@@ -652,7 +625,7 @@ MatrixComposite MatrixComposite::identity (const StateStructure& _structure, con
while (ckey_it != _structure.end())
{
- const auto& ckey = string(1,*ckey_it);
+ const auto& ckey = *ckey_it;
const auto& csize = *csize_it;
m.emplace(rkey, ckey, MatrixXd::Zero(rsize, csize)); // above-diagonal block
@@ -763,7 +736,7 @@ bool StateBlockComposite::isFixed() const
return fixed;
}
-unsigned int StateBlockComposite::remove(const std::string &_sb_type)
+unsigned int StateBlockComposite::remove(const char &_sb_type)
{
return state_block_map_.erase(_sb_type);
}
@@ -778,7 +751,7 @@ bool StateBlockComposite::has(const StateBlockPtr &_sb) const
return found;
}
-StateBlockPtr StateBlockComposite::at(const std::string &_sb_type) const
+StateBlockPtr StateBlockComposite::at(const char &_sb_type) const
{
auto it = state_block_map_.find(_sb_type);
if (it != state_block_map_.end())
@@ -787,7 +760,7 @@ StateBlockPtr StateBlockComposite::at(const std::string &_sb_type) const
return nullptr;
}
-void StateBlockComposite::set(const std::string& _sb_type, const StateBlockPtr &_sb)
+void StateBlockComposite::set(const char& _sb_type, const StateBlockPtr &_sb)
{
auto it = state_block_map_.find(_sb_type);
assert ( it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead.");
@@ -795,7 +768,7 @@ void StateBlockComposite::set(const std::string& _sb_type, const StateBlockPtr &
it->second = _sb;
}
-void StateBlockComposite::set(const std::string& _key, const VectorXd &_vec)
+void StateBlockComposite::set(const char& _key, const VectorXd &_vec)
{
auto it = state_block_map_.find(_key);
assert ( it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead.");
@@ -810,15 +783,14 @@ void StateBlockComposite::setVectors(const StateStructure& _structure, const std
assert(_structure.size() == _vectors.size() && "Sizes of structure and vector list do not match");
auto vec_it = _vectors.begin();
- for (const auto ckey : _structure)
+ for (const auto key : _structure)
{
- string key(1,ckey);
set (key, *vec_it);
vec_it++;
}
}
-bool StateBlockComposite::key(const StateBlockPtr &_sb, std::string &_key) const
+bool StateBlockComposite::key(const StateBlockPtr &_sb, char &_key) const
{
const auto& it = this->find(_sb);
@@ -831,12 +803,12 @@ bool StateBlockComposite::key(const StateBlockPtr &_sb, std::string &_key) const
}
else
{
- _key = "";
+ _key ='0';
return false;
}
}
-std::string StateBlockComposite::key(const StateBlockPtr& _sb) const
+char StateBlockComposite::key(const StateBlockPtr& _sb) const
{
const auto& it = this->find(_sb);
@@ -845,7 +817,7 @@ std::string StateBlockComposite::key(const StateBlockPtr& _sb) const
if (found)
return it->first;
else
- return "";
+ return '0';
}
@@ -853,7 +825,7 @@ StateBlockMapCIter StateBlockComposite::find(const StateBlockPtr &_sb) const
{
const auto& it = std::find_if(state_block_map_.begin(),
state_block_map_.end(),
- [_sb](const std::pair<std::string, StateBlockPtr>& pair)
+ [_sb](const std::pair<char, StateBlockPtr>& pair)
{
return pair.second == _sb;
}
@@ -862,7 +834,7 @@ StateBlockMapCIter StateBlockComposite::find(const StateBlockPtr &_sb) const
return it;
}
-StateBlockPtr StateBlockComposite::add(const std::string &_sb_type, const StateBlockPtr &_sb)
+StateBlockPtr StateBlockComposite::add(const char &_sb_type, const StateBlockPtr &_sb)
{
assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
@@ -955,9 +927,8 @@ SizeEigen StateBlockComposite::stateSize() const
SizeEigen StateBlockComposite::stateSize(const StateStructure &_structure) const
{
SizeEigen size = 0;
- for (const auto& ckey : _structure)
+ for (const auto& key : _structure)
{
- string key(1,ckey);
size += state_block_map_.at(key)->getSize();
}
return size;
@@ -968,9 +939,8 @@ VectorXd StateBlockComposite::stateVector(const StateStructure &_structure) cons
VectorXd x(stateSize(_structure));
SizeEigen index = 0;
SizeEigen size;
- for (const auto& ckey : _structure)
+ for (const auto& key : _structure)
{
- string key(1,ckey);
const auto& sb = state_block_map_.at(key);
size = sb->getSize();
x.segment(index,size) = sb->getState();
@@ -984,9 +954,8 @@ void StateBlockComposite::stateVector(const StateStructure &_structure, VectorXd
_vector.resize(stateSize(_structure));
SizeEigen index = 0;
SizeEigen size;
- for (const auto& ckey : _structure)
+ for (const auto& key : _structure)
{
- string key(1,ckey);
const auto& sb = state_block_map_.at(key);
size = sb->getSize();
_vector.segment(index,size) = sb->getState();
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index 27acfea3069b92076bd30e4abd4ac28d787147bf..a99f7a19551f3e450e6b4958ded294db8980c62c 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -146,15 +146,15 @@ TEST(SE3, composeVectorComposite)
VectorComposite x1, x2, xc("PO", {3,4});
- x1.emplace("P", p1);
- x1.emplace("O", q1.coeffs());
- x2.emplace("P", p2);
- x2.emplace("O", q2.coeffs());
+ x1.emplace('P', p1);
+ x1.emplace('O', q1.coeffs());
+ x2.emplace('P', p2);
+ x2.emplace('O', q2.coeffs());
compose(x1, x2, xc);
- ASSERT_MATRIX_APPROX(xc.at("P"), pc, 1e-8);
- ASSERT_MATRIX_APPROX(xc.at("O"), qc.coeffs(), 1e-8);
+ ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8);
+ ASSERT_MATRIX_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
}
TEST(SE3, composeVectorComposite_Jacobians)
@@ -174,26 +174,26 @@ TEST(SE3, composeVectorComposite_Jacobians)
VectorComposite x1, x2, xc("PO", {3,4});
MatrixComposite J_xc_x1, J_xc_x2;
- x1.emplace("P", p1);
- x1.emplace("O", q1.coeffs());
- x2.emplace("P", p2);
- x2.emplace("O", q2.coeffs());
+ x1.emplace('P', p1);
+ x1.emplace('O', q1.coeffs());
+ x2.emplace('P', p2);
+ x2.emplace('O', q2.coeffs());
// we'll do xc = x1 * x2 and obtain Jacobians
compose(x1, x2, xc, J_xc_x1, J_xc_x2);
- ASSERT_MATRIX_APPROX(xc.at("P"), pc, 1e-8);
- ASSERT_MATRIX_APPROX(xc.at("O"), qc.coeffs(), 1e-8);
+ ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8);
+ ASSERT_MATRIX_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x1.at("P", "P"), Matrix3d::Identity() , 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x1.at("P", "O"), - R1 * skew(p2) , 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x1.at("O", "P"), Matrix3d::Zero() , 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x1.at("O", "O"), R2.transpose() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'P'), Matrix3d::Identity() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'O'), - R1 * skew(p2) , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'P'), Matrix3d::Zero() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'O'), R2.transpose() , 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x2.at("P", "P"), R1 , 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x2.at("P", "O"), Matrix3d::Zero() , 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x2.at("O", "P"), Matrix3d::Zero() , 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x2.at("O", "O"), Matrix3d::Identity() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'P'), R1 , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'O'), Matrix3d::Zero() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'P'), Matrix3d::Zero() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'O'), Matrix3d::Identity() , 1e-8);
}
TEST(SE3, exp_0_Composite)
@@ -205,13 +205,13 @@ TEST(SE3, exp_0_Composite)
VectorComposite tau;
- tau.emplace("P", p);
- tau.emplace("O", theta);
+ tau.emplace('P', p);
+ tau.emplace('O', theta);
VectorComposite x = SE3::exp(tau);
- ASSERT_MATRIX_APPROX(x.at("P"), Vector3d::Zero(), 1e-8);
- ASSERT_MATRIX_APPROX(x.at("O"), Quaterniond::Identity().coeffs(), 1e-8);
+ ASSERT_MATRIX_APPROX(x.at('P'), Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(x.at('O'), Quaterniond::Identity().coeffs(), 1e-8);
}
@@ -223,20 +223,20 @@ TEST(SE3, plus_0_Composite)
Vector4d q; q.setRandom().normalized();
VectorComposite x;
- x.emplace("P", p);
- x.emplace("O", q);
+ x.emplace('P', p);
+ x.emplace('O', q);
Vector3d dp; dp.setZero();
Vector3d dtheta; dtheta.setZero();
VectorComposite tau;
- tau.emplace("P", dp);
- tau.emplace("O", dtheta);
+ tau.emplace('P', dp);
+ tau.emplace('O', dtheta);
VectorComposite res = plus(x, tau);
- ASSERT_MATRIX_APPROX(res.at("P"), p, 1e-8);
- ASSERT_MATRIX_APPROX(res.at("O"), q, 1e-8);
+ ASSERT_MATRIX_APPROX(res.at('P'), p, 1e-8);
+ ASSERT_MATRIX_APPROX(res.at('O'), q, 1e-8);
}
TEST(SE3, interpolate_I_xyz)
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index abdefc83d7d4a3e1f041f5355b3139c1276775cb..8f73d96ac2560d6366410b48da41e2a3ef3fe00b 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -163,7 +163,7 @@ TEST(FrameBase, Constructor_composite)
ASSERT_TRUE (F.isKey());
ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
ASSERT_TRUE (F.getO()->hasLocalParametrization());
- ASSERT_EQ (F.getStateBlock("V")->getSize(), 3);
+ ASSERT_EQ (F.getStateBlock('V')->getSize(), 3);
}
TEST(FrameBase, Constructor_vectors)
@@ -176,7 +176,7 @@ TEST(FrameBase, Constructor_vectors)
ASSERT_TRUE (F.isKey());
ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
ASSERT_TRUE (F.getO()->hasLocalParametrization());
- ASSERT_EQ (F.getStateBlock("V")->getSize(), 3);
+ ASSERT_EQ (F.getStateBlock('V')->getSize(), 3);
}
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index 334aa59be910d88c2994c37dd324bad360b265b8..e17124a2d56687d8fe8de4c8d1144e49285e1f86 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -79,7 +79,7 @@ TEST_F(HasStateBlocksTest, Notifications_add_makeKF)
ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
- F0->addStateBlock("V", sbv0);
+ F0->addStateBlock('V', sbv0);
ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
@@ -102,7 +102,7 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
F1->link(problem->getTrajectory());
F1->setKey();
- F1->addStateBlock("P", sbp1);
+ F1->addStateBlock('P', sbp1);
ASSERT_TRUE(problem->getStateBlockNotification(sbp1, n));
ASSERT_EQ(n, ADD);
@@ -111,7 +111,7 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
ASSERT_FALSE(problem->getStateBlockNotification(sbv1, n));
- F1->addStateBlock("V", sbv1);
+ F1->addStateBlock('V', sbv1);
ASSERT_TRUE(problem->getStateBlockNotification(sbv1, n));
ASSERT_EQ(n, ADD);
@@ -131,7 +131,7 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
F0->link(problem->getTrajectory());
- F0->addStateBlock("V", sbv0);
+ F0->addStateBlock('V', sbv0);
F0->setKey();
ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
@@ -158,7 +158,7 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
// Add again the same SB. This should crash
- ASSERT_DEATH( F0->addStateBlock("V", sbv0) , "" );
+ ASSERT_DEATH( F0->addStateBlock('V', sbv0) , "" );
}
@@ -170,48 +170,48 @@ TEST_F(HasStateBlocksTest, hasStateBlock)
TEST_F(HasStateBlocksTest, stateBlockKey)
{
- std::string key;
+ char key;
ASSERT_TRUE(F0->stateBlockKey(sbp0, key));
- ASSERT_TRUE(key == "P");
+ ASSERT_TRUE(key == 'P');
ASSERT_FALSE(F0->stateBlockKey(sbp1, key));
- ASSERT_TRUE(key == "");
+ ASSERT_TRUE(key == '0');
}
TEST_F(HasStateBlocksTest, getState_structure)
{
- F0->addStateBlock("V", sbv0); // now KF0 is POV
+ F0->addStateBlock('V', sbv0); // now KF0 is POV
WOLF_DEBUG("Retrieving state from F0 with structure ", F0->getStructure());
auto state0 = F0->getState();
WOLF_DEBUG("getState() = ", state0);
ASSERT_EQ(state0.size(), 3);
- ASSERT_TRUE(state0.count("P"));
- ASSERT_TRUE(state0.count("O"));
- ASSERT_TRUE(state0.count("V"));
+ ASSERT_TRUE(state0.count('P'));
+ ASSERT_TRUE(state0.count('O'));
+ ASSERT_TRUE(state0.count('V'));
state0 = F0->getState("PO");
WOLF_DEBUG("getState(\"PO\") = ", state0);
ASSERT_EQ(state0.size(), 2);
- ASSERT_TRUE(state0.count("P"));
- ASSERT_TRUE(state0.count("O"));
- ASSERT_FALSE(state0.count("V"));
+ ASSERT_TRUE(state0.count('P'));
+ ASSERT_TRUE(state0.count('O'));
+ ASSERT_FALSE(state0.count('V'));
state0 = F0->getState("PV");
WOLF_DEBUG("getState(\"PV\") = ", state0);
ASSERT_EQ(state0.size(), 2);
- ASSERT_TRUE(state0.count("P"));
- ASSERT_FALSE(state0.count("O"));
- ASSERT_TRUE(state0.count("V"));
+ ASSERT_TRUE(state0.count('P'));
+ ASSERT_FALSE(state0.count('O'));
+ ASSERT_TRUE(state0.count('V'));
state0 = F0->getState("OW"); // W does not exist
WOLF_DEBUG("getState(\"OW\") = ", state0);
ASSERT_EQ(state0.size(), 1);
- ASSERT_FALSE(state0.count("P"));
- ASSERT_TRUE(state0.count("O"));
- ASSERT_FALSE(state0.count("V"));
- ASSERT_FALSE(state0.count("W"));
+ ASSERT_FALSE(state0.count('P'));
+ ASSERT_TRUE(state0.count('O'));
+ ASSERT_FALSE(state0.count('V'));
+ ASSERT_FALSE(state0.count('W'));
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index 5d0d2cbfb209e6acf4396cbf5dad70fddac11285..8faac803c5ffe7e5780557fbc08155e43c3ca587 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -72,7 +72,7 @@ TEST(ParameterPrior, prior_p_segment)
TEST(ParameterPrior, prior_o_segment)
{
// constraining segment of quaternion is not allowed
- ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter("O", prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2),"");
+ ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter('O', prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2),"");
}
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 1470052b849465705eb860b34600d8d624dfb61c..210fdbee0df5a8e4b6f3e189a28b4fe6e841b9fd 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -349,7 +349,7 @@ TEST(Problem, perturb)
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);
+ sensor->setStateBlockDynamic('I', true);
Vector3d pose; pose << 0,0,0;
@@ -439,7 +439,7 @@ TEST(Problem, check)
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);
+ sensor->setStateBlockDynamic('I', true);
Vector3d pose; pose << 0,0,0;
@@ -554,6 +554,9 @@ TEST(Problem, getState)
WOLF_DEBUG("P () = ", P->getState("P"));
WOLF_DEBUG("PO() = ", P->getState("PO"));
WOLF_DEBUG("x () = ", P->getState());
+ ASSERT_EQ(P->getState("P").size(), 1);
+ ASSERT_EQ(P->getState("PO").size(), 2);
+ ASSERT_EQ(P->getState().size(), 2);
}
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 85332d277caff7667ac27e76d18be4c1b06ab6a2..527aba1030ecc18a034b888d1ca02d75ad1408aa 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -120,12 +120,12 @@ TEST_F(ProcessorMotion_test, getState_structure)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_TRUE (processor->getState("P").count("P"));
- ASSERT_FALSE(processor->getState("P").count("O"));
- ASSERT_FALSE(processor->getState("O").count("P"));
- ASSERT_TRUE (processor->getState("O").count("O"));
+ ASSERT_TRUE (processor->getState("P").count('P'));
+ ASSERT_FALSE(processor->getState("P").count('O'));
+ ASSERT_FALSE(processor->getState("O").count('P'));
+ ASSERT_TRUE (processor->getState("O").count('O'));
- WOLF_DEBUG("processor->getState(\"V\") = ", processor->getState("V"));
+ WOLF_DEBUG("processor->getState(\"V\") = ", processor->getState('V'));
ASSERT_EQ (processor->getState("V").size(), 0);
}
@@ -150,10 +150,10 @@ TEST_F(ProcessorMotion_test, getState_time_structure)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_TRUE (processor->getState(7, "P").count("P"));
- ASSERT_FALSE(processor->getState(7, "P").count("O"));
- ASSERT_FALSE(processor->getState(7, "O").count("P"));
- ASSERT_TRUE (processor->getState(7, "O").count("O"));
+ ASSERT_TRUE (processor->getState(7, "P").count('P'));
+ ASSERT_FALSE(processor->getState(7, "P").count('O'));
+ ASSERT_FALSE(processor->getState(7, "O").count('P'));
+ ASSERT_TRUE (processor->getState(7, "O").count('O'));
WOLF_DEBUG("processor->getState(7, \"V\") = ", processor->getState(7, "V"));
ASSERT_EQ (processor->getState(7, "V").size(), 0);
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
index 56f705aa3b9c3689a076989fcd8e30896d80fe13..f4bcea67bd4e51af793e026f1496f2591fcfe98a 100644
--- a/test/gtest_state_composite.cpp
+++ b/test/gtest_state_composite.cpp
@@ -25,9 +25,9 @@ class StateBlockCompositeInit : public testing::Test
void SetUp() override
{
- sbp = states.emplace("P", Vector3d(1,2,3));
- sbv = states.emplace("V", Vector3d(4,5,6));
- sbq = states.emplace<StateQuaternion>("Q", Vector4d(.5,.5,.5,.5));
+ sbp = states.emplace('P', Vector3d(1,2,3));
+ sbv = states.emplace('V', Vector3d(4,5,6));
+ sbq = states.emplace<StateQuaternion>('Q', Vector4d(.5,.5,.5,.5));
sbx = std::make_shared<StateBlock>(Vector3d(7,8,9));
}
@@ -57,8 +57,8 @@ TEST_F(StateBlockCompositeInit, emplace)
TEST_F(StateBlockCompositeInit, has_key)
{
- ASSERT_TRUE(states.has("P"));
- ASSERT_FALSE(states.has("X"));
+ ASSERT_TRUE(states.has('P'));
+ ASSERT_FALSE(states.has('X'));
}
TEST_F(StateBlockCompositeInit, has_sb)
@@ -69,22 +69,22 @@ TEST_F(StateBlockCompositeInit, has_sb)
TEST_F(StateBlockCompositeInit, at)
{
- ASSERT_EQ(states.at("P"), sbp);
+ ASSERT_EQ(states.at('P'), sbp);
- ASSERT_EQ(states.at("X"), nullptr);
+ ASSERT_EQ(states.at('X'), nullptr);
}
TEST_F(StateBlockCompositeInit, set_sb)
{
- states.set("P", sbx);
+ states.set('P', sbx);
- ASSERT_EQ(states.at("P"), sbx);
+ ASSERT_EQ(states.at('P'), sbx);
- states.set("P", sbp);
+ states.set('P', sbp);
- ASSERT_DEATH(states.set("X", sbx), "");
+ ASSERT_DEATH(states.set('X', sbx), "");
- ASSERT_EQ(states.at("X"), nullptr);
+ ASSERT_EQ(states.at('X'), nullptr);
}
TEST_F(StateBlockCompositeInit, set_vec)
@@ -92,13 +92,13 @@ TEST_F(StateBlockCompositeInit, set_vec)
Vector3d p(11,22,33);
Vector3d x(55,66,77);
- states.set("P", p);
+ states.set('P', p);
- ASSERT_MATRIX_APPROX(states.at("P")->getState(), p, 1e-20);
+ ASSERT_MATRIX_APPROX(states.at('P')->getState(), p, 1e-20);
- ASSERT_DEATH(states.set("X", x), "");
+ ASSERT_DEATH(states.set('X', x), "");
- ASSERT_EQ(states.at("X"), nullptr);
+ ASSERT_EQ(states.at('X'), nullptr);
}
TEST_F(StateBlockCompositeInit, set_vectors)
@@ -109,31 +109,31 @@ TEST_F(StateBlockCompositeInit, set_vectors)
states.setVectors("PQ", {p, q});
- ASSERT_MATRIX_APPROX(states.at("P")->getState(), p, 1e-20);
- ASSERT_MATRIX_APPROX(states.at("Q")->getState(), q, 1e-20);
+ ASSERT_MATRIX_APPROX(states.at('P')->getState(), p, 1e-20);
+ ASSERT_MATRIX_APPROX(states.at('Q')->getState(), q, 1e-20);
ASSERT_DEATH(states.setVectors("PX", {p,x}), "");
- ASSERT_EQ(states.at("X"), nullptr);
+ ASSERT_EQ(states.at('X'), nullptr);
}
TEST_F(StateBlockCompositeInit, key_ref)
{
- std::string key;
+ char key;
ASSERT_TRUE(states.key(sbp, key));
- ASSERT_EQ(key, "P");
+ ASSERT_EQ(key, 'P');
ASSERT_FALSE(states.key(sbx, key));
- ASSERT_EQ(key, "");
+ ASSERT_EQ(key, '0');
}
TEST_F(StateBlockCompositeInit, key_return)
{
// existing key
- ASSERT_EQ(states.key(sbp), "P");
+ ASSERT_EQ(states.key(sbp), 'P');
- // non existing key returns empty string
- ASSERT_EQ(states.key(sbx), "");
+ // non existing key returns zero char
+ ASSERT_EQ(states.key(sbx), '0');
}
TEST_F(StateBlockCompositeInit, find)
@@ -147,81 +147,81 @@ TEST_F(StateBlockCompositeInit, find)
TEST_F(StateBlockCompositeInit, add)
{
- states.add("X", sbx);
+ states.add('X', sbx);
- ASSERT_EQ(states.at("X"), sbx);
+ ASSERT_EQ(states.at('X'), sbx);
}
TEST_F(StateBlockCompositeInit, remove)
{
// remove existing block
- states.remove("V");
+ states.remove('V');
ASSERT_EQ(states.getStateBlockMap().size(), 2);
// remove non existing block -- no effect
- states.remove("X");
+ states.remove('X');
ASSERT_EQ(states.getStateBlockMap().size(), 2);
}
TEST_F(StateBlockCompositeInit, perturb)
{
- ASSERT_TRUE(states.at("P")->getState().isApprox(sbp->getState(), 1e-3));
- ASSERT_TRUE(states.at("V")->getState().isApprox(sbv->getState(), 1e-3));
- ASSERT_TRUE(states.at("Q")->getState().isApprox(sbq->getState(), 1e-3));
+ ASSERT_TRUE(states.at('P')->getState().isApprox(sbp->getState(), 1e-3));
+ ASSERT_TRUE(states.at('V')->getState().isApprox(sbv->getState(), 1e-3));
+ ASSERT_TRUE(states.at('Q')->getState().isApprox(sbq->getState(), 1e-3));
states.perturb(0.1);
// values have moved wrt original
- ASSERT_FALSE(states.at("P")->getState().isApprox(Vector3d(1,2,3), 1e-3));
- ASSERT_FALSE(states.at("V")->getState().isApprox(Vector3d(4,5,6), 1e-3));
- ASSERT_FALSE(states.at("Q")->getState().isApprox(Vector4d(.5,.5,.5,.5), 1e-3));
+ ASSERT_FALSE(states.at('P')->getState().isApprox(Vector3d(1,2,3), 1e-3));
+ ASSERT_FALSE(states.at('V')->getState().isApprox(Vector3d(4,5,6), 1e-3));
+ ASSERT_FALSE(states.at('Q')->getState().isApprox(Vector4d(.5,.5,.5,.5), 1e-3));
}
TEST_F(StateBlockCompositeInit, setIdentity)
{
- ASSERT_TRUE(states.at("P")->getState().isApprox(sbp->getState(), 1e-3));
- ASSERT_TRUE(states.at("V")->getState().isApprox(sbv->getState(), 1e-3));
- ASSERT_TRUE(states.at("Q")->getState().isApprox(sbq->getState(), 1e-3));
+ ASSERT_TRUE(states.at('P')->getState().isApprox(sbp->getState(), 1e-3));
+ ASSERT_TRUE(states.at('V')->getState().isApprox(sbv->getState(), 1e-3));
+ ASSERT_TRUE(states.at('Q')->getState().isApprox(sbq->getState(), 1e-3));
states.setIdentity();
// values have moved wrt original
- ASSERT_TRUE(states.at("P")->getState().isApprox(Vector3d(0,0,0), 1e-10));
- ASSERT_TRUE(states.at("V")->getState().isApprox(Vector3d(0,0,0), 1e-10));
- ASSERT_TRUE(states.at("Q")->getState().isApprox(Vector4d(0,0,0,1), 1e-10));
+ ASSERT_TRUE(states.at('P')->getState().isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(states.at('V')->getState().isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(states.at('Q')->getState().isApprox(Vector4d(0,0,0,1), 1e-10));
}
TEST_F(StateBlockCompositeInit, identity)
{
VectorComposite v = states.identity();
- ASSERT_TRUE(v.at("P").isApprox(Vector3d(0,0,0), 1e-10));
- ASSERT_TRUE(v.at("V").isApprox(Vector3d(0,0,0), 1e-10));
- ASSERT_TRUE(v.at("Q").isApprox(Vector4d(0,0,0,1), 1e-10));
+ ASSERT_TRUE(v.at('P').isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(v.at('V').isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(v.at('Q').isApprox(Vector4d(0,0,0,1), 1e-10));
}
TEST_F(StateBlockCompositeInit, fix)
{
states.fix();
- ASSERT_TRUE(states.at("P")->isFixed());
- ASSERT_TRUE(states.at("V")->isFixed());
- ASSERT_TRUE(states.at("Q")->isFixed());
+ ASSERT_TRUE(states.at('P')->isFixed());
+ ASSERT_TRUE(states.at('V')->isFixed());
+ ASSERT_TRUE(states.at('Q')->isFixed());
}
TEST_F(StateBlockCompositeInit, unfix)
{
states.fix();
- ASSERT_TRUE(states.at("P")->isFixed());
- ASSERT_TRUE(states.at("V")->isFixed());
- ASSERT_TRUE(states.at("Q")->isFixed());
+ ASSERT_TRUE(states.at('P')->isFixed());
+ ASSERT_TRUE(states.at('V')->isFixed());
+ ASSERT_TRUE(states.at('Q')->isFixed());
states.unfix();
- ASSERT_FALSE(states.at("P")->isFixed());
- ASSERT_FALSE(states.at("V")->isFixed());
- ASSERT_FALSE(states.at("Q")->isFixed());
+ ASSERT_FALSE(states.at('P')->isFixed());
+ ASSERT_FALSE(states.at('V')->isFixed());
+ ASSERT_FALSE(states.at('Q')->isFixed());
}
TEST_F(StateBlockCompositeInit, isFixed)
@@ -230,7 +230,7 @@ TEST_F(StateBlockCompositeInit, isFixed)
ASSERT_TRUE(states.isFixed());
- states.at("P")->unfix();
+ states.at('P')->unfix();
ASSERT_FALSE(states.isFixed());
}
@@ -245,23 +245,23 @@ TEST(VectorComposite, constructor_empty)
TEST(VectorComposite, constructor_copy)
{
VectorComposite u;
- u.emplace("a", Vector2d(1,2));
- u.emplace("b", Vector3d(3,4,5));
+ u.emplace('a', Vector2d(1,2));
+ u.emplace('b', Vector3d(3,4,5));
VectorComposite v(u);
ASSERT_FALSE(v.empty());
- ASSERT_MATRIX_APPROX(u["a"], v["a"], 1e-20);
- ASSERT_MATRIX_APPROX(u["b"], v["b"], 1e-20);
+ ASSERT_MATRIX_APPROX(u['a'], v['a'], 1e-20);
+ ASSERT_MATRIX_APPROX(u['b'], v['b'], 1e-20);
}
TEST(VectorComposite, constructor_from_list)
{
VectorComposite v(Vector4d(1,2,3,4), "ab", {3,1});
- ASSERT_MATRIX_APPROX(v.at("a"), Vector3d(1,2,3), 1e-20);
- ASSERT_MATRIX_APPROX(v.at("b"), Vector1d(4), 1e-20);
+ ASSERT_MATRIX_APPROX(v.at('a'), Vector3d(1,2,3), 1e-20);
+ ASSERT_MATRIX_APPROX(v.at('b'), Vector1d(4), 1e-20);
}
TEST(VectorComposite, set)
@@ -270,8 +270,8 @@ TEST(VectorComposite, set)
v.set(Vector4d(1,2,3,4), "ab", {3,1});
- ASSERT_MATRIX_APPROX(v.at("a"), Vector3d(1,2,3), 1e-20);
- ASSERT_MATRIX_APPROX(v.at("b"), Vector1d(4), 1e-20);
+ ASSERT_MATRIX_APPROX(v.at('a'), Vector3d(1,2,3), 1e-20);
+ ASSERT_MATRIX_APPROX(v.at('b'), Vector1d(4), 1e-20);
}
TEST(VectorComposite, operatorStream)
@@ -280,8 +280,8 @@ TEST(VectorComposite, operatorStream)
VectorComposite x;
- x.emplace("P", Vector2d(1,1));
- x.emplace("O", Vector3d(2,2,2));
+ x.emplace('P', Vector2d(1,1));
+ x.emplace('O', Vector3d(2,2,2));
WOLF_DEBUG("X = ", x);
}
@@ -290,54 +290,54 @@ TEST(VectorComposite, operatorPlus)
{
VectorComposite x, y;
- x.emplace("P", Vector2d(1,1));
- x.emplace("O", Vector3d(2,2,2));
- y.emplace("P", Vector2d(1,1));
- y.emplace("O", Vector3d(2,2,2));
+ x.emplace('P', Vector2d(1,1));
+ x.emplace('O', Vector3d(2,2,2));
+ y.emplace('P', Vector2d(1,1));
+ y.emplace('O', Vector3d(2,2,2));
WOLF_DEBUG("x + y = ", x + y);
- ASSERT_MATRIX_APPROX((x+y).at("P"), Vector2d(2,2), 1e-20);
- ASSERT_MATRIX_APPROX((x+y).at("O"), Vector3d(4,4,4), 1e-20);
+ ASSERT_MATRIX_APPROX((x+y).at('P'), Vector2d(2,2), 1e-20);
+ ASSERT_MATRIX_APPROX((x+y).at('O'), Vector3d(4,4,4), 1e-20);
}
TEST(VectorComposite, operatorMinus)
{
VectorComposite x, y;
- x.emplace("P", Vector2d(3,3));
- x.emplace("O", Vector3d(6,6,6));
- y.emplace("P", Vector2d(1,1));
- y.emplace("O", Vector3d(2,2,2));
+ x.emplace('P', Vector2d(3,3));
+ x.emplace('O', Vector3d(6,6,6));
+ y.emplace('P', Vector2d(1,1));
+ y.emplace('O', Vector3d(2,2,2));
WOLF_DEBUG("x = ", x);
WOLF_DEBUG("y = ", y);
WOLF_DEBUG("x - y = ", x - y);
- ASSERT_MATRIX_APPROX((x-y).at("P"), Vector2d(2,2), 1e-20);
- ASSERT_MATRIX_APPROX((x-y).at("O"), Vector3d(4,4,4), 1e-20);
+ ASSERT_MATRIX_APPROX((x-y).at('P'), Vector2d(2,2), 1e-20);
+ ASSERT_MATRIX_APPROX((x-y).at('O'), Vector3d(4,4,4), 1e-20);
}
TEST(VectorComposite, unary_Minus)
{
VectorComposite x, y;
- x.emplace("P", Vector2d(1,1));
- x.emplace("O", Vector3d(2,2,2));
+ x.emplace('P', Vector2d(1,1));
+ x.emplace('O', Vector3d(2,2,2));
WOLF_DEBUG("-x = ", -x);
- ASSERT_MATRIX_APPROX((-x).at("P"), Vector2d(-1,-1), 1e-20);
- ASSERT_MATRIX_APPROX((-x).at("O"), Vector3d(-2,-2,-2), 1e-20);
+ ASSERT_MATRIX_APPROX((-x).at('P'), Vector2d(-1,-1), 1e-20);
+ ASSERT_MATRIX_APPROX((-x).at('O'), Vector3d(-2,-2,-2), 1e-20);
}
TEST(VectorComposite, stateVector)
{
VectorComposite x;
- x.emplace("P", Vector2d(1,1));
- x.emplace("O", Vector3d(2,2,2));
- x.emplace("V", Vector4d(3,3,3,3));
+ x.emplace('P', Vector2d(1,1));
+ x.emplace('O', Vector3d(2,2,2));
+ x.emplace('V', Vector4d(3,3,3,3));
VectorXd y(5); y<<1,1,2,2,2;
ASSERT_MATRIX_APPROX(x.vector("PO"), y, 1e-20);
@@ -365,7 +365,7 @@ TEST(MatrixComposite, Constructor_structure)
MatrixComposite M("PO", "POV");
ASSERT_EQ(M.size() , 2);
- ASSERT_EQ(M.at("P").size() , 3);
+ ASSERT_EQ(M.at('P').size() , 3);
ASSERT_TRUE(M.check());
}
@@ -375,10 +375,10 @@ TEST(MatrixComposite, Constructor_structure_sizes)
MatrixComposite M("PO", {3,4}, "POV", {3,4,3});
ASSERT_EQ(M.size() , 2);
- ASSERT_EQ(M.at("P").size() , 3);
+ ASSERT_EQ(M.at('P').size() , 3);
- ASSERT_EQ(M.at("P","O").rows() , 3);
- ASSERT_EQ(M.at("P","O").cols() , 4);
+ ASSERT_EQ(M.at('P','O').rows() , 3);
+ ASSERT_EQ(M.at('P','O').cols() , 4);
ASSERT_EQ(M.matrix("PO","POV").rows() , 7);
ASSERT_EQ(M.matrix("PO","POV").cols() , 10);
@@ -393,16 +393,16 @@ TEST(MatrixComposite, Constructor_eigenMatrix_structure_sizes)
MatrixComposite M(m, "PO", {3,4}, "POV", {3,4,3});
ASSERT_EQ(M.size() , 2);
- ASSERT_EQ(M.at("P").size() , 3);
+ ASSERT_EQ(M.at('P').size() , 3);
- ASSERT_EQ(M.at("P","O").rows() , 3);
- ASSERT_EQ(M.at("P","O").cols() , 4);
+ ASSERT_EQ(M.at('P','O').rows() , 3);
+ ASSERT_EQ(M.at('P','O').cols() , 4);
ASSERT_EQ(M.matrix("PO","POV").rows() , 7);
ASSERT_EQ(M.matrix("PO","POV").cols() , 10);
- ASSERT_MATRIX_APPROX(M.at("P","O"), m.block(0,3,3,4), 1e-20);
- ASSERT_MATRIX_APPROX(M.at("O","V"), m.block(3,7,4,3), 1e-20);
+ ASSERT_MATRIX_APPROX(M.at('P','O'), m.block(0,3,3,4), 1e-20);
+ ASSERT_MATRIX_APPROX(M.at('O','V'), m.block(3,7,4,3), 1e-20);
ASSERT_TRUE(M.check());
}
@@ -462,10 +462,10 @@ TEST(MatrixComposite, emplace_operatorStream)
Mop.setOnes(); Mop *= 3;
Moo.setOnes(); Moo *= 4;
- ASSERT_TRUE(M.emplace("P", "P", Mpp));
- ASSERT_TRUE(M.emplace("P", "O", Mpo));
- ASSERT_TRUE(M.emplace("O", "P", Mop));
- ASSERT_TRUE(M.emplace("O", "O", Moo));
+ ASSERT_TRUE(M.emplace('P', 'P', Mpp));
+ ASSERT_TRUE(M.emplace('P', 'O', Mpo));
+ ASSERT_TRUE(M.emplace('O', 'P', Mop));
+ ASSERT_TRUE(M.emplace('O', 'O', Moo));
cout << "M = " << M << endl;
}
@@ -485,14 +485,14 @@ TEST(MatrixComposite, emplace_operatorStream)
// Mop.setOnes(); Mop *= 3;
// Moo.setOnes(); Moo *= 4;
//
-// M.emplace("P", "P", Mpp);
-// ASSERT_MATRIX_APPROX( M["P"]["P"], Mpp, 1e-20);
+// M.emplace('P', 'P', Mpp);
+// ASSERT_MATRIX_APPROX( M['P']['P'], Mpp, 1e-20);
//
-// M.emplace("P", "O", Mpo);
-// ASSERT_MATRIX_APPROX( M["P"]["O"], Mpo, 1e-20);
+// M.emplace('P', 'O', Mpo);
+// ASSERT_MATRIX_APPROX( M['P']['O'], Mpo, 1e-20);
//
// // return default empty matrix if block not present
-// MatrixXd N = M["O"]["O"];
+// MatrixXd N = M['O']['O'];
// ASSERT_EQ(N.rows(), 0);
// ASSERT_EQ(N.cols(), 0);
//}
@@ -512,14 +512,14 @@ TEST(MatrixComposite, emplace_operatorStream)
// Mop.setOnes(); Mop *= 3;
// Moo.setOnes(); Moo *= 4;
//
-// M.emplace("P", "P", Mpp);
-// ASSERT_MATRIX_APPROX( M("P", "P"), Mpp, 1e-20);
+// M.emplace('P', 'P', Mpp);
+// ASSERT_MATRIX_APPROX( M('P', 'P'), Mpp, 1e-20);
//
-// M.emplace("P", "O", Mpo);
-// ASSERT_MATRIX_APPROX( M("P", "O"), Mpo, 1e-20);
+// M.emplace('P', 'O', Mpo);
+// ASSERT_MATRIX_APPROX( M('P', 'O'), Mpo, 1e-20);
//
// // return default empty matrix if block not present
-// MatrixXd N = M("O", "O");
+// MatrixXd N = M('O', 'O');
// ASSERT_EQ(N.rows(), 0);
// ASSERT_EQ(N.cols(), 0);
//}
@@ -539,14 +539,14 @@ TEST(MatrixComposite, operatorAt)
Mop.setOnes(); Mop *= 3;
Moo.setOnes(); Moo *= 4;
- M.emplace("P", "P", Mpp);
- ASSERT_MATRIX_APPROX( M.at("P", "P"), Mpp, 1e-20);
+ M.emplace('P', 'P', Mpp);
+ ASSERT_MATRIX_APPROX( M.at('P', 'P'), Mpp, 1e-20);
- M.emplace("P", "O", Mpo);
- ASSERT_MATRIX_APPROX( M.at("P", "O"), Mpo, 1e-20);
+ M.emplace('P', 'O', Mpo);
+ ASSERT_MATRIX_APPROX( M.at('P', 'O'), Mpo, 1e-20);
// error if block not present
- ASSERT_DEATH(MatrixXd N = M.at("O", "O"), "");
+ ASSERT_DEATH(MatrixXd N = M.at('O', 'O'), "");
}
TEST(MatrixComposite, productScalar)
@@ -563,10 +563,10 @@ TEST(MatrixComposite, productScalar)
Mop.setOnes(); Mop *= 3;
Moo.setOnes(); Moo *= 4;
- M.emplace("P", "P", Mpp);
- M.emplace("P", "O", Mpo);
- M.emplace("O", "P", Mop);
- M.emplace("O", "O", Moo);
+ M.emplace('P', 'P', Mpp);
+ M.emplace('P', 'O', Mpo);
+ M.emplace('O', 'P', Mop);
+ M.emplace('O', 'O', Moo);
WOLF_DEBUG("M = " , M);
@@ -589,8 +589,8 @@ TEST(MatrixComposite, productVector)
osize = 3;
VectorComposite x;
- x.emplace("P", Vector2d(1,1));
- x.emplace("O", Vector3d(2,2,2));
+ x.emplace('P', Vector2d(1,1));
+ x.emplace('O', Vector3d(2,2,2));
cout << "x= " << x << endl;
@@ -604,10 +604,10 @@ TEST(MatrixComposite, productVector)
Mop.setOnes(); Mop *= 3;
Moo.setOnes(); Moo *= 4;
- M.emplace("P", "P", Mpp);
- M.emplace("P", "O", Mpo);
- M.emplace("O", "P", Mop);
- M.emplace("O", "O", Moo);
+ M.emplace('P', 'P', Mpp);
+ M.emplace('P', 'O', Mpo);
+ M.emplace('O', 'P', Mop);
+ M.emplace('O', 'O', Moo);
WOLF_DEBUG("M = " , M);
VectorComposite y;
@@ -628,15 +628,15 @@ TEST(MatrixComposite, productVector)
Vector2d yp(14,14);
Vector3d yo(30,30,30);
- ASSERT_MATRIX_APPROX(y.at("P"), yp, 1e-20);
- ASSERT_MATRIX_APPROX(y.at("O"), yo, 1e-20);
+ ASSERT_MATRIX_APPROX(y.at('P'), yp, 1e-20);
+ ASSERT_MATRIX_APPROX(y.at('O'), yo, 1e-20);
// throw if x has extra blocks
- // x.emplace("V", Vector2d(3,3));
+ // x.emplace('V', Vector2d(3,3));
// ASSERT_DEATH(y = M * x , ""); // M * x --> does not die if x has extra blocks wrt M
// throw if x has missing blocks
- // x.erase("O");
+ // x.erase('O');
// cout << "x = " << x << endl;
// ASSERT_DEATH(y = M * x , ""); // M * x --> exception if x has missing blocks wrt M, not caught by ASSERT_DEATH
@@ -657,10 +657,10 @@ TEST(MatrixComposite, product)
Mop.setOnes(); Mop *= 3;
Moo.setOnes(); Moo *= 4;
- M.emplace("P", "P", Mpp);
- M.emplace("P", "O", Mpo);
- M.emplace("O", "P", Mop);
- M.emplace("O", "O", Moo);
+ M.emplace('P', 'P', Mpp);
+ M.emplace('P', 'O', Mpo);
+ M.emplace('O', 'P', Mop);
+ M.emplace('O', 'O', Moo);
WOLF_DEBUG("M = " , M);
MatrixXd Noo(osize,osize), Nov(osize, vsize), Npo(psize,osize), Npv(psize,vsize);
@@ -669,10 +669,10 @@ TEST(MatrixComposite, product)
Npo.setOnes(); Npo *= 3;
Npv.setOnes(); Npv *= 4;
- N.emplace("O", "O", Noo);
- N.emplace("O", "V", Nov);
- N.emplace("P", "O", Npo);
- N.emplace("P", "V", Npv);
+ N.emplace('O', 'O', Noo);
+ N.emplace('O', 'V', Nov);
+ N.emplace('P', 'O', Npo);
+ N.emplace('P', 'V', Npv);
WOLF_DEBUG("N = " , N);
MatrixComposite MN;
@@ -693,10 +693,10 @@ TEST(MatrixComposite, product)
MatrixXd MNoo(MatrixXd::Ones(osize,osize) * 22);
MatrixXd MNov(MatrixXd::Ones(osize,vsize) * 32);
- ASSERT_MATRIX_APPROX(MN.at("P","O"), MNpo, 1e-20);
- ASSERT_MATRIX_APPROX(MN.at("P","V"), MNpv, 1e-20);
- ASSERT_MATRIX_APPROX(MN.at("O","O"), MNoo, 1e-20);
- ASSERT_MATRIX_APPROX(MN.at("O","V"), MNov, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at('P','O'), MNpo, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at('P','V'), MNpv, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at('O','O'), MNoo, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at('O','V'), MNov, 1e-20);
ASSERT_TRUE(MN.check());
}
@@ -717,10 +717,10 @@ TEST(MatrixComposite, propagate)
Qop.setOnes(); Qop *= 2;
Qoo.setOnes(); Qoo *= 3;
- Q.emplace("P", "P", Qpp);
- Q.emplace("P", "O", Qpo);
- Q.emplace("O", "P", Qop);
- Q.emplace("O", "O", Qoo);
+ Q.emplace('P', 'P', Qpp);
+ Q.emplace('P', 'O', Qpo);
+ Q.emplace('O', 'P', Qop);
+ Q.emplace('O', 'O', Qoo);
WOLF_DEBUG("Q = " , Q);
MatrixXd Jvp(vsize,psize), Jvo(vsize, osize), Jwp(wsize,psize), Jwo(wsize,osize);
@@ -729,10 +729,10 @@ TEST(MatrixComposite, propagate)
Jwp.setOnes(); Jwp *= 3;
Jwo.setOnes(); Jwo *= 4;
- J.emplace("V", "P", Jvp);
- J.emplace("V", "O", Jvo);
- J.emplace("W", "P", Jwp);
- J.emplace("W", "O", Jwo);
+ J.emplace('V', 'P', Jvp);
+ J.emplace('V', 'O', Jvo);
+ J.emplace('W', 'P', Jwp);
+ J.emplace('W', 'O', Jwo);
WOLF_DEBUG("J = " , J);
MatrixComposite JQJt;
@@ -763,15 +763,15 @@ TEST(MatrixComposite, sum)
Mop.setOnes(); Mop *= 3;
Moo.setOnes(); Moo *= 4;
- M.emplace("P", "P", Mpp);
- M.emplace("P", "O", Mpo);
- M.emplace("O", "P", Mop);
- M.emplace("O", "O", Moo);
+ M.emplace('P', 'P', Mpp);
+ M.emplace('P', 'O', Mpo);
+ M.emplace('O', 'P', Mop);
+ M.emplace('O', 'O', Moo);
WOLF_DEBUG("M = " , M);
- N.emplace("P", "P", Mpp);
- N.emplace("P", "O", Mpo);
- N.emplace("O", "P", Mop);
- N.emplace("O", "O", Moo);
+ N.emplace('P', 'P', Mpp);
+ N.emplace('P', 'O', Mpo);
+ N.emplace('O', 'P', Mop);
+ N.emplace('O', 'O', Moo);
WOLF_DEBUG("N = " , N);
MatrixComposite MpN;
@@ -780,10 +780,10 @@ TEST(MatrixComposite, sum)
WOLF_DEBUG("MpN = M + N = " , MpN);
- ASSERT_MATRIX_APPROX(MpN.at("P","P"), 2 * Mpp, 1e-10);
- ASSERT_MATRIX_APPROX(MpN.at("P","O"), 2 * Mpo, 1e-10);
- ASSERT_MATRIX_APPROX(MpN.at("O","P"), 2 * Mop, 1e-10);
- ASSERT_MATRIX_APPROX(MpN.at("O","O"), 2 * Moo, 1e-10);
+ ASSERT_MATRIX_APPROX(MpN.at('P','P'), 2 * Mpp, 1e-10);
+ ASSERT_MATRIX_APPROX(MpN.at('P','O'), 2 * Mpo, 1e-10);
+ ASSERT_MATRIX_APPROX(MpN.at('O','P'), 2 * Mop, 1e-10);
+ ASSERT_MATRIX_APPROX(MpN.at('O','O'), 2 * Moo, 1e-10);
}
@@ -801,15 +801,15 @@ TEST(MatrixComposite, difference)
Mop.setOnes(); Mop *= 3;
Moo.setOnes(); Moo *= 4;
- M.emplace("P", "P", Mpp);
- M.emplace("P", "O", Mpo);
- M.emplace("O", "P", Mop);
- M.emplace("O", "O", Moo);
+ M.emplace('P', 'P', Mpp);
+ M.emplace('P', 'O', Mpo);
+ M.emplace('O', 'P', Mop);
+ M.emplace('O', 'O', Moo);
WOLF_DEBUG("M = " , M);
- N.emplace("P", "P", Mpp);
- N.emplace("P", "O", Mpo);
- N.emplace("O", "P", Mop);
- N.emplace("O", "O", Moo);
+ N.emplace('P', 'P', Mpp);
+ N.emplace('P', 'O', Mpo);
+ N.emplace('O', 'P', Mop);
+ N.emplace('O', 'O', Moo);
WOLF_DEBUG("N = " , N);
MatrixComposite MmN;
@@ -818,10 +818,10 @@ TEST(MatrixComposite, difference)
WOLF_DEBUG("MmN = M - N = " , MmN);
- ASSERT_MATRIX_APPROX(MmN.at("P","P"), Mpp * 0, 1e-10);
- ASSERT_MATRIX_APPROX(MmN.at("P","O"), Mpo * 0, 1e-10);
- ASSERT_MATRIX_APPROX(MmN.at("O","P"), Mop * 0, 1e-10);
- ASSERT_MATRIX_APPROX(MmN.at("O","O"), Moo * 0, 1e-10);
+ ASSERT_MATRIX_APPROX(MmN.at('P','P'), Mpp * 0, 1e-10);
+ ASSERT_MATRIX_APPROX(MmN.at('P','O'), Mpo * 0, 1e-10);
+ ASSERT_MATRIX_APPROX(MmN.at('O','P'), Mop * 0, 1e-10);
+ ASSERT_MATRIX_APPROX(MmN.at('O','O'), Moo * 0, 1e-10);
}
@@ -839,10 +839,10 @@ TEST(MatrixComposite, unary_minus)
Mop.setOnes(); Mop *= 3;
Moo.setOnes(); Moo *= 4;
- M.emplace("P", "P", Mpp);
- M.emplace("P", "O", Mpo);
- M.emplace("O", "P", Mop);
- M.emplace("O", "O", Moo);
+ M.emplace('P', 'P', Mpp);
+ M.emplace('P', 'O', Mpo);
+ M.emplace('O', 'P', Mop);
+ M.emplace('O', 'O', Moo);
WOLF_DEBUG("M = " , M);
MatrixComposite m;
@@ -851,10 +851,10 @@ TEST(MatrixComposite, unary_minus)
WOLF_DEBUG("m = - M = " , m);
- ASSERT_MATRIX_APPROX(m.at("P","P"), - M.at("P","P"), 1e-10);
- ASSERT_MATRIX_APPROX(m.at("P","O"), - M.at("P","O"), 1e-10);
- ASSERT_MATRIX_APPROX(m.at("O","P"), - M.at("O","P"), 1e-10);
- ASSERT_MATRIX_APPROX(m.at("O","O"), - M.at("O","O"), 1e-10);
+ ASSERT_MATRIX_APPROX(m.at('P','P'), - M.at('P','P'), 1e-10);
+ ASSERT_MATRIX_APPROX(m.at('P','O'), - M.at('P','O'), 1e-10);
+ ASSERT_MATRIX_APPROX(m.at('O','P'), - M.at('O','P'), 1e-10);
+ ASSERT_MATRIX_APPROX(m.at('O','O'), - M.at('O','O'), 1e-10);
}
int main(int argc, char **argv)