LinearEquality.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/linear/JacobianFactor.h>

namespace gtsam {

class LinearEquality: public JacobianFactor {
public:
  typedef LinearEquality This; 
  typedef JacobianFactor Base; 
  typedef std::shared_ptr<This> shared_ptr; 

private:
  Key dualKey_;

public:
  LinearEquality() :
      Base() {
  }

  explicit LinearEquality(const JacobianFactor& jf, Key dualKey) :
      Base(jf), dualKey_(dualKey) {
    if (!jf.isConstrained()) {
      throw std::runtime_error(
          "Cannot convert an unconstrained JacobianFactor to LinearEquality");
    }
  }

  explicit LinearEquality(const HessianFactor& hf) {
    throw std::runtime_error("Cannot convert HessianFactor to LinearEquality");
  }

  LinearEquality(Key i1, const Matrix& A1, const Vector& b, Key dualKey) :
      Base(i1, A1, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) {
  }

  LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2,
      const Vector& b, Key dualKey) :
      Base(i1, A1, i2, A2, b, noiseModel::Constrained::All(b.rows())), dualKey_(
          dualKey) {
  }

  LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3,
      const Matrix& A3, const Vector& b, Key dualKey) :
      Base(i1, A1, i2, A2, i3, A3, b, noiseModel::Constrained::All(b.rows())), dualKey_(
          dualKey) {
  }

  template<typename TERMS>
  LinearEquality(const TERMS& terms, const Vector& b, Key dualKey) :
      Base(terms, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) {
  }

  ~LinearEquality() override {
  }

  bool equals(const GaussianFactor& lf, double tol = 1e-9) const override {
    return Base::equals(lf, tol);
  }

  void print(const std::string& s = "", const KeyFormatter& formatter =
      DefaultKeyFormatter) const override {
    Base::print(s, formatter);
  }

  GaussianFactor::shared_ptr clone() const override {
    return std::static_pointer_cast < GaussianFactor
        > (std::make_shared < LinearEquality > (*this));
  }

  Key dualKey() const {
    return dualKey_;
  }

  bool active() const {
    return true;
  }

  Vector error_vector(const VectorValues& c) const {
    return unweighted_error(c);
  }

  double error(const VectorValues& c) const override {
    return 0.0;
  }

};
// \ LinearEquality

template<> struct traits<LinearEquality> : public Testable<LinearEquality> {
};

} // \ namespace gtsam