81 lines
2.1 KiB
C++
81 lines
2.1 KiB
C++
/*
|
|
* @file LinearApproxFactor.h
|
|
* @brief A dummy factor that allows a linear factor to act as a nonlinear factor
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <vector>
|
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
|
#include <gtsam/linear/VectorValues.h>
|
|
#include <gtsam/base/Matrix.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* A dummy factor that takes a linearized factor and inserts it into
|
|
* a nonlinear graph. This version uses exactly one type of variable.
|
|
*/
|
|
template <class Values, class Key>
|
|
class LinearApproxFactor : public NonlinearFactor<Values> {
|
|
|
|
public:
|
|
/** type for the variable */
|
|
typedef typename Key::Value X;
|
|
|
|
/** base type */
|
|
typedef NonlinearFactor<Values> Base;
|
|
|
|
/** shared pointer for convenience */
|
|
typedef boost::shared_ptr<LinearApproxFactor<Values,Key> > shared_ptr;
|
|
|
|
/** typedefs for key vectors */
|
|
typedef std::vector<Key> KeyVector;
|
|
|
|
protected:
|
|
/** hold onto the factor itself */
|
|
GaussianFactor::shared_ptr lin_factor_;
|
|
|
|
/** linearization points for error calculation */
|
|
Values lin_points_;
|
|
|
|
/** keep keys for the factor */
|
|
KeyVector nonlinearKeys_;
|
|
|
|
/**
|
|
* use this for derived classes with keys that don't copy easily
|
|
*/
|
|
LinearApproxFactor(size_t dim, const Values& lin_points)
|
|
: Base(noiseModel::Unit::Create(dim)), lin_points_(lin_points) {}
|
|
|
|
public:
|
|
|
|
/** use this constructor when starting with nonlinear keys */
|
|
LinearApproxFactor(GaussianFactor::shared_ptr lin_factor, const Values& lin_points);
|
|
|
|
virtual ~LinearApproxFactor() {}
|
|
|
|
/** Vector of errors, unwhitened ! */
|
|
virtual Vector unwhitenedError(const Values& c) const;
|
|
|
|
/**
|
|
* linearize to a GaussianFactor
|
|
* Just returns a copy of the existing factor
|
|
* NOTE: copies to avoid actual factor getting destroyed
|
|
* during elimination
|
|
*/
|
|
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& c) const;
|
|
|
|
/** get access to nonlinear keys */
|
|
KeyVector nonlinearKeys() const { return nonlinearKeys_; }
|
|
|
|
/** override print function */
|
|
virtual void print(const std::string& s="") const;
|
|
|
|
/** access to b vector of gaussian */
|
|
Vector get_b() const { return lin_factor_->getb(); }
|
|
};
|
|
|
|
} // \namespace gtsam
|