120 lines
4.0 KiB
C++
120 lines
4.0 KiB
C++
|
/**
|
||
|
|
* @file NonlinearFactor.cpp
|
||
|
|
* @brief nonlinear factor versions which assume a Gaussian noise on a measurement
|
||
|
|
* @brief predicted by a non-linear function h nonlinearFactor
|
||
|
|
* @author Kai Ni
|
||
|
|
* @author Carlos Nieto
|
||
|
|
* @author Christian Potthast
|
||
|
|
* @author Frank Dellaert
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "NonlinearFactor.h"
|
||
|
|
|
||
|
|
using namespace std;
|
||
|
|
using namespace gtsam;
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
NonlinearFactor::NonlinearFactor(const Vector& z,
|
||
|
|
const double sigma)
|
||
|
|
{
|
||
|
|
z_ = z;
|
||
|
|
sigma_ = sigma;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
NonlinearFactor1::NonlinearFactor1(const Vector& z,
|
||
|
|
const double sigma,
|
||
|
|
Vector (*h)(const Vector&),
|
||
|
|
const string& key1,
|
||
|
|
Matrix (*H)(const Vector&))
|
||
|
|
: NonlinearFactor(z, sigma), h_(h), key1_(key1), H_(H)
|
||
|
|
{
|
||
|
|
keys_.push_front(key1);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
void NonlinearFactor1::print(const string& s) const {
|
||
|
|
cout << "NonLinearFactor1 " << s << endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
LinearFactor::shared_ptr NonlinearFactor1::linearize(const FGConfig& c) const
|
||
|
|
{
|
||
|
|
// get argument 1 from config
|
||
|
|
Vector x1 = c[key1_];
|
||
|
|
|
||
|
|
// Jacobian A = H(x1)/sigma
|
||
|
|
Matrix A = H_(x1)/sigma_;
|
||
|
|
|
||
|
|
// Right-hand-side b = error(c) = (z - h(x1))/sigma
|
||
|
|
Vector b = (z_ - h_(x1))/sigma_;
|
||
|
|
|
||
|
|
LinearFactor::shared_ptr p(new LinearFactor(key1_,A,b));
|
||
|
|
return p;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
/** http://artis.imag.fr/~Xavier.Decoret/resources/C++/operator==.html */
|
||
|
|
/* ************************************************************************* */
|
||
|
|
bool NonlinearFactor1::equals(const Factor& f, double tol) const {
|
||
|
|
const NonlinearFactor1* p = dynamic_cast<const NonlinearFactor1*>(&f);
|
||
|
|
if (p == NULL) return false;
|
||
|
|
return NonlinearFactor::equals(*p,tol)
|
||
|
|
&& (h_ == p->h_)
|
||
|
|
&& (key1_ == p->key1_)
|
||
|
|
&& (H_ == p->H_);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
NonlinearFactor2::NonlinearFactor2(const Vector& z,
|
||
|
|
const double sigma,
|
||
|
|
Vector (*h)(const Vector&, const Vector&),
|
||
|
|
const string& key1,
|
||
|
|
Matrix (*H1)(const Vector&, const Vector&),
|
||
|
|
const string& key2,
|
||
|
|
Matrix (*H2)(const Vector&, const Vector&)
|
||
|
|
)
|
||
|
|
: NonlinearFactor(z, sigma), h_(h), key1_(key1), H1_(H1), key2_(key2), H2_(H2)
|
||
|
|
{
|
||
|
|
keys_.push_front(key1);
|
||
|
|
keys_.push_front(key2);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
void NonlinearFactor2::print(const string& s) const {
|
||
|
|
cout << "NonLinearFactor2 " << s << endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
LinearFactor::shared_ptr NonlinearFactor2::linearize(const FGConfig& c) const
|
||
|
|
{
|
||
|
|
// get arguments from config
|
||
|
|
Vector x1 = c[key1_];
|
||
|
|
Vector x2 = c[key2_];
|
||
|
|
|
||
|
|
// Jacobian A = H(x)/sigma
|
||
|
|
Matrix A1 = H1_(x1,x2)/sigma_;
|
||
|
|
Matrix A2 = H2_(x1,x2)/sigma_;
|
||
|
|
|
||
|
|
// Right-hand-side b = (z - h(x))/sigma
|
||
|
|
Vector b = (z_ - h_(x1,x2))/sigma_;
|
||
|
|
|
||
|
|
LinearFactor::shared_ptr p(new LinearFactor(key1_,A1,key2_,A2,b));
|
||
|
|
return p;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
bool NonlinearFactor2::equals(const Factor& f, double tol) const {
|
||
|
|
const NonlinearFactor2* p = dynamic_cast<const NonlinearFactor2*>(&f);
|
||
|
|
if (p == NULL) return false;
|
||
|
|
return NonlinearFactor::equals(*p,tol)
|
||
|
|
&& (h_ == p->h_)
|
||
|
|
&& (key1_ == p->key1_)
|
||
|
|
&& (H2_ == p->H1_)
|
||
|
|
&& (key2_ == p->key2_)
|
||
|
|
&& (H1_ == p->H2_);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|