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

Unary prints, but still-faults downstream

release/4.3a0
dellaert 2014-10-11 11:30:06 +02:00
parent 05f78b6dca
commit deed7b8018
2 changed files with 68 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -466,11 +466,11 @@ public:
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
void* raw) const { void* raw) const {
// Record* record = new Record(); Record* record = new (raw) Record();
// p.setFunction(record); trace.setFunction(record);
// A1 a = this->expressionA1_->traceExecution(values, record->trace1); A1 a1 = this->expressionA1_->traceExecution(values, record->trace1,
// return function_(a, record->dTdA1); record + 1);
return T(); return function_(a1, record->dTdA1);
} }
}; };
@ -692,13 +692,12 @@ public:
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
void* raw) const { void* raw) const {
// Record* record = new Record(); Record* record = new (raw) Record();
// p.setFunction(record); trace.setFunction(record);
// A1 a1 = this->expressionA1_->traceExecution(values, record->trace1); A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw);
// A2 a2 = this->expressionA2_->traceExecution(values, record->trace2); A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw);
// A3 a3 = this->expressionA3_->traceExecution(values, record->trace3); A3 a3 = this->expressionA3_->traceExecution(values, record->trace3, raw);
// return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3); return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3);
return T();
} }
}; };

90
gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
View File

@ -131,12 +131,6 @@ TEST(ExpressionFactor, binary) {
values.insert(1, Cal3_S2()); values.insert(1, Cal3_S2());
values.insert(2, Point2(0, 0)); values.insert(2, Point2(0, 0));
// Expected Jacobians
Matrix25 expected25;
expected25 << 0, 0, 0, 1, 0, 0, 0, 0, 0, 1;
Matrix2 expected22;
expected22 << 1, 0, 0, 1;
// traceRaw will fill raw with [Trace<Point2> | Binary::Record] // traceRaw will fill raw with [Trace<Point2> | Binary::Record]
EXPECT_LONGS_EQUAL(8, sizeof(double)); EXPECT_LONGS_EQUAL(8, sizeof(double));
EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace<Point2>)); EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace<Point2>));
@ -153,39 +147,69 @@ TEST(ExpressionFactor, binary) {
Point2 value = tester.binary_.traceExecution(values, trace, raw); Point2 value = tester.binary_.traceExecution(values, trace, raw);
// trace.print(); // trace.print();
// Expected Jacobians
Matrix25 expected25;
expected25 << 0, 0, 0, 1, 0, 0, 0, 0, 0, 1;
Matrix2 expected22;
expected22 << 1, 0, 0, 1;
// Check matrices // Check matrices
boost::optional<Binary::Record*> p = trace.record<Binary::Record>(); boost::optional<Binary::Record*> p = trace.record<Binary::Record>();
CHECK(p); CHECK(p);
EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9)); EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9));
EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9)); EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9));
} }
///* ************************************************************************* */ /* ************************************************************************* */
//// Unary(Binary(Leaf,Leaf)) // Unary(Binary(Leaf,Leaf))
//TEST(ExpressionFactor, shallow) { TEST(ExpressionFactor, shallow) {
//
// // Create some values // Create some values
// Values values; Values values;
// values.insert(1, Pose3()); values.insert(1, Pose3());
// values.insert(2, Point3(0, 0, 1)); values.insert(2, Point3(0, 0, 1));
//
// // Create old-style factor to create expected value and derivatives // Create old-style factor to create expected value and derivatives
// GenericProjectionFactor<Pose3, Point3> old(measured, model, 1, 2, GenericProjectionFactor<Pose3, Point3> old(measured, model, 1, 2,
// boost::make_shared<Cal3_S2>()); boost::make_shared<Cal3_S2>());
// double expected_error = old.error(values); double expected_error = old.error(values);
// GaussianFactor::shared_ptr expected = old.linearize(values); GaussianFactor::shared_ptr expected = old.linearize(values);
//
// // Create leaves // Create leaves
// Pose3_ x(1); Pose3_ x_(1);
// Point3_ p(2); Point3_ p_(2);
//
// // Concise version // Construct expression, concise evrsion
// ExpressionFactor<Point2> f2(model, measured, project(transform_to(x, p))); Point2_ expression = project(transform_to(x_, p_));
// EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
// EXPECT_LONGS_EQUAL(2, f2.dim()); // traceExecution of shallow tree
// boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values); typedef UnaryExpression<Point2, Point3> Unary;
// EXPECT( assert_equal(*expected, *gf2, 1e-9)); typedef BinaryExpression<Point3, Pose3, Point3> Binary;
//} EXPECT_LONGS_EQUAL(80, sizeof(Unary::Record));
// EXPECT_LONGS_EQUAL(272, sizeof(Binary::Record));
size_t size = sizeof(Unary::Record) + sizeof(Binary::Record);
LONGS_EQUAL(352, size);
char raw[size];
ExecutionTrace<Point2> trace;
Point2 value = expression.traceExecution(values, trace, raw);
trace.print();
// Expected Jacobians
Matrix23 expected23;
expected23 << 1, 0, 0, 0, 1, 0;
// Check matrices
boost::optional<Unary::Record*> p = trace.record<Unary::Record>();
CHECK(p);
EXPECT( assert_equal(expected23, (Matrix)(*p)->dTdA1, 1e-9));
// Linearization
ExpressionFactor<Point2> f2(model, measured, expression);
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));
}
///* ************************************************************************* */ ///* ************************************************************************* */
//// Binary(Leaf,Unary(Binary(Leaf,Leaf))) //// Binary(Leaf,Unary(Binary(Leaf,Leaf)))
//TEST(ExpressionFactor, tree) { //TEST(ExpressionFactor, tree) {