EqualityFactorGraph.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/inference/FactorGraph.h>
#include <gtsam_unstable/linear/LinearEquality.h>

namespace gtsam {

class EqualityFactorGraph: public FactorGraph<LinearEquality> {
public:
  typedef std::shared_ptr<EqualityFactorGraph> shared_ptr;

  template <class... Args> void add(Args &&... args) {
    emplace_shared<LinearEquality>(std::forward<Args>(args)...);
  }

  double error(const VectorValues& x) const {
    double total_error = 0.;
    for (const sharedFactor& factor : *this) {
      if (factor)
        total_error += factor->error(x);
    }
    return total_error;
  }
};

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

} // \ namespace gtsam