diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 87fffa2d3158d1b8dbd8844a362a6c164a093acb..b946f35b770aabb02f7f237933221131b30afdbc 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -48,9 +48,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
private:
FrameBaseWPtr frame_ptr_;
FeatureBasePtrList feature_list_;
- FeatureBaseConstPtrList feature_const_list_;
FactorBasePtrList constrained_by_list_;
- FactorBaseConstPtrList constrained_by_const_list_;
SensorBaseWPtr sensor_ptr_; ///< Pointer to sensor
static unsigned int capture_id_count_;
@@ -225,7 +223,10 @@ inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
inline FeatureBaseConstPtrList CaptureBase::getFeatureList() const
{
- return feature_const_list_;
+ FeatureBaseConstPtrList list_const;
+ for (auto&& obj_ptr : feature_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
}
inline FeatureBasePtrList CaptureBase::getFeatureList()
@@ -240,7 +241,10 @@ inline unsigned int CaptureBase::getHits() const
inline FactorBaseConstPtrList CaptureBase::getConstrainedByList() const
{
- return constrained_by_const_list_;
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : constrained_by_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
}
inline FactorBasePtrList CaptureBase::getConstrainedByList()
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 3be53aee698ecda49d198f8164c829cf368c2942..1894148de2ea576614b14d6de81a4f138c4e037c 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -86,11 +86,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list
FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list
LandmarkBaseWPtrList landmark_other_list_; ///< LandmarkBase pointer list
-
- FrameBaseConstWPtrList frame_other_const_list_; ///< FrameBase pointer list
- CaptureBaseConstWPtrList capture_other_const_list_; ///< CaptureBase pointer list
- FeatureBaseConstWPtrList feature_other_const_list_; ///< FeatureBase pointer list
- LandmarkBaseConstWPtrList landmark_other_const_list_; ///< LandmarkBase pointer list
protected:
unsigned int factor_id_;
@@ -218,10 +213,10 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
CaptureBaseWPtrList getCaptureOtherList() { return capture_other_list_; }
FeatureBaseWPtrList getFeatureOtherList() { return feature_other_list_; }
LandmarkBaseWPtrList getLandmarkOtherList() { return landmark_other_list_; }
- FrameBaseConstWPtrList getFrameOtherList() const { return frame_other_const_list_; }
- CaptureBaseConstWPtrList getCaptureOtherList() const { return capture_other_const_list_; }
- FeatureBaseConstWPtrList getFeatureOtherList() const { return feature_other_const_list_; }
- LandmarkBaseConstWPtrList getLandmarkOtherList() const { return landmark_other_const_list_; }
+ FrameBaseConstWPtrList getFrameOtherList() const;
+ CaptureBaseConstWPtrList getCaptureOtherList() const;
+ FeatureBaseConstWPtrList getFeatureOtherList() const;
+ LandmarkBaseConstWPtrList getLandmarkOtherList() const;
bool hasFrameOther(const FrameBaseConstPtr& _frm_other) const;
bool hasCaptureOther(const CaptureBaseConstPtr& _cap_other) const;
@@ -401,5 +396,37 @@ inline const Eigen::MatrixXd& FactorBase::getMeasurementSquareRootInformationUpp
return measurement_sqrt_information_upper_;
}
+inline FrameBaseConstWPtrList FactorBase::getFrameOtherList() const
+{
+ FrameBaseConstWPtrList list_const;
+ for (auto&& obj_ptr : frame_other_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline CaptureBaseConstWPtrList FactorBase::getCaptureOtherList() const
+{
+ CaptureBaseConstWPtrList list_const;
+ for (auto&& obj_ptr : capture_other_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FeatureBaseConstWPtrList FactorBase::getFeatureOtherList() const
+{
+ FeatureBaseConstWPtrList list_const;
+ for (auto&& obj_ptr : feature_other_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline LandmarkBaseConstWPtrList FactorBase::getLandmarkOtherList() const
+{
+ LandmarkBaseConstWPtrList list_const;
+ for (auto&& obj_ptr : landmark_other_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
} // namespace wolf
#endif
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 235a3dc70e50174e99e974ca1e6249b044f2184a..beb5eae22b8667750d43c2a51ade0a24857c6483 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -46,9 +46,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
private:
CaptureBaseWPtr capture_ptr_;
FactorBasePtrList factor_list_;
- FactorBaseConstPtrList factor_const_list_;
FactorBasePtrList constrained_by_list_;
- FactorBaseConstPtrList constrained_by_const_list_;
static unsigned int feature_id_count_;
@@ -186,7 +184,10 @@ inline unsigned int FeatureBase::getHits() const
inline FactorBaseConstPtrList FeatureBase::getFactorList() const
{
- return factor_const_list_;
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : factor_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
}
inline FactorBasePtrList FeatureBase::getFactorList()
@@ -196,7 +197,10 @@ inline FactorBasePtrList FeatureBase::getFactorList()
inline FactorBaseConstPtrList FeatureBase::getConstrainedByList() const
{
- return constrained_by_const_list_;
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : constrained_by_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
}
inline FactorBasePtrList FeatureBase::getConstrainedByList()
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 86ab564e383270fc517e41876c306c4bd826c908..8d426eacab111c81b435e48e630d39a802e02a6f 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -51,9 +51,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
private:
TrajectoryBaseWPtr trajectory_ptr_;
CaptureBasePtrList capture_list_;
- CaptureBaseConstPtrList capture_const_list_;
FactorBasePtrList constrained_by_list_;
- FactorBaseConstPtrList constrained_by_const_list_;
static unsigned int frame_id_count_;
@@ -257,7 +255,10 @@ inline TrajectoryBasePtr FrameBase::getTrajectory()
inline CaptureBaseConstPtrList FrameBase::getCaptureList() const
{
- return capture_const_list_;
+ CaptureBaseConstPtrList list_const;
+ for (auto&& obj_ptr : capture_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
}
inline CaptureBasePtrList FrameBase::getCaptureList()
@@ -272,7 +273,10 @@ inline unsigned int FrameBase::getHits() const
inline FactorBaseConstPtrList FrameBase::getConstrainedByList() const
{
- return constrained_by_const_list_;
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : constrained_by_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
}
inline FactorBasePtrList FrameBase::getConstrainedByList()
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index ea0a977a39492dd6430e0c24c7efd32441f19f14..4e86c8892af30795657610cf5576f724a7c792d3 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -41,7 +41,6 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
private:
SensorBasePtrList sensor_list_;
- SensorBaseConstPtrList sensor_const_list_;
public:
HardwareBase();
@@ -84,7 +83,10 @@ namespace wolf {
inline SensorBaseConstPtrList HardwareBase::getSensorList() const
{
- return sensor_const_list_;
+ SensorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : sensor_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
}
inline SensorBasePtrList HardwareBase::getSensorList()
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 34aefb07e97459bfd67b04c6822075469d900270..1d08d5748404c4bbe6151c8e28cf6827dc79f183 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -49,9 +49,8 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
private:
MapBaseWPtr map_ptr_;
FactorBasePtrList constrained_by_list_;
- FactorBaseConstPtrList constrained_by_const_list_;
- std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
- std::vector<StateBlockConstPtr> state_block_const_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
+ //std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
+ //std::vector<StateBlockConstPtr> state_block_const_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
static unsigned int landmark_id_count_;
@@ -82,8 +81,8 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
void setId(unsigned int _id);
// State blocks
- std::vector<StateBlockConstPtr> getUsedStateBlockVec() const;
- std::vector<StateBlockPtr> getUsedStateBlockVec();
+ //std::vector<StateBlockConstPtr> getUsedStateBlockVec() const;
+ //std::vector<StateBlockPtr> getUsedStateBlockVec();
bool getCovariance(Eigen::MatrixXd& _cov) const;
// Descriptor
@@ -184,7 +183,10 @@ inline unsigned int LandmarkBase::getHits() const
inline FactorBaseConstPtrList LandmarkBase::getConstrainedByList() const
{
- return constrained_by_const_list_;
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : constrained_by_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
}
inline FactorBasePtrList LandmarkBase::getConstrainedByList()
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index ae680c3e726430cd10eb03da148a4ece54b61610..af09f86002c697ad026ad9942d218026808555c4 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -98,7 +98,6 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
private:
LandmarkBasePtrList landmark_list_;
- LandmarkBaseConstPtrList landmark_const_list_;
public:
MapBase();
@@ -140,7 +139,10 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
inline LandmarkBaseConstPtrList MapBase::getLandmarkList() const
{
- return landmark_const_list_;
+ LandmarkBaseConstPtrList list_const;
+ for (auto&& obj_ptr : landmark_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
}
inline LandmarkBasePtrList MapBase::getLandmarkList()
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index a4bcb46858d50726591b7c5e49ae060b0b6892af..3ded670afcb492449f622cd6b2fbdd6ff532d3f4 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -79,7 +79,6 @@ class Problem : public std::enable_shared_from_this<Problem>
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
std::map<int, MotionProviderPtr> motion_provider_map_;
- std::map<int, MotionProviderConstPtr> motion_provider_const_map_;
std::map<std::pair<StateBlockConstPtr, StateBlockConstPtr>, Eigen::MatrixXd> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index df548a3a55d0d46138ff425a6cf7e281cedd86fb..8e6c6db7c6657037e7df0eddeea9b683635e5b71 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -108,7 +108,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
private:
HardwareBaseWPtr hardware_ptr_;
ProcessorBasePtrList processor_list_;
- ProcessorBaseConstPtrList processor_const_list_;
static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory)
@@ -316,7 +315,10 @@ inline unsigned int SensorBase::id() const
inline ProcessorBaseConstPtrList SensorBase::getProcessorList() const
{
- return processor_const_list_;
+ ProcessorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : processor_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
}
inline ProcessorBasePtrList SensorBase::getProcessorList()
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 228798693b187bc148092faa82be3fea8da4866b..21b1ab5be7080da9334a979274d5f4513f9a23f0 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -43,7 +43,6 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
private:
FramePtrMap frame_map_;
- FrameConstPtrMap frame_const_map_;
protected:
StateStructure frame_structure_; // Defines the structure of the Frames in the Trajectory.
@@ -97,7 +96,10 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
inline FrameConstPtrMap TrajectoryBase::getFrameMap() const
{
- return frame_const_map_;
+ FrameConstPtrMap map_const;
+ for (auto&& pair : frame_map_)
+ map_const[pair.first] = pair.second;
+ return map_const;
}
inline FramePtrMap TrajectoryBase::getFrameMap()
@@ -107,9 +109,9 @@ inline FramePtrMap TrajectoryBase::getFrameMap()
inline FrameBaseConstPtr TrajectoryBase::getFirstFrame() const
{
- if (frame_const_map_.empty())
+ if (frame_map_.empty())
return nullptr;
- return frame_const_map_.begin()->second;
+ return frame_map_.begin()->second;
}
inline FrameBasePtr TrajectoryBase::getFirstFrame()
@@ -121,9 +123,9 @@ inline FrameBasePtr TrajectoryBase::getFirstFrame()
inline FrameBaseConstPtr TrajectoryBase::getLastFrame() const
{
- if (frame_const_map_.empty())
+ if (frame_map_.empty())
return nullptr;
- return frame_const_map_.rbegin()->second;
+ return frame_map_.rbegin()->second;
}
inline FrameBasePtr TrajectoryBase::getLastFrame()
@@ -135,22 +137,22 @@ inline FrameBasePtr TrajectoryBase::getLastFrame()
inline SizeEigen TrajectoryBase::size() const
{
- return frame_const_map_.size();
+ return frame_map_.size();
}
inline bool TrajectoryBase::hasTimeStamp(const TimeStamp& _ts) const
{
- return frame_const_map_.count(_ts) != 0;
+ return frame_map_.count(_ts) != 0;
}
inline bool TrajectoryBase::hasFrame(FrameBaseConstPtr _frame) const
{
- return std::find_if(frame_const_map_.begin(),
- frame_const_map_.end(),
+ return std::find_if(frame_map_.begin(),
+ frame_map_.end(),
[&_frame](std::pair<TimeStamp,FrameBaseConstPtr> frm_it)
{
return frm_it.second == _frame;
- }) != frame_const_map_.end();
+ }) != frame_map_.end();
}
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 011a93484b21935183a83fd3319a1a4eb814ae56..e6d4a1e660bd0664f2fa1196c2ab558840664263 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -133,14 +133,12 @@ FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
{
//std::cout << "Adding feature" << std::endl;
feature_list_.push_back(_ft_ptr);
- feature_const_list_.push_back(_ft_ptr);
return _ft_ptr;
}
void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr)
{
feature_list_.remove(_ft_ptr);
- feature_const_list_.remove(_ft_ptr);
}
FactorBaseConstPtrList CaptureBase::getFactorList() const
@@ -172,28 +170,26 @@ void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- constrained_by_const_list_.push_back(_fac_ptr);
return _fac_ptr;
}
void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.remove(_fac_ptr);
- constrained_by_const_list_.remove(_fac_ptr);
}
bool CaptureBase::hasFeature(const FeatureBaseConstPtr &_feature) const
{
- return std::find(feature_const_list_.begin(),
- feature_const_list_.end(),
- _feature) != feature_const_list_.end();
+ return std::find(feature_list_.begin(),
+ feature_list_.end(),
+ _feature) != feature_list_.end();
}
bool CaptureBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
{
- return std::find(constrained_by_const_list_.begin(),
- constrained_by_const_list_.end(),
- _factor) != constrained_by_const_list_.end();
+ return std::find(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ _factor) != constrained_by_list_.end();
}
StateBlockConstPtr CaptureBase::getStateBlock(const char& _key) const
@@ -405,8 +401,6 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBaseConstPtr _cap_ptr, st
}
}
}
- inconsistency_explanation << "constrained_by_list and constrained_by_const_list not equal\n";
- log.assertTrue(constrained_by_const_list_.size() == constrained_by_list_.size(), inconsistency_explanation);
// check frame
auto frm_cap = _cap_ptr->getFrame();
@@ -426,8 +420,6 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBaseConstPtr _cap_ptr, st
<< " -X-> Cap" << id();
log.assertTrue((f->getCapture() == _cap_ptr), inconsistency_explanation);
}
- inconsistency_explanation << "feature_list and feature_const_list not equal\n";
- log.assertTrue(feature_list_.size() == feature_const_list_.size(), inconsistency_explanation);
//Check that the capture is within time tolerance of some processor
auto frame = getFrame();
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index b9fa97ae64b0ab1be8f38c9498852eb91366a751..c83686ef4100dfa4cee61e576f99fa0759a1e94a 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -127,10 +127,12 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
// landmark state blocks
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
- {
- landmark_state_blocks = l_ptr->getUsedStateBlockVec();
- all_state_blocks.insert(all_state_blocks.end(), landmark_state_blocks.begin(), landmark_state_blocks.end());
- }
+ for (auto key : l_ptr->getStructure())
+ {
+ const auto& sb = l_ptr->getStateBlock(key);
+ all_state_blocks.push_back(sb);
+ }
+
// double loop all against all (without repetitions)
for (unsigned int i = 0; i < all_state_blocks.size(); i++)
{
@@ -150,12 +152,12 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
{
// first create a vector containing all state blocks
for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
- for (auto key1 : wolf_problem_->getFrameStructure())
+ for (auto key1 : fr_pair.second->getStructure())
{
const auto& sb1 = fr_pair.second->getStateBlock(key1);
assert(isStateBlockRegisteredDerived(sb1));
- for (auto key2 : wolf_problem_->getFrameStructure())
+ for (auto key2 : fr_pair.second->getStructure())
{
const auto& sb2 = fr_pair.second->getStateBlock(key2);
assert(isStateBlockRegisteredDerived(sb2));
@@ -169,18 +171,23 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
// landmark state blocks
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
- for (auto sb : l_ptr->getUsedStateBlockVec())
+ for (auto key1 : l_ptr->getStructure())
{
- assert(isStateBlockRegisteredDerived(sb));
- for(auto sb2 : l_ptr->getUsedStateBlockVec())
+ const auto& sb1 = l_ptr->getStateBlock(key1);
+ assert(isStateBlockRegisteredDerived(sb1));
+
+ for (auto key2 : l_ptr->getStructure())
{
+ const auto& sb2 = l_ptr->getStateBlock(key2);
assert(isStateBlockRegisteredDerived(sb2));
- state_block_pairs.emplace_back(sb, sb2);
- double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
- if (sb == sb2)
+
+ state_block_pairs.emplace_back(sb1, sb2);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
+ if (sb1 == sb2)
break;
}
}
+
break;
}
case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
@@ -203,35 +210,33 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
getAssociatedMemBlockPtr(last_key_frame->getO()));
// landmarks
- std::vector<StateBlockPtr> landmark_state_blocks;
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
- {
- // load state blocks vector
- landmark_state_blocks = l_ptr->getUsedStateBlockVec();
-
- for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
+ for (auto key : l_ptr->getStructure())
{
- assert(isStateBlockRegisteredDerived(*state_it));
+ const auto& sb = l_ptr->getStateBlock(key);
+ assert(isStateBlockRegisteredDerived(sb));
// robot - landmark
- state_block_pairs.emplace_back(last_key_frame->getP(), *state_it);
- state_block_pairs.emplace_back(last_key_frame->getO(), *state_it);
+ state_block_pairs.emplace_back(last_key_frame->getP(), sb);
+ state_block_pairs.emplace_back(last_key_frame->getO(), sb);
double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getP()),
- getAssociatedMemBlockPtr((*state_it)));
+ getAssociatedMemBlockPtr(sb));
double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getO()),
- getAssociatedMemBlockPtr((*state_it)));
+ getAssociatedMemBlockPtr(sb));
// landmark marginal
- for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++)
+ for (auto key2 : l_ptr->getStructure())
{
- assert(isStateBlockRegisteredDerived(*next_state_it));
+ const auto& sb2 = l_ptr->getStateBlock(key2);
+ assert(isStateBlockRegisteredDerived(sb2));
- state_block_pairs.emplace_back(*state_it, *next_state_it);
- double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
- getAssociatedMemBlockPtr((*next_state_it)));
+ state_block_pairs.emplace_back(sb, sb2);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
+ if (sb == sb2)
+ break;
}
}
- }
+
break;
}
case CovarianceBlocksToBeComputed::GAUSS:
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index faecad96aa27a0c1994a85ac9548fa08dde23426..9ac1675b8b9ddc388f85b7fd6d8d7e34a274e65b 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -44,35 +44,19 @@ FactorBase::FactorBase(const std::string& _tp,
capture_other_list_(),
feature_other_list_(),
landmark_other_list_(),
- frame_other_const_list_(),
- capture_other_const_list_(),
- feature_other_const_list_(),
- landmark_other_const_list_(),
factor_id_(++factor_id_count_),
topology_(_top),
status_(_status),
apply_loss_function_(_apply_loss_function)
{
for (auto Fo : _frame_other_list)
- {
frame_other_list_.push_back(Fo);
- frame_other_const_list_.push_back(Fo);
- }
for (auto Co : _capture_other_list)
- {
capture_other_list_.push_back(Co);
- capture_other_const_list_.push_back(Co);
- }
for (auto fo : _feature_other_list)
- {
feature_other_list_.push_back(fo);
- feature_other_const_list_.push_back(fo);
- }
for (auto Lo : _landmark_other_list)
- {
landmark_other_list_.push_back(Lo);
- landmark_other_const_list_.push_back(Lo);
- }
assert(_feature_ptr && "null feature pointer when creating a factor");
measurement_ = _feature_ptr->getMeasurement();
@@ -212,13 +196,13 @@ void FactorBase::setStatus(FactorStatus _status)
bool FactorBase::hasFrameOther(const FrameBaseConstPtr &_frm_other) const
{
- auto frm_it = find_if(frame_other_const_list_.begin(),
- frame_other_const_list_.end(),
+ auto frm_it = find_if(frame_other_list_.begin(),
+ frame_other_list_.end(),
[_frm_other](const FrameBaseConstWPtr &frm_ow)
{
return frm_ow.lock() == _frm_other;
});
- if (frm_it != frame_other_const_list_.end())
+ if (frm_it != frame_other_list_.end())
return true;
return false;
@@ -226,13 +210,13 @@ bool FactorBase::hasFrameOther(const FrameBaseConstPtr &_frm_other) const
bool FactorBase::hasCaptureOther(const CaptureBaseConstPtr &_cap_other) const
{
- auto cap_it = find_if(capture_other_const_list_.begin(),
- capture_other_const_list_.end(),
+ auto cap_it = find_if(capture_other_list_.begin(),
+ capture_other_list_.end(),
[_cap_other](const CaptureBaseConstWPtr &cap_ow)
{
return cap_ow.lock() == _cap_other;
});
- if (cap_it != capture_other_const_list_.end())
+ if (cap_it != capture_other_list_.end())
return true;
return false;
@@ -240,13 +224,13 @@ bool FactorBase::hasCaptureOther(const CaptureBaseConstPtr &_cap_other) const
bool FactorBase::hasFeatureOther(const FeatureBaseConstPtr &_ftr_other) const
{
- auto ftr_it = find_if(feature_other_const_list_.begin(),
- feature_other_const_list_.end(),
+ auto ftr_it = find_if(feature_other_list_.begin(),
+ feature_other_list_.end(),
[_ftr_other](const FeatureBaseConstWPtr &ftr_ow)
{
return ftr_ow.lock() == _ftr_other;
});
- if (ftr_it != feature_other_const_list_.end())
+ if (ftr_it != feature_other_list_.end())
return true;
return false;
@@ -254,13 +238,13 @@ bool FactorBase::hasFeatureOther(const FeatureBaseConstPtr &_ftr_other) const
bool FactorBase::hasLandmarkOther(const LandmarkBaseConstPtr &_lmk_other) const
{
- auto lmk_it = find_if(landmark_other_const_list_.begin(),
- landmark_other_const_list_.end(),
+ auto lmk_it = find_if(landmark_other_list_.begin(),
+ landmark_other_list_.end(),
[_lmk_other](const LandmarkBaseConstWPtr &lmk_ow)
{
return lmk_ow.lock() == _lmk_other;
});
- if (lmk_it != landmark_other_const_list_.end())
+ if (lmk_it != landmark_other_list_.end())
return true;
return false;
@@ -466,16 +450,6 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
if (_verbose)
_stream << std::endl;
- // const pointer and pointer lists consistency
- inconsistency_explanation << "The frame_other_const_list_ and frame_other_list_ are different\n";
- log.assertTrue(frame_other_const_list_.size() == frame_other_list_.size(), inconsistency_explanation);
- inconsistency_explanation << "The capture_other_const_list_ and capture_other_list_ are different\n";
- log.assertTrue(capture_other_const_list_.size() == capture_other_list_.size(), inconsistency_explanation);
- inconsistency_explanation << "The feature_other_const_list_ and feature_other_list_ are different\n";
- log.assertTrue(feature_other_const_list_.size() == feature_other_list_.size(), inconsistency_explanation);
- inconsistency_explanation << "The landmark_other_const_list_ and landmark_other_list_ are different\n";
- log.assertTrue(landmark_other_const_list_.size() == landmark_other_list_.size(), inconsistency_explanation);
-
//Check Problem and feature ptrs
if (_verbose)
{
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 7fdf1bfa0bf4ac3ed514c1e4e5f3184b0c9e63e3..2cec6eceefadc381c4ce7ba1dd63e0ec7ac0c2bf 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -94,14 +94,12 @@ void FeatureBase::remove(bool viral_remove_empty_parent)
FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr)
{
factor_list_.push_back(_co_ptr);
- factor_const_list_.push_back(_co_ptr);
return _co_ptr;
}
void FeatureBase::removeFactor(FactorBasePtr _co_ptr)
{
factor_list_.remove(_co_ptr);
- factor_const_list_.remove(_co_ptr);
}
FrameBaseConstPtr FeatureBase::getFrame() const
@@ -117,33 +115,32 @@ FrameBasePtr FeatureBase::getFrame()
FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- constrained_by_const_list_.push_back(_fac_ptr);
return _fac_ptr;
}
void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.remove(_fac_ptr);
- constrained_by_const_list_.remove(_fac_ptr);
}
bool FeatureBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
{
- return std::find(constrained_by_const_list_.begin(),
- constrained_by_const_list_.end(),
- _factor) != constrained_by_const_list_.end();
+ return std::find(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ _factor) != constrained_by_list_.end();
}
bool FeatureBase::hasFactor(FactorBaseConstPtr _factor) const
{
- return std::find(factor_const_list_.begin(),
- factor_const_list_.end(),
- _factor) != factor_const_list_.end();
+ return std::find(factor_list_.begin(),
+ factor_list_.end(),
+ _factor) != factor_list_.end();
}
void FeatureBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
{
- _fac_list.insert(_fac_list.end(), factor_const_list_.begin(), factor_const_list_.end());
+ // FIXME
+ _fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
}
void FeatureBase::getFactorList(FactorBasePtrList & _fac_list)
@@ -266,8 +263,6 @@ CheckLog FeatureBase::localCheck(bool _verbose, FeatureBaseConstPtr _ftr_ptr, st
<< " not found among constrained-by factors\n";
log.assertTrue((cby->hasFeatureOther(_ftr_ptr)), inconsistency_explanation);
}
- inconsistency_explanation << "constrained_by_list and constrained_by_const_list not equal\n";
- log.assertTrue(constrained_by_const_list_.size() == constrained_by_list_.size(), inconsistency_explanation);
// Check capture
auto cap_ftr = _ftr_ptr->getCapture();
@@ -286,9 +281,6 @@ CheckLog FeatureBase::localCheck(bool _verbose, FeatureBaseConstPtr _ftr_ptr, st
<< " -X-> Ftr" << id();
log.assertTrue((fac->getFeature() == _ftr_ptr), inconsistency_explanation);
}
- inconsistency_explanation << "factor_list and factor_const_list not equal\n";
- log.assertTrue(factor_list_.size() == factor_const_list_.size(), inconsistency_explanation);
-
return log;
}
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 9490b58a300793e4c12b8b4361ab52da06974040..f295cdbca724a4e4ee89b8d2aa5c52ae487b537a 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -188,14 +188,12 @@ CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
{
WOLF_WARN_COND(getCaptureOf(_capt_ptr->getSensor()) != nullptr, "FrameBase::addCapture adding new capture ", _capt_ptr->id(), " in a frame with another capture of the same sensor: ", getCaptureOf(_capt_ptr->getSensor())->id());
capture_list_.push_back(_capt_ptr);
- capture_const_list_.push_back(_capt_ptr);
return _capt_ptr;
}
void FrameBase::removeCapture(CaptureBasePtr _capt_ptr)
{
capture_list_.remove(_capt_ptr);
- capture_const_list_.remove(_capt_ptr);
}
CaptureBaseConstPtr FrameBase::getCaptureOfType(const std::string& type) const
@@ -364,29 +362,27 @@ void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
bool FrameBase::hasCapture(const CaptureBaseConstPtr& _capture) const
{
- return std::find(capture_const_list_.begin(),
- capture_const_list_.end(),
- _capture) != capture_const_list_.end();
+ return std::find(capture_list_.begin(),
+ capture_list_.end(),
+ _capture) != capture_list_.end();
}
FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- constrained_by_const_list_.push_back(_fac_ptr);
return _fac_ptr;
}
void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.remove(_fac_ptr);
- constrained_by_const_list_.remove(_fac_ptr);
}
bool FrameBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
{
- return std::find(constrained_by_const_list_.begin(),
- constrained_by_const_list_.end(),
- _factor) != constrained_by_const_list_.end();
+ return std::find(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ _factor) != constrained_by_list_.end();
}
void FrameBase::link(TrajectoryBasePtr _trj_ptr)
@@ -514,8 +510,6 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBaseConstPtr _frm_ptr, std::o
}
}
}
- inconsistency_explanation << "constrained_by_list and constrained_by_const_list not equal\n";
- log.assertTrue(constrained_by_const_list_.size() == constrained_by_list_.size(), inconsistency_explanation);
// Trajectory
auto trj_ptr = getTrajectory();
@@ -532,9 +526,6 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBaseConstPtr _frm_ptr, std::o
<< " -X-> Frm" << id();
log.assertTrue((C->getFrame() == _frm_ptr), inconsistency_explanation);
}
- inconsistency_explanation << "capture_list and capture_const_list not equal\n";
- log.assertTrue(capture_list_.size() == capture_const_list_.size(), inconsistency_explanation);
-
return log;
}
@@ -544,7 +535,8 @@ bool FrameBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose,
auto frm_ptr = std::static_pointer_cast<const FrameBase>(_node_ptr);
auto local_log = localCheck(_verbose, frm_ptr, _stream, _tabs);
_log.compose(local_log);
- for(auto C : getCaptureList()) C->check(_log, C, _verbose, _stream, _tabs + " ");
+ for(auto C : getCaptureList())
+ C->check(_log, C, _verbose, _stream, _tabs + " ");
return _log.is_consistent_;
}
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index 428fb58250713a4599e1ed7f5d4eef8d2c142e2a..470057fabef0c9f868d26cc28f88e12a20cc595b 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -38,20 +38,19 @@ HardwareBase::~HardwareBase()
SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
{
sensor_list_.push_back(_sensor_ptr);
- sensor_const_list_.push_back(_sensor_ptr);
return _sensor_ptr;
}
SensorBaseConstPtr HardwareBase::getSensor(const std::string& _sensor_name) const
{
- auto sen_it = std::find_if(sensor_const_list_.begin(),
- sensor_const_list_.end(),
+ auto sen_it = std::find_if(sensor_list_.begin(),
+ sensor_list_.end(),
[&](SensorBaseConstPtr sb)
{
return sb->getName() == _sensor_name;
}); // lambda function for the find_if
- if (sen_it == sensor_const_list_.end())
+ if (sen_it == sensor_list_.end())
return nullptr;
return (*sen_it);
@@ -95,9 +94,6 @@ CheckLog HardwareBase::localCheck(bool _verbose, HardwareBaseConstPtr _hwd_ptr,
_stream << _tabs << "Hrw @ " << _hwd_ptr.get() << std::endl;
}
- inconsistency_explanation << "sensor_list and sensor_const_list not equal\n";
- log.assertTrue(sensor_list_.size() == sensor_const_list_.size(), inconsistency_explanation);
-
// check pointer to Problem
inconsistency_explanation << "Hwd->getProblem() [" << getProblem().get()
<< "] -> " << " Prb->getHardware() [" << getProblem()->getHardware().get() << "] -> Hwd [" << _hwd_ptr.get() << "] Mismatch!\n";
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 20bea02c2388145973752d4d61afa7a90cce5860..e7acd4f0d90ef0883eea1af949928b0458e25a44 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -37,7 +37,7 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
NodeBase("LANDMARK", _type),
HasStateBlocks(""),
map_ptr_(),
- state_block_vec_(0), // Resize in derived constructors if needed.
+ //state_block_vec_(0), // Resize in derived constructors if needed.
landmark_id_(++landmark_id_count_)
{
if (_p_ptr)
@@ -84,45 +84,45 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
}
}
-std::vector<StateBlockConstPtr> LandmarkBase::getUsedStateBlockVec() const
-{
- std::vector<StateBlockConstPtr> used_state_block_vec(0);
-
- // normal state blocks in {P,O,V,W}
- for (auto key : getStructure())
- {
- const auto sbp = getStateBlock(key);
- if (sbp)
- used_state_block_vec.push_back(sbp);
- }
-
- // 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;
-}
-
-std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec()
-{
- std::vector<StateBlockPtr> used_state_block_vec(0);
-
- // normal state blocks in {P,O,V,W}
- for (auto key : getStructure())
- {
- auto sbp = getStateBlock(key);
- if (sbp)
- used_state_block_vec.push_back(sbp);
- }
-
- // 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;
-}
+// std::vector<StateBlockConstPtr> LandmarkBase::getUsedStateBlockVec() const
+// {
+// std::vector<StateBlockConstPtr> used_state_block_vec(0);
+
+// // normal state blocks in {P,O,V,W}
+// for (auto key : getStructure())
+// {
+// const auto sbp = getStateBlock(key);
+// if (sbp)
+// used_state_block_vec.push_back(sbp);
+// }
+
+// // // 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;
+// }
+
+// std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec()
+// {
+// std::vector<StateBlockPtr> used_state_block_vec(0);
+
+// // normal state blocks in {P,O,V,W}
+// for (auto key : getStructure())
+// {
+// auto sbp = getStateBlock(key);
+// if (sbp)
+// used_state_block_vec.push_back(sbp);
+// }
+
+// // // 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;
+// }
bool LandmarkBase::getCovariance(Eigen::MatrixXd& _cov) const
{
@@ -176,21 +176,19 @@ void LandmarkBase::setProblem(ProblemPtr _problem)
FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- constrained_by_const_list_.push_back(_fac_ptr);
return _fac_ptr;
}
void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.remove(_fac_ptr);
- constrained_by_const_list_.remove(_fac_ptr);
}
bool LandmarkBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
{
- return std::find(constrained_by_const_list_.begin(),
- constrained_by_const_list_.end(),
- _factor) != constrained_by_const_list_.end();
+ return std::find(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ _factor) != constrained_by_list_.end();
}
void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
@@ -304,8 +302,6 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr,
}
}
}
- inconsistency_explanation << "constrained_by_list and constrained_by_const_list not equal\n";
- log.assertTrue(constrained_by_list_.size() == constrained_by_const_list_.size(), inconsistency_explanation);
// check map
inconsistency_explanation << "Lmk" << id() << " @ " << _lmk_ptr
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 13df91306bfec1d277668cbc0197ee93a79ef02c..88b5f411dc6b718cab9f5e1a2a70071abffa4f8e 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -57,14 +57,12 @@ MapBase::~MapBase()
LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
{
landmark_list_.push_back(_landmark_ptr);
- landmark_const_list_.push_back(_landmark_ptr);
return _landmark_ptr;
}
void MapBase::removeLandmark(LandmarkBasePtr _landmark_ptr)
{
landmark_list_.remove(_landmark_ptr);
- landmark_const_list_.remove(_landmark_ptr);
}
void MapBase::load(const std::string& _map_file_dot_yaml)
@@ -141,9 +139,6 @@ CheckLog MapBase::localCheck(bool _verbose, MapBaseConstPtr _map_ptr, std::ostre
if (_verbose)
_stream << _tabs << "Map @ " << _map_ptr.get() << std::endl;
- inconsistency_explanation << "landmark_list and landmark_const_list not equal\n";
- log.assertTrue(landmark_list_.size() == landmark_const_list_.size(), inconsistency_explanation);
-
// check pointer to Problem
inconsistency_explanation << "Map->getProblem() [" << getProblem().get()
<< "] -> " << " Prb->getMap() [" << getProblem()->getMap().get() << "] -> Map [" << _map_ptr.get() << "] Mismatch!\n";
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 93abd7bfc4b113bed1399bb0ca8c4d9c9214ec00..996d48263c113555ba055422153bf10459bfb564 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -387,14 +387,12 @@ bool SensorBase::process(const CaptureBasePtr capture_ptr)
ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
{
processor_list_.push_back(_proc_ptr);
- processor_const_list_.push_back(_proc_ptr);
return _proc_ptr;
}
void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr)
{
processor_list_.remove(_proc_ptr);
- processor_const_list_.remove(_proc_ptr);
}
StateBlockConstPtr SensorBase::getStateBlockDynamic(const char& _key) const
@@ -620,8 +618,6 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBaseConstPtr _sen_ptr, std:
<< " -X-> Sen" << id();
log.assertTrue((prc->getSensor() == _sen_ptr), inconsistency_explanation);
}
- inconsistency_explanation << "processor_list and processor_const_list not equal\n";
- log.assertTrue(processor_list_.size() == processor_const_list_.size(), inconsistency_explanation);
// check last_capture_
if (getProblem()->getTimeStamp().ok())
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index d109846bcc45ded6add566208f05f9a6adc999a7..24de3de294a6e65ea409d345a31a23393904ae50 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -27,8 +27,7 @@ namespace wolf {
TrajectoryBase::TrajectoryBase() :
NodeBase("TRAJECTORY", "TrajectoryBase"),
- frame_map_(),
- frame_const_map_()
+ frame_map_()
{
}
@@ -43,7 +42,6 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
assert(frame_map_.count(_frame_ptr->getTimeStamp()) == 0 && "Trying to add a keyframe with the same timestamp of an existing one");
frame_map_.emplace(_frame_ptr->getTimeStamp(), _frame_ptr);
- frame_const_map_.emplace(_frame_ptr->getTimeStamp(), _frame_ptr);
return _frame_ptr;
}
@@ -51,7 +49,6 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
{
frame_map_.erase(_frame_ptr->getTimeStamp());
- frame_const_map_.erase(_frame_ptr->getTimeStamp());
}
void TrajectoryBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
@@ -102,10 +99,10 @@ FrameBaseConstPtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts)
{
auto closest_ts = closestTimeStampToTimeStamp(_ts);
- assert(not closest_ts.ok() or frame_const_map_.count(closest_ts) != 0);
+ assert(not closest_ts.ok() or frame_map_.count(closest_ts) != 0);
if (closest_ts.ok())
- return frame_const_map_.at(closest_ts);
+ return frame_map_.at(closest_ts);
return nullptr;
}
@@ -124,11 +121,11 @@ FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts)
FrameBaseConstPtr TrajectoryBase::getNextFrame(FrameBaseConstPtr _frame, const unsigned int& i) const
{
- auto frame_it = frame_const_map_.find(_frame->getTimeStamp());
- WOLF_WARN_COND(frame_it == frame_const_map_.end(), "Frame not found in the frame map!");
+ auto frame_it = frame_map_.find(_frame->getTimeStamp());
+ WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!");
- if (frame_it == frame_const_map_.end() or
- std::distance(frame_it, frame_const_map_.end()) <= i)
+ if (frame_it == frame_map_.end() or
+ std::distance(frame_it, frame_map_.end()) <= i)
return nullptr;
return std::next(frame_it,i)->second;
@@ -148,11 +145,11 @@ FrameBasePtr TrajectoryBase::getNextFrame(FrameBasePtr _frame, const unsigned in
FrameBaseConstPtr TrajectoryBase::getPreviousFrame(FrameBaseConstPtr _frame, const unsigned int& i) const
{
- auto frame_it = frame_const_map_.find(_frame->getTimeStamp());
- WOLF_WARN_COND(frame_it == frame_const_map_.end(), "Frame not found in the frame map!");
+ auto frame_it = frame_map_.find(_frame->getTimeStamp());
+ WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!");
- if (frame_it == frame_const_map_.end() or
- std::distance(frame_const_map_.begin(), frame_it) < i)
+ if (frame_it == frame_map_.end() or
+ std::distance(frame_map_.begin(), frame_it) < i)
return nullptr;
return std::prev(frame_it,i)->second;
@@ -180,7 +177,7 @@ void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _stat
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 1)
- for (auto F_pair : frame_const_map_)
+ for (auto F_pair : frame_map_)
if (F_pair.second)
F_pair.second->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
@@ -207,7 +204,7 @@ bool TrajectoryBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _ver
auto trj_ptr = std::static_pointer_cast<const TrajectoryBase>(_node_ptr);
auto local_log = localCheck(_verbose, trj_ptr, _stream, _tabs);
_log.compose(local_log);
- for (auto F_pair : frame_const_map_)
+ for (auto F_pair : getFrameMap())
if (F_pair.second)
F_pair.second->check(_log, F_pair.second, _verbose, _stream, _tabs + " ");
return _log.is_consistent_;