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

Implemented value and now testBADFactor also runs

release/4.3a0
dellaert 2014-10-05 15:00:10 +02:00
parent 8db2cd17fc
commit fdf9c10b42
3 changed files with 194 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -21,6 +21,7 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <boost/foreach.hpp>
namespace gtsam {
@ -44,6 +45,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) {
@ -90,7 +102,7 @@ public:
Augmented(const T& t, const JacobianMap& jacobians1,
const JacobianMap& jacobians2) :
value_(t), jacobians_(jacobians1) {
jacobians_.insert(jacobians2.begin(), jacobians2.end());
add(jacobians2);
}
/// Construct value, pre-multiply jacobians by H
@ -143,8 +155,9 @@ protected:
public:
struct Trace {
T t;
T value() const {
return T();
return t;
}
virtual Augmented<T> augmented(const Matrix& H) const = 0;
};
@ -208,11 +221,10 @@ public:
/// Trace structure for reverse AD
typedef typename ExpressionNode<T>::Trace BaseTrace;
struct Trace: public BaseTrace {
T t;
/// Return value and derivatives
virtual Augmented<T> augmented(const Matrix& H) const {
// Base case: just return value and empty map
return Augmented<T>(t);
return Augmented<T>(this->t);
}
};
@ -220,6 +232,8 @@ public:
virtual boost::shared_ptr<BaseTrace> reverse(const Values& values) const {
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
trace->t = constant_;
std::cout << "constant\n:";
GTSAM_PRINT(trace->t);
return trace;
}
};
@ -266,14 +280,12 @@ public:
/// Trace structure for reverse AD
typedef typename ExpressionNode<T>::Trace BaseTrace;
struct Trace: public BaseTrace {
T t;
Key key;
/// Return value and derivatives
virtual Augmented<T> augmented(const Matrix& H) const {
// This is a top-down calculation
// The end-result needs Jacobians to all leaf nodes.
// Since this is a leaf node, we are done and just insert H in the JacobianMap
return Augmented<T>(t, key, H);
// Base case: just insert H in the JacobianMap with correct key
std::cout << "Inserting Jacobian " << DefaultKeyFormatter(key) << "\n";
return Augmented<T>(this->t, key, H);
}
};
@ -282,6 +294,8 @@ public:
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
trace->t = value(values);
trace->key = key_;
std::cout << "Leaf(" << DefaultKeyFormatter(key_) << "):\n";
GTSAM_PRINT(trace->t);
return trace;
}
@ -339,14 +353,13 @@ public:
struct Trace: public BaseTrace {
boost::shared_ptr<typename ExpressionNode<A>::Trace> trace1;
Matrix H1;
T t;
/// Return value and derivatives
virtual Augmented<T> augmented(const Matrix& H) const {
// This is a top-down calculation
// The end-result needs Jacobians to all leaf nodes.
// Since this is not a leaf node, we compute what is needed for leaf nodes here
Augmented<A> augmented1 = trace1->augmented(H * H1);
return Augmented<T>(t, augmented1.jacobians());
return Augmented<T>(this->t, augmented1.jacobians());
}
};
@ -354,7 +367,10 @@ public:
virtual boost::shared_ptr<BaseTrace> reverse(const Values& values) const {
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
trace->trace1 = this->expressionA_->reverse(values);
std::cout << "Unary:\n";
GTSAM_PRINT(trace->trace1->value());
trace->t = function_(trace->trace1->value(), trace->H1);
GTSAM_PRINT(trace->t);
return trace;
}
};
@ -424,7 +440,6 @@ public:
boost::shared_ptr<typename ExpressionNode<A1>::Trace> trace1;
boost::shared_ptr<typename ExpressionNode<A2>::Trace> trace2;
Matrix H1, H2;
T t;
/// Return value and derivatives
virtual Augmented<T> augmented(const Matrix& H) const {
// This is a top-down calculation
@ -432,8 +447,9 @@ public:
// Since this is not a leaf node, we compute what is needed for leaf nodes here
// The binary node represents a fork in the tree, and hence we will get two Augmented maps
Augmented<A1> augmented1 = trace1->augmented(H * H1);
Augmented<A1> augmented2 = trace1->augmented(H * H2);
return Augmented<T>(t, augmented1.jacobians(), augmented2.jacobians());
Augmented<A2> augmented2 = trace2->augmented(H * H2);
return Augmented<T>(this->t, augmented1.jacobians(),
augmented2.jacobians());
}
};
@ -442,8 +458,12 @@ public:
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
trace->trace1 = this->expressionA1_->reverse(values);
trace->trace2 = this->expressionA2_->reverse(values);
std::cout << "Binary:\n";
GTSAM_PRINT(trace->trace1->value());
GTSAM_PRINT(trace->trace2->value());
trace->t = function_(trace->trace1->value(), trace->trace2->value(),
trace->H1, trace->H2);
GTSAM_PRINT(trace->t);
return trace;
}

6
gtsam_unstable/nonlinear/Expression.h
View File

@ -103,10 +103,14 @@ public:
/// Return value and derivatives
Augmented<T> augmented(const Values& values) const {
#define REVERSE_AD
#ifdef REVERSE_AD
boost::shared_ptr<typename ExpressionNode<T>::Trace> trace = root_->reverse(values);
size_t n = T::Dim();
return trace->augmented(Eigen::MatrixXd::Identity(n, n));
// return root_->forward(values);
#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);