VectorNormFactor.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/nonlinear/NonlinearFactor.h>
 
namespace gtsam {

template <int N>
class VectorNormFactor : public NoiseModelFactorN<Eigen::Matrix<double, N, 1>> {
  static_assert(N > 0, "VectorNormFactor requires a fixed positive dimension");
 
 public:
  typedef Eigen::Matrix<double, N, 1> VectorN;
 
 private:
  typedef VectorNormFactor<N> This;
  typedef NoiseModelFactorN<VectorN> Base;
 
  double norm_;  
 
 public:
  static constexpr double kMinNorm = 1e-10;
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  typedef std::shared_ptr<This> shared_ptr;

  VectorNormFactor() : norm_(0.0) {}

  VectorNormFactor(Key key, double norm, const SharedNoiseModel& model)
      : Base(model, key), norm_(norm) {}
 
  ~VectorNormFactor() override {}

  NonlinearFactor::shared_ptr clone() const override {
    return std::make_shared<This>(*this);
  }

  void print(const std::string& s = "",
             const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    std::cout << (s.empty() ? s : s + " ") << "VectorNormFactor on "
              << keyFormatter(this->template key<1>()) << ", norm = " << norm_
              << std::endl;
    this->noiseModel_->print("  noise model: ");
  }
 
  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override {
    const This* e = dynamic_cast<const This*>(&expected);
    return e != nullptr && Base::equals(*e, tol) &&
           std::abs(norm_ - e->norm_) < tol;
  }


  double norm() const { return norm_; }

  Vector evaluateError(const VectorN& v, OptionalMatrixType H) const override {
    const double vnorm = v.norm();
    if (H) {
      if (vnorm > kMinNorm) {
        *H = v.transpose() / vnorm;
      } else {
        // The norm is not differentiable at the origin. Return a zero
        // Jacobian: the factor exerts no pull on a (near-)zero vector.
        H->setZero(1, N);
      }
    }
    return Vector1(vnorm - norm_);
  }
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    ar& boost::serialization::make_nvp(
        "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
    ar& BOOST_SERIALIZATION_NVP(norm_);
  }
#endif
};

template <int N>
struct traits<VectorNormFactor<N>> : public Testable<VectorNormFactor<N>> {};
 
}  // namespace gtsam