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

Merge remote-tracking branch 'origin/feature/BAD' into feature/BAD

release/4.3a0
Sungtae An 2014-10-05 16:16:19 -04:00
parent 40565564f5 16d402f8c9
commit 0a7db2d252
4 changed files with 322 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

150
gtsam_unstable/nonlinear/Expression-inl.h
View File

@ -44,6 +44,17 @@ private:
typedef std::pair<Key, Matrix> Pair;
/// Insert terms into jacobians_, premultiplying by H, adding if already exists
void add(const JacobianMap& terms) {
BOOST_FOREACH(const Pair& term, terms) {
JacobianMap::iterator it = jacobians_.find(term.first);
if (it != jacobians_.end())
it->second += term.second;
else
jacobians_[term.first] = term.second;
}
}
/// Insert terms into jacobians_, premultiplying by H, adding if already exists
void add(const Matrix& H, const JacobianMap& terms) {
BOOST_FOREACH(const Pair& term, terms) {
@ -69,6 +80,12 @@ public:
jacobians_[key] = Eigen::MatrixXd::Identity(n, n);
}
/// Construct value dependent on a single key, with Jacobain H
Augmented(const T& t, Key key, const Matrix& H) :
value_(t) {
jacobians_[key] = H;
}
/// Construct value, pre-multiply jacobians by H
Augmented(const T& t, const Matrix& H, const JacobianMap& jacobians) :
value_(t) {
@ -103,6 +120,11 @@ public:
return jacobians_;
}
/// Return jacobians
JacobianMap& jacobians() {
return jacobians_;
}
/// Not dependent on any key
bool constant() const {
return jacobians_.empty();
@ -117,6 +139,17 @@ public:
}
};
//-----------------------------------------------------------------------------
template<class T>
struct JacobianTrace {
T t;
T value() const {
return t;
}
virtual void reverseAD(JacobianMap& jacobians) const = 0;
virtual void reverseAD(const Matrix& H, JacobianMap& jacobians) const = 0;
};
//-----------------------------------------------------------------------------
/**
* Expression node. The superclass for objects that do the heavy lifting
@ -145,8 +178,11 @@ public:
virtual T value(const Values& values) const = 0;
/// Return value and derivatives
virtual Augmented<T> augmented(const Values& values) const = 0;
virtual Augmented<T> forward(const Values& values) const = 0;
/// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution(
const Values& values) const = 0;
};
//-----------------------------------------------------------------------------
@ -182,11 +218,27 @@ public:
}
/// Return value and derivatives
virtual Augmented<T> augmented(const Values& values) const {
T t = value(values);
return Augmented<T>(t);
virtual Augmented<T> forward(const Values& values) const {
return Augmented<T>(constant_);
}
/// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> {
/// If the expression is just a constant, we do nothing
virtual void reverseAD(JacobianMap& jacobians) const {
}
/// Base case: we simply ignore the given df/dT
virtual void reverseAD(const Matrix& H, JacobianMap& jacobians) const {
}
};
/// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution(
const Values& values) const {
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
trace->t = constant_;
return trace;
}
};
//-----------------------------------------------------------------------------
@ -223,11 +275,38 @@ public:
}
/// Return value and derivatives
virtual Augmented<T> augmented(const Values& values) const {
virtual Augmented<T> forward(const Values& values) const {
T t = value(values);
return Augmented<T>(t, key_);
}
/// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> {
Key key;
/// If the expression is just a leaf, we just insert an identity matrix
virtual void reverseAD(JacobianMap& jacobians) const {
size_t n = T::Dim();
jacobians[key] = Eigen::MatrixXd::Identity(n, n);
}
/// Base case: given df/dT, add it jacobians with correct key and we are done
virtual void reverseAD(const Matrix& H, JacobianMap& jacobians) const {
JacobianMap::iterator it = jacobians.find(key);
if (it != jacobians.end())
it->second += H;
else
jacobians[key] = H;
}
};
/// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution(
const Values& values) const {
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
trace->t = value(values);
trace->key = key_;
return trace;
}
};
//-----------------------------------------------------------------------------
@ -268,15 +347,37 @@ public:
}
/// Return value and derivatives
virtual Augmented<T> augmented(const Values& values) const {
virtual Augmented<T> forward(const Values& values) const {
using boost::none;
Augmented<A> argument = this->expressionA_->augmented(values);
Augmented<A> argument = this->expressionA_->forward(values);
Matrix H;
T t = function_(argument.value(),
argument.constant() ? none : boost::optional<Matrix&>(H));
return Augmented<T>(t, H, argument.jacobians());
}
/// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> {
boost::shared_ptr<JacobianTrace<A> > trace1;
Matrix H1;
/// Start the reverse AD process
virtual void reverseAD(JacobianMap& jacobians) const {
trace1->reverseAD(H1, jacobians);
}
/// Given df/dT, multiply in dT/dA and continue reverse AD process
virtual void reverseAD(const Matrix& H, JacobianMap& jacobians) const {
trace1->reverseAD(H * H1, jacobians);
}
};
/// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution(
const Values& values) const {
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
trace->trace1 = this->expressionA_->traceExecution(values);
trace->t = function_(trace->trace1->value(), trace->H1);
return trace;
}
};
//-----------------------------------------------------------------------------
@ -327,10 +428,10 @@ public:
}
/// Return value and derivatives
virtual Augmented<T> augmented(const Values& values) const {
virtual Augmented<T> forward(const Values& values) const {
using boost::none;
Augmented<A1> argument1 = this->expressionA1_->augmented(values);
Augmented<A2> argument2 = this->expressionA2_->augmented(values);
Augmented<A1> argument1 = this->expressionA1_->forward(values);
Augmented<A2> argument2 = this->expressionA2_->forward(values);
Matrix H1, H2;
T t = function_(argument1.value(), argument2.value(),
argument1.constant() ? none : boost::optional<Matrix&>(H1),
@ -338,7 +439,36 @@ public:
return Augmented<T>(t, H1, argument1.jacobians(), H2, argument2.jacobians());
}
/// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> {
boost::shared_ptr<JacobianTrace<A1> > trace1;
boost::shared_ptr<JacobianTrace<A2> > trace2;
Matrix H1, H2;
/// Start the reverse AD process
virtual void reverseAD(JacobianMap& jacobians) const {
trace1->reverseAD(H1, jacobians);
trace2->reverseAD(H2, jacobians);
}
/// Given df/dT, multiply in dT/dA and continue reverse AD process
virtual void reverseAD(const Matrix& H, JacobianMap& jacobians) const {
trace1->reverseAD(H * H1, jacobians);
trace2->reverseAD(H * H2, jacobians);
}
};
/// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution(
const Values& values) const {
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
trace->trace1 = this->expressionA1_->traceExecution(values);
trace->trace2 = this->expressionA2_->traceExecution(values);
trace->t = function_(trace->trace1->value(), trace->trace2->value(),
trace->H1, trace->H2);
return trace;
}
};
//-----------------------------------------------------------------------------
/// Ternary Expression

10
gtsam_unstable/nonlinear/Expression.h
View File

@ -111,7 +111,15 @@ public:
/// Return value and derivatives
Augmented<T> augmented(const Values& values) const {
return root_->augmented(values);
#define REVERSE_AD
#ifdef REVERSE_AD
boost::shared_ptr<JacobianTrace<T> > trace = root_->traceExecution(values);
Augmented<T> augmented(trace->value());
trace->reverseAD(augmented.jacobians());
return augmented;
#else
return root_->forward(values);
#endif
}
const boost::shared_ptr<ExpressionNode<T> >& root() const {

260
gtsam_unstable/nonlinear/tests/testBADFactor.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam_unstable/slam/expressions.h>
#include <gtsam_unstable/nonlinear/BADFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/base/Testable.h>
@ -29,141 +30,190 @@
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
Point2 measured(-17, 30);
SharedNoiseModel model = noiseModel::Unit::Create(2);
/* ************************************************************************* */
// Unary(Leaf))
TEST(BADFactor, 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);
JacobianFactor expected( //
2, (Matrix(2, 3) << 1, 0, 0, 0, 1, 0), //
(Vector(2) << -17, 30));
// Create leaves
Pose3_ x(1);
Point3_ p(2);
Cal3_S2_ K(3);
// Create expression tree
Point3_ p_cam(x, &Pose3::transform_to, p);
Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
// Create factor and check value, dimension, linearization
BADFactor<Point2> f(model, measured, uv_hat);
EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
EXPECT( assert_equal(*expected, *gf, 1e-9));
// Try concise version
BADFactor<Point2> f2(model, measured,
uncalibrate(K, project(transform_to(x, p))));
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
EXPECT( assert_equal(*expected, *gf2, 1e-9));
}
/* ************************************************************************* */
TEST(BADFactor, compose1) {
// Create expression
Rot3_ R1(1), R2(2);
Rot3_ R3 = R1 * R2;
// Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
// Create some values
Values values;
values.insert(1, Rot3());
values.insert(2, Rot3());
// Check unwhitenedError
std::vector<Matrix> H(2);
Vector actual = f.unwhitenedError(values, H);
EXPECT( assert_equal(eye(3), H[0],1e-9));
EXPECT( assert_equal(eye(3), H[1],1e-9));
// Check linearization
JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
BADFactor<Point2> f(model, measured, project(p));
EXPECT_LONGS_EQUAL(2, f.dim());
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));
EXPECT( assert_equal(expected, *jf, 1e-9));
}
/* ************************************************************************* */
// Test compose with arguments referring to the same rotation
TEST(BADFactor, compose2) {
// Unary(Binary(Leaf,Leaf))
TEST(BADFactor, test1) {
// Create expression
Rot3_ R1(1), R2(1);
Rot3_ R3 = R1 * R2;
// Create some values
Values values;
values.insert(1, Pose3());
values.insert(2, Point3(0, 0, 1));
// Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
// Create old-style factor to create expected value and derivatives
GenericProjectionFactor<Pose3, Point3> old(measured, model, 1, 2,
boost::make_shared<Cal3_S2>());
double expected_error = old.error(values);
GaussianFactor::shared_ptr expected = old.linearize(values);
// Create some values
Values values;
values.insert(1, Rot3());
// Create leaves
Pose3_ x(1);
Point3_ p(2);
// Check unwhitenedError
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(2*eye(3), H[0],1e-9));
// Try concise version
BADFactor<Point2> f2(model, measured, project(transform_to(x, p)));
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
EXPECT( assert_equal(*expected, *gf2, 1e-9));
}
// 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));
}
/* ************************************************************************* */
// Binary(Leaf,Unary(Binary(Leaf,Leaf)))
TEST(BADFactor, test2) {
/* ************************************************************************* */
// Test compose with one arguments referring to a constant same rotation
TEST(BADFactor, compose3) {
// Create some values
Values values;
values.insert(1, Pose3());
values.insert(2, Point3(0, 0, 1));
values.insert(3, Cal3_S2());
// Create expression
Rot3_ R1(Rot3::identity()), R2(3);
Rot3_ R3 = R1 * R2;
// Create old-style factor to create expected value and derivatives
GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3);
double expected_error = old.error(values);
GaussianFactor::shared_ptr expected = old.linearize(values);
// Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
// Create leaves
Pose3_ x(1);
Point3_ p(2);
Cal3_S2_ K(3);
// Create some values
Values values;
values.insert(3, Rot3());
// Create expression tree
Point3_ p_cam(x, &Pose3::transform_to, p);
Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
// Check unwhitenedError
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(eye(3), H[0],1e-9));
// Create factor and check value, dimension, linearization
BADFactor<Point2> f(model, measured, uv_hat);
EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
EXPECT( assert_equal(*expected, *gf, 1e-9));
// Check linearization
JacobianFactor expected(3, 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));
}
// Try concise version
BADFactor<Point2> f2(model, measured,
uncalibrate(K, project(transform_to(x, p))));
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
EXPECT( assert_equal(*expected, *gf2, 1e-9));
}
/* ************************************************************************* */
/* ************************************************************************* */
TEST(BADFactor, compose1) {
// Create expression
Rot3_ R1(1), R2(2);
Rot3_ R3 = R1 * R2;
// Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
// Create some values
Values values;
values.insert(1, Rot3());
values.insert(2, Rot3());
// Check unwhitenedError
std::vector<Matrix> H(2);
Vector actual = f.unwhitenedError(values, H);
EXPECT( assert_equal(eye(3), H[0],1e-9));
EXPECT( assert_equal(eye(3), H[1],1e-9));
// 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));
}
/* ************************************************************************* */
// Test compose with arguments referring to the same rotation
TEST(BADFactor, compose2) {
// Create expression
Rot3_ R1(1), R2(1);
Rot3_ R3 = R1 * R2;
// Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
// Create some values
Values values;
values.insert(1, Rot3());
// Check unwhitenedError
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(2*eye(3), H[0],1e-9));
// 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));
}
/* ************************************************************************* */
// Test compose with one arguments referring to a constant same rotation
TEST(BADFactor, compose3) {
// Create expression
Rot3_ R1(Rot3::identity()), R2(3);
Rot3_ R3 = R1 * R2;
// Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
// Create some values
Values values;
values.insert(3, Rot3());
// Check unwhitenedError
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(eye(3), H[0],1e-9));
// Check linearization
JacobianFactor expected(3, 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));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);

