Merge pull request #774 from borglab/fix/nonlinearequality

gtsam/nonlinear/NonlinearEquality.h

@@ -59,10 +59,10 @@ private:
   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:
 
@@ -73,7 +73,7 @@ public:
   CompareFunction compare_;
 //  bool (*compare_)(const T& a, const T& b);
 
-  /** default constructor - only for serialization */
+  /// Default constructor - only for serialization
   NonlinearEquality() {
   }
 
@@ -109,9 +109,11 @@ public:
 
   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 */
@@ -125,7 +127,7 @@ public:
   /// @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);
@@ -136,7 +138,7 @@ public:
     }
   }
 
-  /** 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_);
@@ -157,7 +159,7 @@ public:
     }
   }
 
-  // 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;
@@ -179,7 +181,7 @@ public:
 
 private:
 
-  /** Serialization function */
+  /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
@@ -212,7 +214,7 @@ protected:
   typedef NoiseModelFactor1<VALUE> Base;
   typedef NonlinearEquality1<VALUE> This;
 
-  /** default constructor to allow for serialization */
+  /// Default constructor to allow for serialization
   NonlinearEquality1() {
   }
 
@@ -231,12 +233,12 @@ public:
    * @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 {
   }
@@ -247,7 +249,7 @@ public:
         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)
@@ -256,7 +258,7 @@ public:
     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())
@@ -269,7 +271,7 @@ public:
 
 private:
 
-  /** Serialization function */
+  /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
@@ -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>
@@ -301,7 +303,7 @@ protected:
 
   GTSAM_CONCEPT_MANIFOLD_TYPE(X)
 
-  /** default constructor to allow for serialization */
+  /// Default constructor to allow for serialization
   NonlinearEquality2() {
   }
 
@@ -309,7 +311,12 @@ 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) {
   }
@@ -322,7 +329,7 @@ public:
         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;
@@ -335,7 +342,7 @@ public:
 
 private:
 
-  /** Serialization function */
+  /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {