diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7bc532fad2f8a2f0520a4bcea8cca45f529f018f..99ec87961134e40e963ac23176a65666a6186bfe 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -203,6 +203,7 @@ SET(HDRS_FRAME
include/core/frame/frame_base.h
)
SET(HDRS_STATE_BLOCK
+ include/core/state_block/has_state_blocks.h
include/core/state_block/local_parametrization_angle.h
include/core/state_block/local_parametrization_base.h
include/core/state_block/local_parametrization_homogeneous.h
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 4c1aa9ab06660e053f6bdd1b922ca976b52c92a7..9aaa79212383afdf27f0f9ea8928cf8b955e0b48 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -112,7 +112,7 @@ int main()
// sensor odometer 2D
IntrinsicsOdom2DPtr intrinsics_odo = std::make_shared<IntrinsicsOdom2D>();
- SensorBasePtr sensor_odo = problem->installSensor("ODOM 2D", "sensor odo", Vector3s(0,0,0), intrinsics_odo);
+ SensorBasePtr sensor_odo = problem->installSensor("SensorOdom2D", "sensor odo", Vector3s(0,0,0), intrinsics_odo);
// processor odometer 2D
ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
@@ -123,19 +123,19 @@ int main()
params_odo->angle_turned = 999;
params_odo->cov_det = 999;
params_odo->unmeasured_perturbation_std = 0.001;
- ProcessorBasePtr processor = problem->installProcessor("ODOM 2D", "processor odo", sensor_odo, params_odo);
+ ProcessorBasePtr processor = problem->installProcessor("ProcessorOdom2D", "processor odo", sensor_odo, params_odo);
// sensor Range and Bearing
IntrinsicsRangeBearingPtr intrinsics_rb = std::make_shared<IntrinsicsRangeBearing>();
intrinsics_rb->noise_range_metres_std = 0.1;
intrinsics_rb->noise_bearing_degrees_std = 1.0;
- SensorBasePtr sensor_rb = problem->installSensor("RANGE BEARING", "sensor RB", Vector3s(1,1,0), intrinsics_rb);
+ SensorBasePtr sensor_rb = problem->installSensor("SensorRangeBearing", "sensor RB", Vector3s(1,1,0), intrinsics_rb);
// processor Range and Bearing
ProcessorParamsRangeBearingPtr params_rb = std::make_shared<ProcessorParamsRangeBearing>();
params_rb->voting_active = false;
params_rb->time_tolerance = 0.01;
- ProcessorBasePtr processor_rb = problem->installProcessor("RANGE BEARING", "processor RB", sensor_rb, params_rb);
+ ProcessorBasePtr processor_rb = problem->installProcessor("ProcessorRangeBearing", "processor RB", sensor_rb, params_rb);
// initialize
TimeStamp t(0.0); // t : 0.0
@@ -226,13 +226,23 @@ int main()
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto& kf : problem->getTrajectory()->getFrameList())
if (kf->isKeyOrAux())
- for (auto& sb : kf->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto& lmk : problem->getMap()->getLandmarkList())
- for (auto& sb : lmk->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+//<<<<<<< HEAD
+ for (auto& pair_key_sb : kf->getStateBlockMap())
+ if (!pair_key_sb.second->isFixed())
+ pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ for (auto& pair_key_sb : lmk->getStateBlockMap())
+ if (!pair_key_sb.second->isFixed())
+ pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
+//=======
+// for (auto& sb : kf->getStateBlockVec())
+// if (sb && !sb->isFixed())
+// sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+// for (auto& lmk : problem->getMap()->getLandmarkList())
+// for (auto& sb : lmk->getStateBlockVec())
+// if (sb && !sb->isFixed())
+// sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+//>>>>>>> devel
problem->print(1,0,1,0);
// SOLVE again
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp
index de8aa4fb7488c8f8b78c703d0b8f363094b97a23..3979a4b45a0023e70f298fcba0c0f470a11a8999 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -209,13 +209,23 @@ int main()
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto& kf : problem->getTrajectory()->getFrameList())
if (kf->isKeyOrAux())
- for (auto& sb : kf->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto& lmk : problem->getMap()->getLandmarkList())
- for (auto& sb : lmk->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+//<<<<<<< HEAD
+ for (auto& pair_key_sb : kf->getStateBlockMap())
+ if (pair_key_sb.second && !pair_key_sb.second->isFixed())
+ pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ for (auto& pair_key_sb : lmk->getStateBlockMap())
+ if (!pair_key_sb.second->isFixed())
+ pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
+//=======
+// for (auto& sb : kf->getStateBlockVec())
+// if (sb && !sb->isFixed())
+// sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+// for (auto& lmk : problem->getMap()->getLandmarkList())
+// for (auto& sb : lmk->getStateBlockVec())
+// if (sb && !sb->isFixed())
+// sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+//>>>>>>> devel
problem->print(1,0,1,0);
// SOLVE again
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index c660f7766163e8c9957e4663964dbda2dc5ec692..ff538651d0bb08709482d29ad87a692cc9426a8b 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -11,13 +11,14 @@ class FeatureBase;
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
+#include "core/state_block/has_state_blocks.h"
//std includes
namespace wolf{
//class CaptureBase
-class CaptureBase : public NodeBase, public std::enable_shared_from_this<CaptureBase>
+class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<CaptureBase>
{
friend FeatureBase;
friend FactorBase;
@@ -28,8 +29,6 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
FeatureBasePtrList feature_list_;
FactorBasePtrList constrained_by_list_;
SensorBaseWPtr sensor_ptr_; ///< Pointer to sensor
- // Deal with sensors with dynamic extrinsics (check dynamic_extrinsic_ in SensorBase)
- std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O, intrinsic.
SizeEigen calib_size_; ///< size of the calibration parameters (dynamic or static sensor params that are not fixed)
static unsigned int capture_id_count_;
@@ -82,23 +81,17 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
const FactorBasePtrList& getConstrainedByList() const;
// State blocks
- const std::vector<StateBlockPtr>& getStateBlockVec() const;
- std::vector<StateBlockPtr>& getStateBlockVec();
- StateBlockPtr getStateBlock(unsigned int _i) const;
- void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
-
+ const std::string& getStructure() const;
+ StateBlockPtr getStateBlock(const std::string& _key) const;
+ StateBlockPtr getStateBlock(const char _key) const { return getStateBlock(std::string(1, _key)); }
StateBlockPtr getSensorP() const;
StateBlockPtr getSensorO() const;
StateBlockPtr getSensorIntrinsic() const;
void removeStateBlocks();
- virtual void registerNewStateBlocks();
+ virtual void registerNewStateBlocks() const;
void fix();
void unfix();
- void fixExtrinsics();
- void unfixExtrinsics();
- void fixIntrinsics();
- void unfixIntrinsics();
bool hasCalibration() {return calib_size_ > 0;}
SizeEigen getCalibSize() const;
@@ -147,34 +140,19 @@ inline void CaptureBase::updateCalibSize()
calib_size_ = computeCalibSize();
}
-inline const std::vector<StateBlockPtr>& CaptureBase::getStateBlockVec() const
-{
- return state_block_vec_;
-}
-
-inline std::vector<StateBlockPtr>& CaptureBase::getStateBlockVec()
-{
- return state_block_vec_;
-}
-
-inline void CaptureBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
-{
- state_block_vec_[_i] = _sb_ptr;
-}
-
inline StateBlockPtr CaptureBase::getSensorP() const
{
- return getStateBlock(0);
+ return getStateBlock("P");
}
inline StateBlockPtr CaptureBase::getSensorO() const
{
- return getStateBlock(1);
+ return getStateBlock("O");
}
inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
{
- return getStateBlock(2);
+ return getStateBlock("I");
}
inline unsigned int CaptureBase::id() const
@@ -232,13 +210,6 @@ inline void CaptureBase::setTimeStampToNow()
time_stamp_.setToNow();
}
-inline bool CaptureBase::process()
-{
- assert (getSensor() != nullptr && "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call sensor->process(capture) instead.");
-
- return getSensor()->process(shared_from_this());
-}
-
} // namespace wolf
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index aed6ff774ed52c5d0d4023188515823feca7ffab..800c680a3897b63d93e8cc5897fca595b76dddc0 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -7,28 +7,30 @@ namespace wolf{
class TrajectoryBase;
class CaptureBase;
class StateBlock;
+
+/** \brief Enumeration of frame types
+ */
+typedef enum
+{
+ KEY = 2, ///< key frame. It plays at optimizations (estimated).
+ AUXILIARY = 1, ///< auxiliary frame. It plays at optimizations (estimated).
+ NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization.
+} FrameType;
}
//Wolf includes
#include "core/common/wolf.h"
#include "core/common/time_stamp.h"
#include "core/common/node_base.h"
+#include "core/state_block/has_state_blocks.h"
//std includes
namespace wolf {
-/** \brief Enumeration of frame types
- */
-typedef enum
-{
- KEY = 2, ///< key frame. It plays at optimizations (estimated).
- AUXILIARY = 1, ///< auxiliary frame. It plays at optimizations (estimated).
- NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization.
-} FrameType;
//class FrameBase
-class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase>
+class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<FrameBase>
{
friend CaptureBase;
friend FactorBase;
@@ -37,7 +39,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
TrajectoryBaseWPtr trajectory_ptr_;
CaptureBasePtrList capture_list_;
FactorBasePtrList constrained_by_list_;
- std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity.
static unsigned int frame_id_count_;
@@ -93,39 +94,18 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
TimeStamp getTimeStamp() const;
void getTimeStamp(TimeStamp& _ts) const;
- // State blocks
- public:
- const std::vector<StateBlockPtr>& getStateBlockVec() const;
- std::vector<StateBlockPtr>& getStateBlockVec();
- protected:
- StateBlockPtr getStateBlock(unsigned int _i) const;
- void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
- void resizeStateBlockVec(unsigned int _size);
-
+ // State blocks ----------------------------------------------------
public:
- StateBlockPtr getP() const;
- StateBlockPtr getO() const;
StateBlockPtr getV() const;
- void setP(const StateBlockPtr _p_ptr);
- void setO(const StateBlockPtr _o_ptr);
void setV(const StateBlockPtr _v_ptr);
protected:
- void registerNewStateBlocks();
+ void registerNewStateBlocks() const;
void removeStateBlocks();
virtual void setProblem(ProblemPtr _problem) final;
- // Fixed / Estimated
- public:
- void fix();
- void unfix();
- bool isFixed() const;
-
// States
public:
void setState(const Eigen::VectorXs& _state);
- Eigen::VectorXs getState() const;
- void getState(Eigen::VectorXs& _state) const;
- unsigned int getSize() const;
bool getCovariance(Eigen::MatrixXs& _cov) const;
// Wolf tree access ---------------------------------------------------
@@ -210,54 +190,14 @@ inline TimeStamp FrameBase::getTimeStamp() const
return time_stamp_;
}
-inline const std::vector<StateBlockPtr>& FrameBase::getStateBlockVec() const
-{
- return state_block_vec_;
-}
-
-inline std::vector<StateBlockPtr>& FrameBase::getStateBlockVec()
-{
- return state_block_vec_;
-}
-
-inline StateBlockPtr FrameBase::getP() const
-{
- return state_block_vec_[0];
-}
-inline void FrameBase::setP(const StateBlockPtr _p_ptr)
-{
- state_block_vec_[0] = _p_ptr;
-}
-
-inline StateBlockPtr FrameBase::getO() const
-{
- return state_block_vec_[1];
-}
-inline void FrameBase::setO(const StateBlockPtr _o_ptr)
-{
- state_block_vec_[1] = _o_ptr;
-}
-
inline StateBlockPtr FrameBase::getV() const
{
- return state_block_vec_[2];
+ return getStateBlock("V");
}
inline void FrameBase::setV(const StateBlockPtr _v_ptr)
{
- state_block_vec_[2] = _v_ptr;
-}
-
-inline StateBlockPtr FrameBase::getStateBlock(unsigned int _i) const
-{
- assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- return state_block_vec_[_i];
-}
-
-inline void FrameBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
-{
- assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- state_block_vec_[_i] = _sb_ptr;
+ addStateBlock("V", _v_ptr);
}
inline TrajectoryBasePtr FrameBase::getTrajectory() const
@@ -270,12 +210,6 @@ inline const CaptureBasePtrList& FrameBase::getCaptureList() const
return capture_list_;
}
-inline void FrameBase::resizeStateBlockVec(unsigned int _size)
-{
- if (_size > state_block_vec_.size())
- state_block_vec_.resize(_size);
-}
-
inline unsigned int FrameBase::getHits() const
{
return constrained_by_list_.size();
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 21e5f1d0cd092a15554ef5cec46563a40317417b..b54690a9c2ade7ae53be655e7de873c3d7e78680 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -11,6 +11,7 @@ class StateBlock;
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
+#include "core/state_block/has_state_blocks.h"
//std includes
@@ -20,14 +21,14 @@ class StateBlock;
namespace wolf {
//class LandmarkBase
-class LandmarkBase : public NodeBase, public std::enable_shared_from_this<LandmarkBase>
+class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<LandmarkBase>
{
friend FactorBase;
private:
MapBaseWPtr map_ptr_;
FactorBasePtrList constrained_by_list_;
- std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O.
+ std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
static unsigned int landmark_id_count_;
@@ -57,27 +58,12 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
unsigned int id() const;
void setId(unsigned int _id);
- // Fix / unfix
- void fix();
- void unfix();
- bool isFixed() const;
-
// State blocks
- const std::vector<StateBlockPtr>& getStateBlockVec() const;
- std::vector<StateBlockPtr>& getStateBlockVec();
std::vector<StateBlockPtr> getUsedStateBlockVec() const;
- StateBlockPtr getStateBlock(unsigned int _i) const;
- void setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr);
- StateBlockPtr getP() const;
- StateBlockPtr getO() const;
- void setP(const StateBlockPtr _p_ptr);
- void setO(const StateBlockPtr _o_ptr);
- Eigen::VectorXs getState() const;
- void getState(Eigen::VectorXs& _state) const;
bool getCovariance(Eigen::MatrixXs& _cov) const;
protected:
- virtual void registerNewStateBlocks();
+ virtual void registerNewStateBlocks() const;
virtual void removeStateBlocks();
// Descriptor
@@ -156,50 +142,6 @@ inline const FactorBasePtrList& LandmarkBase::getConstrainedByList() const
return constrained_by_list_;
}
-inline const std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec() const
-{
- return state_block_vec_;
-}
-
-inline std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec()
-{
- return state_block_vec_;
-}
-
-inline StateBlockPtr LandmarkBase::getStateBlock(unsigned int _i) const
-{
- // assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- if (_i < state_block_vec_.size())
- return state_block_vec_[_i];
- else
- return nullptr;
-}
-
-inline void LandmarkBase::setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr)
-{
- state_block_vec_[_i] = _sb_ptr;
-}
-
-inline StateBlockPtr LandmarkBase::getP() const
-{
- return getStateBlock(0);
-}
-
-inline StateBlockPtr LandmarkBase::getO() const
-{
- return getStateBlock(1);
-}
-
-inline void LandmarkBase::setP(const StateBlockPtr _st_ptr)
-{
- setStateBlock(0, _st_ptr);
-}
-
-inline void LandmarkBase::setO(const StateBlockPtr _st_ptr)
-{
- setStateBlock(1, _st_ptr);
-}
-
inline void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor)
{
descriptor_ = _descriptor;
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 6393a71aa3c9a83facec9d9166a48f04ebf01b0e..c53630febedf7a40c6c811a2c52e090d2897e705 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -13,6 +13,7 @@ class StateBlock;
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
#include "core/common/params_base.h"
+#include "core/state_block/has_state_blocks.h"
//std includes
@@ -81,7 +82,7 @@ struct IntrinsicsBase: public ParamsBase
}
};
-class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBase>
+class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<SensorBase>
{
friend Problem;
friend ProcessorBase;
@@ -89,23 +90,20 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
private:
HardwareBaseWPtr hardware_ptr_;
ProcessorBasePtrList processor_list_;
- std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O, intrinsic.
SizeEigen calib_size_;
static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory)
- protected:
unsigned int sensor_id_; // sensor ID
- bool extrinsic_dynamic_;// extrinsic parameters vary with time? If so, they will be taken from the Capture nodes.
- bool intrinsic_dynamic_;// intrinsic parameters vary with time? If so, they will be taken from the Capture nodes.
- bool has_capture_; // indicates this sensor took at least one capture
+ std::map<std::string, 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_)
+ protected:
Eigen::VectorXs noise_std_; // std of sensor noise
Eigen::MatrixXs noise_cov_; // cov matrix of noise
- std::map<unsigned int,FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by index in state_block_vec_)
-
virtual void setProblem(ProblemPtr _problem) final;
public:
@@ -117,7 +115,9 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
* \param _o_ptr StateBlock pointer to the sensor orientation
* \param _intr_ptr StateBlock pointer to the sensor intrinsic params that might be estimated (if unfixed).
* \param _noise_size dimension of the noise term
- * \param _extr_dyn Flag indicating if extrinsics are dynamic (moving) or static (not moving)
+ * \param _p_dyn Flag indicating if position is dynamic (moving) or static (not moving)
+ * \param _o_dyn Flag indicating if orientation is dynamic (moving) or static (not moving)
+ * \param _intr_dyn Flag indicating if intrinsics is dynamic (moving) or static (not moving)
*
**/
SensorBase(const std::string& _type,
@@ -125,7 +125,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr,
const unsigned int _noise_size,
- const bool _extr_dyn = false,
+ const bool _p_dyn = false,
+ const bool _o_dyn = false,
const bool _intr_dyn = false);
/** \brief Constructor with noise std vector
@@ -136,7 +137,9 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
* \param _o_ptr StateBlock pointer to the sensor orientation
* \param _intr_ptr StateBlock pointer to the sensor intrinsic params that might be estimated (if unfixed).
* \param _noise_std standard deviations of the noise term
- * \param _extr_dyn Flag indicating if extrinsics are dynamic (moving) or static (not moving)
+ * \param _p_dyn Flag indicating if position is dynamic (moving) or static (not moving)
+ * \param _o_dyn Flag indicating if orientation is dynamic (moving) or static (not moving)
+ * \param _intr_dyn Flag indicating if intrinsics is dynamic (moving) or static (not moving)
*
**/
SensorBase(const std::string& _type,
@@ -144,7 +147,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr,
const Eigen::VectorXs & _noise_std,
- const bool _extr_dyn = false,
+ const bool _p_dyn = false,
+ const bool _o_dyn = false,
const bool _intr_dyn = false);
virtual ~SensorBase();
@@ -153,6 +157,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
HardwareBasePtr getHardware() const;
+
private:
void setHardware(const HardwareBasePtr _hw_ptr);
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
@@ -161,24 +166,25 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
public:
const ProcessorBasePtrList& getProcessorList() const;
+ CaptureBasePtr lastCapture(void) const;
CaptureBasePtr lastKeyCapture(void) const;
CaptureBasePtr lastCapture(const TimeStamp& _ts) const;
bool process(const CaptureBasePtr capture_ptr);
// State blocks
- const std::vector<StateBlockPtr>& getStateBlockVec() const;
- std::vector<StateBlockPtr>& getStateBlockVec();
- StateBlockPtr getStateBlockPtrStatic(unsigned int _i) const;
- StateBlockPtr getStateBlock(unsigned int _i) const;
- StateBlockPtr getStateBlock(unsigned int _i, const TimeStamp& _ts) const;
- void setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr);
- void resizeStateBlockVec(unsigned int _size);
-
- bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap) const;
- bool isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap) const;
- bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts) const;
- bool isStateBlockDynamic(unsigned int _i) const;
+ void addStateBlock(const std::string& _key, const StateBlockPtr _sb_ptr);
+ void addStateBlock(const char _key, const StateBlockPtr _sb_ptr) { addStateBlock(std::string(1, _key), _sb_ptr); }
+ 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); }
+
+ 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;
StateBlockPtr getP(const TimeStamp _ts) const;
StateBlockPtr getO(const TimeStamp _ts) const;
@@ -186,18 +192,14 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
StateBlockPtr getP() const;
StateBlockPtr getO() const;
StateBlockPtr getIntrinsic() const;
- void setP(const StateBlockPtr _p_ptr);
- void setO(const StateBlockPtr _o_ptr);
- void setIntrinsic(const StateBlockPtr _intr_ptr);
protected:
void removeStateBlocks();
- virtual void registerNewStateBlocks();
+ virtual void registerNewStateBlocks() const;
+ void setStateBlockDynamic(const std::string& _key, bool _dynamic = true);
public:
- void fix();
- void unfix();
void fixExtrinsics();
void unfixExtrinsics();
void fixIntrinsics();
@@ -213,7 +215,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
* \param _size state segment size (-1: whole state) (not used in quaternions)
*
**/
- void addPriorParameter(const unsigned int _i,
+ void addPriorParameter(const std::string& _key,
const Eigen::VectorXs& _x,
const Eigen::MatrixXs& _cov,
unsigned int _start_idx = 0,
@@ -232,19 +234,10 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
SizeEigen getCalibSize() const;
Eigen::VectorXs getCalibration() const;
- bool isExtrinsicDynamic() const;
- bool isIntrinsicDynamic() const;
- bool hasCapture() const {return has_capture_;}
- void setHasCapture() {has_capture_ = true;}
- bool extrinsicsInCaptures() const { return extrinsic_dynamic_ && has_capture_; }
- bool intrinsicsInCaptures() const { return intrinsic_dynamic_ && has_capture_; }
-
void setNoiseStd(const Eigen::VectorXs & _noise_std);
void setNoiseCov(const Eigen::MatrixXs & _noise_std);
Eigen::VectorXs getNoiseStd() const;
Eigen::MatrixXs getNoiseCov() const;
- void setExtrinsicDynamic(bool _extrinsic_dynamic);
- void setIntrinsicDynamic(bool _intrinsic_dynamic);
void link(HardwareBasePtr);
template<typename classType, typename... T>
@@ -284,45 +277,20 @@ inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
return processor_list_;
}
-inline const std::vector<StateBlockPtr>& SensorBase::getStateBlockVec() const
-{
- return state_block_vec_;
-}
-
-inline std::vector<StateBlockPtr>& SensorBase::getStateBlockVec()
+inline void SensorBase::addStateBlock(const std::string& _key, const StateBlockPtr _sb_ptr)
{
- return state_block_vec_;
+ 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);
}
-inline StateBlockPtr SensorBase::getStateBlockPtrStatic(unsigned int _i) const
+inline void SensorBase::setStateBlockDynamic(const std::string& _key, bool _dynamic)
{
- assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- return state_block_vec_[_i];
+ state_block_dynamic_[_key] = _dynamic;
}
-inline void SensorBase::setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr)
+inline bool SensorBase::isStateBlockDynamic(const std::string& _key) const
{
- assert (_i < state_block_vec_.size() && "Setting a state block pointer out of the vector range!");
- assert((params_prior_map_.find(_i) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
- state_block_vec_[_i] = _sb_ptr;
-}
-
-inline void SensorBase::resizeStateBlockVec(unsigned int _size)
-{
- if (_size > state_block_vec_.size())
- state_block_vec_.resize(_size);
-}
-
-inline bool SensorBase::isExtrinsicDynamic() const
-{
- // If this Sensor has no Capture yet, we'll consider it static
- return extrinsic_dynamic_;
-}
-
-inline bool SensorBase::isIntrinsicDynamic() const
-{
- // If this Sensor has no Capture yet, we'll consider it static
- return intrinsic_dynamic_;
+ return state_block_dynamic_.at(_key);
}
inline Eigen::VectorXs SensorBase::getNoiseStd() const
@@ -340,21 +308,6 @@ inline HardwareBasePtr SensorBase::getHardware() const
return hardware_ptr_.lock();
}
-inline void SensorBase::setP(const StateBlockPtr _p_ptr)
-{
- setStateBlockPtrStatic(0, _p_ptr);
-}
-
-inline void SensorBase::setO(const StateBlockPtr _o_ptr)
-{
- setStateBlockPtrStatic(1, _o_ptr);
-}
-
-inline void SensorBase::setIntrinsic(const StateBlockPtr _intr_ptr)
-{
- setStateBlockPtrStatic(2, _intr_ptr);
-}
-
inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr)
{
hardware_ptr_ = _hw_ptr;
@@ -362,17 +315,17 @@ inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr)
inline void SensorBase::addPriorP(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
{
- addPriorParameter(0, _x, _cov, _start_idx, _size);
+ addPriorParameter("P", _x, _cov, _start_idx, _size);
}
inline void SensorBase::addPriorO(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov)
{
- addPriorParameter(1, _x, _cov);
+ addPriorParameter("O", _x, _cov);
}
inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
{
- addPriorParameter(2, _x, _cov);
+ addPriorParameter("I", _x, _cov);
}
inline SizeEigen SensorBase::getCalibSize() const
@@ -385,16 +338,6 @@ inline void SensorBase::updateCalibSize()
calib_size_ = computeCalibSize();
}
-inline void SensorBase::setExtrinsicDynamic(bool _extrinsic_dynamic)
-{
- extrinsic_dynamic_ = _extrinsic_dynamic;
-}
-
-inline void SensorBase::setIntrinsicDynamic(bool _intrinsic_dynamic)
-{
- intrinsic_dynamic_ = _intrinsic_dynamic;
-}
-
} // namespace wolf
#endif
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
new file mode 100644
index 0000000000000000000000000000000000000000..9a910abd0694c7b7d0d6459a5e271568e9a608dc
--- /dev/null
+++ b/include/core/state_block/has_state_blocks.h
@@ -0,0 +1,279 @@
+/**
+ * \file has_state_blocks.h
+ *
+ * Created on: Aug 27, 2019
+ * \author: jsola
+ */
+
+#ifndef STATE_BLOCK_HAS_STATE_BLOCKS_H_
+#define STATE_BLOCK_HAS_STATE_BLOCKS_H_
+
+#include "core/common/wolf.h"
+
+#include <map>
+
+namespace wolf
+{
+
+class HasStateBlocks
+{
+ public:
+ HasStateBlocks();
+ HasStateBlocks(const std::string& _structure) : structure_(_structure), state_block_map_() {}
+ virtual ~HasStateBlocks();
+
+ public:
+ const std::string& 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; }
+
+ public:
+ const std::map<std::string, StateBlockPtr>& getStateBlockMap() const;
+ std::vector<StateBlockPtr> getStateBlockVec() const;
+
+ public:
+ // Some typical shortcuts -- not all should be coded here, see notes below.
+ StateBlockPtr getP() const;
+ StateBlockPtr getO() const;
+ void setP(const StateBlockPtr _p_ptr);
+ void setO(const StateBlockPtr _o_ptr);
+
+ public:
+ // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
+ void fix();
+ void unfix();
+ bool isFixed() const;
+
+ public:
+ StateBlockPtr addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb);
+ StateBlockPtr addStateBlock(const char _sb_type, const StateBlockPtr& _sb) { return addStateBlock(std::string(1,_sb_type), _sb); }
+ unsigned int removeStateBlock(const std::string& _sb_type);
+ 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)); }
+ 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); }
+
+ // Emplace derived state blocks (angle, quaternion, etc).
+ template<typename SB, typename ... Args>
+ std::shared_ptr<SB> emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_derived_state_block_constructor);
+
+ // Emplace base state blocks.
+ template<typename ... Args>
+ inline StateBlockPtr emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor);
+
+ // States
+ public:
+ void setState(const Eigen::VectorXs& _state, const bool _notify = true);
+ Eigen::VectorXs getState() const;
+ void getState(Eigen::VectorXs& _state) const;
+ unsigned int getSize() const;
+ unsigned int getLocalSize() const;
+
+ private:
+ std::string structure_;
+ std::map<std::string, StateBlockPtr> state_block_map_;
+
+};
+
+} // namespace wolf
+
+
+//////////// IMPLEMENTATION /////////////
+
+#include "core/state_block/state_block.h"
+
+namespace wolf{
+
+inline HasStateBlocks::HasStateBlocks() :
+ structure_(std::string("")),
+ state_block_map_()
+{
+ //
+}
+
+inline HasStateBlocks::~HasStateBlocks()
+{
+ //
+}
+
+inline const std::map<std::string, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const
+{
+ return state_block_map_;
+}
+
+inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() const
+{
+ std::vector<StateBlockPtr> sbv;
+ for (auto& key : structure_)
+ {
+ sbv.push_back(getStateBlock(key));
+ }
+ return sbv;
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::addStateBlock(const std::string& _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.");
+ state_block_map_.emplace(_sb_type, _sb);
+ if (!isInStructure(_sb_type))
+ appendToStructure(_sb_type);
+ return _sb;
+}
+
+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)
+{
+ return state_block_map_.erase(_sb_type);
+}
+
+template<typename SB, typename ... Args>
+inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& _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.");
+ std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
+ state_block_map_.emplace(_sb_type, sb);
+ if (!isInStructure(_sb_type))
+ appendToStructure(_sb_type);
+ return sb;
+}
+
+template<typename ... Args>
+inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _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.");
+ std::shared_ptr<StateBlock> sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
+ state_block_map_.emplace(_sb_type, sb);
+ if (!isInStructure(_sb_type))
+ appendToStructure(_sb_type);
+ return sb;
+}
+
+inline bool HasStateBlocks::setStateBlock(const std::string _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.");
+ state_block_map_.at(_sb_type) = _sb;
+ return true; // success
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const std::string& _sb_type) const
+{
+ auto it = state_block_map_.find(_sb_type);
+ if (it != state_block_map_.end())
+ return it->second;
+ else
+ return nullptr;
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getP() const
+{
+ return getStateBlock("P");
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getO() const
+{
+ return getStateBlock("O");
+}
+
+inline void HasStateBlocks::setP(const StateBlockPtr _p_ptr)
+{
+ addStateBlock("P", _p_ptr);
+}
+
+inline void HasStateBlocks::setO(const StateBlockPtr _o_ptr)
+{
+ addStateBlock("O", _o_ptr);
+}
+
+inline void HasStateBlocks::fix()
+{
+ for (auto pair_key_sbp : state_block_map_)
+ if (pair_key_sbp.second != nullptr)
+ pair_key_sbp.second->fix();
+}
+
+inline void HasStateBlocks::unfix()
+{
+ for (auto pair_key_sbp : state_block_map_)
+ if (pair_key_sbp.second != nullptr)
+ pair_key_sbp.second->unfix();
+}
+
+inline bool HasStateBlocks::isFixed() const
+{
+ bool fixed = true;
+ for (auto pair_key_sbp : state_block_map_)
+ {
+ if (pair_key_sbp.second)
+ fixed &= pair_key_sbp.second->isFixed();
+ }
+ return fixed;
+}
+
+inline void HasStateBlocks::setState(const Eigen::VectorXs& _state, const bool _notify)
+{
+ int size = getSize();
+
+ assert(_state.size() == size && "In FrameBase::setState wrong state size");
+
+ unsigned int index = 0;
+ for (const char key : getStructure())
+ {
+ const auto& sb = getStateBlock(key);
+
+ sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver
+ index += sb->getSize();
+ }
+
+}
+
+inline void HasStateBlocks::getState(Eigen::VectorXs& _state) const
+{
+ _state.resize(getSize());
+
+ unsigned int index = 0;
+ for (const char key : getStructure())
+ {
+ const auto& sb = getStateBlock(key);
+
+ _state.segment(index,sb->getSize()) = sb->getState();
+ index += sb->getSize();
+ }
+
+}
+
+inline Eigen::VectorXs HasStateBlocks::getState() const
+{
+ Eigen::VectorXs state;
+
+ getState(state);
+
+ return state;
+}
+
+inline unsigned int HasStateBlocks::getSize() const
+{
+ unsigned int size = 0;
+ for (const auto& pair_key_sb : getStateBlockMap())
+ size += pair_key_sb.second->getSize();
+ return size;
+}
+
+inline unsigned int HasStateBlocks::getLocalSize() const
+{
+ unsigned int size = 0;
+ for (const auto& pair_key_sb : getStateBlockMap())
+ size += pair_key_sb.second->getLocalSize();
+ return size;
+}
+
+
+} // namespace wolf
+#endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 1001062fcb522bf500dd89985a33669ee08c59ee..63086e7e5f1a7aa332782208df4256cbd787ae43 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -14,9 +14,9 @@ CaptureBase::CaptureBase(const std::string& _type,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr) :
NodeBase("CAPTURE", _type),
+ HasStateBlocks(""),
frame_ptr_(), // nullptr
sensor_ptr_(_sensor_ptr),
- state_block_vec_(3),
calib_size_(0),
capture_id_(++capture_id_count_),
time_stamp_(_ts)
@@ -24,39 +24,38 @@ CaptureBase::CaptureBase(const std::string& _type,
if (_sensor_ptr)
{
- if (_sensor_ptr->isExtrinsicDynamic())
+ if (_sensor_ptr->isStateBlockDynamic("P"))
{
assert(_p_ptr && "Pointer to dynamic position params is null!");
- assert(_o_ptr && "Pointer to dynamic orientation params is null!");
- // assign to Capture's members
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
+ addStateBlock("P", _p_ptr);
}
- else if (_p_ptr || _o_ptr)
+ else
+ assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static");
+
+
+ if (_sensor_ptr->isStateBlockDynamic("O"))
{
- WOLF_ERROR("Provided dynamic sensor extrinsics but the sensor extrinsics are static");
+ assert(_o_ptr && "Pointer to dynamic orientation params is null!");
+ addStateBlock("O", _o_ptr);
}
+ else
+ assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static");
- if (_sensor_ptr->isIntrinsicDynamic())
+ if (_sensor_ptr->isStateBlockDynamic("I"))
{
assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
- // assign to Capture's member
- state_block_vec_[2] = _intr_ptr;
- }
- else if (_intr_ptr)
- {
- WOLF_ERROR("Provided dynamic sensor intrinsics but the sensor intrinsics are static");
+ addStateBlock("I", _intr_ptr);
}
+ else
+ assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static");
- getSensor()->setHasCapture();
}
else if (_p_ptr || _o_ptr || _intr_ptr)
{
WOLF_ERROR("Provided sensor parameters but no sensor pointer");
}
- updateCalibSize();
-// WOLF_TRACE("New Capture ", id(), " -- type ", getType(), " -- t = ", getTimeStamp(), " s");
+ updateCalibSize();
}
CaptureBase::~CaptureBase()
@@ -95,6 +94,15 @@ void CaptureBase::remove(bool viral_remove_empty_parent)
}
}
+bool CaptureBase::process()
+{
+ assert (getSensor() != nullptr && "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call sensor->process(capture) instead.");
+
+ return getSensor()->process(shared_from_this());
+}
+
+
+
FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
{
//std::cout << "Adding feature" << std::endl;
@@ -124,133 +132,71 @@ void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
-StateBlockPtr CaptureBase::getStateBlock(unsigned int _i) const
+const std::string& CaptureBase::getStructure() const
{
if (getSensor())
- {
- if (_i < 2) // _i == 0 is position, 1 is orientation, 2 and onwards are intrinsics
- if (getSensor()->extrinsicsInCaptures())
- {
- assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- return state_block_vec_[_i];
- }
- else
- return getSensor()->getStateBlockPtrStatic(_i);
+ return getSensor()->getStructure();
+ else
+ return HasStateBlocks::getStructure();
+}
- else // 2 and onwards are intrinsics
- if (getSensor()->intrinsicsInCaptures())
- {
- assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- return state_block_vec_[_i];
- }
+StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
+{
+ if (getSensor())
+ {
+ if (getSensor()->isStateBlockDynamic(_key))
+ return HasStateBlocks::getStateBlock(_key);
else
- return getSensor()->getStateBlockPtrStatic(_i);
+ return getSensor()->getStateBlock(_key);
}
else // No sensor associated: assume sensor params are here
- {
- assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- return state_block_vec_[_i];
- }
+ return HasStateBlocks::getStateBlock(_key);
}
void CaptureBase::removeStateBlocks()
{
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
+ for (const auto& key : getStructure()) // note: key is a char
{
- auto sbp = state_block_vec_[i];
+ auto sbp = HasStateBlocks::getStateBlock(key);
if (sbp != nullptr)
{
if (getProblem() != nullptr)
{
getProblem()->notifyStateBlock(sbp, REMOVE);
}
- setStateBlock(i, nullptr);
}
+ removeStateBlock(key);
}
}
void CaptureBase::fix()
{
- for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->fix();
- }
+ HasStateBlocks::fix();
updateCalibSize();
}
void CaptureBase::unfix()
{
- for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->unfix();
- }
- updateCalibSize();
-}
-
-void CaptureBase::fixExtrinsics()
-{
- for (unsigned int i = 0; i < 2; i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->fix();
- }
- updateCalibSize();
-}
-
-void CaptureBase::unfixExtrinsics()
-{
- for (unsigned int i = 0; i < 2; i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->unfix();
- }
- updateCalibSize();
-}
-
-void CaptureBase::fixIntrinsics()
-{
- for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->fix();
- }
- updateCalibSize();
-}
-
-void CaptureBase::unfixIntrinsics()
-{
- for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
- {
- auto sbp = getStateBlock(i);
- if (sbp != nullptr)
- sbp->unfix();
- }
+ HasStateBlocks::unfix();
updateCalibSize();
}
-void CaptureBase::registerNewStateBlocks()
+void CaptureBase::registerNewStateBlocks() const
{
if (getProblem() != nullptr)
{
- for (auto sbp : getStateBlockVec())
- if (sbp != nullptr)
- getProblem()->notifyStateBlock(sbp,ADD);
+ for (auto pair_key_sbp : getStateBlockMap())
+ if (pair_key_sbp.second != nullptr)
+ getProblem()->notifyStateBlock(pair_key_sbp.second,ADD);
}
}
SizeEigen CaptureBase::computeCalibSize() const
{
SizeEigen sz = 0;
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
+ for (const auto& key : getStructure())
{
- auto sb = getStateBlock(i);
+ auto sb = getStateBlock(key);
if (sb && !sb->isFixed())
sz += sb->getSize();
}
@@ -261,9 +207,9 @@ Eigen::VectorXs CaptureBase::getCalibration() const
{
Eigen::VectorXs calib(calib_size_);
SizeEigen index = 0;
- for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
+ for (const auto& key : getStructure())
{
- auto sb = getStateBlock(i);
+ auto sb = getStateBlock(key);
if (sb && !sb->isFixed())
{
calib.segment(index, sb->getSize()) = sb->getState();
@@ -278,9 +224,9 @@ void CaptureBase::setCalibration(const VectorXs& _calib)
updateCalibSize();
assert(_calib.size() == calib_size_ && "Wrong size of calibration vector");
SizeEigen index = 0;
- for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
+ for (const auto& key : getStructure())
{
- auto sb = getStateBlock(i);
+ auto sb = getStateBlock(key);
if (sb && !sb->isFixed())
{
sb->setState(_calib.segment(index, sb->getSize()));
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 7d56eefa61707d3aa78d251c893bf37fef689c46..a1df458e49f2f3f60b5e6b5c6b38a2986cea4112 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -90,9 +90,11 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
//frame state blocks
for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
if (fr_ptr->isKeyOrAux())
- for (auto sb : fr_ptr->getStateBlockVec())
- if (sb)
- all_state_blocks.push_back(sb);
+ for (const auto& key : fr_ptr->getStructure())
+ {
+ const auto& sb = fr_ptr->getStateBlock(key);
+ all_state_blocks.push_back(sb);
+ }
// landmark state blocks
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
@@ -115,16 +117,16 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
// first create a vector containing all state blocks
for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
if (fr_ptr->isKeyOrAux())
- for (auto sb : fr_ptr->getStateBlockVec())
- if (sb)
- for(auto sb2 : fr_ptr->getStateBlockVec())
- if (sb2)
- {
- state_block_pairs.emplace_back(sb, sb2);
- double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
- if (sb == sb2)
- break;
- }
+ for (const auto& key1 : wolf_problem_->getFrameStructure())
+ for (const auto& key2 : wolf_problem_->getFrameStructure())
+ {
+ const auto& sb1 = fr_ptr->getStateBlock(key1);
+ const auto& sb2 = fr_ptr->getStateBlock(key2);
+ state_block_pairs.emplace_back(sb1, sb2);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
+ if (sb1 == sb2)
+ break;
+ }
// landmark state blocks
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index d4202bc490276d0bc8fc858ca6ec54645948619b..ab4f17aef9b3978a63516988d98c5b8d523e7d23 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -13,57 +13,69 @@ unsigned int FrameBase::frame_id_count_ = 0;
FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
NodeBase("FRAME", "Base"),
+ HasStateBlocks(""),
trajectory_ptr_(),
- state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
frame_id_(++frame_id_count_),
type_(NON_ESTIMATED),
time_stamp_(_ts)
{
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _v_ptr;
+ if (_p_ptr)
+ {
+ addStateBlock("P", _p_ptr);
+ }
+ if (_o_ptr)
+ {
+ addStateBlock("O", _o_ptr);
+ }
+ if (_v_ptr)
+ {
+ addStateBlock("V", _v_ptr);
+ }
}
FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
NodeBase("FRAME", "Base"),
+ HasStateBlocks(""),
trajectory_ptr_(),
- state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
frame_id_(++frame_id_count_),
type_(_tp),
time_stamp_(_ts)
{
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _v_ptr;
+ if (_p_ptr)
+ {
+ addStateBlock("P", _p_ptr);
+ }
+ if (_o_ptr)
+ {
+ addStateBlock("O", _o_ptr);
+ }
+ if (_v_ptr)
+ {
+ addStateBlock("V", _v_ptr);
+ }
}
FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) :
NodeBase("FRAME", "Base"),
+ HasStateBlocks(_frame_structure),
trajectory_ptr_(),
- state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
frame_id_(++frame_id_count_),
type_(_tp),
time_stamp_(_ts)
{
if(_frame_structure.compare("PO") == 0 and _dim == 2){
- // auto _x = Eigen::VectorXs::Zero(3);
assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((Scalar) _x(2) ) );
- StateBlockPtr v_ptr ( nullptr );
- state_block_vec_[0] = p_ptr;
- state_block_vec_[1] = o_ptr;
- state_block_vec_[2] = v_ptr;
+ addStateBlock("P", p_ptr);
+ addStateBlock("O", o_ptr);
this->setType("PO 2D");
}else if(_frame_structure.compare("PO") == 0 and _dim == 3){
- // auto _x = Eigen::VectorXs::Zero(7);
assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) );
- StateBlockPtr v_ptr ( nullptr );
- state_block_vec_[0] = p_ptr;
- state_block_vec_[1] = o_ptr;
- state_block_vec_[2] = v_ptr;
+ addStateBlock("P", p_ptr);
+ addStateBlock("O", o_ptr);
this->setType("PO 3D");
}else if(_frame_structure.compare("POV") == 0 and _dim == 3){
// auto _x = Eigen::VectorXs::Zero(10);
@@ -71,15 +83,16 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) );
StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.tail <3> ( ) ) );
- state_block_vec_[0] = p_ptr;
- state_block_vec_[1] = o_ptr;
- state_block_vec_[2] = v_ptr;
+ addStateBlock("P", p_ptr);
+ addStateBlock("O", o_ptr);
+ addStateBlock("V", v_ptr);
this->setType("POV 3D");
}else{
std::cout << _frame_structure << " ^^ " << _dim << std::endl;
throw std::runtime_error("Unknown frame structure");
}
+
}
FrameBase::~FrameBase()
@@ -113,8 +126,6 @@ void FrameBase::remove(bool viral_remove_empty_parent)
// Remove Frame State Blocks
if ( isKeyOrAux() )
removeStateBlocks();
-
-// std::cout << "Removed F" << id() << std::endl;
}
}
@@ -125,29 +136,29 @@ void FrameBase::setTimeStamp(const TimeStamp& _ts)
getTrajectory()->sortFrame(shared_from_this());
}
-void FrameBase::registerNewStateBlocks()
+void FrameBase::registerNewStateBlocks() const
{
if (getProblem() != nullptr)
{
- for (StateBlockPtr sbp : getStateBlockVec())
- if (sbp != nullptr)
- getProblem()->notifyStateBlock(sbp,ADD);
+ for (auto pair_key_sbp : getStateBlockMap())
+ if (pair_key_sbp.second != nullptr)
+ getProblem()->notifyStateBlock(pair_key_sbp.second,ADD);
}
}
void FrameBase::removeStateBlocks()
{
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
+ for (const char key : getStructure()) // note: key is a char
{
- StateBlockPtr sbp = getStateBlock(i);
+ auto sbp = getStateBlock(key);
if (sbp != nullptr)
{
if (getProblem() != nullptr)
{
getProblem()->notifyStateBlock(sbp,REMOVE);
}
- setStateBlock(i, nullptr);
}
+ removeStateBlock(key);
}
}
@@ -194,88 +205,9 @@ void FrameBase::setAux()
}
}
-void FrameBase::fix()
-{
- for (auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->fix();
-}
-
-void FrameBase::unfix()
-{
- for (auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->unfix();
-}
-
-bool FrameBase::isFixed() const
-{
- bool fixed = true;
- for (auto sb : getStateBlockVec())
- {
- if (sb)
- fixed &= sb->isFixed();
- }
- return fixed;
-}
-
void FrameBase::setState(const Eigen::VectorXs& _state)
{
- int size = 0;
- for(unsigned int i = 0; i<state_block_vec_.size(); i++){
- size += (state_block_vec_[i]==nullptr ? 0 : state_block_vec_[i]->getSize());
- }
-
- //State Vector size must be lower or equal to frame state size :
- // example : PQVBB -> if we initialize only position and orientation due to use of processorOdom3D
- assert(_state.size() <= size && "In FrameBase::setState wrong state size");
-
- unsigned int index = 0;
- const unsigned int _st_size = _state.size();
-
- //initialize the FrameBase StateBlocks while StateBlocks list's end not reached and input state_size can go further
- for (StateBlockPtr sb : state_block_vec_)
- if (sb && (index < _st_size))
- {
- sb->setState(_state.segment(index, sb->getSize()), isKeyOrAux()); // do not notify if state block is not estimated by the solver
- index += sb->getSize();
- }
-}
-
-Eigen::VectorXs FrameBase::getState() const
-{
- Eigen::VectorXs state;
-
- getState(state);
-
- return state;
-}
-
-void FrameBase::getState(Eigen::VectorXs& _state) const
-{
- SizeEigen size = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- size += sb->getSize();
-
- _state = Eigen::VectorXs(size);
-
- SizeEigen index = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- {
- _state.segment(index,sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
-}
-
-unsigned int FrameBase::getSize() const
-{
- unsigned int size = 0;
- for (auto st : state_block_vec_)
- if (st)
- size += st->getSize();
- return size;
+ HasStateBlocks::setState(_state, isKeyOrAux());
}
bool FrameBase::getCovariance(Eigen::MatrixXs& _cov) const
@@ -285,10 +217,6 @@ bool FrameBase::getCovariance(Eigen::MatrixXs& _cov) const
FrameBasePtr FrameBase::getPreviousFrame() const
{
- //std::cout << "finding previous frame of " << this->frame_id_ << std::endl;
- if (getTrajectory() == nullptr)
- //std::cout << "This Frame is not linked to any trajectory" << std::endl;
-
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
//look for the position of this node in the upper list (frame list of trajectory)
@@ -299,17 +227,14 @@ FrameBasePtr FrameBase::getPreviousFrame() const
f_it++;
if (f_it != getTrajectory()->getFrameList().rend())
{
- //std::cout << "previous frame found!" << std::endl;
return *f_it;
}
else
{
- //std::cout << "previous frame not found!" << std::endl;
return nullptr;
}
}
}
- //std::cout << "previous frame not found!" << std::endl;
return nullptr;
}
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index a54bf42672a05a8eceb75f52d2ec829e22c2cc8d..cbaf296de143b3467cf1fec60e9763e7f3cb6a69 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -14,15 +14,19 @@ unsigned int LandmarkBase::landmark_id_count_ = 0;
LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
NodeBase("LANDMARK", _type),
+ HasStateBlocks(""),
map_ptr_(),
- state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed.
+ state_block_vec_(0), // Resize in derived constructors if needed.
landmark_id_(++landmark_id_count_)
{
- state_block_vec_[0] = _p_ptr;
+ if (_p_ptr)
+ {
+ addStateBlock("P", _p_ptr);
+ }
if (_o_ptr)
- state_block_vec_[1] = _o_ptr;
- else
- state_block_vec_.resize(1);
+ {
+ addStateBlock("O", _o_ptr);
+ }
}
@@ -54,47 +58,33 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
}
}
-void LandmarkBase::fix()
-{
- for (auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->fix();
-}
-
-void LandmarkBase::unfix()
+std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec() const
{
- for (auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->unfix();
-}
+ std::vector<StateBlockPtr> used_state_block_vec(0);
-bool LandmarkBase::isFixed() const
-{
- bool fixed = true;
- for (auto sb : getStateBlockVec())
+ // normal state blocks in {P,O,V,W}
+ for (const auto& key : getStructure())
{
- if (sb)
- fixed &= sb->isFixed();
+ const auto& sbp = getStateBlock(key);
+ if (sbp)
+ used_state_block_vec.push_back(sbp);
}
- return fixed;
-}
-std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec() const
-{
- std::vector<StateBlockPtr> used_state_block_vec(0);
+ // other state blocks in a vector
for (auto sbp : state_block_vec_)
if (sbp)
used_state_block_vec.push_back(sbp);
+
return used_state_block_vec;
}
-void LandmarkBase::registerNewStateBlocks()
+void LandmarkBase::registerNewStateBlocks() const
{
if (getProblem() != nullptr)
{
- for (auto sbp : getStateBlockVec())
- if (sbp != nullptr)
- getProblem()->notifyStateBlock(sbp,ADD);
+ for (auto pair_key_sbp : getStateBlockMap())
+ if (pair_key_sbp.second != nullptr)
+ getProblem()->notifyStateBlock(pair_key_sbp.second,ADD);
}
}
@@ -105,47 +95,20 @@ bool LandmarkBase::getCovariance(Eigen::MatrixXs& _cov) const
void LandmarkBase::removeStateBlocks()
{
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
+ for (const char key : getStructure()) // note: key is a char
{
- auto sbp = getStateBlock(i);
+ auto sbp = getStateBlock(key);
if (sbp != nullptr)
{
if (getProblem() != nullptr)
{
getProblem()->notifyStateBlock(sbp,REMOVE);
}
- setStateBlock(i, nullptr);
}
+ removeStateBlock(key);
}
}
-Eigen::VectorXs LandmarkBase::getState() const
-{
- Eigen::VectorXs state;
-
- getState(state);
-
- return state;
-}
-
-void LandmarkBase::getState(Eigen::VectorXs& _state) const
-{
- SizeEigen size = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- size += sb->getSize();
-
- _state = Eigen::VectorXs(size);
-
- SizeEigen index = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- {
- _state.segment(index,sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
-}
-
YAML::Node LandmarkBase::saveToYaml() const
{
YAML::Node node;
@@ -156,7 +119,7 @@ YAML::Node LandmarkBase::saveToYaml() const
node["position"] = getP()->getState();
node["position fixed"] = getP()->isFixed();
}
- if (state_block_vec_.size() > 1 && getO() != nullptr)
+ if (getO() != nullptr)
{
node["orientation"] = getO()->getState();
node["orientation fixed"] = getO()->isFixed();
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 2297790b05df066118207a12c437ad7eddcf590e..2e33fb45d42d3f4c2e32fd2d468d5e6a9d737d10 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -670,36 +670,24 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, co
bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance) const
{
bool success(true);
- int i = 0, j = 0;
-
- const auto& state_bloc_vec = _frame_ptr->getStateBlockVec();
-
- // computing size
- SizeEigen sz = 0;
- for (const auto& sb : state_bloc_vec)
- if (sb)
- sz += sb->getLocalSize();
// resizing
- _covariance = Eigen::MatrixXs(sz, sz);
+ SizeEigen sz = _frame_ptr->getLocalSize();
+ _covariance.resize(sz, sz);
// filling covariance
- for (const auto& sb_i : state_bloc_vec)
+ int i = 0, j = 0;
+ for (const auto& sb_i : _frame_ptr->getStateBlockVec())
{
- if (sb_i)
+ j = 0;
+ for (const auto& sb_j : _frame_ptr->getStateBlockVec())
{
- j = 0;
- for (const auto& sb_j : state_bloc_vec)
- {
- if (sb_j)
- {
- success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
- j += sb_j->getLocalSize();
- }
- }
- i += sb_i->getLocalSize();
+ success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
+ j += sb_j->getLocalSize();
}
+ i += sb_i->getLocalSize();
}
+
return success;
}
@@ -718,38 +706,61 @@ bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& cov) const
bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance) const
{
bool success(true);
- int i = 0, j = 0;
-
- const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec();
-
- // computing size
- SizeEigen sz = 0;
- for (const auto& sb : state_bloc_vec)
- if (sb)
- sz += sb->getLocalSize();
// resizing
- _covariance = Eigen::MatrixXs(sz, sz);
+ SizeEigen sz = _landmark_ptr->getLocalSize();
+ _covariance.resize(sz, sz);
// filling covariance
-
- for (const auto& sb_i : state_bloc_vec)
+ int i = 0, j = 0;
+ for (const auto& sb_i : _landmark_ptr->getStateBlockVec())
{
- if (sb_i)
+ j = 0;
+ for (const auto& sb_j : _landmark_ptr->getStateBlockVec())
{
- j = 0;
- for (const auto& sb_j : state_bloc_vec)
- {
- if (sb_j)
- {
- success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
- j += sb_j->getLocalSize();
- }
- }
- i += sb_i->getLocalSize();
+ success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
+ j += sb_j->getLocalSize();
}
+ i += sb_i->getLocalSize();
}
+
return success;
+
+
+
+// bool success(true);
+// int i = 0, j = 0;
+//
+// const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec();
+//
+// // computing size
+// SizeEigen sz = 0;
+// for (const auto& sb : state_bloc_vec)
+// if (sb)
+// sz += sb->getLocalSize();
+//
+// // resizing
+// _covariance = Eigen::MatrixXs(sz, sz);
+//
+// // filling covariance
+//
+// for (const auto& sb_i : state_bloc_vec)
+// {
+// if (sb_i)
+// {
+// j = 0;
+// for (const auto& sb_j : state_bloc_vec)
+// {
+// if (sb_j)
+// {
+// success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
+// j += sb_j->getLocalSize();
+// }
+// }
+// i += sb_i->getLocalSize();
+// }
+// }
+// return success;
}
MapBasePtr Problem::getMap() const
@@ -857,43 +868,40 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
for (auto S : getHardware()->getSensorList())
{
cout << " S" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
- if (!metric && !state_blocks) cout << (S->isExtrinsicDynamic() ? " [Dyn," : " [Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
if (depth < 2)
cout << " -- " << S->getProcessorList().size() << "p";
cout << endl;
if (metric && state_blocks)
{
- for (unsigned int i = 0; i < S->getStateBlockVec().size(); i++)
+ cout << " sb: ";
+ for (auto& _key : S->getStructure())
{
- if (i==0) cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
- if (i==2) cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
- auto sb = S->getStateBlock(i);
- if (sb)
- {
- cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
- }
- if (i==1) cout << " ]" << endl;
+ auto key = std::string(1,_key);
+ auto sb = S->getStateBlock(key);
+ cout << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
}
- if (S->getStateBlockVec().size() > 2) cout << " ]" << endl;
+ cout << endl;
}
else if (metric)
{
- cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( ";
- if (S->getP())
- cout << S->getP()->getState().transpose();
- if (S->getO())
- cout << " " << S->getO()->getState().transpose();
- cout << " )";
- if (S->getIntrinsic())
- cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( " << S->getIntrinsic()->getState().transpose() << " )";
- cout << endl;
+ cout << " ( ";
+ for (auto& _key : S->getStructure())
+ {
+ auto key = std::string(1,_key);
+ auto sb = S->getStateBlock(key);
+ cout << sb->getState().transpose() << " ";
+ }
+ cout << ")" << endl;
}
else if (state_blocks)
{
- cout << " sb:" << (S->isExtrinsicDynamic() ? "[Dyn," : "[Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
- for (auto sb : S->getStateBlockVec())
- if (sb != nullptr)
- cout << " " << (sb->isFixed() ? "Fix" : "Est");
+ cout << " sb: ";
+ for (auto& _key : S->getStructure())
+ {
+ auto key = std::string(1,_key);
+ auto sb = S->getStateBlock(key);
+ cout << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ }
cout << endl;
}
if (depth >= 2)
@@ -958,9 +966,10 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
if (state_blocks)
{
cout << " sb:";
- for (auto sb : F->getStateBlockVec())
- if (sb != nullptr)
- cout << " " << (sb->isFixed() ? "Fix" : "Est");
+ for (const auto& sb : F->getStateBlockVec())
+ {
+ cout << " " << (sb->isFixed() ? "Fix" : "Est");
+ }
cout << endl;
}
if (depth >= 2)
@@ -973,8 +982,6 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
if(C->getSensor() != nullptr)
{
cout << " -> S" << C->getSensor()->id();
- cout << (C->getSensor()->isExtrinsicDynamic() ? " [Dyn, ": " [Sta, ");
- cout << (C->getSensor()->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
}
else
cout << " -> S-";
@@ -999,7 +1006,8 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
cout << endl;
if (state_blocks)
- for(auto sb : C->getStateBlockVec())
+ for (const auto& sb : C->getStateBlockVec())
+ {
if(sb != nullptr)
{
cout << " sb: ";
@@ -1008,6 +1016,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
cout << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
cout << endl;
}
+ }
if (C->isMotion() )
{
@@ -1090,9 +1099,11 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
if (state_blocks)
{
cout << " sb:";
- for (auto sb : L->getStateBlockVec())
+ for (const auto& sb : L->getStateBlockVec())
+ {
if (sb != nullptr)
cout << (sb->isFixed() ? " Fix" : " Est");
+ }
cout << endl;
}
} // for L
@@ -1116,45 +1127,44 @@ std::string Problem::printToString(int depth, bool constr_by, bool metric, bool
for (auto S : getHardware()->getSensorList())
{
result << " S" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
- if (!metric && !state_blocks) result << (S->isExtrinsicDynamic() ? " [Dyn," : " [Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
if (depth < 2)
result << " -- " << S->getProcessorList().size() << "p";
result << endl;
if (metric && state_blocks)
{
- for (unsigned int i = 0; i < S->getStateBlockVec().size(); i++)
+ result << " sb: ";
+ for (auto& _key : S->getStructure())
{
- if (i==0) result << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
- if (i==2) result << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
- auto sb = S->getStateBlock(i);
- if (sb)
- {
- result << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
- }
- if (i==1) result << " ]" << endl;
+ auto key = std::string(1,_key);
+ auto sb = S->getStateBlock(key);
+ result << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
}
- if (S->getStateBlockVec().size() > 2) result << " ]" << endl;
+ result << endl;
}
else if (metric)
{
- result << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( ";
- if (S->getP())
- result << S->getP()->getState().transpose();
- if (S->getO())
- result << " " << S->getO()->getState().transpose();
- result << " )";
- if (S->getIntrinsic())
- result << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( " << S->getIntrinsic()->getState().transpose() << " )";
- result << endl;
+ result << " ( ";
+ for (auto& _key : S->getStructure())
+ {
+ auto key = std::string(1,_key);
+ auto sb = S->getStateBlock(key);
+ result << sb->getState().transpose() << " ";
+ }
+ result << ")" << endl;
}
else if (state_blocks)
{
- result << " sb:" << (S->isExtrinsicDynamic() ? "[Dyn," : "[Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
- for (auto sb : S->getStateBlockVec())
- if (sb != nullptr)
- result << " " << (sb->isFixed() ? "Fix" : "Est");
+ result << " sb: ";
+ for (auto& _key : S->getStructure())
+ {
+ auto key = std::string(1,_key);
+ auto sb = S->getStateBlock(key);
+ result << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ }
result << endl;
}
+
+
if (depth >= 2)
{
// Processors =======================================================================================
@@ -1232,8 +1242,6 @@ std::string Problem::printToString(int depth, bool constr_by, bool metric, bool
if(C->getSensor() != nullptr)
{
result << " -> S" << C->getSensor()->id();
- result << (C->getSensor()->isExtrinsicDynamic() ? " [Dyn, ": " [Sta, ");
- result << (C->getSensor()->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
}
else
result << " -> S-";
@@ -1442,7 +1450,7 @@ bool Problem::check(int verbose_level) const
cout << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " EF") : " F") << F->id() << " @ " << F.get() << endl;
cout << " -> P @ " << F->getProblem().get() << endl;
cout << " -> T @ " << F->getTrajectory().get() << endl;
- for (auto sb : F->getStateBlockVec())
+ for (const auto& sb : F->getStateBlockVec())
{
cout << " sb @ " << sb.get();
if (sb)
@@ -1645,15 +1653,18 @@ bool Problem::check(int verbose_level) const
}
}
// find in own Frame
- found = found || (std::find(F->getStateBlockVec().begin(), F->getStateBlockVec().end(), sb) != F->getStateBlockVec().end());
+ found = found || (std::find_if(F->getStateBlockMap().begin(), F->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != F->getStateBlockMap().end());
// find in own Capture
found = found || (std::find(C->getStateBlockVec().begin(), C->getStateBlockVec().end(), sb) != C->getStateBlockVec().end());
// find in own Sensor
if (S)
- found = found || (std::find(S->getStateBlockVec().begin(), S->getStateBlockVec().end(), sb) != S->getStateBlockVec().end());
+ {
+ auto sb_vec = S->getStateBlockVec();
+ found = found || (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+ }
// find in constrained Frame
if (Fo)
- found = found || (std::find(Fo->getStateBlockVec().begin(), Fo->getStateBlockVec().end(), sb) != Fo->getStateBlockVec().end());
+ found = found || (std::find_if(Fo->getStateBlockMap().begin(), Fo->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != Fo->getStateBlockMap().end());
// find in constrained Capture
if (Co)
found = found || (std::find(Co->getStateBlockVec().begin(), Co->getStateBlockVec().end(), sb) != Co->getStateBlockVec().end());
@@ -1662,7 +1673,7 @@ bool Problem::check(int verbose_level) const
{
// find in constrained feature's Frame
FrameBasePtr foF = fo->getFrame();
- found = found || (std::find(foF->getStateBlockVec().begin(), foF->getStateBlockVec().end(), sb) != foF->getStateBlockVec().end());
+ found = found || (std::find_if(foF->getStateBlockMap().begin(), foF->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != foF->getStateBlockMap().end());
// find in constrained feature's Capture
CaptureBasePtr foC = fo->getCapture();
found = found || (std::find(foC->getStateBlockVec().begin(), foC->getStateBlockVec().end(), sb) != foC->getStateBlockVec().end());
@@ -1672,7 +1683,7 @@ bool Problem::check(int verbose_level) const
}
// find in constrained landmark
if (Lo)
- found = found || (std::find(Lo->getStateBlockVec().begin(), Lo->getStateBlockVec().end(), sb) != Lo->getStateBlockVec().end());
+ found = found || (std::find_if(Lo->getStateBlockMap().begin(), Lo->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != Lo->getStateBlockMap().end());
if (verbose_level > 0)
{
if (found)
@@ -1705,7 +1716,7 @@ bool Problem::check(int verbose_level) const
cout << " L" << L->id() << " @ " << L.get() << endl;
cout << " -> P @ " << L->getProblem().get() << endl;
cout << " -> M @ " << L->getMap().get() << endl;
- for (auto sb : L->getStateBlockVec())
+ for (const auto& sb : L->getStateBlockVec())
{
cout << " sb @ " << sb.get();
if (sb)
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 755a5d7e02864db5b180c6e65bb824bedb65e759..eb5cdf8e82f80164a7ebf5b6d1a6e790396ea7b7 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -14,24 +14,38 @@ SensorBase::SensorBase(const std::string& _type,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr,
const unsigned int _noise_size,
- const bool _extr_dyn,
+ const bool _p_dyn,
+ const bool _o_dyn,
const bool _intr_dyn) :
NodeBase("SENSOR", _type),
+ HasStateBlocks(""),
hardware_ptr_(),
- state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
calib_size_(0),
sensor_id_(++sensor_id_count_), // simple ID factory
- extrinsic_dynamic_(_extr_dyn),
- intrinsic_dynamic_(_intr_dyn),
- has_capture_(false),
noise_std_(_noise_size),
noise_cov_(_noise_size, _noise_size)
{
noise_std_.setZero();
noise_cov_.setZero();
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _intr_ptr;
+
+ if (_p_ptr)
+ {
+ addStateBlock("P", _p_ptr);
+ }
+ setStateBlockDynamic("P", _p_dyn);
+
+ if (_o_ptr)
+ {
+ addStateBlock("O", _o_ptr);
+ }
+ setStateBlockDynamic("O", _o_dyn);
+
+ if (_intr_ptr)
+ {
+ addStateBlock("I", _intr_ptr);
+ }
+ setStateBlockDynamic("I", _intr_dyn);
+
updateCalibSize();
}
@@ -40,23 +54,37 @@ SensorBase::SensorBase(const std::string& _type,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr,
const Eigen::VectorXs & _noise_std,
- const bool _extr_dyn,
+ const bool _p_dyn,
+ const bool _o_dyn,
const bool _intr_dyn) :
NodeBase("SENSOR", _type),
+ HasStateBlocks(""),
hardware_ptr_(),
- state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
calib_size_(0),
sensor_id_(++sensor_id_count_), // simple ID factory
- extrinsic_dynamic_(_extr_dyn),
- intrinsic_dynamic_(_intr_dyn),
- has_capture_(false),
noise_std_(_noise_std),
noise_cov_(_noise_std.size(), _noise_std.size())
{
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _intr_ptr;
setNoiseStd(_noise_std);
+
+ if (_p_ptr)
+ {
+ addStateBlock("P", _p_ptr);
+ }
+ setStateBlockDynamic("P", _p_dyn);
+
+ if (_o_ptr)
+ {
+ addStateBlock("O", _o_ptr);
+ }
+ setStateBlockDynamic("O", _o_dyn);
+
+ if (_intr_ptr)
+ {
+ addStateBlock("I", _intr_ptr);
+ }
+ setStateBlockDynamic("I", _intr_dyn);
+
updateCalibSize();
}
@@ -68,41 +96,26 @@ SensorBase::~SensorBase()
void SensorBase::removeStateBlocks()
{
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
+ for (const auto& _key : getStructure())
{
- auto sbp = getStateBlockPtrStatic(i);
+ auto key = std::string(1,_key);
+ auto sbp = getStateBlock(key);
if (sbp != nullptr)
{
- if (getProblem() != nullptr && !extrinsic_dynamic_)
+ if (getProblem() != nullptr && !isStateBlockInCapture(key))
{
getProblem()->notifyStateBlock(sbp,REMOVE);
}
- setStateBlockPtrStatic(i, nullptr);
+ removeStateBlock(key);
}
}
}
-void SensorBase::fix()
-{
- for( auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->fix();
- updateCalibSize();
-}
-
-void SensorBase::unfix()
-{
- for( auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->unfix();
- updateCalibSize();
-}
-
void SensorBase::fixExtrinsics()
{
- for (unsigned int i = 0; i < 2; i++)
+ for (const auto& key : std::string("PO"))
{
- auto sbp = state_block_vec_[i];
+ auto sbp = getStateBlockDynamic(key);
if (sbp != nullptr)
sbp->fix();
}
@@ -111,9 +124,9 @@ void SensorBase::fixExtrinsics()
void SensorBase::unfixExtrinsics()
{
- for (unsigned int i = 0; i < 2; i++)
+ for (const auto& key : std::string("PO"))
{
- auto sbp = state_block_vec_[i];
+ auto sbp = getStateBlockDynamic(key);
if (sbp != nullptr)
sbp->unfix();
}
@@ -122,32 +135,39 @@ void SensorBase::unfixExtrinsics()
void SensorBase::fixIntrinsics()
{
- for (unsigned int i = 2; i < state_block_vec_.size(); i++)
+ for (const auto& _key : getStructure())
{
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->fix();
+ auto key = std::string(1,_key);
+ if (key != "P" and key != "O") // exclude extrinsics
+ {
+ auto sbp = getStateBlockDynamic(key);
+ if (sbp != nullptr)
+ sbp->fix();
+ }
}
updateCalibSize();
}
void SensorBase::unfixIntrinsics()
{
- for (unsigned int i = 2; i < state_block_vec_.size(); i++)
+ for (const auto& _key : getStructure())
{
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->unfix();
+ auto key = std::string(1,_key);
+ if (key != "P" and key != "O") // exclude extrinsics
+ {
+ auto sbp = getStateBlockDynamic(key);
+ if (sbp != nullptr)
+ sbp->unfix();
+ }
}
updateCalibSize();
}
-void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+void SensorBase::addPriorParameter(const std::string& _key, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
{
- assert(!isStateBlockDynamic(_i) && "SensorBase::addPriorParameter only allowed for static parameters");
- assert(_i < state_block_vec_.size() && "State block not found");
+ assert(!isStateBlockInCapture(_key) && "SensorBase::addPriorParameter only allowed for static parameters");
- StateBlockPtr _sb = getStateBlockPtrStatic(_i);
+ StateBlockPtr _sb = getStateBlock(_key);
bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr);
assert(((!is_quaternion && _x.size() == _cov.rows() && _x.size() == _cov.cols()) ||
@@ -167,11 +187,11 @@ void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs&
}
// remove previous prior (if any)
- if (params_prior_map_.find(_i) != params_prior_map_.end())
- params_prior_map_[_i]->remove();
+ if (params_prior_map_.find(_key) != params_prior_map_.end())
+ params_prior_map_[_key]->remove();
// create feature
- FeatureBasePtr ftr_prior = FeatureBase::emplace<FeatureBase>(nullptr, "ABSOLUTE",_x,_cov);//std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
+ FeatureBasePtr ftr_prior = FeatureBase::emplace<FeatureBase>(nullptr, "ABSOLUTE",_x,_cov); //std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
// set feature problem
ftr_prior->setProblem(getProblem());
@@ -183,27 +203,20 @@ void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs&
FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, _sb, _start_idx, _size);
// store feature in params_prior_map_
- params_prior_map_[_i] = ftr_prior;
+ params_prior_map_[_key] = ftr_prior;
}
-void SensorBase::registerNewStateBlocks()
+void SensorBase::registerNewStateBlocks() const
{
if (getProblem() != nullptr)
{
- for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
+ for (auto & pair_key_sbp : getStateBlockMap())
{
- if (i < 2 && !isExtrinsicDynamic())
- {
- auto sbp = getStateBlockPtrStatic(i);
- if (sbp != nullptr)
- getProblem()->notifyStateBlock(sbp,ADD);
- }
- if (i >= 2 && !isIntrinsicDynamic())
- {
- auto sbp = getStateBlockPtrStatic(i);
- if (sbp != nullptr)
- getProblem()->notifyStateBlock(sbp,ADD);
- }
+ auto key = pair_key_sbp.first;
+ auto sbp = pair_key_sbp.second;
+
+ if (sbp && !isStateBlockInCapture(key))
+ getProblem()->notifyStateBlock(sbp, ADD);
}
}
}
@@ -218,22 +231,45 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
noise_cov_ = _noise_cov;
}
-CaptureBasePtr SensorBase::lastKeyCapture(void) const
+CaptureBasePtr SensorBase::lastCapture(void) const
{
// we search for the most recent Capture of this sensor which belongs to a KeyFrame
CaptureBasePtr capture = nullptr;
- FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
- FrameBaseRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
+ if (getProblem())
{
- if ((*frame_rev_it)->isKey())
+ FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
+ FrameBaseRevIter frame_rev_it = frame_list.rbegin();
+ while (frame_rev_it != frame_list.rend())
{
capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
if (capture)
// found the most recent Capture made by this sensor !
break;
+ frame_rev_it++;
+ }
+ }
+ return capture;
+}
+
+CaptureBasePtr SensorBase::lastKeyCapture(void) const
+{
+ // we search for the most recent Capture of this sensor which belongs to a KeyFrame
+ CaptureBasePtr capture = nullptr;
+ if (getProblem())
+ {
+ FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
+ FrameBaseRevIter frame_rev_it = frame_list.rbegin();
+ while (frame_rev_it != frame_list.rend())
+ {
+ if ((*frame_rev_it)->isKey())
+ {
+ capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ if (capture)
+ // found the most recent Capture made by this sensor !
+ break;
+ }
+ frame_rev_it++;
}
- frame_rev_it++;
}
return capture;
}
@@ -242,60 +278,63 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts) const
{
// we search for the most recent Capture of this sensor before _ts
CaptureBasePtr capture = nullptr;
- FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
-
- // We iterate in reverse since we're likely to find it close to the rbegin() place.
- FrameBaseRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
+ if (getProblem())
{
- if ((*frame_rev_it)->getTimeStamp() <= _ts)
+ FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
+
+ // We iterate in reverse since we're likely to find it close to the rbegin() place.
+ FrameBaseRevIter frame_rev_it = frame_list.rbegin();
+ while (frame_rev_it != frame_list.rend())
{
- CaptureBasePtr capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
- if (capture)
- // found the most recent Capture made by this sensor !
- break;
+ if ((*frame_rev_it)->getTimeStamp() <= _ts)
+ {
+ CaptureBasePtr capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ if (capture)
+ // found the most recent Capture made by this sensor !
+ break;
+ }
+ frame_rev_it++;
}
- frame_rev_it++;
}
return capture;
}
StateBlockPtr SensorBase::getP(const TimeStamp _ts) const
{
- return getStateBlock(0, _ts);
+ return getStateBlockDynamic("P", _ts);
}
StateBlockPtr SensorBase::getO(const TimeStamp _ts) const
{
- return getStateBlock(1, _ts);
+ return getStateBlockDynamic("O", _ts);
}
StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts) const
{
- return getStateBlock(2, _ts);
+ return getStateBlockDynamic("I", _ts);
}
StateBlockPtr SensorBase::getP() const
{
- return getStateBlock(0);
+ return getStateBlockDynamic("P");
}
StateBlockPtr SensorBase::getO() const
{
- return getStateBlock(1);
+ return getStateBlockDynamic("O");
}
StateBlockPtr SensorBase::getIntrinsic() const
{
- return getStateBlock(2);
+ return getStateBlockDynamic("I");
}
SizeEigen SensorBase::computeCalibSize() const
{
SizeEigen sz = 0;
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
+ for (const auto& pair_key_sb : getStateBlockMap())
{
- auto sb = state_block_vec_[i];
+ auto sb = pair_key_sb.second;
if (sb && !sb->isFixed())
sz += sb->getSize();
}
@@ -307,9 +346,9 @@ Eigen::VectorXs SensorBase::getCalibration() const
SizeEigen index = 0;
SizeEigen sz = getCalibSize();
Eigen::VectorXs calib(sz);
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
+ for (const auto& key : getStructure())
{
- auto sb = getStateBlockPtrStatic(i);
+ auto sb = getStateBlockDynamic(key);
if (sb && !sb->isFixed())
{
calib.segment(index, sb->getSize()) = sb->getState();
@@ -342,62 +381,63 @@ void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr)
processor_list_.remove(_proc_ptr);
}
-StateBlockPtr SensorBase::getStateBlock(unsigned int _i) const
+StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key) const
{
CaptureBasePtr cap;
- if (isStateBlockDynamic(_i, cap))
- return cap->getStateBlock(_i);
+ if (isStateBlockInCapture(_key, cap))
+ return cap->getStateBlock(_key);
- return getStateBlockPtrStatic(_i);
+ return getStateBlock(_key);
}
-StateBlockPtr SensorBase::getStateBlock(unsigned int _i, const TimeStamp& _ts) const
+StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key, const TimeStamp& _ts) const
{
CaptureBasePtr cap;
- if (isStateBlockDynamic(_i, _ts, cap))
- return cap->getStateBlock(_i);
+ if (isStateBlockInCapture(_key, _ts, cap))
+ return cap->getStateBlock(_key);
- return getStateBlockPtrStatic(_i);
+ return getStateBlock(_key);
}
-bool SensorBase::isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap) const
+bool SensorBase::isStateBlockInCapture(const std::string& _key, CaptureBasePtr& _cap) const
{
- if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
+ if (isStateBlockDynamic(_key))
{
- cap = lastKeyCapture();
+ _cap = lastCapture();
+ // cap = lastKeyCapture();
- return cap != nullptr;
+ return _cap != nullptr;
}
else
return false;
}
-bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap) const
+bool SensorBase::isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const
{
- if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
+ if (isStateBlockDynamic(_key))
{
- cap = lastCapture(_ts);
+ _cap = lastCapture(_ts);
- return cap != nullptr;
+ return _cap != nullptr;
}
else
return false;
}
-bool SensorBase::isStateBlockDynamic(unsigned int _i) const
+bool SensorBase::isStateBlockInCapture(const std::string& _key) const
{
CaptureBasePtr cap;
- return isStateBlockDynamic(_i,cap);
+ return isStateBlockInCapture(_key, cap);
}
-bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts) const
+bool SensorBase::isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts) const
{
CaptureBasePtr cap;
- return isStateBlockDynamic(_i,_ts,cap);
+ return isStateBlockInCapture(_key, _ts, cap);
}
void SensorBase::setProblem(ProblemPtr _problem)
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 6e4446da2766cb670e23226fc1945d4f86af3f90..429991bdb5175387a16b4fa26f925e4460cf75cf 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -22,7 +22,8 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXs& _extrinsics,
{
radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
getIntrinsic()->setState(Eigen::Vector3s(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
- getIntrinsic()->unfix();
+ unfixIntrinsics();
+// getIntrinsic()->unfix();
}
SensorDiffDrive::~SensorDiffDrive()
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 15be1ff455614ce95b116e865b1666a1a728d24b..7750ddc1c8f07af56a953b4e806bdff0f27294ee 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -59,7 +59,7 @@ TEST(CaptureBase, Dynamic_sensor_params)
StateBlockPtr p(std::make_shared<StateBlock>(2));
StateBlockPtr o(std::make_shared<StateAngle>() );
StateBlockPtr i(std::make_shared<StateBlock>(2));
- SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2, true, true)); // last 2 'true' mark dynamic
+ SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2, true, true, true)); // last 3 'true' mark dynamic
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5
// query sensor blocks -- need KFs to find some Capture with the params
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 2f6da7f75f258e796e037032a33930e4079a05ab..4be7353651ee5c037afa813da4b88f9d3848d897 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -39,7 +39,7 @@ TEST(FrameBase, StateBlocks)
{
FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- ASSERT_EQ(F->getStateBlockVec().size(), (unsigned int) 3);
+ ASSERT_EQ(F->getStateBlockMap().size(),(unsigned int) 2);
ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2);
ASSERT_EQ(F->getO()->getState().size(),(unsigned int) 1);
ASSERT_EQ(F->getV(), nullptr);
@@ -142,17 +142,17 @@ TEST(FrameBase, GetSetState)
// Set the state, check that state blocks hold the current states
F.setState(x);
- ASSERT_TRUE((p - F.getP()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- ASSERT_TRUE((q - F.getO()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- ASSERT_TRUE((v - F.getV()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_MATRIX_APPROX(p, F.getP()->getState(), Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(q, F.getO()->getState(), Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(v, F.getV()->getState(), Constants::EPS_SMALL);
// Get the state, form 1 by reference
F.getState(x1);
- ASSERT_TRUE((x1 - x).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_MATRIX_APPROX(x1 , x, Constants::EPS_SMALL);
// get the state, form 2 by return value
x2 = F.getState();
- ASSERT_TRUE((x2 - x).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_MATRIX_APPROX(x2, x, Constants::EPS_SMALL);
}
int main(int argc, char **argv)
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index 3329f1c7250c6e5cd961c8ee4dd18fee477cd2a5..912cb1809745cdc2e47782828323687e7328de05 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(1, prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2),"");
+ ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter("O", prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2),"");
}