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

Ensured that constraints add keys to keys_

release/4.3a0
Alex Cunningham 2010-02-09 01:52:36 +00:00
parent 8aa440a679
commit 0c74e0c309
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cpp/NonlinearConstraint-inl.h
View File

@ -26,7 +26,9 @@ NonlinearConstraint<Config>::NonlinearConstraint(const LagrangeKey& lagrange_key
size_t dim_lagrange, size_t dim_lagrange,
bool isEquality) : bool isEquality) :
NonlinearFactor<Config>(noiseModel::Constrained::All(dim_lagrange)), NonlinearFactor<Config>(noiseModel::Constrained::All(dim_lagrange)),
lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality) {} lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality) {
this->keys_.push_back(lagrange_key_);
}
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config> template <class Config>
@ -49,6 +51,7 @@ NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
g_(boost::bind(g, _1)), G_(boost::bind(gradG, _1)), key_(key) g_(boost::bind(g, _1)), G_(boost::bind(gradG, _1)), key_(key)
{ {
this->keys_.push_back(key);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -63,6 +66,7 @@ NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
g_(g), G_(gradG), key_(key) g_(g), G_(gradG), key_(key)
{ {
this->keys_.push_back(key);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -143,6 +147,8 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
g_(boost::bind(g, _1)), G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)), g_(boost::bind(g, _1)), G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)),
key1_(key1), key2_(key2) key1_(key1), key2_(key2)
{ {
this->keys_.push_back(key1);
this->keys_.push_back(key2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -160,6 +166,8 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
g_(g), G1_(G1), G2_(G2), g_(g), G1_(G1), G2_(G2),
key1_(key1), key2_(key2) key1_(key1), key2_(key2)
{ {
this->keys_.push_back(key1);
this->keys_.push_back(key2);
} }
/* ************************************************************************* */ /* ************************************************************************* */