CustomFactor.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>
 
using namespace gtsam;
 
namespace gtsam {
 
using JacobianVector = std::vector<Matrix>;
 
class CustomFactor;
 
/*
 * NOTE
 * ==========
 * pybind11 will invoke a copy if this is `JacobianVector &`, and modifications in Python will not be reflected.
 *
 * This is safe because this is passing a const pointer, and pybind11 will maintain the `std::vector` memory layout.
 * Thus the pointer will never be invalidated.
 */
using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, const JacobianVector *)>;
 
class CustomFactor: public NoiseModelFactor {
protected:
  CustomErrorFunction error_function_;
 
protected:
 
  using Base = NoiseModelFactor;
  using This = CustomFactor;
 
public:
 
  CustomFactor() = default;
 
  CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction) :
      Base(noiseModel, keys) {
    this->error_function_ = errorFunction;
  }
 
  ~CustomFactor() override = default;
 
  Vector unwhitenedError(const Values &x, boost::optional<std::vector<Matrix> &> H = boost::none) const override;
 
  void print(const std::string &s,
             const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
 
  bool sendable() const override {
    return false;
  }
 
private:
 
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
    ar & boost::serialization::make_nvp("CustomFactor",
                                        boost::serialization::base_object<Base>(*this));
  }
};
 
}