From 182fd06cb887bc0838dbc775a4d0feba553bd7c5 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 29 May 2014 00:47:37 -0400 Subject: [PATCH 01/60] minor cleanup and comments --- examples/StereoVOExample_large.cpp | 68 +++++++++++++++--------------- 1 file changed, 35 insertions(+), 33 deletions(-) diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index c661967c0..e2caca94f 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -25,45 +25,41 @@ */ #include -#include +#include +#include +#include #include #include -#include -#include -#include - -#include -#include #include +#include +#include +#include #include #include -#include -#include using namespace std; using namespace gtsam; int main(int argc, char** argv){ + Values initial_estimate; NonlinearFactorGraph graph; const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); - Values initial_estimate = Values(); - vector read_vector; - string read_string, parse_string; - string data_folder = "C:/Users/Stephen/Documents/Borg/gtsam/Examples/Data/"; - string calibration_loc = data_folder + "VO_calibration.txt"; - string pose_loc = data_folder + "VO_camera_poses_large.txt"; - string factor_loc = data_folder + "VO_stereo_factors_large.txt"; + string calibration_loc = findExampleDataFile("VO_calibration.txt"); + string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); + string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); //read camera calibration info from file - double fx,fy,s,u,v,b; + // focal lengths fx, fy, skew s, principal point u0, v0, baseline b + double fx, fy, s, u0, v0, b; ifstream calibration_file(calibration_loc); cout << "Reading calibration info" << endl; - calibration_file >> fx >> fy >> s >> u >> v >> b; + calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; + //create stereo camera calibration object - const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u,v,b)); + const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b)); ifstream pose_file(pose_loc); cout << "Reading camera poses" << endl; @@ -77,30 +73,36 @@ int main(int argc, char** argv){ initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); } - double x, l, uL, uR, v, X, Y, Z; + // camera and landmark keys + size_t x, l; + + // pixel coordinates uL, uR, v (same for left/right images due to rectification) + // landmark coordinates X, Y, Z in camera frame, resulting from triangulation + double uL, uR, v, X, Y, Z; ifstream factor_file(factor_loc); cout << "Reading stereo factors" << endl; //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { - graph.push_back( - GenericStereoFactor(StereoPoint2(uL, uR, v), model, - Symbol('x', x), Symbol('l', l), K)); - //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it - if(!initial_estimate.exists(Symbol('l',l))){ - Pose3 camPose = initial_estimate.at(Symbol('x', x)); - //transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space - Point3 worldPoint = camPose.transform_from(Point3(X,Y,Z)); - initial_estimate.insert(Symbol('l',l),worldPoint); - } + graph.push_back( + GenericStereoFactor(StereoPoint2(uL, uR, v), model, + Symbol('x', x), Symbol('l', l), K)); + //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it + if (!initial_estimate.exists(Symbol('l', l))) { + Pose3 camPose = initial_estimate.at(Symbol('x', x)); + //transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space + Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z)); + initial_estimate.insert(Symbol('l', l), worldPoint); + } } Pose3 first_pose = initial_estimate.at(Symbol('x',1)); - first_pose.print("Check estimate poses:\n"); //constrain the first pose such that it cannot change from its original value during optimization + // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky + // QR is much slower than Cholesky, but numerically more stable graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); cout << "Optimizing" << endl; - //create Levenberg-Marquardt optimizer to solve the initial factor graph estimate + //create Levenberg-Marquardt optimizer to optimize the factor graph LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate); Values result = optimizer.optimize(); @@ -109,4 +111,4 @@ int main(int argc, char** argv){ pose_values.print("Final camera poses:\n"); return 0; -} \ No newline at end of file +} From 625b7b074376c703488fbe5c2333f348e5c2f1cf Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Fri, 30 May 2014 11:58:07 -0400 Subject: [PATCH 02/60] Monocular Smart Projection factor example based on stereo data --- .../examples/SmartProjectionFactorExample.cpp | 133 ++++++++++++++++++ 1 file changed, 133 insertions(+) create mode 100644 gtsam_unstable/examples/SmartProjectionFactorExample.cpp diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp new file mode 100644 index 000000000..a9c60afba --- /dev/null +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -0,0 +1,133 @@ +/* ---------------------------------------------------------------------------- + +* 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 SmartProjectionFactorExample.cpp +* @brief A stereo visual odometry example +* @date May 30, 2014 +* @author Stephen Camp +* @author Chris Beall +*/ + + +/** + * A smart projection factor example based on stereo data + * - robot starts at origin + * -moves forward, taking periodic stereo measurements + * -takes stereo readings of many landmarks + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char** argv){ + + typedef SmartProjectionPoseFactor SmartFactor; + + Values initial_estimate; + NonlinearFactorGraph graph; + const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,1); + + string calibration_loc = findExampleDataFile("VO_calibration.txt"); + string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); + string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); + + //read camera calibration info from file + // focal lengths fx, fy, skew s, principal point u0, v0, baseline b + double fx, fy, s, u0, v0, b; + ifstream calibration_file(calibration_loc); + cout << "Reading calibration info" << endl; + calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; + + //create stereo camera calibration object +// const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b)); + + const Cal3_S2::shared_ptr K(new Cal3_S2(fx, fy, s, u0, v0)); + + ifstream pose_file(pose_loc); + cout << "Reading camera poses" << endl; + int pose_id; + MatrixRowMajor m(4,4); + //read camera pose parameters and use to make initial estimates of camera poses + while (pose_file >> pose_id) { + for (int i = 0; i < 16; i++) { + pose_file >> m.data()[i]; + } + initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); + } + + // camera and landmark keys + size_t x, l; + + // pixel coordinates uL, uR, v (same for left/right images due to rectification) + // landmark coordinates X, Y, Z in camera frame, resulting from triangulation + double uL, uR, v, X, Y, Z; + ifstream factor_file(factor_loc); + cout << "Reading stereo factors" << endl; + //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation + + SmartFactor::shared_ptr factor(new SmartFactor()); + size_t current_l = 3; // TODO: Fix me! + + while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { +// graph.push_back( +// GenericStereoFactor(StereoPoint2(uL, uR, v), model, +// Symbol('x', x), Symbol('l', l), K)); + + if(current_l != l) { + graph.push_back(factor); + factor = SmartFactor::shared_ptr(new SmartFactor()); + current_l = l; + } + factor->add(Point2(uL,v), Symbol('x',x), model, K); + + //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it +// if (!initial_estimate.exists(Symbol('l', l))) { +// Pose3 camPose = initial_estimate.at(Symbol('x', x)); +// //transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space +// Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z)); +// initial_estimate.insert(Symbol('l', l), worldPoint); +// } + } + + Pose3 first_pose = initial_estimate.at(Symbol('x',1)); + //constrain the first pose such that it cannot change from its original value during optimization + // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky + // QR is much slower than Cholesky, but numerically more stable + graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + + cout << "Optimizing" << endl; + //create Levenberg-Marquardt optimizer to optimize the factor graph + LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate); + Values result = optimizer.optimize(); + + cout << "Final result sample:" << endl; + Values pose_values = result.filter(); + pose_values.print("Final camera poses:\n"); + + return 0; +} From a7a209ce0c391c987d1e51abea30b53bc9a9631a Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Fri, 30 May 2014 13:31:35 -0400 Subject: [PATCH 03/60] Minor fixes and documentation cleanup --- gtsam_unstable/slam/SmartProjectionFactor.h | 2 +- .../slam/SmartProjectionPoseFactor.h | 32 +++++++++++++------ 2 files changed, 23 insertions(+), 11 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 2124dd6f6..f439397b9 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -54,7 +54,7 @@ public: double f; }; -enum linearizationType { +enum LinearizationMode { HESSIAN, JACOBIAN_SVD, JACOBIAN_Q }; diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactor.h b/gtsam_unstable/slam/SmartProjectionPoseFactor.h index 66497d28d..273102bda 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactor.h @@ -41,9 +41,8 @@ template class SmartProjectionPoseFactor: public SmartProjectionFactor { protected: - linearizationType linearizeTo_; + LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) - // Known calibration std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) public: @@ -69,7 +68,7 @@ public: SmartProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, const bool enableEPI = false, boost::optional body_P_sensor = boost::none, - linearizationType linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, + LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {} @@ -80,7 +79,7 @@ public: /** * add a new measurement and pose key * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement) - * @param poseKey is the index corresponding to the camera observing the same landmark + * @param poseKey is key corresponding to the camera observing the same landmark * @param noise_i is the measurement noise * @param K_i is the (known) camera calibration */ @@ -92,8 +91,11 @@ public: } /** - * add a new measurements and pose keys - * Variant of the previous one in which we include a set of measurements + * Variant of the previous one in which we include a set of measurements + * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) + * @param poseKeys vector of keys corresponding to the camera observing the same landmark + * @param noises vector of measurement noises + * @param Ks vector of calibration objects */ void add(std::vector measurements, std::vector poseKeys, std::vector noises, @@ -105,8 +107,11 @@ public: } /** - * add a new measurements and pose keys * Variant of the previous one in which we include a set of measurements with the same noise and calibration + * @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) + * @param poseKeys vector of keys corresponding to the camera observing the same landmark + * @param noise measurement noise (same for all measurements) + * @param K the (known) camera calibration (same for all measurements) */ void add(std::vector measurements, std::vector poseKeys, const SharedNoiseModel noise, const boost::shared_ptr K) { @@ -141,7 +146,12 @@ public: return 6 * this->keys_.size(); } - // Collect all cameras + /** + * Collect all cameras involved in this factor + * @param values Values structure which must contain camera poses corresponding + * to keys involved in this factor + * @return vector of Values + */ typename Base::Cameras cameras(const Values& values) const { typename Base::Cameras cameras; size_t i=0; @@ -154,7 +164,9 @@ public: } /** - * linear factor on the poses + * Linearize to Gaussian Factor + * @param values Values structure which must contain camera poses for this factor + * @return */ virtual boost::shared_ptr linearize( const Values& values) const { @@ -184,7 +196,7 @@ public: } /** return the calibration object */ - inline const boost::shared_ptr calibration() const { + inline const std::vector > calibration() const { return K_all_; } From ef430410b54cd61a126af95c6e92bb1ac36cbf24 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Fri, 30 May 2014 13:34:24 -0400 Subject: [PATCH 04/60] linux fix --- examples/StereoVOExample_large.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index e2caca94f..b9ab663d9 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -54,14 +54,14 @@ int main(int argc, char** argv){ //read camera calibration info from file // focal lengths fx, fy, skew s, principal point u0, v0, baseline b double fx, fy, s, u0, v0, b; - ifstream calibration_file(calibration_loc); + ifstream calibration_file(calibration_loc.c_str()); cout << "Reading calibration info" << endl; calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; //create stereo camera calibration object const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b)); - ifstream pose_file(pose_loc); + ifstream pose_file(pose_loc.c_str()); cout << "Reading camera poses" << endl; int pose_id; MatrixRowMajor m(4,4); @@ -79,7 +79,7 @@ int main(int argc, char** argv){ // pixel coordinates uL, uR, v (same for left/right images due to rectification) // landmark coordinates X, Y, Z in camera frame, resulting from triangulation double uL, uR, v, X, Y, Z; - ifstream factor_file(factor_loc); + ifstream factor_file(factor_loc.c_str()); cout << "Reading stereo factors" << endl; //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { From 7f32fe3ea07331d91271624d278225c49b17cf54 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Fri, 30 May 2014 16:30:46 -0400 Subject: [PATCH 05/60] fix warning --- gtsam/linear/GaussianFactorGraph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index d6e4663e3..d6836783b 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -70,7 +70,7 @@ namespace gtsam { vector dims_accumulated; dims_accumulated.resize(dims.size()+1,0); dims_accumulated[0]=0; - for (int i=1; i Date: Fri, 30 May 2014 16:39:21 -0400 Subject: [PATCH 06/60] removed the rewritten file --- examples/Data/dubrovnik-3-7-pre-rewritten.txt | 80 ------------------- 1 file changed, 80 deletions(-) delete mode 100644 examples/Data/dubrovnik-3-7-pre-rewritten.txt diff --git a/examples/Data/dubrovnik-3-7-pre-rewritten.txt b/examples/Data/dubrovnik-3-7-pre-rewritten.txt deleted file mode 100644 index 7dea43c9e..000000000 --- a/examples/Data/dubrovnik-3-7-pre-rewritten.txt +++ /dev/null @@ -1,80 +0,0 @@ -3 7 19 - -0 0 -385.989990234375 387.1199951171875 -1 0 -38.439998626708984375 492.1199951171875 -2 0 -667.91998291015625 123.1100006103515625 -0 1 383.8800048828125 -15.299989700317382812 -1 1 559.75 -106.15000152587890625 -0 2 591.54998779296875 136.44000244140625 -1 2 863.8599853515625 -23.469970703125 -2 2 494.720001220703125 112.51999664306640625 -0 3 592.5 125.75 -1 3 861.08001708984375 -35.219970703125 -2 3 498.540008544921875 101.55999755859375 -0 4 348.720001220703125 558.3800048828125 -1 4 776.030029296875 483.529998779296875 -2 4 7.7800288200378417969 326.350006103515625 -0 5 14.010009765625 96.420013427734375 -1 5 207.1300048828125 118.3600006103515625 -0 6 202.7599945068359375 340.989990234375 -1 6 543.18011474609375 294.80999755859375 -2 6 -58.419979095458984375 110.8300018310546875 - - 0.29656188120312942935 --0.035318354384285870207 - 0.31252101755032046793 -0.47230274932665988752 --0.3572340863744113415 --2.0517704282499575896 -1430.031982421875 --7.5572756941255647689e-08 -3.2377570134516087119e-14 - - 0.28532097381985194184 --0.27699838370789808817 -0.048601169984112867206 - -1.2598695987143850861 --0.049063798188844320869 - -1.9586867140445654023 -1432.137451171875 --7.3171918302250560373e-08 -3.1759419042137054801e-14 - -0.057491325683772541433 - 0.34853090049579965592 - 0.47985129303736057116 - 8.1963904289063389541 - 6.5146840788718787252 --3.8392804395897406344 -1572.047119140625 --1.5962623223231275915e-08 --1.6507904730136101212e-14 - --11.317351620610928364 -3.3594874875767186673 --42.755222607849105998 - -4.2648515634753199066 --8.4629358700849355301 --22.252086323427270997 - -10.996977688149536689 --9.2123370180278048025 --29.206739014051372294 - -10.935342607054865383 --9.4338917557810741954 --29.112263909175499776 - -15.714024935401759819 -1.3745079651566265433 --59.286834979937104606 - --1.3624227800805182031 --4.1979357415396094666 --21.034430148188398846 - -6.7690173115899296974 --4.7352452433700786827 --53.605307875695892506 - From 120723b718fa26806b821d887caebe50c47886b5 Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Fri, 30 May 2014 16:41:45 -0400 Subject: [PATCH 07/60] ignore the rewritten file --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 6d274af09..07ebeb8fb 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ /build* /doc* *.pyc -*.DS_Store \ No newline at end of file +*.DS_Store +/examples/Data/dubrovnik-3-7-pre-rewritten.txt \ No newline at end of file From 1836b1c8423cf2dfcd30ec1d92215ee55a15a954 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Fri, 30 May 2014 17:07:09 -0400 Subject: [PATCH 08/60] silence Wunused-local-typedefs warning on GCC 4.8+ --- CMakeLists.txt | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 196b7e4df..b369e1fb0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -273,6 +273,13 @@ if(MSVC) add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344) # Disable non-DLL-exported base class and other warnings endif() +# GCC 4.8+ complains about local typedefs which we use for shared_ptr etc. +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8) + add_definitions(-Wno-unused-local-typedefs) + endif() +endif() + if(GTSAM_ENABLE_CONSISTENCY_CHECKS) add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS) endif() From f0257ee6196268d73cc075953620e7cecc17cb3a Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Fri, 30 May 2014 19:24:48 -0400 Subject: [PATCH 09/60] renamed testSphere2 ->testUnit3 --- gtsam/geometry/tests/{testSphere2.cpp => testUnit3.cpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename gtsam/geometry/tests/{testSphere2.cpp => testUnit3.cpp} (100%) diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testUnit3.cpp similarity index 100% rename from gtsam/geometry/tests/testSphere2.cpp rename to gtsam/geometry/tests/testUnit3.cpp From 41de41deec98fab862eafa47066f5d7833c26670 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Fri, 30 May 2014 19:37:03 -0400 Subject: [PATCH 10/60] minor cleanup --- .../examples/SmartProjectionFactorExample.cpp | 38 +++++++------------ 1 file changed, 13 insertions(+), 25 deletions(-) diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index a9c60afba..31b45cbc9 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -19,10 +19,11 @@ /** - * A smart projection factor example based on stereo data - * - robot starts at origin + * A smart projection factor example based on stereo data, throwing away the + * measurement from the right camera + * -robot starts at origin * -moves forward, taking periodic stereo measurements - * -takes stereo readings of many landmarks + * -makes monocular observations of many landmarks */ #include @@ -32,7 +33,6 @@ #include #include #include -#include #include #include @@ -58,18 +58,16 @@ int main(int argc, char** argv){ //read camera calibration info from file // focal lengths fx, fy, skew s, principal point u0, v0, baseline b - double fx, fy, s, u0, v0, b; - ifstream calibration_file(calibration_loc); cout << "Reading calibration info" << endl; - calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; + ifstream calibration_file(calibration_loc.c_str()); - //create stereo camera calibration object -// const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b)); - + double fx, fy, s, u0, v0, b; + calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; const Cal3_S2::shared_ptr K(new Cal3_S2(fx, fy, s, u0, v0)); - ifstream pose_file(pose_loc); cout << "Reading camera poses" << endl; + ifstream pose_file(pose_loc.c_str()); + int pose_id; MatrixRowMajor m(4,4); //read camera pose parameters and use to make initial estimates of camera poses @@ -86,17 +84,15 @@ int main(int argc, char** argv){ // pixel coordinates uL, uR, v (same for left/right images due to rectification) // landmark coordinates X, Y, Z in camera frame, resulting from triangulation double uL, uR, v, X, Y, Z; - ifstream factor_file(factor_loc); + ifstream factor_file(factor_loc.c_str()); cout << "Reading stereo factors" << endl; - //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation + + //read stereo measurements and construct smart factors SmartFactor::shared_ptr factor(new SmartFactor()); - size_t current_l = 3; // TODO: Fix me! + size_t current_l = 3; // hardcoded landmark ID from first measurement while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { -// graph.push_back( -// GenericStereoFactor(StereoPoint2(uL, uR, v), model, -// Symbol('x', x), Symbol('l', l), K)); if(current_l != l) { graph.push_back(factor); @@ -104,14 +100,6 @@ int main(int argc, char** argv){ current_l = l; } factor->add(Point2(uL,v), Symbol('x',x), model, K); - - //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it -// if (!initial_estimate.exists(Symbol('l', l))) { -// Pose3 camPose = initial_estimate.at(Symbol('x', x)); -// //transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space -// Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z)); -// initial_estimate.insert(Symbol('l', l), worldPoint); -// } } Pose3 first_pose = initial_estimate.at(Symbol('x',1)); From 732409faf38afa0bfd658c0161064419af65573b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 12:51:37 -0400 Subject: [PATCH 11/60] Changed filenames --- gtsam/nonlinear/{LagoInitializer.cpp => lago.cpp} | 4 ++-- gtsam/nonlinear/{LagoInitializer.h => lago.h} | 2 +- .../nonlinear/tests/{testLagoInitializer.cpp => testLago.cpp} | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) rename gtsam/nonlinear/{LagoInitializer.cpp => lago.cpp} (99%) rename gtsam/nonlinear/{LagoInitializer.h => lago.h} (99%) rename gtsam/nonlinear/tests/{testLagoInitializer.cpp => testLago.cpp} (99%) diff --git a/gtsam/nonlinear/LagoInitializer.cpp b/gtsam/nonlinear/lago.cpp similarity index 99% rename from gtsam/nonlinear/LagoInitializer.cpp rename to gtsam/nonlinear/lago.cpp index 3451beaf3..e2f13fded 100644 --- a/gtsam/nonlinear/LagoInitializer.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -10,13 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * @file LagoInitializer.h + * @file lago.h * @author Luca Carlone * @author Frank Dellaert * @date May 14, 2014 */ -#include +#include #include #include diff --git a/gtsam/nonlinear/LagoInitializer.h b/gtsam/nonlinear/lago.h similarity index 99% rename from gtsam/nonlinear/LagoInitializer.h rename to gtsam/nonlinear/lago.h index b351365c1..91f6ecd97 100644 --- a/gtsam/nonlinear/LagoInitializer.h +++ b/gtsam/nonlinear/lago.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file LagoInitializer.h + * @file lago.h * @brief Initialize Pose2 in a factor graph using LAGO * (Linear Approximation for Graph Optimization). see papers: * diff --git a/gtsam/nonlinear/tests/testLagoInitializer.cpp b/gtsam/nonlinear/tests/testLago.cpp similarity index 99% rename from gtsam/nonlinear/tests/testLagoInitializer.cpp rename to gtsam/nonlinear/tests/testLago.cpp index 64e43ae9b..576266f22 100644 --- a/gtsam/nonlinear/tests/testLagoInitializer.cpp +++ b/gtsam/nonlinear/tests/testLago.cpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include From 4afb11a2eda5cfe89b273af92eefa15db041ba03 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 12:51:47 -0400 Subject: [PATCH 12/60] Fixed up examples --- .cproject | 162 +++++++++++++++++------------ examples/Pose2SLAMExample_g2o.cpp | 35 +++---- examples/Pose2SLAMExample_lago.cpp | 38 +++---- 3 files changed, 131 insertions(+), 104 deletions(-) diff --git a/.cproject b/.cproject index af6b32cd2..a43d32f58 100644 --- a/.cproject +++ b/.cproject @@ -568,6 +568,7 @@ make + tests/testBayesTree.run true false @@ -575,6 +576,7 @@ make + testBinaryBayesNet.run true false @@ -622,6 +624,7 @@ make + testSymbolicBayesNet.run true false @@ -629,6 +632,7 @@ make + tests/testSymbolicFactor.run true false @@ -636,6 +640,7 @@ make + testSymbolicFactorGraph.run true false @@ -651,6 +656,7 @@ make + tests/testBayesTree true false @@ -728,46 +734,6 @@ true true - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run - true - true - true - make -j2 @@ -1114,6 +1080,7 @@ make + testErrors.run true false @@ -1159,6 +1126,14 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + make -j2 @@ -1239,14 +1214,6 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - make -j2 @@ -1351,6 +1318,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j2 @@ -1433,7 +1416,6 @@ make - testSimulated2DOriented.run true false @@ -1473,7 +1455,6 @@ make - testSimulated2D.run true false @@ -1481,7 +1462,6 @@ make - testSimulated3D.run true false @@ -1495,22 +1475,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j5 @@ -1768,6 +1732,7 @@ cpack + -G DEB true false @@ -1775,6 +1740,7 @@ cpack + -G RPM true false @@ -1782,6 +1748,7 @@ cpack + -G TGZ true false @@ -1789,6 +1756,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2547,6 +2515,7 @@ make + testGraph.run true false @@ -2554,6 +2523,7 @@ make + testJunctionTree.run true false @@ -2561,6 +2531,7 @@ make + testSymbolicBayesNetB.run true false @@ -2830,6 +2801,62 @@ true true + + make + -j5 + Pose2SLAMExample_lago.run + true + true + true + + + make + -j5 + Pose2SLAMExample_g2o.run + true + true + true + + + make + -j5 + testLago.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testWhiteNoiseFactor.run + true + true + true + make -j4 @@ -2848,7 +2875,6 @@ make - tests/testGaussianISAM2 true false diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 393bba86d..565a26965 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -18,27 +18,22 @@ * @author Luca Carlone */ -#include -#include -#include #include -#include -#include +#include #include -#include -#include #include -#include using namespace std; using namespace gtsam; +int main(const int argc, const char *argv[]) { -int main(const int argc, const char *argv[]){ - + // Read graph from file + string g2oFile; if (argc < 2) - std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl; - const string g2oFile = argv[1]; + g2oFile = "../../examples/Data/noisyToyGraph.txt"; + else + g2oFile = argv[1]; NonlinearFactorGraph graph; Values initial; @@ -46,7 +41,8 @@ int main(const int argc, const char *argv[]){ // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = graph; - noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); std::cout << "Optimizing the factor graph" << std::endl; @@ -54,10 +50,13 @@ int main(const int argc, const char *argv[]){ Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; - const string outputFile = argv[2]; - std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(outputFile, graph, result); - std::cout << "done! " << std::endl; - + if (argc < 3) { + result.print("result"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(outputFile, graph, result); + std::cout << "done! " << std::endl; + } return 0; } diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index d61f1b533..c84ea8c7c 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -12,33 +12,29 @@ /** * @file Pose2SLAMExample_lago.cpp * @brief A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem - * using LAGO (Linear Approximation for Graph Optimization). See class LagoInitializer.h + * using LAGO (Linear Approximation for Graph Optimization). See class lago.h * Output is written on a file, in g2o format * Syntax for the script is ./Pose2SLAMExample_lago input.g2o output.g2o * @date May 15, 2014 * @author Luca Carlone */ -#include -#include -#include +#include #include -#include -#include -#include -#include +#include #include -#include using namespace std; using namespace gtsam; +int main(const int argc, const char *argv[]) { -int main(const int argc, const char *argv[]){ - + // Read graph from file + string g2oFile; if (argc < 2) - std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl; - const string g2oFile = argv[1]; + g2oFile = "../../examples/Data/noisyToyGraph.txt"; + else + g2oFile = argv[1]; NonlinearFactorGraph graph; Values initial; @@ -46,17 +42,23 @@ int main(const int argc, const char *argv[]){ // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = graph; - noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + graphWithPrior.print(); std::cout << "Computing LAGO estimate" << std::endl; Values estimateLago = initializeLago(graphWithPrior); std::cout << "done!" << std::endl; - const string outputFile = argv[2]; - std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(outputFile, graph, estimateLago); - std::cout << "done! " << std::endl; + if (argc < 3) { + estimateLago.print("estimateLago"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(outputFile, graph, estimateLago); + std::cout << "done! " << std::endl; + } return 0; } From acdc3304e05e50556746a01eed11a8a3ed5ae847 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:00:08 -0400 Subject: [PATCH 13/60] Cleaned up headers --- gtsam/nonlinear/lago.cpp | 5 ++++- gtsam/nonlinear/lago.h | 8 ++------ 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index e2f13fded..08fa8175f 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -17,7 +17,10 @@ */ #include -#include +#include +#include +#include + #include namespace gtsam { diff --git a/gtsam/nonlinear/lago.h b/gtsam/nonlinear/lago.h index 91f6ecd97..4450e1108 100644 --- a/gtsam/nonlinear/lago.h +++ b/gtsam/nonlinear/lago.h @@ -34,15 +34,11 @@ #pragma once -#include -#include +#include +#include #include #include #include -#include -#include -#include -#include namespace gtsam { From fe33c80b5f7ad961af0fb5b1a905d01f9a9aaadf Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:04:15 -0400 Subject: [PATCH 14/60] Introduced namespace --- examples/Pose2SLAMExample_lago.cpp | 2 +- gtsam/nonlinear/lago.cpp | 2 ++ gtsam/nonlinear/lago.h | 2 ++ gtsam/nonlinear/tests/testLago.cpp | 1 + 4 files changed, 6 insertions(+), 1 deletion(-) diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index c84ea8c7c..75072d7da 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -48,7 +48,7 @@ int main(const int argc, const char *argv[]) { graphWithPrior.print(); std::cout << "Computing LAGO estimate" << std::endl; - Values estimateLago = initializeLago(graphWithPrior); + Values estimateLago = lago::initializeLago(graphWithPrior); std::cout << "done!" << std::endl; if (argc < 3) { diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index 08fa8175f..28de71b8b 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -24,6 +24,7 @@ #include namespace gtsam { +namespace lago { static Matrix I = eye(1); static Matrix I3 = eye(3); @@ -347,4 +348,5 @@ Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGu return initialGuessLago; } +} // end of namespace lago } // end of namespace gtsam diff --git a/gtsam/nonlinear/lago.h b/gtsam/nonlinear/lago.h index 4450e1108..85d9863b1 100644 --- a/gtsam/nonlinear/lago.h +++ b/gtsam/nonlinear/lago.h @@ -41,6 +41,7 @@ #include namespace gtsam { +namespace lago { typedef std::map key2doubleMap; const Key keyAnchor = symbol('Z',9999999); @@ -93,4 +94,5 @@ GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, bool useOd /* Only corrects the orientation part in initialGuess */ GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess); +} // end of namespace lago } // end of namespace gtsam diff --git a/gtsam/nonlinear/tests/testLago.cpp b/gtsam/nonlinear/tests/testLago.cpp index 576266f22..29dbcc720 100644 --- a/gtsam/nonlinear/tests/testLago.cpp +++ b/gtsam/nonlinear/tests/testLago.cpp @@ -37,6 +37,7 @@ using namespace std; using namespace gtsam; +using namespace lago; using namespace boost::assign; Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3); From 1d43a1f2065baf64870b64c74e5e0ef31dc73b12 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:13:14 -0400 Subject: [PATCH 15/60] removed redundant "Lago" from several function names --- examples/Pose2SLAMExample_lago.cpp | 2 +- gtsam/nonlinear/lago.cpp | 26 +++++++----- gtsam/nonlinear/lago.h | 11 ++--- gtsam/nonlinear/tests/testLago.cpp | 68 +++++++++++++++--------------- 4 files changed, 54 insertions(+), 53 deletions(-) diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index 75072d7da..a6340caf7 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -48,7 +48,7 @@ int main(const int argc, const char *argv[]) { graphWithPrior.print(); std::cout << "Computing LAGO estimate" << std::endl; - Values estimateLago = lago::initializeLago(graphWithPrior); + Values estimateLago = lago::initialize(graphWithPrior); std::cout << "done!" << std::endl; if (argc < 3) { diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index 28de71b8b..4f838ca96 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -26,8 +26,12 @@ namespace gtsam { namespace lago { -static Matrix I = eye(1); -static Matrix I3 = eye(3); +static const Matrix I = eye(1); +static const Matrix I3 = eye(3); + +static const Key keyAnchor = symbol('Z',9999999); +static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); +static const noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); /* ************************************************************************* */ double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, @@ -216,7 +220,7 @@ PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { } /* ************************************************************************* */ -VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){ +VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){ // Find a minimum spanning tree PredecessorMap tree; @@ -244,18 +248,18 @@ VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, boo } /* ************************************************************************* */ -VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath) { +VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useOdometricPath) { // We "extract" the Pose2 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph NonlinearFactorGraph pose2Graph = buildPose2graph(graph); // Get orientations from relative orientation measurements - return computeLagoOrientations(pose2Graph, useOdometricPath); + return computeOrientations(pose2Graph, useOdometricPath); } /* ************************************************************************* */ -Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { +Values computePoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { // Linearized graph on full poses GaussianFactorGraph linearPose2graph; @@ -315,25 +319,25 @@ Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& or } /* ************************************************************************* */ -Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath) { +Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) { // We "extract" the Pose2 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph NonlinearFactorGraph pose2Graph = buildPose2graph(graph); // Get orientations from relative orientation measurements - VectorValues orientationsLago = computeLagoOrientations(pose2Graph, useOdometricPath); + VectorValues orientationsLago = computeOrientations(pose2Graph, useOdometricPath); // Compute the full poses - return computeLagoPoses(pose2Graph, orientationsLago); + return computePoses(pose2Graph, orientationsLago); } /* ************************************************************************* */ -Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) { +Values initialize(const NonlinearFactorGraph& graph, const Values& initialGuess) { Values initialGuessLago; // get the orientation estimates from LAGO - VectorValues orientations = initializeOrientationsLago(graph); + VectorValues orientations = initializeOrientations(graph); // for all nodes in the tree for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){ diff --git a/gtsam/nonlinear/lago.h b/gtsam/nonlinear/lago.h index 85d9863b1..094b4325d 100644 --- a/gtsam/nonlinear/lago.h +++ b/gtsam/nonlinear/lago.h @@ -44,9 +44,6 @@ namespace gtsam { namespace lago { typedef std::map key2doubleMap; -const Key keyAnchor = symbol('Z',9999999); -noiseModel::Diagonal::shared_ptr priorOrientationNoise = noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); -noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); /* This function computes the cumulative orientation (without wrapping) wrt the root of a spanning tree (tree) * for a node (nodeKey). The function starts at the nodes and moves towards the root @@ -83,16 +80,16 @@ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const std::vector */ -GTSAM_EXPORT VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); +GTSAM_EXPORT VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); /* LAGO: Returns the orientations of the Pose2 in a generic factor graph */ -GTSAM_EXPORT VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true); +GTSAM_EXPORT VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useOdometricPath = true); /* Returns the values for the Pose2 in a generic factor graph */ -GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true); +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath = true); /* Only corrects the orientation part in initialGuess */ -GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess); +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& initialGuess); } // end of namespace lago } // end of namespace gtsam diff --git a/gtsam/nonlinear/tests/testLago.cpp b/gtsam/nonlinear/tests/testLago.cpp index 29dbcc720..867e44089 100644 --- a/gtsam/nonlinear/tests/testLago.cpp +++ b/gtsam/nonlinear/tests/testLago.cpp @@ -148,25 +148,25 @@ TEST( Lago, regularizedMeasurements ) { /* *************************************************************************** */ TEST( Lago, smallGraphVectorValues ) { bool useOdometricPath = false; - VectorValues initialGuessLago = initializeOrientationsLago(simple::graph(), useOdometricPath); + VectorValues initial = initializeOrientations(simple::graph(), useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); } /* *************************************************************************** */ TEST( Lago, smallGraphVectorValuesSP ) { - VectorValues initialGuessLago = initializeOrientationsLago(simple::graph()); + VectorValues initial = initializeOrientations(simple::graph()); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -174,26 +174,26 @@ TEST( Lago, multiplePosePriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1, model)); - VectorValues initialGuessLago = initializeOrientationsLago(g, useOdometricPath); + VectorValues initial = initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); } /* *************************************************************************** */ TEST( Lago, multiplePosePriorsSP ) { NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1, model)); - VectorValues initialGuessLago = initializeOrientationsLago(g); + VectorValues initial = initializeOrientations(g); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -201,26 +201,26 @@ TEST( Lago, multiplePoseAndRotPriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1.theta(), model)); - VectorValues initialGuessLago = initializeOrientationsLago(g, useOdometricPath); + VectorValues initial = initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); } /* *************************************************************************** */ TEST( Lago, multiplePoseAndRotPriorsSP ) { NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1.theta(), model)); - VectorValues initialGuessLago = initializeOrientationsLago(g); + VectorValues initial = initializeOrientations(g); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -234,7 +234,7 @@ TEST( Lago, smallGraphValues ) { initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0)); // lago does not touch the Cartesian part and only fixed the orientations - Values actual = initializeLago(simple::graph(), initialGuess); + Values actual = lago::initialize(simple::graph(), initialGuess); // we are in a noiseless case Values expected; @@ -250,7 +250,7 @@ TEST( Lago, smallGraphValues ) { TEST( Lago, smallGraph2 ) { // lago does not touch the Cartesian part and only fixed the orientations - Values actual = initializeLago(simple::graph()); + Values actual = lago::initialize(simple::graph()); // we are in a noiseless case Values expected; @@ -275,7 +275,7 @@ TEST( Lago, largeGraphNoisy_orientations ) { noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - VectorValues actualVV = initializeOrientationsLago(graphWithPrior); + VectorValues actualVV = initializeOrientations(graphWithPrior); Values actual; Key keyAnc = symbol('Z',9999999); for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){ @@ -310,7 +310,7 @@ TEST( Lago, largeGraphNoisy ) { noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - Values actual = initializeLago(graphWithPrior); + Values actual = lago::initialize(graphWithPrior); NonlinearFactorGraph gmatlab; Values expected; From 0ce70befc400d33c7f8e05cb2cbb8bc12c308086 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sat, 31 May 2014 13:13:48 -0400 Subject: [PATCH 16/60] version number --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b369e1fb0..933f2083e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 2.6) # Set the version number for the library set (GTSAM_VERSION_MAJOR 3) -set (GTSAM_VERSION_MINOR 0) +set (GTSAM_VERSION_MINOR 1) set (GTSAM_VERSION_PATCH 0) math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") From 9b8af1bf6efeb36fa5e14157cc73c7bbfd53cdea Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:15:51 -0400 Subject: [PATCH 17/60] Use namespace explicitly --- gtsam/nonlinear/tests/testLago.cpp | 37 +++++++++++++++--------------- 1 file changed, 18 insertions(+), 19 deletions(-) diff --git a/gtsam/nonlinear/tests/testLago.cpp b/gtsam/nonlinear/tests/testLago.cpp index 867e44089..4b116a108 100644 --- a/gtsam/nonlinear/tests/testLago.cpp +++ b/gtsam/nonlinear/tests/testLago.cpp @@ -37,7 +37,6 @@ using namespace std; using namespace gtsam; -using namespace lago; using namespace boost::assign; Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3); @@ -78,10 +77,10 @@ TEST( Lago, checkSTandChords ) { PredecessorMap tree = findMinimumSpanningTree >(g); - key2doubleMap deltaThetaMap; + lago::key2doubleMap deltaThetaMap; vector spanningTreeIds; // ids of between factors forming the spanning tree T vector chordsIds; // ids of between factors corresponding to chordsIds wrt T - getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); + lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); DOUBLES_EQUAL(spanningTreeIds[0], 0, 1e-6); // factor 0 is the first in the ST (0->1) DOUBLES_EQUAL(spanningTreeIds[1], 3, 1e-6); // factor 3 is the second in the ST(2->0) @@ -101,19 +100,19 @@ TEST( Lago, orientationsOverSpanningTree ) { EXPECT_LONGS_EQUAL(tree[x2], x0); EXPECT_LONGS_EQUAL(tree[x3], x0); - key2doubleMap expected; + lago::key2doubleMap expected; expected[x0]= 0; expected[x1]= M_PI/2; // edge x0->x1 (consistent with edge (x0,x1)) expected[x2]= -M_PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0)) expected[x3]= -M_PI/2; // edge x0->x3 (consistent with edge (x0,x3)) - key2doubleMap deltaThetaMap; + lago::key2doubleMap deltaThetaMap; vector spanningTreeIds; // ids of between factors forming the spanning tree T vector chordsIds; // ids of between factors corresponding to chordsIds wrt T - getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); + lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); - key2doubleMap actual; - actual = computeThetasToRoot(deltaThetaMap, tree); + lago::key2doubleMap actual; + actual = lago::computeThetasToRoot(deltaThetaMap, tree); DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6); DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6); DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6); @@ -126,14 +125,14 @@ TEST( Lago, regularizedMeasurements ) { PredecessorMap tree = findMinimumSpanningTree >(g); - key2doubleMap deltaThetaMap; + lago::key2doubleMap deltaThetaMap; vector spanningTreeIds; // ids of between factors forming the spanning tree T vector chordsIds; // ids of between factors corresponding to chordsIds wrt T - getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); + lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); - key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); + lago::key2doubleMap orientationsToRoot = lago::computeThetasToRoot(deltaThetaMap, tree); - GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree); + GaussianFactorGraph lagoGraph = lago::buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree); std::pair actualAb = lagoGraph.jacobian(); // jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded) Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4)); @@ -148,7 +147,7 @@ TEST( Lago, regularizedMeasurements ) { /* *************************************************************************** */ TEST( Lago, smallGraphVectorValues ) { bool useOdometricPath = false; - VectorValues initial = initializeOrientations(simple::graph(), useOdometricPath); + VectorValues initial = lago::initializeOrientations(simple::graph(), useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); @@ -160,7 +159,7 @@ TEST( Lago, smallGraphVectorValues ) { /* *************************************************************************** */ TEST( Lago, smallGraphVectorValuesSP ) { - VectorValues initial = initializeOrientations(simple::graph()); + VectorValues initial = lago::initializeOrientations(simple::graph()); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); @@ -174,7 +173,7 @@ TEST( Lago, multiplePosePriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1, model)); - VectorValues initial = initializeOrientations(g, useOdometricPath); + VectorValues initial = lago::initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); @@ -187,7 +186,7 @@ TEST( Lago, multiplePosePriors ) { TEST( Lago, multiplePosePriorsSP ) { NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1, model)); - VectorValues initial = initializeOrientations(g); + VectorValues initial = lago::initializeOrientations(g); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); @@ -201,7 +200,7 @@ TEST( Lago, multiplePoseAndRotPriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1.theta(), model)); - VectorValues initial = initializeOrientations(g, useOdometricPath); + VectorValues initial = lago::initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); @@ -214,7 +213,7 @@ TEST( Lago, multiplePoseAndRotPriors ) { TEST( Lago, multiplePoseAndRotPriorsSP ) { NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1.theta(), model)); - VectorValues initial = initializeOrientations(g); + VectorValues initial = lago::initializeOrientations(g); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); @@ -275,7 +274,7 @@ TEST( Lago, largeGraphNoisy_orientations ) { noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - VectorValues actualVV = initializeOrientations(graphWithPrior); + VectorValues actualVV = lago::initializeOrientations(graphWithPrior); Values actual; Key keyAnc = symbol('Z',9999999); for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){ From dfb620b5decef984927a39cbdc10ed74b72e0ae4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:21:34 -0400 Subject: [PATCH 18/60] Header discipline --- gtsam/nonlinear/lago.cpp | 1 + gtsam/nonlinear/lago.h | 1 - gtsam/nonlinear/tests/testLago.cpp | 16 +++++----------- 3 files changed, 6 insertions(+), 12 deletions(-) diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index 4f838ca96..792dad80c 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include diff --git a/gtsam/nonlinear/lago.h b/gtsam/nonlinear/lago.h index 094b4325d..8c8ef9a73 100644 --- a/gtsam/nonlinear/lago.h +++ b/gtsam/nonlinear/lago.h @@ -34,7 +34,6 @@ #pragma once -#include #include #include #include diff --git a/gtsam/nonlinear/tests/testLago.cpp b/gtsam/nonlinear/tests/testLago.cpp index 4b116a108..f2cab4506 100644 --- a/gtsam/nonlinear/tests/testLago.cpp +++ b/gtsam/nonlinear/tests/testLago.cpp @@ -19,20 +19,14 @@ * @date May 14, 2014 */ -#include - +#include +#include +#include +#include #include -#include -#include -#include - -#include -#include - -#include #include -#include + #include using namespace std; From e5344d3d92abbd6f15514d58b8dfd683e4985a31 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:26:13 -0400 Subject: [PATCH 19/60] Doxygen documentation --- gtsam/nonlinear/lago.h | 79 +++++++++++++++++++++++++----------------- 1 file changed, 48 insertions(+), 31 deletions(-) diff --git a/gtsam/nonlinear/lago.h b/gtsam/nonlinear/lago.h index 8c8ef9a73..fa8eef995 100644 --- a/gtsam/nonlinear/lago.h +++ b/gtsam/nonlinear/lago.h @@ -42,53 +42,70 @@ namespace gtsam { namespace lago { -typedef std::map key2doubleMap; +typedef std::map key2doubleMap; -/* This function computes the cumulative orientation (without wrapping) wrt the root of a spanning tree (tree) - * for a node (nodeKey). The function starts at the nodes and moves towards the root - * summing up the (directed) rotation measurements. Relative measurements are encoded in "deltaThetaMap" - * The root is assumed to have orientation zero. +/** + * Compute the cumulative orientation (without wrapping) wrt the root of a + * spanning tree (tree) for a node (nodeKey). The function starts at the nodes and + * moves towards the root summing up the (directed) rotation measurements. + * Relative measurements are encoded in "deltaThetaMap". + * The root is assumed to have orientation zero. */ -GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, - const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap); +GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey, + const PredecessorMap& tree, const key2doubleMap& deltaThetaMap, + const key2doubleMap& thetaFromRootMap); -/* This function computes the cumulative orientations (without wrapping) - * for all node wrt the root (root has zero orientation) +/** + * Compute the cumulative orientations (without wrapping) + * for all nodes wrt the root (root has zero orientation). */ -GTSAM_EXPORT key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, - const PredecessorMap& tree); +GTSAM_EXPORT key2doubleMap computeThetasToRoot( + const key2doubleMap& deltaThetaMap, const PredecessorMap& tree); -/* Given a factor graph "g", and a spanning tree "tree", the function selects the nodes belonging to the tree and to g, - * and stores the factor slots corresponding to edges in the tree and to chordsIds wrt this tree - * Also it computes deltaThetaMap which is a fast way to encode relative orientations along the tree: - * for a node key2, s.t. tree[key2]=key1, the values deltaThetaMap[key2] is the relative orientation theta[key2]-theta[key1] +/** + * Given a factor graph "g", and a spanning tree "tree", select the nodes belonging + * to the tree and to g, and stores the factor slots corresponding to edges in the + * tree and to chordsIds wrt this tree. + * Also it computes deltaThetaMap which is a fast way to encode relative + * orientations along the tree: for a node key2, s.t. tree[key2]=key1, + * the value deltaThetaMap[key2] is relative orientation theta[key2]-theta[key1] */ GTSAM_EXPORT void getSymbolicGraph( - /*OUTPUTS*/ std::vector& spanningTreeIds, std::vector& chordsIds, key2doubleMap& deltaThetaMap, - /*INPUTS*/ const PredecessorMap& tree, const NonlinearFactorGraph& g); +/*OUTPUTS*/std::vector& spanningTreeIds, std::vector& chordsIds, + key2doubleMap& deltaThetaMap, + /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g); -/* Retrieves the deltaTheta and the corresponding noise model from a BetweenFactor */ +/** + * Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor + */ GTSAM_EXPORT void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta); -/* Linear factor graph with regularized orientation measurements */ -GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const std::vector& spanningTreeIds, const std::vector& chordsIds, - const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); +/** Linear factor graph with regularized orientation measurements */ +GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( + const std::vector& spanningTreeIds, + const std::vector& chordsIds, const NonlinearFactorGraph& g, + const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); -/* Selects the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */ -GTSAM_EXPORT NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph); +/** Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */ +GTSAM_EXPORT NonlinearFactorGraph buildPose2graph( + const NonlinearFactorGraph& graph); -/* Returns the orientations of a graph including only BetweenFactors */ -GTSAM_EXPORT VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); +/** Return the orientations of a graph including only BetweenFactors */ +GTSAM_EXPORT VectorValues computeOrientations( + const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); -/* LAGO: Returns the orientations of the Pose2 in a generic factor graph */ -GTSAM_EXPORT VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useOdometricPath = true); +/** LAGO: Return the orientations of the Pose2 in a generic factor graph */ +GTSAM_EXPORT VectorValues initializeOrientations( + const NonlinearFactorGraph& graph, bool useOdometricPath = true); -/* Returns the values for the Pose2 in a generic factor graph */ -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath = true); +/** Return the values for the Pose2 in a generic factor graph */ +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, + bool useOdometricPath = true); -/* Only corrects the orientation part in initialGuess */ -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& initialGuess); +/** Only correct the orientation part in initialGuess */ +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, + const Values& initialGuess); } // end of namespace lago } // end of namespace gtsam From 970d49f60b273b7e180931f966bacf1d118ae364 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:37:10 -0400 Subject: [PATCH 20/60] Standard formatting and some BOOST_FOREACH uses --- gtsam/nonlinear/lago.cpp | 205 ++++++++++++++++++++++----------------- 1 file changed, 118 insertions(+), 87 deletions(-) diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index 792dad80c..f8b089539 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -30,9 +30,11 @@ namespace lago { static const Matrix I = eye(1); static const Matrix I3 = eye(3); -static const Key keyAnchor = symbol('Z',9999999); -static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); -static const noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); +static const Key keyAnchor = symbol('Z', 9999999); +static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = + noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); +static const noiseModel::Diagonal::shared_ptr priorPose2Noise = + noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); /* ************************************************************************* */ double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, @@ -41,16 +43,16 @@ double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, double nodeTheta = 0; Key key_child = nodeKey; // the node Key key_parent = 0; // the initialization does not matter - while(1){ + while (1) { // We check if we reached the root - if(tree.at(key_child)==key_child) // if we reached the root + if (tree.at(key_child) == key_child) // if we reached the root break; // we sum the delta theta corresponding to the edge parent->child nodeTheta += deltaThetaMap.at(key_child); // we get the parent key_parent = tree.at(key_child); // the parent // we check if we connected to some part of the tree we know - if(thetaFromRootMap.find(key_parent)!=thetaFromRootMap.end()){ + if (thetaFromRootMap.find(key_parent) != thetaFromRootMap.end()) { nodeTheta += thetaFromRootMap.at(key_parent); break; } @@ -64,15 +66,14 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, const PredecessorMap& tree) { key2doubleMap thetaToRootMap; - key2doubleMap::const_iterator it; // Orientation of the roo thetaToRootMap.insert(std::pair(keyAnchor, 0.0)); // for all nodes in the tree - for(it = deltaThetaMap.begin(); it != deltaThetaMap.end(); ++it ){ + BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) { // compute the orientation wrt root - Key nodeKey = it->first; + Key nodeKey = it.first; double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, thetaToRootMap); thetaToRootMap.insert(std::pair(nodeKey, nodeTheta)); @@ -82,35 +83,38 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, /* ************************************************************************* */ void getSymbolicGraph( - /*OUTPUTS*/ std::vector& spanningTreeIds, std::vector& chordsIds, key2doubleMap& deltaThetaMap, - /*INPUTS*/ const PredecessorMap& tree, const NonlinearFactorGraph& g){ +/*OUTPUTS*/std::vector& spanningTreeIds, std::vector& chordsIds, + key2doubleMap& deltaThetaMap, + /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g) { // Get keys for which you want the orientation - size_t id=0; + size_t id = 0; // Loop over the factors - BOOST_FOREACH(const boost::shared_ptr& factor, g){ - if (factor->keys().size() == 2){ + BOOST_FOREACH(const boost::shared_ptr& factor, g) { + if (factor->keys().size() == 2) { Key key1 = factor->keys()[0]; Key key2 = factor->keys()[1]; // recast to a between - boost::shared_ptr< BetweenFactor > pose2Between = - boost::dynamic_pointer_cast< BetweenFactor >(factor); - if (!pose2Between) continue; + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); + if (!pose2Between) + continue; // get the orientation - measured().theta(); double deltaTheta = pose2Between->measured().theta(); // insert (directed) orientations in the map "deltaThetaMap" - bool inTree=false; - if(tree.at(key1)==key2){ // key2 -> key1 + bool inTree = false; + if (tree.at(key1) == key2) { // key2 -> key1 deltaThetaMap.insert(std::pair(key1, -deltaTheta)); inTree = true; - } else if(tree.at(key2)==key1){ // key1 -> key2 - deltaThetaMap.insert(std::pair(key2, deltaTheta)); + } else if (tree.at(key2) == key1) { // key1 -> key2 + deltaThetaMap.insert(std::pair(key2, deltaTheta)); inTree = true; } // store factor slot, distinguishing spanning tree edges from chordsIds - if(inTree == true) + if (inTree == true) spanningTreeIds.push_back(id); - else // it's a chord! + else + // it's a chord! chordsIds.push_back(id); } id++; @@ -125,7 +129,8 @@ void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, boost::shared_ptr > pose2Between = boost::dynamic_pointer_cast >(factor); if (!pose2Between) - throw std::invalid_argument("buildLinearOrientationGraph: invalid between factor!"); + throw std::invalid_argument( + "buildLinearOrientationGraph: invalid between factor!"); deltaTheta = (Vector(1) << pose2Between->measured().theta()); // Retrieve the noise model for the relative rotation @@ -133,63 +138,73 @@ void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, boost::shared_ptr diagonalModel = boost::dynamic_pointer_cast(model); if (!diagonalModel) - throw std::invalid_argument("buildLinearOrientationGraph: invalid noise model " - "(current version assumes diagonal noise model)!"); - Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2) ); // std on the angular measurement + throw std::invalid_argument( + "buildLinearOrientationGraph: invalid noise model " + "(current version assumes diagonal noise model)!"); + Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)); // std on the angular measurement model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta); } /* ************************************************************************* */ -GaussianFactorGraph buildLinearOrientationGraph(const std::vector& spanningTreeIds, const std::vector& chordsIds, - const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& tree){ +GaussianFactorGraph buildLinearOrientationGraph( + const std::vector& spanningTreeIds, + const std::vector& chordsIds, const NonlinearFactorGraph& g, + const key2doubleMap& orientationsToRoot, const PredecessorMap& tree) { GaussianFactorGraph lagoGraph; Vector deltaTheta; noiseModel::Diagonal::shared_ptr model_deltaTheta; // put original measurements in the spanning tree - BOOST_FOREACH(const size_t& factorId, spanningTreeIds){ + BOOST_FOREACH(const size_t& factorId, spanningTreeIds) { const FastVector& keys = g[factorId]->keys(); Key key1 = keys[0], key2 = keys[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); - lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta)); + lagoGraph.add( + JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta)); } // put regularized measurements in the chordsIds - BOOST_FOREACH(const size_t& factorId, chordsIds){ + BOOST_FOREACH(const size_t& factorId, chordsIds) { const FastVector& keys = g[factorId]->keys(); Key key1 = keys[0], key2 = keys[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); double key1_DeltaTheta_key2 = deltaTheta(0); ///std::cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << std::endl; - double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord - double k = boost::math::round(k2pi_noise/(2*M_PI)); + double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) + - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord + double k = boost::math::round(k2pi_noise / (2 * M_PI)); //if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug - Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI); - lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta)); + Vector deltaThetaRegularized = (Vector(1) + << key1_DeltaTheta_key2 - 2 * k * M_PI); + lagoGraph.add( + JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, + model_deltaTheta)); } // prior on the anchor orientation - lagoGraph.add(JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise)); + lagoGraph.add( + JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise)); return lagoGraph; } /* ************************************************************************* */ -NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph){ +NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { NonlinearFactorGraph pose2Graph; - BOOST_FOREACH(const boost::shared_ptr& factor, graph){ + BOOST_FOREACH(const boost::shared_ptr& factor, graph) { // recast to a between on Pose2 - boost::shared_ptr< BetweenFactor > pose2Between = - boost::dynamic_pointer_cast< BetweenFactor >(factor); + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); if (pose2Between) pose2Graph.add(pose2Between); // recast PriorFactor to BetweenFactor - boost::shared_ptr< PriorFactor > pose2Prior = - boost::dynamic_pointer_cast< PriorFactor >(factor); + boost::shared_ptr > pose2Prior = + boost::dynamic_pointer_cast >(factor); if (pose2Prior) - pose2Graph.add(BetweenFactor(keyAnchor, pose2Prior->keys()[0], - pose2Prior->prior(), pose2Prior->get_noiseModel())); + pose2Graph.add( + BetweenFactor(keyAnchor, pose2Prior->keys()[0], + pose2Prior->prior(), pose2Prior->get_noiseModel())); } return pose2Graph; } @@ -201,46 +216,49 @@ PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { Key minKey; bool minUnassigned = true; - BOOST_FOREACH(const boost::shared_ptr& factor, pose2Graph){ + BOOST_FOREACH(const boost::shared_ptr& factor, pose2Graph) { Key key1 = std::min(factor->keys()[0], factor->keys()[1]); Key key2 = std::max(factor->keys()[0], factor->keys()[1]); - if(minUnassigned){ + if (minUnassigned) { minKey = key1; minUnassigned = false; } - if( key2 - key1 == 1){ // consecutive keys + if (key2 - key1 == 1) { // consecutive keys tree.insert(key2, key1); - if(key1 < minKey) + if (key1 < minKey) minKey = key1; } } - tree.insert(minKey,keyAnchor); - tree.insert(keyAnchor,keyAnchor); // root + tree.insert(minKey, keyAnchor); + tree.insert(keyAnchor, keyAnchor); // root return tree; } /* ************************************************************************* */ -VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){ +VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, + bool useOdometricPath) { // Find a minimum spanning tree PredecessorMap tree; if (useOdometricPath) tree = findOdometricPath(pose2Graph); else - tree = findMinimumSpanningTree >(pose2Graph); + tree = findMinimumSpanningTree >(pose2Graph); // Create a linear factor graph (LFG) of scalars key2doubleMap deltaThetaMap; std::vector spanningTreeIds; // ids of between factors forming the spanning tree T - std::vector chordsIds; // ids of between factors corresponding to chordsIds wrt T + std::vector chordsIds; // ids of between factors corresponding to chordsIds wrt T getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph); // temporary structure to correct wraparounds along loops key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); // regularize measurements and plug everything in a factor graph - GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree); + GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, + chordsIds, pose2Graph, orientationsToRoot, tree); // Solve the LFG VectorValues orientationsLago = lagoGraph.optimize(); @@ -249,7 +267,8 @@ VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool us } /* ************************************************************************* */ -VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useOdometricPath) { +VectorValues initializeOrientations(const NonlinearFactorGraph& graph, + bool useOdometricPath) { // We "extract" the Pose2 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph @@ -260,59 +279,69 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useO } /* ************************************************************************* */ -Values computePoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { +Values computePoses(const NonlinearFactorGraph& pose2graph, + VectorValues& orientationsLago) { // Linearized graph on full poses GaussianFactorGraph linearPose2graph; // We include the linear version of each between factor - BOOST_FOREACH(const boost::shared_ptr& factor, pose2graph){ + BOOST_FOREACH(const boost::shared_ptr& factor, pose2graph) { - boost::shared_ptr< BetweenFactor > pose2Between = - boost::dynamic_pointer_cast< BetweenFactor >(factor); + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); - if(pose2Between){ + if (pose2Between) { Key key1 = pose2Between->keys()[0]; double theta1 = orientationsLago.at(key1)(0); - double s1 = sin(theta1); double c1 = cos(theta1); + double s1 = sin(theta1); + double c1 = cos(theta1); Key key2 = pose2Between->keys()[1]; double theta2 = orientationsLago.at(key2)(0); - double linearDeltaRot = theta2 - theta1 - pose2Between->measured().theta(); + double linearDeltaRot = theta2 - theta1 + - pose2Between->measured().theta(); linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize double dx = pose2Between->measured().x(); double dy = pose2Between->measured().y(); - Vector globalDeltaCart = (Vector(2) << c1*dx - s1*dy, s1*dx + c1*dy); - Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot );// rhs - Matrix J1 = - I3; - J1(0,2) = s1*dx + c1*dy; - J1(1,2) = -c1*dx + s1*dy; + Vector globalDeltaCart = (Vector(2) << c1 * dx - s1 * dy, s1 * dx + + c1 * dy); + Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs + Matrix J1 = -I3; + J1(0, 2) = s1 * dx + c1 * dy; + J1(1, 2) = -c1 * dx + s1 * dy; // Retrieve the noise model for the relative rotation boost::shared_ptr diagonalModel = - boost::dynamic_pointer_cast(pose2Between->get_noiseModel()); + boost::dynamic_pointer_cast( + pose2Between->get_noiseModel()); - linearPose2graph.add(JacobianFactor(key1, J1, key2, I3, b, diagonalModel)); - }else{ - throw std::invalid_argument("computeLagoPoses: cannot manage non between factor here!"); + linearPose2graph.add( + JacobianFactor(key1, J1, key2, I3, b, diagonalModel)); + } else { + throw std::invalid_argument( + "computeLagoPoses: cannot manage non between factor here!"); } } // add prior - noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); - linearPose2graph.add(JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0,0.0,0.0), priorModel)); + noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances( + (Vector(3) << 1e-2, 1e-2, 1e-4)); + linearPose2graph.add( + JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), priorModel)); // optimize VectorValues posesLago = linearPose2graph.optimize(); // put into Values structure Values initialGuessLago; - for(VectorValues::const_iterator it = posesLago.begin(); it != posesLago.end(); ++it ){ - Key key = it->first; - if (key != keyAnchor){ - Vector poseVector = posesLago.at(key); - Pose2 poseLago = Pose2(poseVector(0),poseVector(1),orientationsLago.at(key)(0)+poseVector(2)); + BOOST_FOREACH(const VectorValues::value_type& it, posesLago) { + Key key = it.first; + if (key != keyAnchor) { + const Vector& poseVector = it.second; + Pose2 poseLago = Pose2(poseVector(0), poseVector(1), + orientationsLago.at(key)(0) + poseVector(2)); initialGuessLago.insert(key, poseLago); } } @@ -327,26 +356,28 @@ Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) { NonlinearFactorGraph pose2Graph = buildPose2graph(graph); // Get orientations from relative orientation measurements - VectorValues orientationsLago = computeOrientations(pose2Graph, useOdometricPath); + VectorValues orientationsLago = computeOrientations(pose2Graph, + useOdometricPath); // Compute the full poses return computePoses(pose2Graph, orientationsLago); } /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph, const Values& initialGuess) { +Values initialize(const NonlinearFactorGraph& graph, + const Values& initialGuess) { Values initialGuessLago; // get the orientation estimates from LAGO VectorValues orientations = initializeOrientations(graph); // for all nodes in the tree - for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){ - Key key = it->first; - if (key != keyAnchor){ - Pose2 pose = initialGuess.at(key); - Vector orientation = orientations.at(key); - Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0)); + BOOST_FOREACH(const VectorValues::value_type& it, orientations) { + Key key = it.first; + if (key != keyAnchor) { + const Pose2& pose = initialGuess.at(key); + const Vector& orientation = it.second; + Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0)); initialGuessLago.insert(key, poseLago); } } From 8d54460d2c2fc799b2f2b831c6bebbc03a409abf Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:44:23 -0400 Subject: [PATCH 21/60] Made three helper functions static (not directly called in testLago) --- gtsam/nonlinear/lago.cpp | 24 ++++++++++++++++++------ gtsam/nonlinear/lago.h | 25 ------------------------- 2 files changed, 18 insertions(+), 31 deletions(-) diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index f8b089539..c2a7d9454 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -37,8 +37,16 @@ static const noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); /* ************************************************************************* */ -double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, - const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap) { +/** + * Compute the cumulative orientation (without wrapping) wrt the root of a + * spanning tree (tree) for a node (nodeKey). The function starts at the nodes and + * moves towards the root summing up the (directed) rotation measurements. + * Relative measurements are encoded in "deltaThetaMap". + * The root is assumed to have orientation zero. + */ +static double computeThetaToRoot(const Key nodeKey, + const PredecessorMap& tree, const key2doubleMap& deltaThetaMap, + const key2doubleMap& thetaFromRootMap) { double nodeTheta = 0; Key key_child = nodeKey; // the node @@ -122,7 +130,8 @@ void getSymbolicGraph( } /* ************************************************************************* */ -void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, +// Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor +static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) { // Get the relative rotation measurement from the between factor @@ -187,7 +196,8 @@ GaussianFactorGraph buildLinearOrientationGraph( } /* ************************************************************************* */ -NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { +// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node +static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { NonlinearFactorGraph pose2Graph; BOOST_FOREACH(const boost::shared_ptr& factor, graph) { @@ -210,7 +220,8 @@ NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { } /* ************************************************************************* */ -PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { +static PredecessorMap findOdometricPath( + const NonlinearFactorGraph& pose2Graph) { PredecessorMap tree; Key minKey; @@ -236,7 +247,8 @@ PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { } /* ************************************************************************* */ -VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, +// Return the orientations of a graph including only BetweenFactors +static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath) { // Find a minimum spanning tree diff --git a/gtsam/nonlinear/lago.h b/gtsam/nonlinear/lago.h index fa8eef995..0df80ac42 100644 --- a/gtsam/nonlinear/lago.h +++ b/gtsam/nonlinear/lago.h @@ -44,17 +44,6 @@ namespace lago { typedef std::map key2doubleMap; -/** - * Compute the cumulative orientation (without wrapping) wrt the root of a - * spanning tree (tree) for a node (nodeKey). The function starts at the nodes and - * moves towards the root summing up the (directed) rotation measurements. - * Relative measurements are encoded in "deltaThetaMap". - * The root is assumed to have orientation zero. - */ -GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey, - const PredecessorMap& tree, const key2doubleMap& deltaThetaMap, - const key2doubleMap& thetaFromRootMap); - /** * Compute the cumulative orientations (without wrapping) * for all nodes wrt the root (root has zero orientation). @@ -75,26 +64,12 @@ GTSAM_EXPORT void getSymbolicGraph( key2doubleMap& deltaThetaMap, /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g); -/** - * Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor - */ -GTSAM_EXPORT void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, - Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta); - /** Linear factor graph with regularized orientation measurements */ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( const std::vector& spanningTreeIds, const std::vector& chordsIds, const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); -/** Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */ -GTSAM_EXPORT NonlinearFactorGraph buildPose2graph( - const NonlinearFactorGraph& graph); - -/** Return the orientations of a graph including only BetweenFactors */ -GTSAM_EXPORT VectorValues computeOrientations( - const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); - /** LAGO: Return the orientations of the Pose2 in a generic factor graph */ GTSAM_EXPORT VectorValues initializeOrientations( const NonlinearFactorGraph& graph, bool useOdometricPath = true); From 8fe24183eb3f707eb81f9850d0bfd23a69d56b59 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 14:37:29 -0400 Subject: [PATCH 22/60] Lago timing --- .cproject | 8 ++++ gtsam/nonlinear/lago.cpp | 4 ++ gtsam/nonlinear/tests/CMakeLists.txt | 2 +- gtsam/nonlinear/tests/timeLago.cpp | 72 ++++++++++++++++++++++++++++ 4 files changed, 85 insertions(+), 1 deletion(-) create mode 100644 gtsam/nonlinear/tests/timeLago.cpp diff --git a/.cproject b/.cproject index a43d32f58..d759a7a65 100644 --- a/.cproject +++ b/.cproject @@ -2857,6 +2857,14 @@ true true + + make + -j5 + timeLago.run + true + true + true + make -j4 diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index c2a7d9454..c0f328d9f 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include @@ -198,6 +199,7 @@ GaussianFactorGraph buildLinearOrientationGraph( /* ************************************************************************* */ // Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { + gttic_(buildPose2graph); NonlinearFactorGraph pose2Graph; BOOST_FOREACH(const boost::shared_ptr& factor, graph) { @@ -250,6 +252,7 @@ static PredecessorMap findOdometricPath( // Return the orientations of a graph including only BetweenFactors static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath) { + gttic_(computeOrientations); // Find a minimum spanning tree PredecessorMap tree; @@ -293,6 +296,7 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph, /* ************************************************************************* */ Values computePoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { + gttic_(computePoses); // Linearized graph on full poses GaussianFactorGraph linearPose2graph; diff --git a/gtsam/nonlinear/tests/CMakeLists.txt b/gtsam/nonlinear/tests/CMakeLists.txt index 69a3700f2..378d93de4 100644 --- a/gtsam/nonlinear/tests/CMakeLists.txt +++ b/gtsam/nonlinear/tests/CMakeLists.txt @@ -1 +1 @@ -gtsamAddTestsGlob(nonlinear "test*.cpp" "" "gtsam") +gtsamAddTestsGlob(nonlinear "*.cpp" "" "gtsam") diff --git a/gtsam/nonlinear/tests/timeLago.cpp b/gtsam/nonlinear/tests/timeLago.cpp new file mode 100644 index 000000000..d09756fa0 --- /dev/null +++ b/gtsam/nonlinear/tests/timeLago.cpp @@ -0,0 +1,72 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeVirtual.cpp + * @brief Time the overhead of using virtual destructors and methods + * @author Richard Roberts + * @date Dec 3, 2010 + */ + +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char *argv[]) { + + size_t trials = 1000; + + // read graph + NonlinearFactorGraph g; + Values initial; + string inputFile = findExampleDataFile("noisyToyGraph"); + readG2o(inputFile, g, initial); + + // Add prior on the pose having index (key) = 0 + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Sigmas(Vector3(0, 0, 0)); + g.add(PriorFactor(0, Pose2(), priorModel)); + + // LAGO + for (size_t i = 0; i < trials; i++) { + { + gttic_(lago); + + gttic_(init); + Values lagoInitial = lago::initialize(g); + gttoc_(init); + + gttic_(refine); + GaussNewtonOptimizer optimizer(g, lagoInitial); + Values result = optimizer.optimize(); + gttoc_(refine); + } + + { + gttic_(optimize); + GaussNewtonOptimizer optimizer(g, initial); + Values result = optimizer.optimize(); + } + + tictoc_finishedIteration_(); + } + + tictoc_print_(); + + return 0; +} From 3425a8a37c7fe9598e9ad9047d62123f792c148c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 15:18:21 -0400 Subject: [PATCH 23/60] use namespace std --- gtsam/nonlinear/lago.cpp | 42 +++++++++++++++++++++------------------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index c0f328d9f..8ceb4e34e 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -25,6 +25,8 @@ #include +using namespace std; + namespace gtsam { namespace lago { @@ -77,7 +79,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, key2doubleMap thetaToRootMap; // Orientation of the roo - thetaToRootMap.insert(std::pair(keyAnchor, 0.0)); + thetaToRootMap.insert(pair(keyAnchor, 0.0)); // for all nodes in the tree BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) { @@ -85,14 +87,14 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, Key nodeKey = it.first; double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, thetaToRootMap); - thetaToRootMap.insert(std::pair(nodeKey, nodeTheta)); + thetaToRootMap.insert(pair(nodeKey, nodeTheta)); } return thetaToRootMap; } /* ************************************************************************* */ void getSymbolicGraph( -/*OUTPUTS*/std::vector& spanningTreeIds, std::vector& chordsIds, +/*OUTPUTS*/vector& spanningTreeIds, vector& chordsIds, key2doubleMap& deltaThetaMap, /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g) { @@ -113,10 +115,10 @@ void getSymbolicGraph( // insert (directed) orientations in the map "deltaThetaMap" bool inTree = false; if (tree.at(key1) == key2) { // key2 -> key1 - deltaThetaMap.insert(std::pair(key1, -deltaTheta)); + deltaThetaMap.insert(pair(key1, -deltaTheta)); inTree = true; } else if (tree.at(key2) == key1) { // key1 -> key2 - deltaThetaMap.insert(std::pair(key2, deltaTheta)); + deltaThetaMap.insert(pair(key2, deltaTheta)); inTree = true; } // store factor slot, distinguishing spanning tree edges from chordsIds @@ -139,7 +141,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, boost::shared_ptr > pose2Between = boost::dynamic_pointer_cast >(factor); if (!pose2Between) - throw std::invalid_argument( + throw invalid_argument( "buildLinearOrientationGraph: invalid between factor!"); deltaTheta = (Vector(1) << pose2Between->measured().theta()); @@ -148,18 +150,17 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, boost::shared_ptr diagonalModel = boost::dynamic_pointer_cast(model); if (!diagonalModel) - throw std::invalid_argument( - "buildLinearOrientationGraph: invalid noise model " - "(current version assumes diagonal noise model)!"); + throw invalid_argument("buildLinearOrientationGraph: invalid noise model " + "(current version assumes diagonal noise model)!"); Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)); // std on the angular measurement model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta); } /* ************************************************************************* */ GaussianFactorGraph buildLinearOrientationGraph( - const std::vector& spanningTreeIds, - const std::vector& chordsIds, const NonlinearFactorGraph& g, - const key2doubleMap& orientationsToRoot, const PredecessorMap& tree) { + const vector& spanningTreeIds, const vector& chordsIds, + const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, + const PredecessorMap& tree) { GaussianFactorGraph lagoGraph; Vector deltaTheta; @@ -179,11 +180,11 @@ GaussianFactorGraph buildLinearOrientationGraph( Key key1 = keys[0], key2 = keys[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); double key1_DeltaTheta_key2 = deltaTheta(0); - ///std::cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << std::endl; + ///cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << endl; double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord double k = boost::math::round(k2pi_noise / (2 * M_PI)); - //if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug + //if (k2pi_noise - 2*k*M_PI > 1e-5) cout << k2pi_noise - 2*k*M_PI << endl; // for debug Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2 * k * M_PI); lagoGraph.add( @@ -199,7 +200,7 @@ GaussianFactorGraph buildLinearOrientationGraph( /* ************************************************************************* */ // Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { - gttic_(buildPose2graph); + gttic_(lago_buildPose2graph); NonlinearFactorGraph pose2Graph; BOOST_FOREACH(const boost::shared_ptr& factor, graph) { @@ -252,7 +253,7 @@ static PredecessorMap findOdometricPath( // Return the orientations of a graph including only BetweenFactors static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath) { - gttic_(computeOrientations); + gttic_(lago_computeOrientations); // Find a minimum spanning tree PredecessorMap tree; @@ -264,8 +265,8 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, // Create a linear factor graph (LFG) of scalars key2doubleMap deltaThetaMap; - std::vector spanningTreeIds; // ids of between factors forming the spanning tree T - std::vector chordsIds; // ids of between factors corresponding to chordsIds wrt T + vector spanningTreeIds; // ids of between factors forming the spanning tree T + vector chordsIds; // ids of between factors corresponding to chordsIds wrt T getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph); // temporary structure to correct wraparounds along loops @@ -296,7 +297,7 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph, /* ************************************************************************* */ Values computePoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { - gttic_(computePoses); + gttic_(lago_computePoses); // Linearized graph on full poses GaussianFactorGraph linearPose2graph; @@ -337,7 +338,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, linearPose2graph.add( JacobianFactor(key1, J1, key2, I3, b, diagonalModel)); } else { - throw std::invalid_argument( + throw invalid_argument( "computeLagoPoses: cannot manage non between factor here!"); } } @@ -366,6 +367,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, /* ************************************************************************* */ Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) { + gttic_(lago_initialize); // We "extract" the Pose2 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph From 385dfd8ad6246bb159a37155680a08be5a8c744c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 15:18:45 -0400 Subject: [PATCH 24/60] use non underscore functions --- gtsam/nonlinear/lago.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index 8ceb4e34e..20d3e1dae 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -200,7 +200,7 @@ GaussianFactorGraph buildLinearOrientationGraph( /* ************************************************************************* */ // Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { - gttic_(lago_buildPose2graph); + gttic(lago_buildPose2graph); NonlinearFactorGraph pose2Graph; BOOST_FOREACH(const boost::shared_ptr& factor, graph) { @@ -253,7 +253,7 @@ static PredecessorMap findOdometricPath( // Return the orientations of a graph including only BetweenFactors static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath) { - gttic_(lago_computeOrientations); + gttic(lago_computeOrientations); // Find a minimum spanning tree PredecessorMap tree; @@ -297,7 +297,7 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph, /* ************************************************************************* */ Values computePoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { - gttic_(lago_computePoses); + gttic(lago_computePoses); // Linearized graph on full poses GaussianFactorGraph linearPose2graph; @@ -367,7 +367,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, /* ************************************************************************* */ Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) { - gttic_(lago_initialize); + gttic(lago_initialize); // We "extract" the Pose2 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph From 87f581877630e72a71a5680c23a681a03607343a Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sat, 31 May 2014 15:14:39 -0400 Subject: [PATCH 25/60] Move smart projection factor --- {gtsam_unstable => gtsam}/geometry/TriangulationFactor.h | 0 .../geometry/tests/testTriangulation.cpp | 2 +- {gtsam_unstable => gtsam}/geometry/triangulation.cpp | 2 +- {gtsam_unstable => gtsam}/geometry/triangulation.h | 8 ++++---- {gtsam_unstable => gtsam}/slam/ImplicitSchurFactor.h | 0 {gtsam_unstable => gtsam}/slam/JacobianFactorQ.h | 0 {gtsam_unstable => gtsam}/slam/JacobianFactorQR.h | 2 +- {gtsam_unstable => gtsam}/slam/JacobianFactorSVD.h | 2 +- {gtsam_unstable => gtsam}/slam/JacobianSchurFactor.h | 0 {gtsam_unstable => gtsam}/slam/RegularHessianFactor.h | 0 {gtsam_unstable => gtsam}/slam/SmartFactorBase.h | 0 {gtsam_unstable => gtsam}/slam/SmartProjectionFactor.h | 3 +-- .../slam/SmartProjectionPoseFactor.h | 0 .../slam/tests/testSmartProjectionPoseFactor.cpp | 0 gtsam_unstable/examples/SmartProjectionFactorExample.cpp | 2 +- 15 files changed, 10 insertions(+), 11 deletions(-) rename {gtsam_unstable => gtsam}/geometry/TriangulationFactor.h (100%) rename {gtsam_unstable => gtsam}/geometry/tests/testTriangulation.cpp (99%) rename {gtsam_unstable => gtsam}/geometry/triangulation.cpp (98%) rename {gtsam_unstable => gtsam}/geometry/triangulation.h (97%) rename {gtsam_unstable => gtsam}/slam/ImplicitSchurFactor.h (100%) rename {gtsam_unstable => gtsam}/slam/JacobianFactorQ.h (100%) rename {gtsam_unstable => gtsam}/slam/JacobianFactorQR.h (96%) rename {gtsam_unstable => gtsam}/slam/JacobianFactorSVD.h (97%) rename {gtsam_unstable => gtsam}/slam/JacobianSchurFactor.h (100%) rename {gtsam_unstable => gtsam}/slam/RegularHessianFactor.h (100%) rename {gtsam_unstable => gtsam}/slam/SmartFactorBase.h (100%) rename {gtsam_unstable => gtsam}/slam/SmartProjectionFactor.h (99%) rename {gtsam_unstable => gtsam}/slam/SmartProjectionPoseFactor.h (100%) rename {gtsam_unstable => gtsam}/slam/tests/testSmartProjectionPoseFactor.cpp (100%) diff --git a/gtsam_unstable/geometry/TriangulationFactor.h b/gtsam/geometry/TriangulationFactor.h similarity index 100% rename from gtsam_unstable/geometry/TriangulationFactor.h rename to gtsam/geometry/TriangulationFactor.h diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp similarity index 99% rename from gtsam_unstable/geometry/tests/testTriangulation.cpp rename to gtsam/geometry/tests/testTriangulation.cpp index 8d45311f1..fb05bcf9f 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -16,7 +16,7 @@ * Author: cbeall3 */ -#include +#include #include #include diff --git a/gtsam_unstable/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp similarity index 98% rename from gtsam_unstable/geometry/triangulation.cpp rename to gtsam/geometry/triangulation.cpp index 3017fdf7a..9e1575801 100644 --- a/gtsam_unstable/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -16,7 +16,7 @@ * @author Chris Beall */ -#include +#include #include #include diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam/geometry/triangulation.h similarity index 97% rename from gtsam_unstable/geometry/triangulation.h rename to gtsam/geometry/triangulation.h index f767514c1..6899c616a 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,8 +18,8 @@ #pragma once -#include -#include + +#include #include #include #include @@ -52,7 +52,7 @@ public: * @param rank_tol SVD rank tolerance * @return Triangulated Point3 */ -GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT( +GTSAM_EXPORT Point3 triangulateDLT( const std::vector& projection_matrices, const std::vector& measurements, double rank_tol); @@ -120,7 +120,7 @@ std::pair triangulationGraph( * @param landmarkKey to refer to landmark * @return refined Point3 */ -GTSAM_UNSTABLE_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, +GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey); /** diff --git a/gtsam_unstable/slam/ImplicitSchurFactor.h b/gtsam/slam/ImplicitSchurFactor.h similarity index 100% rename from gtsam_unstable/slam/ImplicitSchurFactor.h rename to gtsam/slam/ImplicitSchurFactor.h diff --git a/gtsam_unstable/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h similarity index 100% rename from gtsam_unstable/slam/JacobianFactorQ.h rename to gtsam/slam/JacobianFactorQ.h diff --git a/gtsam_unstable/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h similarity index 96% rename from gtsam_unstable/slam/JacobianFactorQR.h rename to gtsam/slam/JacobianFactorQR.h index 2d2d5b7a4..a928106a8 100644 --- a/gtsam_unstable/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -6,7 +6,7 @@ */ #pragma once -#include +#include namespace gtsam { /** diff --git a/gtsam_unstable/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h similarity index 97% rename from gtsam_unstable/slam/JacobianFactorSVD.h rename to gtsam/slam/JacobianFactorSVD.h index e8ade3b1b..e28185038 100644 --- a/gtsam_unstable/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -5,7 +5,7 @@ */ #pragma once -#include "gtsam_unstable/slam/JacobianSchurFactor.h" +#include "gtsam/slam/JacobianSchurFactor.h" namespace gtsam { /** diff --git a/gtsam_unstable/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h similarity index 100% rename from gtsam_unstable/slam/JacobianSchurFactor.h rename to gtsam/slam/JacobianSchurFactor.h diff --git a/gtsam_unstable/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h similarity index 100% rename from gtsam_unstable/slam/RegularHessianFactor.h rename to gtsam/slam/RegularHessianFactor.h diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h similarity index 100% rename from gtsam_unstable/slam/SmartFactorBase.h rename to gtsam/slam/SmartFactorBase.h diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h similarity index 99% rename from gtsam_unstable/slam/SmartProjectionFactor.h rename to gtsam/slam/SmartProjectionFactor.h index f439397b9..7c3719cd8 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -21,11 +21,10 @@ #include "SmartFactorBase.h" -#include +#include #include #include #include -#include #include #include diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h similarity index 100% rename from gtsam_unstable/slam/SmartProjectionPoseFactor.h rename to gtsam/slam/SmartProjectionPoseFactor.h diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp similarity index 100% rename from gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp rename to gtsam/slam/tests/testSmartProjectionPoseFactor.cpp diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index 31b45cbc9..40a0a8725 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -35,7 +35,7 @@ #include #include -#include +#include #include #include From a74d82ac712f1c133c68c2ca29b27d4d9c1609a4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 15:49:21 -0400 Subject: [PATCH 26/60] Convenience add functions... --- gtsam/nonlinear/lago.cpp | 23 ++++++++--------------- 1 file changed, 8 insertions(+), 15 deletions(-) diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index 20d3e1dae..5bb6f03f4 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -171,8 +171,7 @@ GaussianFactorGraph buildLinearOrientationGraph( const FastVector& keys = g[factorId]->keys(); Key key1 = keys[0], key2 = keys[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); - lagoGraph.add( - JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta)); + lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta); } // put regularized measurements in the chordsIds BOOST_FOREACH(const size_t& factorId, chordsIds) { @@ -187,13 +186,10 @@ GaussianFactorGraph buildLinearOrientationGraph( //if (k2pi_noise - 2*k*M_PI > 1e-5) cout << k2pi_noise - 2*k*M_PI << endl; // for debug Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2 * k * M_PI); - lagoGraph.add( - JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, - model_deltaTheta)); + lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta); } // prior on the anchor orientation - lagoGraph.add( - JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise)); + lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise); return lagoGraph; } @@ -324,8 +320,8 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, double dx = pose2Between->measured().x(); double dy = pose2Between->measured().y(); - Vector globalDeltaCart = (Vector(2) << c1 * dx - s1 * dy, s1 * dx - + c1 * dy); + Vector globalDeltaCart = // + (Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy); Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs Matrix J1 = -I3; J1(0, 2) = s1 * dx + c1 * dy; @@ -335,18 +331,15 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, boost::dynamic_pointer_cast( pose2Between->get_noiseModel()); - linearPose2graph.add( - JacobianFactor(key1, J1, key2, I3, b, diagonalModel)); + linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel); } else { throw invalid_argument( "computeLagoPoses: cannot manage non between factor here!"); } } // add prior - noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances( - (Vector(3) << 1e-2, 1e-2, 1e-4)); - linearPose2graph.add( - JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), priorModel)); + linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), + priorPose2Noise); // optimize VectorValues posesLago = linearPose2graph.optimize(); From 6e0c2f8560f68105f17655849cbc645818987010 Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Sat, 31 May 2014 15:52:06 -0400 Subject: [PATCH 27/60] merge changes --- examples/Data/pose2example-rewritten.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Data/pose2example-rewritten.txt b/examples/Data/pose2example-rewritten.txt index 6c8fe38a2..775d9ad89 100644 --- a/examples/Data/pose2example-rewritten.txt +++ b/examples/Data/pose2example-rewritten.txt @@ -20,4 +20,4 @@ EDGE_SE2 7 8 -1.02382 -0.013668 -3.08456 44.7214 0 0 44.7214 0 30.9017 EDGE_SE2 8 9 1.02344 0.013984 -0.127624 44.7214 0 0 44.7214 0 30.9017 EDGE_SE2 9 10 1.00335 0.02225 -0.195918 44.7214 0 0 44.7214 0 30.9017 EDGE_SE2 5 9 0.033943 0.032439 3.07364 44.7214 0 0 44.7214 0 30.9017 -EDGE_SE2 3 10 0.04402 0.988477 -1.55351 44.7214 0 0 44.7214 0 30.9017 +EDGE_SE2 3 10 0.04402 0.988477 -1.55351 44.7214 0 0 44.7214 0 30.9017 \ No newline at end of file From bdeeb48c6e1fe8c8e89efaefa33001235d0e9f7a Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Sat, 31 May 2014 15:52:40 -0400 Subject: [PATCH 28/60] removed rewritten file --- examples/Data/pose2example-rewritten.txt | 23 ----------------------- 1 file changed, 23 deletions(-) delete mode 100644 examples/Data/pose2example-rewritten.txt diff --git a/examples/Data/pose2example-rewritten.txt b/examples/Data/pose2example-rewritten.txt deleted file mode 100644 index 775d9ad89..000000000 --- a/examples/Data/pose2example-rewritten.txt +++ /dev/null @@ -1,23 +0,0 @@ -VERTEX_SE2 0 0 0 0 -VERTEX_SE2 1 1.03039 0.01135 -0.081596 -VERTEX_SE2 2 2.03614 -0.129733 -0.301887 -VERTEX_SE2 3 3.0151 -0.442395 -0.345514 -VERTEX_SE2 4 3.34395 0.506678 1.21471 -VERTEX_SE2 5 3.68449 1.46405 1.18379 -VERTEX_SE2 6 4.06463 2.41478 1.17633 -VERTEX_SE2 7 4.42978 3.30018 1.25917 -VERTEX_SE2 8 4.12888 2.32148 -1.82539 -VERTEX_SE2 9 3.88465 1.32751 -1.95302 -VERTEX_SE2 10 3.53107 0.388263 -2.14893 -EDGE_SE2 0 1 1.03039 0.01135 -0.081596 44.7214 0 0 44.7214 0 30.9017 -EDGE_SE2 1 2 1.0139 -0.058639 -0.220291 44.7214 0 0 44.7214 0 30.9017 -EDGE_SE2 2 3 1.02765 -0.007456 -0.043627 44.7214 0 0 44.7214 0 30.9017 -EDGE_SE2 3 4 -0.012016 1.00436 1.56023 44.7214 0 0 44.7214 0 30.9017 -EDGE_SE2 4 5 1.01603 0.014565 -0.03093 44.7214 0 0 44.7214 0 30.9017 -EDGE_SE2 5 6 1.02389 0.006808 -0.007452 44.7214 0 0 44.7214 0 30.9017 -EDGE_SE2 6 7 0.957734 0.003159 0.082836 44.7214 0 0 44.7214 0 30.9017 -EDGE_SE2 7 8 -1.02382 -0.013668 -3.08456 44.7214 0 0 44.7214 0 30.9017 -EDGE_SE2 8 9 1.02344 0.013984 -0.127624 44.7214 0 0 44.7214 0 30.9017 -EDGE_SE2 9 10 1.00335 0.02225 -0.195918 44.7214 0 0 44.7214 0 30.9017 -EDGE_SE2 5 9 0.033943 0.032439 3.07364 44.7214 0 0 44.7214 0 30.9017 -EDGE_SE2 3 10 0.04402 0.988477 -1.55351 44.7214 0 0 44.7214 0 30.9017 \ No newline at end of file From afcddf823a2e3a78c906065c04a8a4809beb86f2 Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Sat, 31 May 2014 15:53:41 -0400 Subject: [PATCH 29/60] unit test now works --- .gitignore | 3 +- .../Data/dubrovnik-3-7-pre.txt-rewritten.txt | 80 +++++++++++++++++++ gtsam/slam/dataset.cpp | 15 ++++ gtsam/slam/dataset.h | 8 +- gtsam/slam/tests/testDataset.cpp | 6 +- 5 files changed, 107 insertions(+), 5 deletions(-) create mode 100644 examples/Data/dubrovnik-3-7-pre.txt-rewritten.txt diff --git a/.gitignore b/.gitignore index 07ebeb8fb..cf23a5147 100644 --- a/.gitignore +++ b/.gitignore @@ -2,4 +2,5 @@ /doc* *.pyc *.DS_Store -/examples/Data/dubrovnik-3-7-pre-rewritten.txt \ No newline at end of file +/examples/Data/dubrovnik-3-7-pre-rewritten.txt +/examples/Data/pose2example-rewritten.txt \ No newline at end of file diff --git a/examples/Data/dubrovnik-3-7-pre.txt-rewritten.txt b/examples/Data/dubrovnik-3-7-pre.txt-rewritten.txt new file mode 100644 index 000000000..f7a95f397 --- /dev/null +++ b/examples/Data/dubrovnik-3-7-pre.txt-rewritten.txt @@ -0,0 +1,80 @@ +3 7 19 + +0 0 -385.989990234375 387.1199951171875 +1 0 -38.439998626708984 492.1199951171875 +2 0 -667.91998291015625 123.11000061035156 +0 1 383.8800048828125 -15.299989700317383 +1 1 559.75 -106.15000152587891 +0 2 591.54998779296875 136.44000244140625 +1 2 863.8599853515625 -23.469970703125 +2 2 494.72000122070312 112.51999664306641 +0 3 592.5 125.75 +1 3 861.08001708984375 -35.219970703125 +2 3 498.54000854492187 101.55999755859375 +0 4 348.72000122070312 558.3800048828125 +1 4 776.030029296875 483.52999877929687 +2 4 7.7800288200378418 326.35000610351562 +0 5 14.010009765625 96.420013427734375 +1 5 207.1300048828125 118.36000061035156 +0 6 202.75999450683594 340.989990234375 +1 6 543.18011474609375 294.80999755859375 +2 6 -58.419979095458984 110.83000183105469 + +-0.016943983733654022 + 0.011171804741024973 +0.0024643507786095142 +0.73030996322631825 +-0.2649081945419311 +-1.7127892971038821 +1430.031982421875 +-7.5572756941255648e-008 +3.2377570134516087e-014 + +0.015049724839627745 +-0.18504564464092257 +-0.29278403520584112 + -1.05904757976532 +-0.036017861217260333 + -1.5720340013504028 +1432.137451171875 +-7.317191830225056e-008 +3.1759419042137055e-014 + +-0.30793598294258112 + 0.32077908515930176 + 0.22253985702991483 + 8.5034484863281268 + 6.74996042251587 +-3.6383814811706534 +1572.047119140625 +-1.5962623223231276e-008 +-1.6507904730136101e-014 + +-12.055994987487793 +12.838775634765625 +-41.099369049072266 + +6.4168906211853027 +0.38897031545639038 +-23.586282730102539 + +13.051100730895996 +3.8387587070465088 +-29.777933120727539 + +13.060946464538574 +3.5910520553588867 +-29.75908088684082 + +14.265764236450195 +24.096216201782227 +-54.823970794677734 + +-0.25292283296585083 +2.2166082859039307 +-21.712127685546875 + +7.6465740203857422 +14.185332298278809 +-52.070301055908203 + diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index f381a0786..22da5ed15 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -67,6 +67,21 @@ string findExampleDataFile(const string& name) { INSTALLED_DATASET_DIR " named\n" + name + ", " + name + ".graph, or " + name + ".txt"); } + +string createRewrittenFileName(const string& name) { + // Search source tree and installed location + if(!exists(fs::path(name))) { + throw std::invalid_argument( + "gtsam::createRewrittenFileName could not find a matching file in\n" + + name); + } + + fs::path p(name); + fs::path newpath = fs::path(p.parent_path().string()) / fs::path(p.stem().string() + "-rewritten.txt" ); + + printf("New path = %s\n", newpath.string().c_str() ); + return newpath.string(); +} #endif /* ************************************************************************* */ diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 98297ac04..c544dcad5 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -35,7 +35,7 @@ namespace gtsam { /** * Find the full path to an example dataset distributed with gtsam. The name * may be specified with or without a file extension - if no extension is - * give, this function first looks for the .graph extension, then .txt. We + * given, this function first looks for the .graph extension, then .txt. We * first check the gtsam source tree for the file, followed by the installed * example dataset location. Both the source tree and installed locations * are obtained from CMake during compilation. @@ -44,6 +44,12 @@ namespace gtsam { * search process described above. */ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); + +/** + * searches for the file using the findExample funtion, if not found , + * creates one in the source tree and returns the full path + */ +GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name); #endif /** diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index add0db29d..1ab4c9bb4 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -175,7 +175,7 @@ TEST( dataSet, writeG2o) Values expectedValues; readG2o(g2oFile, expectedGraph, expectedValues); - const string filenameToWrite = findExampleDataFile("pose2example-rewritten"); + const string filenameToWrite = createRewrittenFileName(g2oFile); writeG2o(filenameToWrite, expectedGraph, expectedValues); NonlinearFactorGraph actualGraph; @@ -249,7 +249,7 @@ TEST( dataSet, writeBAL_Dubrovnik) readBAL(filenameToRead, readData); // Write readData to file filenameToWrite - const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten"); + const string filenameToWrite = createRewrittenFileName(filenameToRead); CHECK(writeBAL(filenameToWrite, readData)); // Read what we wrote @@ -311,7 +311,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ } // Write values and readData to a file - const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten"); + const string filenameToWrite = createRewrittenFileName(filenameToRead); writeBALfromValues(filenameToWrite, readData, value); // Read the file we wrote From 4cc759c0a7b7b077e9c31580c8c1380e67cbdf58 Mon Sep 17 00:00:00 2001 From: jing Date: Sat, 31 May 2014 15:57:13 -0400 Subject: [PATCH 30/60] add a smart factor sfm example --- examples/SFMExample_SmartFactor.cpp | 139 ++++++++++++++++++++++++++++ 1 file changed, 139 insertions(+) create mode 100644 examples/SFMExample_SmartFactor.cpp diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp new file mode 100644 index 000000000..a96edd270 --- /dev/null +++ b/examples/SFMExample_SmartFactor.cpp @@ -0,0 +1,139 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFMExample_SmartFactor.cpp + * @brief A structure-from-motion problem on a simulated dataset, using smart projection factor + * @author Duy-Nguyen Ta + * @author Jing Dong + */ + +/** + * A structure-from-motion example with landmarks + * - The landmarks form a 10 meter cube + * - The robot rotates around the landmarks, always facing towards the cube + */ + +// For loading the data +#include "SFMdata.h" + +// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +#include + +// Each variable in the system (poses and landmarks) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use Symbols +#include + +// In GTSAM, measurement functions are represented as 'factors'. +// The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark, +// The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration, +// The calibration should be known. +#include + +// Also, we will initialize the robot at some location using a Prior factor. +#include + +// When the factors are created, we will add them to a Factor Graph. As the factors we are using +// are nonlinear factors, we will need a Nonlinear Factor Graph. +#include + +// Finally, once all of the factors have been added to our factor graph, we will want to +// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. +// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a +// trust-region method known as Powell's Degleg +#include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + +#include + +using namespace std; +using namespace gtsam; + +// Make the typename short so it looks much cleaner +typedef gtsam::SmartProjectionPoseFactor + SmartFactor; + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Create the set of ground-truth landmarks + vector points = createPoints(); + + // Create the set of ground-truth poses + vector poses = createPoses(); + + // Create a factor graph + NonlinearFactorGraph graph; + + // Add a prior on pose x0. This indirectly specifies where the origin is. + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + + // Simulated measurements from each camera pose, adding them to the factor graph + for (size_t i = 0; i < points.size(); ++i) { + + // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. + SmartFactor::shared_ptr smartfactor(new SmartFactor()); + + for (size_t j = 0; j < poses.size(); ++j) { + + // generate the 2D measurement + SimpleCamera camera(poses[j], *K); + Point2 measurement = camera.project(points[i]); + + // call add() function to add measurment into a single factor, here we need to add: + // 1. the 2D measurement + // 2. the corresponding camera's key + // 3. camera noise model + // 4. camera calibration + smartfactor->add(measurement, Symbol('x', j), measurementNoise, K); + } + + // insert the smart factor in the graph + graph.push_back(smartfactor); + } + + // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained + // Here we add a prior on the second pose x1, so this will fix the scale by indicating the distance between x0 and x1. + // Because these two are fixed, the rest poses will be alse fixed. + graph.push_back(PriorFactor(Symbol('x', 1), poses[1], poseNoise)); // add directly to graph + + graph.print("Factor Graph:\n"); + + // Create the data structure to hold the initial estimate to the solution + // Intentionally initialize the variables off from the ground truth + Values initialEstimate; + for (size_t i = 0; i < poses.size(); ++i) + initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + initialEstimate.print("Initial Estimates:\n"); + + // Optimize the graph and print results + Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + result.print("Final results:\n"); + + + return 0; +} +/* ************************************************************************* */ + From 34962d46691fb97165909ad7da690dff0f1dff12 Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Sat, 31 May 2014 16:03:16 -0400 Subject: [PATCH 31/60] removed debug statement --- gtsam/slam/dataset.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 22da5ed15..04b1fe1df 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -79,7 +79,6 @@ string createRewrittenFileName(const string& name) { fs::path p(name); fs::path newpath = fs::path(p.parent_path().string()) / fs::path(p.stem().string() + "-rewritten.txt" ); - printf("New path = %s\n", newpath.string().c_str() ); return newpath.string(); } #endif From cd1b503985af6a7f3354466da81fda92db702f68 Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Sat, 31 May 2014 16:13:29 -0400 Subject: [PATCH 32/60] comments explaining the funtion --- gtsam/slam/dataset.cpp | 2 ++ gtsam/slam/dataset.h | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 04b1fe1df..fc2d7df4d 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -68,6 +68,7 @@ string findExampleDataFile(const string& name) { name + ", " + name + ".graph, or " + name + ".txt"); } +/* ************************************************************************* */ string createRewrittenFileName(const string& name) { // Search source tree and installed location if(!exists(fs::path(name))) { @@ -81,6 +82,7 @@ string createRewrittenFileName(const string& name) { return newpath.string(); } +/* ************************************************************************* */ #endif /* ************************************************************************* */ diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index c544dcad5..854daa237 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -46,8 +46,8 @@ namespace gtsam { GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); /** - * searches for the file using the findExample funtion, if not found , - * creates one in the source tree and returns the full path + * Creates a temporary file name that needs to be ignored in .gitingnore + * for checking read-write oprations */ GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name); #endif From 19b7734a97dfa8e4cc00f2f9db0fcc9403a60fd0 Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Sat, 31 May 2014 16:21:55 -0400 Subject: [PATCH 33/60] Removed unnecessary file --- .../Data/dubrovnik-3-7-pre.txt-rewritten.txt | 80 ------------------- gtsam/slam/dataset.cpp | 1 + 2 files changed, 1 insertion(+), 80 deletions(-) delete mode 100644 examples/Data/dubrovnik-3-7-pre.txt-rewritten.txt diff --git a/examples/Data/dubrovnik-3-7-pre.txt-rewritten.txt b/examples/Data/dubrovnik-3-7-pre.txt-rewritten.txt deleted file mode 100644 index f7a95f397..000000000 --- a/examples/Data/dubrovnik-3-7-pre.txt-rewritten.txt +++ /dev/null @@ -1,80 +0,0 @@ -3 7 19 - -0 0 -385.989990234375 387.1199951171875 -1 0 -38.439998626708984 492.1199951171875 -2 0 -667.91998291015625 123.11000061035156 -0 1 383.8800048828125 -15.299989700317383 -1 1 559.75 -106.15000152587891 -0 2 591.54998779296875 136.44000244140625 -1 2 863.8599853515625 -23.469970703125 -2 2 494.72000122070312 112.51999664306641 -0 3 592.5 125.75 -1 3 861.08001708984375 -35.219970703125 -2 3 498.54000854492187 101.55999755859375 -0 4 348.72000122070312 558.3800048828125 -1 4 776.030029296875 483.52999877929687 -2 4 7.7800288200378418 326.35000610351562 -0 5 14.010009765625 96.420013427734375 -1 5 207.1300048828125 118.36000061035156 -0 6 202.75999450683594 340.989990234375 -1 6 543.18011474609375 294.80999755859375 -2 6 -58.419979095458984 110.83000183105469 - --0.016943983733654022 - 0.011171804741024973 -0.0024643507786095142 -0.73030996322631825 --0.2649081945419311 --1.7127892971038821 -1430.031982421875 --7.5572756941255648e-008 -3.2377570134516087e-014 - -0.015049724839627745 --0.18504564464092257 --0.29278403520584112 - -1.05904757976532 --0.036017861217260333 - -1.5720340013504028 -1432.137451171875 --7.317191830225056e-008 -3.1759419042137055e-014 - --0.30793598294258112 - 0.32077908515930176 - 0.22253985702991483 - 8.5034484863281268 - 6.74996042251587 --3.6383814811706534 -1572.047119140625 --1.5962623223231276e-008 --1.6507904730136101e-014 - --12.055994987487793 -12.838775634765625 --41.099369049072266 - -6.4168906211853027 -0.38897031545639038 --23.586282730102539 - -13.051100730895996 -3.8387587070465088 --29.777933120727539 - -13.060946464538574 -3.5910520553588867 --29.75908088684082 - -14.265764236450195 -24.096216201782227 --54.823970794677734 - --0.25292283296585083 -2.2166082859039307 --21.712127685546875 - -7.6465740203857422 -14.185332298278809 --52.070301055908203 - diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index fc2d7df4d..7da45a904 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -83,6 +83,7 @@ string createRewrittenFileName(const string& name) { return newpath.string(); } /* ************************************************************************* */ + #endif /* ************************************************************************* */ From 1b04ee747362d4bc7e40daa92c2731c3431cb0b1 Mon Sep 17 00:00:00 2001 From: jing Date: Sat, 31 May 2014 16:23:23 -0400 Subject: [PATCH 34/60] add landmark output in SmartFactor example --- examples/SFMExample_SmartFactor.cpp | 35 ++++++++++++++++++++++++++--- 1 file changed, 32 insertions(+), 3 deletions(-) diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index a96edd270..904919d42 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -86,9 +86,8 @@ int main(int argc, char* argv[]) { // Create a factor graph NonlinearFactorGraph graph; - // Add a prior on pose x0. This indirectly specifies where the origin is. - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + // A vector saved all Smart factors (for get landmark position after optimization) + vector smartfactors_ptr; // Simulated measurements from each camera pose, adding them to the factor graph for (size_t i = 0; i < points.size(); ++i) { @@ -110,10 +109,17 @@ int main(int argc, char* argv[]) { smartfactor->add(measurement, Symbol('x', j), measurementNoise, K); } + // save smartfactors to get landmark position + smartfactors_ptr.push_back(smartfactor); + // insert the smart factor in the graph graph.push_back(smartfactor); } + // Add a prior on pose x0. This indirectly specifies where the origin is. + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained // Here we add a prior on the second pose x1, so this will fix the scale by indicating the distance between x0 and x1. // Because these two are fixed, the rest poses will be alse fixed. @@ -133,6 +139,29 @@ int main(int argc, char* argv[]) { result.print("Final results:\n"); + // Notice: Smart factor represent the 3D point as a factor, not a variable. + // The 3D position of the landmark is not explicitly calculated by the optimizer. + // If you do want to output the landmark's 3D position, you should use the internal function point() + // of the smart factor to get the 3D point. + Values landmark_result; + for (size_t i = 0; i < points.size(); ++i) { + + // The output of point() is in boost::optional, since sometimes + // the triangulation opterations inside smart factor will encounter degeneracy. + // Check the manual of boost::optional for more details for the usages. + boost::optional point; + + // here we use the saved smart factors to call, pass in all optimized pose to calculate landmark positions + point = smartfactors_ptr.at(i)->point(result); + + // ignore if boost::optional return NULL + if (point) + landmark_result.insert(Symbol('l', i), *point); + } + + landmark_result.print("Landmark results:\n"); + + return 0; } /* ************************************************************************* */ From 5aa9f42875826b8e637463037dfea66521d18c4f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 16:24:25 -0400 Subject: [PATCH 35/60] Better exception --- gtsam/slam/dataset.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index f381a0786..a1b96d37d 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -83,7 +83,7 @@ pair load2D( cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); if (!is) - throw std::invalid_argument("load2D: can not find the file!"); + throw std::invalid_argument("load2D: can not find file " + filename); Values::shared_ptr initial(new Values); NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); From 7e6a333b1ac85efde52ec0c293bacbb042792019 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 16:24:40 -0400 Subject: [PATCH 36/60] Constrain orientation --- gtsam/nonlinear/lago.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index 5bb6f03f4..0f69f6ef0 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -35,7 +35,7 @@ static const Matrix I3 = eye(3); static const Key keyAnchor = symbol('Z', 9999999); static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = - noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); + noiseModel::Diagonal::Sigmas((Vector(1) << 0)); static const noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); From 42edec1066a4a45e441a6c34ac2551a5f5f60cda Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 16:24:50 -0400 Subject: [PATCH 37/60] Properly read w10000 --- gtsam/nonlinear/tests/timeLago.cpp | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/gtsam/nonlinear/tests/timeLago.cpp b/gtsam/nonlinear/tests/timeLago.cpp index d09756fa0..85425f64b 100644 --- a/gtsam/nonlinear/tests/timeLago.cpp +++ b/gtsam/nonlinear/tests/timeLago.cpp @@ -29,18 +29,19 @@ using namespace gtsam; int main(int argc, char *argv[]) { - size_t trials = 1000; + size_t trials = 1; // read graph - NonlinearFactorGraph g; - Values initial; - string inputFile = findExampleDataFile("noisyToyGraph"); - readG2o(inputFile, g, initial); + Values::shared_ptr initial; + NonlinearFactorGraph::shared_ptr g; + string inputFile = findExampleDataFile("w10000"); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0)); + boost::tie(g, initial) = load2D(inputFile, model); // Add prior on the pose having index (key) = 0 noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Sigmas(Vector3(0, 0, 0)); - g.add(PriorFactor(0, Pose2(), priorModel)); + noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8)); + g->add(PriorFactor(0, Pose2(), priorModel)); // LAGO for (size_t i = 0; i < trials; i++) { @@ -48,18 +49,18 @@ int main(int argc, char *argv[]) { gttic_(lago); gttic_(init); - Values lagoInitial = lago::initialize(g); + Values lagoInitial = lago::initialize(*g); gttoc_(init); gttic_(refine); - GaussNewtonOptimizer optimizer(g, lagoInitial); + GaussNewtonOptimizer optimizer(*g, lagoInitial); Values result = optimizer.optimize(); gttoc_(refine); } { gttic_(optimize); - GaussNewtonOptimizer optimizer(g, initial); + GaussNewtonOptimizer optimizer(*g, *initial); Values result = optimizer.optimize(); } From 16c28b2e9c6c6f2b043040a225563adec59fb339 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 16:31:48 -0400 Subject: [PATCH 38/60] Formatting only --- gtsam/inference/VariableIndex-inl.h | 60 ++++++++++++++--------------- 1 file changed, 28 insertions(+), 32 deletions(-) diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h index a88ea5d57..82eb85c76 100644 --- a/gtsam/inference/VariableIndex-inl.h +++ b/gtsam/inference/VariableIndex-inl.h @@ -23,43 +23,35 @@ namespace gtsam { /* ************************************************************************* */ template -void VariableIndex::augment(const FG& factors, boost::optional&> newFactorIndices) -{ +void VariableIndex::augment(const FG& factors, + boost::optional&> newFactorIndices) { gttic(VariableIndex_augment); // Augment index for each factor - for(size_t i = 0; i < factors.size(); ++i) - { - if(factors[i]) - { + for (size_t i = 0; i < factors.size(); ++i) { + if (factors[i]) { const size_t globalI = - newFactorIndices ? - (*newFactorIndices)[i] : - nFactors_; - BOOST_FOREACH(const Key key, *factors[i]) - { + newFactorIndices ? (*newFactorIndices)[i] : nFactors_; + BOOST_FOREACH(const Key key, *factors[i]) { index_[key].push_back(globalI); - ++ nEntries_; + ++nEntries_; } } // Increment factor count even if factors are null, to keep indices consistent - if(newFactorIndices) - { - if((*newFactorIndices)[i] >= nFactors_) + if (newFactorIndices) { + if ((*newFactorIndices)[i] >= nFactors_) nFactors_ = (*newFactorIndices)[i] + 1; - } - else - { - ++ nFactors_; + } else { + ++nFactors_; } } } /* ************************************************************************* */ template -void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors) -{ +void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, + const FG& factors) { gttic(VariableIndex_remove); // NOTE: We intentionally do not decrement nFactors_ because the factor @@ -68,17 +60,20 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& // one greater than the highest-numbered factor referenced in a VariableIndex. ITERATOR factorIndex = firstFactor; size_t i = 0; - for( ; factorIndex != lastFactor; ++factorIndex, ++i) { - if(i >= factors.size()) - throw std::invalid_argument("Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); - if(factors[i]) { + for (; factorIndex != lastFactor; ++factorIndex, ++i) { + if (i >= factors.size()) + throw std::invalid_argument( + "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); + if (factors[i]) { BOOST_FOREACH(Key j, *factors[i]) { Factors& factorEntries = internalAt(j); - Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), *factorIndex); - if(entry == factorEntries.end()) - throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index"); + Factors::iterator entry = std::find(factorEntries.begin(), + factorEntries.end(), *factorIndex); + if (entry == factorEntries.end()) + throw std::invalid_argument( + "Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index"); factorEntries.erase(entry); - -- nEntries_; + --nEntries_; } } } @@ -87,10 +82,11 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& /* ************************************************************************* */ template void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) { - for(ITERATOR key = firstKey; key != lastKey; ++key) { + for (ITERATOR key = firstKey; key != lastKey; ++key) { KeyMap::iterator entry = index_.find(*key); - if(!entry->second.empty()) - throw std::invalid_argument("Asking to remove variables from the variable index that are not unused"); + if (!entry->second.empty()) + throw std::invalid_argument( + "Asking to remove variables from the variable index that are not unused"); index_.erase(entry); } } From 76297ea2cc90cf1a351b33a2184555e429bc3827 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 17:15:29 -0400 Subject: [PATCH 39/60] Adding noise does not help much --- gtsam/nonlinear/tests/timeLago.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/tests/timeLago.cpp b/gtsam/nonlinear/tests/timeLago.cpp index 85425f64b..d3c86d0a4 100644 --- a/gtsam/nonlinear/tests/timeLago.cpp +++ b/gtsam/nonlinear/tests/timeLago.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -32,11 +33,19 @@ int main(int argc, char *argv[]) { size_t trials = 1; // read graph - Values::shared_ptr initial; + Values::shared_ptr solution; NonlinearFactorGraph::shared_ptr g; string inputFile = findExampleDataFile("w10000"); SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0)); - boost::tie(g, initial) = load2D(inputFile, model); + boost::tie(g, solution) = load2D(inputFile, model); + + // add noise to create initial estimate + Values initial; + Sampler sampler(42u); + Values::ConstFiltered poses = solution->filter(); + SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0)); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& it, poses) + initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise))); // Add prior on the pose having index (key) = 0 noiseModel::Diagonal::shared_ptr priorModel = // @@ -60,7 +69,7 @@ int main(int argc, char *argv[]) { { gttic_(optimize); - GaussNewtonOptimizer optimizer(*g, *initial); + GaussNewtonOptimizer optimizer(*g, initial); Values result = optimizer.optimize(); } From a84a9a67d6b1830966baf48715dc1da7398eb97b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 18:07:55 -0400 Subject: [PATCH 40/60] moved targets --- .cproject | 128 +++++++++++++++++++++++++++--------------------------- 1 file changed, 64 insertions(+), 64 deletions(-) diff --git a/.cproject b/.cproject index d759a7a65..d5a6ca4d4 100644 --- a/.cproject +++ b/.cproject @@ -2185,70 +2185,6 @@ true true - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j6 -j8 - testAntiFactor.run - true - true - true - - - make - -j6 -j8 - testBetweenFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - make -j2 @@ -2649,6 +2585,70 @@ true true + + make + -j5 + testAntiFactor.run + true + true + true + + + make + -j5 + testBetweenFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + make -j2 From 204ddbee5ef09a653fc9b3cb0345ea257b2ffe0c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 18:08:02 -0400 Subject: [PATCH 41/60] Formatting --- gtsam/slam/dataset.cpp | 361 +++++++++++++++++++++-------------------- 1 file changed, 186 insertions(+), 175 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index cb2d44224..3528424fa 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -43,7 +43,7 @@ string findExampleDataFile(const string& name) { // Search source tree and installed location vector rootsToSearch; rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt - rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt + rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt // Search for filename as given, and with .graph and .txt extensions vector namesToSearch; @@ -55,32 +55,34 @@ string findExampleDataFile(const string& name) { // Find first name that exists BOOST_FOREACH(const fs::path& root, rootsToSearch) { BOOST_FOREACH(const fs::path& name, namesToSearch) { - if(fs::is_regular_file(root / name)) + if (fs::is_regular_file(root / name)) return (root / name).string(); } } // If we did not return already, then we did not find the file - throw std::invalid_argument( - "gtsam::findExampleDataFile could not find a matching file in\n" - SOURCE_TREE_DATASET_DIR " or\n" - INSTALLED_DATASET_DIR " named\n" + - name + ", " + name + ".graph, or " + name + ".txt"); + throw +std::invalid_argument( + "gtsam::findExampleDataFile could not find a matching file in\n" + SOURCE_TREE_DATASET_DIR " or\n" + INSTALLED_DATASET_DIR " named\n" + + name + ", " + name + ".graph, or " + name + ".txt"); } /* ************************************************************************* */ string createRewrittenFileName(const string& name) { // Search source tree and installed location - if(!exists(fs::path(name))) { - throw std::invalid_argument( - "gtsam::createRewrittenFileName could not find a matching file in\n" - + name); + if (!exists(fs::path(name))) { + throw std::invalid_argument( + "gtsam::createRewrittenFileName could not find a matching file in\n" + + name); } - fs::path p(name); - fs::path newpath = fs::path(p.parent_path().string()) / fs::path(p.stem().string() + "-rewritten.txt" ); + fs::path p(name); + fs::path newpath = fs::path(p.parent_path().string()) + / fs::path(p.stem().string() + "-rewritten.txt"); - return newpath.string(); + return newpath.string(); } /* ************************************************************************* */ @@ -95,7 +97,8 @@ pair load2D( /* ************************************************************************* */ pair load2D( - const string& filename, boost::optional model, int maxID, + const string& filename, + boost::optional model, int maxID, bool addNoise, bool smart) { cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); @@ -109,7 +112,7 @@ pair load2D( // load the poses while (is) { - if(! (is >> tag)) + if (!(is >> tag)) break; if ((tag == "VERTEX2") || (tag == "VERTEX")) { @@ -133,7 +136,7 @@ pair load2D( int id1, id2; bool haveLandmark = false; while (is) { - if(! (is >> tag)) + if (!(is >> tag)) break; if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { @@ -144,20 +147,18 @@ pair load2D( is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; // Try to guess covariance matrix layout - Matrix m(3,3); - if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0) - { + Matrix m(3, 3); + if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 + && v6 == 0.0) { // Looks like [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ] - m << v1, v2, v5, v2, v3, v6, v5, v6, v4; - } - else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0) - { + m << v1, v2, v5, v2, v3, v6, v5, v6, v4; + } else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 + && v6 != 0.0) { // Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] - m << v1, v2, v3, v2, v4, v5, v3, v5, v6; - } - else - { - throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file"); + m << v1, v2, v3, v2, v4, v5, v3, v5, v6; + } else { + throw std::invalid_argument( + "load2D: unrecognized covariance matrix format in dataset file"); } // optional filter @@ -203,22 +204,21 @@ pair load2D( // Convert x,y to bearing,range bearing = std::atan2(lmy, lmx); - range = std::sqrt(lmx*lmx + lmy*lmy); + range = std::sqrt(lmx * lmx + lmy * lmy); // In our experience, the x-y covariance on landmark sightings is not very good, so assume // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty. - if(std::abs(v1 - v3) < 1e-4) - { + if (std::abs(v1 - v3) < 1e-4) { bearing_std = sqrt(v1 / 10.0); range_std = sqrt(v1); - } - else - { + } else { bearing_std = 1; range_std = 1; - if(!haveLandmark) { - cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n" - "non-uniform covariance on LANDMARK measurements in this file." << endl; + if (!haveLandmark) { + cout + << "Warning: load2D is a very simple dataset loader and is ignoring the\n" + "non-uniform covariance on LANDMARK measurements in this file." + << endl; haveLandmark = true; } } @@ -244,7 +244,7 @@ pair load2D( initial->insert(id1, Pose2()); if (!initial->exists(L(id2))) { Pose2 pose = initial->at(id1); - Point2 local(cos(bearing)*range,sin(bearing)*range); + Point2 local(cos(bearing) * range, sin(bearing) * range); Point2 global = pose.transform_from(local); initial->insert(L(id2), global); } @@ -265,18 +265,16 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, fstream stream(filename.c_str(), fstream::out); // save poses - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) - { + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) { const Pose2& pose = dynamic_cast(key_value.value); - stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " - << pose.y() << " " << pose.theta() << endl; + stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() + << " " << pose.theta() << endl; } // save edges Matrix R = model->R(); Matrix RR = trans(R) * R; //prod(trans(R),R); - BOOST_FOREACH(boost::shared_ptr factor_, graph) - { + BOOST_FOREACH(boost::shared_ptr factor_, graph) { boost::shared_ptr > factor = boost::dynamic_pointer_cast >(factor_); if (!factor) @@ -284,9 +282,9 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, Pose2 pose = factor->measured().inverse(); stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " " - << pose.x() << " " << pose.y() << " " << pose.theta() << " " - << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " - << RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl; + << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0) + << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " " + << RR(0, 2) << " " << RR(1, 2) << endl; } stream.close(); @@ -411,14 +409,15 @@ pair load2D_robust( noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); - *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); + *graph += BearingRangeFactor(id1, id2, bearing, range, + measurementNoise); // Insert poses or points if they do not exist yet if (!initial->exists(id1)) initial->insert(id1, Pose2()); if (!initial->exists(id2)) { Pose2 pose = initial->at(id1); - Point2 local(cos(bearing)*range,sin(bearing)*range); + Point2 local(cos(bearing) * range, sin(bearing) * range); Point2 global = pose.transform_from(local); initial->insert(id2, global); } @@ -427,69 +426,66 @@ pair load2D_robust( } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } /* ************************************************************************* */ -Rot3 openGLFixedRotation(){ // this is due to different convention for cameras in gtsam and openGL +Rot3 openGLFixedRotation() { // this is due to different convention for cameras in gtsam and openGL /* R = [ 1 0 0 * 0 -1 0 * 0 0 -1] */ - Matrix3 R_mat = Matrix3::Zero(3,3); - R_mat(0,0) = 1.0; R_mat(1,1) = -1.0; R_mat(2,2) = -1.0; + Matrix3 R_mat = Matrix3::Zero(3, 3); + R_mat(0, 0) = 1.0; + R_mat(1, 1) = -1.0; + R_mat(2, 2) = -1.0; return Rot3(R_mat); } /* ************************************************************************* */ -Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) -{ +Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) { Rot3 R90 = openGLFixedRotation(); - Rot3 wRc = ( R.inverse() ).compose(R90); + Rot3 wRc = (R.inverse()).compose(R90); // Our camera-to-world translation wTc = -R'*t - return Pose3 (wRc, R.unrotate(Point3(-tx,-ty,-tz))); + return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz))); } /* ************************************************************************* */ -Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) -{ +Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) { Rot3 R90 = openGLFixedRotation(); - Rot3 cRw_openGL = R90.compose( R.inverse() ); - Point3 t_openGL = cRw_openGL.rotate(Point3(-tx,-ty,-tz)); + Rot3 cRw_openGL = R90.compose(R.inverse()); + Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz)); return Pose3(cRw_openGL, t_openGL); } /* ************************************************************************* */ -Pose3 gtsam2openGL(const Pose3& PoseGTSAM) -{ - return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), PoseGTSAM.z()); +Pose3 gtsam2openGL(const Pose3& PoseGTSAM) { + return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), + PoseGTSAM.z()); } /* ************************************************************************* */ -bool readBundler(const string& filename, SfM_data &data) -{ +bool readBundler(const string& filename, SfM_data &data) { // Load the data file - ifstream is(filename.c_str(),ifstream::in); - if(!is) - { + ifstream is(filename.c_str(), ifstream::in); + if (!is) { cout << "Error in readBundler: can not find the file!!" << endl; return false; } // Ignore the first line char aux[500]; - is.getline(aux,500); + is.getline(aux, 500); // Get the number of camera poses and 3D points size_t nrPoses, nrPoints; is >> nrPoses >> nrPoints; // Get the information for the camera poses - for( size_t i = 0; i < nrPoses; i++ ) - { + for (size_t i = 0; i < nrPoses; i++) { // Get the focal length and the radial distortion parameters float f, k1, k2; is >> f >> k1 >> k2; @@ -499,20 +495,15 @@ bool readBundler(const string& filename, SfM_data &data) float r11, r12, r13; float r21, r22, r23; float r31, r32, r33; - is >> r11 >> r12 >> r13 - >> r21 >> r22 >> r23 - >> r31 >> r32 >> r33; + is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33; // Bundler-OpenGL rotation matrix - Rot3 R( - r11, r12, r13, - r21, r22, r23, - r31, r32, r33); + Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33); // Check for all-zero R, in which case quit - if(r11==0 && r12==0 && r13==0) - { - cout << "Error in readBundler: zero rotation matrix for pose " << i << endl; + if (r11 == 0 && r12 == 0 && r13 == 0) { + cout << "Error in readBundler: zero rotation matrix for pose " << i + << endl; return false; } @@ -520,38 +511,36 @@ bool readBundler(const string& filename, SfM_data &data) float tx, ty, tz; is >> tx >> ty >> tz; - Pose3 pose = openGL2gtsam(R,tx,ty,tz); + Pose3 pose = openGL2gtsam(R, tx, ty, tz); - data.cameras.push_back(SfM_Camera(pose,K)); + data.cameras.push_back(SfM_Camera(pose, K)); } // Get the information for the 3D points - for( size_t j = 0; j < nrPoints; j++ ) - { + for (size_t j = 0; j < nrPoints; j++) { SfM_Track track; // Get the 3D position float x, y, z; is >> x >> y >> z; - track.p = Point3(x,y,z); + track.p = Point3(x, y, z); // Get the color information float r, g, b; is >> r >> g >> b; - track.r = r/255.f; - track.g = g/255.f; - track.b = b/255.f; + track.r = r / 255.f; + track.g = g / 255.f; + track.b = b / 255.f; // Now get the visibility information size_t nvisible = 0; is >> nvisible; - for( size_t k = 0; k < nvisible; k++ ) - { + for (size_t k = 0; k < nvisible; k++) { size_t cam_idx = 0, point_idx = 0; float u, v; is >> cam_idx >> point_idx >> u >> v; - track.measurements.push_back(make_pair(cam_idx,Point2(u,-v))); + track.measurements.push_back(make_pair(cam_idx, Point2(u, -v))); } data.tracks.push_back(track); @@ -562,11 +551,11 @@ bool readBundler(const string& filename, SfM_data &data) } /* ************************************************************************* */ -bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, - const kernelFunctionType kernelFunction){ +bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, + Values& initial, const kernelFunctionType kernelFunction) { ifstream is(g2oFile.c_str()); - if (!is){ + if (!is) { throw std::invalid_argument("File not found!"); return false; } @@ -574,7 +563,7 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in // READ INITIAL GUESS FROM G2O FILE string tag; while (is) { - if(! (is >> tag)) + if (!(is >> tag)) break; if (tag == "VERTEX_SE2" || tag == "VERTEX2") { @@ -591,7 +580,7 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in // READ MEASUREMENTS FROM G2O FILE while (is) { - if(! (is >> tag)) + if (!(is >> tag)) break; if (tag == "EDGE_SE2" || tag == "EDGE2") { @@ -602,23 +591,35 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in is >> id1 >> id2 >> x >> y >> yaw; is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33; Pose2 l1Xl2(x, y, yaw); - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << I11, I22, I33)); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions( + (Vector(3) << I11, I22, I33)); switch (kernelFunction) { - {case QUADRATIC: - NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, l1Xl2, model)); + { + case QUADRATIC: + NonlinearFactor::shared_ptr factor( + new BetweenFactor(id1, id2, l1Xl2, model)); graph.add(factor); - break;} - {case HUBER: - NonlinearFactor::shared_ptr huberFactor(new BetweenFactor(id1, id2, l1Xl2, - noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model))); + break; + } + { + case HUBER: + NonlinearFactor::shared_ptr huberFactor( + new BetweenFactor(id1, id2, l1Xl2, + noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), model))); graph.add(huberFactor); - break;} - {case TUKEY: - NonlinearFactor::shared_ptr tukeyFactor(new BetweenFactor(id1, id2, l1Xl2, - noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model))); + break; + } + { + case TUKEY: + NonlinearFactor::shared_ptr tukeyFactor( + new BetweenFactor(id1, id2, l1Xl2, + noiseModel::Robust::Create( + noiseModel::mEstimator::Tukey::Create(4.6851), model))); graph.add(tukeyFactor); - break;} + break; + } } } is.ignore(LINESIZE, '\n'); @@ -626,31 +627,32 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in // Output which kernel is used switch (kernelFunction) { case QUADRATIC: - break; + break; case HUBER: - std::cout << "Robust kernel: Huber" << std::endl; break; + std::cout << "Robust kernel: Huber" << std::endl; + break; case TUKEY: - std::cout << "Robust kernel: Tukey" << std::endl; break; + std::cout << "Robust kernel: Tukey" << std::endl; + break; } return true; } /* ************************************************************************* */ -bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate){ +bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, + const Values& estimate) { fstream stream(filename.c_str(), fstream::out); // save poses - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) - { + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) { const Pose2& pose = dynamic_cast(key_value.value); stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; } // save edges - BOOST_FOREACH(boost::shared_ptr factor_, graph) - { + BOOST_FOREACH(boost::shared_ptr factor_, graph) { boost::shared_ptr > factor = boost::dynamic_pointer_cast >(factor_); if (!factor) @@ -660,25 +662,25 @@ bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, co boost::shared_ptr diagonalModel = boost::dynamic_pointer_cast(model); if (!diagonalModel) - throw std::invalid_argument("writeG2o: invalid noise model (current version assumes diagonal noise model)!"); + throw std::invalid_argument( + "writeG2o: invalid noise model (current version assumes diagonal noise model)!"); Pose2 pose = factor->measured(); //.inverse(); stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " " - << diagonalModel->precision(1) << " " << 0.0 << " " << diagonalModel->precision(2) << endl; + << diagonalModel->precision(1) << " " << 0.0 << " " + << diagonalModel->precision(2) << endl; } stream.close(); return true; } /* ************************************************************************* */ -bool readBAL(const string& filename, SfM_data &data) -{ +bool readBAL(const string& filename, SfM_data &data) { // Load the data file - ifstream is(filename.c_str(),ifstream::in); - if(!is) - { + ifstream is(filename.c_str(), ifstream::in); + if (!is) { cout << "Error in readBAL: can not find the file!!" << endl; return false; } @@ -690,44 +692,41 @@ bool readBAL(const string& filename, SfM_data &data) data.tracks.resize(nrPoints); // Get the information for the observations - for( size_t k = 0; k < nrObservations; k++ ) - { + for (size_t k = 0; k < nrObservations; k++) { size_t i = 0, j = 0; float u, v; is >> i >> j >> u >> v; - data.tracks[j].measurements.push_back(make_pair(i,Point2(u,-v))); + data.tracks[j].measurements.push_back(make_pair(i, Point2(u, -v))); } // Get the information for the camera poses - for( size_t i = 0; i < nrPoses; i++ ) - { + for (size_t i = 0; i < nrPoses; i++) { // Get the rodriguez vector float wx, wy, wz; is >> wx >> wy >> wz; - Rot3 R = Rot3::rodriguez(wx, wy, wz);// BAL-OpenGL rotation matrix + Rot3 R = Rot3::rodriguez(wx, wy, wz); // BAL-OpenGL rotation matrix // Get the translation vector float tx, ty, tz; is >> tx >> ty >> tz; - Pose3 pose = openGL2gtsam(R,tx,ty,tz); + Pose3 pose = openGL2gtsam(R, tx, ty, tz); // Get the focal length and the radial distortion parameters float f, k1, k2; is >> f >> k1 >> k2; Cal3Bundler K(f, k1, k2); - data.cameras.push_back(SfM_Camera(pose,K)); + data.cameras.push_back(SfM_Camera(pose, K)); } // Get the information for the 3D points - for( size_t j = 0; j < nrPoints; j++ ) - { + for (size_t j = 0; j < nrPoints; j++) { // Get the 3D position float x, y, z; is >> x >> y >> z; SfM_Track& track = data.tracks[j]; - track.p = Point3(x,y,z); + track.p = Point3(x, y, z); track.r = 0.4f; track.g = 0.4f; track.b = 0.4f; @@ -738,8 +737,7 @@ bool readBAL(const string& filename, SfM_data &data) } /* ************************************************************************* */ -bool writeBAL(const string& filename, SfM_data &data) -{ +bool writeBAL(const string& filename, SfM_data &data) { // Open the output file ofstream os; os.open(filename.c_str()); @@ -750,49 +748,55 @@ bool writeBAL(const string& filename, SfM_data &data) } // Write the number of camera poses and 3D points - size_t nrObservations=0; - for (size_t j = 0; j < data.number_tracks(); j++){ + size_t nrObservations = 0; + for (size_t j = 0; j < data.number_tracks(); j++) { nrObservations += data.tracks[j].number_measurements(); } // Write observations - os << data.number_cameras() << " " << data.number_tracks() << " " << nrObservations << endl; + os << data.number_cameras() << " " << data.number_tracks() << " " + << nrObservations << endl; os << endl; - for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j + for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j SfM_Track track = data.tracks[j]; - for(size_t k = 0; k < track.number_measurements(); k++){ // for each observation of the 3D point j + for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j size_t i = track.measurements[k].first; // camera id double u0 = data.cameras[i].calibration().u0(); double v0 = data.cameras[i].calibration().v0(); - if(u0 != 0 || v0 != 0){cout<< "writeBAL has not been tested for calibration with nonzero (u0,v0)"<< endl;} + if (u0 != 0 || v0 != 0) { + cout + << "writeBAL has not been tested for calibration with nonzero (u0,v0)" + << endl; + } double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin - double pixelBALy = - (track.measurements[k].second.y() - v0); // center of image is the origin + double pixelBALy = -(track.measurements[k].second.y() - v0); // center of image is the origin Point2 pixelMeasurement(pixelBALx, pixelBALy); - os << i /*camera id*/ << " " << j /*point id*/ << " " - << pixelMeasurement.x() /*u of the pixel*/ << " " << pixelMeasurement.y() /*v of the pixel*/ << endl; + os << i /*camera id*/<< " " << j /*point id*/<< " " + << pixelMeasurement.x() /*u of the pixel*/<< " " + << pixelMeasurement.y() /*v of the pixel*/<< endl; } } os << endl; // Write cameras - for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera + for (size_t i = 0; i < data.number_cameras(); i++) { // for each camera Pose3 poseGTSAM = data.cameras[i].pose(); Cal3Bundler cameraCalibration = data.cameras[i].calibration(); Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); - os << Rot3::Logmap(poseOpenGL.rotation()) << endl; - os << poseOpenGL.translation().vector() << endl; - os << cameraCalibration.fx() << endl; - os << cameraCalibration.k1() << endl; - os << cameraCalibration.k2() << endl; + os << Rot3::Logmap(poseOpenGL.rotation()) << endl; + os << poseOpenGL.translation().vector() << endl; + os << cameraCalibration.fx() << endl; + os << cameraCalibration.k1() << endl; + os << cameraCalibration.k2() << endl; os << endl; } // Write the points - for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j + for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j Point3 point = data.tracks[j].p; os << point.x() << endl; os << point.y() << endl; @@ -804,48 +808,55 @@ bool writeBAL(const string& filename, SfM_data &data) return true; } -bool writeBALfromValues(const string& filename, const SfM_data &data, Values& values){ +bool writeBALfromValues(const string& filename, const SfM_data &data, + Values& values) { 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 - for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera - Key poseKey = symbol('x',i); + if (valuesPoses.size() == 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); dataValues.cameras[i] = camera; } } else { - Values valuesCameras = values.filter< PinholeCamera >(); - 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 + 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 Key cameraKey = i; // symbol('c',i); - PinholeCamera camera = values.at >(cameraKey); + PinholeCamera 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() << ")!!" << endl; + } else { + cout + << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " + << dataValues.number_cameras() << ") and values (#cameras " + << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" + << endl; return false; } } // Store 3D points in SfM_data Values valuesPoints = values.filter(); - if( valuesPoints.size() != dataValues.number_tracks()){ - cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " << dataValues.number_tracks() - <<") and values (#points " << valuesPoints.size() << ")!!" << endl; + if (valuesPoints.size() != dataValues.number_tracks()) { + cout + << "writeBALfromValues: different number of points in SfM_dataValues (#points= " + << dataValues.number_tracks() << ") and values (#points " + << valuesPoints.size() << ")!!" << endl; } - for (size_t j = 0; j < dataValues.number_tracks(); j++){ // for each point + for (size_t j = 0; j < dataValues.number_tracks(); j++) { // for each point Key pointKey = P(j); - if(values.exists(pointKey)){ + if (values.exists(pointKey)) { Point3 point = values.at(pointKey); dataValues.tracks[j].p = point; - }else{ + } else { dataValues.tracks[j].r = 1.0; dataValues.tracks[j].g = 0.0; dataValues.tracks[j].b = 0.0; @@ -861,7 +872,7 @@ Values initialCamerasEstimate(const SfM_data& db) { Values initial; size_t i = 0; // NO POINTS: j = 0; BOOST_FOREACH(const SfM_Camera& camera, db.cameras) - initial.insert(i++, camera); + initial.insert(i++, camera); return initial; } @@ -869,9 +880,9 @@ Values initialCamerasAndPointsEstimate(const SfM_data& db) { Values initial; size_t i = 0, j = 0; BOOST_FOREACH(const SfM_Camera& camera, db.cameras) - initial.insert((i++), camera); + initial.insert((i++), camera); BOOST_FOREACH(const SfM_Track& track, db.tracks) - initial.insert(P(j++), track.p); + initial.insert(P(j++), track.p); return initial; } From 0e1b52150d1a24b71b474b3885acc6c0e4452c60 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 19:01:54 -0400 Subject: [PATCH 42/60] Switch from optional to (possibly empty) shared_ptr --- gtsam/slam/dataset.cpp | 77 ++++++++++++++++++++++++------------------ gtsam/slam/dataset.h | 62 +++++++++++++++++++--------------- 2 files changed, 80 insertions(+), 59 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 3528424fa..805c8eea6 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -16,18 +16,18 @@ * @brief utility functions for loading datasets */ -#include -#include -#include - -#include - -#include -#include -#include #include #include #include +#include +#include +#include + +#include + +#include +#include +#include using namespace std; namespace fs = boost::filesystem; @@ -90,16 +90,16 @@ string createRewrittenFileName(const string& name) { /* ************************************************************************* */ pair load2D( - pair > dataset, - int maxID, bool addNoise, bool smart) { + pair dataset, int maxID, bool addNoise, + bool smart) { return load2D(dataset.first, dataset.second, maxID, addNoise, smart); } /* ************************************************************************* */ pair load2D( - const string& filename, - boost::optional model, int maxID, - bool addNoise, bool smart) { + const string& filename, SharedNoiseModel model, int maxID, bool addNoise, + bool smart) { + cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); if (!is) @@ -119,9 +119,11 @@ pair load2D( int id; double x, y, yaw; is >> id >> x >> y >> yaw; + // optional filter if (maxID && id >= maxID) continue; + initial->insert(id, Pose2(x, y, yaw)); } is.ignore(LINESIZE, '\n'); @@ -130,7 +132,17 @@ pair load2D( is.seekg(0, ios::beg); // Create a sampler with random number generator - Sampler sampler(42u); + Sampler sampler; + if (addNoise) { + noiseModel::Diagonal::shared_ptr noise; + if (model) + noise = boost::dynamic_pointer_cast(model); + if (!noise) + throw invalid_argument( + "gtsam::load2D: invalid noise model for adding noise" + "(current version assumes diagonal noise model)!"); + sampler = Sampler(noise); + } // Parse the pose constraints int id1, id2; @@ -146,21 +158,6 @@ pair load2D( is >> id1 >> id2 >> x >> y >> yaw; is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; - // Try to guess covariance matrix layout - Matrix m(3, 3); - if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 - && v6 == 0.0) { - // Looks like [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ] - m << v1, v2, v5, v2, v3, v6, v5, v6, v4; - } else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 - && v6 != 0.0) { - // Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] - m << v1, v2, v3, v2, v4, v5, v3, v5, v6; - } else { - throw std::invalid_argument( - "load2D: unrecognized covariance matrix format in dataset file"); - } - // optional filter if (maxID && (id1 >= maxID || id2 >= maxID)) continue; @@ -169,12 +166,28 @@ pair load2D( // SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart); if (!model) { + + // Try to guess covariance matrix layout + Matrix m(3, 3); + if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 + && v6 == 0.0) { + // Looks like [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ] + m << v1, v2, v5, v2, v3, v6, v5, v6, v4; + } else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 + && v6 != 0.0) { + // Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] + m << v1, v2, v3, v2, v4, v5, v3, v5, v6; + } else { + throw std::invalid_argument( + "load2D: unrecognized covariance matrix format in dataset file"); + } + Vector variances = (Vector(3) << m(0, 0), m(1, 1), m(2, 2)); model = noiseModel::Diagonal::Variances(variances, smart); } if (addNoise) - l1Xl2 = l1Xl2.retract(sampler.sampleNewModel(*model)); + l1Xl2 = l1Xl2.retract(sampler.sample()); // Insert vertices if pure odometry file if (!initial->exists(id1)) @@ -183,7 +196,7 @@ pair load2D( initial->insert(id2, initial->at(id1) * l1Xl2); NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, l1Xl2, *model)); + new BetweenFactor(id1, id2, l1Xl2, model)); graph->push_back(factor); } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 854daa237..858217b95 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -60,8 +60,8 @@ GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name); * @param smart try to reduce complexity of covariance to cheapest model */ GTSAM_EXPORT std::pair load2D( - std::pair > dataset, - int maxID = 0, bool addNoise = false, bool smart = true); + std::pair dataset, int maxID = 0, + bool addNoise = false, bool smart = true); /** * Load TORO 2D Graph @@ -72,18 +72,17 @@ GTSAM_EXPORT std::pair loa * @param smart try to reduce complexity of covariance to cheapest model */ GTSAM_EXPORT std::pair load2D( - const std::string& filename, - boost::optional model = boost::optional< - noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false, - bool smart = true); + const std::string& filename, SharedNoiseModel model = SharedNoiseModel(), + int maxID = 0, bool addNoise = false, bool smart = true); GTSAM_EXPORT std::pair load2D_robust( - const std::string& filename, - gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0); + const std::string& filename, noiseModel::Base::shared_ptr& model, + int maxID = 0); /** save 2d graph */ -GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config, - const noiseModel::Diagonal::shared_ptr model, const std::string& filename); +GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, + const Values& config, const noiseModel::Diagonal::shared_ptr model, + const std::string& filename); /** * Load TORO 3D Graph @@ -91,27 +90,31 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config GTSAM_EXPORT bool load3D(const std::string& filename); /// A measurement with its camera index -typedef std::pair SfM_Measurement; +typedef std::pair SfM_Measurement; /// Define the structure for the 3D points -struct SfM_Track -{ - gtsam::Point3 p; ///< 3D position of the point - float r,g,b; ///< RGB color of the 3D point +struct SfM_Track { + Point3 p; ///< 3D position of the point + float r, g, b; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) - size_t number_measurements() const { return measurements.size();} + size_t number_measurements() const { + return measurements.size(); + } }; /// Define the structure for the camera poses -typedef gtsam::PinholeCamera SfM_Camera; +typedef PinholeCamera SfM_Camera; /// Define the structure for SfM data -struct SfM_data -{ - std::vector cameras; ///< Set of cameras +struct SfM_data { + std::vector cameras; ///< Set of cameras std::vector tracks; ///< Sparse set of points - size_t number_cameras() const { return cameras.size();} ///< The number of camera poses - size_t number_tracks() const { return tracks.size();} ///< The number of reconstructed 3D points + size_t number_cameras() const { + return cameras.size(); + } ///< The number of camera poses + size_t number_tracks() const { + return tracks.size(); + } ///< The number of reconstructed 3D points }; /** @@ -130,8 +133,12 @@ GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data); * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal. * @return initial Values containing the initial guess (VERTEX_SE2) */ -enum kernelFunctionType { QUADRATIC, HUBER, TUKEY }; -GTSAM_EXPORT bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC); +enum kernelFunctionType { + QUADRATIC, HUBER, TUKEY +}; +GTSAM_EXPORT bool readG2o(const std::string& g2oFile, + NonlinearFactorGraph& graph, Values& initial, + const kernelFunctionType kernelFunction = QUADRATIC); /** * @brief This function writes a g2o file from @@ -140,7 +147,8 @@ GTSAM_EXPORT bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& grap * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2) * @return estimate Values containing the values (VERTEX_SE2) */ -GTSAM_EXPORT bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate); +GTSAM_EXPORT bool writeG2o(const std::string& filename, + const NonlinearFactorGraph& graph, const Values& estimate); /** * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a @@ -171,7 +179,8 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data); * assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1 * @return true if the parsing was successful, false otherwise */ -GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, const SfM_data &data, Values& values); +GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, + const SfM_data &data, Values& values); /** * @brief This function converts an openGL camera pose to an GTSAM camera pose @@ -214,5 +223,4 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfM_data& db); */ GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db); - } // namespace gtsam From ac3d1ea8b63ce8d0e4abd65c998229bb8fc12c43 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 19:02:41 -0400 Subject: [PATCH 43/60] Remove timeLago from tests that are run.... --- gtsam/nonlinear/tests/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/tests/CMakeLists.txt b/gtsam/nonlinear/tests/CMakeLists.txt index 378d93de4..69a3700f2 100644 --- a/gtsam/nonlinear/tests/CMakeLists.txt +++ b/gtsam/nonlinear/tests/CMakeLists.txt @@ -1 +1 @@ -gtsamAddTestsGlob(nonlinear "*.cpp" "" "gtsam") +gtsamAddTestsGlob(nonlinear "test*.cpp" "" "gtsam") From 10d19c6832faa305b6728325bf197cd7bc83b0cb Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 19:15:43 -0400 Subject: [PATCH 44/60] resurrected test --- gtsam/slam/tests/testDataset.cpp | 23 +++++++++++------------ 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 1ab4c9bb4..89c9cd5eb 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -40,18 +40,17 @@ TEST(dataSet, findExampleDataFile) { } /* ************************************************************************* */ -//TEST( dataSet, load2D) -//{ -// ///< The structure where we will save the SfM data -// const string filename = findExampleDataFile("smallGraph.g2o"); -// boost::tie(graph,initialGuess) = load2D(filename, boost::none, 10000, -// false, false); -//// print -//// -//// print -//// -//// EXPECT(assert_equal(expected,actual,12)); -//} +TEST( dataSet, load2D) +{ + ///< The structure where we will save the SfM data + const string filename = findExampleDataFile("w100.graph"); + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + boost::tie(graph, initial) = // + load2D(filename, SharedNoiseModel(), 10000, false, false); + EXPECT_LONGS_EQUAL(300,graph->size()); + EXPECT_LONGS_EQUAL(100,initial->size()); +} /* ************************************************************************* */ TEST( dataSet, Balbianello) From 89e6e2730142f312b75913fb25ecec40de2abe5c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 20:29:00 -0400 Subject: [PATCH 45/60] Additional unit test --- gtsam/linear/NoiseModel.cpp | 16 +++++++++++++++- gtsam/linear/NoiseModel.h | 10 ++++++++++ gtsam/linear/tests/testNoiseModel.cpp | 11 +++++++++++ 3 files changed, 36 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 45314e023..3d11542db 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -49,7 +49,7 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) { /* ************************************************************************* */ // check *above the diagonal* for non-zero entries -static boost::optional checkIfDiagonal(const Matrix M) { +boost::optional checkIfDiagonal(const Matrix M) { size_t m = M.rows(), n = M.cols(); // check all non-diagonal entries bool full = false; @@ -82,6 +82,20 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { else return shared_ptr(new Gaussian(R.rows(),R)); } +/* ************************************************************************* */ +Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) { + size_t m = M.rows(), n = M.cols(); + if (m != n) throw invalid_argument("Gaussian::Information: R not square"); + boost::optional diagonal = boost::none; + if (smart) + diagonal = checkIfDiagonal(M); + if (diagonal) return Diagonal::Precisions(*diagonal, true); + else { + Matrix R = RtR(M); + return shared_ptr(new Gaussian(R.rows(), R)); + } +} + /* ************************************************************************* */ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) { size_t m = covariance.rows(), n = covariance.cols(); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index e709ea543..55d01c92f 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -164,6 +164,13 @@ namespace gtsam { */ static shared_ptr SqrtInformation(const Matrix& R, bool smart = true); + /** + * A Gaussian noise model created by specifying an information matrix. + * @param M The information matrix + * @param smart check if can be simplified to derived class + */ + static shared_ptr Information(const Matrix& M, bool smart = true); + /** * A Gaussian noise model created by specifying a covariance matrix. * @param covariance The square covariance Matrix @@ -864,6 +871,9 @@ namespace gtsam { ar & boost::serialization::make_nvp("noise_", const_cast(noise_)); } }; + + // Helper function + boost::optional checkIfDiagonal(const Matrix M); } // namespace noiseModel diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index d68caeabe..df0f8a774 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -285,6 +285,17 @@ TEST(NoiseModel, SmartSqrtInformation2 ) EXPECT(assert_equal(*expected,*actual)); } +/* ************************************************************************* */ +TEST(NoiseModel, SmartInformation ) +{ + bool smart = true; + gtsam::SharedGaussian expected = Unit::Isotropic::Variance(3,2); + Matrix M = 0.5*eye(3); + EXPECT(checkIfDiagonal(M)); + gtsam::SharedGaussian actual = Gaussian::Information(M, smart); + EXPECT(assert_equal(*expected,*actual)); +} + /* ************************************************************************* */ TEST(NoiseModel, SmartCovariance ) { From 0f2d98319052876dcd319b21a83ac8dceb1faf8a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 21:57:29 -0400 Subject: [PATCH 46/60] More principled handling of noise parameters --- gtsam/slam/dataset.cpp | 204 ++++++++++++++++++----------------------- gtsam/slam/dataset.h | 25 +++-- 2 files changed, 110 insertions(+), 119 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 805c8eea6..687e67a1f 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -91,14 +91,79 @@ string createRewrittenFileName(const string& name) { /* ************************************************************************* */ pair load2D( pair dataset, int maxID, bool addNoise, - bool smart) { - return load2D(dataset.first, dataset.second, maxID, addNoise, smart); + bool smart, NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { + return load2D(dataset.first, dataset.second, maxID, addNoise, smart, + noiseFormat, kernelFunctionType); +} + +/* ************************************************************************* */ +// Read noise parameters and interpret them according to flags +static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, + NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { + double v1, v2, v3, v4, v5, v6; + is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; + + Matrix M(3, 3); + switch (noiseFormat) { + case NoiseFormatG2O: + // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] + if (v1 == 0.0 || v4 == 0.0 || v6 == 0.0) + throw std::invalid_argument( + "load2D: looks like this is not G2O file format"); + M << v1, v2, v3, v2, v4, v5, v3, v5, v6; + break; + case NoiseFormatTORO: + case NoiseFormatGRAPH: + // http://www.openslam.org/toro.html + // inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr + // i.e., [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ] + if (v1 == 0.0 || v3 == 0.0 || v4 == 0.0) + throw std::invalid_argument( + "load2D: looks like this is not TORO file format"); + M << v1, v2, v5, v2, v3, v6, v5, v6, v4; + break; + default: + throw std::invalid_argument("load2D: invalid noise format"); + } + + SharedNoiseModel model; + switch (noiseFormat) { + case NoiseFormatG2O: + case NoiseFormatTORO: + // In both cases, what is stored in file is the information matrix + model = noiseModel::Gaussian::Information(M, smart); + break; + case NoiseFormatGRAPH: + // but default case expects covariance matrix + model = noiseModel::Gaussian::Covariance(M, smart); + break; + default: + throw std::invalid_argument("load2D: invalid noise format"); + } + + switch (kernelFunctionType) { + case KernelFunctionTypeQUADRATIC: + return model; + break; + case KernelFunctionTypeHUBER: + return noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), model); + break; + case KernelFunctionTypeTUKEY: + return noiseModel::Robust::Create( + noiseModel::mEstimator::Tukey::Create(4.6851), model); + break; + default: + throw std::invalid_argument("load2D: invalid kernel function type"); + } } /* ************************************************************************* */ pair load2D( const string& filename, SharedNoiseModel model, int maxID, bool addNoise, - bool smart) { + bool smart, NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); @@ -131,7 +196,7 @@ pair load2D( is.clear(); /* clears the end-of-file and error flags */ is.seekg(0, ios::beg); - // Create a sampler with random number generator + // If asked, create a sampler with random number generator Sampler sampler; if (addNoise) { noiseModel::Diagonal::shared_ptr noise; @@ -151,40 +216,24 @@ pair load2D( if (!(is >> tag)) break; - if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { - double x, y, yaw; - double v1, v2, v3, v4, v5, v6; + if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") + || (tag == "ODOMETRY")) { + // Read transform + double x, y, yaw; is >> id1 >> id2 >> x >> y >> yaw; - is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; + Pose2 l1Xl2(x, y, yaw); + + // read noise model + SharedNoiseModel modelInFile = readNoiseModel(is, smart, noiseFormat, + kernelFunctionType); // optional filter if (maxID && (id1 >= maxID || id2 >= maxID)) continue; - Pose2 l1Xl2(x, y, yaw); - - // SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart); - if (!model) { - - // Try to guess covariance matrix layout - Matrix m(3, 3); - if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 - && v6 == 0.0) { - // Looks like [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ] - m << v1, v2, v5, v2, v3, v6, v5, v6, v4; - } else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 - && v6 != 0.0) { - // Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] - m << v1, v2, v3, v2, v4, v5, v3, v5, v6; - } else { - throw std::invalid_argument( - "load2D: unrecognized covariance matrix format in dataset file"); - } - - Vector variances = (Vector(3) << m(0, 0), m(1, 1), m(2, 2)); - model = noiseModel::Diagonal::Variances(variances, smart); - } + if (!model) + model = modelInFile; if (addNoise) l1Xl2 = l1Xl2.retract(sampler.sample()); @@ -565,89 +614,18 @@ bool readBundler(const string& filename, SfM_data &data) { /* ************************************************************************* */ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, - Values& initial, const kernelFunctionType kernelFunction) { + Values& initial, KernelFunctionType kernelFunctionType) { - ifstream is(g2oFile.c_str()); - if (!is) { - throw std::invalid_argument("File not found!"); - return false; - } - - // READ INITIAL GUESS FROM G2O FILE - string tag; - while (is) { - if (!(is >> tag)) - break; - - if (tag == "VERTEX_SE2" || tag == "VERTEX2") { - int id; - double x, y, yaw; - is >> id >> x >> y >> yaw; - initial.insert(id, Pose2(x, y, yaw)); - } - is.ignore(LINESIZE, '\n'); - } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); - // initial.print("initial guess"); - - // READ MEASUREMENTS FROM G2O FILE - while (is) { - if (!(is >> tag)) - break; - - if (tag == "EDGE_SE2" || tag == "EDGE2") { - int id1, id2; - double x, y, yaw; - double I11, I12, I13, I22, I23, I33; - - is >> id1 >> id2 >> x >> y >> yaw; - is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33; - Pose2 l1Xl2(x, y, yaw); - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions( - (Vector(3) << I11, I22, I33)); - - switch (kernelFunction) { - { - case QUADRATIC: - NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, l1Xl2, model)); - graph.add(factor); - break; - } - { - case HUBER: - NonlinearFactor::shared_ptr huberFactor( - new BetweenFactor(id1, id2, l1Xl2, - noiseModel::Robust::Create( - noiseModel::mEstimator::Huber::Create(1.345), model))); - graph.add(huberFactor); - break; - } - { - case TUKEY: - NonlinearFactor::shared_ptr tukeyFactor( - new BetweenFactor(id1, id2, l1Xl2, - noiseModel::Robust::Create( - noiseModel::mEstimator::Tukey::Create(4.6851), model))); - graph.add(tukeyFactor); - break; - } - } - } - is.ignore(LINESIZE, '\n'); - } - // Output which kernel is used - switch (kernelFunction) { - case QUADRATIC: - break; - case HUBER: - std::cout << "Robust kernel: Huber" << std::endl; - break; - case TUKEY: - std::cout << "Robust kernel: Tukey" << std::endl; - break; - } + // just call load2D + NonlinearFactorGraph::shared_ptr graph_ptr; + Values::shared_ptr initial_ptr; + int maxID = 0; + bool addNoise = false; + bool smart = true; + boost::tie(graph_ptr, initial_ptr) = load2D(g2oFile, SharedNoiseModel(), + maxID, addNoise, smart, NoiseFormatG2O, kernelFunctionType); + graph = *graph_ptr; + initial = *initial_ptr; return true; } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 858217b95..289891a5b 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -52,6 +52,17 @@ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name); #endif +/// Indicates how noise parameters are stored in file +enum NoiseFormat { + NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33 + NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr + NoiseFormatGRAPH ///< default: toro-style order, but covariance matrix ! +}; + +enum KernelFunctionType { + KernelFunctionTypeQUADRATIC, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY +}; + /** * Load TORO 2D Graph * @param dataset/model pair as constructed by [dataset] @@ -61,7 +72,10 @@ GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name); */ GTSAM_EXPORT std::pair load2D( std::pair dataset, int maxID = 0, - bool addNoise = false, bool smart = true); + bool addNoise = false, + bool smart = true, // + NoiseFormat noiseFormat = NoiseFormatGRAPH, + KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC); /** * Load TORO 2D Graph @@ -73,7 +87,9 @@ GTSAM_EXPORT std::pair loa */ GTSAM_EXPORT std::pair load2D( const std::string& filename, SharedNoiseModel model = SharedNoiseModel(), - int maxID = 0, bool addNoise = false, bool smart = true); + int maxID = 0, bool addNoise = false, bool smart = true, + NoiseFormat noiseFormat = NoiseFormatGRAPH, // + KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC); GTSAM_EXPORT std::pair load2D_robust( const std::string& filename, noiseModel::Base::shared_ptr& model, @@ -133,12 +149,9 @@ GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data); * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal. * @return initial Values containing the initial guess (VERTEX_SE2) */ -enum kernelFunctionType { - QUADRATIC, HUBER, TUKEY -}; GTSAM_EXPORT bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, - const kernelFunctionType kernelFunction = QUADRATIC); + KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC); /** * @brief This function writes a g2o file from From f32968cc037acbcaa8a7640e5de98187faa30e04 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 21:57:36 -0400 Subject: [PATCH 47/60] Added extra test case --- gtsam/slam/tests/testDataset.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 89c9cd5eb..9335372c8 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -50,6 +50,11 @@ TEST( dataSet, load2D) load2D(filename, SharedNoiseModel(), 10000, false, false); EXPECT_LONGS_EQUAL(300,graph->size()); EXPECT_LONGS_EQUAL(100,initial->size()); + noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3); + BetweenFactor expected(1, 0, Pose2(-0.99879,0.0417574,-0.00818381), model); + BetweenFactor::shared_ptr actual = boost::dynamic_pointer_cast< + BetweenFactor >(graph->at(0)); + EXPECT(assert_equal(expected, *actual)); } /* ************************************************************************* */ @@ -118,7 +123,7 @@ TEST( dataSet, readG2oHuber) const string g2oFile = findExampleDataFile("pose2example"); NonlinearFactorGraph actualGraph; Values actualValues; - readG2o(g2oFile, actualGraph, actualValues, HUBER); + readG2o(g2oFile, actualGraph, actualValues, KernelFunctionTypeHUBER); noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); @@ -145,7 +150,7 @@ TEST( dataSet, readG2oTukey) const string g2oFile = findExampleDataFile("pose2example"); NonlinearFactorGraph actualGraph; Values actualValues; - readG2o(g2oFile, actualGraph, actualValues, TUKEY); + readG2o(g2oFile, actualGraph, actualValues, KernelFunctionTypeTUKEY); noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); From 8f493d6ee5b40f268caec6d08615b2c3f73217f4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 22:10:51 -0400 Subject: [PATCH 48/60] Formatting only --- gtsam/linear/NoiseModel.cpp | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 3d11542db..fcaec3afa 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -74,22 +74,27 @@ boost::optional checkIfDiagonal(const Matrix M) { /* ************************************************************************* */ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { size_t m = R.rows(), n = R.cols(); - if (m != n) throw invalid_argument("Gaussian::SqrtInformation: R not square"); + if (m != n) + throw invalid_argument("Gaussian::SqrtInformation: R not square"); boost::optional diagonal = boost::none; if (smart) diagonal = checkIfDiagonal(R); - if (diagonal) return Diagonal::Sigmas(reciprocal(*diagonal),true); - else return shared_ptr(new Gaussian(R.rows(),R)); + if (diagonal) + return Diagonal::Sigmas(reciprocal(*diagonal), true); + else + return shared_ptr(new Gaussian(R.rows(), R)); } /* ************************************************************************* */ Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) { size_t m = M.rows(), n = M.cols(); - if (m != n) throw invalid_argument("Gaussian::Information: R not square"); + if (m != n) + throw invalid_argument("Gaussian::Information: R not square"); boost::optional diagonal = boost::none; if (smart) diagonal = checkIfDiagonal(M); - if (diagonal) return Diagonal::Precisions(*diagonal, true); + if (diagonal) + return Diagonal::Precisions(*diagonal, true); else { Matrix R = RtR(M); return shared_ptr(new Gaussian(R.rows(), R)); @@ -97,14 +102,18 @@ Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) { } /* ************************************************************************* */ -Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) { +Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, + bool smart) { size_t m = covariance.rows(), n = covariance.cols(); - if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square"); + if (m != n) + throw invalid_argument("Gaussian::Covariance: covariance not square"); boost::optional variances = boost::none; if (smart) variances = checkIfDiagonal(covariance); - if (variances) return Diagonal::Variances(*variances,true); - else return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); + if (variances) + return Diagonal::Variances(*variances, true); + else + return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); } /* ************************************************************************* */ From 9ea155dee2199a3be18d2a0a616cecbd36512ad2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 22:11:15 -0400 Subject: [PATCH 49/60] Get default smart flag --- gtsam/slam/tests/testDataset.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 9335372c8..8789c4085 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -46,8 +46,7 @@ TEST( dataSet, load2D) const string filename = findExampleDataFile("w100.graph"); NonlinearFactorGraph::shared_ptr graph; Values::shared_ptr initial; - boost::tie(graph, initial) = // - load2D(filename, SharedNoiseModel(), 10000, false, false); + boost::tie(graph, initial) = load2D(filename); EXPECT_LONGS_EQUAL(300,graph->size()); EXPECT_LONGS_EQUAL(100,initial->size()); noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3); From 3e532a5160515779b0443f8c4bc3581c72200a77 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 22:42:14 -0400 Subject: [PATCH 50/60] Fourth case: sensible order, but covariance matrix --- gtsam/slam/dataset.cpp | 15 ++++++++++----- gtsam/slam/dataset.h | 3 ++- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 687e67a1f..91b1df782 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -104,13 +104,15 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, double v1, v2, v3, v4, v5, v6; is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; + // Read matrix and check that diagonal entries are non-zero Matrix M(3, 3); switch (noiseFormat) { case NoiseFormatG2O: + case NoiseFormatCOV: // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] if (v1 == 0.0 || v4 == 0.0 || v6 == 0.0) - throw std::invalid_argument( - "load2D: looks like this is not G2O file format"); + throw std::runtime_error( + "load2D::readNoiseModel looks like this is not G2O matrix order"); M << v1, v2, v3, v2, v4, v5, v3, v5, v6; break; case NoiseFormatTORO: @@ -120,13 +122,15 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, // i.e., [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ] if (v1 == 0.0 || v3 == 0.0 || v4 == 0.0) throw std::invalid_argument( - "load2D: looks like this is not TORO file format"); + "load2D::readNoiseModel looks like this is not TORO matrix order"); M << v1, v2, v5, v2, v3, v6, v5, v6, v4; break; default: - throw std::invalid_argument("load2D: invalid noise format"); + throw std::runtime_error("load2D: invalid noise format"); } + // Now, create a Gaussian noise model + // The smart flag will try to detect a simpler model, e.g., unit SharedNoiseModel model; switch (noiseFormat) { case NoiseFormatG2O: @@ -135,7 +139,8 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, model = noiseModel::Gaussian::Information(M, smart); break; case NoiseFormatGRAPH: - // but default case expects covariance matrix + case NoiseFormatCOV: + // These cases expect covariance matrix model = noiseModel::Gaussian::Covariance(M, smart); break; default: diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 289891a5b..771328d6d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -56,7 +56,8 @@ GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name); enum NoiseFormat { NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33 NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr - NoiseFormatGRAPH ///< default: toro-style order, but covariance matrix ! + NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix ! + NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33 }; enum KernelFunctionType { From a95cf7c71b32547d47ad4f907f0434b9a9831b24 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 23:15:11 -0400 Subject: [PATCH 51/60] Load VERTEX_SE2 --- gtsam/slam/dataset.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 91b1df782..b397ab60d 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -185,7 +185,7 @@ pair load2D( if (!(is >> tag)) break; - if ((tag == "VERTEX2") || (tag == "VERTEX")) { + if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { int id; double x, y, yaw; is >> id >> x >> y >> yaw; From 351e9ace72d24d7dc780bbddaad7bf7b415bb53e Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Sun, 1 Jun 2014 11:28:20 -0400 Subject: [PATCH 52/60] add GTSAM_EXPORT --- gtsam/linear/NoiseModel.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 55d01c92f..0351cfabd 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -873,7 +873,7 @@ namespace gtsam { }; // Helper function - boost::optional checkIfDiagonal(const Matrix M); + GTSAM_EXPORT boost::optional checkIfDiagonal(const Matrix M); } // namespace noiseModel From 470527ff99ced5c28fbfd769a1066dde08e8d4d3 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Sun, 1 Jun 2014 11:30:04 -0400 Subject: [PATCH 53/60] fix warnings on Windows. No need to create variable in catch statement if you're not going to use it. It generates unreferenced variable warnings. --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 2 +- gtsam/slam/SmartFactorBase.h | 4 ++-- gtsam/slam/SmartProjectionFactor.h | 2 +- wrap/FileWriter.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 2cde6768f..08961db86 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -244,7 +244,7 @@ void LevenbergMarquardtOptimizer::iterate() { try { delta = solve(dampedSystem, state_.values, params_); systemSolvedSuccessfully = true; - } catch (IndeterminantLinearSystemException& e) { + } catch (IndeterminantLinearSystemException) { systemSolvedSuccessfully = false; } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 93931549f..1235fc6fb 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -325,7 +325,7 @@ public: const Cameras& cameras, const Point3& point, const double lambda = 0.0) const { - int numKeys = this->keys_.size(); + size_t numKeys = this->keys_.size(); std::vector Fblocks; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda); @@ -352,7 +352,7 @@ public: Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); Vector s = svd.singularValues(); // Enull = zeros(2 * numKeys, 2 * numKeys - 3); - int numKeys = this->keys_.size(); + size_t numKeys = this->keys_.size(); Enull = svd.matrixU().block(0, 3, 2 * numKeys, 2 * numKeys - 3); // last 2m-3 columns return f; diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 7c3719cd8..043528fea 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -262,7 +262,7 @@ public: try { Point2 reprojectionError(camera.project(point_) - zi); totalReprojError += reprojectionError.vector().norm(); - } catch (CheiralityException& e) { + } catch (CheiralityException) { cheiralityException_ = true; } i += 1; diff --git a/wrap/FileWriter.cpp b/wrap/FileWriter.cpp index c5e64614e..3f5461078 100644 --- a/wrap/FileWriter.cpp +++ b/wrap/FileWriter.cpp @@ -28,7 +28,7 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const { bool file_exists = true; try { existing_contents = file_contents(filename_.c_str(), add_header); - } catch (CantOpenFile& e) { + } catch (CantOpenFile) { file_exists = false; } From 7119d0c3c2469452e1b4ca03c51504230166d09d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 1 Jun 2014 11:46:23 -0400 Subject: [PATCH 54/60] Sanitized G2o I/O interface to conform to what we had before. No sense in having many different styles, and this works better for MATLAB (now wrapped, as well). BAL reading/writing should be similarly cleaned up. --- examples/Pose2SLAMExample_g2o.cpp | 12 +- examples/Pose2SLAMExample_lago.cpp | 10 +- gtsam.h | 7 + gtsam/nonlinear/tests/testLago.cpp | 36 ++--- gtsam/slam/dataset.cpp | 244 ++++++++--------------------- gtsam/slam/dataset.h | 77 ++++----- gtsam/slam/tests/testDataset.cpp | 44 +++--- 7 files changed, 168 insertions(+), 262 deletions(-) diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 565a26965..3a4ee9cff 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -35,18 +35,18 @@ int main(const int argc, const char *argv[]) { else g2oFile = argv[1]; - NonlinearFactorGraph graph; - Values initial; - readG2o(g2oFile, graph, initial); + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + boost::tie(graph, initial) = readG2o(g2oFile); // Add prior on the pose having index (key) = 0 - NonlinearFactorGraph graphWithPrior = graph; + NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); std::cout << "Optimizing the factor graph" << std::endl; - GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters); + GaussNewtonOptimizer optimizer(graphWithPrior, *initial); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; @@ -55,7 +55,7 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(outputFile, graph, result); + writeG2o(*graph, result, outputFile); std::cout << "done! " << std::endl; } return 0; diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index a6340caf7..cf82a05ca 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -36,12 +36,12 @@ int main(const int argc, const char *argv[]) { else g2oFile = argv[1]; - NonlinearFactorGraph graph; - Values initial; - readG2o(g2oFile, graph, initial); + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + boost::tie(graph, initial) = readG2o(g2oFile); // Add prior on the pose having index (key) = 0 - NonlinearFactorGraph graphWithPrior = graph; + NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); @@ -56,7 +56,7 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(outputFile, graph, estimateLago); + writeG2o(*graph, estimateLago, outputFile); std::cout << "done! " << std::endl; } diff --git a/gtsam.h b/gtsam.h index 96a778acf..b7178d534 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2249,6 +2249,13 @@ pair load2D(string filename, pair load2D(string filename); pair load2D_robust(string filename, gtsam::noiseModel::Base* model); +void save2D(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, + string filename); + +pair readG2o(string filename); +void writeG2o(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& estimate, string filename); //************************************************************************* // Navigation diff --git a/gtsam/nonlinear/tests/testLago.cpp b/gtsam/nonlinear/tests/testLago.cpp index f2cab4506..6a94588b4 100644 --- a/gtsam/nonlinear/tests/testLago.cpp +++ b/gtsam/nonlinear/tests/testLago.cpp @@ -258,13 +258,13 @@ TEST( Lago, smallGraph2 ) { /* *************************************************************************** */ TEST( Lago, largeGraphNoisy_orientations ) { - NonlinearFactorGraph g; - Values initial; string inputFile = findExampleDataFile("noisyToyGraph"); - readG2o(inputFile, g, initial); + NonlinearFactorGraph::shared_ptr g; + Values::shared_ptr initial; + boost::tie(g, initial) = readG2o(inputFile); // Add prior on the pose having index (key) = 0 - NonlinearFactorGraph graphWithPrior = g; + NonlinearFactorGraph graphWithPrior = *g; noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); @@ -279,40 +279,40 @@ TEST( Lago, largeGraphNoisy_orientations ) { actual.insert(key, poseLago); } } - NonlinearFactorGraph gmatlab; - Values expected; string matlabFile = findExampleDataFile("orientationsNoisyToyGraph"); - readG2o(matlabFile, gmatlab, expected); + NonlinearFactorGraph::shared_ptr gmatlab; + Values::shared_ptr expected; + boost::tie(gmatlab, expected) = readG2o(matlabFile); - BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){ + BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){ Key k = key_val.key; - EXPECT(assert_equal(expected.at(k), actual.at(k), 1e-5)); + EXPECT(assert_equal(expected->at(k), actual.at(k), 1e-5)); } } /* *************************************************************************** */ TEST( Lago, largeGraphNoisy ) { - NonlinearFactorGraph g; - Values initial; string inputFile = findExampleDataFile("noisyToyGraph"); - readG2o(inputFile, g, initial); + NonlinearFactorGraph::shared_ptr g; + Values::shared_ptr initial; + boost::tie(g, initial) = readG2o(inputFile); // Add prior on the pose having index (key) = 0 - NonlinearFactorGraph graphWithPrior = g; + NonlinearFactorGraph graphWithPrior = *g; noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); Values actual = lago::initialize(graphWithPrior); - NonlinearFactorGraph gmatlab; - Values expected; string matlabFile = findExampleDataFile("optimizedNoisyToyGraph"); - readG2o(matlabFile, gmatlab, expected); + NonlinearFactorGraph::shared_ptr gmatlab; + Values::shared_ptr expected; + boost::tie(gmatlab, expected) = readG2o(matlabFile); - BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){ + BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){ Key k = key_val.key; - EXPECT(assert_equal(expected.at(k), actual.at(k), 1e-2)); + EXPECT(assert_equal(expected->at(k), actual.at(k), 1e-2)); } } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index b397ab60d..b3c9b9557 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -62,7 +62,7 @@ string findExampleDataFile(const string& name) { // If we did not return already, then we did not find the file throw -std::invalid_argument( +invalid_argument( "gtsam::findExampleDataFile could not find a matching file in\n" SOURCE_TREE_DATASET_DIR " or\n" INSTALLED_DATASET_DIR " named\n" + @@ -73,7 +73,7 @@ std::invalid_argument( string createRewrittenFileName(const string& name) { // Search source tree and installed location if (!exists(fs::path(name))) { - throw std::invalid_argument( + throw invalid_argument( "gtsam::createRewrittenFileName could not find a matching file in\n" + name); } @@ -89,9 +89,8 @@ string createRewrittenFileName(const string& name) { #endif /* ************************************************************************* */ -pair load2D( - pair dataset, int maxID, bool addNoise, - bool smart, NoiseFormat noiseFormat, +GraphAndValues load2D(pair dataset, int maxID, + bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { return load2D(dataset.first, dataset.second, maxID, addNoise, smart, noiseFormat, kernelFunctionType); @@ -111,7 +110,7 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, case NoiseFormatCOV: // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] if (v1 == 0.0 || v4 == 0.0 || v6 == 0.0) - throw std::runtime_error( + throw runtime_error( "load2D::readNoiseModel looks like this is not G2O matrix order"); M << v1, v2, v3, v2, v4, v5, v3, v5, v6; break; @@ -121,12 +120,12 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, // inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr // i.e., [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ] if (v1 == 0.0 || v3 == 0.0 || v4 == 0.0) - throw std::invalid_argument( + throw invalid_argument( "load2D::readNoiseModel looks like this is not TORO matrix order"); M << v1, v2, v5, v2, v3, v6, v5, v6, v4; break; default: - throw std::runtime_error("load2D: invalid noise format"); + throw runtime_error("load2D: invalid noise format"); } // Now, create a Gaussian noise model @@ -144,11 +143,11 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, model = noiseModel::Gaussian::Covariance(M, smart); break; default: - throw std::invalid_argument("load2D: invalid noise format"); + throw invalid_argument("load2D: invalid noise format"); } switch (kernelFunctionType) { - case KernelFunctionTypeQUADRATIC: + case KernelFunctionTypeNONE: return model; break; case KernelFunctionTypeHUBER: @@ -160,20 +159,18 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, noiseModel::mEstimator::Tukey::Create(4.6851), model); break; default: - throw std::invalid_argument("load2D: invalid kernel function type"); + throw invalid_argument("load2D: invalid kernel function type"); } } /* ************************************************************************* */ -pair load2D( - const string& filename, SharedNoiseModel model, int maxID, bool addNoise, - bool smart, NoiseFormat noiseFormat, +GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID, + bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { - cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); if (!is) - throw std::invalid_argument("load2D: can not find file " + filename); + throw invalid_argument("load2D: can not find file " + filename); Values::shared_ptr initial(new Values); NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); @@ -270,8 +267,8 @@ pair load2D( is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3; // Convert x,y to bearing,range - bearing = std::atan2(lmy, lmx); - range = std::sqrt(lmx * lmx + lmy * lmy); + bearing = atan2(lmy, lmx); + range = sqrt(lmx * lmx + lmy * lmy); // In our experience, the x-y covariance on landmark sightings is not very good, so assume // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty. @@ -319,12 +316,15 @@ pair load2D( is.ignore(LINESIZE, '\n'); } - cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; - return make_pair(graph, initial); } +/* ************************************************************************* */ +GraphAndValues load2D_robust(const string& filename, + noiseModel::Base::shared_ptr& model, int maxID) { + return load2D(filename, model, maxID); +} + /* ************************************************************************* */ void save2D(const NonlinearFactorGraph& graph, const Values& config, const noiseModel::Diagonal::shared_ptr model, const string& filename) { @@ -357,6 +357,54 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, stream.close(); } +/* ************************************************************************* */ +GraphAndValues readG2o(const string& g2oFile, + KernelFunctionType kernelFunctionType) { + // just call load2D + int maxID = 0; + bool addNoise = false; + bool smart = true; + return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, + NoiseFormatG2O, kernelFunctionType); +} + +/* ************************************************************************* */ +void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, + const string& filename) { + + fstream stream(filename.c_str(), fstream::out); + + // save poses + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) { + const Pose2& pose = dynamic_cast(key_value.value); + stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " + << pose.y() << " " << pose.theta() << endl; + } + + // save edges + BOOST_FOREACH(boost::shared_ptr factor_, graph) { + boost::shared_ptr > factor = + boost::dynamic_pointer_cast >(factor_); + if (!factor) + continue; + + SharedNoiseModel model = factor->get_noiseModel(); + boost::shared_ptr diagonalModel = + boost::dynamic_pointer_cast(model); + if (!diagonalModel) + throw invalid_argument( + "writeG2o: invalid noise model (current version assumes diagonal noise model)!"); + + Pose2 pose = factor->measured(); //.inverse(); + stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " + << pose.x() << " " << pose.y() << " " << pose.theta() << " " + << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " " + << diagonalModel->precision(1) << " " << 0.0 << " " + << diagonalModel->precision(2) << endl; + } + stream.close(); +} + /* ************************************************************************* */ bool load3D(const string& filename) { ifstream is(filename.c_str()); @@ -399,105 +447,6 @@ bool load3D(const string& filename) { return true; } -/* ************************************************************************* */ -pair load2D_robust( - const string& filename, noiseModel::Base::shared_ptr& model, int maxID) { - cout << "Will try to read " << filename << endl; - ifstream is(filename.c_str()); - if (!is) - throw std::invalid_argument("load2D: can not find the file!"); - - Values::shared_ptr initial(new Values); - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); - - string tag; - - // load the poses - while (is) { - is >> tag; - - if ((tag == "VERTEX2") || (tag == "VERTEX")) { - int id; - double x, y, yaw; - is >> id >> x >> y >> yaw; - // optional filter - if (maxID && id >= maxID) - continue; - initial->insert(id, Pose2(x, y, yaw)); - } - is.ignore(LINESIZE, '\n'); - } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); - - // Create a sampler with random number generator - Sampler sampler(42u); - - // load the factors - while (is) { - is >> tag; - - if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { - int id1, id2; - double x, y, yaw; - - is >> id1 >> id2 >> x >> y >> yaw; - Matrix m = eye(3); - is >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2); - m(2, 0) = m(0, 2); - m(2, 1) = m(1, 2); - m(1, 0) = m(0, 1); - - // optional filter - if (maxID && (id1 >= maxID || id2 >= maxID)) - continue; - - Pose2 l1Xl2(x, y, yaw); - - // Insert vertices if pure odometry file - if (!initial->exists(id1)) - initial->insert(id1, Pose2()); - if (!initial->exists(id2)) - initial->insert(id2, initial->at(id1) * l1Xl2); - - NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, l1Xl2, model)); - graph->push_back(factor); - } - if (tag == "BR") { - int id1, id2; - double bearing, range, bearing_std, range_std; - - is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std; - - // optional filter - if (maxID && (id1 >= maxID || id2 >= maxID)) - continue; - - noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); - *graph += BearingRangeFactor(id1, id2, bearing, range, - measurementNoise); - - // Insert poses or points if they do not exist yet - if (!initial->exists(id1)) - initial->insert(id1, Pose2()); - if (!initial->exists(id2)) { - Pose2 pose = initial->at(id1); - Point2 local(cos(bearing) * range, sin(bearing) * range); - Point2 global = pose.transform_from(local); - initial->insert(id2, global); - } - } - is.ignore(LINESIZE, '\n'); - } - - cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; - - return make_pair(graph, initial); -} - /* ************************************************************************* */ Rot3 openGLFixedRotation() { // this is due to different convention for cameras in gtsam and openGL /* R = [ 1 0 0 @@ -617,61 +566,6 @@ bool readBundler(const string& filename, SfM_data &data) { return true; } -/* ************************************************************************* */ -bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, - Values& initial, KernelFunctionType kernelFunctionType) { - - // just call load2D - NonlinearFactorGraph::shared_ptr graph_ptr; - Values::shared_ptr initial_ptr; - int maxID = 0; - bool addNoise = false; - bool smart = true; - boost::tie(graph_ptr, initial_ptr) = load2D(g2oFile, SharedNoiseModel(), - maxID, addNoise, smart, NoiseFormatG2O, kernelFunctionType); - graph = *graph_ptr; - initial = *initial_ptr; - return true; -} - -/* ************************************************************************* */ -bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, - const Values& estimate) { - - fstream stream(filename.c_str(), fstream::out); - - // save poses - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) { - const Pose2& pose = dynamic_cast(key_value.value); - stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " - << pose.y() << " " << pose.theta() << endl; - } - - // save edges - BOOST_FOREACH(boost::shared_ptr factor_, graph) { - boost::shared_ptr > factor = - boost::dynamic_pointer_cast >(factor_); - if (!factor) - continue; - - SharedNoiseModel model = factor->get_noiseModel(); - boost::shared_ptr diagonalModel = - boost::dynamic_pointer_cast(model); - if (!diagonalModel) - throw std::invalid_argument( - "writeG2o: invalid noise model (current version assumes diagonal noise model)!"); - - Pose2 pose = factor->measured(); //.inverse(); - stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " - << pose.x() << " " << pose.y() << " " << pose.theta() << " " - << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " " - << diagonalModel->precision(1) << " " << 0.0 << " " - << diagonalModel->precision(2) << endl; - } - stream.close(); - return true; -} - /* ************************************************************************* */ bool readBAL(const string& filename, SfM_data &data) { // Load the data file diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 771328d6d..8fd75269c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -54,16 +54,20 @@ GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name); /// Indicates how noise parameters are stored in file enum NoiseFormat { - NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33 - NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr + NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33 + NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix ! - NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33 + NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33 }; +/// Robust kernel type to wrap around quadratic noise model enum KernelFunctionType { - KernelFunctionTypeQUADRATIC, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY + KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY }; +/// Return type for load functions +typedef std::pair GraphAndValues; + /** * Load TORO 2D Graph * @param dataset/model pair as constructed by [dataset] @@ -71,36 +75,58 @@ enum KernelFunctionType { * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model */ -GTSAM_EXPORT std::pair load2D( +GTSAM_EXPORT GraphAndValues load2D( std::pair dataset, int maxID = 0, bool addNoise = false, bool smart = true, // NoiseFormat noiseFormat = NoiseFormatGRAPH, - KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC); + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /** - * Load TORO 2D Graph + * Load TORO/G2O style graph files * @param filename * @param model optional noise model to use instead of one specified by file * @param maxID if non-zero cut out vertices >= maxID * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model + * @param noiseFormat how noise parameters are stored + * @param kernelFunctionType whether to wrap the noise model in a robust kernel + * @return graph and initial values */ -GTSAM_EXPORT std::pair load2D( - const std::string& filename, SharedNoiseModel model = SharedNoiseModel(), - int maxID = 0, bool addNoise = false, bool smart = true, - NoiseFormat noiseFormat = NoiseFormatGRAPH, // - KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC); +GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, + SharedNoiseModel model = SharedNoiseModel(), int maxID = 0, bool addNoise = + false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, // + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); -GTSAM_EXPORT std::pair load2D_robust( - const std::string& filename, noiseModel::Base::shared_ptr& model, - int maxID = 0); +/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel +GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename, + noiseModel::Base::shared_ptr& model, int maxID = 0); /** save 2d graph */ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config, const noiseModel::Diagonal::shared_ptr model, const std::string& filename); +/** + * @brief This function parses a g2o file and stores the measurements into a + * NonlinearFactorGraph and the initial guess in a Values structure + * @param filename The name of the g2o file + * @param kernelFunctionType whether to wrap the noise model in a robust kernel + * @return graph and initial values + */ +GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); + +/** + * @brief This function writes a g2o file from + * NonlinearFactorGraph and a Values structure + * @param filename The name of the g2o file to write + * @param graph NonlinearFactor graph storing the measurements + * @param estimate Values + */ +GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, + const Values& estimate, const std::string& filename); + /** * Load TORO 3D Graph */ @@ -143,27 +169,6 @@ struct SfM_data { */ GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data); -/** - * @brief This function parses a g2o file and stores the measurements into a - * NonlinearFactorGraph and the initial guess in a Values structure - * @param filename The name of the g2o file - * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal. - * @return initial Values containing the initial guess (VERTEX_SE2) - */ -GTSAM_EXPORT bool readG2o(const std::string& g2oFile, - NonlinearFactorGraph& graph, Values& initial, - KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC); - -/** - * @brief This function writes a g2o file from - * NonlinearFactorGraph and a Values structure - * @param filename The name of the g2o file to write - * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2) - * @return estimate Values containing the values (VERTEX_SE2) - */ -GTSAM_EXPORT bool writeG2o(const std::string& filename, - const NonlinearFactorGraph& graph, const Values& estimate); - /** * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a * SfM_data structure diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 8789c4085..d7adf9b51 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -81,9 +81,9 @@ TEST( dataSet, Balbianello) TEST( dataSet, readG2o) { const string g2oFile = findExampleDataFile("pose2example"); - NonlinearFactorGraph actualGraph; - Values actualValues; - readG2o(g2oFile, actualGraph, actualValues); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile); Values expectedValues; expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000)); @@ -97,7 +97,7 @@ TEST( dataSet, readG2o) expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391)); expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016)); expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934)); - EXPECT(assert_equal(expectedValues,actualValues,1e-5)); + EXPECT(assert_equal(expectedValues,*actualValues,1e-5)); noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); NonlinearFactorGraph expectedGraph; @@ -113,16 +113,16 @@ TEST( dataSet, readG2o) expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); - EXPECT(assert_equal(actualGraph,expectedGraph,1e-5)); + EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } /* ************************************************************************* */ TEST( dataSet, readG2oHuber) { const string g2oFile = findExampleDataFile("pose2example"); - NonlinearFactorGraph actualGraph; - Values actualValues; - readG2o(g2oFile, actualGraph, actualValues, KernelFunctionTypeHUBER); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeHUBER); noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); @@ -140,16 +140,16 @@ TEST( dataSet, readG2oHuber) expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); - EXPECT(assert_equal(actualGraph,expectedGraph,1e-5)); + EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } /* ************************************************************************* */ TEST( dataSet, readG2oTukey) { const string g2oFile = findExampleDataFile("pose2example"); - NonlinearFactorGraph actualGraph; - Values actualValues; - readG2o(g2oFile, actualGraph, actualValues, KernelFunctionTypeTUKEY); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeTUKEY); noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); @@ -167,25 +167,25 @@ TEST( dataSet, readG2oTukey) expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); - EXPECT(assert_equal(actualGraph,expectedGraph,1e-5)); + EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } /* ************************************************************************* */ TEST( dataSet, writeG2o) { const string g2oFile = findExampleDataFile("pose2example"); - NonlinearFactorGraph expectedGraph; - Values expectedValues; - readG2o(g2oFile, expectedGraph, expectedValues); + NonlinearFactorGraph::shared_ptr expectedGraph; + Values::shared_ptr expectedValues; + boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile); const string filenameToWrite = createRewrittenFileName(g2oFile); - writeG2o(filenameToWrite, expectedGraph, expectedValues); + writeG2o(*expectedGraph, *expectedValues, filenameToWrite); - NonlinearFactorGraph actualGraph; - Values actualValues; - readG2o(filenameToWrite, actualGraph, actualValues); - EXPECT(assert_equal(expectedValues,actualValues,1e-5)); - EXPECT(assert_equal(actualGraph,expectedGraph,1e-5)); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite); + EXPECT(assert_equal(*expectedValues,*actualValues,1e-5)); + EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5)); } /* ************************************************************************* */ From c6de457a4b9e96072e602ae4bcfcea08a11e098d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 1 Jun 2014 12:25:23 -0400 Subject: [PATCH 55/60] Moved LAGO to slam --- examples/Pose2SLAMExample_lago.cpp | 2 +- gtsam/{nonlinear => slam}/lago.cpp | 2 +- gtsam/{nonlinear => slam}/lago.h | 0 gtsam/{nonlinear => slam}/tests/testLago.cpp | 2 +- gtsam/{nonlinear => slam}/tests/timeLago.cpp | 2 +- 5 files changed, 4 insertions(+), 4 deletions(-) rename gtsam/{nonlinear => slam}/lago.cpp (99%) rename gtsam/{nonlinear => slam}/lago.h (100%) rename gtsam/{nonlinear => slam}/tests/testLago.cpp (99%) rename gtsam/{nonlinear => slam}/tests/timeLago.cpp (98%) diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index cf82a05ca..1f5d80d7b 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -19,7 +19,7 @@ * @author Luca Carlone */ -#include +#include #include #include #include diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/slam/lago.cpp similarity index 99% rename from gtsam/nonlinear/lago.cpp rename to gtsam/slam/lago.cpp index 0f69f6ef0..abb72021a 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -16,7 +16,7 @@ * @date May 14, 2014 */ -#include +#include #include #include #include diff --git a/gtsam/nonlinear/lago.h b/gtsam/slam/lago.h similarity index 100% rename from gtsam/nonlinear/lago.h rename to gtsam/slam/lago.h diff --git a/gtsam/nonlinear/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp similarity index 99% rename from gtsam/nonlinear/tests/testLago.cpp rename to gtsam/slam/tests/testLago.cpp index 6a94588b4..2a143e9a5 100644 --- a/gtsam/nonlinear/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -19,7 +19,7 @@ * @date May 14, 2014 */ -#include +#include #include #include #include diff --git a/gtsam/nonlinear/tests/timeLago.cpp b/gtsam/slam/tests/timeLago.cpp similarity index 98% rename from gtsam/nonlinear/tests/timeLago.cpp rename to gtsam/slam/tests/timeLago.cpp index d3c86d0a4..ff60c4a27 100644 --- a/gtsam/nonlinear/tests/timeLago.cpp +++ b/gtsam/slam/tests/timeLago.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include From c56b56a178417649f6508a8816aef9248a94c335 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Sun, 1 Jun 2014 13:00:03 -0400 Subject: [PATCH 56/60] bizarre fix for issue 101 --- gtsam/navigation/MagFactor.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 79677c0c6..c2ccb31ff 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -54,8 +54,11 @@ public: static Point3 unrotate(const Rot2& R, const Point3& p, boost::optional HR = boost::none) { Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR); - if (HR) - *HR = HR->col(2); + if (HR) { + // assign to temporary first to avoid error in Win-Debug mode + Matrix H = HR->col(2); + *HR = H; + } return q; } From 07772b011a387f54dcb7128a49ef80aec256a92a Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sun, 1 Jun 2014 13:04:47 -0400 Subject: [PATCH 57/60] fix formatting --- gtsam/navigation/MagFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index c2ccb31ff..44f543bc9 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -56,9 +56,9 @@ public: Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR); if (HR) { // assign to temporary first to avoid error in Win-Debug mode - Matrix H = HR->col(2); + Matrix H = HR->col(2); *HR = H; - } + } return q; } From e66cd56bf52fbcae0cda2e52b3e8e6840ee44e95 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Sun, 1 Jun 2014 13:49:19 -0400 Subject: [PATCH 58/60] Windows fixes. Made global test variables static --- gtsam/slam/tests/testLago.cpp | 10 +++++----- gtsam/slam/tests/testSmartProjectionPoseFactor.cpp | 6 +++--- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 2a143e9a5..5e4b2faf2 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -33,7 +33,7 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3); +static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3); static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); namespace simple { @@ -48,10 +48,10 @@ namespace simple { // x0 // -Pose2 pose0 = Pose2(0.000000, 0.000000, 0.000000); -Pose2 pose1 = Pose2(1.000000, 1.000000, 1.570796); -Pose2 pose2 = Pose2(0.000000, 2.000000, 3.141593); -Pose2 pose3 = Pose2(-1.000000, 1.000000, 4.712389); +static Pose2 pose0 = Pose2(0.000000, 0.000000, 0.000000); +static Pose2 pose1 = Pose2(1.000000, 1.000000, 1.570796); +static Pose2 pose2 = Pose2(0.000000, 2.000000, 3.141593); +static Pose2 pose3 = Pose2(-1.000000, 1.000000, 4.712389); NonlinearFactorGraph graph() { NonlinearFactorGraph g; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index ee8db1267..29b482861 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -52,9 +52,9 @@ using symbol_shorthand::X; using symbol_shorthand::L; // tests data -Symbol x1('X', 1); -Symbol x2('X', 2); -Symbol x3('X', 3); +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); static Key poseKey1(x1); static Point2 measurement1(323.0, 240.0); From ba67b1ce129326975769cdf4c6d324fcd2f14d8f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 1 Jun 2014 14:06:09 -0400 Subject: [PATCH 59/60] Possible fix for Clang 5.1 issue, for Chris to try on other platforms --- gtsam/3rdparty/CCOLAMD/Source/ccolamd.c | 2 +- gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c index 36ffa1ada..49a4d8f60 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c @@ -617,11 +617,11 @@ #include "ccolamd.h" -#include #include #include #ifdef MATLAB_MEX_FILE +typedef uint16_t char16_t; #include "mex.h" #include "matrix.h" #endif diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c index d43985126..10c9fd5be 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c @@ -13,6 +13,8 @@ #ifndef NPRINT #ifdef MATLAB_MEX_FILE +#include +typedef uint16_t char16_t; #include "mex.h" int (*ccolamd_printf) (const char *, ...) = mexPrintf ; #else From e60f21a22f5c149780184ccb21f55f64dbfb1a7c Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sun, 1 Jun 2014 16:02:24 -0400 Subject: [PATCH 60/60] Missed before: Frank's typedef fix actually needs another include on Linux --- gtsam/3rdparty/CCOLAMD/Source/ccolamd.c | 1 + gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c | 1 + 2 files changed, 2 insertions(+) diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c index 49a4d8f60..1c35ffa41 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c @@ -621,6 +621,7 @@ #include #ifdef MATLAB_MEX_FILE +#include typedef uint16_t char16_t; #include "mex.h" #include "matrix.h" diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c index 10c9fd5be..e470804a6 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c @@ -14,6 +14,7 @@ #ifndef NPRINT #ifdef MATLAB_MEX_FILE #include +#include typedef uint16_t char16_t; #include "mex.h" int (*ccolamd_printf) (const char *, ...) = mexPrintf ;