12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

record continuous keys separately

release/4.3a0
Varun Agrawal 2022-06-03 13:07:46 -04:00
parent 9e737dbbd8
commit 89768cf692
2 changed files with 14 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/hybrid/HybridFactor.cpp
View File

@ -50,7 +50,7 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
/* ************************************************************************ */ /* ************************************************************************ */
HybridFactor::HybridFactor(const KeyVector &keys) HybridFactor::HybridFactor(const KeyVector &keys)
: Base(keys), isContinuous_(true), nrContinuous_(keys.size()) {} : Base(keys), isContinuous_(true), continuousKeys_(keys) {}
/* ************************************************************************ */ /* ************************************************************************ */
HybridFactor::HybridFactor(const KeyVector &continuousKeys, HybridFactor::HybridFactor(const KeyVector &continuousKeys,
@ -59,8 +59,8 @@ HybridFactor::HybridFactor(const KeyVector &continuousKeys,
isDiscrete_((continuousKeys.size() == 0) && (discreteKeys.size() != 0)), isDiscrete_((continuousKeys.size() == 0) && (discreteKeys.size() != 0)),
isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)), isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)),
isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)), isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)),
nrContinuous_(continuousKeys.size()), discreteKeys_(discreteKeys),
discreteKeys_(discreteKeys) {} continuousKeys_(continuousKeys) {}
/* ************************************************************************ */ /* ************************************************************************ */
HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
@ -73,7 +73,8 @@ bool HybridFactor::equals(const HybridFactor &lf, double tol) const {
const This *e = dynamic_cast<const This *>(&lf); const This *e = dynamic_cast<const This *>(&lf);
return e != nullptr && Base::equals(*e, tol) && return e != nullptr && Base::equals(*e, tol) &&
isDiscrete_ == e->isDiscrete_ && isContinuous_ == e->isContinuous_ && isDiscrete_ == e->isDiscrete_ && isContinuous_ == e->isContinuous_ &&
isHybrid_ == e->isHybrid_ && nrContinuous_ == e->nrContinuous_; isHybrid_ == e->isHybrid_ && continuousKeys_ == e->continuousKeys_ &&
discreteKeys_ == e->discreteKeys_;
} }
/* ************************************************************************ */ /* ************************************************************************ */

12
gtsam/hybrid/HybridFactor.h
View File

@ -50,7 +50,10 @@ class GTSAM_EXPORT HybridFactor : public Factor {
size_t nrContinuous_ = 0; size_t nrContinuous_ = 0;
protected: protected:
// Set of DiscreteKeys for this factor.
DiscreteKeys discreteKeys_; DiscreteKeys discreteKeys_;
// For bookkeeping
KeyVector continuousKeys_;
public: public:
// typedefs needed to play nice with gtsam // typedefs needed to play nice with gtsam
@ -117,10 +120,13 @@ class GTSAM_EXPORT HybridFactor : public Factor {
bool isHybrid() const { return isHybrid_; } bool isHybrid() const { return isHybrid_; }
/// Return the number of continuous variables in this factor. /// Return the number of continuous variables in this factor.
size_t nrContinuous() const { return nrContinuous_; } size_t nrContinuous() const { return continuousKeys_.size(); }
/// Return vector of discrete keys. /// Return the discrete keys for this factor.
DiscreteKeys discreteKeys() const { return discreteKeys_; } const DiscreteKeys &discreteKeys() const { return discreteKeys_; }
/// Return only the continuous keys for this factor.
const KeyVector &continuousKeys() const { return continuousKeys_; }
/// @} /// @}
}; };