From 63d7d7c54bfd6319e8dcfaa43186c31924aedf29 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 28 Nov 2018 16:01:54 -0800 Subject: [PATCH 01/35] Attempt to fix GenericValue assignment operator for windows --- gtsam/base/GenericValue.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 67cc2646e..52899fe45 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -164,11 +164,11 @@ public: protected: - // implicit assignment operator for (const GenericValue& rhs) works fine here /// Assignment operator, protected because only the Value or DERIVED /// assignment operators should be used. GenericValue& operator=(const GenericValue& rhs) { - // Nothing to do, do not call base class assignment operator + Value::operator=(static_cast(rhs)); + value_ = rhs.value_; return *this; } From ab0fb0d4a50dc43f212a32efe54c6ee4242f7a47 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 18 Dec 2018 13:56:38 -0500 Subject: [PATCH 02/35] Added iSAM2 test to check issue #412 --- tests/testVisualISAM2.cpp | 125 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 125 insertions(+) create mode 100644 tests/testVisualISAM2.cpp diff --git a/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp new file mode 100644 index 000000000..cd04c4b31 --- /dev/null +++ b/tests/testVisualISAM2.cpp @@ -0,0 +1,125 @@ +/* ---------------------------------------------------------------------------- + + * 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 testVisualISAM2.cpp + * @brief Test convergence of visualSLAM example. + * @author Duy-Nguyen Ta + * @author Frank Dellaert + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(testVisualISAM2, all) +{ + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); + + vector points = createPoints(); + + // Create the set of ground-truth poses + vector poses = createPoses(); + + // Set the parameters + ISAM2Params parameters; + parameters.relinearizeThreshold = 0.01; + parameters.relinearizeSkip = 1; + ISAM2 isam(parameters); + + // Create a Factor Graph and Values to hold the new data + NonlinearFactorGraph graph; + Values initialEstimate; + + // Loop over the poses, adding the observations to iSAM incrementally + for (size_t i = 0; i < poses.size(); ++i) + { + // Add factors for each landmark observation + for (size_t j = 0; j < points.size(); ++j) + { + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + graph.emplace_shared>( + measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); + } + + // Add an initial guess for the current pose + // Intentionally initialize the variables off from the ground truth + static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), + Point3(0.05, -0.10, 0.20)); + initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose); + + // Treat first iteration as special case + if (i == 0) + { + // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw + static auto kPosePrior = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)) + .finished()); + graph.emplace_shared>(Symbol('x', 0), poses[0], + kPosePrior); + + // Add a prior on landmark l0 + static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1); + graph.emplace_shared>(Symbol('l', 0), points[0], + kPointPrior); + + // Add initial guesses to all observed landmarks + // Intentionally initialize the variables off from the ground truth + static Point3 kDeltaPoint(-0.25, 0.20, 0.15); + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(Symbol('l', j), points[j] + kDeltaPoint); + } + else + { + // Update iSAM with the new factors + isam.update(graph, initialEstimate); + + // Optimize + Values currentEstimate = isam.calculateEstimate(); + + // reset for next iteration + graph.resize(0); + initialEstimate.clear(); + } + } // for loop + + auto result = isam.calculateEstimate(); + EXPECT_LONGS_EQUAL(16, result.size()); + for (size_t j = 0; j < points.size(); ++j) + { + Symbol key('l', j); + EXPECT(assert_equal(points[j], result.at(key), 0.01)); + } +} + +/* ************************************************************************* */ +int main() +{ + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 77f3a9195c4eb990b0b58dd12fb492b2ede9b3da Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 18 Dec 2018 14:22:44 -0500 Subject: [PATCH 03/35] Added an iSAM2 convergence test. --- tests/testVisualISAM2.cpp | 127 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 127 insertions(+) create mode 100644 tests/testVisualISAM2.cpp diff --git a/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp new file mode 100644 index 000000000..1ced2af23 --- /dev/null +++ b/tests/testVisualISAM2.cpp @@ -0,0 +1,127 @@ +/* ---------------------------------------------------------------------------- + + * 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 testVisualISAM2.cpp + * @brief Test convergence of visualSLAM example. + * @author Duy-Nguyen Ta + * @author Frank Dellaert + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(testVisualISAM2, all) +{ + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); + + // Create ground truth data + vector points = createPoints(); + vector poses = createPoses(); + + // Set the parameters + ISAM2Params parameters; + parameters.relinearizeThreshold = 0.01; + parameters.relinearizeSkip = 1; + ISAM2 isam(parameters); + + // Create a Factor Graph and Values to hold the new data + NonlinearFactorGraph graph; + Values initialEstimate; + + // Loop over the poses, adding the observations to iSAM incrementally + for (size_t i = 0; i < poses.size(); ++i) + { + // Add factors for each landmark observation + for (size_t j = 0; j < points.size(); ++j) + { + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + graph.emplace_shared>( + measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); + } + + // Add an initial guess for the current pose + // Intentionally initialize the variables off from the ground truth + static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), + Point3(0.05, -0.10, 0.20)); + initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose); + + // Treat first iteration as special case + if (i == 0) + { + // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw + static auto kPosePrior = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)) + .finished()); + graph.emplace_shared>(Symbol('x', 0), poses[0], + kPosePrior); + + // Add a prior on landmark l0 + static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1); + graph.emplace_shared>(Symbol('l', 0), points[0], + kPointPrior); + + // Add initial guesses to all observed landmarks + // Intentionally initialize the variables off from the ground truth + static Point3 kDeltaPoint(-0.25, 0.20, 0.15); + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(Symbol('l', j), points[j] + kDeltaPoint); + } + else + { + // Update iSAM with the new factors + isam.update(graph, initialEstimate); + + // Do an extra update to converge withing these 8 iterations + isam.update(); + + // Optimize + Values currentEstimate = isam.calculateEstimate(); + + // reset for next iteration + graph.resize(0); + initialEstimate.clear(); + } + } // for loop + + auto result = isam.calculateEstimate(); + EXPECT_LONGS_EQUAL(16, result.size()); + for (size_t j = 0; j < points.size(); ++j) + { + Symbol key('l', j); + EXPECT(assert_equal(points[j], result.at(key), 0.01)); + } +} + +/* ************************************************************************* */ +int main() +{ + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 1468250a0d51d8e0c4d6f2a9b0c833c8082c8ee2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 8 Nov 2018 22:09:05 -0500 Subject: [PATCH 04/35] Added ccache support as shown in Issue #390 --- CMakeLists.txt | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index d471e3fd6..fd35d9b18 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -193,6 +193,13 @@ endif() # Find Google perftools find_package(GooglePerfTools) +############################################################################### +# Support ccache, if installed +find_program(CCACHE_FOUND ccache) +if(CCACHE_FOUND) + set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) + set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) +endif(CCACHE_FOUND) ############################################################################### # Find MKL From f17b12bbfd5f6ddd0f7c1f51de26612bb9cc9724 Mon Sep 17 00:00:00 2001 From: cbeall Date: Tue, 18 Dec 2018 15:03:33 -0800 Subject: [PATCH 05/35] Add cmake flag to toggle use of ccache. On by default. --- CMakeLists.txt | 31 +++++++++++++++++++++++++------ 1 file changed, 25 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fd35d9b18..5930742ea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,6 @@ project(GTSAM CXX C) -cmake_minimum_required(VERSION 2.6) +cmake_minimum_required(VERSION 2.8.4) # new feature to Cmake Version > 2.8.12 # Mac ONLY. Define Relative Path on Mac OS @@ -69,6 +69,9 @@ option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions depreca option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) +if(NOT MSVC AND NOT XCODE_VERSION) + option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON) +endif() # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries @@ -195,11 +198,18 @@ find_package(GooglePerfTools) ############################################################################### # Support ccache, if installed -find_program(CCACHE_FOUND ccache) -if(CCACHE_FOUND) - set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) - set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) -endif(CCACHE_FOUND) +if(NOT MSVC AND NOT XCODE_VERSION) + find_program(CCACHE_FOUND ccache) + if(CCACHE_FOUND) + if(GTSAM_BUILD_WITH_CCACHE) + set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) + set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) + else() + set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "") + set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK "") + endif() + endif(CCACHE_FOUND) +endif() ############################################################################### # Find MKL @@ -529,6 +539,15 @@ else() endif() message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}") +if(NOT MSVC AND NOT XCODE_VERSION) + if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE) + message(STATUS " Build with ccache : Yes") + elseif(CCACHE_FOUND) + message(STATUS " Build with ccache : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled") + else() + message(STATUS " Build with ccache : No") + endif() +endif() message(STATUS "Packaging flags ") message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}") From f4d1fec558b024f690dc28ef083e74fa5c6d0ea8 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 19 Dec 2018 11:08:52 -0800 Subject: [PATCH 06/35] Fix compiler error in GenericGraph when GTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF --- gtsam_unstable/partition/GenericGraph.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp index 7200f6168..f941407e9 100644 --- a/gtsam_unstable/partition/GenericGraph.cpp +++ b/gtsam_unstable/partition/GenericGraph.cpp @@ -47,15 +47,15 @@ namespace gtsam { namespace partition { toErase.push_back(itFactor); nrFactors--; continue; } - size_t label1 = dsf.findSet(key1.index); - size_t label2 = dsf.findSet(key2.index); + size_t label1 = dsf.find(key1.index); + size_t label2 = dsf.find(key2.index); if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } // merge two trees if the connection is strong enough, otherwise cache it // an odometry factor always merges two islands if (key1.type == NODE_POSE_2D && key2.type == NODE_POSE_2D) { toErase.push_back(itFactor); nrFactors--; - dsf.makeUnionInPlace(label1, label2); + dsf.merge(label1, label2); succeed = true; break; } @@ -64,7 +64,7 @@ namespace gtsam { namespace partition { if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) || (dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) { toErase.push_back(itFactor); nrFactors--; - dsf.makeUnionInPlace(label1, label2); + dsf.merge(label1, label2); succeed = true; break; } @@ -87,7 +87,7 @@ namespace gtsam { namespace partition { } else { toErase.push_back(itFactor); nrFactors--; toErase.push_back(itCached->second); nrFactors--; - dsf.makeUnionInPlace(label1, label2); + dsf.merge(label1, label2); connections.erase(itCached); succeed = true; break; @@ -150,8 +150,8 @@ namespace gtsam { namespace partition { } if (graph.size() == 178765) cout << "kai22" << endl; - size_t label1 = dsf.findSet(key1.index); - size_t label2 = dsf.findSet(key2.index); + size_t label1 = dsf.find(key1.index); + size_t label2 = dsf.find(key2.index); if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } if (graph.size() == 178765) cout << "kai23" << endl; @@ -160,7 +160,7 @@ namespace gtsam { namespace partition { if ((key1.type == NODE_POSE_3D && key2.type == NODE_LANDMARK_3D) || (key1.type == NODE_POSE_3D && key2.type == NODE_POSE_3D)) { toErase.push_back(itFactor); nrFactors--; - dsf.makeUnionInPlace(label1, label2); + dsf.merge(label1, label2); succeed = true; break; } From 617040f503d8f09f73e6c60f13da8e0b75021d52 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Dec 2018 17:09:14 -0500 Subject: [PATCH 07/35] count method (better than filter, then size). --- gtsam/nonlinear/Values.h | 11 ++++++++ gtsam/nonlinear/tests/testValues.cpp | 1 + gtsam/slam/dataset.cpp | 39 ++++++++++++++-------------- 3 files changed, 31 insertions(+), 20 deletions(-) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index bf17f1f0d..676d30787 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -385,6 +385,17 @@ namespace gtsam { ConstFiltered filter(const boost::function& filterFcn = &_truePredicate) const; + // Count values of given type \c ValueType + template + bool count() const { + size_t i = 0; + for (const auto& key_value : *this) { + if (dynamic_cast*>(&key_value.value)) + ++i; + } + return i; + } + private: // Filters based on ValueType (if not Value) and also based on the user- // supplied \c filter function. diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index bcf01eff5..0dee52570 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -383,6 +383,7 @@ TEST(Values, filter) { ++ i; } EXPECT_LONGS_EQUAL(2, (long)i); + EXPECT_LONGS_EQUAL(2, (long)values.count()); // construct a values with the view Values actualSubValues2(pose_filtered); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index ba51864f1..4f52f3c40 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -432,14 +432,14 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, fstream stream(filename.c_str(), fstream::out); // save 2D & 3D poses - Values::ConstFiltered viewPose2 = estimate.filter(); - for(const Values::ConstFiltered::KeyValuePair& key_value: viewPose2) { + const auto viewPose2 = estimate.filter(); + for(const auto& key_value: viewPose2) { stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " " << key_value.value.y() << " " << key_value.value.theta() << endl; } - Values::ConstFiltered viewPose3 = estimate.filter(); - for(const Values::ConstFiltered::KeyValuePair& key_value: viewPose3) { + const auto viewPose3 = estimate.filter(); + for(const auto& key_value: viewPose3) { Point3 p = key_value.value.translation(); Rot3 R = key_value.value.rotation(); stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " " << p.y() << " " << p.z() @@ -448,7 +448,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, } // save edges (2D or 3D) - for(boost::shared_ptr factor_: graph) { + for(const auto& factor_: graph) { boost::shared_ptr > factor = boost::dynamic_pointer_cast >(factor_); if (factor){ @@ -857,48 +857,47 @@ bool writeBAL(const string& filename, SfM_data &data) { bool writeBALfromValues(const string& filename, const SfM_data &data, Values& values) { - + using Camera = PinholeCamera; SfM_data dataValues = data; // Store poses or cameras in SfM_data - Values valuesPoses = values.filter(); - if (valuesPoses.size() == dataValues.number_cameras()) { // we only estimated camera poses + size_t nrPoses = values.count(); + if (nrPoses == dataValues.number_cameras()) { // we only estimated camera poses for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera Key poseKey = symbol('x', i); Pose3 pose = values.at(poseKey); Cal3Bundler K = dataValues.cameras[i].calibration(); - PinholeCamera camera(pose, K); + Camera camera(pose, K); dataValues.cameras[i] = camera; } } else { - Values valuesCameras = values.filter >(); - if (valuesCameras.size() == dataValues.number_cameras()) { // we only estimated camera poses and calibration - for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera + size_t nrCameras = values.count(); + if (nrCameras == dataValues.number_cameras()) { // we only estimated camera poses and calibration + for (size_t i = 0; i < nrCameras; i++) { // for each camera Key cameraKey = i; // symbol('c',i); - PinholeCamera camera = - values.at >(cameraKey); + Camera camera = values.at(cameraKey); dataValues.cameras[i] = camera; } } else { cout << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " << dataValues.number_cameras() << ") and values (#cameras " - << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" + << nrPoses << ", #poses " << nrCameras << ")!!" << endl; return false; } } // Store 3D points in SfM_data - Values valuesPoints = values.filter(); - if (valuesPoints.size() != dataValues.number_tracks()) { + size_t nrPoints = values.count(), nrTracks = dataValues.number_tracks(); + if (nrPoints != nrTracks) { cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " - << dataValues.number_tracks() << ") and values (#points " - << valuesPoints.size() << ")!!" << endl; + << nrTracks << ") and values (#points " + << nrPoints << ")!!" << endl; } - for (size_t j = 0; j < dataValues.number_tracks(); j++) { // for each point + for (size_t j = 0; j < nrTracks; j++) { // for each point Key pointKey = P(j); if (values.exists(pointKey)) { Point3 point = values.at(pointKey); From 94010aee9d4507ee76ee86c0e6464c32f4811c1a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Dec 2018 17:10:25 -0500 Subject: [PATCH 08/35] Removed redundant (and troublesome on Windows) assignment. --- gtsam/base/GenericValue.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 52899fe45..1ee1a27ba 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -168,7 +168,6 @@ public: /// assignment operators should be used. GenericValue& operator=(const GenericValue& rhs) { Value::operator=(static_cast(rhs)); - value_ = rhs.value_; return *this; } From e2363e90bd8cdec9d2ef052a23594040502069b1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 22 Dec 2018 18:07:00 -0500 Subject: [PATCH 09/35] Fixed issue with 'count' --- gtsam/nonlinear/Values.h | 2 +- gtsam/nonlinear/tests/testValues.cpp | 1 + gtsam/slam/dataset.cpp | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 676d30787..16f8eba16 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -387,7 +387,7 @@ namespace gtsam { // Count values of given type \c ValueType template - bool count() const { + size_t count() const { size_t i = 0; for (const auto& key_value : *this) { if (dynamic_cast*>(&key_value.value)) diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 0dee52570..b3c557b32 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -384,6 +384,7 @@ TEST(Values, filter) { } EXPECT_LONGS_EQUAL(2, (long)i); EXPECT_LONGS_EQUAL(2, (long)values.count()); + EXPECT_LONGS_EQUAL(2, (long)values.count()); // construct a values with the view Values actualSubValues2(pose_filtered); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4f52f3c40..fbb198265 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -880,7 +880,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, } } else { cout - << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " + << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras " << dataValues.number_cameras() << ") and values (#cameras " << nrPoses << ", #poses " << nrCameras << ")!!" << endl; From 70e2534cc283e4ced31aa0af276fcd59adaa8a8d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 22 Dec 2018 18:07:22 -0500 Subject: [PATCH 10/35] Restored assignment --- gtsam/base/GenericValue.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 1ee1a27ba..52899fe45 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -168,6 +168,7 @@ public: /// assignment operators should be used. GenericValue& operator=(const GenericValue& rhs) { Value::operator=(static_cast(rhs)); + value_ = rhs.value_; return *this; } From 6d0a76aec980ccad2bd5be535466857723a0a8c8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 29 Dec 2018 15:11:24 -0500 Subject: [PATCH 11/35] Got rid of filter because of compile issues on Windows --- gtsam/slam/dataset.cpp | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index fbb198265..6efd01feb 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -432,17 +432,21 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, fstream stream(filename.c_str(), fstream::out); // save 2D & 3D poses - const auto viewPose2 = estimate.filter(); - for(const auto& key_value: viewPose2) { - stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " " - << key_value.value.y() << " " << key_value.value.theta() << endl; + for (const auto& key_value : estimate) { + auto p = dynamic_cast*>(&key_value.value); + if (!p) continue; + const Pose2& pose = p->value(); + stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " + << pose.y() << " " << pose.theta() << endl; } - const auto viewPose3 = estimate.filter(); - for(const auto& key_value: viewPose3) { - Point3 p = key_value.value.translation(); - Rot3 R = key_value.value.rotation(); - stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " " << p.y() << " " << p.z() + for(const auto& key_value: estimate) { + auto p = dynamic_cast*>(&key_value.value); + if (!p) continue; + const Pose3& pose = p->value(); + Point3 t = pose.translation(); + Rot3 R = pose.rotation(); + stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " " << t.y() << " " << t.z() << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w() << endl; } From 2166dc23fe300ff07d7b258a6399c35f419e6a00 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Dec 2018 13:01:43 -0500 Subject: [PATCH 12/35] Added virtual destructors to avoid warnings (on Mac) and fixed some lint warnings. --- gtsam/navigation/PreintegrationBase.h | 47 +++++++++++++++++---------- gtsam/navigation/Scenario.h | 11 ++++--- 2 files changed, 36 insertions(+), 22 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 3c22a1d00..e0792f873 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -27,6 +27,8 @@ #include #include +#include +#include namespace gtsam { @@ -61,7 +63,6 @@ class GTSAM_EXPORT PreintegrationBase { typedef PreintegrationParams Params; protected: - /// Parameters. Declared mutable only for deprecated predict method. /// TODO(frank): make const once deprecated method is removed #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 @@ -78,7 +79,10 @@ class GTSAM_EXPORT PreintegrationBase { /// Default constructor for serialization PreintegrationBase() {} -public: + /// Virtual destructor for serialization + virtual ~PreintegrationBase() {} + + public: /// @name Constructors /// @{ @@ -95,7 +99,7 @@ public: /// @name Basic utilities /// @{ /// Re-initialize PreintegratedMeasurements - virtual void resetIntegration()=0; + virtual void resetIntegration() = 0; /// @name Basic utilities /// @{ @@ -129,10 +133,10 @@ public: const imuBias::ConstantBias& biasHat() const { return biasHat_; } double deltaTij() const { return deltaTij_; } - virtual Vector3 deltaPij() const=0; - virtual Vector3 deltaVij() const=0; - virtual Rot3 deltaRij() const=0; - virtual NavState deltaXij() const=0; + virtual Vector3 deltaPij() const = 0; + virtual Vector3 deltaVij() const = 0; + virtual Rot3 deltaRij() const = 0; + virtual NavState deltaXij() const = 0; // Exposed for MATLAB Vector6 biasHatVector() const { return biasHat_.vector(); } @@ -147,20 +151,24 @@ public: /// @name Main functionality /// @{ - /// Subtract estimate and correct for sensor pose - /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc) - /// Ignore D_correctedOmega_measuredAcc as it is trivially zero + /** + * Subtract estimate and correct for sensor pose + * Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc) + * Ignore D_correctedOmega_measuredAcc as it is trivially zero + */ std::pair correctMeasurementsBySensorPose( const Vector3& unbiasedAcc, const Vector3& unbiasedOmega, OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none, OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none, OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const; - /// Update preintegrated measurements and get derivatives - /// It takes measured quantities in the j frame - /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + /** + * Update preintegrated measurements and get derivatives + * It takes measured quantities in the j frame + * Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + */ virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega, - const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0; + const double dt, Matrix9* A, Matrix93* B, Matrix93* C) = 0; /// Version without derivatives virtual void integrateMeasurement(const Vector3& measuredAcc, @@ -169,7 +177,7 @@ public: /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 6> H = boost::none) const=0; + OptionalJacobian<9, 6> H = boost::none) const = 0; /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, @@ -182,7 +190,10 @@ public: OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, OptionalJacobian<9, 6> H3) const; - /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j + /** + * Compute errors w.r.t. preintegrated measurements and jacobians + * wrt pose_i, vel_i, bias_i, pose_j, bias_j + */ Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 = @@ -202,8 +213,8 @@ public: /// @} #endif -public: + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} /// namespace gtsam +} /// namespace gtsam diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index acb8f46f5..fff7e7e50 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -24,12 +24,15 @@ namespace gtsam { /// Simple trajectory simulator. class Scenario { public: + /// virtual destructor + virtual ~Scenario() {} + // Quantities a Scenario needs to specify: - virtual Pose3 pose(double t) const = 0; - virtual Vector3 omega_b(double t) const = 0; - virtual Vector3 velocity_n(double t) const = 0; - virtual Vector3 acceleration_n(double t) const = 0; + virtual Pose3 pose(double t) const = 0; ///< pose at time t + virtual Vector3 omega_b(double t) const = 0; ///< angular velocity in body frame + virtual Vector3 velocity_n(double t) const = 0; ///< velocity at time t, in nav frame + virtual Vector3 acceleration_n(double t) const = 0; ///< acceleration in nav frame // Derived quantities: From 66959f8423cabd8be4ffc4b0fae2157f4056e773 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Dec 2018 14:34:06 -0500 Subject: [PATCH 13/35] Added noise model to make test succeed --- matlab/gtsam_tests/testJacobianFactor.m | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/matlab/gtsam_tests/testJacobianFactor.m b/matlab/gtsam_tests/testJacobianFactor.m index bba6ca5ac..1c214c3bc 100644 --- a/matlab/gtsam_tests/testJacobianFactor.m +++ b/matlab/gtsam_tests/testJacobianFactor.m @@ -54,7 +54,8 @@ S13 = [ +0.00,-8.94427 ]; d=[2.23607;-1.56525]; -expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13); +unit2 = noiseModel.Unit.Create(2); +expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13,unit2); % check if the result matches CHECK('actualCG.equals(expectedCG,1e-5)',actualCG.equals(expectedCG,1e-4)); From 2c8f55a2d8ff6a54dae83490b63e6a251000de44 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Dec 2018 14:34:55 -0500 Subject: [PATCH 14/35] Adding GenericValue for Values serialization, as suggested by Callum Robinson and Mike Sheffler in issue #398 --- gtsam.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam.h b/gtsam.h index 24a717c3c..01b23e6cc 100644 --- a/gtsam.h +++ b/gtsam.h @@ -228,6 +228,12 @@ virtual class Value { size_t dim() const; }; +#include +template +virtual class GenericValue : gtsam::Value { + void serializable() const; +}; + #include class LieScalar { // Standard constructors From cdf2a6335b288e4282f74855039dfe5020e9e4a9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Dec 2018 16:23:43 -0500 Subject: [PATCH 15/35] Added serialization for two more factors --- gtsam.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam.h b/gtsam.h index 01b23e6cc..d681ddb0d 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2265,6 +2265,9 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor { template virtual class RangeFactor : gtsam::NoiseModelFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); + + // enabling serialization functionality + void serialize() const; }; typedef gtsam::RangeFactor RangeFactorPosePoint2; @@ -2281,6 +2284,9 @@ typedef gtsam::RangeFactor RangeFactor template virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor); + + // enabling serialization functionality + void serialize() const; }; typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPosePoint2; From fe1daec086aa5b20966d4ab9615be137e665adc9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Dec 2018 16:24:19 -0500 Subject: [PATCH 16/35] Changed include error as in http://boost.2283326.n4.nabble.com/boost-serialization-Serializing-Dynamically-Loaded-Libraries-quot-Unregistered-Void-Cast-quot-td2570981.html --- gtsam/base/serialization.h | 2 +- wrap/Module.cpp | 4 ++-- wrap/tests/expected/geometry_wrapper.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index ebd893ad1..954d3e86b 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -24,7 +24,6 @@ #include // includes for standard serialization types -#include #include #include #include @@ -39,6 +38,7 @@ #include #include #include +#include namespace gtsam { diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 9eee686cb..5cf3b5d6c 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -273,9 +273,9 @@ void Module::generate_matlab_wrapper(const string& toolboxPath) const { // Include boost.serialization archive headers before other class headers if (hasSerialiable) { - wrapperFile.oss << "#include \n"; wrapperFile.oss << "#include \n"; - wrapperFile.oss << "#include \n\n"; + wrapperFile.oss << "#include \n"; + wrapperFile.oss << "#include \n\n"; } // Generate includes while avoiding redundant includes diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index 7e0cb0e47..dec78b80c 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -1,9 +1,9 @@ #include #include -#include #include #include +#include #include #include From 63acd1a50c9362190d351279c671d76cd9d82f80 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Dec 2018 18:35:33 -0500 Subject: [PATCH 17/35] Add bearing and range factor tests --- matlab/gtsam_tests/testSerialization.m | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/matlab/gtsam_tests/testSerialization.m b/matlab/gtsam_tests/testSerialization.m index 9f49328cd..baacb198b 100644 --- a/matlab/gtsam_tests/testSerialization.m +++ b/matlab/gtsam_tests/testSerialization.m @@ -52,14 +52,26 @@ priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph -% Between Factors - FAIL: unregistered class +% Between Factors odometry = Pose2(2.0, 0.0, 0.0); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); -% BearingRange Factors - FAIL: unregistered class +% Range Factors +rNoise = noiseModel.Diagonal.Sigmas([0.2]); +graph.add(RangeFactorPosePoint2(i1, j1, sqrt(4+4), rNoise)); +graph.add(RangeFactorPosePoint2(i2, j1, 2, rNoise)); +graph.add(RangeFactorPosePoint2(i3, j2, 2, rNoise)); + +% Bearing Factors degrees = pi/180; +bNoise = noiseModel.Diagonal.Sigmas([0.1]); +graph.add(BearingFactor2D(i1, j1, Rot2(45*degrees), bNoise)); +graph.add(BearingFactor2D(i2, j1, Rot2(90*degrees), bNoise)); +graph.add(BearingFactor2D(i3, j2, Rot2(90*degrees), bNoise)); + +% BearingRange Factors brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); From 3b3e39381712d69f3252a27af6fa0da65acec68b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Dec 2018 18:37:38 -0500 Subject: [PATCH 18/35] Add serialization functions that call base classes, to avoid "unregistered void cast" errors (in MATLAB, as flagged by issue #398), as mentioned in http://tb-nguyen.blogspot.com/2009/08/more-on-using-boost-serialization-and.html --- gtsam/nonlinear/ExpressionFactor.h | 7 +++++++ gtsam/sam/BearingFactor.h | 8 ++++++++ gtsam/sam/BearingRangeFactor.h | 8 ++++++++ gtsam/sam/RangeFactor.h | 8 ++++++++ 4 files changed, 31 insertions(+) diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 34ba8e1ff..04d82fe9a 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -266,6 +266,13 @@ class ExpressionFactor2 : public ExpressionFactor { virtual Expression expression() const { return expression(this->keys_[0], this->keys_[1]); } + + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "ExpressionFactor", boost::serialization::base_object >(*this)); + } }; // ExpressionFactor2 diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h index f190e683c..a9ed5ef4b 100644 --- a/gtsam/sam/BearingFactor.h +++ b/gtsam/sam/BearingFactor.h @@ -60,6 +60,14 @@ struct BearingFactor : public ExpressionFactor2 { std::cout << s << "BearingFactor" << std::endl; Base::print(s, kf); } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + } }; // BearingFactor /// traits diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index 2dd1fecb8..44740f8ff 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -73,6 +73,14 @@ class BearingRangeFactor Base::print(s, kf); } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + } }; // BearingRangeFactor /// traits diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h index a5bcac822..40a9cf758 100644 --- a/gtsam/sam/RangeFactor.h +++ b/gtsam/sam/RangeFactor.h @@ -65,6 +65,14 @@ class RangeFactor : public ExpressionFactor2 { std::cout << s << "RangeFactor" << std::endl; Base::print(s, kf); } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + } }; // \ RangeFactor /// traits From e1e8de7cedf78b1a69dd459113db868a11fff7a7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Dec 2018 10:24:48 -0500 Subject: [PATCH 19/35] Fixed issue with GTSAM 4 deprecated retract --- matlab/gtsam_examples/SFMExample.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/matlab/gtsam_examples/SFMExample.m b/matlab/gtsam_examples/SFMExample.m index 4115fa6e3..6700e90d2 100644 --- a/matlab/gtsam_examples/SFMExample.m +++ b/matlab/gtsam_examples/SFMExample.m @@ -58,7 +58,7 @@ for i=1:size(truth.cameras,2) initialEstimate.insert(symbol('x',i), pose_i); end for j=1:size(truth.points,2) - point_j = truth.points{j}.retract(0.1*randn(3,1)); + point_j = Point3(truth.points{j}.vector() + 0.1*randn(3,1)); initialEstimate.insert(symbol('p',j), point_j); end initialEstimate.print(sprintf('\nInitial estimate:\n ')); From f54b078447284a7d436fdc9d8489dd85b81eaaf7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Dec 2018 11:06:32 -0500 Subject: [PATCH 20/35] Fixed retract for SBA --- matlab/gtsam_examples/SBAExample.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/matlab/gtsam_examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m index b0f754044..7f50f2db8 100644 --- a/matlab/gtsam_examples/SBAExample.m +++ b/matlab/gtsam_examples/SBAExample.m @@ -64,7 +64,7 @@ for i=1:size(truth.cameras,2) initialEstimate.insert(symbol('c',i), camera_i); end for j=1:size(truth.points,2) - point_j = truth.points{j}.retract(0.1*randn(3,1)); + point_j = Point3(truth.points{j}.vector() + 0.1*randn(3,1)); initialEstimate.insert(symbol('p',j), point_j); end initialEstimate.print(sprintf('\nInitial estimate:\n ')); From fbcfbf0cdddef17ccf9dfe26b236fa7f4ba71fdf Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Dec 2018 11:19:46 -0500 Subject: [PATCH 21/35] Made naming convention in wrapper uniform. 2D means Pose2 + Point2 3D means Pose3 + Point3 --- gtsam.h | 10 ++++--- gtsam_unstable/slam/serialization.cpp | 8 +++--- gtsampy.h | 4 +-- matlab/+gtsam/Contents.m | 4 +-- .../gtsam_examples/RangeISAMExample_plaza.m | 6 ++-- .../gtsam_examples/RangeSLAMExample_plaza.m | 2 +- matlab/gtsam_tests/testSerialization.m | 6 ++-- .../SmartRangeFactorExample.m | 20 ++++++------- tests/testSerializationSLAM.cpp | 28 +++++++++---------- wrap/Module.cpp | 2 +- 10 files changed, 46 insertions(+), 44 deletions(-) diff --git a/gtsam.h b/gtsam.h index d681ddb0d..629fd70cf 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2270,8 +2270,8 @@ virtual class RangeFactor : gtsam::NoiseModelFactor { void serialize() const; }; -typedef gtsam::RangeFactor RangeFactorPosePoint2; -typedef gtsam::RangeFactor RangeFactorPosePoint3; +typedef gtsam::RangeFactor RangeFactor2D; +typedef gtsam::RangeFactor RangeFactor3D; typedef gtsam::RangeFactor RangeFactorPose2; typedef gtsam::RangeFactor RangeFactorPose3; typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; @@ -2289,8 +2289,8 @@ virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { void serialize() const; }; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPosePoint2; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPosePoint3; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform2D; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform3D; typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose2; typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose3; @@ -2304,6 +2304,7 @@ virtual class BearingFactor : gtsam::NoiseModelFactor { }; typedef gtsam::BearingFactor BearingFactor2D; +typedef gtsam::BearingFactor BearingFactorPose2; #include template @@ -2317,6 +2318,7 @@ virtual class BearingRangeFactor : gtsam::NoiseModelFactor { }; typedef gtsam::BearingRangeFactor BearingRangeFactor2D; +typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; #include diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 7928a2aac..17c95e614 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -70,8 +70,8 @@ typedef NonlinearEquality NonlinearEqualityCalibratedCamera; typedef NonlinearEquality NonlinearEqualitySimpleCamera; typedef NonlinearEquality NonlinearEqualityStereoCamera; -typedef RangeFactor RangeFactorPosePoint2; -typedef RangeFactor RangeFactorPosePoint3; +typedef RangeFactor RangeFactor2D; +typedef RangeFactor RangeFactor3D; typedef RangeFactor RangeFactorPose2; typedef RangeFactor RangeFactorPose3; typedef RangeFactor RangeFactorCalibratedCameraPoint; @@ -172,8 +172,8 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqua BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3"); +BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D"); +BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); diff --git a/gtsampy.h b/gtsampy.h index 9cadf6be3..27af74e74 100644 --- a/gtsampy.h +++ b/gtsampy.h @@ -2258,8 +2258,8 @@ virtual class RangeFactor : gtsam::NoiseModelFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); }; -typedef gtsam::RangeFactor RangeFactorPosePoint2; -typedef gtsam::RangeFactor RangeFactorPosePoint3; +typedef gtsam::RangeFactor RangeFactor2D; +typedef gtsam::RangeFactor RangeFactor3D; typedef gtsam::RangeFactor RangeFactorPose2; typedef gtsam::RangeFactor RangeFactorPose3; typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 023c61dbe..10fd5e142 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -142,8 +142,8 @@ % RangeFactorCalibratedCameraPoint - class RangeFactorCalibratedCameraPoint, see Doxygen page for details % RangeFactorPose2 - class RangeFactorPose2, see Doxygen page for details % RangeFactorPose3 - class RangeFactorPose3, see Doxygen page for details -% RangeFactorPosePoint2 - class RangeFactorPosePoint2, see Doxygen page for details -% RangeFactorPosePoint3 - class RangeFactorPosePoint3, see Doxygen page for details +% RangeFactor2D - class RangeFactor2D, see Doxygen page for details +% RangeFactor3D - class RangeFactor3D, see Doxygen page for details % RangeFactorSimpleCamera - class RangeFactorSimpleCamera, see Doxygen page for details % RangeFactorSimpleCameraPoint - class RangeFactorSimpleCameraPoint, see Doxygen page for details % VisualISAMGenerateData - VisualISAMGenerateData creates data for viusalSLAM::iSAM examples diff --git a/matlab/gtsam_examples/RangeISAMExample_plaza.m b/matlab/gtsam_examples/RangeISAMExample_plaza.m index cffe81c30..e31fb18a8 100644 --- a/matlab/gtsam_examples/RangeISAMExample_plaza.m +++ b/matlab/gtsam_examples/RangeISAMExample_plaza.m @@ -125,7 +125,7 @@ for i=1:M % M j = TD(k,3); range = TD(k,4); if addRange - factor = RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range); + factor = RangeFactor2D(i, symbol('L',j), range, noiseModels.range); % Throw out obvious outliers based on current landmark estimates error=factor.unwhitenedError(landmarkEstimates); if k<=minK || abs(error)<5 @@ -146,14 +146,14 @@ for i=1:M % M end isam.update(newFactors, initial); result = isam.calculateEstimate(); - lastPose = result.at(i); + lastPose = result.atPose2(i); % update landmark estimates if addRange landmarkEstimates = Values; for jj=1:size(TL,1) j=TL(jj,1); key = symbol('L',j); - landmarkEstimates.insert(key,result.at(key)); + landmarkEstimates.insert(key,result.atPoint2(key)); end end newFactors = NonlinearFactorGraph; diff --git a/matlab/gtsam_examples/RangeSLAMExample_plaza.m b/matlab/gtsam_examples/RangeSLAMExample_plaza.m index bd643d854..d2d1e9d70 100644 --- a/matlab/gtsam_examples/RangeSLAMExample_plaza.m +++ b/matlab/gtsam_examples/RangeSLAMExample_plaza.m @@ -76,7 +76,7 @@ for i=1:M while k<=K && t>=TD(k,1) j = TD(k,3); range = TD(k,4); - factor = RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range); + factor = RangeFactor2D(i, symbol('L',j), range, noiseModels.range); graph.add(factor); k=k+1; end diff --git a/matlab/gtsam_tests/testSerialization.m b/matlab/gtsam_tests/testSerialization.m index baacb198b..f8b21b7ad 100644 --- a/matlab/gtsam_tests/testSerialization.m +++ b/matlab/gtsam_tests/testSerialization.m @@ -60,9 +60,9 @@ graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); % Range Factors rNoise = noiseModel.Diagonal.Sigmas([0.2]); -graph.add(RangeFactorPosePoint2(i1, j1, sqrt(4+4), rNoise)); -graph.add(RangeFactorPosePoint2(i2, j1, 2, rNoise)); -graph.add(RangeFactorPosePoint2(i3, j2, 2, rNoise)); +graph.add(RangeFactor2D(i1, j1, sqrt(4+4), rNoise)); +graph.add(RangeFactor2D(i2, j1, 2, rNoise)); +graph.add(RangeFactor2D(i3, j2, 2, rNoise)); % Bearing Factors degrees = pi/180; diff --git a/matlab/unstable_examples/SmartRangeFactorExample.m b/matlab/unstable_examples/SmartRangeFactorExample.m index 7535447df..a192a1f5e 100644 --- a/matlab/unstable_examples/SmartRangeFactorExample.m +++ b/matlab/unstable_examples/SmartRangeFactorExample.m @@ -103,9 +103,9 @@ for ind_pose = 2:7 r2 = curr_pose.range(lmk1); % range of lmk1 wrt x2 srf1.addRange(key_curr, r2); - rangef1 = RangeFactorPosePoint2(key_prev, lmkKey(1), r1, noiseRange); + rangef1 = RangeFactor2D(key_prev, lmkKey(1), r1, noiseRange); fullGraph.add(rangef1); - rangef2 = RangeFactorPosePoint2(key_curr, lmkKey(1), r2, noiseRange); + rangef2 = RangeFactor2D(key_curr, lmkKey(1), r2, noiseRange); fullGraph.add(rangef2); if goodInitFlag_lmk1==1 @@ -123,9 +123,9 @@ for ind_pose = 2:7 r4 = curr_pose.range(lmk2); % range of lmk2 wrt x3 srf2.addRange(key_curr, r4); - rangef3 = RangeFactorPosePoint2(key_curr, lmkKey(1), r3, noiseRange); + rangef3 = RangeFactor2D(key_curr, lmkKey(1), r3, noiseRange); fullGraph.add(rangef3); - rangef4 = RangeFactorPosePoint2(key_curr, lmkKey(2), r4, noiseRange); + rangef4 = RangeFactor2D(key_curr, lmkKey(2), r4, noiseRange); % IF WE ADD FACTOR HERE IT CRASHES: fullGraph.add(rangef4); %==================================================================== case 4 @@ -138,9 +138,9 @@ for ind_pose = 2:7 % DELAYED INITIALIZATION: fullGraph.add(rangef4); - rangef5 = RangeFactorPosePoint2(key_curr, lmkKey(2), r5, noiseRange); + rangef5 = RangeFactor2D(key_curr, lmkKey(2), r5, noiseRange); fullGraph.add(rangef5); - rangef6 = RangeFactorPosePoint2(key_curr, lmkKey(3), r6, noiseRange); + rangef6 = RangeFactor2D(key_curr, lmkKey(3), r6, noiseRange); % IF WE ADD FACTOR HERE IT CRASHES: fullGraph.add(rangef6); if goodInitFlag_lmk2==1 @@ -160,9 +160,9 @@ for ind_pose = 2:7 % DELAYED INITIALIZATION: fullGraph.add(rangef6); - rangef7 = RangeFactorPosePoint2(key_curr, lmkKey(2), r7, noiseRange); + rangef7 = RangeFactor2D(key_curr, lmkKey(2), r7, noiseRange); fullGraph.add(rangef7); - rangef8 = RangeFactorPosePoint2(key_curr, lmkKey(3), r8, noiseRange); + rangef8 = RangeFactor2D(key_curr, lmkKey(3), r8, noiseRange); fullGraph.add(rangef8); if goodInitFlag_lmk3==1 @@ -176,7 +176,7 @@ for ind_pose = 2:7 r9 = curr_pose.range(lmk3); % range of lmk3 wrt x6 srf3.addRange(key_curr, r9); - rangef9 = RangeFactorPosePoint2(key_curr, lmkKey(3), r9, noiseRange); + rangef9 = RangeFactor2D(key_curr, lmkKey(3), r9, noiseRange); fullGraph.add(rangef9); case 7 % x6-lmk3 @@ -184,7 +184,7 @@ for ind_pose = 2:7 srf3.addRange(key_curr, r10); smartGraph.add(srf3); - rangef10 = RangeFactorPosePoint2(key_curr, lmkKey(3), r10, noiseRange); + rangef10 = RangeFactor2D(key_curr, lmkKey(3), r10, noiseRange); fullGraph.add(rangef10); end diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 33453d7d3..6bc155214 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -96,8 +96,8 @@ typedef NonlinearEquality NonlinearEqualityCalibratedCamera; typedef NonlinearEquality NonlinearEqualitySimpleCamera; typedef NonlinearEquality NonlinearEqualityStereoCamera; -typedef RangeFactor RangeFactorPosePoint2; -typedef RangeFactor RangeFactorPosePoint3; +typedef RangeFactor RangeFactor2D; +typedef RangeFactor RangeFactor3D; typedef RangeFactor RangeFactorPose2; typedef RangeFactor RangeFactorPose3; typedef RangeFactor RangeFactorCalibratedCameraPoint; @@ -204,8 +204,8 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqua BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3"); +BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D"); +BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); @@ -378,8 +378,8 @@ TEST (testSerializationSLAM, factors) { NonlinearEqualitySimpleCamera nonlinearEqualitySimpleCamera(a13, simpleCamera); NonlinearEqualityStereoCamera nonlinearEqualityStereoCamera(a14, stereoCamera); - RangeFactorPosePoint2 rangeFactorPosePoint2(a08, a03, 2.0, model1); - RangeFactorPosePoint3 rangeFactorPosePoint3(a09, a05, 2.0, model1); + RangeFactor2D rangeFactor2D(a08, a03, 2.0, model1); + RangeFactor3D rangeFactor3D(a09, a05, 2.0, model1); RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1); RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1); RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1); @@ -439,8 +439,8 @@ TEST (testSerializationSLAM, factors) { graph += nonlinearEqualitySimpleCamera; graph += nonlinearEqualityStereoCamera; - graph += rangeFactorPosePoint2; - graph += rangeFactorPosePoint3; + graph += rangeFactor2D; + graph += rangeFactor3D; graph += rangeFactorPose2; graph += rangeFactorPose3; graph += rangeFactorCalibratedCameraPoint; @@ -505,8 +505,8 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsObj(nonlinearEqualitySimpleCamera)); EXPECT(equalsObj(nonlinearEqualityStereoCamera)); - EXPECT(equalsObj(rangeFactorPosePoint2)); - EXPECT(equalsObj(rangeFactorPosePoint3)); + EXPECT(equalsObj(rangeFactor2D)); + EXPECT(equalsObj(rangeFactor3D)); EXPECT(equalsObj(rangeFactorPose2)); EXPECT(equalsObj(rangeFactorPose3)); EXPECT(equalsObj(rangeFactorCalibratedCameraPoint)); @@ -571,8 +571,8 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsXML(nonlinearEqualitySimpleCamera)); EXPECT(equalsXML(nonlinearEqualityStereoCamera)); - EXPECT(equalsXML(rangeFactorPosePoint2)); - EXPECT(equalsXML(rangeFactorPosePoint3)); + EXPECT(equalsXML(rangeFactor2D)); + EXPECT(equalsXML(rangeFactor3D)); EXPECT(equalsXML(rangeFactorPose2)); EXPECT(equalsXML(rangeFactorPose3)); EXPECT(equalsXML(rangeFactorCalibratedCameraPoint)); @@ -637,8 +637,8 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(nonlinearEqualitySimpleCamera)); EXPECT(equalsBinary(nonlinearEqualityStereoCamera)); - EXPECT(equalsBinary(rangeFactorPosePoint2)); - EXPECT(equalsBinary(rangeFactorPosePoint3)); + EXPECT(equalsBinary(rangeFactor2D)); + EXPECT(equalsBinary(rangeFactor3D)); EXPECT(equalsBinary(rangeFactorPose2)); EXPECT(equalsBinary(rangeFactorPose3)); EXPECT(equalsBinary(rangeFactorCalibratedCameraPoint)); diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 5cf3b5d6c..a3b8df630 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -127,7 +127,7 @@ void Module::parseMarkup(const std::string& data) { TemplateInstantiationTypedef singleInstantiation, singleInstantiation0; TypeListGrammar<'<','>'> typelist_g(singleInstantiation.typeList); - // typedef gtsam::RangeFactor RangeFactorPosePoint2; + // typedef gtsam::RangeFactor RangeFactor2D; TypeGrammar instantiationClass_g(singleInstantiation.class_); Rule templateSingleInstantiation_p = (str_p("typedef") >> instantiationClass_g >> From 609019b5851379969a151e4c8acca6c958a44b2e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Dec 2018 12:30:53 -0500 Subject: [PATCH 22/35] Fixed warning --- examples/ISAM2_SmartFactorStereo_IMU.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp index 968f1edc7..f39e9f4eb 100644 --- a/examples/ISAM2_SmartFactorStereo_IMU.cpp +++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include using namespace std; @@ -224,7 +225,7 @@ int main(int argc, char* argv[]) { smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K); } else { - throw runtime_error("unexpected data type: " + type); + throw runtime_error("unexpected data type: " + string(1, type)); } lastFrame = frame; From 6a58e886313eeb09485567c76f7264dd1effb099 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Dec 2018 12:33:13 -0500 Subject: [PATCH 23/35] Added example by Wenqiang Zhou given in issue #369 --- examples/Pose2SLAMStressTest.cpp | 89 ++++++++++++++++++++++++++++++++ 1 file changed, 89 insertions(+) create mode 100644 examples/Pose2SLAMStressTest.cpp diff --git a/examples/Pose2SLAMStressTest.cpp b/examples/Pose2SLAMStressTest.cpp new file mode 100644 index 000000000..030834894 --- /dev/null +++ b/examples/Pose2SLAMStressTest.cpp @@ -0,0 +1,89 @@ +/** + * @file Pose2SLAMStressTest.cpp + * @brief Test GTSAM on large open-loop chains + * @date May 23, 2018 + * @author Wenqiang Zhou + */ + +// Create N 3D poses, add relative motion between each consecutive poses. (The +// relative motion is simply a unit translation(1, 0, 0), no rotation). For each +// each pose, add some random noise to the x value of the translation part. +// Use gtsam to create a prior factor for the first pose and N-1 between factors +// and run optimization. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +void testGtsam(int numberNodes) { + std::random_device rd; + std::mt19937 e2(rd()); + std::uniform_real_distribution<> dist(0, 1); + + vector poses; + for (int i = 0; i < numberNodes; ++i) { + Matrix4 M; + double r = dist(e2); + r = (r - 0.5) / 10 + i; + M << 1, 0, 0, r, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; + poses.push_back(Pose3(M)); + } + + // prior factor for the first pose + auto priorModel = noiseModel::Isotropic::Variance(6, 1e-4); + Matrix4 first_M; + first_M << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; + Pose3 first = Pose3(first_M); + + NonlinearFactorGraph graph; + graph.add(PriorFactor(0, first, priorModel)); + + // vo noise model + auto VOCovarianceModel = noiseModel::Isotropic::Variance(6, 1e-3); + + // relative VO motion + Matrix4 vo_M; + vo_M << 1, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; + Pose3 relativeMotion(vo_M); + for (int i = 0; i < numberNodes - 1; ++i) { + graph.add( + BetweenFactor(i, i + 1, relativeMotion, VOCovarianceModel)); + } + + // inital values + Values initial; + for (int i = 0; i < numberNodes; ++i) { + initial.insert(i, poses[i]); + } + + LevenbergMarquardtParams params; + params.verbosity = NonlinearOptimizerParams::ERROR; + // params.setLinearSolverType("MULTIFRONTAL_QR"); + LevenbergMarquardtOptimizer optimizer(graph, initial, params); + + // GaussNewtonParams params_gn; + // params_gn.setVerbosity("ERROR"); + // params_gn.setMaxIterations(20); + // params_gn.setLinearSolverType("MULTIFRONTAL_QR"); + // GaussNewtonOptimizer optimizer(graph, initial, params_gn ); + auto result = optimizer.optimize(); +} + +int main(int args, char* argv[]) { + int numberNodes = stoi(argv[1]); + cout << "number of_nodes: " << numberNodes << endl; + testGtsam(numberNodes); + return 0; +} From 3c3f6d2b7c42f9cb289bf53e891a456f1fdf1a37 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Dec 2018 13:08:41 -0500 Subject: [PATCH 24/35] Switching to METIS ordering fixes out of memory error for large examples. --- examples/Pose2SLAMStressTest.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/examples/Pose2SLAMStressTest.cpp b/examples/Pose2SLAMStressTest.cpp index 030834894..0f306b7f4 100644 --- a/examples/Pose2SLAMStressTest.cpp +++ b/examples/Pose2SLAMStressTest.cpp @@ -69,15 +69,10 @@ void testGtsam(int numberNodes) { } LevenbergMarquardtParams params; - params.verbosity = NonlinearOptimizerParams::ERROR; - // params.setLinearSolverType("MULTIFRONTAL_QR"); + params.setVerbosity("ERROR"); + params.setOrderingType("METIS"); + params.setLinearSolverType("MULTIFRONTAL_CHOLESKY"); LevenbergMarquardtOptimizer optimizer(graph, initial, params); - - // GaussNewtonParams params_gn; - // params_gn.setVerbosity("ERROR"); - // params_gn.setMaxIterations(20); - // params_gn.setLinearSolverType("MULTIFRONTAL_QR"); - // GaussNewtonOptimizer optimizer(graph, initial, params_gn ); auto result = optimizer.optimize(); } From eada1ee505aecc2e6dc4bb4ddf55ae8502c1d257 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Dec 2018 14:48:22 -0500 Subject: [PATCH 25/35] Adding adjoint and adjoint transpose functions --- gtsam/geometry/Pose2.h | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index f03e0852e..079c649f3 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -132,6 +132,8 @@ public: * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ Matrix3 AdjointMap() const; + + /// Apply AdjointMap to twist xi inline Vector3 Adjoint(const Vector3& xi) const { return AdjointMap()*xi; } @@ -141,6 +143,20 @@ public: */ static Matrix3 adjointMap(const Vector3& v); + /** + * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives + */ + Vector3 adjoint(const Vector3& xi, const Vector3& y) { + return adjointMap(xi) * y; + } + + /** + * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. + */ + Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) { + return adjointMap(xi).transpose() * y; + } + /** * wedge for SE(2): * @param xi 3-dim twist (v,omega) where From 409a0215b88d548524d39cdddbee73de63889bb9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Dec 2018 15:37:31 -0500 Subject: [PATCH 26/35] Added adjoint operators etc. --- gtsam.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam.h b/gtsam.h index 629fd70cf..d9052f65b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -573,8 +573,13 @@ class Pose2 { // Lie Group static gtsam::Pose2 Expmap(Vector v); static Vector Logmap(const gtsam::Pose2& p); + static Matrix ExpmapDerivative(Vector v); + static Matrix LogmapDerivative(const gtsam::Pose2& v); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; + static Matrix adjointMap(Vector v); + Vector adjoint(Vector xi, Vector y); + Vector adjointTranspose(Vector xi, Vector y); static Matrix wedge(double vx, double vy, double w); // Group Actions on Point2 @@ -623,6 +628,11 @@ class Pose3 { static Vector Logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; + static Matrix adjointMap(Vector xi); + static Vector adjoint(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); + static Matrix ExpmapDerivative(Vector xi); + static Matrix LogmapDerivative(const gtsam::Pose3& xi); static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); // Group Action on Point3 From 1999fba7ae06957ec0f941307fcb71e9714cb1e8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Dec 2018 15:38:12 -0500 Subject: [PATCH 27/35] Cleaned up Pose3 unit test, added unit test for adjoint. --- cython/gtsam/tests/test_Pose3.py | 36 ++++++++++++++++++++++---------- 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py index 8fa50b90c..4752a4b02 100644 --- a/cython/gtsam/tests/test_Pose3.py +++ b/cython/gtsam/tests/test_Pose3.py @@ -1,25 +1,32 @@ +"""Pose3 unit tests.""" import math import unittest -from gtsam import Point3, Rot3, Pose3 +import numpy as np + +from gtsam import Point3, Pose3, Rot3 class TestPose3(unittest.TestCase): + """Test selected Pose3 methods.""" - def test__between(self): - T2 = Pose3(Rot3.Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)) + def test_between(self): + """Test between method.""" + T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2)) T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3)) expected = T2.inverse().compose(T3) actual = T2.between(T3) self.assertTrue(actual.equals(expected, 1e-6)) def test_transform_to(self): - transform = Pose3(Rot3.Rodrigues(0,0,-1.570796), Point3(2,4, 0)) - actual = transform.transform_to(Point3(3,2,10)) - expected = Point3 (2,1,10) + """Test transform_to method.""" + transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)) + actual = transform.transform_to(Point3(3, 2, 10)) + expected = Point3(2, 1, 10) self.assertTrue(actual.equals(expected, 1e-6)) def test_range(self): + """Test range method.""" l1 = Point3(1, 0, 0) l2 = Point3(1, 1, 0) x1 = Pose3() @@ -28,16 +35,23 @@ class TestPose3(unittest.TestCase): xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)) # establish range is indeed zero - self.assertEqual(1,x1.range(point=l1)) + self.assertEqual(1, x1.range(point=l1)) # establish range is indeed sqrt2 - self.assertEqual(math.sqrt(2.0),x1.range(point=l2)) + self.assertEqual(math.sqrt(2.0), x1.range(point=l2)) # establish range is indeed zero - self.assertEqual(1,x1.range(pose=xl1)) - + self.assertEqual(1, x1.range(pose=xl1)) + # establish range is indeed sqrt2 - self.assertEqual(math.sqrt(2.0),x1.range(pose=xl2)) + self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2)) + + def test_adjoint(self): + """Test adjoint method.""" + xi = np.array([1, 2, 3, 4, 5, 6]) + expected = np.dot(Pose3.adjointMap(xi), xi) + actual = Pose3.adjoint(xi, xi) + np.testing.assert_array_equal(actual, expected) if __name__ == "__main__": From 0a2e4e34e89f4f84cd31ee9c97600b667e43d609 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 2 Jan 2019 14:32:49 -0500 Subject: [PATCH 28/35] Added extra types included by Jacob Thomson in (declined) PR #269 --- gtsam.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index 629fd70cf..2741799d7 100644 --- a/gtsam.h +++ b/gtsam.h @@ -229,7 +229,7 @@ virtual class Value { }; #include -template +template virtual class GenericValue : gtsam::Value { void serializable() const; }; From f558ccbb2de2d25c93705b96d482b07e0c7a19d2 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 2 Jan 2019 13:26:53 -0800 Subject: [PATCH 29/35] Update LICENSE to enumerate all dependencies in gtsam/3rdparty --- LICENSE | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/LICENSE b/LICENSE index e1c3be202..d828deb55 100644 --- a/LICENSE +++ b/LICENSE @@ -1,18 +1,25 @@ GTSAM is released under the simplified BSD license, reproduced in the file LICENSE.BSD in this directory. -GTSAM contains two third party libraries, with documentation of licensing and -modifications as follows: +GTSAM contains several third party libraries, with documentation of licensing +and modifications as follows: -- CCOLAMD 2.9.3: Tim Davis' constrained column approximate minimum degree +- CCOLAMD 2.9.6: Tim Davis' constrained column approximate minimum degree ordering library - - Included unmodified in gtsam/3rdparty/CCOLAMD and gtsam/3rdparty/UFconfig + - Included unmodified in gtsam/3rdparty/CCOLAMD and + gtsam/3rdparty/SuiteSparse_config - http://faculty.cse.tamu.edu/davis/suitesparse.html - Licenced under BSD-3, provided in gtsam/3rdparty/CCOLAMD/Doc/License.txt -- Eigen 3.2: General C++ matrix and linear algebra library - - Modified with 3 patches that have been contributed back to the Eigen team: - - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection) - - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code) - - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=716 (Improved comma initialization) +- ceres: Google's nonlinear least-squares optimization library + - Includes only auto-diff/jet code, with minor modifications to includes + - http://ceres-solver.org/license.html +- Eigen 3.3.7: General C++ matrix and linear algebra library - Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README - - Some code that is 3rd-party to Eigen is BSD and LGPL \ No newline at end of file + - Some code that is 3rd-party to Eigen is BSD and LGPL +- GeographicLib 1.35: Charles Karney's geographic conversion utility library + - Included unmodified in gtsam/3rdparty/GeographicLib + - Licenced under MIT, provided in gtsam/3rdparty/GeographicLib/LICENSE.txt +- METIS 5.1.0: Graph partitioning and fill-reducing matrix ordering library + - Included unmodified in gtsam/3rdparty/metis + - Licenced under Apache License v 2.0, provided in + gtsam/3rdparty/metis/LICENSE.txt From d4398fb0928d3fafddac28d758dceb7219cb7a7e Mon Sep 17 00:00:00 2001 From: Thomas Horstink Date: Fri, 4 Jan 2019 11:31:41 +0100 Subject: [PATCH 30/35] expression example of estimating trajectory, landmarks and sensor-body-transform simultaneously --- ...leExpressions_BearinRangeWithTransform.cpp | 122 ++++++++++++++++++ 1 file changed, 122 insertions(+) create mode 100644 examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp diff --git a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp new file mode 100644 index 000000000..a78328c48 --- /dev/null +++ b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp @@ -0,0 +1,122 @@ +/** + * @file Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp + * @brief A simultanious optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions + * @author Thomas Horstink + * @date January 4th, 2019 + */ + +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +typedef BearingRange BearingRange3D; + +// These functions are very similar to those in the SFM example, except that it you can give it a fixed delta in between poses. +/* ************************************************************************* */ +std::vector createPoints() { + + // Create the set of ground-truth landmarks + std::vector points; + points.push_back(Point3(10.0,10.0,10.0)); + points.push_back(Point3(-10.0,10.0,10.0)); + points.push_back(Point3(-10.0,-10.0,10.0)); + points.push_back(Point3(10.0,-10.0,10.0)); + points.push_back(Point3(10.0,10.0,-10.0)); + points.push_back(Point3(-10.0,10.0,-10.0)); + points.push_back(Point3(-10.0,-10.0,-10.0)); + points.push_back(Point3(10.0,-10.0,-10.0)); + + return points; +} + +/* ************************************************************************* */ +std::vector createPoses(Pose3 delta, int steps) { + + // Create the set of ground-truth poses + std::vector poses; + Pose3 pose = Pose3(); + int i = 0; + for(; i < steps; ++i) { + poses.push_back(pose); + pose = pose.compose(delta); + } + poses.push_back(pose); + + return poses; +} + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + // Move around so the whole state (including the sensor tf) is observable + Pose3 delta_pose1 = Pose3(Rot3().Yaw(2*M_PI/8).Pitch(M_PI/8), Point3(1, 0, 0)); + Pose3 delta_pose2 = Pose3(Rot3().Pitch(-M_PI/8), Point3(1, 0, 0)); + Pose3 delta_pose3 = Pose3(Rot3().Yaw(-2*M_PI/8), Point3(1, 0, 0)); + + int steps = 4; + auto poses = createPoses(delta_pose1, steps); + auto poses2 = createPoses(delta_pose2, steps); + auto poses3 = createPoses(delta_pose3, steps); + + // concatenate poses to create trajectory + poses.insert( poses.end(), poses2.begin(), poses2.end() ); + poses.insert( poses.end(), poses3.begin(), poses3.end() ); // std::vector of Pose3 + auto points = createPoints(); // std::vector of Point3 + + // (ground-truth) sensor pose in body frame, further an unknown variable + Pose3 body_T_sensor_gt(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + // a graph + ExpressionFactorGraph graph; + // Specify uncertainty on first pose prior and also for between factor (simplicity reasons) + auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished()); + // Uncertainty bearing range measurement; + auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished()); + // Expressions for body-frame at key 0 and sensor-tf + Pose3_ x_('x', 0); + Pose3_ body_T_sensor_('T', 0); + // add a prior on the body-pose and sensor-tf. + graph.addExpressionFactor(x_, poses[0], poseNoise); + // Simulated measurements from pose, adding them to the factor graph + for (size_t i = 0; i < poses.size(); ++i) { + auto world_T_sensor = poses[i].compose(body_T_sensor_gt); + for (size_t j = 0; j < points.size(); ++j) { + // Create the expression + auto prediction = Expression( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j)); + // Create a *perfect* measurement + auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j])); + graph.addExpressionFactor(prediction, measurement, bearingRangeNoise); + } + // and add a between factor + if (i > 0) + { + // And also we have a nice measurement for the between factor. + graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise); + } + } + + // Create perturbed initial + Values initial; + Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + for (size_t i = 0; i < poses.size(); ++i) + initial.insert(Symbol('x', i), poses[i].compose(delta)); + for (size_t j = 0; j < points.size(); ++j) + initial.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); + + // initialize body_T_sensor wrongly (because we do not know!) + initial.insert(Symbol('T',0), Pose3()); + + std::cout << "initial error: " << graph.error(initial) << std::endl; + Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + std::cout << "final error: " << graph.error(result) << std::endl; + + initial.at(Symbol('T',0)).print("\nInitial estimate body_T_sensor\n"); /* initial sensor_P_body estimate */ + result.at(Symbol('T',0)).print("\nFinal estimate body_T_sensor\n"); /* optimized sensor_P_body estimate */ + body_T_sensor_gt.print("\nGround truth body_T_sensor\n"); /* sensor_P_body ground truth */ + + return 0; +} +/* ************************************************************************* */ \ No newline at end of file From 7bb6863e7567a4f72d1f011d3505ca1339e62112 Mon Sep 17 00:00:00 2001 From: Thomas Horstink Date: Fri, 4 Jan 2019 11:50:20 +0100 Subject: [PATCH 31/35] little typo in a comment --- .../Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp index a78328c48..69409e87b 100644 --- a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp +++ b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp @@ -16,7 +16,7 @@ using namespace gtsam; typedef BearingRange BearingRange3D; -// These functions are very similar to those in the SFM example, except that it you can give it a fixed delta in between poses. +// These functions are very similar to those in the SFM example, except that you can give it a fixed delta in between poses for n steps. /* ************************************************************************* */ std::vector createPoints() { From 986346f2b9c43918a9d4f626f4f5f0859e25dd3d Mon Sep 17 00:00:00 2001 From: Thomas Horstink Date: Fri, 4 Jan 2019 11:53:50 +0100 Subject: [PATCH 32/35] another comment update --- ...xampleExpressions_BearinRangeWithTransform.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp index 69409e87b..6ba7caca3 100644 --- a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp +++ b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp @@ -72,28 +72,29 @@ int main(int argc, char* argv[]) { // a graph ExpressionFactorGraph graph; // Specify uncertainty on first pose prior and also for between factor (simplicity reasons) - auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished()); + auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());ExpressiExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampon examp // Uncertainty bearing range measurement; auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished()); // Expressions for body-frame at key 0 and sensor-tf - Pose3_ x_('x', 0); + Pose3_ x_('x', 0);nice Pose3_ body_T_sensor_('T', 0); - // add a prior on the body-pose and sensor-tf. + // add a prior on the body-pose. graph.addExpressionFactor(x_, poses[0], poseNoise); - // Simulated measurements from pose, adding them to the factor graph + // Simulated measurements from pose for (size_t i = 0; i < poses.size(); ++i) { auto world_T_sensor = poses[i].compose(body_T_sensor_gt); for (size_t j = 0; j < points.size(); ++j) { // Create the expression auto prediction = Expression( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j)); - // Create a *perfect* measurement + // Create a *perfect* measurementExpression exampExpreExpression exampExpression exampExpression exampssion examp auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j])); + // Add factor graph.addExpressionFactor(prediction, measurement, bearingRangeNoise); } - // and add a between factor + // and add a between factor to the graph if (i > 0) { - // And also we have a nice measurement for the between factor. + // And also we have a *perfect* measurement for the between factor. graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise); } } From ba03b398f4d0c6290de1d6cbaac85dbed9726b6e Mon Sep 17 00:00:00 2001 From: Thomas Horstink Date: Fri, 4 Jan 2019 11:55:26 +0100 Subject: [PATCH 33/35] type in filename.... --- ...> Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename examples/{Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp => Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp} (98%) diff --git a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp similarity index 98% rename from examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp rename to examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp index 6ba7caca3..8faa6b182 100644 --- a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp +++ b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp @@ -1,5 +1,5 @@ /** - * @file Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp + * @file Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp * @brief A simultanious optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions * @author Thomas Horstink * @date January 4th, 2019 From 9c382b6c144fd778dec1ecc81c166bfe15a69ffb Mon Sep 17 00:00:00 2001 From: Thomas Horstink Date: Fri, 4 Jan 2019 16:17:33 +0100 Subject: [PATCH 34/35] changed the SFMdata functions so that it allows the passage of function arguments to generate a trajectory; default arguments result in the original behaviour (described in header). In the range bearing examples: fixed weirdo text-artifacts, add newline for readability, added underscore the prediction expression. --- ...eExpressions_BearingRangeWithTransform.cpp | 73 +++++++------------ examples/SFMdata.h | 28 +++---- 2 files changed, 40 insertions(+), 61 deletions(-) diff --git a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp index 8faa6b182..39157a4f6 100644 --- a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp +++ b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp @@ -11,86 +11,65 @@ #include #include #include +#include using namespace gtsam; typedef BearingRange BearingRange3D; -// These functions are very similar to those in the SFM example, except that you can give it a fixed delta in between poses for n steps. -/* ************************************************************************* */ -std::vector createPoints() { - - // Create the set of ground-truth landmarks - std::vector points; - points.push_back(Point3(10.0,10.0,10.0)); - points.push_back(Point3(-10.0,10.0,10.0)); - points.push_back(Point3(-10.0,-10.0,10.0)); - points.push_back(Point3(10.0,-10.0,10.0)); - points.push_back(Point3(10.0,10.0,-10.0)); - points.push_back(Point3(-10.0,10.0,-10.0)); - points.push_back(Point3(-10.0,-10.0,-10.0)); - points.push_back(Point3(10.0,-10.0,-10.0)); - - return points; -} - -/* ************************************************************************* */ -std::vector createPoses(Pose3 delta, int steps) { - - // Create the set of ground-truth poses - std::vector poses; - Pose3 pose = Pose3(); - int i = 0; - for(; i < steps; ++i) { - poses.push_back(pose); - pose = pose.compose(delta); - } - poses.push_back(pose); - - return poses; -} - /* ************************************************************************* */ int main(int argc, char* argv[]) { + // Move around so the whole state (including the sensor tf) is observable + Pose3 init_pose = Pose3(); Pose3 delta_pose1 = Pose3(Rot3().Yaw(2*M_PI/8).Pitch(M_PI/8), Point3(1, 0, 0)); Pose3 delta_pose2 = Pose3(Rot3().Pitch(-M_PI/8), Point3(1, 0, 0)); Pose3 delta_pose3 = Pose3(Rot3().Yaw(-2*M_PI/8), Point3(1, 0, 0)); int steps = 4; - auto poses = createPoses(delta_pose1, steps); - auto poses2 = createPoses(delta_pose2, steps); - auto poses3 = createPoses(delta_pose3, steps); + auto poses = createPoses(init_pose, delta_pose1, steps); + auto poses2 = createPoses(init_pose, delta_pose2, steps); + auto poses3 = createPoses(init_pose, delta_pose3, steps); - // concatenate poses to create trajectory + // Concatenate poses to create trajectory poses.insert( poses.end(), poses2.begin(), poses2.end() ); poses.insert( poses.end(), poses3.begin(), poses3.end() ); // std::vector of Pose3 auto points = createPoints(); // std::vector of Point3 // (ground-truth) sensor pose in body frame, further an unknown variable Pose3 body_T_sensor_gt(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - // a graph + + // The graph ExpressionFactorGraph graph; + // Specify uncertainty on first pose prior and also for between factor (simplicity reasons) - auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());ExpressiExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampon examp + auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished()); + // Uncertainty bearing range measurement; auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished()); + // Expressions for body-frame at key 0 and sensor-tf - Pose3_ x_('x', 0);nice + Pose3_ x_('x', 0); Pose3_ body_T_sensor_('T', 0); - // add a prior on the body-pose. + + // Add a prior on the body-pose graph.addExpressionFactor(x_, poses[0], poseNoise); + // Simulated measurements from pose for (size_t i = 0; i < poses.size(); ++i) { auto world_T_sensor = poses[i].compose(body_T_sensor_gt); for (size_t j = 0; j < points.size(); ++j) { - // Create the expression - auto prediction = Expression( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j)); - // Create a *perfect* measurementExpression exampExpreExpression exampExpression exampExpression exampssion examp + + // This expression is the key feature of this example: it creates a differentiable expression of the measurement after being displaced by sensor transform. + auto prediction_ = Expression( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j)); + + // Create a *perfect* measurement auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j])); + // Add factor - graph.addExpressionFactor(prediction, measurement, bearingRangeNoise); + graph.addExpressionFactor(prediction_, measurement, bearingRangeNoise); } + // and add a between factor to the graph if (i > 0) { @@ -107,7 +86,7 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) initial.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); - // initialize body_T_sensor wrongly (because we do not know!) + // Initialize body_T_sensor wrongly (because we do not know!) initial.insert(Symbol('T',0), Pose3()); std::cout << "initial error: " << graph.error(initial) << std::endl; diff --git a/examples/SFMdata.h b/examples/SFMdata.h index 25442d527..af1f761ee 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -16,9 +16,10 @@ */ /** - * A structure-from-motion example with landmarks + * A structure-from-motion example with landmarks, default function arguments give * - The landmarks form a 10 meter cube * - The robot rotates around the landmarks, always facing towards the cube + * Passing function argument allows to specificy an initial position, a pose increment and step count. */ // As this is a full 3D problem, we will use Pose3 variables to represent the camera @@ -49,20 +50,19 @@ std::vector createPoints() { } /* ************************************************************************* */ -std::vector createPoses() { - +std::vector createPoses( + const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)), + const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))), + int steps = 8) { + // Create the set of ground-truth poses + // Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center std::vector poses; - double radius = 30.0; - int i = 0; - double theta = 0.0; - gtsam::Point3 up(0,0,1); - gtsam::Point3 target(0,0,0); - for(; i < 8; ++i, theta += 2*M_PI/8) { - gtsam::Point3 position = gtsam::Point3(radius*cos(theta), radius*sin(theta), 0.0); - gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(position, target, up); - poses.push_back(camera.pose()); + int i = 1; + poses.push_back(init); + for(; i < steps; ++i) { + poses.push_back(poses[i-1].compose(delta)); } + return poses; -} -/* ************************************************************************* */ +} \ No newline at end of file From e7d6cd4faf4ecfe1bbf1b247379a8cdd39d55b35 Mon Sep 17 00:00:00 2001 From: Thomas Horstink Date: Fri, 4 Jan 2019 17:12:04 +0100 Subject: [PATCH 35/35] fixed typo in description --- .../Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp index 39157a4f6..68a3fd7e7 100644 --- a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp +++ b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp @@ -1,6 +1,6 @@ /** * @file Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp - * @brief A simultanious optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions + * @brief A simultaneous optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions * @author Thomas Horstink * @date January 4th, 2019 */