diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index aaa76c9220c4351f1cca5b6a38bf290ef8e69c58..70fee2c15f074e571fd273d7cfccfbdc21c20552 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -101,7 +101,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
protected:
- SizeEigen computeCalibSize() const;
+ virtual SizeEigen computeCalibSize() const;
private:
FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h
index 5115c2a1bab9a3430494842147bbc5b0a802e025..802ab7b49dabf93455f8f93fc8c30745415fe33e 100644
--- a/include/core/ceres_wrapper/ceres_manager.h
+++ b/include/core/ceres_wrapper/ceres_manager.h
@@ -84,6 +84,8 @@ class CeresManager : public SolverManager
void check();
+ const Eigen::SparseMatrixd computeHessian() const;
+
protected:
std::string solveImpl(const ReportVerbosity report_level) override;
diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index eb55faae757f7fe1b02d2fe29c75a049e0b04299..0743548b7b4aad0517ecdd2590955c4cdf7c023c 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/include/core/factor/factor_block_absolute.h
@@ -41,7 +41,7 @@ class FactorBlockAbsolute : public FactorAnalytic
ProcessorBasePtr _processor_ptr = nullptr,
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic("BLOCK ABS",
+ FactorAnalytic("FactorBlockAbsolute",
nullptr,
nullptr,
nullptr,
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
index 8f62b63838d79c6a654c80120bff4e4a144f4098..d3558ef21aa6e6577fc54d26015db3f9e3193312 100644
--- a/include/core/factor/factor_block_difference.h
+++ b/include/core/factor/factor_block_difference.h
@@ -37,8 +37,12 @@ class FactorBlockDifference : public FactorAnalytic
*
*/
FactorBlockDifference(
- StateBlockPtr _sb1_ptr,
- StateBlockPtr _sb2_ptr,
+ const StateBlockPtr& _sb1_ptr,
+ const StateBlockPtr& _sb2_ptr,
+ const FrameBasePtr& _frame_other = nullptr,
+ const CaptureBasePtr& _cap_other = nullptr,
+ const FeatureBasePtr& _feat_other = nullptr,
+ const LandmarkBasePtr& _lmk_other = nullptr,
unsigned int _start_idx1 = 0,
int _size1 = -1,
unsigned int _start_idx2 = 0,
@@ -46,11 +50,11 @@ class FactorBlockDifference : public FactorAnalytic
ProcessorBasePtr _processor_ptr = nullptr,
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic("BLOCK ABS",
- nullptr,
- nullptr,
- nullptr,
- nullptr,
+ FactorAnalytic("FactorBlockDifference",
+ _frame_other,
+ _cap_other,
+ _feat_other,
+ _lmk_other,
_processor_ptr,
_apply_loss_function,
_status,
@@ -146,10 +150,16 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
-
- // normalized jacobian
- _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
- _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
+
+ // normalized jacobian, computed according to the _compute_jacobian flag
+ if (_compute_jacobian[0]){
+ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+ }
+ if (_compute_jacobian[1]){
+ _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ }
}
inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
@@ -163,10 +173,16 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
-
- // normalized jacobian
- _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
- _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
+
+ // normalized jacobian, computed according to the _compute_jacobian flag
+ if (_compute_jacobian[0]){
+ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+ }
+ if (_compute_jacobian[1]){
+ _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ }
}
inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& _pure_jacobians) const
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 65f3b604de57df0d48c6e562191bb49f0960261d..f2b19ef05c22f7a4c6777f889031dbda45d80a5b 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -46,8 +46,7 @@ class Problem : public std::enable_shared_from_this<Problem>
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
- IsMotionPtr processor_motion_ptr_;
-// IsMotionPtr is_motion_ptr_;
+ std::list<IsMotionPtr> processor_is_motion_list_;
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
@@ -147,15 +146,15 @@ class Problem : public std::enable_shared_from_this<Problem>
protected:
/** \brief Set the processor motion
*
- * Set the processor motion.
+ * Add a new processor of type is motion to the processor is motion list.
*/
- void setProcessorMotion(IsMotionPtr _processor_motion_ptr);
- IsMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
- void clearProcessorMotion();
+ void addProcessorIsMotion(IsMotionPtr _processor_motion_ptr);
+ void clearProcessorIsMotion(IsMotionPtr proc);
public:
- IsMotionPtr& getProcessorMotion();
-
+ IsMotionPtr getProcessorIsMotion();
+ std::list<IsMotionPtr> getProcessorIsMotionList();
+
// Trajectory branch ----------------------------------
TrajectoryBasePtr getTrajectory() const;
virtual FrameBasePtr setPrior(const Eigen::VectorXd& _prior_state, //
@@ -271,10 +270,10 @@ class Problem : public std::enable_shared_from_this<Problem>
// Zero state provider
Eigen::VectorXd zeroState ( ) const;
bool priorIsSet() const;
+ void setPriorIsSet(bool _prior_is_set);
// Perturb state
void perturb(double amplitude = 0.01);
-
// Map branch -----------------------------------------
MapBasePtr getMap() const;
void loadMap(const std::string& _filename_dot_yaml);
@@ -367,9 +366,22 @@ inline bool Problem::priorIsSet() const
return prior_is_set_;
}
-inline IsMotionPtr& Problem::getProcessorMotion()
+inline void Problem::setPriorIsSet(bool _prior_is_set)
+{
+ prior_is_set_ = _prior_is_set;
+}
+
+inline IsMotionPtr Problem::getProcessorIsMotion()
+{
+ if (!processor_is_motion_list_.empty())
+ return processor_is_motion_list_.front();
+ return nullptr;
+}
+
+
+inline std::list<IsMotionPtr> Problem::getProcessorIsMotionList()
{
- return processor_motion_ptr_;
+ return processor_is_motion_list_;
}
inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificationMap()
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index f3cf0d781f39c9fda6ffb9e64817ceed0acfa162..8c99fe72e37698a2210e35a3af0c14bf9168f65e 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -43,6 +43,12 @@ class IsMotion
TimeStamp getCurrentTimeStamp() const;
Eigen::VectorXd getState(const TimeStamp& _ts) const;
+ std::string getStateStructure(){return state_structure_;};
+ void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;};
+
+ protected:
+ std::string state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
+
};
}
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index f3feeb56a3bcc6787d8efeb1355175b989dd3f3e..dc62abcac8375fc238f7c93617a33ccb8662d246 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -117,13 +117,18 @@ public:
*
* elements are ordered from most recent to oldest
*/
- std::map<TimeStamp,T> getContainer();
+ const std::map<TimeStamp,T>& getContainer();
/**\brief Remove all packs in the buffer with a time stamp older than the specified
*
*/
void removeUpTo(const TimeStamp& _time_stamp);
+ /**\brief Remove all packs in the buffer with a time stamp older than the specified
+ *
+ */
+ void removeUpToLower(const TimeStamp& _time_stamp);
+
/**\brief Clear the buffer
*
*/
@@ -503,7 +508,7 @@ void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element)
}
template <typename T>
-std::map<TimeStamp,T> Buffer<T>::getContainer()
+const std::map<TimeStamp,T>& Buffer<T>::getContainer()
{
return container_;
}
@@ -533,6 +538,13 @@ inline void Buffer<T>::removeUpTo(const TimeStamp& _time_stamp)
container_.erase(container_.begin(), post); // erasing by range
}
+template <typename T>
+inline void Buffer<T>::removeUpToLower(const TimeStamp& _time_stamp)
+{
+ Buffer::Iterator post = container_.lower_bound(_time_stamp);
+ container_.erase(container_.begin(), post); // erasing by range
+}
+
template <typename T>
inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1,
const TimeStamp& _time_stamp2, const double& _time_tolerance2)
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 2b8d4b7b86acc974d01b5864f2093a6d8e871a56..81a9184ad4798ccbaac1ebd61a194f6e287331b4 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -148,6 +148,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
// This is the main public interface
public:
ProcessorMotion(const std::string& _type,
+ std::string _state_structure,
int _dim,
SizeEigen _state_size,
SizeEigen _delta_size,
@@ -505,11 +506,12 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const
assert(origin_ptr_ && "Trying to access origin_ptr_ but it is nullptr!");
// ensure proper size of the provided reference
- _x.resize( getProblem()->getFrameStructureSize() );
+ Eigen::VectorXd curr_x = origin_ptr_->getFrame()->getState(state_structure_);
+ _x.resize( curr_x.size() );
// do get timestamp and state corrected by possible self-calibrations
double Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
- statePlusDelta(origin_ptr_->getFrame()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
+ statePlusDelta(curr_x, last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
}
inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index ef55d163be4764393c4e1b08ac0003ae67b1d223..97c570cb8550d5fb9aa6a2bf076e7e9674ab75c5 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -15,6 +15,10 @@
namespace wolf
{
+/// State of nodes containing several state blocks
+typedef std::unordered_map<std::string, Eigen::VectorXd> State;
+
+
class HasStateBlocks
{
public:
@@ -62,11 +66,13 @@ class HasStateBlocks
void removeStateBlocks(ProblemPtr _problem);
// States
- virtual void setState(const Eigen::VectorXd& _state, const bool _notify = true);
- Eigen::VectorXd getState() const;
- void getState(Eigen::VectorXd& _state) const;
- unsigned int getSize() const;
- unsigned int getLocalSize() const;
+ inline void setState(const Eigen::VectorXd& _state, std::string _sub_structure="", const bool _notify = true);
+ void getState(Eigen::VectorXd& _state, std::string structure="") const;
+ Eigen::VectorXd getState(std::string structure="") const;
+ unsigned int getSize(std::string _sub_structure="") const;
+ unsigned int getLocalSize(std::string _sub_structure="") const;
+
+ State getStateComposite();
// Perturb state
void perturb(double amplitude = 0.01);
@@ -198,60 +204,100 @@ inline bool HasStateBlocks::isFixed() const
return fixed;
}
-inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const bool _notify)
+inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string _sub_structure, const bool _notify)
{
- int size = getSize();
-
+ if (_sub_structure == ""){
+ _sub_structure = structure_;
+ }
+ int size = getSize(_sub_structure);
assert(_state.size() == size && "In FrameBase::setState wrong state size");
unsigned int index = 0;
- for (const char key : getStructure())
+ for (const char key : _sub_structure)
{
const auto& sb = getStateBlock(key);
-
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
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::VectorXd& _state) const
+// _sub_structure can be either stateblock structure of the node or a subset of this structure
+inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_structure) const
{
- _state.resize(getSize());
+ if (_sub_structure == ""){
+ _sub_structure = structure_;
+ }
+ _state.resize(getSize(_sub_structure));
unsigned int index = 0;
- for (const char key : getStructure())
+ for (const char key : _sub_structure)
{
const auto& sb = getStateBlock(key);
-
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
_state.segment(index,sb->getSize()) = sb->getState();
index += sb->getSize();
}
}
-inline Eigen::VectorXd HasStateBlocks::getState() const
+inline Eigen::VectorXd HasStateBlocks::getState(std::string _sub_structure) const
{
Eigen::VectorXd state;
- getState(state);
+ getState(state, _sub_structure);
+
+ return state;
+}
+inline State HasStateBlocks::getStateComposite()
+{
+ State state;
+ for (auto& pair_key_kf : state_block_map_)
+ {
+ state.emplace(pair_key_kf.first, pair_key_kf.second->getState());
+ }
return state;
}
-inline unsigned int HasStateBlocks::getSize() const
+inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const
{
+ if (_sub_structure == ""){
+ _sub_structure = structure_;
+ }
unsigned int size = 0;
- for (const auto& pair_key_sb : getStateBlockMap())
- size += pair_key_sb.second->getSize();
+ for (const char key : _sub_structure)
+ {
+ const auto& sb = getStateBlock(key);
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
+ size += sb->getSize();
+ }
+
return size;
}
-inline unsigned int HasStateBlocks::getLocalSize() const
+inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) const
{
+ if (_sub_structure == ""){
+ _sub_structure = structure_;
+ }
unsigned int size = 0;
- for (const auto& pair_key_sb : getStateBlockMap())
- size += pair_key_sb.second->getLocalSize();
+ for (const char key : _sub_structure)
+ {
+ const auto& sb = getStateBlock(key);
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
+ size += sb->getLocalSize();
+ }
+
return size;
}
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 33aa6fcb4b221ea1dc23f24b21f4a0fb12cbfc0d..ab3bb7147f3c81f57a3436216412274394acc04a 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -74,6 +74,10 @@ public:
**/
Eigen::VectorXd getState() const;
+ /** \brief Returns the state vector data array
+ **/
+ double* getStateData();
+
/** \brief Sets the state vector
**/
void setState(const Eigen::VectorXd& _state, const bool _notify = true);
@@ -283,6 +287,11 @@ inline void StateBlock::resetLocalParamUpdated()
local_param_updated_.store(false);
}
+inline double* StateBlock::getStateData()
+{
+ return state_.data();
+}
+
}// namespace wolf
#endif
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index ecf57e7d7e29b4b8ccfecf09f8c39d8eacdafdbb..0d8cba6f97cb7e2c2cdf067e7ebcb65d8ac79c10 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -443,6 +443,77 @@ void CeresManager::check()
}
}
+void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
+{
+ for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
+ for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
+ original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col);
+
+ original.makeCompressed();
+}
+
+const Eigen::SparseMatrixd CeresManager::computeHessian() const
+{
+ Eigen::SparseMatrixd H;
+ Eigen::SparseMatrixd A;
+ // fac_2_residual_idx_
+ // fac_2_costfunction_
+ // state_blocks_local_param_
+ int ix_row = 0; // index of the factor/measurement in the
+ for (auto fac_res_pair: fac_2_residual_idx_){
+ FactorBasePtr fac_ptr = fac_res_pair.first;
+ ix_row += fac_ptr->getSize();
+
+ // retrieve all stateblocks data related to this factor
+ std::vector<const double*> fac_states_ptr;
+ for (auto sb : fac_res_pair.first->getStateBlockPtrVector()){
+ fac_states_ptr.push_back(sb->getStateData());
+ }
+
+ // retrieve residual value, not used afterwards in this case since we just care about jacobians
+ Eigen::VectorXd residual(fac_ptr->getSize());
+ // retrieve jacobian in group size, not local size
+ std::vector<Eigen::MatrixXd> jacobians;
+ fac_ptr->evaluate(fac_states_ptr, residual, jacobians);
+
+ // Retrieve the block row sparse matrix of this factor
+ Eigen::SparseMatrixd A_block_row(fac_ptr->getSize(), A.cols());
+ for (auto i = 0; i < jacobians.size(); i++)
+ {
+ StateBlockPtr sb = fac_ptr->getStateBlockPtrVector()[i];
+ if (!sb->isFixed())
+ {
+ assert((state_blocks_local_param_.find(sb) != state_blocks_local_param_.end()) && "factor involving a state block not added");
+ assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols()) - 1 && "bad A number of cols");
+
+ // insert since A_block_row has just been created so it's empty for sure
+ if (sb->hasLocalParametrization()){
+ // if the state block has a local parameterization, we need to right multiply by the manifold element / tangent element jacobian
+ Eigen::MatrixXd J_manif_tang(sb->getSize(), sb->getLocalSize());
+ Eigen::Map<Eigen::MatrixRowXd> J_manif_tang_map(J_manif_tang.data(), sb->getSize(), sb->getLocalSize());
+ Eigen::Map<const Eigen::VectorXd> sb_state_map(sb->getStateData(), sb->getSize());
+
+ sb->getLocalParametrization()->computeJacobian(sb_state_map, J_manif_tang_map);
+ insertSparseBlock(jacobians[i] * J_manif_tang, A_block_row, 0, sb->getLocalSize()); // (to_insert, matrix_to_fill, row, col)
+ }
+ else {
+ insertSparseBlock(jacobians[i], A_block_row, 0, sb->getLocalSize());
+ }
+ }
+ }
+
+ // fill the weighted jacobian matrix block row
+ A.block(ix_row, 0, fac_ptr->getSize(), A.cols());
+
+ }
+
+ // compute the hessian matrix from the weighted jacobian
+ H = A.transpose() * A;
+
+ return H;
+}
+
+
} // namespace wolf
#include "core/solver/solver_factory.h"
namespace wolf {
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 85f94bd2a97af8a68e4b1551d3352daa8e8906d8..42a11b3782a5a669264471bcae49571ec1322840 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -64,14 +64,14 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
time_stamp_(_ts)
{
if(_frame_structure.compare("PO") == 0 and _dim == 2){
- assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2d!");
+ assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for PO 2D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((double) _x(2) ) );
addStateBlock("P", p_ptr);
addStateBlock("O", o_ptr);
this->setType("PO 2d");
}else if(_frame_structure.compare("PO") == 0 and _dim == 3){
- assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3d!");
+ assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for PO 3D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) );
addStateBlock("P", p_ptr);
@@ -79,7 +79,7 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
this->setType("PO 3d");
}else if(_frame_structure.compare("POV") == 0 and _dim == 3){
// auto _x = Eigen::VectorXd::Zero(10);
- assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for 3d!");
+ assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for POV 3D!");
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> ( ) ) );
@@ -87,6 +87,22 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
addStateBlock("O", o_ptr);
addStateBlock("V", v_ptr);
this->setType("POV 3d");
+ }else if(_frame_structure.compare("POVCDL") == 0 and _dim == 3){
+ // auto _x = Eigen::VectorXd::Zero(10);
+ assert(_x.size() == 19 && "Wrong state vector size. Should be 19 for POVCDL 3D!");
+ 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.segment <3> (7 ) ) );
+ StateBlockPtr c_ptr ( std::make_shared<StateBlock> (_x.segment <3> (10) ) );
+ StateBlockPtr cd_ptr ( std::make_shared<StateBlock> (_x.segment <3> (13) ) );
+ StateBlockPtr Lc_ptr ( std::make_shared<StateBlock> (_x.segment <3> (16) ) );
+ addStateBlock("P", p_ptr);
+ addStateBlock("O", o_ptr);
+ addStateBlock("V", v_ptr);
+ addStateBlock("C", c_ptr);
+ addStateBlock("D", cd_ptr);
+ addStateBlock("L", Lc_ptr);
+ this->setType("POVCDL 3d");
}else{
std::cout << _frame_structure << " ^^ " << _dim << std::endl;
throw std::runtime_error("Unknown frame structure");
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 8e74f8411fcad44e8c8fcc8fbfd62bf96e8d0f90..e79de9572f545e85142450497f3d4780621adc7b 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -35,25 +35,27 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
map_ptr_(std::make_shared<MapBase>()),
- processor_motion_ptr_(),
+ processor_is_motion_list_(std::list<IsMotionPtr>()),
prior_is_set_(false),
frame_structure_(_frame_structure)
{
+ dim_ = _dim;
if (_frame_structure == "PO" and _dim == 2)
{
state_size_ = 3;
state_cov_size_ = 3;
- dim_ = 2;
}else if (_frame_structure == "PO" and _dim == 3)
{
state_size_ = 7;
state_cov_size_ = 6;
- dim_ = 3;
- } else if (_frame_structure == "POV" and _dim == 3)
+ }else if (_frame_structure == "POV" and _dim == 3)
{
state_size_ = 10;
state_cov_size_ = 9;
- dim_ = 3;
+ }else if (_frame_structure == "POVCDL" and _dim == 3)
+ {
+ state_size_ = 19;
+ state_cov_size_ = 18;
}
else std::runtime_error(
"Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
@@ -226,7 +228,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
//Dimension check
int prc_dim = prc_ptr->getDim();
auto prb = this;
- assert((prc_dim == 0 or prc_dim == prb->getDim()) && "Processor and Problem do not agree on dimension");
+ assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
prc_ptr->configure(_corresponding_sensor_ptr);
prc_ptr->link(_corresponding_sensor_ptr);
@@ -235,10 +237,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
if (prc_ptr->isMotion() && prior_is_set_)
(std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
- // setting the main processor motion
- if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
- processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(prc_ptr);
-
return prc_ptr;
}
@@ -274,7 +272,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
//Dimension check
int prc_dim = prc_ptr->getDim();
auto prb = this;
- assert((prc_dim == 0 or prc_dim == prb->getDim()) && "Processor and Problem do not agree on dimension");
+ assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
prc_ptr->configure(sen_ptr);
prc_ptr->link(sen_ptr);
@@ -284,10 +282,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
if (prc_ptr->isMotion() && prior_is_set_)
(std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
- // setting the main processor motion
- if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
- processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr);
-
return prc_ptr;
}
@@ -306,47 +300,6 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
return (*sen_it);
}
-IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
-{
- for (auto sen : getHardware()->getSensorList()) // loop all sensors
- {
- auto prc_it = std::find_if(sen->getProcessorList().begin(), // find processor by its name
- sen->getProcessorList().end(),
- [&](ProcessorBasePtr prc)
- {
- return prc->getName() == _processor_name;
- }); // lambda function for the find_if
-
- if (prc_it != sen->getProcessorList().end()) // found something!
- {
- if ((*prc_it)->isMotion()) // found, and it's motion!
- {
- std::cout << "Found processor '" << _processor_name << "', of type Motion, and set as the main motion processor." << std::endl;
- processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(*prc_it);
- return processor_motion_ptr_;
- }
- else // found, but it's not motion!
- {
- std::cout << "Found processor '" << _processor_name << "', but not of type Motion!" << std::endl;
- return nullptr;
- }
- }
- }
- // nothing found!
- std::cout << "Processor '" << _processor_name << "' not found!" << std::endl;
- return nullptr;
-}
-
-void Problem::setProcessorMotion(IsMotionPtr _processor_motion_ptr)
-{
- processor_motion_ptr_ = _processor_motion_ptr;
-}
-
-void Problem::clearProcessorMotion()
-{
- processor_motion_ptr_.reset();
-}
-
FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
const SizeEigen _dim, //
FrameType _frame_key_type, //
@@ -387,41 +340,108 @@ Eigen::VectorXd Problem::getCurrentState() const
void Problem::getCurrentState(Eigen::VectorXd& _state) const
{
- if (processor_motion_ptr_ != nullptr)
- processor_motion_ptr_->getCurrentState(_state);
- else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
- trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state);
- else
- _state = zeroState();
+ TimeStamp ts; // throwaway timestamp
+ getCurrentStateAndStamp(_state, ts);
}
-void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& ts) const
-{
- if (processor_motion_ptr_ != nullptr)
+void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) const
+{
+ if (!processor_is_motion_list_.empty())
{
- processor_motion_ptr_->getCurrentState(_state);
- processor_motion_ptr_->getCurrentTimeStamp(ts);
+ // retrieve the minimum of the most recent ts in all processor is motion then call getSate(ts, state)
+ std::list<TimeStamp> proc_is_motion_current_ts;
+ for (auto proc: processor_is_motion_list_){
+ proc_is_motion_current_ts.push_back(proc->getCurrentTimeStamp());
+ }
+ auto min_it = std::min_element(proc_is_motion_current_ts.begin(), proc_is_motion_current_ts.end());
+ getState(*min_it, _state);
+ _ts = *min_it;
}
else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
{
- trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(ts);
+ // kind of redundant with getState(_ts, _state)
+ trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(_ts);
trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state);
}
else
_state = zeroState();
}
+
+// Problem of this implmentation: if more state
void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
{
- // try to get the state from processor_motion if any, otherwise...
- if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, _state))
- {
- FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+ // if _ts is too recent, for some of the processor is motion, they return the state corresponding to their last frame timestamp
+ FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+ if (processor_is_motion_list_.empty()){
if (closest_frame != nullptr)
- closest_frame->getState(_state);
+ _state = closest_frame->getState();
else
_state = zeroState();
}
+
+ // RETRIEVE FROM PROCESSOR MOTION
+ // TODO: current implementation messy, would be much easier with a state being an std::unordered_map
+ else {
+ // Iterate over the problem state structure and get the corresponding state
+ // in the first processor is motion that provides it
+ // finally check if the state to concatenate list has the same total size as the problem state_size
+
+ // an map is necessary as a temporary structure because we do not know in which order we will find the state blocks in processors
+ std::unordered_map<char, Eigen::VectorXd> states_to_concat_map; // not necessary to be ordered
+ for (auto proc: processor_is_motion_list_){
+ Eigen::VectorXd proc_state = proc->getState(_ts);
+
+ int idx = 0;
+ for (char sb_name: proc->getStateStructure()){
+ // not already there
+ if (states_to_concat_map.find(sb_name) == states_to_concat_map.end()){
+ if (sb_name == 'O'){
+ int size_sb = dim_ == 3 ? 4 : 1; // really bad: should be more transparent
+ states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
+ idx += size_sb;
+ }
+ else{
+ int size_sb = dim_ == 3 ? 3 : 2;
+ states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
+ idx += size_sb;
+ }
+ }
+ }
+ }
+
+ int concat_size = 0;
+ for (auto state_map_it: states_to_concat_map){
+ concat_size += state_map_it.second.size();
+ }
+ // assert(concat_size == state_size_ && "Problem with the concatenated size: " );
+
+ // fill the state value from the state concatenation in the order dictated by frame_structure_
+ int idx = 0;
+ _state.resize(state_size_);
+ for (char sb_name: frame_structure_){
+ Eigen::VectorXd sb_state;
+ int size_sb; // really bad...
+ if (sb_name == 'O'){
+ size_sb = dim_ == 3 ? 4 : 1;
+ }
+ else {
+ size_sb = dim_ == 3 ? 3 : 2;
+ }
+ if (states_to_concat_map.find(sb_name) != states_to_concat_map.end()){
+ sb_state = states_to_concat_map[sb_name];
+ }
+ else {
+ // Should be taken from the last state but too messy already
+ sb_state.resize(size_sb);
+ sb_state.setZero();
+ }
+
+ _state.segment(idx, size_sb) = sb_state;
+ idx += size_sb;
+
+ }
+ }
}
Eigen::VectorXd Problem::getState(const TimeStamp& _ts) const
@@ -451,6 +471,18 @@ std::string Problem::getFrameStructure() const
return frame_structure_;
}
+void Problem::addProcessorIsMotion(IsMotionPtr _processor_motion_ptr)
+{
+ processor_is_motion_list_.push_back(_processor_motion_ptr);
+}
+
+void Problem::clearProcessorIsMotion(IsMotionPtr proc){
+ auto it = std::find(processor_is_motion_list_.begin(), processor_is_motion_list_.end(), proc);
+ if (it != processor_is_motion_list_.end()){
+ processor_is_motion_list_.erase(it);
+ }
+}
+
Eigen::VectorXd Problem::zeroState() const
{
Eigen::VectorXd state = Eigen::VectorXd::Zero(getFrameStructureSize());
@@ -846,10 +878,16 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXd& _prior_state, const Eigen:
// create origin capture with the given state as data
// Capture fix only takes 3d position and Quaternion orientation
CapturePosePtr init_capture;
- if (this->getFrameStructure() == "POV" and this->getDim() == 3)
+ // if (this->getFrameStructure() == "POV" and this->getDim() == 3)
+ // init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
+ // else
+ // init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
+
+ if (this->getDim() == 3)
init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
else
- init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
+ init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(3), _prior_cov.topLeftCorner(3,3));
+
// emplace feature and factor
init_capture->emplaceFeatureAndFactor();
@@ -1003,6 +1041,15 @@ void Problem::print(int depth, std::ostream& stream, bool constr_by, bool metric
}
stream << std::endl;
}
+ // if (state_blocks)
+ // {
+ // cout << " sb:";
+ // for (const auto& sb : F->getStateBlockVec())
+ // {
+ // cout << " " << (sb->isFixed() ? "Fix" : "Est");
+ // }
+ // cout << endl;
+ // }
if (depth >= 2)
{
// Captures =======================================================================================
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index a35097f2c70d3b047117ae6d0c9bf81a5fe2f5da..f61f861b021e51a08dd92db301aa1bd8de159272 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -53,12 +53,13 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture");
WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
- // if trigger, process directly without buffering
- if (triggerInCapture(_capture_ptr))
- processCapture(_capture_ptr);
// asking if capture should be stored
if (storeCapture(_capture_ptr))
buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr);
+
+ // if trigger, process directly without buffering
+ if (triggerInCapture(_capture_ptr))
+ processCapture(_capture_ptr);
}
void ProcessorBase::remove()
@@ -68,12 +69,12 @@ void ProcessorBase::remove()
is_removing_ = true;
ProcessorBasePtr this_p = shared_from_this();
- // clear Problem::processor_motion_ptr_
if (isMotion())
{
ProblemPtr P = getProblem();
- if(P && (P->getProcessorMotion() == std::dynamic_pointer_cast<IsMotion>( shared_from_this() ) ) )
- P->clearProcessorMotion();
+ auto this_proc_cast_attempt = std::dynamic_pointer_cast<IsMotion>( shared_from_this() );
+ if(P && this_proc_cast_attempt )
+ P->clearProcessorIsMotion(this_proc_cast_attempt);
}
// remove from upstream
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 634d1f6d1284997573ef763bd329dcdf37cc94d3..aded1532cc17fc6a0f2e0d48edbbca857a1ab28b 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -7,6 +7,7 @@ namespace wolf
{
ProcessorMotion::ProcessorMotion(const std::string& _type,
+ std::string _state_structure,
int _dim,
SizeEigen _state_size,
SizeEigen _delta_size,
@@ -25,7 +26,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
origin_ptr_(),
last_ptr_(),
incoming_ptr_(),
- dt_(0.0), x_(_state_size),
+ dt_(0.0),
+ x_(_state_size),
delta_(_delta_size),
delta_cov_(_delta_cov_size, _delta_cov_size),
delta_integrated_(_delta_size),
@@ -34,7 +36,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
jacobian_delta_(delta_cov_size_, delta_cov_size_),
jacobian_calib_(delta_cov_size_, calib_size_)
-{
+{
+ setStateStructure(_state_structure);
//
}
@@ -267,7 +270,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Update state and time stamps
last_ptr_->setTimeStamp(getCurrentTimeStamp());
last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp());
- last_ptr_->getFrame()->setState(getCurrentState());
+ last_ptr_->getFrame()->setState(getProblem()->getState(getCurrentTimeStamp()));
if (permittedKeyFrame() && voteForKeyFrame())
{
@@ -315,7 +318,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// create a new frame
auto frame_new = getProblem()->emplaceFrame(NON_ESTIMATED,
- getCurrentState(),
+ getProblem()->getCurrentState(),
getCurrentTimeStamp());
// create a new capture
auto capture_new = emplaceCapture(frame_new,
@@ -346,6 +349,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
postProcess();
}
+// _x needs to have the size of the processor state
bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
{
CaptureMotionPtr capture_motion;
@@ -360,7 +364,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
{
// Get origin state and calibration
CaptureBasePtr cap_orig = capture_motion->getOriginCapture();
- VectorXd state_0 = cap_orig->getFrame()->getState();
+ VectorXd state_0 = cap_orig->getFrame()->getState(state_structure_);
VectorXd calib = cap_orig->getCalibration();
// Get delta and correct it with new calibration params
@@ -371,7 +375,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step);
// ensure proper size of the provided reference
- _x.resize( getProblem()->getFrameStructureSize() );
+ _x.resize( state_0.size() );
// Compose on top of origin state using the buffered time stamp, not the query time stamp
double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_;
@@ -380,7 +384,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
else
{
// We could not find any CaptureMotion for the time stamp requested
- WOLF_ERROR("Could not find any Capture for the time stamp requested. ");
+ WOLF_ERROR("Could not find any Capture for the time stamp requested. ", _ts);
WOLF_TRACE("Did you forget to call Problem::setPrior() in your application?")
return false;
}
@@ -415,9 +419,11 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- LAST ----------
// Make non-key-frame for last Capture
+ TimeStamp origin_ts = _origin_frame->getTimeStamp();
auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
- _origin_frame->getState(),
- _origin_frame->getTimeStamp());
+ _origin_frame->getState(),
+ origin_ts);
+
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(new_frame_ptr,
getSensor(),
@@ -562,7 +568,8 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
++frame_rev_iter)
{
frame = *frame_rev_iter;
- capture = frame->getCaptureOf(getSensor());
+ auto sensor = getSensor();
+ capture = frame->getCaptureOf(sensor);
if (capture != nullptr)
{
// Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
@@ -621,6 +628,9 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
void ProcessorMotion::setProblem(ProblemPtr _problem)
{
+ std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D";
+ assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str());
+
if (_problem == nullptr or _problem == this->getProblem())
return;
@@ -629,11 +639,10 @@ void ProcessorMotion::setProblem(ProblemPtr _problem)
// set the origin
if (origin_ptr_ == nullptr && this->getProblem()->getLastKeyFrame() != nullptr)
this->setOrigin(this->getProblem()->getLastKeyFrame());
-
- // set the main processor motion
- if (this->getProblem()->getProcessorMotion() == nullptr)
- this->getProblem()->setProcessorMotion(std::static_pointer_cast<ProcessorMotion>(shared_from_this()));
-};
+
+ // adding processor is motion to the processor is motion vector
+ getProblem()->addProcessorIsMotion(std::dynamic_pointer_cast<IsMotion>(shared_from_this()));
+}
bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr)
{
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index c986ebf7cf9cd7fb83df1417ea19c42beed66c74..929c1ed7ceb825fdaa265a1af592fa8125832789 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -6,7 +6,7 @@ namespace wolf
{
ProcessorOdom2d::ProcessorOdom2d(ProcessorParamsOdom2dPtr _params) :
- ProcessorMotion("ProcessorOdom2d", 2, 3, 3, 3, 2, 0, _params),
+ ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params),
params_odom_2d_(_params)
{
unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3d::Identity();
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 967031e9ccd1a68fcee41567be0f7dbf7523ef2c..3e1268eb1d705b3a335d9c8d9389e0c9449ebba1 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -3,7 +3,7 @@ namespace wolf
{
ProcessorOdom3d::ProcessorOdom3d(ProcessorParamsOdom3dPtr _params) :
- ProcessorMotion("ProcessorOdom3d", 3, 7, 7, 6, 6, 0, _params),
+ ProcessorMotion("ProcessorOdom3d", "PO", 3, 7, 7, 6, 6, 0, _params),
params_odom_3d_ (_params),
k_disp_to_disp_ (0),
k_disp_to_rot_ (0),
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 20bbe2c42ed27688820569a3da345596da3903f1..1e06c7a80377b99197a721e85473319f9bf34862 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -50,18 +50,17 @@ class FixtureFactorBlockDifference : public testing::Test
Vector10d x_origin = problem_->zeroState();
Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
- KF0_ =problem_->setPrior(x_origin, cov_prior, t0, 0.1);
+ KF0_ = problem_->setPrior(x_origin, cov_prior, t0, 0.1);
CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
- // KF0_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t0);
KF1_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t1);
- Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "DIFF", t1);
+ Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
Eigen::Matrix3d cov = 0.2 * Eigen::Matrix3d::Identity();
- Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "DIFF", zero3, cov);
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, cov);
}
virtual void TearDown() override {}
@@ -150,6 +149,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPx)
Feat_->setMeasurementCovariance(cov_diff);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
0, 1, 0, 1
);
@@ -174,6 +174,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPxy)
Feat_->setMeasurementCovariance(cov_diff);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
0, 2, 0, 2
);
@@ -195,6 +196,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz)
Feat_->setMeasurementCovariance(cov_diff);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
1, 2, 1, 2
);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index e0ba9a42a17936b62ec329c1fb022052d5ae6c30..ae7e14ae893449e8c42df4736eae792dfbd4b20d 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -94,7 +94,7 @@ TEST(Problem, Processor)
ProblemPtr P = Problem::create("PO", 3);
// check motion processor is null
- ASSERT_FALSE(P->getProcessorMotion());
+ ASSERT_FALSE(P->getProcessorIsMotion());
// add a motion sensor and processor
auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), ParamsSensorOdom3d());
@@ -103,7 +103,7 @@ TEST(Problem, Processor)
auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ProcessorParamsOdom3d>());
// check motion processor IS NOT by emplace
- ASSERT_EQ(P->getProcessorMotion(), Pm);
+ ASSERT_EQ(P->getProcessorIsMotion(), Pm);
}
TEST(Problem, Installers)
@@ -118,16 +118,16 @@ TEST(Problem, Installers)
auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer");
// check motion processor IS NOT set
- ASSERT_FALSE(P->getProcessorMotion());
+ ASSERT_FALSE(P->getProcessorIsMotion());
// install processor motion
ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
// check motion processor is set
- ASSERT_TRUE(P->getProcessorMotion());
+ ASSERT_TRUE(P->getProcessorIsMotion() != nullptr);
// check motion processor is correct
- ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorMotion()), pm);
+ ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorIsMotion()), pm);
}
TEST(Problem, SetOrigin_PO_2d)
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 1933da8599234e61c534e9f05008710ac4396661..c4a1c4b0470c92bf09df1a950a1c19e57d35642d 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -125,7 +125,7 @@ TEST(ProcessorBase, KeyFrameCallback)
capt_trk = make_shared<CaptureVoid>(t, sens_trk);
proc_trk->captureCallback(capt_trk);
-// problem->print(4,1,1,0);
+ problem->print(4,1,1,0);
// Only odom creating KFs
ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2d")==0 );