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

LinearInequality now only supports single-valued function. Add active/activate/inactivate function to use in the qp active set method.

release/4.3a0
thduynguyen 2014-12-12 12:01:16 -05:00
parent ba9e73785a
commit 4f28eace7e
1 changed files with 38 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
gtsam_unstable/linear/LinearInequality.h
View File

@ -34,6 +34,7 @@ public:
private: private:
Key dualKey_; Key dualKey_;
bool active_;
public: public:
/** default constructor for I/O */ /** default constructor for I/O */
@ -48,53 +49,55 @@ public:
} }
/** Construct unary factor */ /** Construct unary factor */
LinearInequality(Key i1, const Matrix& A1, const Vector& b, Key dualKey) : LinearInequality(Key i1, const Matrix& A1, double b, Key dualKey) :
Base(i1, A1, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { Base(i1, A1, (Vector(1) << b), noiseModel::Constrained::All(1)), dualKey_(
dualKey) {
} }
/** Construct binary factor */ /** Construct binary factor */
LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, double b,
const Vector& b, Key dualKey) : Key dualKey) :
Base(i1, A1, i2, A2, b, noiseModel::Constrained::All(b.rows())), dualKey_( Base(i1, A1, i2, A2, (Vector(1) << b), noiseModel::Constrained::All(1)), dualKey_(
dualKey) { dualKey) {
} }
/** Construct ternary factor */ /** Construct ternary factor */
LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3,
const Matrix& A3, const Vector& b, Key dualKey) : const Matrix& A3, double b, Key dualKey) :
Base(i1, A1, i2, A2, i3, A3, b, noiseModel::Constrained::All(b.rows())), dualKey_( Base(i1, A1, i2, A2, i3, A3, (Vector(1) << b),
dualKey) { noiseModel::Constrained::All(1)), dualKey_(dualKey) {
} }
/** Construct four-ary factor */ /** Construct four-ary factor */
LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3,
const Matrix& A3, Key i4, const Matrix& A4, const Vector& b, Key dualKey) : const Matrix& A3, Key i4, const Matrix& A4, double b, Key dualKey) :
Base(i1, A1, i2, A2, i3, A3, i4, A4, b, Base(i1, A1, i2, A2, i3, A3, i4, A4, (Vector(1) << b),
noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { noiseModel::Constrained::All(1)), dualKey_(dualKey) {
} }
/** Construct five-ary factor */ /** Construct five-ary factor */
LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3,
const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5, const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5,
const Vector& b, Key dualKey) : double b, Key dualKey) :
Base(i1, A1, i2, A2, i3, A3, i4, A4, i5, A5, b, Base(i1, A1, i2, A2, i3, A3, i4, A4, i5, A5, (Vector(1) << b),
noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { noiseModel::Constrained::All(1)), dualKey_(dualKey) {
} }
/** Construct six-ary factor */ /** Construct six-ary factor */
LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3,
const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5, const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5,
Key i6, const Matrix& A6, const Vector& b, Key dualKey) : Key i6, const Matrix& A6, double b, Key dualKey) :
Base(i1, A1, i2, A2, i3, A3, i4, A4, i5, A5, i6, A6, b, Base(i1, A1, i2, A2, i3, A3, i4, A4, i5, A5, i6, A6, (Vector(1) << b),
noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { noiseModel::Constrained::All(1)), dualKey_(dualKey) {
} }
/** Construct an n-ary factor /** Construct an n-ary factor
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the * @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */ * collection of keys and matrices making up the factor. */
template<typename TERMS> template<typename TERMS>
LinearInequality(const TERMS& terms, const Vector& b, Key dualKey) : LinearInequality(const TERMS& terms, double b, Key dualKey) :
Base(terms, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { Base(terms, (Vector(1) << b), noiseModel::Constrained::All(1)), dualKey_(
dualKey) {
} }
/** Virtual destructor */ /** Virtual destructor */
@ -121,28 +124,34 @@ public:
/// dual key /// dual key
Key dualKey() const { return dualKey_; } Key dualKey() const { return dualKey_; }
/// return true if this constraint is active
bool active() const { return active_; }
/// Make this inequality constraint active
void activate() { active_ = true; }
/// Make this inequality constraint inactive
void inactivate() { active_ = false; }
/** Special error_vector for constraints (A*x-b) */ /** Special error_vector for constraints (A*x-b) */
Vector error_vector(const VectorValues& c) const { Vector error_vector(const VectorValues& c) const {
return unweighted_error(c); return unweighted_error(c);
} }
/** Special error for constraints. /** Special error for single-valued inequality constraints. */
* I think it should be zero, as this function is meant for objective cost.
* But the name "error" can be misleading.
* TODO: confirm with Frank!! */
virtual double error(const VectorValues& c) const { virtual double error(const VectorValues& c) const {
return 0.0; return error_vector(c)[0];
} }
/** dot product of row s with the corresponding vector in p */ /** dot product of row s with the corresponding vector in p */
double dotProductRow(size_t s, const VectorValues& p) const { double dotProductRow(const VectorValues& p) const {
double ajTp = 0.0; double aTp = 0.0;
for (const_iterator xj = begin(); xj != end(); ++xj) { for (const_iterator xj = begin(); xj != end(); ++xj) {
Vector pj = p.at(*xj); Vector pj = p.at(*xj);
Vector aj = getA(xj).row(s); Vector aj = getA(xj).transpose();
ajTp += aj.dot(pj); aTp += aj.dot(pj);
} }
return ajTp; return aTp;
} }
}; };