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
#include <ostream>
#include "NoiseModel.h"
#include "NonlinearFactor.h"
#include "Pose2.h"
@ -22,7 +23,7 @@ namespace gtsam {
typedef NonlinearFactor1<Config, Key, T> Base;
T prior_; /** The measurement */
Matrix square_root_inverse_covariance_; /** sqrt(inv(covariance)) */
boost::shared_ptr<GaussianNoiseModel> noiseModel_;
public:
@ -30,9 +31,14 @@ namespace gtsam {
typedef typename boost::shared_ptr<PriorFactor> shared_ptr;
/** Constructor */
PriorFactor(const Key& key, const T& prior, const Matrix& covariance) :
Base(1.0, key), prior_(prior) {
square_root_inverse_covariance_ = inverse_square_root(covariance);
PriorFactor(const Key& key, const T& prior,
const boost::shared_ptr<GaussianNoiseModel>& model) :
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 */
@ -41,8 +47,7 @@ namespace gtsam {
void print(const std::string& s) const {
Base::print(s);
prior_.print("prior");
gtsam::print(square_root_inverse_covariance_,
"Square Root Inverse Covariance");
// Todo print NoiseModel
}
/** equals */
@ -51,15 +56,16 @@ namespace gtsam {
Config, Key, T>*> (&expected);
return e != NULL && Base::equals(expected) && this->prior_.equals(
e->prior_, tol);
// Todo check NoiseModel
}
/** implement functions needed to derive from Factor */
/** vector of errors */
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))
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
static double sx=0.5, sy=0.5,st=0.1;
static Matrix covariance = Matrix_(3,3,
sx*sx, 0.0, 0.0,
0.0, sy*sy, 0.0,
0.0, 0.0, st*st
);
static boost::shared_ptr<GaussianNoiseModel> model(new Sigmas(Vector_(3,sx,sy,st)));
/* ************************************************************************* */
// Very simple test establishing Ax-b \approx z-h(x)
@ -29,7 +25,7 @@ TEST( Pose2Prior, error )
x0.insert(1, p1);
// Create factor
Pose2Prior factor(1, p1, covariance);
Pose2Prior factor(1, p1, model);
// Actual linearization
boost::shared_ptr<GaussianFactor> linear = factor.linearize(x0);
@ -52,7 +48,7 @@ TEST( Pose2Prior, error )
/* ************************************************************************* */
// common Pose2Prior for tests below
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