From cb1672d29211bd1b20cb80eac10841498865a19c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jul 2015 15:10:00 -0700 Subject: [PATCH 01/11] slight refactor --- gtsam/geometry/SO3.cpp | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 7e755d680..0fcf28dfb 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -32,18 +32,23 @@ SO3 SO3::Rodrigues(const Vector3& axis, double theta) { // get components of axis \omega double wx = axis(0), wy = axis(1), wz = axis(2); - double c = cos(theta), s = sin(theta), c_1 = 1 - c; - double wwTxx = wx * wx, wwTyy = wy * wy, wwTzz = wz * wz; - double swx = wx * s, swy = wy * s, swz = wz * s; + double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta; + double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta, wz_sintheta = wz * sintheta; - double C00 = c_1 * wwTxx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz; - double C11 = c_1 * wwTyy, C12 = c_1 * wy * wz; - double C22 = c_1 * wwTzz; + double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz; + double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz; + double C22 = c_1 * wz * wz; Matrix3 R; - R << c + C00, -swz + C01, swy + C02, // - swz + C01, c + C11, -swx + C12, // - -swy + C02, swx + C12, c + C22; + R(0, 0) = costheta + C00; + R(1, 0) = wz_sintheta + C01; + R(2, 0) = -wy_sintheta + C02; + R(0, 1) = -wz_sintheta + C01; + R(1, 1) = costheta + C11; + R(2, 1) = wx_sintheta + C12; + R(0, 2) = wy_sintheta + C02; + R(1, 2) = -wx_sintheta + C12; + R(2, 2) = costheta + C22; return R; } From 8ceaa8a3cc8e6d7327c139e522c3c72c9aa7e8d1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jul 2015 15:25:18 -0700 Subject: [PATCH 02/11] Check ceres rotation convention --- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 3f477098a..6b64aedfa 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -59,6 +59,16 @@ typedef PinholeCamera Camera; using namespace std; using namespace gtsam; +/* ************************************************************************* */ +// Make sure rotation convention is the same +TEST(AdaptAutoDiff, Rotation) { + Vector3 axisAngle(0.1,0.2,0.3); + Matrix3 expected = Rot3::rodriguez(axisAngle).matrix(); + Matrix3 actual; + ceres::AngleAxisToRotationMatrix(axisAngle.data(), actual.data()); + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ // charts TEST(AdaptAutoDiff, Canonical) { From a70815b6541e8906de132bee018780a8a640a954 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jul 2015 15:54:46 -0700 Subject: [PATCH 03/11] Fixed gtsam-opengl convention mismatch --- timing/timeSFMBAL.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 68b3c4932..763c22af0 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -34,7 +34,7 @@ using namespace std; using namespace gtsam; -#define USE_GTSAM_FACTOR +//#define USE_GTSAM_FACTOR #ifdef USE_GTSAM_FACTOR #include #include @@ -92,14 +92,15 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < db.number_tracks(); j++) { BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { size_t i = m.first; - Point2 measurement = m.second; + Point2 z = m.second; #ifdef USE_GTSAM_FACTOR - graph.push_back(SfmFactor(measurement, unit2, i, P(j))); + graph.push_back(SfmFactor(z, unit2, i, P(j))); #else Expression camera_(i); Expression point_(P(j)); - graph.addExpressionFactor(unit2, measurement, - Expression(snavely, camera_, point_)); + // Snavely expects measurements in OpenGL format, with y increasing upwards + graph.addExpressionFactor(unit2, Point2(z.x(), -z.y()), + Expression(snavely, camera_, point_)); #endif } } @@ -110,21 +111,23 @@ int main(int argc, char* argv[]) { #ifdef USE_GTSAM_FACTOR initial.insert((i++), camera); #else - Camera ceresCamera(camera.pose(), camera.calibration()); + // readBAL converts to GTSAM format, so we need to convert back ! + Camera ceresCamera(gtsam2openGL(camera.pose()), camera.calibration()); initial.insert((i++), ceresCamera); #endif } BOOST_FOREACH(const SfM_Track& track, db.tracks) initial.insert(P(j++), track.p); - // Check projection + // Check projection of first point in first camera Point2 expected = db.tracks.front().measurements.front().second; Camera camera = initial.at(0); Point3 point = initial.at(P(0)); #ifdef USE_GTSAM_FACTOR Point2 actual = camera.project(point); #else - Point2 actual = snavely(camera, point); + Point2 opengl = snavely(camera, point); + Point2 actual(opengl.x(), -opengl.y()); #endif assert_equal(expected,actual,10); From 23f8a98d66461ee1d4e72b610d9dc135c394b16c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jul 2015 16:27:26 -0700 Subject: [PATCH 04/11] Some refactoring --- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 54 +++++++++++---------- 1 file changed, 28 insertions(+), 26 deletions(-) diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 6b64aedfa..49c5ab9ef 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -36,23 +36,23 @@ using boost::assign::map_list_of; namespace gtsam { // Special version of Cal3Bundler so that default constructor = 0,0,0 -struct Cal: public Cal3Bundler { - Cal(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, double v0 = 0) : +struct Cal3Bundler0: public Cal3Bundler { + Cal3Bundler0(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, double v0 = 0) : Cal3Bundler(f, k1, k2, u0, v0) { } - Cal retract(const Vector& d) const { - return Cal(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); + Cal3Bundler0 retract(const Vector& d) const { + return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); } - Vector3 localCoordinates(const Cal& T2) const { + Vector3 localCoordinates(const Cal3Bundler0& T2) const { return T2.vector() - vector(); } }; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold {}; // With that, camera below behaves like Snavely's 9-dim vector -typedef PinholeCamera Camera; +typedef PinholeCamera Camera; } @@ -60,7 +60,7 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -// Make sure rotation convention is the same +// Check that ceres rotation convention is the same TEST(AdaptAutoDiff, Rotation) { Vector3 axisAngle(0.1,0.2,0.3); Matrix3 expected = Rot3::rodriguez(axisAngle).matrix(); @@ -70,15 +70,15 @@ TEST(AdaptAutoDiff, Rotation) { } /* ************************************************************************* */ -// charts +// Canonical sets up Local/Retract around the default-constructed value +// The tests below check this for all types that play a role i SFM TEST(AdaptAutoDiff, Canonical) { Canonical chart1; EXPECT(chart1.Local(Point2(1, 0))==Vector2(1, 0)); EXPECT(chart1.Retract(Vector2(1, 0))==Point2(1, 0)); - Vector v2(2); - v2 << 1, 0; + Vector2 v2(1, 0); Canonical chart2; EXPECT(assert_equal(v2, chart2.Local(Vector2(1, 0)))); EXPECT(chart2.Retract(v2)==Vector2(1, 0)); @@ -91,8 +91,7 @@ TEST(AdaptAutoDiff, Canonical) { Canonical chart4; Point3 point(1, 2, 3); - Vector v3(3); - v3 << 1, 2, 3; + Vector3 v3(1, 2, 3); EXPECT(assert_equal(v3, chart4.Local(point))); EXPECT(assert_equal(chart4.Retract(v3), point)); @@ -103,8 +102,8 @@ TEST(AdaptAutoDiff, Canonical) { EXPECT(assert_equal(v6, chart5.Local(pose))); EXPECT(assert_equal(chart5.Retract(v6), pose)); - Canonical chart6; - Cal cal0; + Canonical chart6; + Cal3Bundler0 cal0; Vector z3 = Vector3::Zero(); EXPECT(assert_equal(z3, chart6.Local(cal0))); EXPECT(assert_equal(chart6.Retract(z3), cal0)); @@ -207,17 +206,18 @@ Vector2 adapted(const Vector9& P, const Vector3& X) { throw std::runtime_error("Snavely fail"); } +/* ************************************************************************* */ +namespace example { +// zero rotation, (0,5,0) translation, focal length 1 +Vector9 P = (Vector9() << 0, 0, 0, 0, 5, 0, 1, 0, 0).finished(); +Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely! +} + +/* ************************************************************************* */ TEST(AdaptAutoDiff, AutoDiff2) { + using namespace example; using ceres::internal::AutoDiff; - // Instantiate function - SnavelyProjection snavely; - - // Make arguments - Vector9 P; // zero rotation, (0,5,0) translation, focal length 1 - P << 0, 0, 0, 0, 5, 0, 1, 0, 0; - Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely! - // Apply the mapping, to get image point b_x. Vector expected = Vector2(2, 1); Vector2 actual = adapted(P, X); @@ -227,6 +227,9 @@ TEST(AdaptAutoDiff, AutoDiff2) { Matrix E1 = numericalDerivative21(adapted, P, X); Matrix E2 = numericalDerivative22(adapted, P, X); + // Instantiate function + SnavelyProjection snavely; + // Get derivatives with AutoDiff Vector2 actual2; MatrixRowMajor H1(2, 9), H2(2, 3); @@ -241,9 +244,8 @@ TEST(AdaptAutoDiff, AutoDiff2) { /* ************************************************************************* */ // Test AutoDiff wrapper Snavely TEST(AdaptAutoDiff, AutoDiff3) { - // Make arguments - Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal(1, 0, 0)); + Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0)); Point3 X(10, 0, -5); // negative Z-axis convention of Snavely! typedef AdaptAutoDiff Adaptor; @@ -269,7 +271,7 @@ TEST(AdaptAutoDiff, AutoDiff3) { /* ************************************************************************* */ // Test AutoDiff wrapper in an expression -TEST(AdaptAutoDiff, Snavely) { +TEST(AdaptAutoDiff, SnavelyExpression) { Expression P(1); Expression X(2); typedef AdaptAutoDiff Adaptor; From 1a81cd9929941ec663b730a38bfb2c9af62b2493 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jul 2015 16:40:46 -0700 Subject: [PATCH 05/11] charts are types --- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 73 ++++++++++++--------- 1 file changed, 41 insertions(+), 32 deletions(-) diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 49c5ab9ef..a4a3ade87 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -71,48 +71,48 @@ TEST(AdaptAutoDiff, Rotation) { /* ************************************************************************* */ // Canonical sets up Local/Retract around the default-constructed value -// The tests below check this for all types that play a role i SFM +// The tests below check this for all types that play a role in SFM TEST(AdaptAutoDiff, Canonical) { - Canonical chart1; - EXPECT(chart1.Local(Point2(1, 0))==Vector2(1, 0)); - EXPECT(chart1.Retract(Vector2(1, 0))==Point2(1, 0)); + typedef Canonical Chart1; + EXPECT(Chart1::Local(Point2(1, 0))==Vector2(1, 0)); + EXPECT(Chart1::Retract(Vector2(1, 0))==Point2(1, 0)); Vector2 v2(1, 0); - Canonical chart2; - EXPECT(assert_equal(v2, chart2.Local(Vector2(1, 0)))); - EXPECT(chart2.Retract(v2)==Vector2(1, 0)); + typedef Canonical Chart2; + EXPECT(assert_equal(v2, Chart2::Local(Vector2(1, 0)))); + EXPECT(Chart2::Retract(v2)==Vector2(1, 0)); - Canonical chart3; + typedef Canonical Chart3; Eigen::Matrix v1; v1 << 1; - EXPECT(chart3.Local(1)==v1); - EXPECT_DOUBLES_EQUAL(chart3.Retract(v1), 1, 1e-9); + EXPECT(Chart3::Local(1)==v1); + EXPECT_DOUBLES_EQUAL(Chart3::Retract(v1), 1, 1e-9); - Canonical chart4; + typedef Canonical Chart4; Point3 point(1, 2, 3); Vector3 v3(1, 2, 3); - EXPECT(assert_equal(v3, chart4.Local(point))); - EXPECT(assert_equal(chart4.Retract(v3), point)); + EXPECT(assert_equal(v3, Chart4::Local(point))); + EXPECT(assert_equal(Chart4::Retract(v3), point)); - Canonical chart5; + typedef Canonical Chart5; Pose3 pose(Rot3::identity(), point); Vector v6(6); v6 << 0, 0, 0, 1, 2, 3; - EXPECT(assert_equal(v6, chart5.Local(pose))); - EXPECT(assert_equal(chart5.Retract(v6), pose)); + EXPECT(assert_equal(v6, Chart5::Local(pose))); + EXPECT(assert_equal(Chart5::Retract(v6), pose)); - Canonical chart6; + typedef Canonical Chart6; Cal3Bundler0 cal0; Vector z3 = Vector3::Zero(); - EXPECT(assert_equal(z3, chart6.Local(cal0))); - EXPECT(assert_equal(chart6.Retract(z3), cal0)); + EXPECT(assert_equal(z3, Chart6::Local(cal0))); + EXPECT(assert_equal(Chart6::Retract(z3), cal0)); - Canonical chart7; + typedef Canonical Chart7; Camera camera(Pose3(), cal0); Vector z9 = Vector9::Zero(); - EXPECT(assert_equal(z9, chart7.Local(camera))); - EXPECT(assert_equal(chart7.Retract(z9), camera)); + EXPECT(assert_equal(z9, Chart7::Local(camera))); + EXPECT(assert_equal(Chart7::Retract(z9), camera)); } /* ************************************************************************* */ @@ -208,9 +208,20 @@ Vector2 adapted(const Vector9& P, const Vector3& X) { /* ************************************************************************* */ namespace example { -// zero rotation, (0,5,0) translation, focal length 1 -Vector9 P = (Vector9() << 0, 0, 0, 0, 5, 0, 1, 0, 0).finished(); -Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely! +Camera camera(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0)); +Point3 point(10, 0, -5); // negative Z-axis convention of Snavely! +Vector9 P = Canonical::Local(camera); +Vector3 X = Canonical::Local(point); +} + +/* ************************************************************************* */ +// Check that Local worked as expected +TEST(AdaptAutoDiff, Local) { + using namespace example; + Vector9 expectedP = (Vector9() << 0, 0, 0, 0, 5, 0, 1, 0, 0).finished(); + EXPECT(equal_with_abs_tol(expectedP, P)); + Vector3 expectedX(10, 0, -5); // negative Z-axis convention of Snavely! + EXPECT(equal_with_abs_tol(expectedX, X)); } /* ************************************************************************* */ @@ -244,26 +255,24 @@ TEST(AdaptAutoDiff, AutoDiff2) { /* ************************************************************************* */ // Test AutoDiff wrapper Snavely TEST(AdaptAutoDiff, AutoDiff3) { - // Make arguments - Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0)); - Point3 X(10, 0, -5); // negative Z-axis convention of Snavely! + using namespace example; typedef AdaptAutoDiff Adaptor; Adaptor snavely; // Apply the mapping, to get image point b_x. Point2 expected(2, 1); - Point2 actual = snavely(P, X); + Point2 actual = snavely(camera, point); EXPECT(assert_equal(expected,actual,1e-9)); // // Get expected derivatives - Matrix E1 = numericalDerivative21(Adaptor(), P, X); - Matrix E2 = numericalDerivative22(Adaptor(), P, X); + Matrix E1 = numericalDerivative21(Adaptor(), camera, point); + Matrix E2 = numericalDerivative22(Adaptor(), camera, point); // Get derivatives with AutoDiff, not gives RowMajor results! Matrix29 H1; Matrix23 H2; - Point2 actual2 = snavely(P, X, H1, H2); + Point2 actual2 = snavely(camera, point, H1, H2); EXPECT(assert_equal(expected,actual2,1e-9)); EXPECT(assert_equal(E1,H1,1e-8)); EXPECT(assert_equal(E2,H2,1e-8)); From bded06f97f13f099881ced87034f3253baabea0c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jul 2015 16:50:46 -0700 Subject: [PATCH 06/11] Made testcase with nonzero rotation: fails test! --- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 39 +++++++++++---------- 1 file changed, 20 insertions(+), 19 deletions(-) diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index a4a3ade87..2ab52f6bc 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -208,31 +208,33 @@ Vector2 adapted(const Vector9& P, const Vector3& X) { /* ************************************************************************* */ namespace example { -Camera camera(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0)); +Camera camera(Pose3(Rot3::rodriguez(0.1, 0.2, 0.3), Point3(0, 5, 0)), + Cal3Bundler0(1, 0, 0)); Point3 point(10, 0, -5); // negative Z-axis convention of Snavely! Vector9 P = Canonical::Local(camera); Vector3 X = Canonical::Local(point); +Point2 expectedMeasurement(1.2431567, 1.2525694); } /* ************************************************************************* */ // Check that Local worked as expected TEST(AdaptAutoDiff, Local) { using namespace example; - Vector9 expectedP = (Vector9() << 0, 0, 0, 0, 5, 0, 1, 0, 0).finished(); + Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0, 5, 0, 1, 0, 0).finished(); EXPECT(equal_with_abs_tol(expectedP, P)); Vector3 expectedX(10, 0, -5); // negative Z-axis convention of Snavely! EXPECT(equal_with_abs_tol(expectedX, X)); } /* ************************************************************************* */ +// Test Ceres AutoDiff TEST(AdaptAutoDiff, AutoDiff2) { using namespace example; using ceres::internal::AutoDiff; // Apply the mapping, to get image point b_x. - Vector expected = Vector2(2, 1); Vector2 actual = adapted(P, X); - EXPECT(assert_equal(expected,actual,1e-9)); + EXPECT(assert_equal(expectedMeasurement.vector(), actual, 1e-6)); // Get expected derivatives Matrix E1 = numericalDerivative21(adapted, P, X); @@ -244,28 +246,27 @@ TEST(AdaptAutoDiff, AutoDiff2) { // Get derivatives with AutoDiff Vector2 actual2; MatrixRowMajor H1(2, 9), H2(2, 3); - double *parameters[] = { P.data(), X.data() }; - double *jacobians[] = { H1.data(), H2.data() }; - CHECK( - (AutoDiff::Differentiate( snavely, parameters, 2, actual2.data(), jacobians))); - EXPECT(assert_equal(E1,H1,1e-8)); - EXPECT(assert_equal(E2,H2,1e-8)); + double* parameters[] = {P.data(), X.data()}; + double* jacobians[] = {H1.data(), H2.data()}; + CHECK((AutoDiff::Differentiate( + snavely, parameters, 2, actual2.data(), jacobians))); + EXPECT(assert_equal(E1, H1, 1e-8)); + EXPECT(assert_equal(E2, H2, 1e-8)); } /* ************************************************************************* */ // Test AutoDiff wrapper Snavely -TEST(AdaptAutoDiff, AutoDiff3) { +TEST(AdaptAutoDiff, AdaptAutoDiff) { using namespace example; typedef AdaptAutoDiff Adaptor; Adaptor snavely; // Apply the mapping, to get image point b_x. - Point2 expected(2, 1); Point2 actual = snavely(camera, point); - EXPECT(assert_equal(expected,actual,1e-9)); + EXPECT(assert_equal(expectedMeasurement, actual, 1e-6)); -// // Get expected derivatives + // // Get expected derivatives Matrix E1 = numericalDerivative21(Adaptor(), camera, point); Matrix E2 = numericalDerivative22(Adaptor(), camera, point); @@ -273,9 +274,9 @@ TEST(AdaptAutoDiff, AutoDiff3) { Matrix29 H1; Matrix23 H2; Point2 actual2 = snavely(camera, point, H1, H2); - EXPECT(assert_equal(expected,actual2,1e-9)); - EXPECT(assert_equal(E1,H1,1e-8)); - EXPECT(assert_equal(E2,H2,1e-8)); + EXPECT(assert_equal(expectedMeasurement, actual2, 1e-6)); + EXPECT(assert_equal(E1, H1, 1e-8)); + EXPECT(assert_equal(E2, H2, 1e-8)); } /* ************************************************************************* */ @@ -286,10 +287,10 @@ TEST(AdaptAutoDiff, SnavelyExpression) { typedef AdaptAutoDiff Adaptor; Expression expression(Adaptor(), P, X); #ifdef GTSAM_USE_QUATERNIONS - EXPECT_LONGS_EQUAL(384,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(384,expression.traceSize()); // TODO(frank): should be zero #else EXPECT_LONGS_EQUAL(sizeof(internal::BinaryExpression::Record), - expression.traceSize()); // Todo, should be zero + expression.traceSize()); // TODO(frank): should be zero #endif set expected = list_of(1)(2); EXPECT(expected == expression.keys()); From b752f8446ce95d3e3b459378aab63c945812a1e3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jul 2015 18:06:01 -0700 Subject: [PATCH 07/11] Fixed and simplified (quite broken) AdaptAutoDiff, now works with fixed Vectors --- gtsam/nonlinear/AdaptAutoDiff.h | 100 ++++---------- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 140 ++++++-------------- 2 files changed, 68 insertions(+), 172 deletions(-) diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index 341bb7d10..96ea9b182 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -27,95 +27,44 @@ namespace gtsam { -namespace detail { - -// By default, we assume an Identity element -template -struct Origin { T operator()() { return traits::Identity();} }; - -// but simple manifolds don't have one, so we just use the default constructor -template -struct Origin { T operator()() { return T();} }; - -} // \ detail - -/** - * Canonical is a template that creates canonical coordinates for a given type. - * A simple manifold type (i.e., not a Lie Group) has no concept of identity, - * hence in that case we use the value given by the default constructor T() as - * the origin of a "canonical coordinate" parameterization. - */ -template -struct Canonical { - - GTSAM_CONCEPT_MANIFOLD_TYPE(T) - - typedef traits Traits; - enum { dimension = Traits::dimension }; - typedef typename Traits::TangentVector TangentVector; - typedef typename Traits::structure_category Category; - typedef detail::Origin Origin; - - static TangentVector Local(const T& other) { - return Traits::Local(Origin()(), other); - } - - static T Retract(const TangentVector& v) { - return Traits::Retract(Origin()(), v); - } -}; - /** * The AdaptAutoDiff class uses ceres-style autodiff to adapt a ceres-style - * Function evaluation, i.e., a function F that defines an operator - * template bool operator()(const T* const, const T* const, T* predicted) const; + * Function evaluation, i.e., a function FUNCTOR that defines an operator + * template bool operator()(const T* const, const T* const, T* + * predicted) const; * For now only binary operators are supported. */ -template +template class AdaptAutoDiff { + typedef Eigen::Matrix RowMajor1; + typedef Eigen::Matrix RowMajor2; - static const int N = traits::dimension; - static const int M1 = traits::dimension; - static const int M2 = traits::dimension; + typedef Eigen::Matrix VectorT; + typedef Eigen::Matrix Vector1; + typedef Eigen::Matrix Vector2; - typedef Eigen::Matrix RowMajor1; - typedef Eigen::Matrix RowMajor2; - - typedef Canonical CanonicalT; - typedef Canonical Canonical1; - typedef Canonical Canonical2; - - typedef typename CanonicalT::TangentVector VectorT; - typedef typename Canonical1::TangentVector Vector1; - typedef typename Canonical2::TangentVector Vector2; - - F f; - -public: - - T operator()(const A1& a1, const A2& a2, OptionalJacobian H1 = boost::none, - OptionalJacobian H2 = boost::none) { + FUNCTOR f; + public: + VectorT operator()(const Vector1& v1, const Vector2& v2, + OptionalJacobian H1 = boost::none, + OptionalJacobian H2 = boost::none) { using ceres::internal::AutoDiff; - // Make arguments - Vector1 v1 = Canonical1::Local(a1); - Vector2 v2 = Canonical2::Local(a2); - bool success; VectorT result; if (H1 || H2) { - // Get derivatives with AutoDiff - double *parameters[] = { v1.data(), v2.data() }; - double rowMajor1[N * M1], rowMajor2[N * M2]; // on the stack - double *jacobians[] = { rowMajor1, rowMajor2 }; - success = AutoDiff::Differentiate(f, parameters, 2, - result.data(), jacobians); + const double* parameters[] = {v1.data(), v2.data()}; + double rowMajor1[M * N1], rowMajor2[M * N2]; // on the stack + double* jacobians[] = {rowMajor1, rowMajor2}; + success = AutoDiff::Differentiate( + f, parameters, M, result.data(), jacobians); // Convert from row-major to columnn-major - // TODO: if this is a bottleneck (probably not!) fix Autodiff to be Column-Major + // TODO: if this is a bottleneck (probably not!) fix Autodiff to be + // Column-Major *H1 = Eigen::Map(rowMajor1); *H2 = Eigen::Map(rowMajor2); @@ -126,9 +75,8 @@ public: if (!success) throw std::runtime_error( "AdaptAutoDiff: function call resulted in failure"); - return CanonicalT::Retract(result); + return result; } - }; -} +} // namespace gtsam diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 2ab52f6bc..59f8cb7cd 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -36,10 +36,10 @@ using boost::assign::map_list_of; namespace gtsam { // Special version of Cal3Bundler so that default constructor = 0,0,0 -struct Cal3Bundler0: public Cal3Bundler { - Cal3Bundler0(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, double v0 = 0) : - Cal3Bundler(f, k1, k2, u0, v0) { - } +struct Cal3Bundler0 : public Cal3Bundler { + Cal3Bundler0(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, + double v0 = 0) + : Cal3Bundler(f, k1, k2, u0, v0) {} Cal3Bundler0 retract(const Vector& d) const { return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); } @@ -48,12 +48,11 @@ struct Cal3Bundler0: public Cal3Bundler { } }; -template<> +template <> struct traits : public internal::Manifold {}; // With that, camera below behaves like Snavely's 9-dim vector typedef PinholeCamera Camera; - } using namespace std; @@ -62,64 +61,18 @@ using namespace gtsam; /* ************************************************************************* */ // Check that ceres rotation convention is the same TEST(AdaptAutoDiff, Rotation) { - Vector3 axisAngle(0.1,0.2,0.3); + Vector3 axisAngle(0.1, 0.2, 0.3); Matrix3 expected = Rot3::rodriguez(axisAngle).matrix(); Matrix3 actual; ceres::AngleAxisToRotationMatrix(axisAngle.data(), actual.data()); EXPECT(assert_equal(expected, actual)); } -/* ************************************************************************* */ -// Canonical sets up Local/Retract around the default-constructed value -// The tests below check this for all types that play a role in SFM -TEST(AdaptAutoDiff, Canonical) { - - typedef Canonical Chart1; - EXPECT(Chart1::Local(Point2(1, 0))==Vector2(1, 0)); - EXPECT(Chart1::Retract(Vector2(1, 0))==Point2(1, 0)); - - Vector2 v2(1, 0); - typedef Canonical Chart2; - EXPECT(assert_equal(v2, Chart2::Local(Vector2(1, 0)))); - EXPECT(Chart2::Retract(v2)==Vector2(1, 0)); - - typedef Canonical Chart3; - Eigen::Matrix v1; - v1 << 1; - EXPECT(Chart3::Local(1)==v1); - EXPECT_DOUBLES_EQUAL(Chart3::Retract(v1), 1, 1e-9); - - typedef Canonical Chart4; - Point3 point(1, 2, 3); - Vector3 v3(1, 2, 3); - EXPECT(assert_equal(v3, Chart4::Local(point))); - EXPECT(assert_equal(Chart4::Retract(v3), point)); - - typedef Canonical Chart5; - Pose3 pose(Rot3::identity(), point); - Vector v6(6); - v6 << 0, 0, 0, 1, 2, 3; - EXPECT(assert_equal(v6, Chart5::Local(pose))); - EXPECT(assert_equal(Chart5::Retract(v6), pose)); - - typedef Canonical Chart6; - Cal3Bundler0 cal0; - Vector z3 = Vector3::Zero(); - EXPECT(assert_equal(z3, Chart6::Local(cal0))); - EXPECT(assert_equal(Chart6::Retract(z3), cal0)); - - typedef Canonical Chart7; - Camera camera(Pose3(), cal0); - Vector z9 = Vector9::Zero(); - EXPECT(assert_equal(z9, Chart7::Local(camera))); - EXPECT(assert_equal(Chart7::Retract(z9), camera)); -} - /* ************************************************************************* */ // Some Ceres Snippets copied for testing // Copyright 2010, 2011, 2012 Google Inc. All rights reserved. -template inline T &RowMajorAccess(T *base, int rows, int cols, - int i, int j) { +template +inline T& RowMajorAccess(T* base, int rows, int cols, int i, int j) { return base[cols * i + j]; } @@ -133,14 +86,14 @@ inline double RandDouble() { struct Projective { // Function that takes P and X as separate vectors: // P, X -> x - template + template bool operator()(A const P[12], A const X[4], A x[2]) const { A PX[3]; for (int i = 0; i < 3; ++i) { - PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] - + RowMajorAccess(P, 3, 4, i, 1) * X[1] - + RowMajorAccess(P, 3, 4, i, 2) * X[2] - + RowMajorAccess(P, 3, 4, i, 3) * X[3]; + PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] + + RowMajorAccess(P, 3, 4, i, 1) * X[1] + + RowMajorAccess(P, 3, 4, i, 2) * X[2] + + RowMajorAccess(P, 3, 4, i, 3) * X[3]; } if (PX[2] != 0.0) { x[0] = PX[0] / PX[2]; @@ -169,29 +122,31 @@ TEST(AdaptAutoDiff, AutoDiff) { Projective projective; // Make arguments - typedef Eigen::Matrix M; - M P; + typedef Eigen::Matrix RowMajorMatrix34; + RowMajorMatrix34 P; P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0; Vector4 X(10, 0, 5, 1); // Apply the mapping, to get image point b_x. Vector expected = Vector2(2, 1); Vector2 actual = projective(P, X); - EXPECT(assert_equal(expected,actual,1e-9)); + EXPECT(assert_equal(expected, actual, 1e-9)); // Get expected derivatives - Matrix E1 = numericalDerivative21(Projective(), P, X); - Matrix E2 = numericalDerivative22(Projective(), P, X); + Matrix E1 = numericalDerivative21( + Projective(), P, X); + Matrix E2 = numericalDerivative22( + Projective(), P, X); // Get derivatives with AutoDiff Vector2 actual2; MatrixRowMajor H1(2, 12), H2(2, 4); - double *parameters[] = { P.data(), X.data() }; - double *jacobians[] = { H1.data(), H2.data() }; - CHECK( - (AutoDiff::Differentiate( projective, parameters, 2, actual2.data(), jacobians))); - EXPECT(assert_equal(E1,H1,1e-8)); - EXPECT(assert_equal(E2,H2,1e-8)); + double* parameters[] = {P.data(), X.data()}; + double* jacobians[] = {H1.data(), H2.data()}; + CHECK((AutoDiff::Differentiate( + projective, parameters, 2, actual2.data(), jacobians))); + EXPECT(assert_equal(E1, H1, 1e-8)); + EXPECT(assert_equal(E2, H2, 1e-8)); } /* ************************************************************************* */ @@ -211,9 +166,11 @@ namespace example { Camera camera(Pose3(Rot3::rodriguez(0.1, 0.2, 0.3), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0)); Point3 point(10, 0, -5); // negative Z-axis convention of Snavely! -Vector9 P = Canonical::Local(camera); -Vector3 X = Canonical::Local(point); -Point2 expectedMeasurement(1.2431567, 1.2525694); +Vector9 P = Camera().localCoordinates(camera); +Vector3 X = Point3::Logmap(point); +Vector2 expectedMeasurement(1.2431567, 1.2525694); +Matrix E1 = numericalDerivative21(adapted, P, X); +Matrix E2 = numericalDerivative22(adapted, P, X); } /* ************************************************************************* */ @@ -234,11 +191,7 @@ TEST(AdaptAutoDiff, AutoDiff2) { // Apply the mapping, to get image point b_x. Vector2 actual = adapted(P, X); - EXPECT(assert_equal(expectedMeasurement.vector(), actual, 1e-6)); - - // Get expected derivatives - Matrix E1 = numericalDerivative21(adapted, P, X); - Matrix E2 = numericalDerivative22(adapted, P, X); + EXPECT(assert_equal(expectedMeasurement, actual, 1e-6)); // Instantiate function SnavelyProjection snavely; @@ -259,21 +212,17 @@ TEST(AdaptAutoDiff, AutoDiff2) { TEST(AdaptAutoDiff, AdaptAutoDiff) { using namespace example; - typedef AdaptAutoDiff Adaptor; + typedef AdaptAutoDiff Adaptor; Adaptor snavely; // Apply the mapping, to get image point b_x. - Point2 actual = snavely(camera, point); + Vector2 actual = snavely(P, X); EXPECT(assert_equal(expectedMeasurement, actual, 1e-6)); - // // Get expected derivatives - Matrix E1 = numericalDerivative21(Adaptor(), camera, point); - Matrix E2 = numericalDerivative22(Adaptor(), camera, point); - // Get derivatives with AutoDiff, not gives RowMajor results! Matrix29 H1; Matrix23 H2; - Point2 actual2 = snavely(camera, point, H1, H2); + Vector2 actual2 = snavely(P, X, H1, H2); EXPECT(assert_equal(expectedMeasurement, actual2, 1e-6)); EXPECT(assert_equal(E1, H1, 1e-8)); EXPECT(assert_equal(E2, H2, 1e-8)); @@ -282,15 +231,15 @@ TEST(AdaptAutoDiff, AdaptAutoDiff) { /* ************************************************************************* */ // Test AutoDiff wrapper in an expression TEST(AdaptAutoDiff, SnavelyExpression) { - Expression P(1); - Expression X(2); - typedef AdaptAutoDiff Adaptor; - Expression expression(Adaptor(), P, X); + Expression P(1); + Expression X(2); + typedef AdaptAutoDiff Adaptor; + Expression expression(Adaptor(), P, X); + EXPECT_LONGS_EQUAL( + sizeof(internal::BinaryExpression::Record), + expression.traceSize()); #ifdef GTSAM_USE_QUATERNIONS - EXPECT_LONGS_EQUAL(384,expression.traceSize()); // TODO(frank): should be zero -#else - EXPECT_LONGS_EQUAL(sizeof(internal::BinaryExpression::Record), - expression.traceSize()); // TODO(frank): should be zero + EXPECT_LONGS_EQUAL(336, expression.traceSize()); #endif set expected = list_of(1)(2); EXPECT(expected == expression.keys()); @@ -302,4 +251,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - From a305218bd93da95de2ced2cce1e8094b9cf7c1c7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jul 2015 19:14:25 -0700 Subject: [PATCH 08/11] Made output more directly comparable with ceres --- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 25 +++++++++++-------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 59bb2d42b..ace35e530 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -25,6 +25,9 @@ #include #include +#include +#include + #include #include #include @@ -236,7 +239,7 @@ void LevenbergMarquardtOptimizer::iterate() { // Keep increasing lambda until we make make progress while (true) { - + boost::timer::cpu_timer timer; if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "trying lambda = " << state_.lambda << endl; @@ -248,7 +251,7 @@ void LevenbergMarquardtOptimizer::iterate() { double modelFidelity = 0.0; bool step_is_successful = false; bool stopSearchingLambda = false; - double newError = numeric_limits::infinity(); + double newError = numeric_limits::infinity(), costChange; Values newValues; VectorValues delta; @@ -261,8 +264,6 @@ void LevenbergMarquardtOptimizer::iterate() { systemSolvedSuccessfully = false; } - double linearizedCostChange = 0, - newlinearizedError = 0; if (systemSolvedSuccessfully) { state_.reuseDiagonal = true; @@ -272,9 +273,9 @@ void LevenbergMarquardtOptimizer::iterate() { delta.print("delta"); // cost change in the linearized system (old - new) - newlinearizedError = linear->error(delta); + double newlinearizedError = linear->error(delta); - linearizedCostChange = state_.error - newlinearizedError; + double linearizedCostChange = state_.error - newlinearizedError; if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "newlinearizedError = " << newlinearizedError << " linearizedCostChange = " << linearizedCostChange << endl; @@ -299,7 +300,7 @@ void LevenbergMarquardtOptimizer::iterate() { << ") new (tentative) error (" << newError << ")" << endl; // cost change in the original, nonlinear system (old - new) - double costChange = state_.error - newError; + costChange = state_.error - newError; if (linearizedCostChange > 1e-20) { // the (linear) error has to decrease to satisfy this condition // fidelity of linearized model VS original system between @@ -322,9 +323,13 @@ void LevenbergMarquardtOptimizer::iterate() { } if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) { - cout << "[" << state_.iterations << "]: " << "new error = " << newlinearizedError - << ", delta = " << linearizedCostChange << ", lambda = " << state_.lambda - << ", success = " << systemSolvedSuccessfully << std::endl; + // do timing + double iterationTime = 1e-9 * timer.elapsed().wall; + if (state_.iterations == 0) + cout << "iter cost cost_change lambda success iter_time" << endl; + cout << boost::format("% 4d % 8e % 3.2e % 3.2e % 4d % 3.2e") % + state_.iterations % newError % costChange % state_.lambda % + systemSolvedSuccessfully % iterationTime << endl; } ++state_.totalNumberInnerIterations; From 0fd1ff7b26d52cea75e942454eafb32c7f851fe3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jul 2015 19:14:56 -0700 Subject: [PATCH 09/11] Now use Vector unknowns -> exactly same result as ceres... --- timing/timeSFMBAL.cpp | 37 ++++++++++++++++++++++++------------- 1 file changed, 24 insertions(+), 13 deletions(-) diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 763c22af0..fce535d14 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -45,6 +45,7 @@ typedef GeneralSFMFactor SfmFactor; #include #include #include +// See http://www.cs.cornell.edu/~snavely/bundler/bundler-v0.3-manual.html for conventions // Special version of Cal3Bundler so that default constructor = 0,0,0 struct CeresCalibration: public Cal3Bundler { CeresCalibration(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, @@ -76,14 +77,15 @@ int main(int argc, char* argv[]) { using symbol_shorthand::P; // Load BAL file (default is tiny) - string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre"); + //string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre"); + string defaultFilename = "/home/frank/problem-16-22106-pre.txt"; SfM_data db; bool success = readBAL(argc > 1 ? argv[1] : defaultFilename, db); if (!success) throw runtime_error("Could not access file!"); #ifndef USE_GTSAM_FACTOR - AdaptAutoDiff snavely; + AdaptAutoDiff snavely; #endif // Build graph @@ -96,11 +98,11 @@ int main(int argc, char* argv[]) { #ifdef USE_GTSAM_FACTOR graph.push_back(SfmFactor(z, unit2, i, P(j))); #else - Expression camera_(i); - Expression point_(P(j)); + Expression camera_(i); + Expression point_(P(j)); // Snavely expects measurements in OpenGL format, with y increasing upwards - graph.addExpressionFactor(unit2, Point2(z.x(), -z.y()), - Expression(snavely, camera_, point_)); + graph.addExpressionFactor(unit2, Vector2(z.x(), -z.y()), + Expression(snavely, camera_, point_)); #endif } } @@ -113,21 +115,31 @@ int main(int argc, char* argv[]) { #else // readBAL converts to GTSAM format, so we need to convert back ! Camera ceresCamera(gtsam2openGL(camera.pose()), camera.calibration()); - initial.insert((i++), ceresCamera); + Vector9 v9 = Camera().localCoordinates(ceresCamera); + initial.insert((i++), v9); #endif } - BOOST_FOREACH(const SfM_Track& track, db.tracks) + BOOST_FOREACH(const SfM_Track& track, db.tracks) { +#ifdef USE_GTSAM_FACTOR initial.insert(P(j++), track.p); +#else + Vector3 v3 = track.p.vector(); + initial.insert(P(j++), v3); +#endif + } // Check projection of first point in first camera Point2 expected = db.tracks.front().measurements.front().second; +#ifdef USE_GTSAM_FACTOR Camera camera = initial.at(0); Point3 point = initial.at(P(0)); -#ifdef USE_GTSAM_FACTOR Point2 actual = camera.project(point); #else - Point2 opengl = snavely(camera, point); - Point2 actual(opengl.x(), -opengl.y()); + Vector9 camera = initial.at(0); + Vector3 point = initial.at(P(0)); + Point2 z = snavely(camera, point); + // Again: fix y to increase upwards + Point2 actual(z.x(), -z.y()); #endif assert_equal(expected,actual,10); @@ -149,8 +161,7 @@ int main(int argc, char* argv[]) { LevenbergMarquardtParams params; LevenbergMarquardtParams::SetCeresDefaults(¶ms); params.setOrdering(ordering); - params.setVerbosity("ERROR"); - params.setVerbosityLM("TRYLAMBDA"); + params.setVerbosityLM("SUMMARY"); LevenbergMarquardtOptimizer lm(graph, initial, params); Values result = lm.optimize(); From b5a366e8f1f435d3491e4d61a9d5ffd18d1831e7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jul 2015 19:32:23 -0700 Subject: [PATCH 10/11] Fixed timing script --- gtsam/nonlinear/AdaptAutoDiff.h | 4 ++-- timing/timeAdaptAutoDiff.cpp | 15 +++++++++------ 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index 96ea9b182..ff059ef78 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -65,8 +65,8 @@ class AdaptAutoDiff { // Convert from row-major to columnn-major // TODO: if this is a bottleneck (probably not!) fix Autodiff to be // Column-Major - *H1 = Eigen::Map(rowMajor1); - *H2 = Eigen::Map(rowMajor2); + if (H1) *H1 = Eigen::Map(rowMajor1); + if (H2) *H2 = Eigen::Map(rowMajor2); } else { // Apply the mapping, to get result diff --git a/timing/timeAdaptAutoDiff.cpp b/timing/timeAdaptAutoDiff.cpp index edfd420ef..3a9b5297a 100644 --- a/timing/timeAdaptAutoDiff.cpp +++ b/timing/timeAdaptAutoDiff.cpp @@ -57,16 +57,19 @@ int main() { f1 = boost::make_shared >(z, model, 1, 2); time("GeneralSFMFactor : ", f1, values); - // AdaptAutoDiff - typedef AdaptAutoDiff AdaptedSnavely; - Point2_ expression(AdaptedSnavely(), camera, point); - f2 = boost::make_shared >(model, z, expression); - time("Point2_(AdaptedSnavely(), camera, point): ", f2, values); - // ExpressionFactor Point2_ expression2(camera, &Camera::project2, point); f3 = boost::make_shared >(model, z, expression2); time("Point2_(camera, &Camera::project, point): ", f3, values); + // AdaptAutoDiff + values.clear(); + values.insert(1,Vector9(Vector9::Zero())); + values.insert(2,Vector3(0,0,1)); + typedef AdaptAutoDiff AdaptedSnavely; + Expression expression(AdaptedSnavely(), Expression(1), Expression(2)); + f2 = boost::make_shared >(model, z.vector(), expression); + time("Point2_(AdaptedSnavely(), camera, point): ", f2, values); + return 0; } From bfa8fbac994af2b5c5c939587d152b4d4a3a7226 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jul 2015 23:39:51 -0700 Subject: [PATCH 11/11] Use retract so tests pass for both Rot3/Quaternion --- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 59f8cb7cd..377d6cd34 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -163,7 +163,7 @@ Vector2 adapted(const Vector9& P, const Vector3& X) { /* ************************************************************************* */ namespace example { -Camera camera(Pose3(Rot3::rodriguez(0.1, 0.2, 0.3), Point3(0, 5, 0)), +Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0)); Point3 point(10, 0, -5); // negative Z-axis convention of Snavely! Vector9 P = Camera().localCoordinates(camera);