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; } diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index 341bb7d10..ff059ef78 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,97 +27,46 @@ 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 - *H1 = Eigen::Map(rowMajor1); - *H2 = Eigen::Map(rowMajor2); + // TODO: if this is a bottleneck (probably not!) fix Autodiff to be + // Column-Major + if (H1) *H1 = Eigen::Map(rowMajor1); + if (H2) *H2 = Eigen::Map(rowMajor2); } else { // Apply the mapping, to get result @@ -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/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; diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 3f477098a..377d6cd34 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,81 +36,43 @@ 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) : - 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()); } - Cal retract(const Vector& d) const { - return Cal(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 {}; +template <> +struct traits : public internal::Manifold {}; // With that, camera below behaves like Snavely's 9-dim vector -typedef PinholeCamera Camera; - +typedef PinholeCamera Camera; } using namespace std; using namespace gtsam; /* ************************************************************************* */ -// charts -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; - Canonical chart2; - EXPECT(assert_equal(v2, chart2.Local(Vector2(1, 0)))); - EXPECT(chart2.Retract(v2)==Vector2(1, 0)); - - Canonical chart3; - Eigen::Matrix v1; - v1 << 1; - EXPECT(chart3.Local(1)==v1); - EXPECT_DOUBLES_EQUAL(chart3.Retract(v1), 1, 1e-9); - - Canonical chart4; - Point3 point(1, 2, 3); - Vector v3(3); - v3 << 1, 2, 3; - EXPECT(assert_equal(v3, chart4.Local(point))); - EXPECT(assert_equal(chart4.Retract(v3), point)); - - 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)); - - Canonical chart6; - Cal cal0; - Vector z3 = Vector3::Zero(); - EXPECT(assert_equal(z3, chart6.Local(cal0))); - EXPECT(assert_equal(chart6.Retract(z3), cal0)); - - Canonical chart7; - Camera camera(Pose3(), cal0); - Vector z9 = Vector9::Zero(); - EXPECT(assert_equal(z9, chart7.Local(camera))); - EXPECT(assert_equal(chart7.Retract(z9), camera)); +// 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(); + Matrix3 actual; + ceres::AngleAxisToRotationMatrix(axisAngle.data(), actual.data()); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ // 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]; } @@ -124,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]; @@ -160,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)); } /* ************************************************************************* */ @@ -197,78 +161,85 @@ Vector2 adapted(const Vector9& P, const Vector3& X) { throw std::runtime_error("Snavely fail"); } +/* ************************************************************************* */ +namespace example { +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); +Vector3 X = Point3::Logmap(point); +Vector2 expectedMeasurement(1.2431567, 1.2525694); +Matrix E1 = numericalDerivative21(adapted, P, X); +Matrix E2 = numericalDerivative22(adapted, P, X); +} + +/* ************************************************************************* */ +// Check that Local worked as expected +TEST(AdaptAutoDiff, Local) { + using namespace example; + 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. + Vector2 actual = adapted(P, X); + EXPECT(assert_equal(expectedMeasurement, actual, 1e-6)); + // 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); - EXPECT(assert_equal(expected,actual,1e-9)); - - // Get expected derivatives - Matrix E1 = numericalDerivative21(adapted, P, X); - Matrix E2 = numericalDerivative22(adapted, P, X); - // 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; - // Make arguments - Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal(1, 0, 0)); - Point3 X(10, 0, -5); // negative Z-axis convention of Snavely! - - typedef AdaptAutoDiff Adaptor; + typedef AdaptAutoDiff Adaptor; Adaptor snavely; // Apply the mapping, to get image point b_x. - Point2 expected(2, 1); - Point2 actual = snavely(P, X); - EXPECT(assert_equal(expected,actual,1e-9)); - -// // Get expected derivatives - Matrix E1 = numericalDerivative21(Adaptor(), P, X); - Matrix E2 = numericalDerivative22(Adaptor(), P, X); + Vector2 actual = snavely(P, X); + EXPECT(assert_equal(expectedMeasurement, actual, 1e-6)); // Get derivatives with AutoDiff, not gives RowMajor results! Matrix29 H1; Matrix23 H2; - Point2 actual2 = snavely(P, X, H1, H2); - EXPECT(assert_equal(expected,actual2,1e-9)); - EXPECT(assert_equal(E1,H1,1e-8)); - EXPECT(assert_equal(E2,H2,1e-8)); + 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)); } /* ************************************************************************* */ // Test AutoDiff wrapper in an expression -TEST(AdaptAutoDiff, Snavely) { - Expression P(1); - Expression X(2); - typedef AdaptAutoDiff Adaptor; - Expression expression(Adaptor(), P, X); +TEST(AdaptAutoDiff, SnavelyExpression) { + 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, should be zero -#else - EXPECT_LONGS_EQUAL(sizeof(internal::BinaryExpression::Record), - expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(336, expression.traceSize()); #endif set expected = list_of(1)(2); EXPECT(expected == expression.keys()); @@ -280,4 +251,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - 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; } diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 68b3c4932..fce535d14 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 @@ -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 @@ -92,14 +94,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_)); + Expression camera_(i); + Expression point_(P(j)); + // Snavely expects measurements in OpenGL format, with y increasing upwards + graph.addExpressionFactor(unit2, Vector2(z.x(), -z.y()), + Expression(snavely, camera_, point_)); #endif } } @@ -110,21 +113,33 @@ int main(int argc, char* argv[]) { #ifdef USE_GTSAM_FACTOR initial.insert((i++), camera); #else - Camera ceresCamera(camera.pose(), camera.calibration()); - initial.insert((i++), ceresCamera); + // readBAL converts to GTSAM format, so we need to convert back ! + Camera ceresCamera(gtsam2openGL(camera.pose()), camera.calibration()); + 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 + // 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 actual = snavely(camera, point); + 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); @@ -146,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();