33
gtsam_unstable/nonlinear/tests/testExpression.cpp
View File

@ -22,6 +22,7 @@
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam_unstable/nonlinear/Expression.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieScalar.h>
#include <CppUnitLite/TestHarness.h>
@ -36,13 +37,15 @@ Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix&> Dcal,
return K.uncalibrate(p, Dcal, Dp);
}
static const Rot3 someR = Rot3::RzRyRx(1,2,3);
/* ************************************************************************* */
TEST(Expression, constant) {
Expression<Rot3> R(Rot3::identity());
Expression<Rot3> R(someR);
Values values;
Augmented<Rot3> a = R.augmented(values);
EXPECT(assert_equal(Rot3::identity(), a.value()));
EXPECT(assert_equal(someR, a.value()));
JacobianMap expected;
EXPECT(a.jacobians() == expected);
}
@ -52,9 +55,9 @@ TEST(Expression, constant) {
TEST(Expression, leaf) {
Expression<Rot3> R(100);
Values values;
values.insert(100,Rot3::identity());
values.insert(100,someR);
Augmented<Rot3> a = R.augmented(values);
EXPECT(assert_equal(Rot3::identity(), a.value()));
EXPECT(assert_equal(someR, a.value()));
JacobianMap expected;
expected[100] = eye(3);
EXPECT(a.jacobians() == expected);
@ -62,17 +65,17 @@ TEST(Expression, leaf) {
/* ************************************************************************* */
TEST(Expression, nullaryMethod) {
Expression<Point3> p(67);
Expression<double> norm(p, &Point3::norm);
Values values;
values.insert(67,Point3(3,4,5));
Augmented<double> a = norm.augmented(values);
EXPECT(a.value() == sqrt(50));
JacobianMap expected;
expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50));
EXPECT(assert_equal(expected.at(67),a.jacobians().at(67)));
}
//TEST(Expression, nullaryMethod) {
// Expression<Point3> p(67);
// Expression<LieScalar> norm(p, &Point3::norm);
// Values values;
// values.insert(67,Point3(3,4,5));
// Augmented<LieScalar> a = norm.augmented(values);
// EXPECT(a.value() == sqrt(50));
// JacobianMap expected;
// expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50));
// EXPECT(assert_equal(expected.at(67),a.jacobians().at(67)));
//}
/* ************************************************************************* */