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

Split off BADFactor code from Expression

release/4.3a0
dellaert 2014-09-30 12:29:57 +02:00
parent 5a1ea6071b
commit ef52e12f87
3 changed files with 11 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam_unstable/base/Expression-inl.h
View File

@ -17,7 +17,7 @@
* @brief Internals for Expression.h, not for general consumption
*/
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/Values.h>
#include <boost/foreach.hpp>
namespace gtsam {

63
gtsam_unstable/base/Expression.h
View File

@ -18,9 +18,7 @@
*/
#include "Expression-inl.h"
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/Key.h>
#include <boost/make_shared.hpp>
#include <boost/bind.hpp>
namespace gtsam {
@ -94,66 +92,5 @@ Expression<T> operator*(const Expression<T>& expression1,
expression1, expression2);
}
/**
* BAD Factor that supports arbitrary expressions via AD
*/
template<class T>
class BADFactor: NonlinearFactor {
const T measurement_;
const Expression<T> 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 value.localCoordinates(measurement_);
}
public:
/// Constructor
BADFactor(const T& measurement, const Expression<T>& expression) :
measurement_(measurement), expression_(expression) {
}
/// Constructor
BADFactor(const T& measurement, const ExpressionNode<T>& expression) :
measurement_(measurement), expression_(expression) {
}
/**
* Calculate the error of the factor.
* 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.
*/
virtual double error(const Values& values) const {
if (this->active(values)) {
const Vector e = unwhitenedError(values);
return 0.5 * e.squaredNorm();
} else {
return 0.0;
}
}
/// get the dimension of the factor (number of rows on linearization)
size_t dim() const {
return 0;
}
/// linearize to a GaussianFactor
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
// We will construct an n-ary factor below, where terms is a container whose
// value type is std::pair<Key, Matrix>, specifying the
// collection of keys and matrices making up the factor.
std::map<Key, Matrix> terms;
expression_.value(values, terms);
Vector b = unwhitenedError(values);
SharedDiagonal model = SharedDiagonal();
return boost::shared_ptr<JacobianFactor>(
new JacobianFactor(terms, b, model));
}
};
// BADFactor
}

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

@ -17,7 +17,7 @@
* @brief unit tests for Block Automatic Differentiation
*/
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam_unstable/base/Expression.h>
@ -49,19 +49,6 @@ Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix&> Dcal,
TEST(BAD, test) {
// Create some values
Values values;
values.insert(1, Pose3());
values.insert(2, Point3(0, 0, 1));
values.insert(3, Cal3_S2());
// Create old-style factor to create expected value and derivatives
Point2 measured(-17, 30);
SharedNoiseModel model = noiseModel::Unit::Create(2);
GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3);
double expected_error = old.error(values);
GaussianFactor::shared_ptr expected = old.linearize(values);
// Test Constant expression
Expression<int> c(0);
@ -81,19 +68,6 @@ TEST(BAD, test) {
expectedKeys.insert(2);
expectedKeys.insert(3);
EXPECT(expectedKeys == uv_hat.keys());
// Create factor
BADFactor<Point2> f(measured, uv_hat);
// Check value
EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
// Check dimension
EXPECT_LONGS_EQUAL(0, f.dim());
// Check linearization
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
EXPECT( assert_equal(*expected, *gf, 1e-9));
}
/* ************************************************************************* */
@ -104,20 +78,11 @@ TEST(BAD, compose) {
Expression<Rot3> R1(1), R2(2);
Expression<Rot3> R3 = R1 * R2;
// Create factor
BADFactor<Rot3> f(Rot3(), R3);
// Create some values
Values values;
values.insert(1, Rot3());
values.insert(2, Rot3());
// Check linearization
JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
// Check keys
std::set<Key> expectedKeys;
expectedKeys.insert(1);
expectedKeys.insert(2);
EXPECT(expectedKeys == R3.keys());
}
/* ************************************************************************* */
@ -128,19 +93,10 @@ TEST(BAD, compose2) {
Expression<Rot3> R1(1), R2(1);
Expression<Rot3> R3 = R1 * R2;
// Create factor
BADFactor<Rot3> f(Rot3(), R3);
// Create some values
Values values;
values.insert(1, Rot3());
// Check linearization
JacobianFactor expected(1, 2*eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
// Check keys
std::set<Key> expectedKeys;
expectedKeys.insert(1);
EXPECT(expectedKeys == R3.keys());
}
/* ************************************************************************* */