1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file EqualityFactorGraph.h
14 * @brief Factor graph of all LinearEquality factors
15 * @date Dec 8, 2014
16 * @author Duy-Nguyen Ta
17 */
18
19#pragma once
20
21#include <gtsam/inference/FactorGraph.h>
22#include <gtsam_unstable/linear/LinearEquality.h>
23
24namespace gtsam {
25
26/**
27 * Collection of all Linear Equality constraints Ax=b of
28 * a Programming problem as a Factor Graph
29 */
30class EqualityFactorGraph: public FactorGraph<LinearEquality> {
31public:
32 typedef std::shared_ptr<EqualityFactorGraph> shared_ptr;
33
34 /// Add a linear inequality, forwards arguments to LinearInequality.
35 template <class... Args> void add(Args &&... args) {
36 emplace_shared<LinearEquality>(std::forward<Args>(args)...);
37 }
38
39 /// Compute error of a guess.
40 double error(const VectorValues& x) const {
41 double total_error = 0.;
42 for (const sharedFactor& factor : *this) {
43 if (factor)
44 total_error += factor->error(c: x);
45 }
46 return total_error;
47 }
48};
49
50/// traits
51template<> struct traits<EqualityFactorGraph> : public Testable<
52 EqualityFactorGraph> {
53};
54
55} // \ namespace gtsam
56
57