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

Progress on error

release/4.3a0
dellaert 2014-09-21 17:13:25 +02:00
parent d9fafc1bf1
commit 59b0e6a657
1 changed files with 21 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam_unstable/base/tests/testBAD.cpp
View File

@ -56,7 +56,7 @@ class LeafExpression {
public: public:
/// Constructor with a single key /// Constructor with a single key
LeafExpression(Key key) { LeafExpression(Key key):key_(key) {
} }
T value(const Values& values) const { T value(const Values& values) const {
@ -95,6 +95,12 @@ class BADFactor: NonlinearFactor {
const T measurement_; const T measurement_;
const E expression_; const E expression_;
/// get value from expression and calculate error with respect to measurement
Vector unwhitenedError(const Values& values) const {
const T& value = expression_.value(values);
return measurement_.localCoordinates(value);
}
public: public:
/// Constructor /// Constructor
@ -103,11 +109,18 @@ public:
} }
/** /**
* Calculate the error of the factor * Calculate the error of the factor.
* This is typically equal to log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/sigma^2 \f$ * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/ */
double error(const Values& c) const { virtual double error(const Values& values) const {
return 0; if (this->active(values)) {
const Vector e = unwhitenedError(values);
return 0.5 * e.norm();
} else {
return 0.0;
}
} }
/// get the dimension of the factor (number of rows on linearization) /// get the dimension of the factor (number of rows on linearization)
@ -121,8 +134,7 @@ public:
// value type is std::pair<Key, Matrix>, specifying the // value type is std::pair<Key, Matrix>, specifying the
// collection of keys and matrices making up the factor. // collection of keys and matrices making up the factor.
std::map<Key, Matrix> terms; std::map<Key, Matrix> terms;
const T& value = expression_.value(values); Vector b = unwhitenedError(values);
Vector b = measurement_.localCoordinates(value);
SharedDiagonal model = SharedDiagonal(); SharedDiagonal model = SharedDiagonal();
return boost::shared_ptr<JacobianFactor>( return boost::shared_ptr<JacobianFactor>(
new JacobianFactor(terms, b, model)); new JacobianFactor(terms, b, model));
@ -148,6 +160,7 @@ TEST(BAD, test) {
Point2 measured(0, 1); Point2 measured(0, 1);
SharedNoiseModel model = noiseModel::Unit::Create(2); SharedNoiseModel model = noiseModel::Unit::Create(2);
GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3); GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3);
double expected_error = old.error(values);
GaussianFactor::shared_ptr expected = old.linearize(values); GaussianFactor::shared_ptr expected = old.linearize(values);
// Create leaves // Create leaves
@ -164,7 +177,7 @@ TEST(BAD, test) {
BADFactor<Point2, LeafExpression<Point2> > f(measured, uv_hat); BADFactor<Point2, LeafExpression<Point2> > f(measured, uv_hat);
// Check value // Check value
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
// Check dimension // Check dimension
EXPECT_LONGS_EQUAL(0, f.dim()); EXPECT_LONGS_EQUAL(0, f.dim());