NonlinearFactor.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
 
 * -------------------------------------------------------------------------- */

 
// \callgraph
 
#pragma once
 
#include <gtsam/nonlinear/Values.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/base/utilities.h>
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/base_object.hpp>
#endif
#include <cstddef>
 
namespace gtsam {
 
class NonlinearEqualityConstraints;
class NonlinearFactorGraph;
 
/* ************************************************************************* */

#define OptionalNone static_cast<gtsam::Matrix*>(nullptr)

using OptionalMatrixType = Matrix*;

using OptionalMatrixVecType = std::vector<Matrix>*;

class GTSAM_EXPORT NonlinearFactor: public Factor {
 
protected:
 
  // Some handy typedefs
  typedef Factor Base;
  typedef NonlinearFactor This;
 
public:
 
  typedef std::shared_ptr<This> shared_ptr;


  NonlinearFactor() {}

  template<typename CONTAINER>
  NonlinearFactor(const CONTAINER& keys) :
    Base(keys) {}


  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
                                            DefaultKeyFormatter) const override;

  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;


  virtual double error(const Values& c) const;

  double error(const HybridValues& c) const override;

  virtual size_t dim() const = 0;

  virtual bool active(const Values& c) const { return true; }

  virtual std::shared_ptr<GaussianFactor>
  linearize(const Values& c) const = 0;

  virtual void qcqpFactors(NonlinearFactorGraph* costs,
                           NonlinearEqualityConstraints* constraints,
                           size_t columnDimension = 1) const;

  virtual shared_ptr clone() const {
    // TODO: choose better exception to throw here
    throw std::runtime_error("NonlinearFactor::clone(): Attempting to clone factor with no clone() implemented!");
    return shared_ptr();
  }

  virtual shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;

  virtual shared_ptr rekey(const KeyVector& new_keys) const;

   virtual bool sendable() const {
    return true;
  }
 
}; // \class NonlinearFactor

template<> struct traits<NonlinearFactor> : public Testable<NonlinearFactor> {
};
 
/* ************************************************************************* */
class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor {
 
protected:
 
  // handy typedefs
  typedef NonlinearFactor Base;
  typedef NoiseModelFactor This;
 
  SharedNoiseModel noiseModel_; 
 
public:
 
  typedef std::shared_ptr<This> shared_ptr;

  NoiseModelFactor() {}

  ~NoiseModelFactor() override {}

  template<typename CONTAINER>
  NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) :
    Base(keys), noiseModel_(noiseModel) {}
 
protected:

  NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {}
 
public:
  void print(const std::string& s = "",
    const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;

  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;

  size_t dim() const override {
    if (!noiseModel_)
      throw std::runtime_error("NoiseModelFactor::dim(): no noise model set");
    return noiseModel_->dim();
  }

  const SharedNoiseModel& noiseModel() const {
    return noiseModel_;
  }

  virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const = 0;

  Vector unwhitenedError(const Values& x, std::vector<Matrix>& H) const {
    return unwhitenedError(x, &H);
  }

  Vector whitenedError(const Values& c) const;

  Vector unweightedWhitenedError(const Values& c) const;

  double weight(const Values& c) const;
 
  using NonlinearFactor::error;

  double error(const Values& c) const override;

  std::shared_ptr<GaussianFactor> linearize(const Values& x) const override;

  shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const;
 
 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("NonlinearFactor",
         boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(noiseModel_);
  }
#endif
 
}; // \class NoiseModelFactor
 
}  // namespace gtsam