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/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 3176f13c2c007c8489b16746d00a5698151e2d73..ccde83ed32c34c64bd4ca0d3e0c3e211b6ec0719 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -48,7 +48,9 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
private:
FrameBaseWPtr frame_ptr_;
FeatureBasePtrList feature_list_;
+ FeatureBaseConstPtrList feature_const_list_;
FactorBasePtrList constrained_by_list_;
+ FactorBaseConstPtrList constrained_by_const_list_;
SensorBaseWPtr sensor_ptr_; ///< Pointer to sensor
static unsigned int capture_id_count_;
@@ -86,10 +88,12 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
void setFrame(const FrameBasePtr _frm_ptr);
public:
- FeatureBaseConstPtrList getFeatureList() const;
+ const FeatureBaseConstPtrList& getFeatureList() const;
+ const FeatureBasePtrList& getFeatureList();
+
+ FactorBaseConstPtrList getFactorList() const;
FactorBasePtrList getFactorList();
-
- void getFactorList(FeatureBaseConstPtrList& _fac_list) const;
+ void getFactorList(FactorBaseConstPtrList& _fac_list) const;
void getFactorList(FactorBasePtrList& _fac_list);
SensorBaseConstPtr getSensor() const;
@@ -103,9 +107,9 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
public:
unsigned int getHits() const;
- FactorBaseConstPtrList getConstrainedByList() const;
- FactorBasePtrList getConstrainedByList();
- bool isConstrainedBy(const FactorBasePtr &_factor) const;
+ const FactorBaseConstPtrList& getConstrainedByList() const;
+ const FactorBasePtrList& getConstrainedByList();
+ bool isConstrainedBy(const FactorBaseConstPtr &_factor) const;
// State blocks
StateBlockConstPtr getStateBlock(const char& _key) const;
@@ -141,8 +145,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);
@@ -218,7 +222,12 @@ inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
frame_ptr_ = _frm_ptr;
}
-inline const FeatureBasePtrList& CaptureBase::getFeatureList() const
+inline const FeatureBaseConstPtrList& CaptureBase::getFeatureList() const
+{
+ return feature_const_list_;
+}
+
+inline const FeatureBasePtrList& CaptureBase::getFeatureList()
{
return feature_list_;
}
@@ -228,18 +237,27 @@ inline unsigned int CaptureBase::getHits() const
return constrained_by_list_.size();
}
-inline const FactorBasePtrList& CaptureBase::getConstrainedByList() const
+inline const FactorBaseConstPtrList& CaptureBase::getConstrainedByList() const
{
- return constrained_by_list_;
+ return constrained_by_const_list_;
}
+inline const 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();
}
@@ -259,7 +277,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/wolf.h b/include/core/common/wolf.h
index 0f4346ff54d127eec462a82709a1170c83deb883..2553117f39370d0ccc01e4344a14f7a05f1e5180 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -201,22 +201,27 @@ struct MatrixSizeCheck
class ClassName; \
typedef std::shared_ptr<ClassName> ClassName##Ptr; \
typedef std::shared_ptr<const ClassName> ClassName##ConstPtr; \
- typedef std::weak_ptr<ClassName> ClassName##WPtr;
+ typedef std::weak_ptr<ClassName> ClassName##WPtr; \
+ typedef std::weak_ptr<const ClassName> ClassName##ConstWPtr; \
#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##ConstPtr> ClassName##ConstPtrList; \
- typedef ClassName##ConstPtrList::iterator ClassName##ConstIter; \
- typedef ClassName##ConstPtrList::const_iterator ClassName##ConstConstIter; \
- typedef ClassName##ConstPtrList::reverse_iterator ClassName##ConstRevIter;
+ 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; \
@@ -262,7 +267,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..5b9ccf175d4415b891f500e17663027cccd8d139 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:
@@ -67,7 +68,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..b51d4cee4482fad97d3ae94512bf7b5ee69392e3 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_;
@@ -107,9 +108,12 @@ class FactorAutodiff : public FactorBase
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})
+ 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 +305,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 +315,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 +349,273 @@ 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);
- }
-
- ~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_;
- }
+ 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) :
+ 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}),
+ 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;
+ }
- 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,255 +623,263 @@ 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;
- }
+ 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) :
+ 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}),
+ 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 ////////////////////////////////////////////////////////////////////////
@@ -863,245 +887,253 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
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;
- }
+ 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) :
+ 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}),
+ 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 ////////////////////////////////////////////////////////////////////////
@@ -1109,1594 +1141,1650 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
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_;
- }
+ 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) :
+ 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}),
+ 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;
+ }
+};
- std::vector<unsigned int> getStateSizes() const override
- {
- return state_block_sizes_;
- }
+////////////////// 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) :
+ 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}),
+ 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;
+ }
+};
- 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) :
+ 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}),
+ 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_;
-
- 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;
- }
+ 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) :
+ 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}),
+ 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_;
-
- 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;
- }
+ 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) :
+ 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}),
+ 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) :
+ 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}),
+ 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) :
+ 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}),
+ 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) :
+ 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}),
+ 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..a7e0235490ba9212a766f414f5b72539c93347f8 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,22 +77,30 @@ 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)
static unsigned int factor_id_count_;
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
+
+ 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
+
+ FrameBaseConstWPtrList frame_other_const_list_; ///< FrameBase pointer list
+ CaptureBaseConstWPtrList capture_other_const_list_; ///< CaptureBase pointer list
+ FeatureBaseConstWPtrList feature_other_const_list_; ///< FeatureBase pointer list
+ LandmarkBaseConstWPtrList landmark_other_const_list_; ///< LandmarkBase pointer list
+
+ ProcessorBaseWPtr processor_ptr_; ///< Processor pointer
+
+ Eigen::VectorXd measurement_; ///< the measurement vector
+ Eigen::MatrixXd measurement_sqrt_information_upper_; ///< the squared root information matrix
void setProblem(ProblemPtr) override final;
public:
@@ -154,7 +162,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 +179,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 +215,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_; }
+ 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 { return frame_other_const_list_; }
+ CaptureBaseConstWPtrList getCaptureOtherList() const { return capture_other_const_list_; }
+ FeatureBaseConstWPtrList getFeatureOtherList() const { return feature_other_const_list_; }
+ LandmarkBaseConstWPtrList getLandmarkOtherList() const { return landmark_other_const_list_; }
+
+ 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;
- 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;
-
- public:
/**
* @brief getProcessor
* @return
*/
- ProcessorBasePtr getProcessor() const;
+ ProcessorBaseConstPtr getProcessor() const;
+ ProcessorBasePtr getProcessor();
void link(FeatureBasePtr ftr);
template<typename classType, typename... T>
@@ -252,8 +273,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 +312,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 +332,7 @@ 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;
@@ -314,7 +340,23 @@ inline FrameBasePtr FactorBase::getFrameOther() const
return frame_other_list_.front().lock();
}
-inline CaptureBasePtr FactorBase::getCaptureOther() const
+inline FrameBasePtr FactorBase::getFrameOther()
+{
+ if (frame_other_list_.empty()) return nullptr;
+ if (frame_other_list_.front().expired()) return nullptr;
+
+ return frame_other_list_.front().lock();
+}
+
+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 +364,15 @@ 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;
+
+ return feature_other_list_.front().lock();
+}
+
+inline FeatureBasePtr FactorBase::getFeatureOther()
{
if (feature_other_list_.empty()) return nullptr;
if (feature_other_list_.front().expired()) return nullptr;
@@ -330,7 +380,7 @@ inline FeatureBasePtr FactorBase::getFeatureOther() const
return feature_other_list_.front().lock();
}
-inline LandmarkBasePtr FactorBase::getLandmarkOther() const
+inline LandmarkBaseConstPtr FactorBase::getLandmarkOther() const
{
if (landmark_other_list_.empty()) return nullptr;
if (landmark_other_list_.front().expired()) return nullptr;
@@ -338,7 +388,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();
}
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..76154db503d77a7356ed4103304ea45ee5f6f10c 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -46,7 +46,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
private:
CaptureBaseWPtr capture_ptr_;
FactorBasePtrList factor_list_;
+ FactorBaseConstPtrList factor_const_list_;
FactorBasePtrList constrained_by_list_;
+ FactorBaseConstPtrList constrained_by_const_list_;
static unsigned int feature_id_count_;
@@ -116,20 +118,21 @@ 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;
+ const FactorBaseConstPtrList& getFactorList() const;
+ const FactorBasePtrList& getFactorList();
+ void getFactorList(FactorBaseConstPtrList & _fac_list) const;
+ void getFactorList(FactorBasePtrList & _fac_list);
unsigned int getHits() const;
- const FactorBasePtrList& getConstrainedByList() const;
- bool isConstrainedBy(const FactorBasePtr &_factor) const;
-
-
-
- // all factors
- void getFactorList(FactorBasePtrList & _fac_list) const;
+ const FactorBaseConstPtrList& getConstrainedByList() const;
+ const FactorBasePtrList& getConstrainedByList();
+ bool isConstrainedBy(const FactorBaseConstPtr &_factor) const;
void link(CaptureBasePtr cap_ptr);
template<typename classType, typename... T>
@@ -148,8 +151,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 +170,25 @@ 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 const FactorBaseConstPtrList& FeatureBase::getConstrainedByList() const
+{
+ return constrained_by_const_list_;
+}
+
+inline const FactorBasePtrList& FeatureBase::getConstrainedByList()
{
return constrained_by_list_;
}
@@ -190,7 +198,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..a228317ff8a9203a3584e703220b74e77a19f728 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -51,7 +51,9 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
private:
TrajectoryBaseWPtr trajectory_ptr_;
CaptureBasePtrList capture_list_;
+ CaptureBaseConstPtrList capture_const_list_;
FactorBasePtrList constrained_by_list_;
+ FactorBaseConstPtrList constrained_by_const_list_;
static unsigned int frame_id_count_;
@@ -104,7 +106,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 +119,58 @@ 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;
+ FrameBaseConstPtr getNextFrame() const;
+ FrameBasePtr getPreviousFrame();
+ FrameBasePtr getNextFrame();
+
+ const CaptureBaseConstPtrList& getCaptureList() const;
+ const CaptureBasePtrList& getCaptureList();
+
+ FactorBaseConstPtrList getFactorList() const;
+ FactorBasePtrList getFactorList();
+ void getFactorList(FactorBaseConstPtrList& _fac_list) const;
+ void getFactorList(FactorBasePtrList& _fac_list);
+
+ const FactorBaseConstPtrList& getConstrainedByList() const;
+ 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 +187,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 +234,32 @@ 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 const CaptureBaseConstPtrList& FrameBase::getCaptureList() const
+{
+ return capture_const_list_;
+}
+
+inline const CaptureBasePtrList& FrameBase::getCaptureList()
{
return capture_list_;
}
@@ -224,7 +269,12 @@ inline unsigned int FrameBase::getHits() const
return constrained_by_list_.size();
}
-inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
+inline const FactorBaseConstPtrList& FrameBase::getConstrainedByList() const
+{
+ return constrained_by_const_list_;
+}
+
+inline const FactorBasePtrList& FrameBase::getConstrainedByList()
{
return constrained_by_list_;
}
@@ -235,19 +285,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..3dc69962ca482aa29dbf30de9097132e98dcbcb1 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -41,12 +41,14 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
private:
SensorBasePtrList sensor_list_;
+ SensorBaseConstPtrList sensor_const_list_;
public:
HardwareBase();
~HardwareBase() override;
- const SensorBasePtrList& getSensorList() const;
+ const SensorBaseConstPtrList& getSensorList() const;
+ const SensorBasePtrList& getSensorList();
virtual void printHeader(int depth, //
bool constr_by, //
@@ -61,8 +63,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 +76,12 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
namespace wolf {
-inline const SensorBasePtrList& HardwareBase::getSensorList() const
+inline const SensorBaseConstPtrList& HardwareBase::getSensorList() const
+{
+ return sensor_const_list_;
+}
+
+inline const SensorBasePtrList& HardwareBase::getSensorList()
{
return sensor_list_;
}
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 7984e6773a80615aa33a84f67156cf83f1c42ab6..bd8056f45b6cbff177d567044b24425b5008c2d9 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -49,7 +49,9 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
private:
MapBaseWPtr map_ptr_;
FactorBasePtrList constrained_by_list_;
+ FactorBaseConstPtrList constrained_by_const_list_;
std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
+ std::vector<StateBlockConstPtr> state_block_const_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
static unsigned int landmark_id_count_;
@@ -80,7 +82,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 +92,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;
+ const FactorBaseConstPtrList& getConstrainedByList() const;
+ 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 +124,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 +150,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 +182,12 @@ inline unsigned int LandmarkBase::getHits() const
return constrained_by_list_.size();
}
-inline const FactorBasePtrList& LandmarkBase::getConstrainedByList() const
+inline const FactorBaseConstPtrList& LandmarkBase::getConstrainedByList() const
+{
+ return constrained_by_const_list_;
+}
+
+inline const 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..9c32c1411bf4b236d74b39bfd957be15fa7fad4a 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -98,6 +98,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
private:
LandmarkBasePtrList landmark_list_;
+ LandmarkBaseConstPtrList landmark_const_list_;
public:
MapBase();
@@ -111,7 +112,8 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
virtual void removeLandmark(LandmarkBasePtr _landmark_ptr);
public:
- const LandmarkBasePtrList& getLandmarkList() const;
+ const LandmarkBaseConstPtrList& getLandmarkList() const;
+ 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 +131,19 @@ 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 const LandmarkBaseConstPtrList& MapBase::getLandmarkList() const
+{
+ return landmark_const_list_;
+}
+
+inline const LandmarkBasePtrList& MapBase::getLandmarkList()
{
return landmark_list_;
}
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 7405ae8320deb339ed6efdfe7319b4df8ec14cff..a4bcb46858d50726591b7c5e49ae060b0b6892af 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -74,12 +74,13 @@ 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<int, MotionProviderConstPtr> motion_provider_const_map_;
+ std::map<std::pair<StateBlockConstPtr, StateBlockConstPtr>, Eigen::MatrixXd> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
std::map<StateBlockPtr, Notification> state_block_notification_map_;
@@ -113,13 +114,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,7 +154,8 @@ 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 getSensor(const std::string& _sensor_name) const;
+ SensorBaseConstPtr getSensor(const std::string& _sensor_name) const;
+ SensorBasePtr getSensor(const std::string& _sensor_name);
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
@@ -197,11 +201,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;
+ const std::map<int,MotionProviderConstPtr>& getMotionProviderMap() const;
+ const std::map<int,MotionProviderPtr>& getMotionProviderMap();
// Trajectory branch ----------------------------------
- TrajectoryBasePtr getTrajectory() const;
+ TrajectoryBaseConstPtr getTrajectory() const;
+ TrajectoryBasePtr getTrajectory();
// Prior
bool isPriorSet() const;
@@ -229,9 +234,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
@@ -246,8 +251,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
@@ -278,8 +283,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
@@ -294,7 +299,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
@@ -311,8 +316,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
*
@@ -342,7 +349,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, //
@@ -358,9 +366,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;
@@ -433,7 +441,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_;
}
@@ -443,12 +456,12 @@ inline bool Problem::isPriorSet() const
return prior_options_ == nullptr;
}
-inline std::map<int,MotionProviderPtr>& Problem::getMotionProviderMap()
+inline const std::map<int,MotionProviderConstPtr>& Problem::getMotionProviderMap() const
{
- return motion_provider_map_;
+ return motion_provider_const_map_;
}
-inline const std::map<int,MotionProviderPtr>& Problem::getMotionProviderMap() const
+inline const 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/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 0fdcac98b7c6c317b29f2a6dd718ae155db6e018..33b1cafbd298bbd7bdff31d182ecec47169f5325 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.
@@ -111,6 +108,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
private:
HardwareBaseWPtr hardware_ptr_;
ProcessorBasePtrList processor_list_;
+ ProcessorBaseConstPtrList processor_const_list_;
static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory)
@@ -177,8 +175,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 +184,53 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
void removeProcessor(ProcessorBasePtr _proc_ptr);
public:
- const ProcessorBasePtrList& getProcessorList() const;
+ const ProcessorBaseConstPtrList& getProcessorList() const;
+ 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();
public:
@@ -274,8 +285,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 +315,22 @@ inline unsigned int SensorBase::id() const
return sensor_id_;
}
-inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
+inline const ProcessorBaseConstPtrList& SensorBase::getProcessorList() const
+{
+ return processor_const_list_;
+}
+
+inline const 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 +365,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 035ee20728d743bd42cd2ff62666c74b88f0dfbe..3d4d7326d059338a437ddf8442560c130337055a 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -49,7 +49,7 @@ class HasStateBlocks
void appendToStructure(const char& _frame_type){ structure_ += _frame_type; }
bool isInStructure(const char& _sb_type) const { return structure_.find(_sb_type) != std::string::npos; }
- const std::unordered_map<char, StateBlockPtr>& getStateBlockMap() 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();
@@ -68,12 +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;
+ bool hasStateBlock(const StateBlockConstPtr& _sb) const;
bool setStateBlock(const char _sb_type, const StateBlockPtr& _sb);
- bool stateBlockKey(const StateBlockPtr& _sb, char& _key) 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, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb) const;
+ 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>
@@ -396,11 +397,11 @@ inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) co
return size;
}
-inline std::unordered_map<char, StateBlockConstPtr>::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, StateBlockPtr>& pair)
+ [_sb](const std::pair<char, StateBlockConstPtr>& pair)
{
return pair.second == _sb;
}
@@ -422,18 +423,18 @@ 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 it != 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..c601d10c35287393c1837b5cc4e4c10e6f5cb7ab 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -39,17 +39,20 @@ class FrameBase;
namespace wolf {
-class TrajectoryIter : public std::map<TimeStamp, FrameBasePtr>::const_iterator
+typedef std::map<TimeStamp, FrameBasePtr>::const_iterator TrajectoryIter;
+typedef std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator TrajectoryRevIter;
+typedef std::map<TimeStamp, FrameBaseConstPtr>::const_iterator TrajectoryConstIter;
+typedef std::map<TimeStamp, FrameBaseConstPtr>::const_reverse_iterator TrajectoryConstRevIter;
+
+/*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 {
+ const FrameBasePtr& operator*() const
+ {
return std::map<TimeStamp, FrameBasePtr>::const_iterator::operator*().second;
}
};
@@ -59,23 +62,50 @@ class TrajectoryRevIter : public std::map<TimeStamp, FrameBasePtr>::const_revers
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 TrajectoryConstIter : public std::map<TimeStamp, FrameBaseConstPtr>::const_iterator
+{
+ public:
+ TrajectoryConstIter(std::map<TimeStamp, FrameBaseConstPtr>::const_iterator src)
+ : std::map<TimeStamp, FrameBaseConstPtr>::const_iterator(std::move(src))
+ {}
// override the indirection operator
- const FrameBasePtr& operator*() const {
- return std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator::operator*().second;
+ const FrameBaseConstPtr& operator*() const
+ {
+ return std::map<TimeStamp, FrameBaseConstPtr>::const_iterator::operator*().second;
}
};
+class TrajectoryConstRevIter : public std::map<TimeStamp, FrameBaseConstPtr>::const_reverse_iterator
+{
+ public:
+ TrajectoryConstRevIter(std::map<TimeStamp, FrameBaseConstPtr>::const_reverse_iterator src)
+ : std::map<TimeStamp, FrameBaseConstPtr>::const_reverse_iterator(std::move(src))
+ {}
+
+ // override the indirection operator
+ const FrameBaseConstPtr& operator*() const
+ {
+ return std::map<TimeStamp, FrameBaseConstPtr>::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_;
+ FrameConstPtrMap frame_const_map_;
protected:
StateStructure frame_structure_; // Defines the structure of the Frames in the Trajectory.
@@ -85,14 +115,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;
+ const FrameConstPtrMap& getFrameMap() const;
+ const FramePtrMap& getFrameMap();
+ FrameBaseConstPtr getLastFrame() const;
+ FrameBasePtr getLastFrame();
+ FrameBaseConstPtr getFirstFrame() const;
+ FrameBasePtr getFirstFrame();
+ FrameBaseConstPtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
+ FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts);
+ TrajectoryIter begin();
+ TrajectoryIter end();
+ TrajectoryRevIter rbegin();
+ TrajectoryRevIter rend();
+ TrajectoryConstIter begin() const;
+ TrajectoryConstIter end() const;
+ TrajectoryConstRevIter rbegin() const;
+ TrajectoryConstRevIter rend() const;
virtual void printHeader(int depth, //
bool constr_by, //
@@ -107,50 +145,86 @@ 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 const FrameConstPtrMap& TrajectoryBase::getFrameMap() const
+{
+ return frame_const_map_;
+}
+
+inline const FramePtrMap& TrajectoryBase::getFrameMap()
{
return frame_map_;
}
-inline FrameBasePtr TrajectoryBase::getFirstFrame() const
+inline FrameBaseConstPtr TrajectoryBase::getFirstFrame() const
+{
+ return frame_const_map_.begin()->second;
+}
+
+inline FrameBasePtr TrajectoryBase::getFirstFrame()
+{
+ return frame_map_.begin()->second;
+}
+
+inline FrameBaseConstPtr TrajectoryBase::getLastFrame() const
{
- auto it = static_cast<TrajectoryIter>(frame_map_.begin());
- return *it;
+ return frame_const_map_.rbegin()->second;
}
-inline TrajectoryIter TrajectoryBase::begin() const
+
+inline FrameBasePtr TrajectoryBase::getLastFrame()
{
- return static_cast<TrajectoryIter>(frame_map_.begin());
+ return frame_map_.rbegin()->second;
}
-inline TrajectoryIter TrajectoryBase::end() const
+
+inline TrajectoryConstIter TrajectoryBase::begin() const
{
- return static_cast<TrajectoryIter>(frame_map_.end());
+ return frame_const_map_.begin();
}
-inline TrajectoryRevIter TrajectoryBase::rbegin() const
+
+inline TrajectoryIter TrajectoryBase::begin()
{
- return static_cast<TrajectoryRevIter>(frame_map_.rbegin());
+ return frame_map_.begin();
}
-inline TrajectoryRevIter TrajectoryBase::rend() const
+
+inline TrajectoryConstIter TrajectoryBase::end() const
+{
+ return frame_const_map_.end();
+}
+
+inline TrajectoryIter TrajectoryBase::end()
+{
+ return frame_map_.end();
+}
+
+inline TrajectoryConstRevIter TrajectoryBase::rbegin() const
+{
+ return frame_const_map_.rbegin();
+}
+
+inline TrajectoryRevIter TrajectoryBase::rbegin()
+{
+ return frame_map_.rbegin();
+}
+
+inline TrajectoryConstRevIter TrajectoryBase::rend() const
{
- return static_cast<TrajectoryRevIter>(frame_map_.rend());
+ return frame_const_map_.rend();
}
-inline FrameBasePtr TrajectoryBase::getLastFrame() const
+inline TrajectoryRevIter TrajectoryBase::rend()
{
- // return last_key_frame_ptr_;
- auto last = this->rbegin();
- if(last != this->rend()) return *(this->rbegin());
- else return nullptr;
+ return frame_map_.rend();
}
} // namespace wolf
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 8e91f7e75caedf348de594a725f0d0ee2c707d3d..656a43398b02f116546bcc01ea716b7b1c48fab2 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -126,28 +126,41 @@ bool CaptureBase::process()
return getSensor()->process(shared_from_this());
}
-
-
FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
{
//std::cout << "Adding feature" << std::endl;
feature_list_.push_back(_ft_ptr);
+ feature_const_list_.push_back(_ft_ptr);
return _ft_ptr;
}
void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr)
{
feature_list_.remove(_ft_ptr);
+ feature_const_list_.remove(_ft_ptr);
}
-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);
@@ -156,31 +169,48 @@ void CaptureBase::getFactorList(FactorBasePtrList& _fac_list) const
FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
+ constrained_by_const_list_.push_back(_fac_ptr);
return _fac_ptr;
}
void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.remove(_fac_ptr);
+ constrained_by_const_list_.remove(_fac_ptr);
}
-bool CaptureBase::isConstrainedBy(const FactorBasePtr &_factor) const
+bool CaptureBase::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_const_list_.begin(),
+ constrained_by_const_list_.end(),
+ _factor) != constrained_by_const_list_.end();
+ // FactorBaseConstPtrConstIter cby_it = std::find_if(constrained_by_const_list_.begin(),
+ // constrained_by_const_list_.end(),
+ // [_factor](const FactorBaseConstPtr & cby)
+ // {
+ // return cby == _factor;
+ // }
+ // );
+ // if (cby_it != constrained_by_const_list_.end())
+ // return true;
+ // else
+ // return false;
}
+StateBlockConstPtr CaptureBase::getStateBlock(const char& _key) const
+{
+ 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 +333,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;
@@ -376,49 +406,57 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::os
}
}
}
+ inconsistency_explanation << "constrained_by_list and constrained_by_const_list not equal\n";
+ log.assertTrue(constrained_by_const_list_.size() == constrained_by_list_.size(), inconsistency_explanation);
- 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);
+ }
+ inconsistency_explanation << "feature_list and feature_const_list not equal\n";
+ log.assertTrue(feature_list_.size() == feature_const_list_.size(), inconsistency_explanation);
+
+ //Check that the capture is within time tolerance of some processor
+ auto frame = getFrame();
+ 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 const& 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..da38aa7366ed25252a014cb80cef7ca9bd83b820 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -118,10 +118,10 @@ 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 (const 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);
}
@@ -149,15 +149,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(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
for (const auto& key1 : wolf_problem_->getFrameStructure())
{
- 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())
{
- 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);
diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp
index bf7fb3d0256dde347713302ab1daee5353655bbc..cfd768ec8ad6d4a9e3039b82c1bb0c8f58d7ac4c 100644
--- a/src/factor/factor_analytic.cpp
+++ b/src/factor/factor_analytic.cpp
@@ -37,7 +37,9 @@ FactorAnalytic::FactorAnalytic(const std::string& _tp,
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_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 +54,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 +82,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..a1a8331a94f07128619f6215587459d2640c8d5a 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -47,16 +47,32 @@ FactorBase::FactorBase(const std::string& _tp,
capture_other_list_(),
feature_other_list_(),
landmark_other_list_(),
+ frame_other_const_list_(),
+ capture_other_const_list_(),
+ feature_other_const_list_(),
+ landmark_other_const_list_(),
processor_ptr_(_processor_ptr)
{
if (_frame_other_ptr)
+ {
frame_other_list_.push_back(_frame_other_ptr);
+ frame_other_const_list_.push_back(_frame_other_ptr);
+ }
if (_capture_other_ptr)
+ {
capture_other_list_.push_back(_capture_other_ptr);
+ capture_other_const_list_.push_back(_capture_other_ptr);
+ }
if (_feature_other_ptr)
+ {
feature_other_list_.push_back(_feature_other_ptr);
+ feature_other_const_list_.push_back(_feature_other_ptr);
+ }
if (_landmark_other_ptr)
+ {
landmark_other_list_.push_back(_landmark_other_ptr);
+ landmark_other_const_list_.push_back(_landmark_other_ptr);
+ }
assert(_feature_ptr && "null feature pointer when creating a factor");
measurement_ = _feature_ptr->getMeasurement();
@@ -83,16 +99,32 @@ FactorBase::FactorBase(const std::string& _tp,
capture_other_list_(),
feature_other_list_(),
landmark_other_list_(),
+ frame_other_const_list_(),
+ capture_other_const_list_(),
+ feature_other_const_list_(),
+ landmark_other_const_list_(),
processor_ptr_(_processor_ptr)
{
for (auto& Fo : _frame_other_list)
+ {
frame_other_list_.push_back(Fo);
+ frame_other_const_list_.push_back(Fo);
+ }
for (auto& Co : _capture_other_list)
+ {
capture_other_list_.push_back(Co);
+ capture_other_const_list_.push_back(Co);
+ }
for (auto& fo : _feature_other_list)
+ {
feature_other_list_.push_back(fo);
+ feature_other_const_list_.push_back(fo);
+ }
for (auto& Lo : landmark_other_list_)
+ {
landmark_other_list_.push_back(Lo);
+ landmark_other_const_list_.push_back(Lo);
+ }
assert(_feature_ptr && "null feature pointer when creating a factor");
measurement_ = _feature_ptr->getMeasurement();
@@ -174,21 +206,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;
+}
+
+FrameBasePtr FactorBase::getFrame()
{
auto cpt = getCapture();
if(cpt != nullptr) return cpt->getFrame();
else return nullptr;
}
-SensorBasePtr FactorBase::getSensor() const
+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,61 +262,57 @@ 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;
- }
- );
- if (frm_it != frame_other_list_.end())
+ auto frm_it = find_if(frame_other_const_list_.begin(),
+ frame_other_const_list_.end(),
+ [_frm_other](const FrameBaseConstWPtr &frm_ow)
+ {
+ return frm_ow.lock() == _frm_other;
+ });
+ if (frm_it != frame_other_const_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;
- }
- );
- if (cap_it != capture_other_list_.end())
+ auto cap_it = find_if(capture_other_const_list_.begin(),
+ capture_other_const_list_.end(),
+ [_cap_other](const CaptureBaseConstWPtr &cap_ow)
+ {
+ return cap_ow.lock() == _cap_other;
+ });
+ if (cap_it != capture_other_const_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;
- }
- );
- if (ftr_it != feature_other_list_.end())
+ auto ftr_it = find_if(feature_other_const_list_.begin(),
+ feature_other_const_list_.end(),
+ [_ftr_other](const FeatureBaseConstWPtr &ftr_ow)
+ {
+ return ftr_ow.lock() == _ftr_other;
+ });
+ if (ftr_it != feature_other_const_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;
- }
- );
- if (lmk_it != landmark_other_list_.end())
+ auto lmk_it = find_if(landmark_other_const_list_.begin(),
+ landmark_other_const_list_.end(),
+ [_lmk_other](const LandmarkBaseConstWPtr &lmk_ow)
+ {
+ return lmk_ow.lock() == _lmk_other;
+ });
+ if (lmk_it != landmark_other_const_list_.end())
return true;
return false;
@@ -355,7 +404,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;
@@ -468,6 +517,17 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
_stream << ")";
if (_verbose)
_stream << std::endl;
+
+ // const pointer and pointer lists consistency
+ inconsistency_explanation << "The frame_other_const_list_ and frame_other_list_ are different\n";
+ log.assertTrue(frame_other_const_list_.size() == frame_other_list_.size(), inconsistency_explanation);
+ inconsistency_explanation << "The capture_other_const_list_ and capture_other_list_ are different\n";
+ log.assertTrue(capture_other_const_list_.size() == capture_other_list_.size(), inconsistency_explanation);
+ inconsistency_explanation << "The feature_other_const_list_ and feature_other_list_ are different\n";
+ log.assertTrue(feature_other_const_list_.size() == feature_other_list_.size(), inconsistency_explanation);
+ inconsistency_explanation << "The landmark_other_const_list_ and landmark_other_list_ are different\n";
+ log.assertTrue(landmark_other_const_list_.size() == landmark_other_list_.size(), inconsistency_explanation);
+
//Check Problem and feature ptrs
if (_verbose)
{
@@ -475,6 +535,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 +546,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())
{
@@ -626,9 +687,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 11db6b079bd8371a48a660547997f7c68d3198bf..f81cff7025750f7c0beedb0b73a78227cf8e5c65 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -94,15 +94,22 @@ void FeatureBase::remove(bool viral_remove_empty_parent)
FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr)
{
factor_list_.push_back(_co_ptr);
+ factor_const_list_.push_back(_co_ptr);
return _co_ptr;
}
void FeatureBase::removeFactor(FactorBasePtr _co_ptr)
{
factor_list_.remove(_co_ptr);
+ factor_const_list_.remove(_co_ptr);
}
-FrameBasePtr FeatureBase::getFrame() const
+FrameBaseConstPtr FeatureBase::getFrame() const
+{
+ return capture_ptr_.lock()->getFrame();
+}
+
+FrameBasePtr FeatureBase::getFrame()
{
return capture_ptr_.lock()->getFrame();
}
@@ -110,35 +117,39 @@ FrameBasePtr FeatureBase::getFrame() const
FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
+ constrained_by_const_list_.push_back(_fac_ptr);
return _fac_ptr;
}
void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.remove(_fac_ptr);
+ constrained_by_const_list_.remove(_fac_ptr);
}
-bool FeatureBase::isConstrainedBy(const 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_const_list_.begin(),
+ constrained_by_const_list_.end(),
+ _factor) != constrained_by_const_list_.end();
+}
+
+const FactorBaseConstPtrList& FeatureBase::getFactorList() const
+{
+ return factor_const_list_;
}
-const FactorBasePtrList& FeatureBase::getFactorList() const
+const FactorBasePtrList& FeatureBase::getFactorList()
{
return factor_list_;
}
-void FeatureBase::getFactorList(FactorBasePtrList & _fac_list) const
+void FeatureBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
+{
+ _fac_list.insert(_fac_list.end(), factor_const_list_.begin(), factor_const_list_.end());
+}
+
+void FeatureBase::getFactorList(FactorBasePtrList & _fac_list)
{
_fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
}
@@ -222,7 +233,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;
@@ -257,17 +268,20 @@ 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);
-
}
+ inconsistency_explanation << "constrained_by_list and constrained_by_const_list not equal\n";
+ log.assertTrue(constrained_by_const_list_.size() == constrained_by_list_.size(), inconsistency_explanation);
+ // Check capture
auto cap_ftr = _ftr_ptr->getCapture();
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 +289,15 @@ CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::os
<< " -X-> Ftr" << id();
log.assertTrue((fac->getFeature() == _ftr_ptr), inconsistency_explanation);
}
+ inconsistency_explanation << "factor_list and factor_const_list not equal\n";
+ log.assertTrue(factor_list_.size() == factor_const_list_.size(), inconsistency_explanation);
+
+
return log;
}
-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..8275e5a087d61dcecf2bb23c33f86120eaea29a7 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -156,7 +156,7 @@ bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const
return getProblem()->getFrameCovariance(shared_from_this(), _cov);
}
-FrameBasePtr FrameBase::getPreviousFrame() const
+FrameBaseConstPtr FrameBase::getPreviousFrame() const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
@@ -170,7 +170,35 @@ FrameBasePtr FrameBase::getPreviousFrame() const
return std::prev(current_frame_it)->second;
}
-FrameBasePtr FrameBase::getNextFrame() const
+FrameBasePtr FrameBase::getPreviousFrame()
+{
+ 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!");
+
+ if (current_frame_it == getTrajectory()->getFrameMap().begin())
+ return nullptr;
+
+ return std::prev(current_frame_it)->second;
+}
+
+FrameBaseConstPtr FrameBase::getNextFrame() 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!");
+
+ if (std::next(current_frame_it) == getTrajectory()->getFrameMap().end())
+ return nullptr;
+
+ return std::next(current_frame_it)->second;
+}
+
+FrameBasePtr FrameBase::getNextFrame()
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
@@ -187,92 +215,176 @@ FrameBasePtr FrameBase::getNextFrame() const
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);
+ capture_const_list_.push_back(_capt_ptr);
return _capt_ptr;
}
void FrameBase::removeCapture(CaptureBasePtr _capt_ptr)
{
capture_list_.remove(_capt_ptr);
+ capture_const_list_.remove(_capt_ptr);
+}
+
+CaptureBaseConstPtr FrameBase::getCaptureOfType(const std::string& type) const
+{
+ for (auto capture_ptr : getCaptureList())
+ if (capture_ptr->getType() == type)
+ return capture_ptr;
+ return nullptr;
}
-CaptureBasePtr FrameBase::getCaptureOfType(const std::string& type) const
+CaptureBasePtr FrameBase::getCaptureOfType(const std::string& type)
{
- 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
+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 (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
+CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type)
+{
+ for (auto capture_ptr : getCaptureList())
+ if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
+ return capture_ptr;
+ return nullptr;
+}
+
+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);
@@ -281,27 +393,21 @@ void FrameBase::getFactorList(FactorBasePtrList& _fac_list) const
FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
+ constrained_by_const_list_.push_back(_fac_ptr);
return _fac_ptr;
}
void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.remove(_fac_ptr);
+ constrained_by_const_list_.remove(_fac_ptr);
}
-bool FrameBase::isConstrainedBy(const 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_const_list_.begin(),
+ constrained_by_const_list_.end(),
+ _factor) != constrained_by_const_list_.end();
}
void FrameBase::link(TrajectoryBasePtr _trj_ptr)
@@ -360,7 +466,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 +477,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;
@@ -412,8 +519,7 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
// 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())
{
@@ -429,14 +535,23 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
}
}
}
+ inconsistency_explanation << "constrained_by_list and constrained_by_const_list not equal\n";
+ log.assertTrue(constrained_by_const_list_.size() == constrained_by_list_.size(), inconsistency_explanation);
+ // Trajectory
auto trj_ptr = getTrajectory();
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;});
+ auto trj_has_frm = std::find_if(trj_ptr->begin(),
+ trj_ptr->end(),
+ [&_frm_ptr](TrajectoryIter frm_it)
+ {
+ return frm_it->second == _frm_ptr;
+ });
log.assertTrue(trj_has_frm != trj_ptr->end(), inconsistency_explanation);
+ // Captures
for(auto C : getCaptureList())
{
inconsistency_explanation << "Frm " << id() << " @ " << _frm_ptr
@@ -444,12 +559,16 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
<< " -X-> Frm" << id();
log.assertTrue((C->getFrame() == _frm_ptr), inconsistency_explanation);
}
+ inconsistency_explanation << "capture_list and capture_const_list not equal\n";
+ log.assertTrue(capture_list_.size() == capture_const_list_.size(), inconsistency_explanation);
+
+
return log;
}
-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 + " ");
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index 9db6b0c373e8f49838ea0d354a0694bffba0da04..3f9de63e64a4f7d17f967d0ca0e6ba5a1ce09d7c 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -38,6 +38,7 @@ HardwareBase::~HardwareBase()
SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
{
sensor_list_.push_back(_sensor_ptr);
+ sensor_const_list_.push_back(_sensor_ptr);
return _sensor_ptr;
}
@@ -55,7 +56,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;
@@ -64,15 +65,19 @@ CheckLog HardwareBase::localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::
_stream << _tabs << "Hrw @ " << _hwd_ptr.get() << std::endl;
}
+ inconsistency_explanation << "sensor_list and sensor_const_list not equal\n";
+ log.assertTrue(sensor_list_.size() == sensor_const_list_.size(), inconsistency_explanation);
+
// check pointer to Problem
inconsistency_explanation << "Hwd->getProblem() [" << getProblem().get()
<< "] -> " << " Prb->getHardware() [" << getProblem()->getHardware().get() << "] -> Hwd [" << _hwd_ptr.get() << "] Mismatch!\n";
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..fd28b61869c75850d956ca6d69012b5c5194ef51 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -84,7 +84,27 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
}
}
-std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec() const
+std::vector<StateBlockConstPtr> LandmarkBase::getUsedStateBlockVec() const
+{
+ std::vector<StateBlockConstPtr> used_state_block_vec(0);
+
+ // normal state blocks in {P,O,V,W}
+ for (const auto& key : getStructure())
+ {
+ 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);
@@ -156,27 +176,21 @@ void LandmarkBase::setProblem(ProblemPtr _problem)
FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
+ constrained_by_const_list_.push_back(_fac_ptr);
return _fac_ptr;
}
void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.remove(_fac_ptr);
+ constrained_by_const_list_.remove(_fac_ptr);
}
-bool LandmarkBase::isConstrainedBy(const 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_const_list_.begin(),
+ constrained_by_const_list_.end(),
+ _factor) != constrained_by_const_list_.end();
}
void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
@@ -224,7 +238,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;
@@ -256,6 +270,7 @@ 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)
@@ -273,7 +288,6 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::
_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);
@@ -290,7 +304,10 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::
}
}
}
+ inconsistency_explanation << "constrained_by_list and constrained_by_const_list not equal\n";
+ log.assertTrue(constrained_by_list_.size() == constrained_by_const_list_.size(), inconsistency_explanation);
+ // check map
inconsistency_explanation << "Lmk" << id() << " @ " << _lmk_ptr
<< " ---> Map" << map_ptr
<< " -X-> Lmk" << id();
@@ -300,9 +317,9 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::
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..13df91306bfec1d277668cbc0197ee93a79ef02c 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -57,12 +57,14 @@ MapBase::~MapBase()
LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
{
landmark_list_.push_back(_landmark_ptr);
+ landmark_const_list_.push_back(_landmark_ptr);
return _landmark_ptr;
}
void MapBase::removeLandmark(LandmarkBasePtr _landmark_ptr)
{
landmark_list_.remove(_landmark_ptr);
+ landmark_const_list_.remove(_landmark_ptr);
}
void MapBase::load(const std::string& _map_file_dot_yaml)
@@ -121,6 +123,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 +133,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;
@@ -138,21 +141,26 @@ CheckLog MapBase::localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _
if (_verbose)
_stream << _tabs << "Map @ " << _map_ptr.get() << std::endl;
+ inconsistency_explanation << "landmark_list and landmark_const_list not equal\n";
+ log.assertTrue(landmark_list_.size() == landmark_const_list_.size(), inconsistency_explanation);
+
// check pointer to Problem
inconsistency_explanation << "Map->getProblem() [" << getProblem().get()
<< "] -> " << " Prb->getMap() [" << getProblem()->getMap().get() << "] -> Map [" << _map_ptr.get() << "] Mismatch!\n";
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 1e5c88a2db6cf68a2032c8be1b50dbbe3c2206d9..ec734700de21982cfe907a947b94566ef1413747 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -335,7 +335,22 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
return prc_ptr;
}
-SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
+SensorBaseConstPtr Problem::getSensor(const std::string& _sensor_name) const
+{
+ auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
+ getHardware()->getSensorList().end(),
+ [&](SensorBaseConstPtr sb)
+ {
+ return sb->getName() == _sensor_name;
+ }); // lambda function for the find_if
+
+ if (sen_it == getHardware()->getSensorList().end())
+ return nullptr;
+
+ return (*sen_it);
+}
+
+SensorBasePtr Problem::getSensor(const std::string& _sensor_name)
{
auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
getHardware()->getSensorList().end(),
@@ -351,9 +366,9 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
}
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;
@@ -642,7 +657,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)
@@ -666,6 +680,7 @@ void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr)
// add to map ordered by priority
motion_provider_map_.emplace(_motion_provider_ptr->getStatePriority(), _motion_provider_ptr);
+ motion_provider_const_map_.emplace(_motion_provider_ptr->getStatePriority(), _motion_provider_ptr);
appendToStructure(_motion_provider_ptr->getStateStructure());
}
@@ -674,6 +689,7 @@ void Problem::removeMotionProvider(MotionProviderPtr proc)
WOLF_WARN_COND(motion_provider_map_.count(proc->getStatePriority()) == 0, "Problem::clearMotionProvider: missing processor");
motion_provider_map_.erase(proc->getStatePriority());
+ motion_provider_const_map_.erase(proc->getStatePriority());
// rebuild frame structure with remaining motion processors
frame_structure_.clear();
@@ -852,7 +868,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;
@@ -870,12 +889,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.");
@@ -885,18 +904,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())
@@ -926,7 +945,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);
}
@@ -936,7 +955,7 @@ 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
@@ -957,7 +976,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);
}
@@ -966,7 +985,7 @@ 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
@@ -985,7 +1004,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_;
}
@@ -997,22 +1021,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);
}
@@ -1230,10 +1274,10 @@ void Problem::perturb(double amplitude)
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);
}
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index adeac3db58b63afc2907d12892325fcd8feefc8d..02d8beefd9d719a65218577bd594ed3bcb4f97bd 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;
@@ -192,9 +192,9 @@ CheckLog ProcessorBase::localCheck(bool _verbose, ProcessorBasePtr _prc_ptr, std
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_motion.cpp b/src/processor/processor_motion.cpp
index 805f9bb4cac6847906a3aa9ed2e5f577dbdc63c4..2f222a50bec0aaf82c55f3053a292e912ce8d1a7 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -898,14 +898,14 @@ 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;
+ FrameBaseConstPtr frame = nullptr;
+ CaptureBaseConstPtr capture = nullptr;
+ CaptureMotionConstPtr capture_motion = nullptr;
for (auto frame_rev_iter = getProblem()->getTrajectory()->rbegin();
frame_rev_iter != getProblem()->getTrajectory()->rend();
++frame_rev_iter)
{
- frame = *frame_rev_iter;
+ frame = frame_rev_iter->second;
auto sensor = getSensor();
capture = frame->getCaptureOf(sensor);
if (capture != nullptr)
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 0e649ec5cce5c11ca522c21bab70c67855922839..915dc1d190eb90bf0bcc6c70e31d551732d8f5e9 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -203,7 +203,7 @@ void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x,
params_prior_map_[_key] = ftr_prior;
}
-void SensorBase::registerNewStateBlocks() const
+void SensorBase::registerNewStateBlocks()
{
if (getProblem() != nullptr)
{
@@ -262,7 +262,33 @@ 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_rev_it = trajectory->rbegin();
+ while (frame_rev_it != trajectory->rend())
+ {
+ if ((*frame_rev_it)->getTimeStamp() <= _ts)
+ {
+ auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ if (capture)
+ // found the most recent Capture made by this sensor !
+ return capture;
+ }
+ frame_rev_it++;
+ }
+
+ return nullptr;
+}
+
+CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts)
{
// we search for the most recent Capture of this sensor before _ts
if (not getProblem())
@@ -288,32 +314,62 @@ CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts) const
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');
}
@@ -348,15 +404,27 @@ bool SensorBase::process(const CaptureBasePtr capture_ptr)
ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
{
processor_list_.push_back(_proc_ptr);
+ processor_const_list_.push_back(_proc_ptr);
return _proc_ptr;
}
void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr)
{
processor_list_.remove(_proc_ptr);
+ processor_const_list_.remove(_proc_ptr);
+}
+
+StateBlockConstPtr SensorBase::getStateBlockDynamic(const char& _key) const
+{
+ CaptureBaseConstPtr cap;
+
+ if (isStateBlockInCapture(_key, cap))
+ return cap->getStateBlock(_key);
+
+ return getStateBlock(_key);
}
-StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key) const
+StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key)
{
CaptureBasePtr cap;
@@ -366,7 +434,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 +454,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 +466,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 +504,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);
}
@@ -490,7 +592,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 +616,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";
@@ -526,6 +629,7 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr
auto hwd_has_sen = std::find_if(hwd_sen_list.begin(), hwd_sen_list.end(), [&_sen_ptr](SensorBasePtr sen){ return sen == _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 +637,9 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr
<< " -X-> Sen" << id();
log.assertTrue((prc->getSensor() == _sen_ptr), inconsistency_explanation);
}
+ inconsistency_explanation << "processor_list and processor_const_list not equal\n";
+ log.assertTrue(processor_list_.size() == processor_const_list_.size(), inconsistency_explanation);
+
// check last_capture_
if (getProblem()->getTimeStamp().ok())
{
@@ -550,9 +657,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/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 4bf0282fd90354856dc43b896a12a97ad88b1d7c..98f600fc1dae521ee0aac6f2e2745c759ee6a31b 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -133,7 +133,7 @@ CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, s
}
bool TrajectoryBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _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)