diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index 2e8ee85c22155f08307637e0f6f41101a9d4e8b9..e634dac6f2de62d79296525dd6ca2631b4041108 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.cpp
@@ -309,9 +309,9 @@ int main(int argc, char *argv[])
// draw landmarks
std::vector<double> landmark_vector;
- for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
+ for (auto landmark : wolf_manager_QR->getProblem()->getMap()->getLandmarkList())
{
- double* position_ptr = (*landmark_it)->getP()->get();
+ double* position_ptr = landmark->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -364,9 +364,9 @@ int main(int argc, char *argv[])
// Draw Final result -------------------------
std::cout << "Drawing final results..." << std::endl;
std::vector<double> landmark_vector;
- for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
+ for (auto landmark : wolf_manager_QR->getProblem()->getMap()->getLandmarkList())
{
- double* position_ptr = (*landmark_it)->getP()->get();
+ double* position_ptr = landmark->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -398,10 +398,10 @@ int main(int argc, char *argv[])
std::cout << "Landmarks..." << std::endl;
i = 0;
Eigen::VectorXd landmarks(wolf_manager_QR->getProblem()->getMap()->getLandmarkList().size() * 2);
- for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
+ for (auto landmark : wolf_manager_QR->getProblem()->getMap()->getLandmarkList())
{
- Eigen::Map<Eigen::Vector2d> landmark((*landmark_it)->getP()->get());
- landmarks.segment(i, 2) = landmark;
+ Eigen::Map<Eigen::Vector2d> landmark_p(landmark->getP()->get());
+ landmarks.segment(i, 2) = landmark_p;
i += 2;
}
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 1c2f80ab61ba3ab597506a4f9a3e690a290d5cc9..87fffa2d3158d1b8dbd8844a362a6c164a093acb 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -90,6 +90,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
public:
FeatureBaseConstPtrList getFeatureList() const;
FeatureBasePtrList getFeatureList();
+ bool hasFeature(const FeatureBaseConstPtr &_feature) const;
FactorBaseConstPtrList getFactorList() const;
FactorBasePtrList getFactorList();
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 3e7d29310f42b80f81396041026638236783a448..235a3dc70e50174e99e974ca1e6249b044f2184a 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -128,6 +128,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
FactorBasePtrList getFactorList();
void getFactorList(FactorBaseConstPtrList & _fac_list) const;
void getFactorList(FactorBasePtrList & _fac_list);
+ bool hasFactor(FactorBaseConstPtr _fac) const;
unsigned int getHits() const;
FactorBaseConstPtrList getConstrainedByList() const;
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index fbdac3eb8d398055be5db4b995e2e9a029aac446..86ab564e383270fc517e41876c306c4bd826c908 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -129,6 +129,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
CaptureBaseConstPtrList getCaptureList() const;
CaptureBasePtrList getCaptureList();
+ bool hasCapture(const CaptureBaseConstPtr& _capture) const;
FactorBaseConstPtrList getFactorList() const;
FactorBasePtrList getFactorList();
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index 88d72e2211b8fa7ccc993d658a6ad8509fbbbf43..ea0a977a39492dd6430e0c24c7efd32441f19f14 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -50,6 +50,12 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
SensorBaseConstPtrList getSensorList() const;
SensorBasePtrList getSensorList();
+ /** \brief get a sensor pointer by its name
+ * \param _sensor_name The sensor name, as it was installed with installSensor()
+ */
+ SensorBaseConstPtr getSensor(const std::string& _sensor_name) const;
+ SensorBasePtr getSensor(const std::string& _sensor_name);
+
virtual void printHeader(int depth, //
bool constr_by, //
bool metric, //
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index ccfbc51485f904932cf8a6fadb5ea9be31585b09..ac82297f5e36b5130151bd1b0464a110264b5fe7 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -425,9 +425,7 @@ inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::f
inline bool HasStateBlocks::hasStateBlock(const StateBlockConstPtr &_sb) const
{
- const auto& it = this->find(_sb);
-
- return it != state_block_const_map_.end();
+ return this->find(_sb) != state_block_const_map_.end();
}
inline bool HasStateBlocks::stateBlockKey(const StateBlockConstPtr &_sb, char& _key) const
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index becf854fa604e4b496e51cc2fcede63c7274eacc..228798693b187bc148092faa82be3fea8da4866b 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -67,6 +67,8 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
TimeStamp closestTimeStampToTimeStamp(const TimeStamp& _ts) const;
FrameBaseConstPtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts);
+ bool hasTimeStamp(const TimeStamp& _ts) const;
+ bool hasFrame(FrameBaseConstPtr _frame) const;
virtual void printHeader(int depth, //
bool constr_by, //
@@ -136,6 +138,22 @@ inline SizeEigen TrajectoryBase::size() const
return frame_const_map_.size();
}
+inline bool TrajectoryBase::hasTimeStamp(const TimeStamp& _ts) const
+{
+ return frame_const_map_.count(_ts) != 0;
+}
+
+inline bool TrajectoryBase::hasFrame(FrameBaseConstPtr _frame) const
+{
+ return std::find_if(frame_const_map_.begin(),
+ frame_const_map_.end(),
+ [&_frame](std::pair<TimeStamp,FrameBaseConstPtr> frm_it)
+ {
+ return frm_it.second == _frame;
+ }) != frame_const_map_.end();
+}
+
+
} // namespace wolf
#endif
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 656a43398b02f116546bcc01ea716b7b1c48fab2..011a93484b21935183a83fd3319a1a4eb814ae56 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -46,6 +46,7 @@ CaptureBase::CaptureBase(const std::string& _type,
{
if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic('P'))
{
+ WOLF_ERROR_COND(not _p_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state P dynamic but provided _p_ptr is nullptr")
assert(_p_ptr && "Pointer to dynamic position params is null!");
addStateBlock('P', _p_ptr, nullptr);
}
@@ -54,6 +55,7 @@ CaptureBase::CaptureBase(const std::string& _type,
if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic('O'))
{
+ WOLF_ERROR_COND(not _o_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state O dynamic but provided _o_ptr is nullptr")
assert(_o_ptr && "Pointer to dynamic orientation params is null!");
addStateBlock('O', _o_ptr, nullptr);
}
@@ -62,6 +64,7 @@ CaptureBase::CaptureBase(const std::string& _type,
if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic('I'))
{
+ WOLF_ERROR_COND(not _intr_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state I dynamic but provided _i_ptr is nullptr")
assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
addStateBlock('I', _intr_ptr, nullptr);
}
@@ -179,22 +182,18 @@ void CaptureBase::removeConstrainedBy(FactorBasePtr _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();
+}
+
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();
- // FactorBaseConstPtrConstIter cby_it = std::find_if(constrained_by_const_list_.begin(),
- // constrained_by_const_list_.end(),
- // [_factor](const FactorBaseConstPtr & cby)
- // {
- // return cby == _factor;
- // }
- // );
- // if (cby_it != constrained_by_const_list_.end())
- // return true;
- // else
- // return false;
}
StateBlockConstPtr CaptureBase::getStateBlock(const char& _key) const
@@ -376,12 +375,12 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBaseConstPtr _cap_ptr, st
// check contrained_by
- for (const auto& cby : getConstrainedByList())
+ for (auto cby : getConstrainedByList())
{
if (_verbose)
{
_stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
- for (const auto& Cow : cby->getCaptureOtherList())
+ for (auto Cow : cby->getCaptureOtherList())
_stream << " Cap" << Cow.lock()->id();
_stream << std::endl;
}
@@ -441,7 +440,7 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBaseConstPtr _cap_ptr, st
if(getSensor() != nullptr )
{
match_any_prc_timetolerance = getSensor()->getProcessorList().empty();
- for(auto const& prc : getSensor()->getProcessorList())
+ for(auto prc : getSensor()->getProcessorList())
{
match_any_prc_timetolerance = match_any_prc_timetolerance or (time_diff <= prc->getTimeTolerance());
}
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index da38aa7366ed25252a014cb80cef7ca9bd83b820..b9fa97ae64b0ab1be8f38c9498852eb91366a751 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -119,7 +119,7 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
//frame state blocks
for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
- for (const auto& key : fr_pair.second->getStructure())
+ for (auto key : fr_pair.second->getStructure())
{
const auto& sb = fr_pair.second->getStateBlock(key);
all_state_blocks.push_back(sb);
@@ -150,12 +150,12 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
{
// first create a vector containing all state blocks
for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
- for (const auto& key1 : wolf_problem_->getFrameStructure())
+ for (auto key1 : wolf_problem_->getFrameStructure())
{
const auto& sb1 = fr_pair.second->getStateBlock(key1);
assert(isStateBlockRegisteredDerived(sb1));
- for (const auto& key2 : wolf_problem_->getFrameStructure())
+ for (auto key2 : wolf_problem_->getFrameStructure())
{
const auto& sb2 = fr_pair.second->getStateBlock(key2);
assert(isStateBlockRegisteredDerived(sb2));
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 37e1bcdaa4625017048c5104dd120d6f71c41674..faecad96aa27a0c1994a85ac9548fa08dde23426 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -332,16 +332,16 @@ void FactorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
&& getLandmarkOtherList().empty())
_stream << " Abs";
- for (const auto& Fow : getFrameOtherList())
+ for (auto Fow : getFrameOtherList())
if (!Fow.expired())
_stream << " Frm" << Fow.lock()->id();
- for (const auto& Cow : getCaptureOtherList())
+ for (auto Cow : getCaptureOtherList())
if (!Cow.expired())
_stream << " Cap" << Cow.lock()->id();
- for (const auto& fow : getFeatureOtherList())
+ for (auto fow : getFeatureOtherList())
if (!fow.expired())
_stream << " Ftr" << fow.lock()->id();
- for (const auto& Low : getLandmarkOtherList())
+ for (auto Low : getLandmarkOtherList())
if (!Low.expired())
_stream << " Lmk" << Low.lock()->id();
_stream << std::endl;
@@ -370,7 +370,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
}
// find constrained_by pointer in constrained frame
- for (const auto& Fow : getFrameOtherList())
+ for (auto Fow : getFrameOtherList())
{
if (!Fow.expired())
{
@@ -393,7 +393,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
if (_verbose && !getFrameOtherList().empty())
_stream << ")";
// find constrained_by pointer in constrained capture
- for (const auto& Cow : getCaptureOtherList())
+ for (auto Cow : getCaptureOtherList())
{
if (!Cow.expired())
{
@@ -417,7 +417,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
if (_verbose && !getCaptureOtherList().empty())
_stream << ")";
// find constrained_by pointer in constrained feature
- for (const auto& fow : getFeatureOtherList())
+ for (auto fow : getFeatureOtherList())
{
if (!fow.expired())
{
@@ -440,7 +440,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
_stream << ")";
// find constrained_by pointer in constrained landmark
- for (const auto& Low : getLandmarkOtherList())
+ for (auto Low : getLandmarkOtherList())
{
if (Low.expired())
{
@@ -546,7 +546,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
// find in constrained Frame
- for (const auto& Fow : getFrameOtherList())
+ for (auto Fow : getFrameOtherList())
{
if (!Fow.expired())
{
@@ -567,7 +567,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
}
// find in constrained Capture
- for (const auto& Cow : getCaptureOtherList())
+ for (auto Cow : getCaptureOtherList())
{
if (!Cow.expired())
{
@@ -579,7 +579,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
}
// find in constrained Feature
- for (const auto& fow : getFeatureOtherList())
+ for (auto fow : getFeatureOtherList())
{
if (!fow.expired())
{
@@ -605,7 +605,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
}
// find in constrained landmark
- for (const auto& Low : getLandmarkOtherList())
+ for (auto Low : getLandmarkOtherList())
{
if (!Low.expired())
{
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 09971a9f9764c852d70afb7ed6e9b15464bb8035..7fdf1bfa0bf4ac3ed514c1e4e5f3184b0c9e63e3 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -134,6 +134,13 @@ bool FeatureBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
_factor) != constrained_by_const_list_.end();
}
+bool FeatureBase::hasFactor(FactorBaseConstPtr _factor) const
+{
+ return std::find(factor_const_list_.begin(),
+ factor_const_list_.end(),
+ _factor) != factor_const_list_.end();
+}
+
void FeatureBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
{
_fac_list.insert(_fac_list.end(), factor_const_list_.begin(), factor_const_list_.end());
@@ -249,7 +256,7 @@ CheckLog FeatureBase::localCheck(bool _verbose, FeatureBaseConstPtr _ftr_ptr, st
if (_verbose)
{
_stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
- for (const auto& fow : cby->getFeatureOtherList())
+ for (auto fow : cby->getFeatureOtherList())
_stream << " Ftr" << fow.lock()->id();
_stream << std::endl;
}
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index c76d30896265afe23200f194f872a27e7183dd10..9490b58a300793e4c12b8b4361ab52da06974040 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -362,6 +362,13 @@ void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
c_ptr->getFactorList(_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();
+}
+
FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
@@ -484,7 +491,7 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBaseConstPtr _frm_ptr, std::o
if (_verbose)
{
_stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
- for (const auto& Fow : cby->getFrameOtherList())
+ for (auto Fow : cby->getFrameOtherList())
_stream << " Frm" << Fow.lock()->id() << std::endl;
@@ -515,13 +522,7 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBaseConstPtr _frm_ptr, std::o
inconsistency_explanation << "Frm" << id() << " @ " << _frm_ptr
<< " ---> Trj" << " @ " << trj_ptr
<< " -X-> Frm" << id();
- auto trj_has_frm = std::find_if(trj_ptr->getFrameMap().begin(),
- trj_ptr->getFrameMap().end(),
- [&_frm_ptr](std::pair<TimeStamp,FrameBaseConstPtr> frm_it)
- {
- return frm_it.second == _frm_ptr;
- });
- log.assertTrue(trj_has_frm != trj_ptr->getFrameMap().end(), inconsistency_explanation);
+ log.assertTrue(trj_ptr->hasFrame(_frm_ptr), inconsistency_explanation);
// Captures
for(auto C : getCaptureList())
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index 3f9de63e64a4f7d17f967d0ca0e6ba5a1ce09d7c..428fb58250713a4599e1ed7f5d4eef8d2c142e2a 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -42,6 +42,36 @@ SensorBasePtr HardwareBase::addSensor(SensorBasePtr _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(),
+ [&](SensorBaseConstPtr sb)
+ {
+ return sb->getName() == _sensor_name;
+ }); // lambda function for the find_if
+
+ if (sen_it == sensor_const_list_.end())
+ return nullptr;
+
+ return (*sen_it);
+}
+
+SensorBasePtr HardwareBase::getSensor(const std::string& _sensor_name)
+{
+ auto sen_it = std::find_if(sensor_list_.begin(),
+ sensor_list_.end(),
+ [&](SensorBasePtr sb)
+ {
+ return sb->getName() == _sensor_name;
+ }); // lambda function for the find_if
+
+ if (sen_it == sensor_list_.end())
+ return nullptr;
+
+ return (*sen_it);
+}
+
void HardwareBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
_stream << _tabs << "Hardware" << ((_depth < 1) ? (" -- " + std::to_string(getSensorList().size()) + "S") : "") << std::endl;
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 8a72934b9be01569080466a3548ce6c5782ed105..20bea02c2388145973752d4d61afa7a90cce5860 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -89,9 +89,9 @@ std::vector<StateBlockConstPtr> LandmarkBase::getUsedStateBlockVec() const
std::vector<StateBlockConstPtr> used_state_block_vec(0);
// normal state blocks in {P,O,V,W}
- for (const auto& key : getStructure())
+ for (auto key : getStructure())
{
- const auto& sbp = getStateBlock(key);
+ const auto sbp = getStateBlock(key);
if (sbp)
used_state_block_vec.push_back(sbp);
}
@@ -109,9 +109,9 @@ std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec()
std::vector<StateBlockPtr> used_state_block_vec(0);
// normal state blocks in {P,O,V,W}
- for (const auto& key : getStructure())
+ for (auto key : getStructure())
{
- const auto& sbp = getStateBlock(key);
+ auto sbp = getStateBlock(key);
if (sbp)
used_state_block_vec.push_back(sbp);
}
@@ -248,7 +248,7 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr,
_stream << _tabs << "Lmk" << id() << " @ " << _lmk_ptr.get() << std::endl;
_stream << _tabs << " -> Prb @ " << getProblem().get() << std::endl;
_stream << _tabs << " -> Map @ " << getMap().get() << std::endl;
- for (const auto& pair : getStateBlockMap())
+ for (auto pair : getStateBlockMap())
{
auto sb = pair.second;
_stream << _tabs << " " << pair.first << " sb @ " << sb.get();
@@ -277,11 +277,11 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr,
{
_stream << _tabs << " " << "<- Fac" << cby->id() << " ->";
- for (const auto& Low : cby->getLandmarkOtherList())
+ for (auto Low : cby->getLandmarkOtherList())
{
if (!Low.expired())
{
- const auto& Lo = Low.lock();
+ auto Lo = Low.lock();
_stream << " Lmk" << Lo->id();
}
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 04854fba62b4f02c8ec583f80e747eeb406b8a47..cc51fa6805640e04f0417e9680daf30e7a641633 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -299,6 +299,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
const std::string& _params_filename)
{
SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
+ WOLF_INFO("installProcessor _corresponding_sensor_name", _corresponding_sensor_name, " found sensor name ", sen_ptr->getName())
if (sen_ptr == nullptr)
throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
if (_params_filename == "")
@@ -335,32 +336,12 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
SensorBaseConstPtr Problem::getSensor(const std::string& _sensor_name) const
{
- auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
- getHardware()->getSensorList().end(),
- [&](SensorBaseConstPtr sb)
- {
- return sb->getName() == _sensor_name;
- }); // lambda function for the find_if
-
- if (sen_it == getHardware()->getSensorList().end())
- return nullptr;
-
- return (*sen_it);
+ return getHardware()->getSensor(_sensor_name);
}
SensorBasePtr Problem::getSensor(const std::string& _sensor_name)
{
- auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
- getHardware()->getSensorList().end(),
- [&](SensorBasePtr sb)
- {
- return sb->getName() == _sensor_name;
- }); // lambda function for the find_if
-
- if (sen_it == getHardware()->getSensorList().end())
- return nullptr;
-
- return (*sen_it);
+ return getHardware()->getSensor(_sensor_name);
}
FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
@@ -371,7 +352,7 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
VectorComposite state;
SizeEigen index = 0;
SizeEigen size = 0;
- for (const auto& key : _frame_structure)
+ for (auto key : _frame_structure)
{
if (key == 'O')
{
@@ -425,9 +406,9 @@ FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
const VectorComposite& _frame_state)
{
// append new keys to frame structure
- for (const auto& pair_key_vec : _frame_state)
+ for (auto pair_key_vec : _frame_state)
{
- const auto& key = pair_key_vec.first;
+ auto key = pair_key_vec.first;
if (frame_structure_.find(key) == std::string::npos) // not found
frame_structure_ += std::string(1,key);
}
@@ -458,7 +439,7 @@ TimeStamp Problem::getTimeStamp ( ) const
{
TimeStamp ts = TimeStamp::Invalid();
- for (const auto& prc_pair : motion_provider_map_)
+ for (auto prc_pair : motion_provider_map_)
if (prc_pair.second->getTimeStamp().ok())
if ( (not ts.ok() ) or prc_pair.second->getTimeStamp() > ts)
ts = prc_pair.second->getTimeStamp();
@@ -466,7 +447,7 @@ TimeStamp Problem::getTimeStamp ( ) const
if (not ts.ok())
{
- const auto& last_kf = trajectory_ptr_->getLastFrame();
+ auto last_kf = trajectory_ptr_->getLastFrame();
if (last_kf)
ts = last_kf->getTimeStamp(); // Use last estimated frame's state
@@ -486,12 +467,12 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
VectorComposite state;
// compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
- for (const auto& prc_pair : motion_provider_map_)
+ for (auto prc_pair : motion_provider_map_)
{
- const auto& prc_state = prc_pair.second->getState(structure);
+ auto prc_state = prc_pair.second->getState(structure);
// transfer processor vector blocks to problem state
- for (const auto& pair_key_vec : prc_state)
+ for (auto pair_key_vec : prc_state)
{
if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
{
@@ -510,7 +491,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
VectorComposite state_last;
- const auto& last_kf = trajectory_ptr_->getLastFrame();
+ auto last_kf = trajectory_ptr_->getLastFrame();
if (last_kf)
state_last = last_kf->getState(structure);
@@ -518,7 +499,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
else if (prior_options_ and prior_options_->mode != "nothing")
state_last = prior_options_->state;
- for (const auto& key : structure)
+ for (auto key : structure)
{
if (state.count(key) == 0)
{
@@ -542,12 +523,12 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
VectorComposite state;
// compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
- for (const auto& prc_pair : motion_provider_map_)
+ for (auto prc_pair : motion_provider_map_)
{
- const auto& prc_state = prc_pair.second->getState(_ts, structure);
+ auto prc_state = prc_pair.second->getState(_ts, structure);
// transfer processor vector blocks to problem state
- for (const auto& pair_key_vec : prc_state)
+ for (auto pair_key_vec : prc_state)
{
if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
state.insert(pair_key_vec);
@@ -564,7 +545,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
VectorComposite state_last;
- const auto& last_kf = trajectory_ptr_->closestFrameToTimeStamp(_ts);
+ auto last_kf = trajectory_ptr_->closestFrameToTimeStamp(_ts);
if (last_kf)
state_last = last_kf->getState(structure);
@@ -572,7 +553,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
else if (prior_options_ and prior_options_->mode != "nothing")
state_last = prior_options_->state;
- for (const auto& key : structure)
+ for (auto key : structure)
{
if (state.count(key) == 0)
{
@@ -596,12 +577,12 @@ VectorComposite Problem::getOdometry(const StateStructure& _structure) const
VectorComposite odom_state;
// compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
- for (const auto& prc_pair : motion_provider_map_)
+ for (auto prc_pair : motion_provider_map_)
{
- const auto& prc_state = prc_pair.second->getOdometry();
+ auto prc_state = prc_pair.second->getOdometry();
// transfer processor vector blocks to problem state
- for (const auto& pair_key_vec : prc_state)
+ for (auto pair_key_vec : prc_state)
{
if (odom_state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
odom_state.insert(pair_key_vec);
@@ -615,7 +596,7 @@ VectorComposite Problem::getOdometry(const StateStructure& _structure) const
if (prior_options_ and prior_options_->mode != "nothing")
state_last = prior_options_->state;
- for (const auto& key : structure)
+ for (auto key : structure)
{
if (odom_state.count(key) == 0)
{
@@ -644,7 +625,7 @@ const StateStructure& Problem::getFrameStructure() const
void Problem::appendToStructure(const StateStructure& _structure)
{
- for (const auto& key : _structure)
+ for (auto key : _structure)
if (frame_structure_.find(key) == std::string::npos) // now key not found in problem structure -> append!
frame_structure_ += key;
}
@@ -691,7 +672,7 @@ void Problem::removeMotionProvider(MotionProviderPtr proc)
// rebuild frame structure with remaining motion processors
frame_structure_.clear();
- for (const auto& pm : motion_provider_map_)
+ for (auto pm : motion_provider_map_)
appendToStructure(pm.second->getStateStructure());
}
@@ -700,7 +681,7 @@ VectorComposite Problem::stateZero ( const StateStructure& _structure ) const
StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
VectorComposite state;
- for (const auto& key : structure)
+ for (auto key : structure)
{
VectorXd vec;
if (key == 'O')
@@ -958,10 +939,10 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd&
// filling covariance
int i = 0, j = 0;
- for (const auto& sb_i : _frame_ptr->getStateBlockVec())
+ for (auto sb_i : _frame_ptr->getStateBlockVec())
{
j = 0;
- for (const auto& sb_j : _frame_ptr->getStateBlockVec())
+ for (auto sb_j : _frame_ptr->getStateBlockVec())
{
success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
j += sb_j->getLocalSize();
@@ -988,10 +969,10 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
// filling covariance
int i = 0, j = 0;
- for (const auto& sb_i : _landmark_ptr->getStateBlockVec())
+ for (auto sb_i : _landmark_ptr->getStateBlockVec())
{
j = 0;
- for (const auto& sb_j : _landmark_ptr->getStateBlockVec())
+ for (auto sb_j : _landmark_ptr->getStateBlockVec())
{
success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
j += sb_j->getLocalSize();
@@ -1127,10 +1108,10 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
auto prior_cap = CaptureBase::emplace<CaptureVoid>(prior_keyframe, _ts, nullptr);
// Emplace a feature and a factor for each state block
- for (const auto& pair_key_sb : prior_keyframe->getStateBlockMap())
+ for (auto pair_key_sb : prior_keyframe->getStateBlockMap())
{
- const auto& key = pair_key_sb.first;
- const auto& sb = pair_key_sb.second;
+ auto key = pair_key_sb.first;
+ auto sb = pair_key_sb.second;
const auto& state_blk = prior_options_->state.at(key);
const auto& covar_blk = prior_options_->cov.at(key,key);
@@ -1268,11 +1249,11 @@ bool Problem::check(int _verbose_level) const
void Problem::perturb(double amplitude)
{
// Sensors
- for (auto& S : getHardware()->getSensorList())
+ for (auto S : getHardware()->getSensorList())
S->perturb(amplitude);
// Frames and Captures
- for (auto& F : getTrajectory()->getFrameMap())
+ for (auto F : getTrajectory()->getFrameMap())
{
F.second->perturb(amplitude);
for (auto& C : F.second->getCaptureList())
@@ -1280,7 +1261,7 @@ void Problem::perturb(double amplitude)
}
// Landmarks
- for (auto& L : getMap()->getLandmarkList())
+ for (auto L : getMap()->getLandmarkList())
L->perturb(amplitude);
}
diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
index 2c8e16ce62b9cb38c47849ae7520dba095a51776..288fbcfd2567f057ba958a2503b797935b931fcc 100644
--- a/src/processor/processor_loop_closure.cpp
+++ b/src/processor/processor_loop_closure.cpp
@@ -159,7 +159,7 @@ void ProcessorLoopClosure::process(CaptureBasePtr _capture)
// Emplace factors for each LC if validated
auto n_loops = 0;
- for (const auto& match_pair : match_lc_map)
+ for (auto match_pair : match_lc_map)
if (validateLoopClosure(match_pair.second))
{
emplaceFactors(match_pair.second);
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index ef4b1ea19c681434f3b5ce9bc1c01a9a43df8ede..0ad4a754dd665d4533303f2e27569b0e7c151048 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -418,7 +418,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Update state and time stamps
- const auto& ts = getTimeStamp();
+ auto ts = getTimeStamp();
last_ptr_->setTimeStamp( ts );
last_ptr_->getFrame()->setTimeStamp( ts );
VectorComposite state_propa = getState( ts );
@@ -552,7 +552,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
*/
// Get state of origin
- const auto& x_origin = getOrigin()->getFrame()->getState(state_structure_);
+ auto x_origin = getOrigin()->getFrame()->getState(state_structure_);
// Get most recent motion
const auto& motion = last_ptr_->getBuffer().back();
@@ -561,7 +561,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
const auto& delta_preint = motion.delta_integr_;
// Get calibration preint -- stored in last capture
- const auto& calib_preint = last_ptr_->getCalibrationPreint();
+ auto calib_preint = last_ptr_->getCalibrationPreint();
VectorComposite state;
//WOLF_INFO("processorMotion last timestamp: ", last_ptr_->getTimeStamp());
@@ -569,7 +569,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
if ( hasCalibration())
{
// Get current calibration -- from origin capture
- const auto& calib = getCalibration(origin_ptr_);
+ auto calib = getCalibration(origin_ptr_);
// get Jacobian of delta wrt calibration
const auto& J_delta_calib = motion.jacobian_calib_;
@@ -578,7 +578,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
const auto& delta_step = J_delta_calib * (calib - calib_preint);
// correct delta // this is (+)
- const auto& delta_corrected = correctDelta(delta_preint, delta_step);
+ auto delta_corrected = correctDelta(delta_preint, delta_step);
// compute current state // this is [+]
statePlusDelta(x_origin, delta_corrected, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), state);
@@ -651,7 +651,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
// Get state of origin
auto cap_orig = capture_motion->getOriginCapture();
- const auto& x_origin = cap_orig->getFrame()->getState(state_structure_);
+ auto x_origin = cap_orig->getFrame()->getState(state_structure_);
// Get motion at time stamp
const auto& motion = capture_motion->getBuffer().getMotion(_ts);
@@ -660,14 +660,14 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
const auto& delta_preint = motion.delta_integr_;
// Get calibration preint -- stored in last capture
- const auto& calib_preint = capture_motion->getCalibrationPreint();
+ auto calib_preint = capture_motion->getCalibrationPreint();
VectorComposite state;
if ( hasCalibration() )
{
// Get current calibration -- from origin capture
- const auto& calib = getCalibration(cap_orig);
+ auto calib = getCalibration(cap_orig);
// get Jacobian of delta wrt calibration
const auto& J_delta_calib = motion.jacobian_calib_;
@@ -676,7 +676,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
const auto& delta_step = J_delta_calib * (calib - calib_preint);
// correct delta // this is (+)
- const auto& delta_corrected = correctDelta(delta_preint, delta_step);
+ auto delta_corrected = correctDelta(delta_preint, delta_step);
// compute current state // this is [+]
statePlusDelta(x_origin, delta_corrected, _ts - cap_orig->getTimeStamp(), state);
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 1958a86156c773665f3f157bbb8193cec840e731..93abd7bfc4b113bed1399bb0ca8c4d9c9214ec00 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -104,7 +104,7 @@ SensorBase::~SensorBase()
void SensorBase::fixExtrinsics()
{
- for (const auto& key : std::string("PO"))
+ for (auto key : std::string("PO"))
{
auto sbp = getStateBlockDynamic(key);
if (sbp != nullptr)
@@ -114,7 +114,7 @@ void SensorBase::fixExtrinsics()
void SensorBase::unfixExtrinsics()
{
- for (const auto& key : std::string("PO"))
+ for (auto key : std::string("PO"))
{
auto sbp = getStateBlockDynamic(key);
if (sbp != nullptr)
@@ -124,7 +124,7 @@ void SensorBase::unfixExtrinsics()
void SensorBase::fixIntrinsics()
{
- for (const auto& key : getStructure())
+ for (auto key : getStructure())
{
if (key != 'P' and key != 'O') // exclude extrinsics
{
@@ -137,7 +137,7 @@ void SensorBase::fixIntrinsics()
void SensorBase::unfixIntrinsics()
{
- for (const auto& key : getStructure())
+ for (auto key : getStructure())
{
if (key != 'P' and key != 'O') // exclude extrinsics
{
@@ -539,7 +539,7 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
{
if (_metric && _state_blocks)
{
- for (const auto &key : getStructure())
+ for (auto key : getStructure())
{
auto sb = getStateBlockDynamic(key);
if (sb)
@@ -555,9 +555,9 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
else if (_state_blocks)
{
_stream << _tabs << " " << "sb:";
- for (const auto &key : getStructure())
+ for (auto key : getStructure())
{
- const auto &sb = getStateBlockDynamic(key);
+ auto sb = getStateBlockDynamic(key);
if (sb)
_stream << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
}
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index e0168dd1bc83a6f349f32f31995d08a776fa5699..c9d192a536151f0340a7fd1f8f9f5e88acf54c26 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -250,7 +250,7 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
*
* Notification is put back on problem notifications, it will be added next update() call.
*/
- for (const auto& st : fac_ptr->getStateBlockPtrVector())
+ for (auto st : fac_ptr->getStateBlockPtrVector())
if (state_blocks_.count(st) == 0)
{
// Check if it was stored as a 'floating' state block
@@ -273,7 +273,7 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
factors_.insert(fac_ptr);
// state-factor map
- for (const auto& st : fac_ptr->getStateBlockPtrVector())
+ for (auto st : fac_ptr->getStateBlockPtrVector())
{
assert(state_blocks_.count(st) != 0 && "SolverManager::addFactor before adding any involved state block");
assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::addFactor before adding any involved state block");
@@ -300,7 +300,7 @@ void SolverManager::removeFactor(const FactorBasePtr& fac_ptr)
factors_.erase(fac_ptr);
// state-factor map
- for (const auto& st : fac_ptr->getStateBlockPtrVector())
+ for (auto st : fac_ptr->getStateBlockPtrVector())
{
assert(state_blocks_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block");
assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block");
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index 0f4d8283734110d8a7d19bd8b8a3681ad5f1641f..84f6f74682e43a17d39d74f55a5d1aedb43944ee 100644
--- a/src/solver_suitesparse/solver_manager.cpp
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "../../include/core/ceres_wrapper/solver_ceres.h"
+#include "core/ceres_wrapper/solver_ceres.h"
SolverManager::SolverManager()
{
@@ -49,14 +49,14 @@ void SolverManager::update(const WolfProblemPtr _problem_ptr)
}
else
{
- // ADD/UPDATE STATE UNITS
- for(auto state_unit_it = _problem_ptr->getStateList().begin(); state_unit_it!=_problem_ptr->getStateList().end(); state_unit_it++)
+ // ADD/UPDATE STATE BLOKS
+ for(auto state_block : _problem_ptr->getStateList())
{
- if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
- addStateUnit(*state_unit_it);
+ if (state_block->getPendingStatus() == ADD_PENDING)
+ addStateUnit(state_block);
- else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
- updateStateUnitStatus(*state_unit_it);
+ else if(state_block->getPendingStatus() == UPDATE_PENDING)
+ updateStateUnitStatus(state_block);
}
//std::cout << "state units updated!" << std::endl;
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 350479924ff32b575836bbd4a6f51e9e2a52023f..047469dc899a8973b7bdf07df318b2079c155ef9 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -80,7 +80,7 @@ void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream&
{
if (_metric && _state_blocks)
{
- for (const auto &key : getStructure())
+ for (auto key : getStructure())
{
auto sb = getStateBlock(key);
if (sb)
@@ -99,9 +99,9 @@ void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream&
else if (_state_blocks)
{
_stream << _tabs << " " << "sb:";
- for (const auto &key : getStructure())
+ for (auto key : getStructure())
{
- const auto &sb = getStateBlock(key);
+ auto sb = getStateBlock(key);
if (sb)
_stream << " " << key
<< " [" << (sb->isFixed() ? "Fix" : "Est")
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index 9f3e729eafd84f37fe67655142fb77555034253f..ff1888db04f37a69588d84a569e847abe3d937d3 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -200,11 +200,11 @@ bool MatrixComposite::emplace(const char &_row, const char &_col, const Eigen::M
bool MatrixComposite::at(const char &_row, const char &_col, Eigen::MatrixXd &_mat_blk) const
{
- const auto &row_it = this->find(_row);
+ auto row_it = this->find(_row);
if(row_it != this->end())
return false;
- const auto &col_it = row_it->second.find(_col);
+ auto col_it = row_it->second.find(_col);
if(col_it != row_it->second.end())
return false;
@@ -215,10 +215,10 @@ bool MatrixComposite::at(const char &_row, const char &_col, Eigen::MatrixXd &_m
Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col)
{
- const auto &row_it = this->find(_row);
+ auto row_it = this->find(_row);
assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
- const auto &col_it = row_it->second.find(_col);
+ auto col_it = row_it->second.find(_col);
assert(col_it != row_it->second.end() && "Requested matrix block does not exist in matrix composite.");
return col_it->second;
@@ -226,10 +226,10 @@ Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col)
const Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col) const
{
- const auto &row_it = this->find(_row);
+ auto row_it = this->find(_row);
assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
- const auto &col_it = row_it->second.find(_col);
+ auto col_it = row_it->second.find(_col);
assert(col_it != row_it->second.end() && "Requested matrix block does not exist in matrix composite.");
return col_it->second;
diff --git a/src/utils/graph_search.cpp b/src/utils/graph_search.cpp
index 20dc65e0e7040cfe92c7d6a39b14f66092ef3f14..85d1b3f528aca99336dcffa5f420d6a7ca2de243 100644
--- a/src/utils/graph_search.cpp
+++ b/src/utils/graph_search.cpp
@@ -96,7 +96,7 @@ std::set<FrameBasePtr> GraphSearch::getNeighborFrames(const std::set<FrameBasePt
FactorBasePtrList facs_by = frm->getConstrainedByList();
// Iterate over all factors_by
- for (auto && fac_by : facs_by)
+ for (auto fac_by : facs_by)
{
//WOLF_INFO_COND(fac_by, "fac_by: ", fac_by->id());
//WOLF_INFO_COND(fac_by->getFrame(), "fac_by->getFrame(): ", fac_by->getFrame()->id());
@@ -115,7 +115,7 @@ std::set<FrameBasePtr> GraphSearch::getNeighborFrames(const std::set<FrameBasePt
frm->getFactorList(facs_own);
// Iterate over all factors_own
- for (auto && fac_own : facs_own)
+ for (auto fac_own : facs_own)
{
//WOLF_INFO_COND(fac_own, "fac_own: ", fac_own->id());
//WOLF_INFO_COND(fac_own->getFrameOtherList().empty(), "fac_own->getFrameOtherList() is empty");
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index ffecd322bc2a021abf7ae01850c3a6b38369208c..fa4c1807c394cc4f24ed440ff1d4e79df9392829 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -144,8 +144,9 @@ TEST(Problem, SetOrigin_PO_2d)
TrajectoryBasePtr T = P->getTrajectory();
FrameBasePtr F = P->getLastFrame();
CaptureBasePtr C = F->getCaptureList().front();
- FeatureBasePtr fo = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
- FeatureBasePtr fp = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
+ auto feature_list = C->getFeatureList();
+ FeatureBasePtr fo = (*std::find_if(feature_list.begin(), feature_list.end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
+ FeatureBasePtr fp = (*std::find_if(feature_list.begin(), feature_list.end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
FactorBasePtrList fac_list;
F->getFactorList(fac_list);
@@ -201,10 +202,9 @@ TEST(Problem, SetOrigin_PO_3d)
TrajectoryBasePtr T = P->getTrajectory();
FrameBasePtr F = P->getLastFrame();
CaptureBasePtr C = F->getCaptureList().front();
- // FeatureBasePtr fo = C->getFeatureList().front();
- // FeatureBasePtr fp = C->getFeatureList().back();
- FeatureBasePtr fo = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
- FeatureBasePtr fp = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
+ auto feature_list = C->getFeatureList();
+ FeatureBasePtr fo = (*std::find_if(feature_list.begin(), feature_list.end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
+ FeatureBasePtr fp = (*std::find_if(feature_list.begin(), feature_list.end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
FactorBasePtrList fac_list;
F->getFactorList(fac_list);
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 7a142212ad896a2fd2b6d4a930ce9b0b3e475d13..b1dbdfee8a84823106a59e699da1cd28bb532982 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -74,15 +74,17 @@ class ProcessorTrackerFeatureDummyDummy : public ProcessorTrackerFeatureDummy
bool isFeatureLinked(FeatureBasePtr ftr, CaptureBasePtr cap)
{
+ auto feature_list = cap->getFeatureList();
return ftr->getCapture() == cap &&
- std::find(cap->getFeatureList().begin(), cap->getFeatureList().end(), ftr) != cap->getFeatureList().end();
+ std::find(feature_list.begin(), feature_list.end(), ftr) != feature_list.end();
}
bool isFactorLinked(FactorBasePtr fac, FeatureBasePtr ftr)
{
+ auto factor_list = ftr->getFactorList();
return fac->getFeature() == ftr &&
- std::find(ftr->getFactorList().begin(), ftr->getFactorList().end(), fac) != ftr->getFactorList().end();
+ std::find(factor_list.begin(), factor_list.end(), fac) != factor_list.end();
}
@@ -249,7 +251,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, emplaceFactor)
ASSERT_EQ(fac->getCaptureOther(),nullptr);
ASSERT_EQ(fac->getFeatureOther(),ftr_other);
ASSERT_EQ(fac->getLandmarkOther(),nullptr);
- ASSERT_TRUE(std::find(ftr_other->getConstrainedByList().begin(),ftr_other->getConstrainedByList().end(), fac) != ftr_other->getConstrainedByList().end());
+ ASSERT_TRUE(ftr_other->isConstrainedBy(fac));
}
TEST_F(ProcessorTrackerFeatureDummyTest, establishFactors)
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 8017ad6049b31efec2f1efa5d5126c542429ab0a..4ed3c0846017e15fe5882607ff1807fbbbbc93f5 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -85,22 +85,25 @@ class ProcessorTrackerLandmarkDummyDummy : public ProcessorTrackerLandmarkDummy
bool isFeatureLinked(FeatureBasePtr ftr, CaptureBasePtr cap)
{
+ auto feature_list = cap->getFeatureList();
return ftr->getCapture() == cap &&
- std::find(cap->getFeatureList().begin(), cap->getFeatureList().end(), ftr) != cap->getFeatureList().end();
+ std::find(feature_list.begin(), feature_list.end(), ftr) != feature_list.end();
}
bool isFactorLinked(FactorBasePtr fac, FeatureBasePtr ftr)
{
+ auto factor_list = ftr->getFactorList();
return fac->getFeature() == ftr &&
- std::find(ftr->getFactorList().begin(), ftr->getFactorList().end(), fac) != ftr->getFactorList().end();
+ std::find(factor_list.begin(), factor_list.end(), fac) != factor_list.end();
}
bool isLandmarkLinked(LandmarkBasePtr lmk, MapBasePtr map)
{
+ auto landmark_list = map->getLandmarkList();
return lmk->getMap() == map &&
- std::find(map->getLandmarkList().begin(), map->getLandmarkList().end(), lmk) != map->getLandmarkList().end();
+ std::find(landmark_list.begin(), landmark_list.end(), lmk) != landmark_list.end();
}