91 lines
1.9 KiB
C
91 lines
1.9 KiB
C
|
/*
|
||
|
|
* EqualityFactor.h
|
||
|
|
*
|
||
|
|
* Created on: Aug 10, 2009
|
||
|
|
* Author: alexgc
|
||
|
|
*/
|
||
|
|
|
||
|
|
#ifndef EQUALITYFACTOR_H_
|
||
|
|
#define EQUALITYFACTOR_H_
|
||
|
|
|
||
|
|
#include "Factor.h"
|
||
|
|
#include "DeltaFunction.h"
|
||
|
|
|
||
|
|
namespace gtsam {
|
||
|
|
|
||
|
|
class EqualityFactor: public Factor {
|
||
|
|
public:
|
||
|
|
typedef boost::shared_ptr<EqualityFactor> shared_ptr;
|
||
|
|
|
||
|
|
|
||
|
|
protected:
|
||
|
|
Vector value; /// forces a variable be equal to this value
|
||
|
|
std::string key; /// name of variable factor is attached to
|
||
|
|
|
||
|
|
public:
|
||
|
|
/**
|
||
|
|
* Default constructor
|
||
|
|
*/
|
||
|
|
EqualityFactor();
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Constructor with initializiation
|
||
|
|
* @param constraint the value that the variable node is defined as equal to
|
||
|
|
* @param key identifier for the variable node
|
||
|
|
*/
|
||
|
|
EqualityFactor(const Vector& constraint, const std::string& key);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Default Destructor
|
||
|
|
*/
|
||
|
|
~EqualityFactor() {}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* negative log probability
|
||
|
|
*/
|
||
|
|
double error(const FGConfig& c) const;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* print
|
||
|
|
* @param s optional string naming the factor
|
||
|
|
*/
|
||
|
|
void print(const std::string& s="") const;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* equality up to tolerance
|
||
|
|
*/
|
||
|
|
bool equals(const Factor& f, double tol=1e-9) const;
|
||
|
|
bool equals(const EqualityFactor& f, double tol=1e-9) const;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* linearize
|
||
|
|
*/
|
||
|
|
EqualityFactor::shared_ptr linearize() const;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* returns a version of the factor as a string
|
||
|
|
*/
|
||
|
|
std::string dump() const;
|
||
|
|
|
||
|
|
// get functions
|
||
|
|
std::string get_key() const {return key;}
|
||
|
|
Vector get_value() const {return value;}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @return the number of nodes the factor connects
|
||
|
|
*/
|
||
|
|
std::size_t size() const {return 1;}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Returns the corresponding delta function for elimination
|
||
|
|
*/
|
||
|
|
DeltaFunction::shared_ptr getDeltaFunction() const;
|
||
|
|
};
|
||
|
|
|
||
|
|
/** assert equals for testing - prints when not equal */
|
||
|
|
bool assert_equal(const EqualityFactor& actual, const EqualityFactor& expected, double tol=1e-9);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif /* EQUALITYFACTOR_H_ */
|