diff --git a/.cproject b/.cproject index af6b32cd2..d5a6ca4d4 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 @@ -2217,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 @@ -2547,6 +2451,7 @@ make + testGraph.run true false @@ -2554,6 +2459,7 @@ make + testJunctionTree.run true false @@ -2561,6 +2467,7 @@ make + testSymbolicBayesNetB.run true false @@ -2678,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 @@ -2830,6 +2801,70 @@ 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 + -j5 + timeLago.run + true + true + true + make -j4 @@ -2848,7 +2883,6 @@ make - tests/testGaussianISAM2 true false diff --git a/.gitignore b/.gitignore index 6d274af09..cf23a5147 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,6 @@ /build* /doc* *.pyc -*.DS_Store \ No newline at end of file +*.DS_Store +/examples/Data/dubrovnik-3-7-pre-rewritten.txt +/examples/Data/pose2example-rewritten.txt \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 196b7e4df..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}") @@ -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() 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 - diff --git a/examples/Data/pose2example-rewritten.txt b/examples/Data/pose2example-rewritten.txt deleted file mode 100644 index 6c8fe38a2..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 diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 393bba86d..3a4ee9cff 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -18,46 +18,45 @@ * @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; - 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; - noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + 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; - 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(*graph, result, outputFile); + std::cout << "done! " << std::endl; + } return 0; } diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index d61f1b533..1f5d80d7b 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -12,51 +12,53 @@ /** * @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; - 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; - noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + 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)); + graphWithPrior.print(); std::cout << "Computing LAGO estimate" << std::endl; - Values estimateLago = initializeLago(graphWithPrior); + Values estimateLago = lago::initialize(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(*graph, estimateLago, outputFile); + std::cout << "done! " << std::endl; + } return 0; } diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp new file mode 100644 index 000000000..904919d42 --- /dev/null +++ b/examples/SFMExample_SmartFactor.cpp @@ -0,0 +1,168 @@ +/* ---------------------------------------------------------------------------- + + * 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; + + // 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) { + + // 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); + } + + // 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. + 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"); + + + // 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; +} +/* ************************************************************************* */ + diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index c661967c0..b9ab663d9 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -25,47 +25,43 @@ */ #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; - ifstream calibration_file(calibration_loc); + // focal lengths fx, fy, skew s, principal point u0, v0, baseline b + double fx, fy, s, u0, v0, b; + ifstream calibration_file(calibration_loc.c_str()); 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); + ifstream pose_file(pose_loc.c_str()); cout << "Reading camera poses" << endl; int pose_id; MatrixRowMajor m(4,4); @@ -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; - ifstream factor_file(factor_loc); + // 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.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) { - 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 +} 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/3rdparty/CCOLAMD/Source/ccolamd.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c index 36ffa1ada..1c35ffa41 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c @@ -617,11 +617,12 @@ #include "ccolamd.h" -#include #include #include #ifdef MATLAB_MEX_FILE +#include +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..e470804a6 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c @@ -13,6 +13,9 @@ #ifndef NPRINT #ifdef MATLAB_MEX_FILE +#include +#include +typedef uint16_t char16_t; #include "mex.h" int (*ccolamd_printf) (const char *, ...) = mexPrintf ; #else 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/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 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/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); } } 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 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; @@ -74,23 +74,46 @@ static 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::Covariance(const Matrix& covariance, bool smart) { +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(); - 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))); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index e709ea543..0351cfabd 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 + GTSAM_EXPORT 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 ) { diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 79677c0c6..44f543bc9 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; } diff --git a/gtsam/nonlinear/LagoInitializer.h b/gtsam/nonlinear/LagoInitializer.h deleted file mode 100644 index b351365c1..000000000 --- a/gtsam/nonlinear/LagoInitializer.h +++ /dev/null @@ -1,100 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 LagoInitializer.h - * @brief Initialize Pose2 in a factor graph using LAGO - * (Linear Approximation for Graph Optimization). see papers: - * - * L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate - * approximation for planar pose graph optimization, IJRR, 2014. - * - * L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation - * for graph-based simultaneous localization and mapping, RSS, 2011. - * - * @param graph: nonlinear factor graph (can include arbitrary factors but we assume - * that there is a subgraph involving Pose2 and betweenFactors). Also in the current - * version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc) - * and a prior on x0. This assumption can be relaxed by using the extra argument - * useOdometricPath = false, although this part of code is not stable yet. - * @return Values: initial guess from LAGO (only pose2 are initialized) - * - * @author Luca Carlone - * @author Frank Dellaert - * @date May 14, 2014 - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace gtsam { - -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 - * 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); - -/* This function computes the cumulative orientations (without wrapping) - * for all node wrt the root (root has zero orientation) - */ -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] - */ -GTSAM_EXPORT void getSymbolicGraph( - /*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 */ -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); - -/* Selects 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 computeLagoOrientations(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); - -/* Returns the values for the Pose2 in a generic factor graph */ -GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true); - -/* Only corrects the orientation part in initialGuess */ -GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess); - -} // end of namespace gtsam 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_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 99% rename from gtsam_unstable/slam/SmartFactorBase.h rename to gtsam/slam/SmartFactorBase.h index 93931549f..1235fc6fb 100644 --- a/gtsam_unstable/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_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 2124dd6f6..043528fea 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 @@ -54,7 +53,7 @@ public: double f; }; -enum linearizationType { +enum LinearizationMode { HESSIAN, JACOBIAN_SVD, JACOBIAN_Q }; @@ -263,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/gtsam_unstable/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h similarity index 81% rename from gtsam_unstable/slam/SmartProjectionPoseFactor.h rename to gtsam/slam/SmartProjectionPoseFactor.h index 66497d28d..273102bda 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactor.h +++ b/gtsam/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_; } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index f381a0786..b3c9b9557 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; @@ -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,35 +55,122 @@ 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 +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 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"); + + return newpath.string(); +} +/* ************************************************************************* */ + #endif /* ************************************************************************* */ -pair load2D( - pair > dataset, - int maxID, bool addNoise, bool smart) { - return load2D(dataset.first, dataset.second, maxID, addNoise, smart); +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); } /* ************************************************************************* */ -pair load2D( - const string& filename, boost::optional model, int maxID, - bool addNoise, bool smart) { - cout << "Will try to read " << filename << endl; +// 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; + + // 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 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: + 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 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 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: + case NoiseFormatTORO: + // In both cases, what is stored in file is the information matrix + model = noiseModel::Gaussian::Information(M, smart); + break; + case NoiseFormatGRAPH: + case NoiseFormatCOV: + // These cases expect covariance matrix + model = noiseModel::Gaussian::Covariance(M, smart); + break; + default: + throw invalid_argument("load2D: invalid noise format"); + } + + switch (kernelFunctionType) { + case KernelFunctionTypeNONE: + 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 invalid_argument("load2D: invalid kernel function type"); + } +} + +/* ************************************************************************* */ +GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID, + bool addNoise, bool smart, NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { + ifstream is(filename.c_str()); if (!is) - throw std::invalid_argument("load2D: can not find the file!"); + throw invalid_argument("load2D: can not find file " + filename); Values::shared_ptr initial(new Values); NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); @@ -92,16 +179,18 @@ pair load2D( // load the poses while (is) { - if(! (is >> tag)) + 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; + // optional filter if (maxID && id >= maxID) continue; + initial->insert(id, Pose2(x, y, yaw)); } is.ignore(LINESIZE, '\n'); @@ -109,54 +198,47 @@ pair load2D( 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); + // If asked, create a sampler with random number generator + 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; bool haveLandmark = false; while (is) { - if(! (is >> tag)) + if (!(is >> tag)) break; - if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { + if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") + || (tag == "ODOMETRY")) { + + // Read transform double x, y, yaw; - double v1, v2, v3, v4, v5, v6; - is >> id1 >> id2 >> x >> y >> yaw; - is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; + Pose2 l1Xl2(x, y, yaw); - // 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"); - } + // 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) { - 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.sampleNewModel(*model)); + l1Xl2 = l1Xl2.retract(sampler.sample()); // Insert vertices if pure odometry file if (!initial->exists(id1)) @@ -165,7 +247,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); } @@ -185,23 +267,22 @@ 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. - 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; } } @@ -227,7 +308,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); } @@ -235,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) { @@ -248,18 +332,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) @@ -267,14 +349,62 @@ 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(); } +/* ************************************************************************* */ +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()); @@ -318,161 +448,60 @@ bool load3D(const string& filename) { } /* ************************************************************************* */ -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 +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; @@ -482,20 +511,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; } @@ -503,38 +527,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); @@ -545,123 +567,10 @@ bool readBundler(const string& filename, SfM_data &data) } /* ************************************************************************* */ -bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, - const kernelFunctionType kernelFunction){ - - 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; - } - 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) -{ +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; } @@ -673,44 +582,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; @@ -721,8 +627,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()); @@ -733,49 +638,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; @@ -787,48 +698,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; @@ -844,7 +762,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; } @@ -852,9 +770,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; } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 98297ac04..8fd75269c 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,8 +44,30 @@ namespace gtsam { * search process described above. */ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); + +/** + * 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 +/// 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 ! + NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33 +}; + +/// Robust kernel type to wrap around quadratic noise model +enum KernelFunctionType { + KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY +}; + +/// Return type for load functions +typedef std::pair GraphAndValues; + /** * Load TORO 2D Graph * @param dataset/model pair as constructed by [dataset] @@ -53,31 +75,57 @@ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); * @param addNoise add noise to the edges * @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); +GTSAM_EXPORT GraphAndValues load2D( + std::pair dataset, int maxID = 0, + bool addNoise = false, + bool smart = true, // + NoiseFormat noiseFormat = NoiseFormatGRAPH, + 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, - boost::optional model = boost::optional< - noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false, - bool smart = true); +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, - gtsam::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); +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 @@ -85,27 +133,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 }; /** @@ -117,25 +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) - */ -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 - * 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 @@ -165,7 +198,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 @@ -208,5 +242,4 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfM_data& db); */ GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db); - } // namespace gtsam diff --git a/gtsam/nonlinear/LagoInitializer.cpp b/gtsam/slam/lago.cpp similarity index 51% rename from gtsam/nonlinear/LagoInitializer.cpp rename to gtsam/slam/lago.cpp index 3451beaf3..abb72021a 100644 --- a/gtsam/nonlinear/LagoInitializer.cpp +++ b/gtsam/slam/lago.cpp @@ -10,38 +10,60 @@ * -------------------------------------------------------------------------- */ /** - * @file LagoInitializer.h + * @file lago.h * @author Luca Carlone * @author Frank Dellaert * @date May 14, 2014 */ -#include -#include +#include +#include +#include +#include +#include +#include + #include -namespace gtsam { +using namespace std; -static Matrix I = eye(1); -static Matrix I3 = eye(3); +namespace gtsam { +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::Sigmas((Vector(1) << 0)); +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 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; } @@ -55,53 +77,55 @@ 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)); + thetaToRootMap.insert(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)); + thetaToRootMap.insert(pair(nodeKey, nodeTheta)); } return thetaToRootMap; } /* ************************************************************************* */ void getSymbolicGraph( - /*OUTPUTS*/ std::vector& spanningTreeIds, std::vector& chordsIds, key2doubleMap& deltaThetaMap, - /*INPUTS*/ const PredecessorMap& tree, const NonlinearFactorGraph& g){ +/*OUTPUTS*/vector& spanningTreeIds, 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 - deltaThetaMap.insert(std::pair(key1, -deltaTheta)); + bool inTree = false; + if (tree.at(key1) == key2) { // key2 -> key1 + deltaThetaMap.insert(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(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++; @@ -109,14 +133,16 @@ 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 boost::shared_ptr > pose2Between = boost::dynamic_pointer_cast >(factor); if (!pose2Between) - throw std::invalid_argument("buildLinearOrientationGraph: invalid between factor!"); + throw invalid_argument( + "buildLinearOrientationGraph: invalid between factor!"); deltaTheta = (Vector(1) << pose2Between->measured().theta()); // Retrieve the noise model for the relative rotation @@ -124,114 +150,127 @@ 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 " + 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 + 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 vector& spanningTreeIds, const 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(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)); - //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)); + ///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) cout << k2pi_noise - 2*k*M_PI << endl; // for debug + Vector deltaThetaRegularized = (Vector(1) + << key1_DeltaTheta_key2 - 2 * k * M_PI); + 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; } /* ************************************************************************* */ -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) { + gttic(lago_buildPose2graph); 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; } /* ************************************************************************* */ -PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { +static PredecessorMap findOdometricPath( + const NonlinearFactorGraph& pose2Graph) { PredecessorMap tree; 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 computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){ +// Return the orientations of a graph including only BetweenFactors +static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, + bool useOdometricPath) { + gttic(lago_computeOrientations); // 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 + 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 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(); @@ -240,70 +279,79 @@ 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) { + gttic(lago_computePoses); // 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(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(); // 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); } } @@ -311,37 +359,41 @@ Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& or } /* ************************************************************************* */ -Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath) { +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 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 ){ - 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); } } return initialGuessLago; } +} // end of namespace lago } // end of namespace gtsam diff --git a/gtsam/slam/lago.h b/gtsam/slam/lago.h new file mode 100644 index 000000000..0df80ac42 --- /dev/null +++ b/gtsam/slam/lago.h @@ -0,0 +1,86 @@ +/* ---------------------------------------------------------------------------- + + * 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 lago.h + * @brief Initialize Pose2 in a factor graph using LAGO + * (Linear Approximation for Graph Optimization). see papers: + * + * L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate + * approximation for planar pose graph optimization, IJRR, 2014. + * + * L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation + * for graph-based simultaneous localization and mapping, RSS, 2011. + * + * @param graph: nonlinear factor graph (can include arbitrary factors but we assume + * that there is a subgraph involving Pose2 and betweenFactors). Also in the current + * version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc) + * and a prior on x0. This assumption can be relaxed by using the extra argument + * useOdometricPath = false, although this part of code is not stable yet. + * @return Values: initial guess from LAGO (only pose2 are initialized) + * + * @author Luca Carlone + * @author Frank Dellaert + * @date May 14, 2014 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { +namespace lago { + +typedef std::map key2doubleMap; + +/** + * 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); + +/** + * 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); + +/** 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); + +/** LAGO: Return the orientations of the Pose2 in a generic factor graph */ +GTSAM_EXPORT VectorValues initializeOrientations( + 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 correct the orientation part in initialGuess */ +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, + const Values& initialGuess); + +} // end of namespace lago +} // end of namespace gtsam diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index add0db29d..d7adf9b51 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -40,18 +40,21 @@ 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); + 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)); +} /* ************************************************************************* */ TEST( dataSet, Balbianello) @@ -78,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)); @@ -94,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; @@ -110,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, HUBER); + 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); @@ -137,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, TUKEY); + 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); @@ -164,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 = findExampleDataFile("pose2example-rewritten"); - writeG2o(filenameToWrite, expectedGraph, expectedValues); + const string filenameToWrite = createRewrittenFileName(g2oFile); + 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)); } /* ************************************************************************* */ @@ -249,7 +252,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 +314,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 diff --git a/gtsam/nonlinear/tests/testLagoInitializer.cpp b/gtsam/slam/tests/testLago.cpp similarity index 68% rename from gtsam/nonlinear/tests/testLagoInitializer.cpp rename to gtsam/slam/tests/testLago.cpp index 64e43ae9b..5e4b2faf2 100644 --- a/gtsam/nonlinear/tests/testLagoInitializer.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -19,27 +19,21 @@ * @date May 14, 2014 */ -#include - +#include +#include +#include +#include #include -#include -#include -#include - -#include -#include - -#include #include -#include + #include 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 { @@ -54,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; @@ -77,10 +71,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) @@ -100,19 +94,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); @@ -125,14 +119,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)); @@ -147,25 +141,25 @@ TEST( Lago, regularizedMeasurements ) { /* *************************************************************************** */ TEST( Lago, smallGraphVectorValues ) { bool useOdometricPath = false; - VectorValues initialGuessLago = initializeOrientationsLago(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), 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 = 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), 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)); } /* *************************************************************************** */ @@ -173,26 +167,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 = 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), 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 = 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), 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)); } /* *************************************************************************** */ @@ -200,26 +194,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 = 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), 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 = 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), 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)); } /* *************************************************************************** */ @@ -233,7 +227,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; @@ -249,7 +243,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; @@ -264,17 +258,17 @@ 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)); - VectorValues actualVV = initializeOrientationsLago(graphWithPrior); + VectorValues actualVV = lago::initializeOrientations(graphWithPrior); Values actual; Key keyAnc = symbol('Z',9999999); for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){ @@ -285,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 = initializeLago(graphWithPrior); + 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_unstable/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp similarity index 99% rename from gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp rename to gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index ee8db1267..29b482861 100644 --- a/gtsam_unstable/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); diff --git a/gtsam/slam/tests/timeLago.cpp b/gtsam/slam/tests/timeLago.cpp new file mode 100644 index 000000000..ff60c4a27 --- /dev/null +++ b/gtsam/slam/tests/timeLago.cpp @@ -0,0 +1,82 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char *argv[]) { + + size_t trials = 1; + + // read graph + 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, 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 = // + 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++) { + { + 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; +} diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp new file mode 100644 index 000000000..40a0a8725 --- /dev/null +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -0,0 +1,121 @@ +/* ---------------------------------------------------------------------------- + +* GTSAM Copyright 2010, Georgia Tech Research Corporation, +* Atlanta, Georgia 30332-0415 +* All Rights Reserved +* Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +* See LICENSE for the license information + +* -------------------------------------------------------------------------- */ + +/** +* @file 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, throwing away the + * measurement from the right camera + * -robot starts at origin + * -moves forward, taking periodic stereo measurements + * -makes monocular observations of many landmarks + */ + +#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 + cout << "Reading calibration info" << endl; + ifstream calibration_file(calibration_loc.c_str()); + + 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)); + + 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 + 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.c_str()); + cout << "Reading stereo factors" << endl; + + //read stereo measurements and construct smart factors + + SmartFactor::shared_ptr factor(new SmartFactor()); + size_t current_l = 3; // hardcoded landmark ID from first measurement + + while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { + + 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); + } + + 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; +} 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; }