12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Noisemodel works in PriorFactor

release/4.3a0
Frank Dellaert 2010-01-17 03:56:42 +00:00
parent 8967027198
commit a3fa194ca1
2 changed files with 17 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cpp/PriorFactor.h
View File

@ -5,6 +5,7 @@
#pragma once #pragma once
#include <ostream> #include <ostream>
#include "NoiseModel.h"
#include "NonlinearFactor.h" #include "NonlinearFactor.h"
#include "Pose2.h" #include "Pose2.h"
@ -22,7 +23,7 @@ namespace gtsam {
typedef NonlinearFactor1<Config, Key, T> Base; typedef NonlinearFactor1<Config, Key, T> Base;
T prior_; /** The measurement */ T prior_; /** The measurement */
Matrix square_root_inverse_covariance_; /** sqrt(inv(covariance)) */ boost::shared_ptr<GaussianNoiseModel> noiseModel_;
public: public:
@ -30,9 +31,14 @@ namespace gtsam {
typedef typename boost::shared_ptr<PriorFactor> shared_ptr; typedef typename boost::shared_ptr<PriorFactor> shared_ptr;
/** Constructor */ /** Constructor */
PriorFactor(const Key& key, const T& prior, const Matrix& covariance) : PriorFactor(const Key& key, const T& prior,
Base(1.0, key), prior_(prior) { const boost::shared_ptr<GaussianNoiseModel>& model) :
square_root_inverse_covariance_ = inverse_square_root(covariance); Base(1.0, key), prior_(prior), noiseModel_(model) {
}
/** Constructor */
PriorFactor(const Key& key, const T& prior, const Matrix& cov) :
Base(1.0, key), prior_(prior), noiseModel_(new FullCovariance(cov)) {
} }
/** implement functions needed for Testable */ /** implement functions needed for Testable */
@ -41,8 +47,7 @@ namespace gtsam {
void print(const std::string& s) const { void print(const std::string& s) const {
Base::print(s); Base::print(s);
prior_.print("prior"); prior_.print("prior");
gtsam::print(square_root_inverse_covariance_, // Todo print NoiseModel
"Square Root Inverse Covariance");
} }
/** equals */ /** equals */
@ -51,15 +56,16 @@ namespace gtsam {
Config, Key, T>*> (&expected); Config, Key, T>*> (&expected);
return e != NULL && Base::equals(expected) && this->prior_.equals( return e != NULL && Base::equals(expected) && this->prior_.equals(
e->prior_, tol); e->prior_, tol);
// Todo check NoiseModel
} }
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** vector of errors */ /** vector of errors */
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
if (H) (*H) = square_root_inverse_covariance_; if (H) (*H) = noiseModel_->R();
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
return square_root_inverse_covariance_ * logmap(prior_, p); return noiseModel_->whiten(logmap(prior_, p));
} }
}; };

10
cpp/testPose2Prior.cpp
View File

@ -13,11 +13,7 @@ using namespace gtsam;
// Common measurement covariance // Common measurement covariance
static double sx=0.5, sy=0.5,st=0.1; static double sx=0.5, sy=0.5,st=0.1;
static Matrix covariance = Matrix_(3,3, static boost::shared_ptr<GaussianNoiseModel> model(new Sigmas(Vector_(3,sx,sy,st)));
sx*sx, 0.0, 0.0,
0.0, sy*sy, 0.0,
0.0, 0.0, st*st
);
/* ************************************************************************* */ /* ************************************************************************* */
// Very simple test establishing Ax-b \approx z-h(x) // Very simple test establishing Ax-b \approx z-h(x)
@ -29,7 +25,7 @@ TEST( Pose2Prior, error )
x0.insert(1, p1); x0.insert(1, p1);
// Create factor // Create factor
Pose2Prior factor(1, p1, covariance); Pose2Prior factor(1, p1, model);
// Actual linearization // Actual linearization
boost::shared_ptr<GaussianFactor> linear = factor.linearize(x0); boost::shared_ptr<GaussianFactor> linear = factor.linearize(x0);
@ -52,7 +48,7 @@ TEST( Pose2Prior, error )
/* ************************************************************************* */ /* ************************************************************************* */
// common Pose2Prior for tests below // common Pose2Prior for tests below
static Pose2 prior(2,2,M_PI_2); static Pose2 prior(2,2,M_PI_2);
static Pose2Prior factor(1,prior, covariance); static Pose2Prior factor(1,prior, model);
/* ************************************************************************* */ /* ************************************************************************* */
// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector