Skip to content

Commit

Permalink
Merge pull request #774 from borglab/fix/nonlinearequality
Browse files Browse the repository at this point in the history
  • Loading branch information
@varunagrawal
varunagrawal authored Jun 10, 2021
2 parents b582b5b + a55e474 commit f77af12
Showing 1 changed file with 30 additions and 23 deletions.
53 changes: 30 additions & 23 deletions gtsam/nonlinear/NonlinearEquality.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,10 +59,10 @@ class NonlinearEquality: public NoiseModelFactor1<VALUE> {
double error_gain_;

// typedef to this class
typedef NonlinearEquality<VALUE> This;
using This = NonlinearEquality<VALUE>;

// typedef to base class
typedef NoiseModelFactor1<VALUE> Base;
using Base = NoiseModelFactor1<VALUE>;

public:

Expand All @@ -73,7 +73,7 @@ class NonlinearEquality: public NoiseModelFactor1<VALUE> {
CompareFunction compare_;
// bool (*compare_)(const T& a, const T& b);

/** default constructor - only for serialization */
/// Default constructor - only for serialization
NonlinearEquality() {
}

Expand Down Expand Up @@ -109,9 +109,11 @@ class NonlinearEquality: public NoiseModelFactor1<VALUE> {

void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n";
std::cout << (s.empty() ? s : s + " ") << "Constraint: on ["
<< keyFormatter(this->key()) << "]\n";
traits<VALUE>::Print(feasible_, "Feasible Point:\n");
std::cout << "Variable Dimension: " << traits<T>::GetDimension(feasible_) << std::endl;
std::cout << "Variable Dimension: " << traits<T>::GetDimension(feasible_)
<< std::endl;
}

/** Check if two factors are equal */
Expand All @@ -125,7 +127,7 @@ class NonlinearEquality: public NoiseModelFactor1<VALUE> {
/// @name Standard Interface
/// @{

/** actual error function calculation */
/// Actual error function calculation
double error(const Values& c) const override {
const T& xj = c.at<T>(this->key());
Vector e = this->unwhitenedError(c);
Expand All @@ -136,7 +138,7 @@ class NonlinearEquality: public NoiseModelFactor1<VALUE> {
}
}

/** error function */
/// Error function
Vector evaluateError(const T& xj,
boost::optional<Matrix&> H = boost::none) const override {
const size_t nj = traits<T>::GetDimension(feasible_);
Expand All @@ -157,7 +159,7 @@ class NonlinearEquality: public NoiseModelFactor1<VALUE> {
}
}

// Linearize is over-written, because base linearization tries to whiten
/// Linearize is over-written, because base linearization tries to whiten
GaussianFactor::shared_ptr linearize(const Values& x) const override {
const T& xj = x.at<T>(this->key());
Matrix A;
Expand All @@ -179,7 +181,7 @@ class NonlinearEquality: public NoiseModelFactor1<VALUE> {

private:

/** Serialization function */
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
Expand Down Expand Up @@ -212,7 +214,7 @@ class NonlinearEquality1: public NoiseModelFactor1<VALUE> {
typedef NoiseModelFactor1<VALUE> Base;
typedef NonlinearEquality1<VALUE> This;

/** default constructor to allow for serialization */
/// Default constructor to allow for serialization
NonlinearEquality1() {
}

Expand All @@ -231,12 +233,12 @@ class NonlinearEquality1: public NoiseModelFactor1<VALUE> {
* @param value feasible value that values(key) shouild be equal to
* @param key the key for the unknown variable to be constrained
* @param mu a parameter which really turns this into a strong prior
*
*/
NonlinearEquality1(const X& value, Key key, double mu = 1000.0) :
Base( noiseModel::Constrained::All(traits<X>::GetDimension(value),
std::abs(mu)), key), value_(value) {
}
NonlinearEquality1(const X& value, Key key, double mu = 1000.0)
: Base(noiseModel::Constrained::All(traits<X>::GetDimension(value),
std::abs(mu)),
key),
value_(value) {}

~NonlinearEquality1() override {
}
Expand All @@ -247,7 +249,7 @@ class NonlinearEquality1: public NoiseModelFactor1<VALUE> {
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

/** g(x) with optional derivative */
/// g(x) with optional derivative
Vector evaluateError(const X& x1,
boost::optional<Matrix&> H = boost::none) const override {
if (H)
Expand All @@ -256,7 +258,7 @@ class NonlinearEquality1: public NoiseModelFactor1<VALUE> {
return traits<X>::Local(value_,x1);
}

/** Print */
/// Print
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
Expand All @@ -269,7 +271,7 @@ class NonlinearEquality1: public NoiseModelFactor1<VALUE> {

private:

/** Serialization function */
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
Expand All @@ -287,7 +289,7 @@ struct traits<NonlinearEquality1<VALUE> > : Testable<NonlinearEquality1<VALUE> >

/* ************************************************************************* */
/**
* Simple binary equality constraint - this constraint forces two factors to
* Simple binary equality constraint - this constraint forces two variables to
* be the same.
*/
template<class VALUE>
Expand All @@ -301,15 +303,20 @@ class NonlinearEquality2: public NoiseModelFactor2<VALUE, VALUE> {

GTSAM_CONCEPT_MANIFOLD_TYPE(X)

/** default constructor to allow for serialization */
/// Default constructor to allow for serialization
NonlinearEquality2() {
}

public:

typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr;

///TODO: comment
/**
* Constructor
* @param key1 the key for the first unknown variable to be constrained
* @param key2 the key for the second unknown variable to be constrained
* @param mu a parameter which really turns this into a strong prior
*/
NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) :
Base(noiseModel::Constrained::All(traits<X>::dimension, std::abs(mu)), key1, key2) {
}
Expand All @@ -322,7 +329,7 @@ class NonlinearEquality2: public NoiseModelFactor2<VALUE, VALUE> {
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

/** g(x) with optional derivative2 */
/// g(x) with optional derivative2
Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
static const size_t p = traits<X>::dimension;
Expand All @@ -335,7 +342,7 @@ class NonlinearEquality2: public NoiseModelFactor2<VALUE, VALUE> {

private:

/** Serialization function */
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
Expand Down

0 comments on commit f77af12

Please sign in to comment.