LinearizedFactor.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 <vector>
#include <gtsam_unstable/dllexport.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>

namespace gtsam {

class GTSAM_UNSTABLE_EXPORT LinearizedGaussianFactor : public NonlinearFactor {
public:
  typedef NonlinearFactor Base;
  typedef LinearizedGaussianFactor This;

  typedef boost::shared_ptr<LinearizedGaussianFactor> shared_ptr;

protected:

  Values lin_points_;

public:

  LinearizedGaussianFactor() {};

  LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Values& lin_points);

  ~LinearizedGaussianFactor() override {};

  // access functions
  const Values& linearizationPoint() const { return lin_points_; }

private:
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & boost::serialization::make_nvp("LinearizedGaussianFactor",
        boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(lin_points_);
  }

};

class GTSAM_UNSTABLE_EXPORT LinearizedJacobianFactor : public LinearizedGaussianFactor {

public:
  typedef LinearizedGaussianFactor Base;
  typedef LinearizedJacobianFactor This;

  typedef boost::shared_ptr<LinearizedJacobianFactor> shared_ptr;

  typedef VerticalBlockMatrix::Block ABlock;
  typedef VerticalBlockMatrix::constBlock constABlock;
  typedef VerticalBlockMatrix::Block::ColXpr BVector;
  typedef VerticalBlockMatrix::constBlock::ConstColXpr constBVector;

protected:

//  // store components of a jacobian factor
//  typedef std::map<Key, Matrix> KeyMatrixMap;
//  KeyMatrixMap matrices_;
//  Vector b_;

  VerticalBlockMatrix Ab_;      // the block view of the full matrix

public:

  LinearizedJacobianFactor();

  LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian, const Values& lin_points);

  ~LinearizedJacobianFactor() override {}

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this))); }

  // Testable

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

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

  // access functions
  const constBVector b() const { return Ab_(size()).col(0); }
  const constABlock A() const { return Ab_.range(0, size()); };
  const constABlock A(Key key) const { return Ab_(std::find(begin(), end(), key) - begin()); }

  size_t dim() const override { return Ab_.rows(); };

  double error(const Values& c) const override;

  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override;

  Vector error_vector(const Values& c) const;

private:
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & boost::serialization::make_nvp("LinearizedJacobianFactor",
        boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(Ab_);
  }
};

template<>
struct traits<LinearizedJacobianFactor> : public Testable<LinearizedJacobianFactor> {
};

class GTSAM_UNSTABLE_EXPORT LinearizedHessianFactor : public LinearizedGaussianFactor {

public:
  typedef LinearizedGaussianFactor Base;
  typedef LinearizedHessianFactor This;

  typedef boost::shared_ptr<LinearizedHessianFactor> shared_ptr;

  typedef SymmetricBlockMatrix::Block Block; 
  typedef SymmetricBlockMatrix::constBlock constBlock; 

  typedef SymmetricBlockMatrix::Block::ColXpr Column; 
  typedef SymmetricBlockMatrix::constBlock::ColXpr constColumn; 

protected:

  SymmetricBlockMatrix info_; 

public:

  LinearizedHessianFactor();

  LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian, const Values& lin_points);

  ~LinearizedHessianFactor() override {}

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this))); }

  // Testable

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

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

  double constantTerm() const {
    const auto block = info_.diagonalBlock(size());
    return block(0, 0);
  }

  constColumn linearTerm(const_iterator j) const {
    return info_.aboveDiagonalRange(j - begin(), size(), size(), size() + 1).col(0);
  }

  constColumn linearTerm() const {
    return info_.aboveDiagonalRange(0, size(), size(), size() + 1).col(0);
  }

  Matrix squaredTerm(const_iterator j1, const_iterator j2) const {
    const DenseIndex J1 = j1 - begin();
    const DenseIndex J2 = j2 - begin();
    return info_.block(J1, J2);
  }

  Eigen::SelfAdjointView<constBlock, Eigen::Upper> squaredTerm() const {
    return info_.selfadjointView(0, size());
  }

  size_t dim() const override { return info_.rows() - 1; }

  double error(const Values& c) const override;

  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override;

private:
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & boost::serialization::make_nvp("LinearizedHessianFactor",
        boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(info_);
  }
};

template<>
struct traits<LinearizedHessianFactor> : public Testable<LinearizedHessianFactor> {
};

} // \namespace aspn