From be37e1ed0540e403d4d51bd6f058379cefcf0cde Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Wed, 21 Jan 2015 13:02:35 -0500 Subject: [PATCH 1/5] Fix for Issue #201 It seems like MSVC was unable to identify the template specialization for the 'const' keyword. So added a specialization in each of these files for that --- gtsam/geometry/Cal3Bundler.h | 3 +++ gtsam/geometry/Cal3DS2.h | 3 +++ gtsam/geometry/Cal3Unified.h | 3 +++ gtsam/geometry/Cal3_S2.h | 3 +++ gtsam/geometry/Cal3_S2Stereo.h | 4 ++++ gtsam/geometry/CalibratedCamera.h | 3 +++ gtsam/geometry/EssentialMatrix.h | 3 +++ gtsam/geometry/PinholeCamera.h | 3 +++ gtsam/geometry/Point3.h | 3 +++ gtsam/geometry/Pose2.h | 3 +++ gtsam/geometry/Pose3.h | 4 ++++ gtsam/geometry/Rot2.h | 3 +++ gtsam/geometry/Rot3.h | 3 +++ gtsam/geometry/SO3.h | 3 ++- gtsam/geometry/StereoCamera.h | 2 ++ gtsam/geometry/StereoPoint2.h | 3 +++ gtsam/geometry/Unit3.h | 2 ++ gtsam/slam/dataset.cpp | 1 + 18 files changed, 51 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index ce9f94c48..e90ae32a4 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -174,4 +174,7 @@ private: template<> struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::Manifold {}; + } // namespace gtsam diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 0c77eebbc..0cb914ce3 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -112,5 +112,8 @@ private: template<> struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::Manifold {}; + } diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index d0e0f3891..624d7dbb4 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -142,5 +142,8 @@ private: template<> struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::Manifold {}; + } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 0c5386822..b9e2ef581 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -226,4 +226,7 @@ private: template<> struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::Manifold {}; + } // \ namespace gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 3585fb156..651a7fabb 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -158,4 +158,8 @@ namespace gtsam { struct traits : public internal::Manifold { }; + template<> + struct traits : public internal::Manifold { + }; + } // \ namespace gtsam diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index cda01b600..9e907f1d5 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -204,5 +204,8 @@ private: template<> struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::Manifold {}; + } diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index c9e702078..606f62f87 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -201,5 +201,8 @@ private: template<> struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::Manifold {}; + } // gtsam diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index b409c956d..12eb0235d 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -499,4 +499,7 @@ private: template struct traits< PinholeCamera > : public internal::Manifold > {}; +template +struct traits< const PinholeCamera > : public internal::Manifold > {}; + } // \ gtsam diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index c9dee9003..95f728e39 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -197,4 +197,7 @@ namespace gtsam { template<> struct traits : public internal::VectorSpace {}; + + template<> + struct traits : public internal::VectorSpace {}; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index be860107e..d4b647949 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -293,5 +293,8 @@ GTSAM_EXPORT boost::optional align(const std::vector& pairs); template<> struct traits : public internal::LieGroupTraits {}; +template<> +struct traits : public internal::LieGroupTraits {}; + } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d30bd4167..4130a252e 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -325,4 +325,8 @@ GTSAM_EXPORT boost::optional align(const std::vector& pairs); template<> struct traits : public internal::LieGroupTraits {}; +template<> +struct traits : public internal::LieGroupTraits {}; + + } // namespace gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 95f025622..deae1f3a0 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -210,4 +210,7 @@ namespace gtsam { template<> struct traits : public internal::LieGroupTraits {}; + template<> + struct traits : public internal::LieGroupTraits {}; + } // gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index e9568fb26..4e42d7efb 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -463,5 +463,8 @@ namespace gtsam { template<> struct traits : public internal::LieGroupTraits {}; + + template<> + struct traits : public internal::LieGroupTraits {}; } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index bed2f1212..e52eaae1e 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -85,6 +85,7 @@ public: template<> struct traits : public internal::LieGroupTraits {}; - +template<> +struct traits : public internal::LieGroupTraits {}; } // end namespace gtsam diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 49abcf36b..913b1eab3 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -149,4 +149,6 @@ private: template<> struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 2ec745fd8..803c59a4b 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -176,4 +176,7 @@ namespace gtsam { template<> struct traits : public internal::Manifold {}; + + template<> + struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 8784d0eb8..50ffb55b7 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -171,5 +171,7 @@ private: // Define GTSAM traits template <> struct traits : public internal::Manifold {}; +template <> struct traits : public internal::Manifold {}; + } // namespace gtsam diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index a6fb2d830..d3a7c1e84 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include From e1ae980d455945ec401af1ec762b74b4f8d4a2e7 Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Wed, 21 Jan 2015 13:16:13 -0500 Subject: [PATCH 2/5] Fix for #204 int to size_t conversions and few others --- examples/RangeISAMExample_plaza2.cpp | 3 ++- examples/TimeTBB.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 252372f39..04632e9e3 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -82,7 +82,8 @@ vector readTriples() { ifstream is(data_file.c_str()); while (is) { - double t, sender, receiver, range; + double t, sender, range; + size_t receiver; is >> t >> sender >> receiver >> range; triples.push_back(RangeTriple(t, receiver, range)); } diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp index 077331848..a35980836 100644 --- a/examples/TimeTBB.cpp +++ b/examples/TimeTBB.cpp @@ -134,7 +134,7 @@ map testWithMemoryAllocation() tbb::parallel_for(tbb::blocked_range(0, numberOfProblems), WorkerWithAllocation(results)); tbb::tick_count t1 = tbb::tick_count::now(); cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl; - timingResults[grainSize] = (t1 - t0).seconds(); + timingResults[(int)grainSize] = (t1 - t0).seconds(); } return timingResults; @@ -152,9 +152,9 @@ int main(int argc, char* argv[]) BOOST_FOREACH(size_t n, numThreads) { cout << "With " << n << " threads:" << endl; - tbb::task_scheduler_init init(n); - results[n].grainSizesWithoutAllocation = testWithoutMemoryAllocation(); - results[n].grainSizesWithAllocation = testWithMemoryAllocation(); + tbb::task_scheduler_init init((int)n); + results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation(); + results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation(); cout << endl; } From 5b917f55ba41702d2b12da0d259dc5484c0efa01 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 00:06:02 +0100 Subject: [PATCH 3/5] getPose with derivative (for expressions) --- gtsam/geometry/PinholeCamera.h | 11 ++++++++++- gtsam/geometry/tests/testPinholeCamera.cpp | 17 +++++++++++++++-- 2 files changed, 25 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 12eb0235d..46df47ecb 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -151,11 +151,20 @@ public: return pose_; } - /// return pose + /// return pose, constant version inline const Pose3& pose() const { return pose_; } + /// return pose, with derivative + inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const { + if (H) { + H->setZero(); + H->block(0, 0, 6, 6) = I_6x6; + } + return pose_; + } + /// return calibration inline Calibration& calibration() { return K_; diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 6cb84ec85..fb186259a 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -51,8 +51,21 @@ static const Point3 point4_inf( 0.16,-0.16, -1.0); /* ************************************************************************* */ TEST( PinholeCamera, constructor) { - EXPECT(assert_equal( camera.calibration(), K)); - EXPECT(assert_equal( camera.pose(), pose)); + EXPECT(assert_equal( K, camera.calibration())); + EXPECT(assert_equal( pose, camera.pose())); +} + +//****************************************************************************** +TEST(PinholeCamera, Pose) { + + Matrix actualH; + EXPECT(assert_equal(pose, camera.pose(actualH))); + + // Check derivative + boost::function f = // + boost::bind(&Camera::pose,_1,boost::none); + Matrix numericalH = numericalDerivative11(&Camera::getPose,camera); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); } /* ************************************************************************* */ From 12fb5fdf53d7e485d55ab9a79375e687d7527c94 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 00:06:12 +0100 Subject: [PATCH 4/5] Fix crash --- examples/SFMExample_bal.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 43177dc42..0e11adaed 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -43,7 +43,7 @@ int main (int argc, char* argv[]) { // Load the SfM data from file SfM_data mydata; - assert(readBAL(filename, mydata)); + readBAL(filename, mydata); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); // Create a factor graph From d79ccfed2146fbe216a8b3c7d781522bbc49a6d7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 00:06:29 +0100 Subject: [PATCH 5/5] New example reads BAL file, creates expressions --- examples/SFMExampleExpressions_bal.cpp | 121 +++++++++++++++++++++++++ 1 file changed, 121 insertions(+) create mode 100644 examples/SFMExampleExpressions_bal.cpp diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp new file mode 100644 index 000000000..66beefb35 --- /dev/null +++ b/examples/SFMExampleExpressions_bal.cpp @@ -0,0 +1,121 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SFMExampleExpressions_bal.cpp + * @brief A structure-from-motion example done with Expressions + * @author Frank Dellaert + * @date January 2015 + */ + +/** + * This is the Expression version of SFMExample + * See detailed description of headers there, this focuses on explaining the AD part + */ + +// The two new headers that allow using our Automatic Differentiation Expression framework +#include +#include + +// Header order is close to far +#include +#include +#include // for loading BAL datasets ! +#include + +using namespace std; +using namespace gtsam; +using namespace noiseModel; +using symbol_shorthand::C; +using symbol_shorthand::P; + +// An SfM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration +// and has a total of 9 free parameters + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Find default file, but if an argument is given, try loading a file + string filename = findExampleDataFile("dubrovnik-3-7-pre"); + if (argc > 1) + filename = string(argv[1]); + + // Load the SfM data from file + SfM_data mydata; + readBAL(filename, mydata); + cout + << boost::format("read %1% tracks on %2% cameras\n") + % mydata.number_tracks() % mydata.number_cameras(); + + // Create a factor graph + ExpressionFactorGraph graph; + + // Here we don't use a PriorFactor but directly the ExpressionFactor class + // First, we create an expression to the pose from the first camera + Expression camera0_(C(0)); + // Then, to get its pose: + Pose3_ pose0_(&SfM_Camera::getPose, camera0_); + // Finally, we say it should be equal to first guess + graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(), + noiseModel::Isotropic::Sigma(6, 0.1)); + + // similarly, we create a prior on the first point + Point3_ point0_(P(0)); + graph.addExpressionFactor(point0_, mydata.tracks[0].p, + noiseModel::Isotropic::Sigma(3, 0.1)); + + // We share *one* noiseModel between all projection factors + noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2, + 1.0); // one pixel in u and v + + // Simulated measurements from each camera pose, adding them to the factor graph + size_t j = 0; + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { + // Leaf expression for j^th point + Point3_ point_('p', j); + BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { + size_t i = m.first; + Point2 uv = m.second; + // Leaf expression for i^th camera + Expression camera_(C(i)); + // Below an expression for the prediction of the measurement: + Point2_ predict_ = project2(camera_, point_); + // Again, here we use an ExpressionFactor + graph.addExpressionFactor(predict_, uv, noise); + } + j += 1; + } + + // Create initial estimate + Values initial; + size_t i = 0; + j = 0; + BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) + initial.insert(C(i++), camera); + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) + initial.insert(P(j++), track.p); + + /* Optimize the graph and print results */ + Values result; + try { + LevenbergMarquardtParams params; + params.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer lm(graph, initial, params); + result = lm.optimize(); + } catch (exception& e) { + cout << e.what(); + } + cout << "final error: " << graph.error(result) << endl; + + return 0; +} +/* ************************************************************************* */ +