diff --git a/.clang-format b/.clang-format
new file mode 100644
index 0000000000000000000000000000000000000000..ea2a2257babe01c1814f5764875509ada2d8b465
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1,113 @@
+---
+Language: Cpp
+BasedOnStyle: Google
+AccessModifierOffset: 0
+AlignAfterOpenBracket: Align
+AlignConsecutiveAssignments: true
+AlignConsecutiveDeclarations: true
+AlignEscapedNewlines: Left
+AlignOperands: true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: true
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: true
+BinPackArguments: true
+BinPackParameters: false
+BraceWrapping:
+ AfterClass: false
+ AfterControlStatement: false
+ AfterEnum: false
+ AfterFunction: false
+ AfterNamespace: false
+ AfterObjCDeclaration: false
+ AfterStruct: false
+ AfterUnion: false
+ AfterExternBlock: false
+ BeforeCatch: false
+ BeforeElse: false
+ IndentBraces: false
+ SplitEmptyFunction: true
+ SplitEmptyRecord: true
+ SplitEmptyNamespace: true
+BreakBeforeBinaryOperators: None
+BreakBeforeBraces: Linux
+BreakBeforeInheritanceComma: false
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeColon
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: true
+ColumnLimit: 119
+CommentPragmas: "^ IWYU pragma:"
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: true
+DisableFormat: false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+ForEachMacros:
+ - foreach
+ - Q_FOREACH
+ - BOOST_FOREACH
+IncludeBlocks: Preserve
+IncludeCategories:
+ - Regex: '^<pinocchio/fwd\.hpp>'
+ Priority: 1
+ - Regex: '^<ext/.*\.h>'
+ Priority: 3
+ - Regex: '^<.*\.h>'
+ Priority: 2
+ - Regex: "^<.*"
+ Priority: 3
+ - Regex: ".*"
+ Priority: 4
+IncludeIsMainRegex: "([-_](test|unittest))?$"
+IndentCaseLabels: true
+IndentPPDirectives: None
+IndentWidth: 4
+IndentWrappedFunctionNames: false
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: false
+MacroBlockBegin: ""
+MacroBlockEnd: ""
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakAssignment: 2
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+ReflowComments: true
+SortIncludes: false
+SortUsingDeclarations: true
+SpaceAfterCStyleCast: false
+SpaceAfterTemplateKeyword: true
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeParens: ControlStatements
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard: Auto
+TabWidth: 4
+UseTab: Never
diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index 0625a61b5fce5bd66d240e3ac1da89e5d584e455..29a27f5d9c1662a6e6ab7bd4c9d7c02515476c4e 100644
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
@@ -295,8 +295,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameMap().front();
- FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameMap().front();
+ FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFirstFrame();
+ FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFirstFrame();
CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3d::Identity() * 0.01);
CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3d::Identity() * 0.01);
first_frame_autodiff->addCapture(initial_covariance_autodiff);
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index 037ff094a158592c24adee70c28ddfb8cddb1d35..bc8982ca2ce696719aab35105f059639d6c72f23 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -322,8 +322,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameMap().front();
- FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameMap().front();
+ FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFirstFrame();
+ FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFirstFrame();
CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3d::Identity() * 0.01);
CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3d::Identity() * 0.01);
first_frame_full->addCapture(initial_covariance_full);
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 5500b724a95cf12984d12b1327749833a5ff8acc..793d409e4c7522ee2ebcc18ad01ec61cbbf1bbbc 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -253,11 +253,11 @@ int main()
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto& kf : *problem->getTrajectory())
+ for (auto& kf_pair : problem->getTrajectory()->getFrameMap())
{
Eigen::MatrixXd cov;
- kf->getCovariance(cov);
- WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+ kf_pair.second->getCovariance(cov);
+ WOLF_TRACE("KF", kf_pair.second->id(), "_cov = \n", cov);
}
for (auto& lmk : problem->getMap()->getLandmarkList())
{
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index 27214245096f91fefad1351f611613516010f40a..39492e1975dbe66cabf86ac9c751d3bd72584deb 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -243,11 +243,11 @@ int main()
// GET COVARIANCES of all states
WOLF_INFO("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto& kf : *problem->getTrajectory())
+ for (auto& kf_pair : problem->getTrajectory()->getFrameMap())
{
Eigen::MatrixXd cov;
- kf->getCovariance(cov);
- WOLF_INFO("KF", kf->id(), "_cov = \n", cov);
+ kf_pair.second->getCovariance(cov);
+ WOLF_INFO("KF", kf_pair.second->id(), "_cov = \n", cov);
}
for (auto& lmk : problem->getMap()->getLandmarkList())
{
diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index 821ffefeb052acb75f81ec5d3327ebdaf04c4cd7..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
@@ -384,13 +384,13 @@ int main(int argc, char *argv[])
// Vehicle poses
std::cout << "Vehicle poses..." << std::endl;
int i = 0;
- Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().size() * 3);
- for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().end(); frame_it++)
+ Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->size() * 3);
+ for (auto frame : wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap())
{
if (complex_angle)
- state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), atan2(*(*frame_it)->getO()->get(), *((*frame_it)->getO()->get() + 1));
+ state_poses.segment(i, 3) << *frame->getP()->get(), *(frame->getP()->get() + 1), atan2(*frame->getO()->get(), *(frame->getO()->get() + 1));
else
- state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), *(*frame_it)->getO()->get();
+ state_poses.segment(i, 3) << *frame->getP()->get(), *(frame->getP()->get() + 1), *frame->getO()->get();
i += 3;
}
@@ -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 d73f17d8ec26cd8765ad1dcbeddaee8b66c91944..b946f35b770aabb02f7f237933221131b30afdbc 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -80,34 +80,45 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
void setTimeStamp(const TimeStamp& _ts);
void setTimeStampToNow();
- FrameBasePtr getFrame() const;
+ FrameBaseConstPtr getFrame() const;
+ FrameBasePtr getFrame();
private:
void setFrame(const FrameBasePtr _frm_ptr);
public:
- const FeatureBasePtrList& getFeatureList() const;
-
- void getFactorList(FactorBasePtrList& _fac_list) const;
- FactorBasePtrList getFactorList() const;
-
- SensorBasePtr getSensor() const;
+ FeatureBaseConstPtrList getFeatureList() const;
+ FeatureBasePtrList getFeatureList();
+ bool hasFeature(const FeatureBaseConstPtr &_feature) const;
+
+ FactorBaseConstPtrList getFactorList() const;
+ FactorBasePtrList getFactorList();
+ void getFactorList(FactorBaseConstPtrList& _fac_list) const;
+ void getFactorList(FactorBasePtrList& _fac_list);
+
+ SensorBaseConstPtr getSensor() const;
+ SensorBasePtr getSensor();
virtual void setSensor(const SensorBasePtr sensor_ptr);
// constrained by
private:
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
+
public:
unsigned int getHits() const;
- const FactorBasePtrList& getConstrainedByList() const;
- bool isConstrainedBy(const FactorBasePtr &_factor) const;
-
+ FactorBaseConstPtrList getConstrainedByList() const;
+ FactorBasePtrList getConstrainedByList();
+ bool isConstrainedBy(const FactorBaseConstPtr &_factor) const;
// State blocks
- StateBlockPtr getStateBlock(const char& _key) const;
- StateBlockPtr getSensorP() const;
- StateBlockPtr getSensorO() const;
- StateBlockPtr getSensorIntrinsic() const;
+ StateBlockConstPtr getStateBlock(const char& _key) const;
+ StateBlockPtr getStateBlock(const char& _key);
+ StateBlockConstPtr getSensorP() const;
+ StateBlockPtr getSensorP();
+ StateBlockConstPtr getSensorO() const;
+ StateBlockPtr getSensorO();
+ StateBlockConstPtr getSensorIntrinsic() const;
+ StateBlockPtr getSensorIntrinsic();
void fix() override;
void unfix() override;
@@ -133,8 +144,8 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, CaptureBaseConstPtr _cap_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
@@ -158,17 +169,32 @@ std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... al
return cpt;
}
-inline StateBlockPtr CaptureBase::getSensorP() const
+inline StateBlockConstPtr CaptureBase::getSensorP() const
{
return getStateBlock('P');
}
-inline StateBlockPtr CaptureBase::getSensorO() const
+inline StateBlockPtr CaptureBase::getSensorP()
+{
+ return getStateBlock('P');
+}
+
+inline StateBlockConstPtr CaptureBase::getSensorO() const
{
return getStateBlock('O');
}
-inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
+inline StateBlockPtr CaptureBase::getSensorO()
+{
+ return getStateBlock('O');
+}
+
+inline StateBlockConstPtr CaptureBase::getSensorIntrinsic() const
+{
+ return getStateBlock('I');
+}
+
+inline StateBlockPtr CaptureBase::getSensorIntrinsic()
{
return getStateBlock('I');
}
@@ -178,7 +204,13 @@ inline unsigned int CaptureBase::id() const
return capture_id_;
}
-inline FrameBasePtr CaptureBase::getFrame() const
+inline FrameBaseConstPtr CaptureBase::getFrame() const
+{
+ if(frame_ptr_.expired()) return nullptr;
+ else return frame_ptr_.lock();
+}
+
+inline FrameBasePtr CaptureBase::getFrame()
{
if(frame_ptr_.expired()) return nullptr;
else return frame_ptr_.lock();
@@ -189,7 +221,15 @@ inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
frame_ptr_ = _frm_ptr;
}
-inline const FeatureBasePtrList& CaptureBase::getFeatureList() const
+inline FeatureBaseConstPtrList CaptureBase::getFeatureList() const
+{
+ FeatureBaseConstPtrList list_const;
+ for (auto&& obj_ptr : feature_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FeatureBasePtrList CaptureBase::getFeatureList()
{
return feature_list_;
}
@@ -199,18 +239,30 @@ inline unsigned int CaptureBase::getHits() const
return constrained_by_list_.size();
}
-inline const FactorBasePtrList& CaptureBase::getConstrainedByList() const
+inline FactorBaseConstPtrList CaptureBase::getConstrainedByList() const
{
- return constrained_by_list_;
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : constrained_by_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
}
+inline FactorBasePtrList CaptureBase::getConstrainedByList()
+{
+ return constrained_by_list_;
+}
inline TimeStamp CaptureBase::getTimeStamp() const
{
return time_stamp_;
}
-inline SensorBasePtr CaptureBase::getSensor() const
+inline SensorBaseConstPtr CaptureBase::getSensor() const
+{
+ return sensor_ptr_.lock();
+}
+
+inline SensorBasePtr CaptureBase::getSensor()
{
return sensor_ptr_.lock();
}
@@ -230,7 +282,6 @@ inline void CaptureBase::setTimeStampToNow()
time_stamp_.setToNow();
}
-
} // namespace wolf
#endif
diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index a70a54b2aba7e19d8078a209c5b43cad63693671..719c1f800cec4ae0e5d019f56a09b656b2c3f9f5 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -112,17 +112,17 @@ class CaptureMotion : public CaptureBase
// Origin frame and capture
CaptureBasePtr getOriginCapture();
- CaptureBasePtr getOriginCapture() const;
+ CaptureBaseConstPtr getOriginCapture() const;
void setOriginCapture(CaptureBasePtr _capture_origin_ptr);
- bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance);
+ bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance) const;
void printHeader(int depth, //
- bool constr_by, //
- bool metric, //
- bool state_blocks,
- std::ostream& stream ,
- std::string _tabs = "") const override;
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const override;
// member data:
private:
@@ -181,7 +181,7 @@ inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture()
return capture_origin_ptr_.lock();
}
-inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture() const
+inline wolf::CaptureBaseConstPtr CaptureMotion::getOriginCapture() const
{
return capture_origin_ptr_.lock();
}
diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h
index f450b60deb50b1ffb65e53c87cf22bac28052f84..1f7cf9eaff36bdbae80a8802981ea88f9ba019a6 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.h
@@ -105,7 +105,8 @@ class NodeBase
void setType(const std::string& _type);
void setName(const std::string& _name);
- ProblemPtr getProblem() const;
+ ProblemConstPtr getProblem() const;
+ ProblemPtr getProblem();
};
} // namespace wolf
@@ -160,7 +161,12 @@ inline void NodeBase::setName(const std::string& _name)
node_name_ = _name;
}
-inline ProblemPtr NodeBase::getProblem() const
+inline ProblemConstPtr NodeBase::getProblem() const
+{
+ return problem_ptr_.lock();
+}
+
+inline ProblemPtr NodeBase::getProblem()
{
return problem_ptr_.lock();
}
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 197a6bfb1b30d79a64376f9468b9481c0a4bc3e3..25e9325fd3b61f425f085bd47e9b788ebd494aa9 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -197,27 +197,39 @@ struct MatrixSizeCheck
// TYPEDEFS FOR POINTERS, LISTS AND ITERATORS IN THE WOLF TREE
/////////////////////////////////////////////////////////////////////////
+#define WOLF_DECLARED_PTR_TYPEDEFS(Name) \
+ typedef std::shared_ptr<Name> Name##Ptr; \
+ typedef std::shared_ptr<const Name> Name##ConstPtr; \
+ typedef std::weak_ptr<Name> Name##WPtr; \
+ typedef std::weak_ptr<const Name> Name##ConstWPtr; \
+
#define WOLF_PTR_TYPEDEFS(ClassName) \
class ClassName; \
- typedef std::shared_ptr<ClassName> ClassName##Ptr; \
- typedef std::shared_ptr<const ClassName> ClassName##ConstPtr; \
- typedef std::weak_ptr<ClassName> ClassName##WPtr;
+ WOLF_DECLARED_PTR_TYPEDEFS(ClassName); \
+
+#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
+ struct StructName; \
+ WOLF_DECLARED_PTR_TYPEDEFS(StructName); \
#define WOLF_LIST_TYPEDEFS(ClassName) \
class ClassName; \
- typedef std::list<ClassName##Ptr> ClassName##PtrList; \
- typedef ClassName##PtrList::iterator ClassName##Iter; \
- typedef ClassName##PtrList::const_iterator ClassName##ConstIter; \
- typedef ClassName##PtrList::reverse_iterator ClassName##RevIter; \
- typedef std::list<ClassName##WPtr> ClassName##WPtrList; \
- typedef ClassName##WPtrList::iterator ClassName##WIter; \
- typedef ClassName##WPtrList::const_iterator ClassName##WConstIter; \
- typedef ClassName##WPtrList::reverse_iterator ClassName##WRevIter;
+ typedef std::list<ClassName##Ptr> ClassName##PtrList; \
+ typedef ClassName##PtrList::iterator ClassName##PtrListIter; \
+ typedef ClassName##PtrList::const_iterator ClassName##PtrListConstIter; \
+ typedef ClassName##PtrList::reverse_iterator ClassName##PtrListRevIter; \
+ typedef std::list<ClassName##WPtr> ClassName##WPtrList; \
+ typedef ClassName##WPtrList::iterator ClassName##WPtrListIter; \
+ typedef ClassName##WPtrList::const_iterator ClassName##WPtrListConstIter; \
+ typedef ClassName##WPtrList::reverse_iterator ClassName##WPtrListRevIter; \
+ typedef std::list<ClassName##ConstPtr> ClassName##ConstPtrList; \
+ typedef ClassName##ConstPtrList::iterator ClassName##ConstPtrListIter; \
+ typedef ClassName##ConstPtrList::const_iterator ClassName##ConstPtrListConstIter; \
+ typedef ClassName##ConstPtrList::reverse_iterator ClassName##ConstPtrListRevIter; \
+ typedef std::list<ClassName##ConstWPtr> ClassName##ConstWPtrList; \
+ typedef ClassName##ConstWPtrList::iterator ClassName##ConstWPtrListIter; \
+ typedef ClassName##ConstWPtrList::const_iterator ClassName##ConstWPtrListConstIter; \
+ typedef ClassName##ConstWPtrList::reverse_iterator ClassName##ConstWPtrListRevIter; \
-#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
- struct StructName; \
- typedef std::shared_ptr<StructName> StructName##Ptr; \
- typedef std::shared_ptr<const StructName> StructName##ConstPtr; \
// NodeBase
WOLF_PTR_TYPEDEFS(NodeBase);
@@ -258,7 +270,8 @@ WOLF_PTR_TYPEDEFS(FrameBase);
WOLF_LIST_TYPEDEFS(FrameBase);
class TimeStamp;
-typedef std::map<TimeStamp, FrameBasePtr> FrameMap;
+typedef std::map<TimeStamp, FrameBasePtr> FramePtrMap;
+typedef std::map<TimeStamp, FrameBaseConstPtr> FrameConstPtrMap;
// - Capture
WOLF_PTR_TYPEDEFS(CaptureBase);
diff --git a/include/core/factor/factor_analytic.h b/include/core/factor/factor_analytic.h
index b2c0d0e4f6f2d589278a243e4cc34de6f6640c3e..88f28a3880d513ace93166d6e38cc80a178455be 100644
--- a/include/core/factor/factor_analytic.h
+++ b/include/core/factor/factor_analytic.h
@@ -35,6 +35,7 @@ class FactorAnalytic: public FactorBase
{
protected:
std::vector<StateBlockPtr> state_ptr_vector_;
+ std::vector<StateBlockConstPtr> state_ptr_const_vector_;
std::vector<unsigned int> state_block_sizes_vector_;
public:
@@ -60,6 +61,27 @@ class FactorAnalytic: public FactorBase
StateBlockPtr _state8Ptr = nullptr,
StateBlockPtr _state9Ptr = nullptr );
+ FactorAnalytic(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr = nullptr,
+ StateBlockPtr _state2Ptr = nullptr,
+ StateBlockPtr _state3Ptr = nullptr,
+ StateBlockPtr _state4Ptr = nullptr,
+ StateBlockPtr _state5Ptr = nullptr,
+ StateBlockPtr _state6Ptr = nullptr,
+ StateBlockPtr _state7Ptr = nullptr,
+ StateBlockPtr _state8Ptr = nullptr,
+ StateBlockPtr _state9Ptr = nullptr );
+
~FactorAnalytic() override = default;
/** \brief Returns a vector of pointers to the states
@@ -67,7 +89,8 @@ class FactorAnalytic: public FactorBase
* Returns a vector of pointers to the state in which this factor depends
*
**/
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override;
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override;
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override;
/** \brief Returns a vector of sizes of the state blocks
*
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index 0b8f9e111d007024ff457b2303cd704604a4fb10..c10c0b58b799f6171a418c9a6217b4d69a9e6f88 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -64,7 +64,8 @@ class FactorAutodiff : public FactorBase
protected:
- std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
static const std::vector<unsigned int> jacobian_locations_;
mutable std::array<WolfJet, RES> residuals_jets_;
@@ -106,10 +107,58 @@ class FactorAutodiff : public FactorBase
StateBlockPtr _state9Ptr,
StateBlockPtr _state10Ptr,
StateBlockPtr _state11Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr})
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr,
+ _state8Ptr,
+ _state9Ptr,
+ _state10Ptr,
+ _state11Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr})
{
// asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
assert(_state0Ptr != nullptr && "nullptr state block");
assert(_state1Ptr != nullptr && "nullptr state block");
assert(_state2Ptr != nullptr && "nullptr state block");
@@ -301,9 +350,9 @@ class FactorAutodiff : public FactorBase
jacobians_.push_back(Ji);
}
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
+ // print jacobian matrices
+ // for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
}
/** \brief Returns a vector of pointers to the states
@@ -311,7 +360,11 @@ class FactorAutodiff : public FactorBase
* Returns a vector of pointers to the state in which this factor depends
*
**/
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
{
return state_ptrs_;
}
@@ -341,265 +394,316 @@ class FactorAutodiff : public FactorBase
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10>
class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public FactorBase
{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = B9;
- static const unsigned int block10Size = B10;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 11;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7 + B8 + B9 + B10> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
- mutable std::array<WolfJet, B4> jets_4_;
- mutable std::array<WolfJet, B5> jets_5_;
- mutable std::array<WolfJet, B6> jets_6_;
- mutable std::array<WolfJet, B7> jets_7_;
- mutable std::array<WolfJet, B8> jets_8_;
- mutable std::array<WolfJet, B9> jets_9_;
- mutable std::array<WolfJet, B10> jets_10_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr,
- StateBlockPtr _state8Ptr,
- StateBlockPtr _state9Ptr,
- StateBlockPtr _state10Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
- assert(_state4Ptr != nullptr && "nullptr state block");
- assert(_state5Ptr != nullptr && "nullptr state block");
- assert(_state6Ptr != nullptr && "nullptr state block");
- assert(_state7Ptr != nullptr && "nullptr state block");
- assert(_state8Ptr != nullptr && "nullptr state block");
- assert(_state9Ptr != nullptr && "nullptr state block");
- assert(_state10Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B8; i++)
- jets_8_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B9; i++)
- jets_9_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B10; i++)
- jets_10_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = B8;
+ static const unsigned int block9Size = B9;
+ static const unsigned int block10Size = B10;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 11;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8 + B9 + B10> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+ mutable std::array<WolfJet, B10> jets_10_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr,
+ _state8Ptr,
+ _state9Ptr,
+ _state10Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+ assert(_state10Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ parameters[10],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i].a = parameters[9][i];
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i].a = parameters[10][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ // for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- parameters[7],
- parameters[8],
- parameters[9],
- parameters[10],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- jets_10_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i].a = parameters[5][i];
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i].a = parameters[6][i];
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i].a = parameters[7][i];
- for (unsigned int i = 0; i < B8; i++)
- jets_8_[i].a = parameters[8][i];
- for (unsigned int i = 0; i < B9; i++)
- jets_9_[i].a = parameters[9][i];
- for (unsigned int i = 0; i < B10; i++)
- jets_10_[i].a = parameters[10][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- jets_10_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
};
////////////////// SPECIALIZATION 10 BLOCKS ////////////////////////////////////////////////////////////////////////
@@ -607,2096 +711,2488 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public FactorBase
{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = B9;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 10;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7 + B8 + B9> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
- mutable std::array<WolfJet, B4> jets_4_;
- mutable std::array<WolfJet, B5> jets_5_;
- mutable std::array<WolfJet, B6> jets_6_;
- mutable std::array<WolfJet, B7> jets_7_;
- mutable std::array<WolfJet, B8> jets_8_;
- mutable std::array<WolfJet, B9> jets_9_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr,
- StateBlockPtr _state8Ptr,
- StateBlockPtr _state9Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
- assert(_state4Ptr != nullptr && "nullptr state block");
- assert(_state5Ptr != nullptr && "nullptr state block");
- assert(_state6Ptr != nullptr && "nullptr state block");
- assert(_state7Ptr != nullptr && "nullptr state block");
- assert(_state8Ptr != nullptr && "nullptr state block");
- assert(_state9Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B8; i++)
- jets_8_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B9; i++)
- jets_9_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- parameters[7],
- parameters[8],
- parameters[9],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i].a = parameters[5][i];
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i].a = parameters[6][i];
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i].a = parameters[7][i];
- for (unsigned int i = 0; i < B8; i++)
- jets_8_[i].a = parameters[8][i];
- for (unsigned int i = 0; i < B9; i++)
- jets_9_[i].a = parameters[9][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 9 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 9;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7 + B8> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
- mutable std::array<WolfJet, B4> jets_4_;
- mutable std::array<WolfJet, B5> jets_5_;
- mutable std::array<WolfJet, B6> jets_6_;
- mutable std::array<WolfJet, B7> jets_7_;
- mutable std::array<WolfJet, B8> jets_8_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr,
- StateBlockPtr _state8Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
- assert(_state4Ptr != nullptr && "nullptr state block");
- assert(_state5Ptr != nullptr && "nullptr state block");
- assert(_state6Ptr != nullptr && "nullptr state block");
- assert(_state7Ptr != nullptr && "nullptr state block");
- assert(_state8Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B8; i++)
- jets_8_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- parameters[7],
- parameters[8],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i].a = parameters[5][i];
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i].a = parameters[6][i];
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i].a = parameters[7][i];
- for (unsigned int i = 0; i < B8; i++)
- jets_8_[i].a = parameters[8][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 8 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 8;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
- mutable std::array<WolfJet, B4> jets_4_;
- mutable std::array<WolfJet, B5> jets_5_;
- mutable std::array<WolfJet, B6> jets_6_;
- mutable std::array<WolfJet, B7> jets_7_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
- assert(_state4Ptr != nullptr && "nullptr state block");
- assert(_state5Ptr != nullptr && "nullptr state block");
- assert(_state6Ptr != nullptr && "nullptr state block");
- assert(_state7Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- parameters[7],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i].a = parameters[5][i];
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i].a = parameters[6][i];
- for (unsigned int i = 0; i < B7; i++)
- jets_7_[i].a = parameters[7][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 7 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 7;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5 + B6> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
- mutable std::array<WolfJet, B4> jets_4_;
- mutable std::array<WolfJet, B5> jets_5_;
- mutable std::array<WolfJet, B6> jets_6_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
- assert(_state4Ptr != nullptr && "nullptr state block");
- assert(_state5Ptr != nullptr && "nullptr state block");
- assert(_state6Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i].a = parameters[5][i];
- for (unsigned int i = 0; i < B6; i++)
- jets_6_[i].a = parameters[6][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 6 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 6;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
- mutable std::array<WolfJet, B4> jets_4_;
- mutable std::array<WolfJet, B5> jets_5_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
- assert(_state4Ptr != nullptr && "nullptr state block");
- assert(_state5Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i].a = parameters[4][i];
- for (unsigned int i = 0; i < B5; i++)
- jets_5_[i].a = parameters[5][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 5 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 5;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
- mutable std::array<WolfJet, B4> jets_4_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
- assert(_state4Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- for (unsigned int i = 0; i < B4; i++)
- jets_4_[i].a = parameters[4][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 4 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
-{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 4;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2 + B3> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
- mutable std::array<WolfJet, B3> jets_3_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
- assert(_state3Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- for (unsigned int i = 0; i < B3; i++)
- jets_3_[i].a = parameters[3][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
-};
-
-////////////////// SPECIALIZATION 3 BLOCKS ////////////////////////////////////////////////////////////////////////
-
-template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = B8;
+ static const unsigned int block9Size = B9;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 10;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8 + B9> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr,
+ _state8Ptr,
+ _state9Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i].a = parameters[9][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ // for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 9 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = B8;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 9;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr,
+ _state8Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ // for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 8 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 8;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ // for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 7 BLOCKS ////////////////////////////////////////////////////////////////////////
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 7;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ //for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 6 BLOCKS ////////////////////////////////////////////////////////////////////////
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 6;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 5 BLOCKS ////////////////////////////////////////////////////////////////////////
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 5;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 4 BLOCKS ////////////////////////////////////////////////////////////////////////
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = 0;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 4;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ //for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 3 BLOCKS ////////////////////////////////////////////////////////////////////////
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 3;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1 + B2> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
- mutable std::array<WolfJet, B2> jets_2_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
- assert(_state2Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- for (unsigned int i = 0; i < B2; i++)
- jets_2_[i].a = parameters[2][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = 0;
+ static const unsigned int block4Size = 0;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 3;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ //for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
};
////////////////// SPECIALIZATION 2 BLOCKS ////////////////////////////////////////////////////////////////////////
-
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = 0;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 2;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0 + B1> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
- mutable std::array<WolfJet, B1> jets_1_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
- assert(_state1Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- for (unsigned int i = 0; i < B1; i++)
- jets_1_[i].a = parameters[1][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = 0;
+ static const unsigned int block3Size = 0;
+ static const unsigned int block4Size = 0;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 2;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ //for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
};
////////////////// SPECIALIZATION 1 BLOCK ////////////////////////////////////////////////////////////////////////
-
template <class FacT,unsigned int RES,unsigned int B0>
class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
- public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = 0;
- static const unsigned int block2Size = 0;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
- static const unsigned int n_blocks = 1;
-
- static const std::vector<unsigned int> state_block_sizes_;
-
- typedef ceres::Jet<double, B0> WolfJet;
-
- protected:
-
- std::vector<StateBlockPtr> state_ptrs_;
-
- static const std::vector<unsigned int> jacobian_locations_;
- mutable std::array<WolfJet, RES> residuals_jets_;
- mutable std::array<WolfJet, B0> jets_0_;
-
- public:
-
- FactorAutodiff(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status,
- StateBlockPtr _state0Ptr) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr})
- {
- // asserts for null states
- assert(_state0Ptr != nullptr && "nullptr state block");
-
- // initialize jets
- unsigned int last_jet_idx = 0;
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
- state_ptrs_.resize(n_blocks);
- }
-
- ~FactorAutodiff() override
- {
- }
-
- JacobianMethod getJacobianMethod() const override
- {
- return JAC_AUTO;
- }
-
- bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*static_cast<FacT const*>(this))(parameters[0],
- residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- std::vector<double const*> param_vec;
- param_vec.assign(parameters,parameters+n_blocks);
- updateJetsRealPart(param_vec);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- residuals_jets_.data());
-
- // fill the residual array
- for (unsigned int i = 0; i < RES; i++)
- residuals[i] = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- jacobians[i] + row * state_block_sizes_.at(i));
- }
- return true;
- }
-
- void updateJetsRealPart(const std::vector<double const*>& parameters) const
- {
- // update jets real part
- for (unsigned int i = 0; i < B0; i++)
- jets_0_[i].a = parameters[0][i];
- }
-
- void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
- {
- assert(residual_.size() == RES);
- jacobians_.clear();
-
- assert(_states_ptr.size() == n_blocks);
-
- // update jets real part
- updateJetsRealPart(_states_ptr);
-
- // call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- residuals_jets_.data());
-
- // fill the residual vector
- for (unsigned int i = 0; i < RES; i++)
- residual_(i) = residuals_jets_[i].a;
-
- // fill the jacobian matrices
- for (unsigned int i = 0; i<n_blocks; i++)
- {
- // Create a row major Jacobian
- Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
- if (!state_ptrs_[i]->isFixed())
- for (unsigned int row = 0; row < RES; row++)
- std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
- residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
- Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
-
- // print jacobian matrices
-// for (unsigned int i = 0; i < n_blocks; i++)
-// std::cout << jacobians_[i] << std::endl << std::endl;
- }
-
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
- {
- return state_ptrs_;
- }
-
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
-
- unsigned int getSize() const override
- {
- return RES;
- }
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = 0;
+ static const unsigned int block2Size = 0;
+ static const unsigned int block3Size = 0;
+ static const unsigned int block4Size = 0;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 1;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr}),
+ state_ptrs_const_({_state0Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+
+ }
+
+ ~FactorAutodiff() override
+ {
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+ //for (unsigned int i = 0; i < n_blocks; i++)
+ // std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 05508895ca766ee28ae61bd4bd615cbb27288edf..1894148de2ea576614b14d6de81a4f138c4e037c 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -25,8 +25,8 @@
// Forward declarations for node templates
namespace wolf{
class FeatureBase;
-class TrajectoryIter;
-class TrajectoryRevIter;
+//class TrajectoryIter;
+//class TrajectoryRevIter;
}
//Wolf includes
@@ -77,43 +77,28 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
{
friend FeatureBase;
private:
- FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
+ FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
+ ProcessorBaseWPtr processor_ptr_; ///< Processor pointer
static unsigned int factor_id_count_;
+ FrameBaseWPtrList frame_other_list_; ///< FrameBase pointer list
+ CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list
+ FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list
+ LandmarkBaseWPtrList landmark_other_list_; ///< LandmarkBase pointer list
+
protected:
unsigned int factor_id_;
- FactorTopology topology_; ///< topology of factor
- FactorStatus status_; ///< status of factor
- bool apply_loss_function_; ///< flag for applying loss function to this factor
- FrameBaseWPtrList frame_other_list_; ///< FrameBase pointer list
- CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list
- FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list
- LandmarkBaseWPtrList landmark_other_list_; ///< LandmarkBase pointer list
- ProcessorBaseWPtr processor_ptr_; ///< Processor pointer
- Eigen::VectorXd measurement_; ///< the measurement vector
- Eigen::MatrixXd measurement_sqrt_information_upper_;///< the squared root information matrix ///< ProcessorBase pointer list
+ FactorTopology topology_; ///< topology of factor
+ FactorStatus status_; ///< status of factor
+ bool apply_loss_function_; ///< flag for applying loss function to this factor
+
+ Eigen::VectorXd measurement_; ///< the measurement vector
+ Eigen::MatrixXd measurement_sqrt_information_upper_; ///< the squared root information matrix
void setProblem(ProblemPtr) override final;
public:
- /** \brief Default constructor.
- *
- * IMPORTANT: "other" means "of another branch of the wolf tree".
- * You should only provide a non-nullptr in frame/capture/feature_other_ptr in case of a frame/capture/feature involved in this factor
- * that does not located in the same branch.
- **/
- FactorBase(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
FactorBase(const std::string& _tp,
const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
@@ -125,8 +110,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
-
-
~FactorBase() override = default;
virtual void remove(bool viral_remove_empty_parent=false);
@@ -154,7 +137,8 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a vector of pointers to the states in which this factor depends
**/
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const = 0;
+ virtual std::vector<StateBlockConstPtr> getStateBlockPtrVector() const = 0;
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() = 0;
/** \brief Returns a vector of the states sizes
**/
@@ -170,19 +154,23 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a pointer to the feature constrained from
**/
- FeatureBasePtr getFeature() const;
+ FeatureBaseConstPtr getFeature() const;
+ FeatureBasePtr getFeature();
/** \brief Returns a pointer to its capture
**/
- CaptureBasePtr getCapture() const;
+ CaptureBaseConstPtr getCapture() const;
+ CaptureBasePtr getCapture();
/** \brief Returns a pointer to its frame
**/
- FrameBasePtr getFrame() const;
+ FrameBaseConstPtr getFrame() const;
+ FrameBasePtr getFrame();
/** \brief Returns a pointer to its capture's sensor
**/
- SensorBasePtr getSensor() const;
+ SensorBaseConstPtr getSensor() const;
+ SensorBasePtr getSensor();
/** \brief Returns the factor residual size
**/
@@ -202,37 +190,45 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a pointer to the first frame constrained to
**/
- FrameBasePtr getFrameOther() const;
+ FrameBaseConstPtr getFrameOther() const;
+ FrameBasePtr getFrameOther();
/** \brief Returns a pointer to the first capture constrained to
**/
- CaptureBasePtr getCaptureOther() const;
+ CaptureBaseConstPtr getCaptureOther() const;
+ CaptureBasePtr getCaptureOther();
/** \brief Returns a pointer to the first feature constrained to
**/
- FeatureBasePtr getFeatureOther() const;
+ FeatureBaseConstPtr getFeatureOther() const;
+ FeatureBasePtr getFeatureOther();
/** \brief Returns a pointer to the first landmark constrained to
**/
- LandmarkBasePtr getLandmarkOther() const;
+ LandmarkBaseConstPtr getLandmarkOther() const;
+ LandmarkBasePtr getLandmarkOther();
// get pointer lists to other nodes
- FrameBaseWPtrList getFrameOtherList() const { return frame_other_list_; }
- CaptureBaseWPtrList getCaptureOtherList() const { return capture_other_list_; }
- FeatureBaseWPtrList getFeatureOtherList() const { return feature_other_list_; }
- LandmarkBaseWPtrList getLandmarkOtherList() const { return landmark_other_list_; }
-
- bool hasFrameOther(const FrameBasePtr& _frm_other) const;
- bool hasCaptureOther(const CaptureBasePtr& _cap_other) const;
- bool hasFeatureOther(const FeatureBasePtr& _ftr_other) const;
- bool hasLandmarkOther(const LandmarkBasePtr& _lmk_other) const;
+ FrameBaseWPtrList getFrameOtherList() { return frame_other_list_; }
+ CaptureBaseWPtrList getCaptureOtherList() { return capture_other_list_; }
+ FeatureBaseWPtrList getFeatureOtherList() { return feature_other_list_; }
+ LandmarkBaseWPtrList getLandmarkOtherList() { return landmark_other_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;
+ bool hasFeatureOther(const FeatureBaseConstPtr& _ftr_other) const;
+ bool hasLandmarkOther(const LandmarkBaseConstPtr& _lmk_other) const;
- public:
/**
* @brief getProcessor
* @return
*/
- ProcessorBasePtr getProcessor() const;
+ ProcessorBaseConstPtr getProcessor() const;
+ ProcessorBasePtr getProcessor();
void link(FeatureBasePtr ftr);
template<typename classType, typename... T>
@@ -252,8 +248,8 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
@@ -291,7 +287,12 @@ inline unsigned int FactorBase::id() const
return factor_id_;
}
-inline FeatureBasePtr FactorBase::getFeature() const
+inline FeatureBaseConstPtr FactorBase::getFeature() const
+{
+ return feature_ptr_.lock();
+}
+
+inline FeatureBasePtr FactorBase::getFeature()
{
return feature_ptr_.lock();
}
@@ -306,7 +307,15 @@ inline bool FactorBase::getApplyLossFunction() const
return apply_loss_function_;
}
-inline FrameBasePtr FactorBase::getFrameOther() const
+inline FrameBaseConstPtr FactorBase::getFrameOther() const
+{
+ if (frame_other_list_.empty()) return nullptr;
+ if (frame_other_list_.front().expired()) return nullptr;
+
+ return frame_other_list_.front().lock();
+}
+
+inline FrameBasePtr FactorBase::getFrameOther()
{
if (frame_other_list_.empty()) return nullptr;
if (frame_other_list_.front().expired()) return nullptr;
@@ -314,7 +323,15 @@ inline FrameBasePtr FactorBase::getFrameOther() const
return frame_other_list_.front().lock();
}
-inline CaptureBasePtr FactorBase::getCaptureOther() const
+inline CaptureBaseConstPtr FactorBase::getCaptureOther() const
+{
+ if (capture_other_list_.empty()) return nullptr;
+ if (capture_other_list_.front().expired()) return nullptr;
+
+ return capture_other_list_.front().lock();
+}
+
+inline CaptureBasePtr FactorBase::getCaptureOther()
{
if (capture_other_list_.empty()) return nullptr;
if (capture_other_list_.front().expired()) return nullptr;
@@ -322,7 +339,7 @@ inline CaptureBasePtr FactorBase::getCaptureOther() const
return capture_other_list_.front().lock();
}
-inline FeatureBasePtr FactorBase::getFeatureOther() const
+inline FeatureBaseConstPtr FactorBase::getFeatureOther() const
{
if (feature_other_list_.empty()) return nullptr;
if (feature_other_list_.front().expired()) return nullptr;
@@ -330,7 +347,15 @@ inline FeatureBasePtr FactorBase::getFeatureOther() const
return feature_other_list_.front().lock();
}
-inline LandmarkBasePtr FactorBase::getLandmarkOther() const
+inline FeatureBasePtr FactorBase::getFeatureOther()
+{
+ if (feature_other_list_.empty()) return nullptr;
+ if (feature_other_list_.front().expired()) return nullptr;
+
+ return feature_other_list_.front().lock();
+}
+
+inline LandmarkBaseConstPtr FactorBase::getLandmarkOther() const
{
if (landmark_other_list_.empty()) return nullptr;
if (landmark_other_list_.front().expired()) return nullptr;
@@ -338,7 +363,20 @@ inline LandmarkBasePtr FactorBase::getLandmarkOther() const
return landmark_other_list_.front().lock();
}
-inline ProcessorBasePtr FactorBase::getProcessor() const
+inline LandmarkBasePtr FactorBase::getLandmarkOther()
+{
+ if (landmark_other_list_.empty()) return nullptr;
+ if (landmark_other_list_.front().expired()) return nullptr;
+
+ return landmark_other_list_.front().lock();
+}
+
+inline ProcessorBaseConstPtr FactorBase::getProcessor() const
+{
+ return processor_ptr_.lock();
+}
+
+inline ProcessorBasePtr FactorBase::getProcessor()
{
return processor_ptr_.lock();
}
@@ -358,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/factor/factor_relative_pose_3d.h b/include/core/factor/factor_relative_pose_3d.h
index a132c3c0ac105fc0476f18ab2c625ee5a5801bad..6003c446732399f1abe6c4127dfb257543c82c2c 100644
--- a/include/core/factor/factor_relative_pose_3d.h
+++ b/include/core/factor/factor_relative_pose_3d.h
@@ -166,8 +166,8 @@ inline bool FactorRelativePose3d::expectation(const T* const _p_current,
inline Eigen::VectorXd FactorRelativePose3d::expectation() const
{
Eigen::VectorXd exp(7);
- FrameBasePtr frm_current = getFeature()->getFrame();
- FrameBasePtr frm_past = getFrameOther();
+ auto frm_current = getFeature()->getFrame();
+ auto frm_past = getFrameOther();
const Eigen::VectorXd frame_current_pos = frm_current->getP()->getState();
const Eigen::VectorXd frame_current_ori = frm_current->getO()->getState();
const Eigen::VectorXd frame_past_pos = frm_past->getP()->getState();
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 5259e5c51f9d5dece25eb3b3f35d7cb5570541d7..beb5eae22b8667750d43c2a51ade0a24857c6483 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -116,20 +116,22 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
void setExpectation(const Eigen::VectorXd& expectation);
// wolf tree access
- FrameBasePtr getFrame() const;
+ FrameBaseConstPtr getFrame() const;
+ FrameBasePtr getFrame();
- CaptureBasePtr getCapture() const;
+ CaptureBaseConstPtr getCapture() const;
+ CaptureBasePtr getCapture();
- const FactorBasePtrList& getFactorList() const;
+ FactorBaseConstPtrList getFactorList() const;
+ FactorBasePtrList getFactorList();
+ void getFactorList(FactorBaseConstPtrList & _fac_list) const;
+ void getFactorList(FactorBasePtrList & _fac_list);
+ bool hasFactor(FactorBaseConstPtr _fac) const;
unsigned int getHits() const;
- const FactorBasePtrList& getConstrainedByList() const;
- bool isConstrainedBy(const FactorBasePtr &_factor) const;
-
-
-
- // all factors
- void getFactorList(FactorBasePtrList & _fac_list) const;
+ FactorBaseConstPtrList getConstrainedByList() const;
+ FactorBasePtrList getConstrainedByList();
+ bool isConstrainedBy(const FactorBaseConstPtr &_factor) const;
void link(CaptureBasePtr cap_ptr);
template<typename classType, typename... T>
@@ -148,8 +150,8 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, FeatureBasePtr _feature_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, FeatureBaseConstPtr _feature_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
@@ -167,20 +169,41 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
namespace wolf{
- template<typename classType, typename... T>
- std::shared_ptr<classType> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all)
- {
- std::shared_ptr<classType> ftr = std::make_shared<classType>(std::forward<T>(all)...);
- ftr->link(_cpt_ptr);
- return ftr;
- }
+template<typename classType, typename... T>
+std::shared_ptr<classType> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all)
+{
+ std::shared_ptr<classType> ftr = std::make_shared<classType>(std::forward<T>(all)...);
+ ftr->link(_cpt_ptr);
+ return ftr;
+}
inline unsigned int FeatureBase::getHits() const
{
return constrained_by_list_.size();
}
-inline const FactorBasePtrList& FeatureBase::getConstrainedByList() const
+inline FactorBaseConstPtrList FeatureBase::getFactorList() const
+{
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : factor_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FactorBasePtrList FeatureBase::getFactorList()
+{
+ return factor_list_;
+}
+
+inline FactorBaseConstPtrList FeatureBase::getConstrainedByList() const
+{
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : constrained_by_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FactorBasePtrList FeatureBase::getConstrainedByList()
{
return constrained_by_list_;
}
@@ -190,7 +213,12 @@ inline unsigned int FeatureBase::id() const
return feature_id_;
}
-inline CaptureBasePtr FeatureBase::getCapture() const
+inline CaptureBaseConstPtr FeatureBase::getCapture() const
+{
+ return capture_ptr_.lock();
+}
+
+inline CaptureBasePtr FeatureBase::getCapture()
{
return capture_ptr_.lock();
}
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index c7ab25754729c29711e65d532a2e23b36d7d309a..8d426eacab111c81b435e48e630d39a802e02a6f 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -104,7 +104,8 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
// State blocks ----------------------------------------------------
public:
- StateBlockPtr getV() const;
+ StateBlockConstPtr getV() const;
+ StateBlockPtr getV();
protected:
void setProblem(ProblemPtr _problem) override final;
@@ -116,31 +117,59 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
// Wolf tree access ---------------------------------------------------
public:
- TrajectoryBasePtr getTrajectory() const;
+ TrajectoryBaseConstPtr getTrajectory() const;
+ TrajectoryBasePtr getTrajectory();
- FrameBasePtr getPreviousFrame() const;
- FrameBasePtr getNextFrame() const;
+ FrameBaseConstPtr getPreviousFrame(const unsigned int& i = 1) const;
+ FrameBaseConstPtr getNextFrame(const unsigned int& i = 1) const;
+ FrameBasePtr getPreviousFrame(const unsigned int& i = 1);
+ FrameBasePtr getNextFrame(const unsigned int& i = 1);
+
+ CaptureBaseConstPtrList getCaptureList() const;
+ CaptureBasePtrList getCaptureList();
+ bool hasCapture(const CaptureBaseConstPtr& _capture) const;
+
+ FactorBaseConstPtrList getFactorList() const;
+ FactorBasePtrList getFactorList();
+ void getFactorList(FactorBaseConstPtrList& _fac_list) const;
+ void getFactorList(FactorBasePtrList& _fac_list);
+
+ FactorBaseConstPtrList getConstrainedByList() const;
+ FactorBasePtrList getConstrainedByList();
+ bool isConstrainedBy(const FactorBaseConstPtr& _factor) const;
- const CaptureBasePtrList& getCaptureList() const;
template <class C>
- CaptureBasePtr getCaptureOfType() const;
- CaptureBasePtr getCaptureOfType(const std::string& type) const;
+ CaptureBaseConstPtr getCaptureOfType() const;
template <class C>
- CaptureBasePtrList getCapturesOfType() const;
- CaptureBasePtrList getCapturesOfType(const std::string& type) const;
- CaptureBasePtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const;
- CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const;
- CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr) const;
+ CaptureBasePtr getCaptureOfType();
- FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr) const;
- FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const;
- FactorBasePtrList getFactorList() const;
- void getFactorList(FactorBasePtrList& _fac_list) const;
+ CaptureBaseConstPtr getCaptureOfType(const std::string& type) const;
+ CaptureBasePtr getCaptureOfType(const std::string& type);
- unsigned int getHits() const;
- const FactorBasePtrList& getConstrainedByList() const;
- bool isConstrainedBy(const FactorBasePtr& _factor) const;
+ template <class C>
+ CaptureBaseConstPtrList getCapturesOfType() const;
+ template <class C>
+ CaptureBasePtrList getCapturesOfType();
+ CaptureBaseConstPtrList getCapturesOfType(const std::string& type) const;
+ CaptureBasePtrList getCapturesOfType(const std::string& type);
+
+ CaptureBaseConstPtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const;
+ CaptureBasePtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr);
+
+ CaptureBaseConstPtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const;
+ CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type);
+
+ CaptureBaseConstPtrList getCapturesOf(const SensorBasePtr _sensor_ptr) const;
+ CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr);
+
+ FactorBaseConstPtr getFactorOf(const ProcessorBasePtr _processor_ptr) const;
+ FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr);
+
+ FactorBaseConstPtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const;
+ FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type);
+
+ unsigned int getHits() const;
// Debug and info -------------------------------------------------------
virtual void printHeader(int depth, //
@@ -157,8 +186,8 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, FrameBaseConstPtr _frm_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
@@ -204,17 +233,35 @@ inline TimeStamp FrameBase::getTimeStamp() const
return time_stamp_;
}
-inline StateBlockPtr FrameBase::getV() const
+inline StateBlockConstPtr FrameBase::getV() const
+{
+ return getStateBlock('V');
+}
+
+inline StateBlockPtr FrameBase::getV()
{
return getStateBlock('V');
}
-inline TrajectoryBasePtr FrameBase::getTrajectory() const
+inline TrajectoryBaseConstPtr FrameBase::getTrajectory() const
+{
+ return trajectory_ptr_.lock();
+}
+
+inline TrajectoryBasePtr FrameBase::getTrajectory()
{
return trajectory_ptr_.lock();
}
-inline const CaptureBasePtrList& FrameBase::getCaptureList() const
+inline CaptureBaseConstPtrList FrameBase::getCaptureList() const
+{
+ CaptureBaseConstPtrList list_const;
+ for (auto&& obj_ptr : capture_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline CaptureBasePtrList FrameBase::getCaptureList()
{
return capture_list_;
}
@@ -224,7 +271,15 @@ inline unsigned int FrameBase::getHits() const
return constrained_by_list_.size();
}
-inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
+inline FactorBaseConstPtrList FrameBase::getConstrainedByList() const
+{
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : constrained_by_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FactorBasePtrList FrameBase::getConstrainedByList()
{
return constrained_by_list_;
}
@@ -235,19 +290,38 @@ inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
}
template <class C>
-inline CaptureBasePtr FrameBase::getCaptureOfType() const
+inline CaptureBaseConstPtr FrameBase::getCaptureOfType() const
+{
+ for (auto capture_ptr : getCaptureList())
+ if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
+ return capture_ptr;
+ return nullptr;
+}
+
+template <class C>
+inline CaptureBasePtr FrameBase::getCaptureOfType()
{
- for (CaptureBasePtr capture_ptr : getCaptureList())
+ for (auto capture_ptr : getCaptureList())
if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
return capture_ptr;
return nullptr;
}
template <class C>
-inline CaptureBasePtrList FrameBase::getCapturesOfType() const
+inline CaptureBaseConstPtrList FrameBase::getCapturesOfType() const
+{
+ CaptureBaseConstPtrList captures;
+ for (auto capture_ptr : getCaptureList())
+ if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
+ captures.push_back(capture_ptr);
+ return captures;
+}
+
+template <class C>
+inline CaptureBasePtrList FrameBase::getCapturesOfType()
{
CaptureBasePtrList captures;
- for (CaptureBasePtr capture_ptr : getCaptureList())
+ for (auto capture_ptr : getCaptureList())
if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
captures.push_back(capture_ptr);
return captures;
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index 59f423f4295472897cc2d8065811aba72538fbfb..4e86c8892af30795657610cf5576f724a7c792d3 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -46,7 +46,14 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
HardwareBase();
~HardwareBase() override;
- const SensorBasePtrList& getSensorList() const;
+ 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, //
@@ -61,8 +68,8 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, HardwareBaseConstPtr _hwd_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
@@ -74,7 +81,15 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
namespace wolf {
-inline const SensorBasePtrList& HardwareBase::getSensorList() const
+inline SensorBaseConstPtrList HardwareBase::getSensorList() const
+{
+ SensorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : sensor_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline SensorBasePtrList HardwareBase::getSensorList()
{
return sensor_list_;
}
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 7984e6773a80615aa33a84f67156cf83f1c42ab6..1d08d5748404c4bbe6151c8e28cf6827dc79f183 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -49,7 +49,8 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
private:
MapBaseWPtr map_ptr_;
FactorBasePtrList constrained_by_list_;
- std::vector<StateBlockPtr> state_block_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_;
@@ -80,7 +81,8 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
void setId(unsigned int _id);
// State blocks
- std::vector<StateBlockPtr> getUsedStateBlockVec() const;
+ //std::vector<StateBlockConstPtr> getUsedStateBlockVec() const;
+ //std::vector<StateBlockPtr> getUsedStateBlockVec();
bool getCovariance(Eigen::MatrixXd& _cov) const;
// Descriptor
@@ -89,13 +91,14 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
double getDescriptor(unsigned int _ii) const;
void setDescriptor(const Eigen::VectorXd& _descriptor);
-
unsigned int getHits() const;
- const FactorBasePtrList& getConstrainedByList() const;
- bool isConstrainedBy(const FactorBasePtr &_factor) const;
+ FactorBaseConstPtrList getConstrainedByList() const;
+ FactorBasePtrList getConstrainedByList();
+ bool isConstrainedBy(const FactorBaseConstPtr &_factor) const;
+ MapBaseConstPtr getMap() const;
+ MapBasePtr getMap();
- MapBasePtr getMap() const;
void link(MapBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(MapBasePtr _map_ptr, T&&... all);
@@ -120,8 +123,8 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
@@ -146,7 +149,12 @@ std::shared_ptr<classType> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&... all
return lmk;
}
-inline MapBasePtr LandmarkBase::getMap() const
+inline MapBaseConstPtr LandmarkBase::getMap() const
+{
+ return map_ptr_.lock();
+}
+
+inline MapBasePtr LandmarkBase::getMap()
{
return map_ptr_.lock();
}
@@ -173,7 +181,15 @@ inline unsigned int LandmarkBase::getHits() const
return constrained_by_list_.size();
}
-inline const FactorBasePtrList& LandmarkBase::getConstrainedByList() const
+inline FactorBaseConstPtrList LandmarkBase::getConstrainedByList() const
+{
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : constrained_by_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FactorBasePtrList LandmarkBase::getConstrainedByList()
{
return constrained_by_list_;
}
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 797d2bd3892e339c96a8c40e591eb56d39818ee5..af09f86002c697ad026ad9942d218026808555c4 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -111,7 +111,8 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
virtual void removeLandmark(LandmarkBasePtr _landmark_ptr);
public:
- const LandmarkBasePtrList& getLandmarkList() const;
+ LandmarkBaseConstPtrList getLandmarkList() const;
+ LandmarkBasePtrList getLandmarkList();
void load(const std::string& _map_file_yaml);
void save(const std::string& _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf");
@@ -129,13 +130,22 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, MapBaseConstPtr _map_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
private:
std::string dateTimeNow();
};
-inline const LandmarkBasePtrList& MapBase::getLandmarkList() const
+inline LandmarkBaseConstPtrList MapBase::getLandmarkList() const
+{
+ LandmarkBaseConstPtrList list_const;
+ for (auto&& obj_ptr : landmark_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline LandmarkBasePtrList MapBase::getLandmarkList()
{
return landmark_list_;
}
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 08a7ab52fd0598315adf2b9150ad80efd77043d0..69e55ea8556dc03a323e0aa62e90d125b93015a0 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -74,12 +74,12 @@ class Problem : public std::enable_shared_from_this<Problem>
friend MotionProvider;
protected:
- TreeManagerBasePtr tree_manager_;
+ TreeManagerBasePtr tree_manager_;
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
std::map<int, MotionProviderPtr> motion_provider_map_;
- std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
+ std::map<std::pair<StateBlockConstPtr, StateBlockConstPtr>, Eigen::MatrixXd> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
std::map<StateBlockPtr, Notification> state_block_notification_map_;
@@ -113,13 +113,15 @@ class Problem : public std::enable_shared_from_this<Problem>
// Tree manager --------------------------------------
public:
void setTreeManager(TreeManagerBasePtr _gm);
- TreeManagerBasePtr getTreeManager() const;
+ TreeManagerBaseConstPtr getTreeManager() const;
+ TreeManagerBasePtr getTreeManager();
// Hardware branch ------------------------------------
- HardwareBasePtr getHardware() const;
+ HardwareBaseConstPtr getHardware() const;
+ HardwareBasePtr getHardware();
/** \brief Factory method to install (create and add) sensors only from its properties
* \param _sen_type type of sensor
@@ -151,11 +153,14 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief get a sensor pointer by its name
* \param _sensor_name The sensor name, as it was installed with installSensor()
*/
- SensorBasePtr findSensor(const std::string& _sensor_name) const;
+ SensorBaseConstPtr findSensor(const std::string& _sensor_name) const;
+ SensorBasePtr findSensor(const std::string& _sensor_name);
+
/** \brief get a processor pointer by its name
* \param _processor_name The processor name, as it was installed with installProcessor()
*/
- ProcessorBasePtr findProcessor(const std::string& _processor_name) const;
+ ProcessorBaseConstPtr findProcessor(const std::string& _processor_name) const;
+ ProcessorBasePtr findProcessor(const std::string& _processor_name);
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
@@ -201,11 +206,12 @@ class Problem : public std::enable_shared_from_this<Problem>
void removeMotionProvider(MotionProviderPtr proc);
public:
- std::map<int,MotionProviderPtr>& getMotionProviderMap();
- const std::map<int,MotionProviderPtr>& getMotionProviderMap() const;
+ std::map<int,MotionProviderConstPtr> getMotionProviderMap() const;
+ std::map<int,MotionProviderPtr> getMotionProviderMap();
// Trajectory branch ----------------------------------
- TrajectoryBasePtr getTrajectory() const;
+ TrajectoryBaseConstPtr getTrajectory() const;
+ TrajectoryBasePtr getTrajectory();
// Prior
bool isPriorSet() const;
@@ -233,9 +239,9 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const SizeEigen _dim, //
- const Eigen::VectorXd& _frame_state);
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim, //
+ const Eigen::VectorXd& _frame_state);
/** \brief Emplace frame from string frame_structure and state
* \param _time_stamp Time stamp of the frame
@@ -250,8 +256,8 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const VectorComposite& _frame_state);
+ const StateStructure& _frame_structure, //
+ const VectorComposite& _frame_state);
/** \brief Emplace frame from state
* \param _time_stamp Time stamp of the frame
@@ -282,8 +288,8 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const SizeEigen _dim);
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim);
/** \brief Emplace frame from state vector
* \param _time_stamp Time stamp of the frame
@@ -298,7 +304,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
- const Eigen::VectorXd& _frame_state);
+ const Eigen::VectorXd& _frame_state);
/** \brief Emplace frame, guess all values
* \param _time_stamp Time stamp of the frame
@@ -315,8 +321,10 @@ class Problem : public std::enable_shared_from_this<Problem>
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp);
// Frame getters
- FrameBasePtr getLastFrame( ) const;
- FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
+ FrameBaseConstPtr getLastFrame( ) const;
+ FrameBasePtr getLastFrame( );
+ FrameBaseConstPtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
+ FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts);
/** \brief Give the permission to a processor to create a new key Frame
*
@@ -346,7 +354,8 @@ class Problem : public std::enable_shared_from_this<Problem>
// Map branch -----------------------------------------
- MapBasePtr getMap() const;
+ MapBaseConstPtr getMap() const;
+ MapBasePtr getMap();
void setMap(MapBasePtr);
void loadMap(const std::string& _filename_dot_yaml);
void saveMap(const std::string& _filename_dot_yaml, //
@@ -362,9 +371,9 @@ class Problem : public std::enable_shared_from_this<Problem>
void clearCovariance();
void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXd& _cov);
void addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _cov);
- bool getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXd& _cov, const int _row = 0, const int _col=0) const;
- bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const;
- bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
+ bool getCovarianceBlock(StateBlockConstPtr _state1, StateBlockConstPtr _state2, Eigen::MatrixXd& _cov, const int _row = 0, const int _col=0) const;
+ bool getCovarianceBlock(std::map<StateBlockConstPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const;
+ bool getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const;
bool getLastFrameCovariance(Eigen::MatrixXd& _covariance) const;
bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const;
@@ -437,7 +446,12 @@ class Problem : public std::enable_shared_from_this<Problem>
namespace wolf
{
-inline TreeManagerBasePtr Problem::getTreeManager() const
+inline TreeManagerBaseConstPtr Problem::getTreeManager() const
+{
+ return tree_manager_;
+}
+
+inline TreeManagerBasePtr Problem::getTreeManager()
{
return tree_manager_;
}
@@ -447,12 +461,15 @@ inline bool Problem::isPriorSet() const
return prior_options_ == nullptr;
}
-inline std::map<int,MotionProviderPtr>& Problem::getMotionProviderMap()
+inline std::map<int,MotionProviderConstPtr> Problem::getMotionProviderMap() const
{
- return motion_provider_map_;
+ std::map<int,MotionProviderConstPtr> map_const;
+ for (auto&& pair : motion_provider_map_)
+ map_const[pair.first] = pair.second;
+ return map_const;
}
-inline const std::map<int,MotionProviderPtr>& Problem::getMotionProviderMap() const
+inline std::map<int,MotionProviderPtr> Problem::getMotionProviderMap()
{
return motion_provider_map_;
}
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index b2e0f6fdb42e6715b0ed41f3c720b18f8b12792e..fc93357dd46a948c4d225e982b8ade7bf4682af9 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -327,14 +327,15 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
*/
void captureCallback(CaptureBasePtr _capture);
- SensorBasePtr getSensor() const;
+ SensorBaseConstPtr getSensor() const;
+ SensorBasePtr getSensor();
+
private:
void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
public:
bool isMotionProvider() const;
-
bool isVotingActive() const;
void setVotingActive(bool _voting_active = true);
@@ -367,8 +368,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, ProcessorBasePtr _prc_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, ProcessorBaseConstPtr _prc_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
};
inline void ProcessorBase::startCaptureProfiling()
@@ -418,7 +419,12 @@ inline unsigned int ProcessorBase::id() const
return processor_id_;
}
-inline SensorBasePtr ProcessorBase::getSensor() const
+inline SensorBaseConstPtr ProcessorBase::getSensor() const
+{
+ return sensor_ptr_.lock();
+}
+
+inline SensorBasePtr ProcessorBase::getSensor()
{
return sensor_ptr_.lock();
}
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index e9cf00b45a14bbf5ddbab51c8cbfd093d02cd985..20ed2330334aea821c7ca8bc6510f7438c555063 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -81,7 +81,7 @@ class ProcessorDiffDrive : public ProcessorOdom2d
FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
- VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const override;
+ VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
@@ -90,7 +90,7 @@ class ProcessorDiffDrive : public ProcessorOdom2d
};
-inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBasePtr _capture) const
+inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBaseConstPtr _capture) const
{
if (_capture)
return _capture->getStateBlock('I')->getState();
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 66f40e6e62e32e6312270fe994e7e38ceb426822..7c0cf9b2c776be5808c853c54c5d5c3f0cca5ff7 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -210,7 +210,8 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
/** \brief Finds the capture that contains the closest previous motion of _ts
* \return a pointer to the capture (if it exists) or a nullptr (otherwise)
*/
- CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
+ CaptureMotionConstPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
+ CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts);
/** Set the origin of all motion for this processor
* \param _origin_frame the keyframe to be the origin
@@ -447,7 +448,7 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
* @param cap_prev : the first capture motion to be merged (input)
* @param cap_target : the second capture motion (modified)
*/
- void mergeCaptures(CaptureMotionConstPtr cap_prev,
+ void mergeCaptures(CaptureMotionPtr cap_prev,
CaptureMotionPtr cap_target);
protected:
@@ -487,13 +488,16 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
public:
- virtual VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const = 0;
+ virtual VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const = 0;
bool hasCalibration() const {return calib_size_ > 0;}
//getters
- CaptureMotionPtr getOrigin() const;
- CaptureMotionPtr getLast() const;
- CaptureMotionPtr getIncoming() const;
+ CaptureMotionConstPtr getOrigin() const;
+ CaptureMotionConstPtr getLast() const;
+ CaptureMotionConstPtr getIncoming() const;
+ CaptureMotionPtr getOrigin();
+ CaptureMotionPtr getLast();
+ CaptureMotionPtr getIncoming();
double getMaxTimeSpan() const;
double getMaxBuffLength() const;
@@ -609,17 +613,32 @@ inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts) const
);
}
-inline CaptureMotionPtr ProcessorMotion::getOrigin() const
+inline CaptureMotionConstPtr ProcessorMotion::getOrigin() const
{
return origin_ptr_;
}
-inline CaptureMotionPtr ProcessorMotion::getLast() const
+inline CaptureMotionConstPtr ProcessorMotion::getLast() const
{
return last_ptr_;
}
-inline CaptureMotionPtr ProcessorMotion::getIncoming() const
+inline CaptureMotionConstPtr ProcessorMotion::getIncoming() const
+{
+ return incoming_ptr_;
+}
+
+inline CaptureMotionPtr ProcessorMotion::getOrigin()
+{
+ return origin_ptr_;
+}
+
+inline CaptureMotionPtr ProcessorMotion::getLast()
+{
+ return last_ptr_;
+}
+
+inline CaptureMotionPtr ProcessorMotion::getIncoming()
{
return incoming_ptr_;
}
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index f3a50080d135b9c7d19444c2b7bed4f0237f6e2a..fea39c4d6808a7a798a83d7b8b99dfbb47abec9d 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -107,7 +107,7 @@ class ProcessorOdom2d : public ProcessorMotion
FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin) override;
- VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
@@ -128,7 +128,7 @@ inline Eigen::VectorXd ProcessorOdom2d::correctDelta (const Eigen::VectorXd& del
return delta_corrected;
}
-inline VectorXd ProcessorOdom2d::getCalibration (const CaptureBasePtr _capture) const
+inline VectorXd ProcessorOdom2d::getCalibration (const CaptureBaseConstPtr _capture) const
{
return VectorXd::Zero(0);
}
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index 83baf370b1b536bc8e472cab19200abdc34d0314..02cdbd5ef4480ccd1a0ba28ab0db5c44886fa58c 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -129,7 +129,7 @@ class ProcessorOdom3d : public ProcessorMotion
FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
- VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index f03ed4ea4d5e2ae46864479e65ad19c7d16aa9a9..d93c9baa367b8ba65374a4b64d21bab194ce1d2a 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -138,9 +138,12 @@ class ProcessorTracker : public ProcessorBase
StateStructure getStateStructure() const;
- virtual CaptureBasePtr getOrigin() const;
- virtual CaptureBasePtr getLast() const;
- virtual CaptureBasePtr getIncoming() const;
+ virtual CaptureBaseConstPtr getOrigin() const;
+ virtual CaptureBasePtr getOrigin();
+ virtual CaptureBaseConstPtr getLast() const;
+ virtual CaptureBasePtr getLast();
+ virtual CaptureBaseConstPtr getIncoming() const;
+ virtual CaptureBasePtr getIncoming();
protected:
/** \brief process an incoming capture
@@ -261,7 +264,8 @@ class ProcessorTracker : public ProcessorBase
public:
- FeatureBasePtrList& getNewFeaturesListLast();
+ FeatureBaseConstPtrList getNewFeaturesListLast() const;
+ FeatureBasePtrList getNewFeaturesListLast();
std::string print() const {
return this->params_tracker_->print();
@@ -286,7 +290,15 @@ class ProcessorTracker : public ProcessorBase
};
-inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListLast()
+inline FeatureBaseConstPtrList ProcessorTracker::getNewFeaturesListLast() const
+{
+ FeatureBaseConstPtrList list_const;
+ for (auto&& obj_ptr : new_features_last_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FeatureBasePtrList ProcessorTracker::getNewFeaturesListLast()
{
return new_features_last_;
}
@@ -311,17 +323,32 @@ inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
new_features_incoming_.push_back(_feature_ptr);
}
-inline CaptureBasePtr ProcessorTracker::getOrigin() const
+inline CaptureBaseConstPtr ProcessorTracker::getOrigin() const
+{
+ return origin_ptr_;
+}
+
+inline CaptureBasePtr ProcessorTracker::getOrigin()
{
return origin_ptr_;
}
-inline CaptureBasePtr ProcessorTracker::getLast() const
+inline CaptureBaseConstPtr ProcessorTracker::getLast() const
{
return last_ptr_;
}
-inline CaptureBasePtr ProcessorTracker::getIncoming() const
+inline CaptureBasePtr ProcessorTracker::getLast()
+{
+ return last_ptr_;
+}
+
+inline CaptureBaseConstPtr ProcessorTracker::getIncoming() const
+{
+ return incoming_ptr_;
+}
+
+inline CaptureBasePtr ProcessorTracker::getIncoming()
{
return incoming_ptr_;
}
diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index e60a59f8233abb7fade714f20cbabc417a76e091..f609dc8efefbbb6fb7c74b7dac46ec8f052b0403 100644
--- a/include/core/processor/track_matrix.h
+++ b/include/core/processor/track_matrix.h
@@ -45,9 +45,12 @@ using std::list;
using std::pair;
using std::shared_ptr;
-typedef map<TimeStamp, FeatureBasePtr> Track;
-typedef map<size_t, FeatureBasePtr > Snapshot;
-typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id
+typedef map<TimeStamp, FeatureBasePtr> Track;
+typedef map<TimeStamp, FeatureBaseConstPtr> TrackConst;
+typedef map<SizeStd, FeatureBasePtr > Snapshot;
+typedef map<SizeStd, FeatureBaseConstPtr > SnapshotConst;
+typedef map<SizeStd, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id
+typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> > TrackMatchesConst; // matched feature pairs indexed by track_id
/** \brief Matrix of tracked features, by track and by snapshot (Captures or time stamps)
* This class implements the following data structure:
@@ -73,7 +76,7 @@ typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // mat
*
* Snapshot: capture-wise: a set of features tracked in a single Capture, indexed by track id:
*
- * map<size_t track_id, FeatureBasePtr f>
+ * map<SizeStd track_id, FeatureBasePtr f>
*
* The class makes sure that both accesses are consistent each time some addition or removal of features is performed.
*
@@ -107,25 +110,33 @@ class TrackMatrix
void remove (CaptureBasePtr _cap);
SizeStd numTracks () const;
SizeStd trackSize (const SizeStd& _track_id) const;
- Track track (const SizeStd& _track_id) const;
- Snapshot snapshot (CaptureBasePtr _capture) const;
- vector<FeatureBasePtr>
- trackAsVector(const SizeStd& _track_id) const;
- list<FeatureBasePtr>
- snapshotAsList(CaptureBasePtr _cap) const;
- TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2) const;
- FeatureBasePtr firstFeature(const SizeStd& _track_id) const;
- FeatureBasePtr lastFeature (const SizeStd& _track_id) const;
- FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap) const;
- CaptureBasePtr firstCapture(const SizeStd& _track_id) const;
-
- list<size_t> trackIds() const;
+ TrackConst track (const SizeStd& _track_id) const;
+ Track track (const SizeStd& _track_id);
+ SnapshotConst snapshot (CaptureBaseConstPtr _capture) const;
+ Snapshot snapshot (CaptureBasePtr _capture);
+ vector<FeatureBaseConstPtr> trackAsVector(const SizeStd& _track_id) const;
+ vector<FeatureBasePtr> trackAsVector(const SizeStd& _track_id);
+ FeatureBaseConstPtrList snapshotAsList(CaptureBaseConstPtr _cap) const;
+ FeatureBasePtrList snapshotAsList(CaptureBasePtr _cap);
+ TrackMatchesConst matches (CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const;
+ TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2);
+ FeatureBaseConstPtr firstFeature(const SizeStd& _track_id) const;
+ FeatureBasePtr firstFeature(const SizeStd& _track_id);
+ FeatureBaseConstPtr lastFeature (const SizeStd& _track_id) const;
+ FeatureBasePtr lastFeature (const SizeStd& _track_id);
+ FeatureBaseConstPtr feature (const SizeStd& _track_id, CaptureBaseConstPtr _cap) const;
+ FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap);
+ CaptureBaseConstPtr firstCapture(const SizeStd& _track_id) const;
+ CaptureBasePtr firstCapture(const SizeStd& _track_id);
+
+ list<SizeStd> trackIds() const;
// tracks across captures that belong to keyframe
- Track trackAtKeyframes(size_t _track_id) const;
+ TrackConst trackAtKeyframes(const SizeStd& _track_id) const;
+ Track trackAtKeyframes(const SizeStd& _track_id);
- const map<SizeStd, Track>& getTracks() const {return tracks_;}
- const map<CaptureBasePtr, Snapshot >& getSnapshots() const {return snapshots_;}
+ const map<SizeStd, Track>& getTracks() {return tracks_;}
+ const map<CaptureBasePtr, Snapshot >& getSnapshots() {return snapshots_;}
private:
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 0fdcac98b7c6c317b29f2a6dd718ae155db6e018..8e6c6db7c6657037e7df0eddeea9b683635e5b71 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -40,7 +40,6 @@ class StateBlock;
namespace wolf {
-
/*
* Macro for defining Autoconf sensor creator.
*
@@ -86,8 +85,6 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _ex
} \
-
-
/** \brief base struct for intrinsic sensor parameters
*
* Derive from this struct to create structs of sensor intrinsic parameters.
@@ -120,12 +117,12 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
std::map<char, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
+ CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
+
protected:
Eigen::VectorXd noise_std_; // std of sensor noise
Eigen::MatrixXd noise_cov_; // cov matrix of noise
- CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
-
void setProblem(ProblemPtr _problem) override final;
public:
@@ -177,8 +174,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
unsigned int id() const;
-
- HardwareBasePtr getHardware() const;
+ HardwareBaseConstPtr getHardware() const;
+ HardwareBasePtr getHardware();
private:
void setHardware(const HardwareBasePtr _hw_ptr);
@@ -186,40 +183,52 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
void removeProcessor(ProcessorBasePtr _proc_ptr);
public:
- const ProcessorBasePtrList& getProcessorList() const;
+ ProcessorBaseConstPtrList getProcessorList() const;
+ ProcessorBasePtrList getProcessorList();
- CaptureBasePtr getLastCapture() const;
+ CaptureBaseConstPtr getLastCapture() const;
+ CaptureBasePtr getLastCapture();
void setLastCapture(CaptureBasePtr);
void updateLastCapture();
- CaptureBasePtr findLastCaptureBefore(const TimeStamp& _ts) const;
+ CaptureBaseConstPtr findLastCaptureBefore(const TimeStamp& _ts) const;
+ CaptureBasePtr findLastCaptureBefore(const TimeStamp& _ts);
bool process(const CaptureBasePtr capture_ptr);
// State blocks
using HasStateBlocks::addStateBlock;
StateBlockPtr addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false);
- StateBlockPtr getStateBlockDynamic(const char& _key) const;
- StateBlockPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const;
+ StateBlockConstPtr getStateBlockDynamic(const char& _key) const;
+ StateBlockPtr getStateBlockDynamic(const char& _key);
+ StateBlockConstPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const;
+ StateBlockPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts);
// Declare a state block as dynamic or static (with _dymanic = false)
void setStateBlockDynamic(const char& _key, bool _dynamic = true);
bool isStateBlockDynamic(const char& _key) const;
- bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const;
- bool isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) const;
+ bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBaseConstPtr& _cap) const;
+ bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap);
+ bool isStateBlockInCapture(const char& _key, CaptureBaseConstPtr& _cap) const;
+ bool isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap);
bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts) const;
bool isStateBlockInCapture(const char& _key) const;
- StateBlockPtr getP(const TimeStamp _ts) const;
- StateBlockPtr getO(const TimeStamp _ts) const;
- StateBlockPtr getIntrinsic(const TimeStamp _ts) const;
- StateBlockPtr getP() const;
- StateBlockPtr getO() const;
- StateBlockPtr getIntrinsic() const;
+ StateBlockConstPtr getP(const TimeStamp _ts) const;
+ StateBlockPtr getP(const TimeStamp _ts);
+ StateBlockConstPtr getO(const TimeStamp _ts) const;
+ StateBlockPtr getO(const TimeStamp _ts);
+ StateBlockConstPtr getIntrinsic(const TimeStamp _ts) const;
+ StateBlockPtr getIntrinsic(const TimeStamp _ts);
+ StateBlockConstPtr getP() const;
+ StateBlockPtr getP();
+ StateBlockConstPtr getO() const;
+ StateBlockPtr getO();
+ StateBlockConstPtr getIntrinsic() const;
+ StateBlockPtr getIntrinsic();
protected:
- void removeStateBlocks();
- virtual void registerNewStateBlocks() const;
+ virtual void registerNewStateBlocks(ProblemPtr _problem) override;
public:
@@ -274,8 +283,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, SensorBaseConstPtr _sen_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
void link(HardwareBasePtr);
template<typename classType, typename... T>
@@ -304,12 +313,25 @@ inline unsigned int SensorBase::id() const
return sensor_id_;
}
-inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
+inline ProcessorBaseConstPtrList SensorBase::getProcessorList() const
+{
+ ProcessorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : processor_list_)
+ list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline ProcessorBasePtrList SensorBase::getProcessorList()
{
return processor_list_;
}
-inline CaptureBasePtr SensorBase::getLastCapture() const
+inline CaptureBaseConstPtr SensorBase::getLastCapture() const
+{
+ return last_capture_;
+}
+
+inline CaptureBasePtr SensorBase::getLastCapture()
{
return last_capture_;
}
@@ -344,7 +366,12 @@ inline Eigen::MatrixXd SensorBase::getNoiseCov() const
return noise_cov_;
}
-inline HardwareBasePtr SensorBase::getHardware() const
+inline HardwareBaseConstPtr SensorBase::getHardware() const
+{
+ return hardware_ptr_.lock();
+}
+
+inline HardwareBasePtr SensorBase::getHardware()
{
return hardware_ptr_.lock();
}
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index f1bcd4519ae9fdb88fe182cf45270740cacc452f..ac82297f5e36b5130151bd1b0464a110264b5fe7 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -47,14 +47,18 @@ class HasStateBlocks
const StateStructure& getStructure() const { return structure_; }
void appendToStructure(const char& _frame_type){ structure_ += _frame_type; }
- bool isInStructure(const char& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
+ bool isInStructure(const char& _sb_type) const { return structure_.find(_sb_type) != std::string::npos; }
- const std::unordered_map<char, StateBlockPtr>& getStateBlockMap() const;
- std::vector<StateBlockPtr> getStateBlockVec() const;
+ const std::unordered_map<char, StateBlockConstPtr>& getStateBlockMap() const;
+ const std::unordered_map<char, StateBlockPtr>& getStateBlockMap();
+ std::vector<StateBlockConstPtr> getStateBlockVec() const;
+ std::vector<StateBlockPtr> getStateBlockVec();
// Some typical shortcuts -- not all should be coded here, see notes below.
- StateBlockPtr getP() const;
- StateBlockPtr getO() const;
+ StateBlockConstPtr getP() const;
+ StateBlockConstPtr getO() const;
+ StateBlockPtr getP();
+ StateBlockPtr getO();
// These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
virtual void fix();
@@ -64,11 +68,13 @@ class HasStateBlocks
virtual StateBlockPtr addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem);
virtual unsigned int removeStateBlock(const char& _sb_type);
bool hasStateBlock(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
- bool hasStateBlock(const StateBlockPtr& _sb) const;
- StateBlockPtr getStateBlock(const char& _sb_type) const;
+ bool hasStateBlock(const StateBlockConstPtr& _sb) const;
bool setStateBlock(const char _sb_type, const StateBlockPtr& _sb);
- bool stateBlockKey(const StateBlockPtr& _sb, char& _key) const;
- std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb) const;
+ bool stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const;
+ StateBlockConstPtr getStateBlock(const char& _sb_type) const;
+ StateBlockPtr getStateBlock(const char& _sb_type);
+ std::unordered_map<char, StateBlockConstPtr>::const_iterator find(const StateBlockConstPtr& _sb) const;
+ std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb);
// Emplace derived state blocks (angle, quaternion, etc).
template<typename SB, typename ... Args>
@@ -79,8 +85,8 @@ class HasStateBlocks
StateBlockPtr emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
// Register/remove state blocks to/from wolf::Problem
- void registerNewStateBlocks(ProblemPtr _problem) const;
- void removeStateBlocks(ProblemPtr _problem);
+ virtual void registerNewStateBlocks(ProblemPtr _problem);
+ virtual void removeStateBlocks(ProblemPtr _problem);
// States
VectorComposite getState(const StateStructure& structure="") const;
@@ -103,6 +109,7 @@ class HasStateBlocks
private:
StateStructure structure_;
std::unordered_map<char, StateBlockPtr> state_block_map_;
+ std::unordered_map<char, StateBlockConstPtr> state_block_const_map_;
};
@@ -117,7 +124,8 @@ namespace wolf{
inline HasStateBlocks::HasStateBlocks() :
structure_(std::string("")),
- state_block_map_()
+ state_block_map_(),
+ state_block_const_map_()
{
//
}
@@ -127,14 +135,19 @@ inline HasStateBlocks::~HasStateBlocks()
//
}
-inline const std::unordered_map<char, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const
+inline const std::unordered_map<char, StateBlockConstPtr>& HasStateBlocks::getStateBlockMap() const
+{
+ return state_block_const_map_;
+}
+
+inline const std::unordered_map<char, StateBlockPtr>& HasStateBlocks::getStateBlockMap()
{
return state_block_map_;
}
-inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() const
+inline std::vector<StateBlockConstPtr> HasStateBlocks::getStateBlockVec() const
{
- std::vector<StateBlockPtr> sbv;
+ std::vector<StateBlockConstPtr> sbv;
for (auto& key : structure_)
{
sbv.push_back(getStateBlock(key));
@@ -142,20 +155,26 @@ inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() const
return sbv;
}
-//inline unsigned int HasStateBlocks::removeStateBlock(const char _sb_type)
-//{
-// return removeStateBlock(std::string(1, _sb_type));
-//}
+inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec()
+{
+ std::vector<StateBlockPtr> sbv;
+ for (auto& key : structure_)
+ {
+ sbv.push_back(getStateBlock(key));
+ }
+ return sbv;
+}
inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_type)
{
return state_block_map_.erase(_sb_type);
+ return state_block_const_map_.erase(_sb_type);
}
template<typename SB, typename ... Args>
inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
{
- assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
addStateBlock(_sb_type, sb, _problem);
@@ -166,7 +185,7 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_typ
template<typename ... Args>
inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor)
{
- assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
addStateBlock(_sb_type, sb, _problem);
@@ -177,12 +196,22 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type, Pro
inline bool HasStateBlocks::setStateBlock(const char _sb_type, const StateBlockPtr& _sb)
{
assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
- assert ( state_block_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
+ assert ( state_block_map_.count(_sb_type) > 0 && state_block_const_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
state_block_map_.at(_sb_type) = _sb;
+ state_block_const_map_.at(_sb_type) = _sb;
return true; // success
}
-inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type) const
+inline wolf::StateBlockConstPtr HasStateBlocks::getStateBlock(const char& _sb_type) const
+{
+ auto it = state_block_const_map_.find(_sb_type);
+ if (it != state_block_const_map_.end())
+ return it->second;
+ else
+ return nullptr;
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type)
{
auto it = state_block_map_.find(_sb_type);
if (it != state_block_map_.end())
@@ -191,12 +220,22 @@ inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type) c
return nullptr;
}
-inline wolf::StateBlockPtr HasStateBlocks::getP() const
+inline wolf::StateBlockConstPtr HasStateBlocks::getP() const
{
return getStateBlock('P');
}
-inline wolf::StateBlockPtr HasStateBlocks::getO() const
+inline wolf::StateBlockPtr HasStateBlocks::getP()
+{
+ return getStateBlock('P');
+}
+
+inline wolf::StateBlockConstPtr HasStateBlocks::getO() const
+{
+ return getStateBlock('O');
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getO()
{
return getStateBlock('O');
}
@@ -358,7 +397,20 @@ inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) co
return size;
}
-inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
+inline std::unordered_map<char, StateBlockConstPtr>::const_iterator HasStateBlocks::find(const StateBlockConstPtr& _sb) const
+{
+ const auto& it = std::find_if(state_block_const_map_.begin(),
+ state_block_const_map_.end(),
+ [_sb](const std::pair<char, StateBlockConstPtr>& pair)
+ {
+ return pair.second == _sb;
+ }
+ );
+
+ return it;
+}
+
+inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb)
{
const auto& it = std::find_if(state_block_map_.begin(),
state_block_map_.end(),
@@ -371,18 +423,16 @@ inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::f
return it;
}
-inline bool HasStateBlocks::hasStateBlock(const StateBlockPtr &_sb) const
+inline bool HasStateBlocks::hasStateBlock(const StateBlockConstPtr &_sb) const
{
- const auto& it = this->find(_sb);
-
- return it != state_block_map_.end();
+ return this->find(_sb) != state_block_const_map_.end();
}
-inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, char& _key) const
+inline bool HasStateBlocks::stateBlockKey(const StateBlockConstPtr &_sb, char& _key) const
{
const auto& it = this->find(_sb);
- bool found = (it != state_block_map_.end());
+ bool found = (it != state_block_const_map_.end());
if (found)
{
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 46cce552ccbfe28ec30981b905b191ec2b9456cc..21b1ab5be7080da9334a979274d5f4513f9a23f0 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -35,47 +35,14 @@ class FrameBase;
#include "core/common/time_stamp.h"
#include "core/state_block/state_composite.h"
-//std includes
-
namespace wolf {
-class TrajectoryIter : public std::map<TimeStamp, FrameBasePtr>::const_iterator
-{
- public:
- TrajectoryIter(std::map<TimeStamp, FrameBasePtr>::const_iterator src)
- : std::map<TimeStamp, FrameBasePtr>::const_iterator(std::move(src))
- {
-
- }
-
- // override the indirection operator
- const FrameBasePtr& operator*() const {
- return std::map<TimeStamp, FrameBasePtr>::const_iterator::operator*().second;
- }
-};
-
-class TrajectoryRevIter : public std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator
-{
- public:
- TrajectoryRevIter(std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator src)
- : std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator(std::move(src))
- {
-
- }
-
- // override the indirection operator
- const FrameBasePtr& operator*() const {
- return std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator::operator*().second;
- }
-};
-
-//class TrajectoryBase
class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<TrajectoryBase>
{
friend FrameBase;
private:
- FrameMap frame_map_;
+ FramePtrMap frame_map_;
protected:
StateStructure frame_structure_; // Defines the structure of the Frames in the Trajectory.
@@ -85,14 +52,22 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
~TrajectoryBase() override;
// Frames
- const FrameMap& getFrameMap() const;
- FrameBasePtr getLastFrame() const;
- FrameBasePtr getFirstFrame() const;
- FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
- TrajectoryIter begin() const;
- TrajectoryIter end() const;
- TrajectoryRevIter rbegin() const;
- TrajectoryRevIter rend() const;
+ SizeEigen size() const;
+ FrameConstPtrMap getFrameMap() const;
+ FramePtrMap getFrameMap();
+ FrameBaseConstPtr getLastFrame() const;
+ FrameBasePtr getLastFrame();
+ FrameBaseConstPtr getFirstFrame() const;
+ FrameBasePtr getFirstFrame();
+ FrameBaseConstPtr getNextFrame(FrameBaseConstPtr, const unsigned int& i = 1) const;
+ FrameBasePtr getNextFrame(FrameBasePtr, const unsigned int& i = 1);
+ FrameBaseConstPtr getPreviousFrame(FrameBaseConstPtr, const unsigned int& i = 1) const;
+ FrameBasePtr getPreviousFrame(FrameBasePtr, const unsigned int& i = 1);
+ 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, //
@@ -107,52 +82,80 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, std::ostream& _stream, std::string _tabs = "") const;
- bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, TrajectoryBaseConstPtr _trj_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
void removeFrame(FrameBasePtr _frame_ptr);
public:
// factors
- void getFactorList(FactorBasePtrList & _fac_list) const;
+ void getFactorList(FactorBaseConstPtrList & _fac_list) const;
+ void getFactorList(FactorBasePtrList & _fac_list);
};
-inline const FrameMap& TrajectoryBase::getFrameMap() const
+inline FrameConstPtrMap TrajectoryBase::getFrameMap() const
+{
+ FrameConstPtrMap map_const;
+ for (auto&& pair : frame_map_)
+ map_const[pair.first] = pair.second;
+ return map_const;
+}
+
+inline FramePtrMap TrajectoryBase::getFrameMap()
{
return frame_map_;
}
-inline FrameBasePtr TrajectoryBase::getFirstFrame() const
+inline FrameBaseConstPtr TrajectoryBase::getFirstFrame() const
+{
+ if (frame_map_.empty())
+ return nullptr;
+ return frame_map_.begin()->second;
+}
+
+inline FrameBasePtr TrajectoryBase::getFirstFrame()
{
- auto it = static_cast<TrajectoryIter>(frame_map_.begin());
- return *it;
+ if (frame_map_.empty())
+ return nullptr;
+ return frame_map_.begin()->second;
}
-inline TrajectoryIter TrajectoryBase::begin() const
+
+inline FrameBaseConstPtr TrajectoryBase::getLastFrame() const
{
- return static_cast<TrajectoryIter>(frame_map_.begin());
+ if (frame_map_.empty())
+ return nullptr;
+ return frame_map_.rbegin()->second;
}
-inline TrajectoryIter TrajectoryBase::end() const
+
+inline FrameBasePtr TrajectoryBase::getLastFrame()
{
- return static_cast<TrajectoryIter>(frame_map_.end());
+ if (frame_map_.empty())
+ return nullptr;
+ return frame_map_.rbegin()->second;
}
-inline TrajectoryRevIter TrajectoryBase::rbegin() const
+
+inline SizeEigen TrajectoryBase::size() const
{
- return static_cast<TrajectoryRevIter>(frame_map_.rbegin());
+ return frame_map_.size();
}
-inline TrajectoryRevIter TrajectoryBase::rend() const
+
+inline bool TrajectoryBase::hasTimeStamp(const TimeStamp& _ts) const
{
- return static_cast<TrajectoryRevIter>(frame_map_.rend());
+ return frame_map_.count(_ts) != 0;
}
-inline FrameBasePtr TrajectoryBase::getLastFrame() const
+inline bool TrajectoryBase::hasFrame(FrameBaseConstPtr _frame) const
{
- // return last_key_frame_ptr_;
- auto last = this->rbegin();
- if(last != this->rend()) return *(this->rbegin());
- else return nullptr;
+ return std::find_if(frame_map_.begin(),
+ frame_map_.end(),
+ [&_frame](std::pair<TimeStamp,FrameBaseConstPtr> frm_it)
+ {
+ return frm_it.second == _frame;
+ }) != frame_map_.end();
}
+
} // namespace wolf
#endif
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 8e91f7e75caedf348de594a725f0d0ee2c707d3d..e6d4a1e660bd0664f2fa1196c2ab558840664263 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);
}
@@ -126,8 +129,6 @@ bool CaptureBase::process()
return getSensor()->process(shared_from_this());
}
-
-
FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
{
//std::cout << "Adding feature" << std::endl;
@@ -140,14 +141,27 @@ void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr)
feature_list_.remove(_ft_ptr);
}
-FactorBasePtrList CaptureBase::getFactorList() const
+FactorBaseConstPtrList CaptureBase::getFactorList() const
+{
+ FactorBaseConstPtrList fac_list;
+ getFactorList(fac_list);
+ return fac_list;
+}
+
+FactorBasePtrList CaptureBase::getFactorList()
{
FactorBasePtrList fac_list;
getFactorList(fac_list);
return fac_list;
}
-void CaptureBase::getFactorList(FactorBasePtrList& _fac_list) const
+void CaptureBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
+{
+ for (auto f_ptr : getFeatureList())
+ f_ptr->getFactorList(_fac_list);
+}
+
+void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
{
for (auto f_ptr : getFeatureList())
f_ptr->getFactorList(_fac_list);
@@ -164,23 +178,34 @@ void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
-bool CaptureBase::isConstrainedBy(const FactorBasePtr &_factor) const
+bool CaptureBase::hasFeature(const FeatureBaseConstPtr &_feature) const
{
- FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
- constrained_by_list_.end(),
- [_factor](const FactorBasePtr & cby)
- {
- return cby == _factor;
- }
- );
- if (cby_it != constrained_by_list_.end())
- return true;
- else
- return false;
+ 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_list_.begin(),
+ constrained_by_list_.end(),
+ _factor) != constrained_by_list_.end();
+}
+
+StateBlockConstPtr CaptureBase::getStateBlock(const char& _key) const
+{
+ if (getSensor() and getSensor()->hasStateBlock(_key))
+ {
+ if (getSensor()->isStateBlockDynamic(_key))
+ return HasStateBlocks::getStateBlock(_key);
+ else
+ return getSensor()->getStateBlock(_key);
+ }
+ else // No sensor associated, or sensor without this state block: assume sensor params are here
+ return HasStateBlocks::getStateBlock(_key);
+}
-StateBlockPtr CaptureBase::getStateBlock(const char& _key) const
+StateBlockPtr CaptureBase::getStateBlock(const char& _key)
{
if (getSensor() and getSensor()->hasStateBlock(_key))
{
@@ -303,7 +328,7 @@ void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b
f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
-CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog CaptureBase::localCheck(bool _verbose, CaptureBaseConstPtr _cap_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -346,12 +371,12 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::os
// 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;
}
@@ -377,48 +402,52 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::os
}
}
- auto frm_cap = _cap_ptr->getFrame();
- inconsistency_explanation << "Cap" << id() << " @ " << _cap_ptr
- << " ---> Frm" << frm_cap->id() << " @ " << frm_cap
- << " -X-> Frm" << id() << "\n";
- auto frm_cap_list = frm_cap->getCaptureList();
- auto frame_has_cap = std::find_if(frm_cap_list.begin(), frm_cap_list.end(), [&_cap_ptr](CaptureBasePtr cap){ return cap == _cap_ptr;});
- log.assertTrue(frame_has_cap != frm_cap_list.end(), inconsistency_explanation);
+ // check frame
+ auto frm_cap = _cap_ptr->getFrame();
+ inconsistency_explanation << "Cap" << id() << " @ " << _cap_ptr
+ << " ---> Frm" << frm_cap->id() << " @ " << frm_cap
+ << " -X-> Frm" << id() << "\n";
+ auto frm_cap_list = frm_cap->getCaptureList();
+ auto frame_has_cap = std::find(frm_cap_list.begin(), frm_cap_list.end(), _cap_ptr);
+ log.assertTrue(frame_has_cap != frm_cap_list.end(), inconsistency_explanation);
- for(auto f : getFeatureList())
- {
- inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr
- << " ---> Ftr" << f->id() << " @ " << f
- << " -X-> Cap" << id();
- log.assertTrue((f->getCapture() == _cap_ptr), inconsistency_explanation);
- }
- //Check that the capture is within time tolerance of some processor
- auto frame = getFrame();
- double time_diff = fabs(getTimeStamp() - frame->getTimeStamp());
-
- //It looks like some gtests add captures by hand, without using processors, so the processor list is empty.
- //This inicialization is needed because if the list empty the execution does not go into the loop and the
- //assertion fails
- bool match_any_prc_timetolerance;
- if(getSensor() != nullptr )
+
+ // check features
+ for(auto f : getFeatureList())
+ {
+ inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr
+ << " ---> Ftr" << f->id() << " @ " << f
+ << " -X-> Cap" << id();
+ log.assertTrue((f->getCapture() == _cap_ptr), inconsistency_explanation);
+ }
+
+ //Check that the capture is within time tolerance of some processor
+ auto frame = getFrame();
+ double time_diff = fabs(getTimeStamp() - frame->getTimeStamp());
+
+ //It looks like some gtests add captures by hand, without using processors, so the processor list is empty.
+ //This inicialization is needed because if the list empty the execution does not go into the loop and the
+ //assertion fails
+ bool match_any_prc_timetolerance;
+ if(getSensor() != nullptr )
+ {
+ match_any_prc_timetolerance = getSensor()->getProcessorList().empty();
+ for(auto prc : getSensor()->getProcessorList())
{
- match_any_prc_timetolerance = getSensor()->getProcessorList().empty();
- for(auto const& prc : getSensor()->getProcessorList())
- {
- match_any_prc_timetolerance = match_any_prc_timetolerance or (time_diff <= prc->getTimeTolerance());
- }
- inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr
- << " ts =" << getTimeStamp() << "Frm" << frame->id()
- << " ts = " << frame->getTimeStamp() << " their time difference (" << time_diff << ") does not match any time tolerance of"
- << " any processor in sensor " << getSensor()->id() << "\n";
- log.assertTrue((match_any_prc_timetolerance), inconsistency_explanation);
+ match_any_prc_timetolerance = match_any_prc_timetolerance or (time_diff <= prc->getTimeTolerance());
}
+ inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr
+ << " ts =" << getTimeStamp() << "Frm" << frame->id()
+ << " ts = " << frame->getTimeStamp() << " their time difference (" << time_diff << ") does not match any time tolerance of"
+ << " any processor in sensor " << getSensor()->id() << "\n";
+ log.assertTrue((match_any_prc_timetolerance), inconsistency_explanation);
+ }
return log;
}
-bool CaptureBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool CaptureBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto sen_ptr = std::static_pointer_cast<CaptureBase>(_node_ptr);
+ auto sen_ptr = std::static_pointer_cast<const CaptureBase>(_node_ptr);
auto local_log = localCheck(_verbose, sen_ptr, _stream, _tabs);
_log.compose(local_log);
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index 3f925fd62a6acb93d7bcff2b3a45fbb28edc1bd7..e8f48dcd7b52cb8b5d2dc0ba95cc0c0f689de9ea 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -68,7 +68,7 @@ CaptureMotion::~CaptureMotion()
//
}
-bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolerance)
+bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolerance) const
{
assert(_ts.ok() and this->time_stamp_.ok());
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index ab8753351aee84c5b4f290af171b370663060499..c83686ef4100dfa4cee61e576f99fa0759a1e94a 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -118,19 +118,21 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
// first create a vector containing all state blocks
std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
//frame state blocks
- for(auto fr_ptr : *wolf_problem_->getTrajectory())
- for (const auto& key : fr_ptr->getStructure())
+ for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
+ for (auto key : fr_pair.second->getStructure())
{
- const auto& sb = fr_ptr->getStateBlock(key);
+ const auto& sb = fr_pair.second->getStateBlock(key);
all_state_blocks.push_back(sb);
}
// 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++)
{
@@ -149,15 +151,15 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
case CovarianceBlocksToBeComputed::ALL_MARGINALS:
{
// first create a vector containing all state blocks
- for(auto fr_ptr : *wolf_problem_->getTrajectory())
- for (const auto& key1 : wolf_problem_->getFrameStructure())
+ for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
+ for (auto key1 : fr_pair.second->getStructure())
{
- const auto& sb1 = fr_ptr->getStateBlock(key1);
+ const auto& sb1 = fr_pair.second->getStateBlock(key1);
assert(isStateBlockRegisteredDerived(sb1));
- for (const auto& key2 : wolf_problem_->getFrameStructure())
+ for (auto key2 : fr_pair.second->getStructure())
{
- const auto& sb2 = fr_ptr->getStateBlock(key2);
+ const auto& sb2 = fr_pair.second->getStateBlock(key2);
assert(isStateBlockRegisteredDerived(sb2));
state_block_pairs.emplace_back(sb1, 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_analytic.cpp b/src/factor/factor_analytic.cpp
index bf7fb3d0256dde347713302ab1daee5353655bbc..b41394215b429155edd7d23afcc7d41916456c40 100644
--- a/src/factor/factor_analytic.cpp
+++ b/src/factor/factor_analytic.cpp
@@ -32,12 +32,49 @@ FactorAnalytic::FactorAnalytic(const std::string& _tp,
const FeatureBasePtr& _feature_other_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status,
+ bool _apply_loss_function,
+ FactorStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorAnalytic(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
+ _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr){}
+
+FactorAnalytic::FactorAnalytic(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
+ FactorBase(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_list,
+ _capture_other_list,
+ _feature_other_list,
+ _landmark_other_list,
+ _processor_ptr,
+ _apply_loss_function,
+ _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
- _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
+ _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
+ state_ptr_const_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
+ _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
_state1Ptr ? (unsigned int) _state1Ptr->getSize() : 0,
_state2Ptr ? (unsigned int) _state2Ptr->getSize() : 0,
@@ -52,7 +89,12 @@ FactorAnalytic::FactorAnalytic(const std::string& _tp,
resizeVectors();
}
-std::vector<StateBlockPtr> FactorAnalytic::getStateBlockPtrVector() const
+std::vector<StateBlockConstPtr> FactorAnalytic::getStateBlockPtrVector() const
+{
+ return state_ptr_const_vector_;
+}
+
+std::vector<StateBlockPtr> FactorAnalytic::getStateBlockPtrVector()
{
return state_ptr_vector_;
}
@@ -75,6 +117,7 @@ void FactorAnalytic::resizeVectors()
if (state_ptr_vector_.at(ii) == nullptr)
{
state_ptr_vector_.resize(ii);
+ state_ptr_const_vector_.resize(ii);
state_block_sizes_vector_.resize(ii);
break;
}
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 46151eba686f2fa4f12a1c734565575da362eeec..9ac1675b8b9ddc388f85b7fd6d8d7e34a274e65b 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -27,42 +27,6 @@ namespace wolf {
unsigned int FactorBase::factor_id_count_ = 0;
-FactorBase::FactorBase(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- NodeBase("FACTOR", _tp),
- feature_ptr_(), // will be filled in link()
- factor_id_(++factor_id_count_),
- topology_(_top),
- status_(_status),
- apply_loss_function_(_apply_loss_function),
- frame_other_list_(),
- capture_other_list_(),
- feature_other_list_(),
- landmark_other_list_(),
- processor_ptr_(_processor_ptr)
-{
- if (_frame_other_ptr)
- frame_other_list_.push_back(_frame_other_ptr);
- if (_capture_other_ptr)
- capture_other_list_.push_back(_capture_other_ptr);
- if (_feature_other_ptr)
- feature_other_list_.push_back(_feature_other_ptr);
- if (_landmark_other_ptr)
- landmark_other_list_.push_back(_landmark_other_ptr);
-
- assert(_feature_ptr && "null feature pointer when creating a factor");
- measurement_ = _feature_ptr->getMeasurement();
- measurement_sqrt_information_upper_ = _feature_ptr->getMeasurementSquareRootInformationUpper();
-}
-
FactorBase::FactorBase(const std::string& _tp,
const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
@@ -75,23 +39,23 @@ FactorBase::FactorBase(const std::string& _tp,
FactorStatus _status) :
NodeBase("FACTOR", _tp),
feature_ptr_(), // will be filled in link()
- factor_id_(++factor_id_count_),
- topology_(_top),
- status_(_status),
- apply_loss_function_(_apply_loss_function),
+ processor_ptr_(_processor_ptr),
frame_other_list_(),
capture_other_list_(),
feature_other_list_(),
landmark_other_list_(),
- processor_ptr_(_processor_ptr)
+ factor_id_(++factor_id_count_),
+ topology_(_top),
+ status_(_status),
+ apply_loss_function_(_apply_loss_function)
{
- for (auto& Fo : _frame_other_list)
+ for (auto Fo : _frame_other_list)
frame_other_list_.push_back(Fo);
- for (auto& Co : _capture_other_list)
+ for (auto Co : _capture_other_list)
capture_other_list_.push_back(Co);
- for (auto& fo : _feature_other_list)
+ for (auto fo : _feature_other_list)
feature_other_list_.push_back(fo);
- for (auto& Lo : landmark_other_list_)
+ for (auto Lo : _landmark_other_list)
landmark_other_list_.push_back(Lo);
assert(_feature_ptr && "null feature pointer when creating a factor");
@@ -174,21 +138,42 @@ void FactorBase::remove(bool viral_remove_empty_parent)
}
}
-CaptureBasePtr FactorBase::getCapture() const
+CaptureBaseConstPtr FactorBase::getCapture() const
{
auto ftr = getFeature();
if (ftr != nullptr) return ftr->getCapture();
else return nullptr;
}
-FrameBasePtr FactorBase::getFrame() const
+CaptureBasePtr FactorBase::getCapture()
+{
+ auto ftr = getFeature();
+ if (ftr != nullptr) return ftr->getCapture();
+ else return nullptr;
+}
+
+FrameBaseConstPtr FactorBase::getFrame() const
{
auto cpt = getCapture();
if(cpt != nullptr) return cpt->getFrame();
else return nullptr;
}
-SensorBasePtr FactorBase::getSensor() const
+FrameBasePtr FactorBase::getFrame()
+{
+ auto cpt = getCapture();
+ if(cpt != nullptr) return cpt->getFrame();
+ else return nullptr;
+}
+
+SensorBaseConstPtr FactorBase::getSensor() const
+{
+ auto cpt = getCapture();
+ if(cpt != nullptr) return cpt->getSensor();
+ else return nullptr;
+}
+
+SensorBasePtr FactorBase::getSensor()
{
auto cpt = getCapture();
if(cpt != nullptr) return cpt->getSensor();
@@ -209,60 +194,56 @@ void FactorBase::setStatus(FactorStatus _status)
status_ = _status;
}
-bool FactorBase::hasFrameOther(const FrameBasePtr &_frm_other) const
+bool FactorBase::hasFrameOther(const FrameBaseConstPtr &_frm_other) const
{
- FrameBaseWConstIter frm_it = find_if(frame_other_list_.begin(),
- frame_other_list_.end(),
- [_frm_other](const FrameBaseWPtr &frm_ow)
- {
- return frm_ow.lock() == _frm_other;
- }
- );
+ 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_list_.end())
return true;
return false;
}
-bool FactorBase::hasCaptureOther(const CaptureBasePtr &_cap_other) const
+bool FactorBase::hasCaptureOther(const CaptureBaseConstPtr &_cap_other) const
{
- CaptureBaseWConstIter cap_it = find_if(capture_other_list_.begin(),
- capture_other_list_.end(),
- [_cap_other](const CaptureBaseWPtr &cap_ow)
- {
- return cap_ow.lock() == _cap_other;
- }
- );
+ 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_list_.end())
return true;
return false;
}
-bool FactorBase::hasFeatureOther(const FeatureBasePtr &_ftr_other) const
+bool FactorBase::hasFeatureOther(const FeatureBaseConstPtr &_ftr_other) const
{
- FeatureBaseWConstIter ftr_it = find_if(feature_other_list_.begin(),
- feature_other_list_.end(),
- [_ftr_other](const FeatureBaseWPtr &ftr_ow)
- {
- return ftr_ow.lock() == _ftr_other;
- }
- );
+ 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_list_.end())
return true;
return false;
}
-bool FactorBase::hasLandmarkOther(const LandmarkBasePtr &_lmk_other) const
+bool FactorBase::hasLandmarkOther(const LandmarkBaseConstPtr &_lmk_other) const
{
- LandmarkBaseWConstIter lmk_it = find_if(landmark_other_list_.begin(),
- landmark_other_list_.end(),
- [_lmk_other](const LandmarkBaseWPtr &lmk_ow)
- {
- return lmk_ow.lock() == _lmk_other;
- }
- );
+ 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_list_.end())
return true;
@@ -335,16 +316,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;
@@ -355,7 +336,7 @@ void FactorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_bl
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
}
-CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -373,7 +354,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
}
// find constrained_by pointer in constrained frame
- for (const auto& Fow : getFrameOtherList())
+ for (auto Fow : getFrameOtherList())
{
if (!Fow.expired())
{
@@ -396,7 +377,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
if (_verbose && !getFrameOtherList().empty())
_stream << ")";
// find constrained_by pointer in constrained capture
- for (const auto& Cow : getCaptureOtherList())
+ for (auto Cow : getCaptureOtherList())
{
if (!Cow.expired())
{
@@ -420,7 +401,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
if (_verbose && !getCaptureOtherList().empty())
_stream << ")";
// find constrained_by pointer in constrained feature
- for (const auto& fow : getFeatureOtherList())
+ for (auto fow : getFeatureOtherList())
{
if (!fow.expired())
{
@@ -443,7 +424,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
_stream << ")";
// find constrained_by pointer in constrained landmark
- for (const auto& Low : getLandmarkOtherList())
+ for (auto Low : getLandmarkOtherList())
{
if (Low.expired())
{
@@ -468,6 +449,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
_stream << ")";
if (_verbose)
_stream << std::endl;
+
//Check Problem and feature ptrs
if (_verbose)
{
@@ -475,6 +457,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
_stream << _tabs << " " << "-> Ftr" << getFeature()->id() << " @ " << getFeature().get() << std::endl;
}
auto ftr_ptr = getFeature();
+
// check problem and feature pointers
inconsistency_explanation << "The factor " << id() << " @ " << _fac_ptr.get() << " problem ptr " << getProblem().get()
<< " is different from Feature's problem ptr " << ftr_ptr->getProblem().get() << "\n";
@@ -485,13 +468,13 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
<< " ---> Ftr" << ftr_ptr->id() << " @ " << ftr_ptr
<< " -X-> Fac" << id();
auto ftr_fac_list = ftr_ptr->getFactorList();
- auto ftr_has_fac = std::find_if(ftr_fac_list.begin(), ftr_fac_list.end(), [&_fac_ptr](FactorBasePtr fac){ return fac == _fac_ptr;});
+ auto ftr_has_fac = std::find_if(ftr_fac_list.begin(), ftr_fac_list.end(), [&_fac_ptr](FactorBaseConstPtr fac){ return fac == _fac_ptr;});
log.assertTrue(ftr_has_fac!= ftr_fac_list.end(), inconsistency_explanation);
// find state block pointers in all constrained nodes
- SensorBasePtr S = getSensor(); // get own sensor to check sb
- FrameBasePtr F = getFrame();
- CaptureBasePtr C = getCapture();
+ auto S = getSensor(); // get own sensor to check sb
+ auto F = getFrame();
+ auto C = getCapture();
for (auto sb : getStateBlockPtrVector())
{
@@ -537,7 +520,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
// find in constrained Frame
- for (const auto& Fow : getFrameOtherList())
+ for (auto Fow : getFrameOtherList())
{
if (!Fow.expired())
{
@@ -558,7 +541,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
}
// find in constrained Capture
- for (const auto& Cow : getCaptureOtherList())
+ for (auto Cow : getCaptureOtherList())
{
if (!Cow.expired())
{
@@ -570,7 +553,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
}
// find in constrained Feature
- for (const auto& fow : getFeatureOtherList())
+ for (auto fow : getFeatureOtherList())
{
if (!fow.expired())
{
@@ -596,7 +579,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
}
// find in constrained landmark
- for (const auto& Low : getLandmarkOtherList())
+ for (auto Low : getLandmarkOtherList())
{
if (!Low.expired())
{
@@ -626,9 +609,9 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
return log;
}
-bool FactorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool FactorBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto fac_ptr = std::static_pointer_cast<FactorBase>(_node_ptr);
+ auto fac_ptr = std::static_pointer_cast<const FactorBase>(_node_ptr);
auto local_log = localCheck(_verbose, fac_ptr, _stream, _tabs);
_log.compose(local_log);
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 5e3516cccc2f6095e2c4c8905ceb90649cfeda9e..fcef79178934508f840a2a2d51b918b85827c1af 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -102,7 +102,12 @@ void FeatureBase::removeFactor(FactorBasePtr _co_ptr)
factor_list_.remove(_co_ptr);
}
-FrameBasePtr FeatureBase::getFrame() const
+FrameBaseConstPtr FeatureBase::getFrame() const
+{
+ return capture_ptr_.lock()->getFrame();
+}
+
+FrameBasePtr FeatureBase::getFrame()
{
return capture_ptr_.lock()->getFrame();
}
@@ -118,27 +123,27 @@ void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
-bool FeatureBase::isConstrainedBy(const FactorBasePtr &_factor) const
+bool FeatureBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
{
- FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
- constrained_by_list_.end(),
- [_factor](const FactorBasePtr & cby)
- {
- return cby == _factor;
- }
- );
- if (cby_it != constrained_by_list_.end())
- return true;
- else
- return false;
+ return std::find(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ _factor) != constrained_by_list_.end();
}
-const FactorBasePtrList& FeatureBase::getFactorList() const
+bool FeatureBase::hasFactor(FactorBaseConstPtr _factor) const
{
- return factor_list_;
+ return std::find(factor_list_.begin(),
+ factor_list_.end(),
+ _factor) != factor_list_.end();
}
-void FeatureBase::getFactorList(FactorBasePtrList & _fac_list) const
+void FeatureBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
+{
+ // FIXME
+ _fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
+}
+
+void FeatureBase::getFactorList(FactorBasePtrList & _fac_list)
{
_fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
}
@@ -222,7 +227,7 @@ void FeatureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b
c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
-CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog FeatureBase::localCheck(bool _verbose, FeatureBaseConstPtr _ftr_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -248,7 +253,7 @@ CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::os
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;
}
@@ -257,17 +262,18 @@ CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::os
inconsistency_explanation << "constrained by Feature " << id() << " @ " << _ftr_ptr.get()
<< " not found among constrained-by factors\n";
log.assertTrue((cby->hasFeatureOther(_ftr_ptr)), inconsistency_explanation);
-
}
+ // Check capture
auto cap_ftr = _ftr_ptr->getCapture();
inconsistency_explanation << "Ftr" << id() << " @ " << _ftr_ptr
<< " ---> Cap" << cap_ftr->id() << " @ " << cap_ftr
<< " -X-> Ftr" << id();
auto cap_ftr_list = cap_ftr->getFeatureList();
- auto frame_has_cap = std::find_if(cap_ftr_list.begin(), cap_ftr_list.end(), [&_ftr_ptr](FeatureBasePtr ftr){ return ftr == _ftr_ptr;});
+ auto frame_has_cap = std::find(cap_ftr_list.begin(), cap_ftr_list.end(), _ftr_ptr);
log.assertTrue(frame_has_cap != cap_ftr_list.end(), inconsistency_explanation);
+ // Check factors
for(auto fac : getFactorList())
{
inconsistency_explanation << "Ftr" << id() << " @ " << _ftr_ptr
@@ -275,11 +281,12 @@ CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::os
<< " -X-> Ftr" << id();
log.assertTrue((fac->getFeature() == _ftr_ptr), inconsistency_explanation);
}
+
return log;
}
-bool FeatureBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool FeatureBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto ftr_ptr = std::static_pointer_cast<FeatureBase>(_node_ptr);
+ auto ftr_ptr = std::static_pointer_cast<const FeatureBase>(_node_ptr);
auto local_log = localCheck(_verbose, ftr_ptr, _stream, _tabs);
_log.compose(local_log);
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 951d544ea9afb33dbc412128f8cda51ac7d1cdb9..f295cdbca724a4e4ee89b8d2aa5c52ae487b537a 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -156,38 +156,38 @@ bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const
return getProblem()->getFrameCovariance(shared_from_this(), _cov);
}
-FrameBasePtr FrameBase::getPreviousFrame() const
+FrameBaseConstPtr FrameBase::getPreviousFrame(const unsigned int& i) const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
-
- assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
+ return getTrajectory()->getPreviousFrame(shared_from_this(), i);
+}
- if (current_frame_it == getTrajectory()->getFrameMap().begin())
- return nullptr;
+FrameBasePtr FrameBase::getPreviousFrame(const unsigned int& i)
+{
+ assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- return std::prev(current_frame_it)->second;
+ return getTrajectory()->getPreviousFrame(shared_from_this(), i);
}
-FrameBasePtr FrameBase::getNextFrame() const
+FrameBaseConstPtr FrameBase::getNextFrame(const unsigned int& i) const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
-
- assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
+ return getTrajectory()->getNextFrame(shared_from_this(), i);
+}
- if (std::next(current_frame_it) == getTrajectory()->getFrameMap().end())
- return nullptr;
+FrameBasePtr FrameBase::getNextFrame(const unsigned int& i)
+{
+ assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- return std::next(current_frame_it)->second;
+ return getTrajectory()->getNextFrame(shared_from_this(), i);
}
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_list_.push_back(_capt_ptr);
return _capt_ptr;
}
@@ -196,88 +196,177 @@ void FrameBase::removeCapture(CaptureBasePtr _capt_ptr)
capture_list_.remove(_capt_ptr);
}
-CaptureBasePtr FrameBase::getCaptureOfType(const std::string& type) const
+CaptureBaseConstPtr FrameBase::getCaptureOfType(const std::string& type) const
{
- for (CaptureBasePtr capture_ptr : getCaptureList())
+ for (auto capture_ptr : getCaptureList())
if (capture_ptr->getType() == type)
return capture_ptr;
return nullptr;
}
-CaptureBasePtrList FrameBase::getCapturesOfType(const std::string& type) const
+CaptureBasePtr FrameBase::getCaptureOfType(const std::string& type)
+{
+ for (auto capture_ptr : getCaptureList())
+ if (capture_ptr->getType() == type)
+ return capture_ptr;
+ return nullptr;
+}
+
+CaptureBaseConstPtrList FrameBase::getCapturesOfType(const std::string& type) const
+{
+ CaptureBaseConstPtrList captures;
+ for (auto capture_ptr : getCaptureList())
+ if (capture_ptr->getType() == type)
+ captures.push_back(capture_ptr);
+ return captures;
+}
+
+CaptureBasePtrList FrameBase::getCapturesOfType(const std::string& type)
{
CaptureBasePtrList captures;
- for (CaptureBasePtr capture_ptr : getCaptureList())
+ for (auto capture_ptr : getCaptureList())
if (capture_ptr->getType() == type)
captures.push_back(capture_ptr);
return captures;
}
-CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const
+CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const
+{
+ for (auto capture_ptr : getCaptureList())
+ if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
+ return capture_ptr;
+ return nullptr;
+}
+
+CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type)
{
- for (CaptureBasePtr capture_ptr : getCaptureList())
+ for (auto capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
return capture_ptr;
return nullptr;
}
-CaptureBasePtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const
+CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const
{
if (_sensor_ptr)
- for (CaptureBasePtr capture_ptr : getCaptureList())
+ for (auto capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr)
return capture_ptr;
return nullptr;
}
-CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr) const
+CaptureBasePtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr)
+{
+ if (_sensor_ptr)
+ for (auto capture_ptr : getCaptureList())
+ if (capture_ptr->getSensor() == _sensor_ptr)
+ return capture_ptr;
+ return nullptr;
+}
+
+CaptureBaseConstPtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr) const
+{
+ CaptureBaseConstPtrList captures;
+ for (auto capture_ptr : getCaptureList())
+ if (capture_ptr->getSensor() == _sensor_ptr)
+ captures.push_back(capture_ptr);
+ return captures;
+}
+
+CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
{
CaptureBasePtrList captures;
- for (CaptureBasePtr capture_ptr : getCaptureList())
+ for (auto capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr)
captures.push_back(capture_ptr);
return captures;
}
-FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const
+FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const
{
- for (const FactorBasePtr& factor_ptr : getConstrainedByList())
+ for (auto factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
return factor_ptr;
- for (const FactorBasePtr& factor_ptr : getFactorList())
+ for (auto factor_ptr : getFactorList())
if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
return factor_ptr;
return nullptr;
}
-FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) const
+FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type)
{
- for (const FactorBasePtr& factor_ptr : getConstrainedByList())
+ for (auto factor_ptr : getConstrainedByList())
+ if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
+ return factor_ptr;
+
+ for (auto factor_ptr : getFactorList())
+ if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
+ return factor_ptr;
+
+ return nullptr;
+}
+
+FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) const
+{
+ for (auto factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr)
return factor_ptr;
- for (const FactorBasePtr& factor_ptr : getFactorList())
+ for (auto factor_ptr : getFactorList())
if (factor_ptr->getProcessor() == _processor_ptr)
return factor_ptr;
return nullptr;
}
-FactorBasePtrList FrameBase::getFactorList() const
+FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr)
+{
+ for (auto factor_ptr : getConstrainedByList())
+ if (factor_ptr->getProcessor() == _processor_ptr)
+ return factor_ptr;
+
+ for (auto factor_ptr : getFactorList())
+ if (factor_ptr->getProcessor() == _processor_ptr)
+ return factor_ptr;
+
+ return nullptr;
+}
+
+FactorBaseConstPtrList FrameBase::getFactorList() const
+{
+ FactorBaseConstPtrList fac_list;
+ getFactorList(fac_list);
+ return fac_list;
+}
+
+FactorBasePtrList FrameBase::getFactorList()
{
FactorBasePtrList fac_list;
getFactorList(fac_list);
return fac_list;
}
-void FrameBase::getFactorList(FactorBasePtrList& _fac_list) const
+void FrameBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
+{
+ for (auto c_ptr : getCaptureList())
+ c_ptr->getFactorList(_fac_list);
+}
+
+void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
{
for (auto c_ptr : getCaptureList())
c_ptr->getFactorList(_fac_list);
}
+bool FrameBase::hasCapture(const CaptureBaseConstPtr& _capture) const
+{
+ 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);
@@ -289,19 +378,11 @@ void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
-bool FrameBase::isConstrainedBy(const FactorBasePtr &_factor) const
+bool FrameBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
{
- FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
- constrained_by_list_.end(),
- [_factor](const FactorBasePtr & cby)
- {
- return cby == _factor;
- }
- );
- if (cby_it != constrained_by_list_.end())
- return true;
- else
- return false;
+ return std::find(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ _factor) != constrained_by_list_.end();
}
void FrameBase::link(TrajectoryBasePtr _trj_ptr)
@@ -360,7 +441,7 @@ void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blo
C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
-CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog FrameBase::localCheck(bool _verbose, FrameBaseConstPtr _frm_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -371,7 +452,8 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
_stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
_stream << _tabs << " " << "-> Trj @ " << getTrajectory().get() << std::endl;
}
- for (const auto &pair: getStateBlockMap()) {
+ for (const auto &pair: getStateBlockMap())
+ {
auto sb = pair.second;
@@ -405,15 +487,14 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
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;
// check constrained_by pointer to this frame
inconsistency_explanation << "constrained-by frame " << id() << " @ " << _frm_ptr
<< " not found among constrained-by factors\n";
- auto F = std::static_pointer_cast<FrameBase>(_frm_ptr);
- log.assertTrue((cby->hasFrameOther(F)), inconsistency_explanation);
+ log.assertTrue((cby->hasFrameOther(_frm_ptr)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector())
{
@@ -430,13 +511,14 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
}
}
+ // Trajectory
auto trj_ptr = getTrajectory();
inconsistency_explanation << "Frm" << id() << " @ " << _frm_ptr
<< " ---> Trj" << " @ " << trj_ptr
<< " -X-> Frm" << id();
- auto trj_has_frm = std::find_if(trj_ptr->begin(), trj_ptr->end(), [&_frm_ptr](FrameBasePtr frm){ return frm == _frm_ptr;});
- log.assertTrue(trj_has_frm != trj_ptr->end(), inconsistency_explanation);
+ log.assertTrue(trj_ptr->hasFrame(_frm_ptr), inconsistency_explanation);
+ // Captures
for(auto C : getCaptureList())
{
inconsistency_explanation << "Frm " << id() << " @ " << _frm_ptr
@@ -444,15 +526,17 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
<< " -X-> Frm" << id();
log.assertTrue((C->getFrame() == _frm_ptr), inconsistency_explanation);
}
+
return log;
}
-bool FrameBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool FrameBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto frm_ptr = std::static_pointer_cast<FrameBase>(_node_ptr);
+ 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 9db6b0c373e8f49838ea0d354a0694bffba0da04..470057fabef0c9f868d26cc28f88e12a20cc595b 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -41,6 +41,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_list_.begin(),
+ sensor_list_.end(),
+ [&](SensorBaseConstPtr sb)
+ {
+ return sb->getName() == _sensor_name;
+ }); // lambda function for the find_if
+
+ if (sen_it == sensor_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;
@@ -55,7 +85,7 @@ void HardwareBase::print(int _depth, bool _constr_by, bool _metric, bool _state_
S->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
-CheckLog HardwareBase::localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog HardwareBase::localCheck(bool _verbose, HardwareBaseConstPtr _hwd_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -68,11 +98,12 @@ CheckLog HardwareBase::localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::
inconsistency_explanation << "Hwd->getProblem() [" << getProblem().get()
<< "] -> " << " Prb->getHardware() [" << getProblem()->getHardware().get() << "] -> Hwd [" << _hwd_ptr.get() << "] Mismatch!\n";
log.assertTrue((_hwd_ptr->getProblem()->getHardware().get() == _hwd_ptr.get()), inconsistency_explanation);
+
return log;
}
-bool HardwareBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool HardwareBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto hwd_ptr = std::static_pointer_cast<HardwareBase>(_node_ptr);
+ auto hwd_ptr = std::static_pointer_cast<const HardwareBase>(_node_ptr);
auto local_log = localCheck(_verbose, hwd_ptr, _stream, _tabs);
_log.compose(local_log);
for (auto S : getSensorList())
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 09d317544ff4c0ae6ce7b6a24e2d3b2acf2cfe93..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,25 +84,45 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
}
}
-std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec() const
-{
- std::vector<StateBlockPtr> used_state_block_vec(0);
-
- // normal state blocks in {P,O,V,W}
- for (const 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<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
{
@@ -164,19 +184,11 @@ void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
-bool LandmarkBase::isConstrainedBy(const FactorBasePtr &_factor) const
+bool LandmarkBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
{
- FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
- constrained_by_list_.end(),
- [_factor](const FactorBasePtr & cby)
- {
- return cby == _factor;
- }
- );
- if (cby_it != constrained_by_list_.end())
- return true;
- else
- return false;
+ 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
@@ -224,7 +236,7 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
return lmk;
}
-CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -234,7 +246,7 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::
_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();
@@ -256,24 +268,24 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::
log.assertTrue((getProblem() == map_ptr->getProblem()), inconsistency_explanation);
+ // check constrained-by factors
for (auto cby : getConstrainedByList())
{
if (_verbose)
{
_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();
}
}
_stream << std::endl;
}
- // check constrained-by factors
inconsistency_explanation << "constrained-by landmark " << id() << " @ " << _lmk_ptr.get()
<< " not found among constrained-by factors\n";
log.assertTrue((cby->hasLandmarkOther(_lmk_ptr)), inconsistency_explanation);
@@ -291,18 +303,21 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::
}
}
+ // check map
inconsistency_explanation << "Lmk" << id() << " @ " << _lmk_ptr
<< " ---> Map" << map_ptr
<< " -X-> Lmk" << id();
auto map_lmk_list = map_ptr->getLandmarkList();
- auto map_has_lmk = std::find_if(map_lmk_list.begin(), map_lmk_list.end(), [&_lmk_ptr](LandmarkBasePtr lmk){ return lmk == _lmk_ptr;});
+ auto map_has_lmk = std::find(map_lmk_list.begin(),
+ map_lmk_list.end(),
+ _lmk_ptr);
log.assertTrue(map_has_lmk != map_lmk_list.end(), inconsistency_explanation);
return log;
}
-bool LandmarkBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool LandmarkBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto lmk_ptr = std::static_pointer_cast<LandmarkBase>(_node_ptr);
+ auto lmk_ptr = std::static_pointer_cast<const LandmarkBase>(_node_ptr);
auto local_log = localCheck(_verbose, lmk_ptr, _stream, _tabs);
_log.compose(local_log);
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 8194c87d95dc05bb20b5f736afdb704a11a75560..88b5f411dc6b718cab9f5e1a2a70071abffa4f8e 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -121,6 +121,7 @@ void MapBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state
{
_stream << _tabs << "Map" << ((_depth < 1) ? (" -- " + std::to_string(getLandmarkList().size()) + "L") : "") << std::endl;
}
+
void MapBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
@@ -130,7 +131,7 @@ void MapBase::print(int _depth, bool _constr_by, bool _metric, bool _state_block
L->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
-CheckLog MapBase::localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog MapBase::localCheck(bool _verbose, MapBaseConstPtr _map_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -144,15 +145,17 @@ CheckLog MapBase::localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _
log.assertTrue((_map_ptr->getProblem()->getMap().get() == _map_ptr.get()), inconsistency_explanation);
return log;
}
-bool MapBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+
+bool MapBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto map_ptr = std::static_pointer_cast<MapBase>(_node_ptr);
+ auto map_ptr = std::static_pointer_cast<const MapBase>(_node_ptr);
auto local_log = localCheck(_verbose, map_ptr, _stream, _tabs);
_log.compose(local_log);
for (auto L : getLandmarkList())
L->check(_log, L, _verbose, _stream, _tabs + " ");
return _log.is_consistent_;
}
+
} // namespace wolf
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index d80dafb51c2d4e2dfd772c19fa0786e4f5c173b7..aa07e14a3cf1e3e4c2e55f93d9f71cf88b038807 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -47,8 +47,6 @@
#include "core/math/covariance.h"
#include "core/state_block/factory_state_block.h"
-// IRI libs includes
-
// C++ includes
#include <algorithm>
#include <map>
@@ -58,7 +56,6 @@
#include <vector>
#include <unordered_set>
-
namespace wolf
{
@@ -335,22 +332,17 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
return prc_ptr;
}
-SensorBasePtr Problem::findSensor(const std::string& _sensor_name) const
+SensorBaseConstPtr Problem::findSensor(const std::string& _sensor_name) const
{
- 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 getHardware()->getSensor(_sensor_name);
+}
- return (*sen_it);
+SensorBasePtr Problem::findSensor(const std::string& _sensor_name)
+{
+ return getHardware()->getSensor(_sensor_name);
}
-ProcessorBasePtr Problem::findProcessor(const std::string& _processor_name) const
+ProcessorBaseConstPtr Problem::findProcessor(const std::string& _processor_name) const
{
for (const auto& sensor : getHardware()->getSensorList())
for (const auto& processor : sensor->getProcessorList())
@@ -359,16 +351,24 @@ ProcessorBasePtr Problem::findProcessor(const std::string& _processor_name) cons
return nullptr;
}
+ProcessorBasePtr Problem::findProcessor(const std::string& _processor_name)
+{
+ for (const auto& sensor : getHardware()->getSensorList())
+ for (const auto& processor : sensor->getProcessorList())
+ if (processor->getName() == _processor_name)
+ return processor;
+ return nullptr;
+}
FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const SizeEigen _dim, //
- const Eigen::VectorXd& _frame_state)
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim, //
+ const Eigen::VectorXd& _frame_state)
{
VectorComposite state;
SizeEigen index = 0;
SizeEigen size = 0;
- for (const auto& key : _frame_structure)
+ for (auto key : _frame_structure)
{
if (key == 'O')
{
@@ -422,9 +422,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);
}
@@ -455,7 +455,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();
@@ -463,7 +463,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
@@ -483,12 +483,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
{
@@ -507,7 +507,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);
@@ -515,7 +515,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)
{
@@ -539,12 +539,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);
@@ -561,7 +561,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);
@@ -569,7 +569,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)
{
@@ -593,12 +593,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);
@@ -612,7 +612,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)
{
@@ -641,7 +641,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;
}
@@ -652,7 +652,6 @@ void Problem::setTreeManager(TreeManagerBasePtr _gm)
tree_manager_->setProblem(nullptr);
tree_manager_ = _gm;
tree_manager_->setProblem(shared_from_this());
-
}
void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr)
@@ -687,7 +686,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());
}
@@ -696,7 +695,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')
@@ -862,7 +861,10 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _
covariances_[std::make_pair(_state1, _state1)] = _cov;
}
-bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXd& _cov, const int _row,
+bool Problem::getCovarianceBlock(StateBlockConstPtr _state1,
+ StateBlockConstPtr _state2,
+ Eigen::MatrixXd& _cov,
+ const int _row,
const int _col) const
{
//std::cout << "entire cov to be filled:" << std::endl << _cov << std::endl;
@@ -880,12 +882,12 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
std::lock_guard<std::mutex> lock(mut_covariances_);
- if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
+ if (covariances_.find(std::pair<StateBlockConstPtr, StateBlockConstPtr>(_state1, _state2)) != covariances_.end())
+ _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
+ covariances_.at(std::pair<StateBlockConstPtr, StateBlockConstPtr>(_state1, _state2));
+ else if (covariances_.find(std::pair<StateBlockConstPtr, StateBlockConstPtr>(_state2, _state1)) != covariances_.end())
_cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
- covariances_.at(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2));
- else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
- _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
- covariances_.at(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)).transpose();
+ covariances_.at(std::pair<StateBlockConstPtr, StateBlockConstPtr>(_state2, _state1)).transpose();
else
{
WOLF_DEBUG("Could not find the requested covariance block.");
@@ -895,18 +897,18 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
return true;
}
-bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const
+bool Problem::getCovarianceBlock(std::map<StateBlockConstPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const
{
std::lock_guard<std::mutex> lock(mut_covariances_);
- // fill covariance
+ // fill _cov
for (auto it1 = _sb_2_idx.begin(); it1 != _sb_2_idx.end(); it1++)
for (auto it2 = it1; it2 != _sb_2_idx.end(); it2++)
{
- StateBlockPtr sb1 = it1->first;
- StateBlockPtr sb2 = it2->first;
- std::pair<StateBlockPtr, StateBlockPtr> pair_12(sb1, sb2);
- std::pair<StateBlockPtr, StateBlockPtr> pair_21(sb2, sb1);
+ auto sb1 = it1->first;
+ auto sb2 = it2->first;
+ std::pair<StateBlockConstPtr, StateBlockConstPtr> pair_12(sb1, sb2);
+ std::pair<StateBlockConstPtr, StateBlockConstPtr> pair_21(sb2, sb1);
// search st1 & st2
if (covariances_.find(pair_12) != covariances_.end())
@@ -936,7 +938,7 @@ bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx
return true;
}
-bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col) const
+bool Problem::getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col) const
{
return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col);
}
@@ -946,15 +948,15 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd&
bool success(true);
// resizing
- SizeEigen sz = _frame_ptr->getLocalSize();
+ SizeEigen sz = _frame_ptr->getLocalSize();
_covariance.resize(sz, sz);
// 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();
@@ -967,7 +969,7 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd&
bool Problem::getLastFrameCovariance(Eigen::MatrixXd& cov) const
{
- FrameBasePtr frm = getLastFrame();
+ auto frm = getLastFrame();
return getFrameCovariance(frm, cov);
}
@@ -976,15 +978,15 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
bool success(true);
// resizing
- SizeEigen sz = _landmark_ptr->getLocalSize();
+ SizeEigen sz = _landmark_ptr->getLocalSize();
_covariance.resize(sz, sz);
// 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();
@@ -995,7 +997,12 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
return success;
}
-MapBasePtr Problem::getMap() const
+MapBaseConstPtr Problem::getMap() const
+{
+ return map_ptr_;
+}
+
+MapBasePtr Problem::getMap()
{
return map_ptr_;
}
@@ -1007,22 +1014,42 @@ void Problem::setMap(MapBasePtr _map_ptr)
map_ptr_ = _map_ptr;
}
-TrajectoryBasePtr Problem::getTrajectory() const
+TrajectoryBaseConstPtr Problem::getTrajectory() const
{
return trajectory_ptr_;
}
-HardwareBasePtr Problem::getHardware() const
+TrajectoryBasePtr Problem::getTrajectory()
+{
+ return trajectory_ptr_;
+}
+
+HardwareBaseConstPtr Problem::getHardware() const
+{
+ return hardware_ptr_;
+}
+
+HardwareBasePtr Problem::getHardware()
{
return hardware_ptr_;
}
-FrameBasePtr Problem::getLastFrame() const
+FrameBaseConstPtr Problem::getLastFrame() const
+{
+ return trajectory_ptr_->getLastFrame();
+}
+
+FrameBasePtr Problem::getLastFrame()
{
return trajectory_ptr_->getLastFrame();
}
-FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts) const
+FrameBaseConstPtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts) const
+{
+ return trajectory_ptr_->closestFrameToTimeStamp(_ts);
+}
+
+FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts)
{
return trajectory_ptr_->closestFrameToTimeStamp(_ts);
}
@@ -1095,10 +1122,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);
@@ -1236,19 +1263,19 @@ 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()))
+ for (auto F : getTrajectory()->getFrameMap())
{
- F->perturb(amplitude);
- for (auto& C : F->getCaptureList())
+ F.second->perturb(amplitude);
+ for (auto& C : F.second->getCaptureList())
C->perturb(amplitude);
}
// Landmarks
- for (auto& L : getMap()->getLandmarkList())
+ for (auto L : getMap()->getLandmarkList())
L->perturb(amplitude);
}
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index adeac3db58b63afc2907d12892325fcd8feefc8d..90e40f241c6d15a131b469f31db5c5b9b2938ee2 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -165,7 +165,7 @@ void ProcessorBase::print(int _depth, bool _metric, bool _state_blocks, bool _co
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
}
-CheckLog ProcessorBase::localCheck(bool _verbose, ProcessorBasePtr _prc_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog ProcessorBase::localCheck(bool _verbose, ProcessorBaseConstPtr _prc_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -187,14 +187,14 @@ CheckLog ProcessorBase::localCheck(bool _verbose, ProcessorBasePtr _prc_ptr, std
<< " ---> Sen" << sen_ptr->id() << " @ " << sen_ptr
<< " -X-> Prc" << id();
auto sen_prc_list = sen_ptr->getProcessorList();
- auto sen_has_prc = std::find_if(sen_prc_list.begin(), sen_prc_list.end(), [&_prc_ptr](ProcessorBasePtr prc){ return prc == _prc_ptr;});
+ auto sen_has_prc = std::find(sen_prc_list.begin(), sen_prc_list.end(), _prc_ptr);
log.assertTrue(sen_has_prc != sen_prc_list.end(), inconsistency_explanation);
return log;
}
-bool ProcessorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool ProcessorBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto prc_ptr = std::static_pointer_cast<ProcessorBase>(_node_ptr);
+ auto prc_ptr = std::static_pointer_cast<const ProcessorBase>(_node_ptr);
auto local_log = localCheck(_verbose, prc_ptr, _stream, _tabs);
_log.compose(local_log);
return _log.is_consistent_;
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 805f9bb4cac6847906a3aa9ed2e5f577dbdc63c4..0ad4a754dd665d4533303f2e27569b0e7c151048 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -82,7 +82,7 @@ ProcessorMotion::~ProcessorMotion()
}
-void ProcessorMotion::mergeCaptures(CaptureMotionConstPtr cap_prev,
+void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev,
CaptureMotionPtr cap_target)
{
assert(cap_prev != nullptr);
@@ -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);
@@ -618,7 +618,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
const StateStructure& structure = (_structure == "" ? state_structure_ : _structure);
// We need to search for the capture containing a motion buffer with the queried time stamp
- CaptureMotionPtr capture_motion = findCaptureContainingTimeStamp(_ts);
+ auto capture_motion = findCaptureContainingTimeStamp(_ts);
if (capture_motion == nullptr) // we do not have any info of where to find a valid state
{
@@ -650,8 +650,8 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
*/
// Get state of origin
- CaptureBasePtr cap_orig = capture_motion->getOriginCapture();
- const auto& x_origin = cap_orig->getFrame()->getState(state_structure_);
+ auto cap_orig = capture_motion->getOriginCapture();
+ 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);
@@ -876,7 +876,7 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
}
}
-CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
+CaptureMotionConstPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
{
assert(_ts.ok());
// assert(last_ptr_ != nullptr);
@@ -898,14 +898,66 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
// 1. See that the KF contains a CaptureMotion
// 2. See that the TS is smaller than the KF's TS
// 3. See that the TS is bigger than the first Motion in the CaptureMotion's buffer (if any)
- FrameBasePtr frame = nullptr;
- CaptureBasePtr capture = nullptr;
- CaptureMotionPtr capture_motion = nullptr;
- for (auto frame_rev_iter = getProblem()->getTrajectory()->rbegin();
- frame_rev_iter != getProblem()->getTrajectory()->rend();
- ++frame_rev_iter)
+ FrameBaseConstPtr frame = nullptr;
+ CaptureBaseConstPtr capture = nullptr;
+ CaptureMotionConstPtr capture_motion = nullptr;
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
{
- frame = *frame_rev_iter;
+ frame = frame_rev_iter->second;
+ auto sensor = getSensor();
+ capture = frame->getCaptureOf(sensor);
+ if (capture != nullptr)
+ {
+ assert(std::dynamic_pointer_cast<const CaptureMotion>(capture) != nullptr);
+ // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
+ capture_motion = std::static_pointer_cast<const CaptureMotion>(capture);
+
+ if (capture_motion->containsTimeStamp(_ts, params_->time_tolerance))
+ {
+ // Found time stamp satisfying rule 3 above !! ==> break for loop
+ break;
+ }
+ else
+ capture_motion = nullptr;
+ }
+ }
+ return capture_motion;
+}
+
+CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts)
+{
+ assert(_ts.ok());
+ // assert(last_ptr_ != nullptr);
+
+ //Need to uncomment this line so that wolf_ros_apriltag doesn't crash
+ if(last_ptr_ == nullptr) return nullptr;
+
+ // First check if last_ptr is the one we are looking for
+ if (last_ptr_->containsTimeStamp(_ts, this->params_->time_tolerance))
+ return last_ptr_;
+
+
+ // Then look in the Wolf tree...
+ // -----------------------------
+ //
+
+ // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
+ // Note: since the buffer goes from a KF in the past until the next KF, we need to:
+ // 1. See that the KF contains a CaptureMotion
+ // 2. See that the TS is smaller than the KF's TS
+ // 3. See that the TS is bigger than the first Motion in the CaptureMotion's buffer (if any)
+ FrameBasePtr frame = nullptr;
+ CaptureBasePtr capture = nullptr;
+ CaptureMotionPtr capture_motion = nullptr;
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
+ {
+ frame = frame_rev_iter->second;
auto sensor = getSensor();
capture = frame->getCaptureOf(sensor);
if (capture != nullptr)
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index bf24a1d4e2d04c9111605792120df484e369f888..471dd627361266aad20356543345355a5d18bdbf 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -242,7 +242,7 @@ FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, Cap
return fac_odom;
}
-VectorXd ProcessorOdom3d::getCalibration (const CaptureBasePtr _capture) const
+VectorXd ProcessorOdom3d::getCalibration (const CaptureBaseConstPtr _capture) const
{
return VectorXd::Zero(0);
}
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index d7154a788b5727b6e2f8cd201b382643110d5bf1..de101750edfe66eec72fae0c3be295164e8b7d45 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -31,7 +31,7 @@
namespace wolf
{
-size_t TrackMatrix::track_id_count_ = 0;
+SizeStd TrackMatrix::track_id_count_ = 0;
TrackMatrix::TrackMatrix()
{
@@ -43,7 +43,17 @@ TrackMatrix::~TrackMatrix()
//
}
-Track TrackMatrix::track(const SizeStd& _track_id) const
+TrackConst TrackMatrix::track(const SizeStd& _track_id) const
+{
+ TrackConst track_const;
+ if (tracks_.count(_track_id) > 0)
+ for (auto&& pair : tracks_.at(_track_id))
+ track_const[pair.first]=pair.second;
+
+ return track_const;
+}
+
+Track TrackMatrix::track(const SizeStd& _track_id)
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id);
@@ -51,7 +61,27 @@ Track TrackMatrix::track(const SizeStd& _track_id) const
return Track();
}
-Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture) const
+SnapshotConst TrackMatrix::snapshot(CaptureBaseConstPtr _capture) const
+{
+ const auto& it = std::find_if(snapshots_.begin(),
+ snapshots_.end(),
+ [_capture](const std::pair<CaptureBasePtr, Snapshot>& pair)
+ {
+ return pair.first == _capture;
+ }
+ );
+
+ if (it == snapshots_.end())
+ return SnapshotConst();
+
+ SnapshotConst snapshot_const;
+ for (auto&& pair : it->second)
+ snapshot_const[pair.first]=pair.second;
+
+ return snapshot_const;
+}
+
+Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture)
{
if (_capture && snapshots_.count(_capture) > 0)
return snapshots_.at(_capture);
@@ -91,7 +121,6 @@ void TrackMatrix::remove(const SizeStd& _track_id)
snapshots_.at(cap).erase(_track_id);
if (snapshots_.at(cap).empty())
snapshots_.erase(cap);
-
}
// Remove track
@@ -127,13 +156,13 @@ void TrackMatrix::remove(FeatureBasePtr _ftr)
{
if(tracks_.count(_ftr->trackId()) && tracks_.at(_ftr->trackId()).count(cap->getTimeStamp()))
{
- tracks_ .at(_ftr->trackId()).erase(cap->getTimeStamp());
+ tracks_ .at(_ftr->trackId()).erase(cap->getTimeStamp());
if (tracks_.at(_ftr->trackId()).empty())
tracks_.erase(_ftr->trackId());
}
if(snapshots_.count(cap) && snapshots_.at(cap).count(_ftr->trackId()))
{
- snapshots_. at(cap).erase(_ftr->trackId());
+ snapshots_ .at(cap).erase(_ftr->trackId());
if (snapshots_.at(cap).empty())
snapshots_.erase(cap);
}
@@ -141,17 +170,17 @@ void TrackMatrix::remove(FeatureBasePtr _ftr)
}
}
-size_t TrackMatrix::numTracks() const
+SizeStd TrackMatrix::numTracks() const
{
return tracks_.size();
}
-size_t TrackMatrix::trackSize(const SizeStd& _track_id) const
+SizeStd TrackMatrix::trackSize(const SizeStd& _track_id) const
{
return track(_track_id).size();
}
-FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id) const
+FeatureBaseConstPtr TrackMatrix::firstFeature(const SizeStd& _track_id) const
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id).begin()->second;
@@ -159,7 +188,23 @@ FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id) const
return nullptr;
}
-FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id) const
+FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id)
+{
+ if (tracks_.count(_track_id) > 0)
+ return tracks_.at(_track_id).begin()->second;
+ else
+ return nullptr;
+}
+
+FeatureBaseConstPtr TrackMatrix::lastFeature(const SizeStd& _track_id) const
+{
+ if (tracks_.count(_track_id) > 0)
+ return tracks_.at(_track_id).rbegin()->second;
+ else
+ return nullptr;
+}
+
+FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id)
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id).rbegin()->second;
@@ -167,7 +212,19 @@ FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id) const
return nullptr;
}
-vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) const
+vector<FeatureBaseConstPtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) const
+{
+ vector<FeatureBaseConstPtr> vec;
+ if (tracks_.count(_track_id))
+ {
+ vec.reserve(trackSize(_track_id));
+ for (auto const& pair_time_ftr : tracks_.at(_track_id))
+ vec.push_back(pair_time_ftr.second);
+ }
+ return vec;
+}
+
+vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id)
{
vector<FeatureBasePtr> vec;
if (tracks_.count(_track_id))
@@ -179,23 +236,59 @@ vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) cons
return vec;
}
-std::list<FeatureBasePtr> TrackMatrix::snapshotAsList(CaptureBasePtr _cap) const
+FeatureBaseConstPtrList TrackMatrix::snapshotAsList(CaptureBaseConstPtr _cap) const
{
- std::list<FeatureBasePtr> lst;
+ SnapshotConst snapshot_const = snapshot(_cap);
+
+ FeatureBaseConstPtrList lst;
+
+ for (auto const& pair_trkid_ftr : snapshot_const)
+ lst.push_back(pair_trkid_ftr.second);
+
+ return lst;
+}
+
+FeatureBasePtrList TrackMatrix::snapshotAsList(CaptureBasePtr _cap)
+{
+ FeatureBasePtrList lst;
if (snapshots_.count(_cap))
for (auto const& pair_trkid_ftr : snapshots_.at(_cap))
lst.push_back(pair_trkid_ftr.second);
return lst;
}
-TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2) const
+TrackMatchesConst TrackMatrix::matches(CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const
+{
+ TrackMatchesConst pairs;
+
+ auto s_1 = snapshot(_cap_1);
+ auto s_2 = snapshot(_cap_2);
+ auto s_short = s_1;
+ auto s_long = s_2;
+ if (s_1.size() > s_2.size())
+ {
+ s_long = s_1;
+ s_short = s_2;
+ }
+
+ for (auto const & pair_trkid_ftr : s_short)
+ {
+ SizeStd trk_id = pair_trkid_ftr.first;
+ if (s_long.count(trk_id))
+ pairs[trk_id] = pair<FeatureBaseConstPtr, FeatureBaseConstPtr>(s_1.at(trk_id), s_2.at(trk_id));
+ }
+
+ return pairs;
+}
+
+TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
{
TrackMatches pairs;
- Snapshot s_1 = snapshot(_cap_1);
- Snapshot s_2 = snapshot(_cap_2);
- Snapshot s_short = s_1;
- Snapshot s_long = s_2;
+ auto s_1 = snapshot(_cap_1);
+ auto s_2 = snapshot(_cap_2);
+ auto s_short = s_1;
+ auto s_long = s_2;
if (s_1.size() > s_2.size())
{
s_long = s_1;
@@ -212,7 +305,15 @@ TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
return pairs;
}
-FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _cap) const
+FeatureBaseConstPtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBaseConstPtr _cap) const
+{
+ if (snapshot(_cap).count(_track_id))
+ return snapshot(_cap).at(_track_id);
+ else
+ return nullptr;
+}
+
+FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _cap)
{
if (snapshot(_cap).count(_track_id))
return snapshot(_cap).at(_track_id);
@@ -220,12 +321,36 @@ FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _ca
return nullptr;
}
-CaptureBasePtr TrackMatrix::firstCapture(const SizeStd& _track_id) const
+CaptureBaseConstPtr TrackMatrix::firstCapture(const SizeStd& _track_id) const
{
return firstFeature(_track_id)->getCapture();
}
-Track TrackMatrix::trackAtKeyframes(size_t _track_id) const
+CaptureBasePtr TrackMatrix::firstCapture(const SizeStd& _track_id)
+{
+ return firstFeature(_track_id)->getCapture();
+}
+
+TrackConst TrackMatrix::trackAtKeyframes(const SizeStd& _track_id) const
+{
+ // We assemble a track_kf on the fly by checking each capture's frame.
+ if (tracks_.count(_track_id))
+ {
+ TrackConst track_kf;
+ for (auto& pair_ts_ftr : tracks_.at(_track_id))
+ {
+ auto& ts = pair_ts_ftr.first;
+ auto& ftr = pair_ts_ftr.second;
+ if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->getProblem())
+ track_kf[ts] = ftr;
+ }
+ return track_kf;
+ }
+ else
+ return TrackConst();
+}
+
+Track TrackMatrix::trackAtKeyframes(const SizeStd& _track_id)
{
// We assemble a track_kf on the fly by checking each capture's frame.
if (tracks_.count(_track_id))
@@ -244,9 +369,9 @@ Track TrackMatrix::trackAtKeyframes(size_t _track_id) const
return Track();
}
-list<size_t> TrackMatrix::trackIds() const
+list<SizeStd> TrackMatrix::trackIds() const
{
- list<size_t> track_ids;
+ list<SizeStd> track_ids;
for (auto track : tracks_)
{
track_ids.push_back(track.first);
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 0e649ec5cce5c11ca522c21bab70c67855922839..996d48263c113555ba055422153bf10459bfb564 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -42,9 +42,11 @@ SensorBase::SensorBase(const std::string& _type,
HasStateBlocks(""),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
+ state_block_dynamic_(),
+ params_prior_map_(),
+ last_capture_(nullptr),
noise_std_(_noise_size),
- noise_cov_(_noise_size, _noise_size),
- last_capture_(nullptr)
+ noise_cov_(_noise_size, _noise_size)
{
assert((_p_ptr or not _p_dyn) and "Trying to set dynamic position state block without providing a position state block. It is required anyway.");
assert((_o_ptr or not _o_dyn) and "Trying to set dynamic orientation state block without providing an orientation state block. It is required anyway.");
@@ -76,9 +78,11 @@ SensorBase::SensorBase(const std::string& _type,
HasStateBlocks(""),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
+ state_block_dynamic_(),
+ params_prior_map_(),
+ last_capture_(nullptr),
noise_std_(_noise_std),
- noise_cov_(_noise_std.size(), _noise_std.size()),
- last_capture_(nullptr)
+ noise_cov_(_noise_std.size(), _noise_std.size())
{
setNoiseStd(_noise_std);
@@ -95,28 +99,12 @@ SensorBase::SensorBase(const std::string& _type,
SensorBase::~SensorBase()
{
// Remove State Blocks
- removeStateBlocks();
-}
-
-void SensorBase::removeStateBlocks()
-{
- for (const auto& key : getStructure())
- {
- auto sbp = getStateBlock(key);
- if (sbp != nullptr)
- {
- if (getProblem() != nullptr && !isStateBlockInCapture(key))
- {
- getProblem()->notifyStateBlock(sbp,REMOVE);
- }
- removeStateBlock(key);
- }
- }
+ removeStateBlocks(getProblem());
}
void SensorBase::fixExtrinsics()
{
- for (const auto& key : std::string("PO"))
+ for (auto key : std::string("PO"))
{
auto sbp = getStateBlockDynamic(key);
if (sbp != nullptr)
@@ -126,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)
@@ -136,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
{
@@ -149,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
{
@@ -203,17 +191,17 @@ void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x,
params_prior_map_[_key] = ftr_prior;
}
-void SensorBase::registerNewStateBlocks() const
+void SensorBase::registerNewStateBlocks(ProblemPtr _problem)
{
- if (getProblem() != nullptr)
+ if (_problem != nullptr)
{
for (auto & pair_key_sbp : getStateBlockMap())
{
auto key = pair_key_sbp.first;
auto sbp = pair_key_sbp.second;
- if (sbp and not isStateBlockDynamic(key))//!isStateBlockInCapture(key))
- getProblem()->notifyStateBlock(sbp, ADD);
+ if (sbp and not isStateBlockDynamic(key))
+ _problem->notifyStateBlock(sbp, ADD);
}
}
}
@@ -240,21 +228,20 @@ void SensorBase::setLastCapture(CaptureBasePtr cap)
void SensorBase::updateLastCapture()
{
// we search for the most recent Capture of this sensor which belongs to a KeyFrame
- if (getProblem())
+ if (not getProblem())
+ return;
+
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
{
- // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
- auto trajectory = getProblem()->getTrajectory();
- TrajectoryRevIter frame_rev_it = trajectory->rbegin();
- while (frame_rev_it != trajectory->rend())
+ auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this());
+ if (capture and not capture->isRemoving())
{
- auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
- if (capture and not capture->isRemoving())
- {
- // found the most recent Capture made by this sensor !
- last_capture_ = capture;
- return;
- }
- frame_rev_it++;
+ // found the most recent Capture made by this sensor !
+ last_capture_ = capture;
+ return;
}
}
@@ -262,58 +249,110 @@ void SensorBase::updateLastCapture()
last_capture_ = nullptr;
}
-CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts) const
+CaptureBaseConstPtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts) const
{
// we search for the most recent Capture of this sensor before _ts
if (not getProblem())
return nullptr;
- // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
- auto trajectory = getProblem()->getTrajectory();
+ // We iterate in reverse since we're likely to find it close to the rbegin() place.
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
+ {
+ if (frame_rev_iter->second->getTimeStamp() <= _ts)
+ {
+ auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this());
+ if (capture)
+ // found the most recent Capture made by this sensor !
+ return capture;
+ }
+ }
+
+ return nullptr;
+}
+
+CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts)
+{
+ // we search for the most recent Capture of this sensor before _ts
+ if (not getProblem())
+ return nullptr;
// We iterate in reverse since we're likely to find it close to the rbegin() place.
- TrajectoryRevIter frame_rev_it = trajectory->rbegin();
- while (frame_rev_it != trajectory->rend())
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
{
- if ((*frame_rev_it)->getTimeStamp() <= _ts)
+ if (frame_rev_iter->second->getTimeStamp() <= _ts)
{
- auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this());
if (capture)
// found the most recent Capture made by this sensor !
return capture;
}
- frame_rev_it++;
}
return nullptr;
}
-StateBlockPtr SensorBase::getP(const TimeStamp _ts) const
+StateBlockConstPtr SensorBase::getP(const TimeStamp _ts) const
+{
+ return getStateBlockDynamic('P', _ts);
+}
+
+StateBlockConstPtr SensorBase::getO(const TimeStamp _ts) const
+{
+ return getStateBlockDynamic('O', _ts);
+}
+
+StateBlockConstPtr SensorBase::getIntrinsic(const TimeStamp _ts) const
+{
+ return getStateBlockDynamic('I', _ts);
+}
+
+StateBlockConstPtr SensorBase::getP() const
+{
+ return getStateBlockDynamic('P');
+}
+
+StateBlockConstPtr SensorBase::getO() const
+{
+ return getStateBlockDynamic('O');
+}
+
+StateBlockConstPtr SensorBase::getIntrinsic() const
+{
+ return getStateBlockDynamic('I');
+}
+
+StateBlockPtr SensorBase::getP(const TimeStamp _ts)
{
return getStateBlockDynamic('P', _ts);
}
-StateBlockPtr SensorBase::getO(const TimeStamp _ts) const
+StateBlockPtr SensorBase::getO(const TimeStamp _ts)
{
return getStateBlockDynamic('O', _ts);
}
-StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts) const
+StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts)
{
return getStateBlockDynamic('I', _ts);
}
-StateBlockPtr SensorBase::getP() const
+StateBlockPtr SensorBase::getP()
{
return getStateBlockDynamic('P');
}
-StateBlockPtr SensorBase::getO() const
+StateBlockPtr SensorBase::getO()
{
return getStateBlockDynamic('O');
}
-StateBlockPtr SensorBase::getIntrinsic() const
+StateBlockPtr SensorBase::getIntrinsic()
{
return getStateBlockDynamic('I');
}
@@ -356,7 +395,17 @@ void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr)
processor_list_.remove(_proc_ptr);
}
-StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key) const
+StateBlockConstPtr SensorBase::getStateBlockDynamic(const char& _key) const
+{
+ CaptureBaseConstPtr cap;
+
+ if (isStateBlockInCapture(_key, cap))
+ return cap->getStateBlock(_key);
+
+ return getStateBlock(_key);
+}
+
+StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key)
{
CaptureBasePtr cap;
@@ -366,7 +415,17 @@ StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key) const
return getStateBlock(_key);
}
-StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const
+StateBlockConstPtr SensorBase::getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const
+{
+ CaptureBaseConstPtr cap;
+
+ if (isStateBlockInCapture(_key, _ts, cap))
+ return cap->getStateBlock(_key);
+
+ return getStateBlock(_key);
+}
+
+StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key, const TimeStamp& _ts)
{
CaptureBasePtr cap;
@@ -376,7 +435,7 @@ StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key, const TimeStamp
return getStateBlock(_key);
}
-bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) const
+bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBaseConstPtr& _cap) const
{
if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
{
@@ -388,7 +447,31 @@ bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) c
return false;
}
-bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const
+bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap)
+{
+ if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
+ {
+ _cap = last_capture_;
+
+ return _cap != nullptr;
+ }
+ else
+ return false;
+}
+
+bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBaseConstPtr& _cap) const
+{
+ if (isStateBlockDynamic(_key))
+ {
+ _cap = findLastCaptureBefore(_ts);
+
+ return _cap != nullptr;
+ }
+ else
+ return false;
+}
+
+bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap)
{
if (isStateBlockDynamic(_key))
{
@@ -402,14 +485,14 @@ bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, C
bool SensorBase::isStateBlockInCapture(const char& _key) const
{
- CaptureBasePtr cap;
+ CaptureBaseConstPtr cap;
return isStateBlockInCapture(_key, cap);
}
bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts) const
{
- CaptureBasePtr cap;
+ CaptureBaseConstPtr cap;
return isStateBlockInCapture(_key, _ts, cap);
}
@@ -432,7 +515,7 @@ void SensorBase::link(HardwareBasePtr _hw_ptr)
this->setHardware(_hw_ptr);
this->getHardware()->addSensor(shared_from_this());
this->setProblem(_hw_ptr->getProblem());
- this->registerNewStateBlocks();
+ this->registerNewStateBlocks(getProblem());
}
else
{
@@ -454,7 +537,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)
@@ -470,9 +553,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;
}
@@ -490,7 +573,7 @@ void SensorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_bl
p->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
-CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog SensorBase::localCheck(bool _verbose, SensorBaseConstPtr _sen_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
if (_verbose)
@@ -514,6 +597,7 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr
std::stringstream inconsistency_explanation;
auto hwd_ptr = getHardware();
+
// check problem and hardware pointers
inconsistency_explanation << "Sensor problem pointer " << getProblem().get()
<< " different from Hardware problem pointer " << hwd_ptr->getProblem().get() << "\n";
@@ -523,9 +607,10 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr
<< " ---> Hwd" << " @ " << hwd_ptr
<< " -X-> Sen" << id();
auto hwd_sen_list = hwd_ptr->getSensorList();
- auto hwd_has_sen = std::find_if(hwd_sen_list.begin(), hwd_sen_list.end(), [&_sen_ptr](SensorBasePtr sen){ return sen == _sen_ptr;});
+ auto hwd_has_sen = std::find(hwd_sen_list.begin(), hwd_sen_list.end(), _sen_ptr);
log.assertTrue(hwd_has_sen != hwd_sen_list.end(), inconsistency_explanation);
+ // Check processors
for(auto prc : getProcessorList())
{
inconsistency_explanation << "Sen" << id() << " @ " << _sen_ptr
@@ -533,6 +618,7 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr
<< " -X-> Sen" << id();
log.assertTrue((prc->getSensor() == _sen_ptr), inconsistency_explanation);
}
+
// check last_capture_
if (getProblem()->getTimeStamp().ok())
{
@@ -550,9 +636,9 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr
return log;
}
-bool SensorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool SensorBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto sen_ptr = std::static_pointer_cast<SensorBase>(_node_ptr);
+ auto sen_ptr = std::static_pointer_cast<const SensorBase>(_node_ptr);
auto local_log = localCheck(_verbose, sen_ptr, _stream, _tabs);
_log.compose(local_log);
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 36cada937e0412330e7242d2092948807549fea9..047469dc899a8973b7bdf07df318b2079c155ef9 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -27,8 +27,9 @@ namespace wolf
StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
{
- assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
state_block_map_.emplace(_sb_type, _sb);
+ state_block_const_map_.emplace(_sb_type, _sb);
if (!isInStructure(_sb_type))
appendToStructure(_sb_type);
@@ -39,7 +40,7 @@ StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_type, const StateBlo
return _sb;
}
-void HasStateBlocks::registerNewStateBlocks(ProblemPtr _problem) const
+void HasStateBlocks::registerNewStateBlocks(ProblemPtr _problem)
{
if (_problem != nullptr)
{
@@ -79,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)
@@ -98,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/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 4bf0282fd90354856dc43b896a12a97ad88b1d7c..24de3de294a6e65ea409d345a31a23393904ae50 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -21,13 +21,14 @@
//--------LICENSE_END--------
#include "core/trajectory/trajectory_base.h"
#include "core/frame/frame_base.h"
+#include <iterator>
namespace wolf {
TrajectoryBase::TrajectoryBase() :
- NodeBase("TRAJECTORY", "TrajectoryBase")
+ NodeBase("TRAJECTORY", "TrajectoryBase"),
+ frame_map_()
{
- frame_map_ = FrameMap();
}
TrajectoryBase::~TrajectoryBase()
@@ -47,75 +48,141 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
{
- // add to list
- // frame_map_.erase(_frame_ptr);
frame_map_.erase(_frame_ptr->getTimeStamp());
}
-void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const
+void TrajectoryBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
{
- for(auto fr_ptr : *this)
- fr_ptr->getFactorList(_fac_list);
+ for(auto fr_pair: frame_map_)
+ fr_pair.second->getFactorList(_fac_list);
}
+void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
+{
+ for(auto fr_pair: frame_map_)
+ fr_pair.second->getFactorList(_fac_list);
+}
-FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) const
+TimeStamp TrajectoryBase::closestTimeStampToTimeStamp(const TimeStamp& _ts) const
{
- FrameBasePtr closest_kf = nullptr;
//If frame_map_ is empty then closestFrameToTimeStamp is meaningless
- if(not frame_map_.empty())
- {
- //Let me use shorter names for this explanation: lower_bound -> lb & upper_bound -> ub
- //In the std they fulfill the following property:
- // lb is the first element such that ts <= lb, alternatively the smallest element that is NOT less than ts.
- // lb definition is NOT the ACTUAL lower bound but the following position so, lb = lb_true + 1.
-
- auto lower_bound = frame_map_.lower_bound(_ts);
- if((lower_bound != this->end() and lower_bound->first == _ts) or lower_bound == this->begin() ) closest_kf = lower_bound->second;
- else
- {
- auto upper_bound = lower_bound;
- //I find it easier to reason if lb < ts < ub. Remember that we have got rid of the
- //equality case and the out of bounds cases so this inequality is complete (it is not misssing cases).
- //Therefore, we need to decrease the lower_bound to the previous element
- lower_bound = std::prev(lower_bound);
-
- //If ub points to end() it means that the last frame is still less than _ts, therefore certainly
- //it will be the closest one
- if(upper_bound == this->end()) closest_kf = lower_bound->second;
- else
- {
- //Easy stuff just calculate the distance return minimum
- auto lb_diff = fabs(lower_bound->first - _ts);
- auto ub_diff = fabs(upper_bound->first - _ts);
- if(lb_diff < ub_diff)
- {
- closest_kf = lower_bound->second;
- }
- else
- {
- closest_kf = upper_bound->second;
- }
- }
- }
- }
- return closest_kf;
+ if(frame_map_.empty())
+ return TimeStamp::Invalid();
+
+ // Lower bound provides the first iterator that does not go before ts (maybe equal or greater)
+ auto lower_bound = frame_map_.lower_bound(_ts);
+
+ // If first frame does not go before ts, it is the closest one
+ if ( lower_bound == frame_map_.begin())
+ return frame_map_.begin()->first;
+
+ // If last frame goes before ts, it is the closest one as well
+ if ( lower_bound == frame_map_.end())
+ return frame_map_.rbegin()->first;
+
+ // Otherwise we have to compare lb and its previous
+ auto upper_bound = lower_bound;
+ lower_bound = std::prev(lower_bound);
+
+ auto lb_dist = fabs(lower_bound->first - _ts);
+ auto ub_dist = fabs(upper_bound->first - _ts);
+ if(lb_dist < ub_dist)
+ return lower_bound->first;
+ else
+ return upper_bound->first;
+
+ // unreachable
+ return TimeStamp::Invalid();
+}
+
+FrameBaseConstPtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) const
+{
+ auto closest_ts = closestTimeStampToTimeStamp(_ts);
+
+ assert(not closest_ts.ok() or frame_map_.count(closest_ts) != 0);
+
+ if (closest_ts.ok())
+ return frame_map_.at(closest_ts);
+
+ return nullptr;
+}
+
+FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts)
+{
+ auto closest_ts = closestTimeStampToTimeStamp(_ts);
+
+ assert(not closest_ts.ok() or frame_map_.count(closest_ts) != 0);
+
+ if (closest_ts.ok())
+ return frame_map_.at(closest_ts);
+
+ return nullptr;
+}
+
+FrameBaseConstPtr TrajectoryBase::getNextFrame(FrameBaseConstPtr _frame, const unsigned int& i) const
+{
+ 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_map_.end() or
+ std::distance(frame_it, frame_map_.end()) <= i)
+ return nullptr;
+
+ return std::next(frame_it,i)->second;
+}
+
+FrameBasePtr TrajectoryBase::getNextFrame(FrameBasePtr _frame, const unsigned int& i)
+{
+ 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_map_.end() or
+ std::distance(frame_it, frame_map_.end()) <= i)
+ return nullptr;
+
+ return std::next(frame_it,i)->second;
+}
+
+FrameBaseConstPtr TrajectoryBase::getPreviousFrame(FrameBaseConstPtr _frame, const unsigned int& i) const
+{
+ 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_map_.end() or
+ std::distance(frame_map_.begin(), frame_it) < i)
+ return nullptr;
+
+ return std::prev(frame_it,i)->second;
+}
+
+FrameBasePtr TrajectoryBase::getPreviousFrame(FrameBasePtr _frame, const unsigned int& i)
+{
+ 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_map_.end() or
+ std::distance(frame_map_.begin(), frame_it) < i)
+ return nullptr;
+
+ return std::prev(frame_it,i)->second;
}
+
void TrajectoryBase::printHeader(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
{
_stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameMap().size()) + "F") : "") << std::endl;
}
+
void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 1)
- for (auto F : *this)
- if (F)
- F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ for (auto F_pair : frame_map_)
+ if (F_pair.second)
+ F_pair.second->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
-CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBaseConstPtr _trj_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -131,13 +198,15 @@ CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, s
log.assertTrue((_trj_ptr->getProblem()->getTrajectory().get() == _trj_ptr.get()), inconsistency_explanation);
return log;
}
-bool TrajectoryBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+
+bool TrajectoryBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- auto trj_ptr = std::static_pointer_cast<TrajectoryBase>(_node_ptr);
+ 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 : *this)
- F->check(_log, F, _verbose, _stream, _tabs + " ");
+ for (auto F_pair : getFrameMap())
+ if (F_pair.second)
+ F_pair.second->check(_log, F_pair.second, _verbose, _stream, _tabs + " ");
return _log.is_consistent_;
}
} // namespace wolf
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index 5d6f1345ee09674750d129e64a89a87ca081902d..1a0f44507a010891a792ce2f01af16d2233ce6cb 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -26,7 +26,7 @@ namespace wolf
void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
{
- int n_f = getProblem()->getTrajectory()->getFrameMap().size();
+ int n_f = getProblem()->getTrajectory()->size();
bool remove_first = (n_f > params_sw_->n_frames);
int n_fix = (n_f >= params_sw_->n_frames ?
params_sw_->n_fix_first_frames :
diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
index b2ab0e2fdc0d807e6953c0fab95c59ed391b5311..85b9c4cf7f92164f93aa1c10a46c22fa53c07ef8 100644
--- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
@@ -36,7 +36,7 @@ TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeMan
void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
{
- int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames
+ int n_f = getProblem()->getTrajectory()->size(); // in trajectory there are only key frames
// recent segment not complete
if (n_f <= params_swdr_->n_frames_recent)
@@ -46,10 +46,8 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
if (count_frames_ != 0)
{
WOLF_DEBUG("TreeManagerSlidingWindow removing the oldest of recent frames");
- FrameBasePtr remove_recent_frame = std::next(getProblem()->getTrajectory()->rbegin(),
- params_swdr_->n_frames_recent)->second;
- FrameBasePtr keep_recent_frame = std::next(getProblem()->getTrajectory()->rbegin(),
- params_swdr_->n_frames_recent - 1)->second;
+ FrameBasePtr remove_recent_frame = getProblem()->getTrajectory()->getLastFrame()->getPreviousFrame(params_swdr_->n_frames_recent);
+ FrameBasePtr keep_recent_frame = remove_recent_frame->getNextFrame();
// compose motion captures for all processors motion
for (auto motion_provider_pair : getProblem()->getMotionProviderMap())
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/dummy/factor_dummy.h b/test/dummy/factor_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..73174d1468e07807e26ab2a6dcf4579ad5a00ef8
--- /dev/null
+++ b/test/dummy/factor_dummy.h
@@ -0,0 +1,112 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FACTOR_DUMMY_H
+#define FACTOR_DUMMY_H
+
+#include "core/factor/factor_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorDummy);
+
+class FactorDummy : public FactorBase
+{
+ public:
+ FactorDummy(const FeatureBasePtr& _feature,
+ const FrameBasePtr& _frame_other,
+ const CaptureBasePtr& _capture_other,
+ const FeatureBasePtr& _feature_other,
+ const LandmarkBasePtr& _landmark_other) :
+ FactorBase("Dummy",
+ TOP_OTHER,
+ _feature,
+ _frame_other ? FrameBasePtrList({_frame_other}) : FrameBasePtrList(),
+ _capture_other ? CaptureBasePtrList({_capture_other}) : CaptureBasePtrList(),
+ _feature_other ? FeatureBasePtrList({_feature_other}) : FeatureBasePtrList(),
+ _landmark_other ? LandmarkBasePtrList({_landmark_other}) : LandmarkBasePtrList(),
+ nullptr,
+ false)
+ {
+ //
+ }
+ FactorDummy(const FeatureBasePtr& _feature,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list) :
+ FactorBase("Dummy",
+ TOP_OTHER,
+ _feature,
+ _frame_other_list,
+ _capture_other_list,
+ _feature_other_list,
+ _landmark_other_list,
+ nullptr,
+ false)
+ {
+ //
+ }
+ ~FactorDummy() override = default;
+
+ bool evaluate(double const* const* _parameters,
+ double* _residuals,
+ double** _jacobians) const override
+ {
+ return true;
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr,
+ Eigen::VectorXd& _residual,
+ std::vector<Eigen::MatrixXd>& _jacobians) const override
+ {
+
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JacobianMethod::JAC_ANALYTIC;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return std::vector<StateBlockConstPtr>();
+ }
+
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return std::vector<StateBlockPtr>();
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return std::vector<unsigned int>();
+ }
+
+ unsigned int getSize() const override
+ {
+ return 0;
+ }
+};
+
+}
+
+#endif // FACTOR_DUMMY_H
\ No newline at end of file
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
index c04043d857a43f6db5391b2c463b5328ab331a90..b8f8266c43dea8060c28d5677cbb1082527f11a8 100644
--- a/test/dummy/factor_feature_dummy.h
+++ b/test/dummy/factor_feature_dummy.h
@@ -53,7 +53,8 @@ class FactorFeatureDummy : public FactorBase
/** \brief Returns a vector of pointers to the states in which this factor depends
**/
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockConstPtr>(0);}
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override {return std::vector<StateBlockPtr>(0);}
/** \brief Returns the factor residual size
**/
@@ -78,10 +79,10 @@ inline FactorFeatureDummy::FactorFeatureDummy(const FeatureBasePtr& _feature_ptr
FactorBase("FactorFeatureDummy",
TOP_OTHER,
_feature_ptr,
- nullptr,
- nullptr,
- _feature_other_ptr,
- nullptr,
+ FrameBasePtrList(),
+ CaptureBasePtrList(),
+ FeatureBasePtrList({_feature_other_ptr}),
+ LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status)
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
index f4c53fa10da0908a5cd82ef3c83fe9796070abee..ceff8247e73c02bb8ac9bb2ca7151486e3f69f87 100644
--- a/test/dummy/factor_landmark_dummy.h
+++ b/test/dummy/factor_landmark_dummy.h
@@ -42,14 +42,14 @@ class FactorLandmarkDummy : public FactorBase
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
bool evaluate(double const* const* parameters,
- double* residuals,
- double** jacobians) const override {return true;};
+ double* residuals,
+ double** jacobians) const override {return true;};
/** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
**/
void evaluate(const std::vector<const double*>& _states_ptr,
- Eigen::VectorXd& residual_,
- std::vector<Eigen::MatrixXd>& jacobians_) const override {};
+ Eigen::VectorXd& residual_,
+ std::vector<Eigen::MatrixXd>& jacobians_) const override {};
/** \brief Returns the jacobians computation method
**/
@@ -57,7 +57,11 @@ class FactorLandmarkDummy : public FactorBase
/** \brief Returns a vector of pointers to the states in which this factor depends
**/
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return std::vector<StateBlockConstPtr>(0);
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
{
return std::vector<StateBlockPtr>(0);
}
@@ -88,10 +92,10 @@ inline FactorLandmarkDummy::FactorLandmarkDummy(const FeatureBasePtr& _feature_p
FactorBase("FactorFeatureDummy",
TOP_OTHER,
_feature_ptr,
- nullptr,
- nullptr,
- nullptr,
- _landmark_other_ptr,
+ FrameBasePtrList(),
+ CaptureBasePtrList(),
+ FeatureBasePtrList(),
+ LandmarkBasePtrList({_landmark_other_ptr}),
_processor_ptr,
_apply_loss_function,
_status)
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index c8a04dcd16131c9e0549d5b34ed9d81d4c353ec8..fca61dad1708a8ad71ff73b8049cbf99c0d3d9bf 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -29,6 +29,7 @@
#include "core/utils/utils_gtest.h"
#include "core/factor/factor_base.h"
+#include "dummy/factor_dummy.h"
using namespace wolf;
using namespace Eigen;
@@ -55,58 +56,6 @@ class FactorBaseTest : public testing::Test
// virtual void TearDown(){}
};
-class FactorDummy : public FactorBase
-{
- public:
- FactorDummy(const FeatureBasePtr& _feature,
- const FrameBasePtr& _frame_other,
- const CaptureBasePtr& _capture_other,
- const FeatureBasePtr& _feature_other,
- const LandmarkBasePtr& _landmark_other) :
- FactorBase("Dummy",
- TOP_OTHER,
- _feature,
- _frame_other,
- _capture_other,
- _feature_other,
- _landmark_other,
- nullptr,
- false)
- {
- //
- }
- FactorDummy(const FeatureBasePtr& _feature,
- const FrameBasePtrList& _frame_other_list,
- const CaptureBasePtrList& _capture_other_list,
- const FeatureBasePtrList& _feature_other_list,
- const LandmarkBasePtrList& _landmark_other_list) :
- FactorBase("Dummy",
- TOP_OTHER,
- _feature,
- _frame_other_list,
- _capture_other_list,
- _feature_other_list,
- _landmark_other_list,
- nullptr,
- false)
- {
- //
- }
- ~FactorDummy() override = default;
-
- bool evaluate(double const* const* _parameters,
- double* _residuals,
- double** _jacobians) const override {return true;}
- void evaluate(const std::vector<const double*>& _states_ptr,
- Eigen::VectorXd& _residual,
- std::vector<Eigen::MatrixXd>& _jacobians) const override {}
- JacobianMethod getJacobianMethod() const override {return JacobianMethod::JAC_ANALYTIC;}
- std::vector<StateBlockPtr> getStateBlockPtrVector() const override {std::vector<StateBlockPtr> v; return v;}
- std::vector<unsigned int> getStateSizes() const override {std::vector<unsigned int> v; return v;}
- unsigned int getSize() const override {return 0;}
-
-};
-
TEST_F(FactorBaseTest, constructor_from_pointers)
{
FactorDummy fac(f0,nullptr,C0,f1,nullptr);
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 4f280a6ff5482d8d02961438b0aaa119990bf78d..8283b62b8ef35e591dad8533256a517ad65f7718 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -83,7 +83,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
- VectorXd getCalibration (const CaptureBasePtr _capture) const override
+ VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override
{
return Base::getCalibration(_capture);
}
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 26c9b3cc0fe7f5aff4eb533c93826c84cb9d0d55..eb61f798d41b2aec792b864dffb39cb1cdb7d098 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -170,28 +170,226 @@ TEST(FrameBase, Frames)
ASSERT_EQ(F9, T->closestFrameToTimeStamp(9));
// Next frame
- ASSERT_EQ(F1, F0->getNextFrame());
- ASSERT_EQ(F2, F1->getNextFrame());
- ASSERT_EQ(F3, F2->getNextFrame());
- ASSERT_EQ(F4, F3->getNextFrame());
- ASSERT_EQ(F5, F4->getNextFrame());
- ASSERT_EQ(F6, F5->getNextFrame());
- ASSERT_EQ(F7, F6->getNextFrame());
- ASSERT_EQ(F8, F7->getNextFrame());
- ASSERT_EQ(F9, F8->getNextFrame());
- ASSERT_EQ(nullptr, F9->getNextFrame());
+ EXPECT_EQ(F1, F0->getNextFrame(1));
+ EXPECT_EQ(F2, F0->getNextFrame(2));
+ EXPECT_EQ(F3, F0->getNextFrame(3));
+ EXPECT_EQ(F4, F0->getNextFrame(4));
+ EXPECT_EQ(F5, F0->getNextFrame(5));
+ EXPECT_EQ(F6, F0->getNextFrame(6));
+ EXPECT_EQ(F7, F0->getNextFrame(7));
+ EXPECT_EQ(F8, F0->getNextFrame(8));
+ EXPECT_EQ(F9, F0->getNextFrame(9));
+ EXPECT_EQ(nullptr, F0->getNextFrame(10));
+
+ EXPECT_EQ(F2, F1->getNextFrame(1));
+ EXPECT_EQ(F3, F1->getNextFrame(2));
+ EXPECT_EQ(F4, F1->getNextFrame(3));
+ EXPECT_EQ(F5, F1->getNextFrame(4));
+ EXPECT_EQ(F6, F1->getNextFrame(5));
+ EXPECT_EQ(F7, F1->getNextFrame(6));
+ EXPECT_EQ(F8, F1->getNextFrame(7));
+ EXPECT_EQ(F9, F1->getNextFrame(8));
+ EXPECT_EQ(nullptr, F1->getNextFrame(9));
+ EXPECT_EQ(nullptr, F1->getNextFrame(10));
+
+ EXPECT_EQ(F3, F2->getNextFrame(1));
+ EXPECT_EQ(F4, F2->getNextFrame(2));
+ EXPECT_EQ(F5, F2->getNextFrame(3));
+ EXPECT_EQ(F6, F2->getNextFrame(4));
+ EXPECT_EQ(F7, F2->getNextFrame(5));
+ EXPECT_EQ(F8, F2->getNextFrame(6));
+ EXPECT_EQ(F9, F2->getNextFrame(7));
+ EXPECT_EQ(nullptr, F2->getNextFrame(8));
+ EXPECT_EQ(nullptr, F2->getNextFrame(9));
+ EXPECT_EQ(nullptr, F2->getNextFrame(10));
+
+ EXPECT_EQ(F4, F3->getNextFrame(1));
+ EXPECT_EQ(F5, F3->getNextFrame(2));
+ EXPECT_EQ(F6, F3->getNextFrame(3));
+ EXPECT_EQ(F7, F3->getNextFrame(4));
+ EXPECT_EQ(F8, F3->getNextFrame(5));
+ EXPECT_EQ(F9, F3->getNextFrame(6));
+ EXPECT_EQ(nullptr, F3->getNextFrame(7));
+ EXPECT_EQ(nullptr, F3->getNextFrame(8));
+ EXPECT_EQ(nullptr, F3->getNextFrame(9));
+ EXPECT_EQ(nullptr, F3->getNextFrame(10));
+
+ EXPECT_EQ(F5, F4->getNextFrame(1));
+ EXPECT_EQ(F6, F4->getNextFrame(2));
+ EXPECT_EQ(F7, F4->getNextFrame(3));
+ EXPECT_EQ(F8, F4->getNextFrame(4));
+ EXPECT_EQ(F9, F4->getNextFrame(5));
+ EXPECT_EQ(nullptr, F4->getNextFrame(6));
+ EXPECT_EQ(nullptr, F4->getNextFrame(7));
+ EXPECT_EQ(nullptr, F4->getNextFrame(8));
+ EXPECT_EQ(nullptr, F4->getNextFrame(9));
+ EXPECT_EQ(nullptr, F4->getNextFrame(10));
+
+ EXPECT_EQ(F6, F5->getNextFrame(1));
+ EXPECT_EQ(F7, F5->getNextFrame(2));
+ EXPECT_EQ(F8, F5->getNextFrame(3));
+ EXPECT_EQ(F9, F5->getNextFrame(4));
+ EXPECT_EQ(nullptr, F5->getNextFrame(5));
+ EXPECT_EQ(nullptr, F5->getNextFrame(6));
+ EXPECT_EQ(nullptr, F5->getNextFrame(7));
+ EXPECT_EQ(nullptr, F5->getNextFrame(8));
+ EXPECT_EQ(nullptr, F5->getNextFrame(9));
+ EXPECT_EQ(nullptr, F5->getNextFrame(10));
+
+ EXPECT_EQ(F7, F6->getNextFrame(1));
+ EXPECT_EQ(F8, F6->getNextFrame(2));
+ EXPECT_EQ(F9, F6->getNextFrame(3));
+ EXPECT_EQ(nullptr, F6->getNextFrame(4));
+ EXPECT_EQ(nullptr, F6->getNextFrame(5));
+ EXPECT_EQ(nullptr, F6->getNextFrame(6));
+ EXPECT_EQ(nullptr, F6->getNextFrame(7));
+ EXPECT_EQ(nullptr, F6->getNextFrame(8));
+ EXPECT_EQ(nullptr, F6->getNextFrame(9));
+ EXPECT_EQ(nullptr, F6->getNextFrame(10));
+
+ EXPECT_EQ(F8, F7->getNextFrame(1));
+ EXPECT_EQ(F9, F7->getNextFrame(2));
+ EXPECT_EQ(nullptr, F7->getNextFrame(3));
+ EXPECT_EQ(nullptr, F7->getNextFrame(4));
+ EXPECT_EQ(nullptr, F7->getNextFrame(5));
+ EXPECT_EQ(nullptr, F7->getNextFrame(6));
+ EXPECT_EQ(nullptr, F7->getNextFrame(7));
+ EXPECT_EQ(nullptr, F7->getNextFrame(8));
+ EXPECT_EQ(nullptr, F7->getNextFrame(9));
+ EXPECT_EQ(nullptr, F7->getNextFrame(10));
+
+ EXPECT_EQ(F9, F8->getNextFrame(1));
+ EXPECT_EQ(nullptr, F8->getNextFrame(2));
+ EXPECT_EQ(nullptr, F8->getNextFrame(3));
+ EXPECT_EQ(nullptr, F8->getNextFrame(4));
+ EXPECT_EQ(nullptr, F8->getNextFrame(5));
+ EXPECT_EQ(nullptr, F8->getNextFrame(6));
+ EXPECT_EQ(nullptr, F8->getNextFrame(7));
+ EXPECT_EQ(nullptr, F8->getNextFrame(8));
+ EXPECT_EQ(nullptr, F8->getNextFrame(9));
+ EXPECT_EQ(nullptr, F8->getNextFrame(10));
+
+ EXPECT_EQ(nullptr, F9->getNextFrame(1));
+ EXPECT_EQ(nullptr, F9->getNextFrame(2));
+ EXPECT_EQ(nullptr, F9->getNextFrame(3));
+ EXPECT_EQ(nullptr, F9->getNextFrame(4));
+ EXPECT_EQ(nullptr, F9->getNextFrame(5));
+ EXPECT_EQ(nullptr, F9->getNextFrame(6));
+ EXPECT_EQ(nullptr, F9->getNextFrame(7));
+ EXPECT_EQ(nullptr, F9->getNextFrame(8));
+ EXPECT_EQ(nullptr, F9->getNextFrame(9));
+ EXPECT_EQ(nullptr, F9->getNextFrame(10));
// Prev frame
- ASSERT_EQ(nullptr, F0->getPreviousFrame());
- ASSERT_EQ(F0, F1->getPreviousFrame());
- ASSERT_EQ(F1, F2->getPreviousFrame());
- ASSERT_EQ(F2, F3->getPreviousFrame());
- ASSERT_EQ(F3, F4->getPreviousFrame());
- ASSERT_EQ(F4, F5->getPreviousFrame());
- ASSERT_EQ(F5, F6->getPreviousFrame());
- ASSERT_EQ(F6, F7->getPreviousFrame());
- ASSERT_EQ(F7, F8->getPreviousFrame());
- ASSERT_EQ(F8, F9->getPreviousFrame());
+ EXPECT_EQ(F8, F9->getPreviousFrame(1));
+ EXPECT_EQ(F7, F9->getPreviousFrame(2));
+ EXPECT_EQ(F6, F9->getPreviousFrame(3));
+ EXPECT_EQ(F5, F9->getPreviousFrame(4));
+ EXPECT_EQ(F4, F9->getPreviousFrame(5));
+ EXPECT_EQ(F3, F9->getPreviousFrame(6));
+ EXPECT_EQ(F2, F9->getPreviousFrame(7));
+ EXPECT_EQ(F1, F9->getPreviousFrame(8));
+ EXPECT_EQ(F0, F9->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F9->getPreviousFrame(10));
+
+ EXPECT_EQ(F7, F8->getPreviousFrame(1));
+ EXPECT_EQ(F6, F8->getPreviousFrame(2));
+ EXPECT_EQ(F5, F8->getPreviousFrame(3));
+ EXPECT_EQ(F4, F8->getPreviousFrame(4));
+ EXPECT_EQ(F3, F8->getPreviousFrame(5));
+ EXPECT_EQ(F2, F8->getPreviousFrame(6));
+ EXPECT_EQ(F1, F8->getPreviousFrame(7));
+ EXPECT_EQ(F0, F8->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F8->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F8->getPreviousFrame(10));
+
+ EXPECT_EQ(F6, F7->getPreviousFrame(1));
+ EXPECT_EQ(F5, F7->getPreviousFrame(2));
+ EXPECT_EQ(F4, F7->getPreviousFrame(3));
+ EXPECT_EQ(F3, F7->getPreviousFrame(4));
+ EXPECT_EQ(F2, F7->getPreviousFrame(5));
+ EXPECT_EQ(F1, F7->getPreviousFrame(6));
+ EXPECT_EQ(F0, F7->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F7->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F7->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F7->getPreviousFrame(10));
+
+ EXPECT_EQ(F5, F6->getPreviousFrame(1));
+ EXPECT_EQ(F4, F6->getPreviousFrame(2));
+ EXPECT_EQ(F3, F6->getPreviousFrame(3));
+ EXPECT_EQ(F2, F6->getPreviousFrame(4));
+ EXPECT_EQ(F1, F6->getPreviousFrame(5));
+ EXPECT_EQ(F0, F6->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F6->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F6->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F6->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F6->getPreviousFrame(10));
+
+ EXPECT_EQ(F4, F5->getPreviousFrame(1));
+ EXPECT_EQ(F3, F5->getPreviousFrame(2));
+ EXPECT_EQ(F2, F5->getPreviousFrame(3));
+ EXPECT_EQ(F1, F5->getPreviousFrame(4));
+ EXPECT_EQ(F0, F5->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(10));
+
+ EXPECT_EQ(F3, F4->getPreviousFrame(1));
+ EXPECT_EQ(F2, F4->getPreviousFrame(2));
+ EXPECT_EQ(F1, F4->getPreviousFrame(3));
+ EXPECT_EQ(F0, F4->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(10));
+
+ EXPECT_EQ(F2, F3->getPreviousFrame(1));
+ EXPECT_EQ(F1, F3->getPreviousFrame(2));
+ EXPECT_EQ(F0, F3->getPreviousFrame(3));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(10));
+
+ EXPECT_EQ(F1, F2->getPreviousFrame(1));
+ EXPECT_EQ(F0, F2->getPreviousFrame(2));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(3));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(10));
+
+ EXPECT_EQ(F0, F1->getPreviousFrame(1));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(2));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(3));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(10));
+
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(1));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(2));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(3));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(10));
}
#include "core/state_block/state_quaternion.h"
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 225a49326c54d4e743bd7cd6c43bf839785b2e3d..04d838944dd50139da61d890c0171e0b41a33cb0 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -95,8 +95,9 @@ void show(const ProblemPtr& problem)
using std::endl;
cout << std::setprecision(4);
- for (FrameBasePtr F : *problem->getTrajectory())
+ for (auto F_pair : problem->getTrajectory()->getFrameMap())
{
+ auto F = F_pair.second;
cout << "----- Key Frame " << F->id() << " -----" << endl;
if (!F->getCaptureList().empty())
{
@@ -324,8 +325,9 @@ TEST(Odom2d, VoteForKfAndSolve)
// Check the 3 KFs' states and covariances
MatrixXd computed_cov;
int n = 0;
- for (auto F : *problem->getTrajectory())
+ for (auto F_pair : problem->getTrajectory()->getFrameMap())
{
+ auto F = F_pair.second;
ASSERT_POSE2d_APPROX(F->getStateVector("PO"), integrated_pose_vector[n] , 1e-6);
ASSERT_TRUE (F->getCovariance(computed_cov));
ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[n] , 1e-6);
@@ -397,7 +399,6 @@ TEST(Odom2d, KF_callback)
// std::cout << "\nIntegrating data..." << std::endl;
// Capture to use as container for all incoming data
- auto capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
std::cout << "t: " << t << std::endl;
for (int n=1; n<=N; n++)
@@ -405,7 +406,8 @@ TEST(Odom2d, KF_callback)
t += dt;
// re-use capture with updated timestamp
- capture->setTimeStamp(t);
+ auto capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
+ //capture->setTimeStamp(t);
// Processor
capture->process();
@@ -442,7 +444,7 @@ TEST(Odom2d, KF_callback)
FrameBasePtr keyframe_2 = problem->emplaceFrame(t_split, x_split);
// there must be 2KF and one F
- ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(), 2);
+ ASSERT_EQ(problem->getTrajectory()->size(), 2);
// The last KF must have TS = 0.08
ASSERT_EQ(problem->getLastFrame()->getTimeStamp().getNanoSeconds(), 80000000);
@@ -450,7 +452,7 @@ TEST(Odom2d, KF_callback)
processor_odom2d->keyFrameCallback(keyframe_2);
ASSERT_TRUE(problem->check(0));
t += dt;
- capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
+ auto capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
capture->process();
ASSERT_TRUE(problem->check(0));
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 80fe044c410c9b07f5e059461875a97199d1c6de..522246910cc6bd3944f6924a832fbf028cb2b75c 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);
@@ -202,10 +203,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);
@@ -253,7 +253,7 @@ TEST(Problem, emplaceFrame_factory)
// check that all frames are effectively in the trajectory
- ASSERT_EQ(P->getTrajectory()->getFrameMap().size(), (SizeStd) 3);
+ ASSERT_EQ(P->getTrajectory()->size(), (SizeStd) 3);
ASSERT_EQ(f0->getStateVector().size(), 3 );
ASSERT_EQ(f1->getStateVector().size(), 7 );
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();
}
diff --git a/test/gtest_solver_ceres_multithread.cpp b/test/gtest_solver_ceres_multithread.cpp
index c0004ad3992e256e7a3e5d883ebaea1707295775..8c5a2685c5155a9c5a5efbe28fb2ea26a7c899a6 100644
--- a/test/gtest_solver_ceres_multithread.cpp
+++ b/test/gtest_solver_ceres_multithread.cpp
@@ -84,8 +84,8 @@ TEST(SolverCeresMultithread, MultiThreadingTruncatedNotifications)
ts += 1.0;
- if (P->getTrajectory()->getFrameMap().size() > 10)
- (*P->getTrajectory()->begin())->remove();
+ if (P->getTrajectory()->size() > 10)
+ P->getTrajectory()->getFirstFrame()->remove();
if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
break;
diff --git a/test/gtest_solver_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp
index 8c21ca410b7b9111f11994ebad9f74cfc91cfdba..099297e77a3e3a4d9e2663b078b50919d1a52d24 100644
--- a/test/gtest_solver_manager_multithread.cpp
+++ b/test/gtest_solver_manager_multithread.cpp
@@ -79,8 +79,8 @@ TEST(SolverManagerMultithread, MultiThreadingTruncatedNotifications)
ts += 1.0;
- if (P->getTrajectory()->getFrameMap().size() > 10)
- (*P->getTrajectory()->begin())->remove();
+ if (P->getTrajectory()->size() > 10)
+ P->getTrajectory()->getFirstFrame()->remove();
if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
break;