diff --git a/CMakeLists.txt b/CMakeLists.txt index 30699dd01..da8c4e81a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,7 +34,7 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") endif() -# See whether gtsam_unstable is available (it will be present only if we're using an SVN checkout) +# See whether gtsam_unstable is available (it will be present only if we're using a git checkout) if(EXISTS "${PROJECT_SOURCE_DIR}/gtsam_unstable" AND IS_DIRECTORY "${PROJECT_SOURCE_DIR}/gtsam_unstable") set(GTSAM_UNSTABLE_AVAILABLE 1) else() @@ -94,7 +94,7 @@ if(MSVC) # Disable autolinking if(NOT Boost_USE_STATIC_LIBS) add_definitions(-DBOOST_ALL_NO_LIB) - add_definitions(-DBOOST_ALL_DYN_LINK) + add_definitions(-DBOOST_ALL_DYN_LINK) endif() endif() diff --git a/cmake/GTSAMCMakeToolsConfig.cmake b/cmake/GTSAMCMakeToolsConfig.cmake index c79a2f5c2..126bcd2d9 100644 --- a/cmake/GTSAMCMakeToolsConfig.cmake +++ b/cmake/GTSAMCMakeToolsConfig.cmake @@ -1,3 +1,4 @@ # This config file modifies CMAKE_MODULE_PATH so that the GTSAM-CMakeTools files may be included - + +set(GTSAM_CMAKE_TOOLS_DIR "${CMAKE_CURRENT_LIST_DIR}") list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}") diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 3a4ee9cff..b46a53198 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -31,7 +31,7 @@ int main(const int argc, const char *argv[]) { // Read graph from file string g2oFile; if (argc < 2) - g2oFile = "../../examples/Data/noisyToyGraph.txt"; + g2oFile = findExampleDataFile("noisyToyGraph.txt"); else g2oFile = argv[1]; diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 449144fef..86be74c75 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -32,7 +32,8 @@ int main (int argc, char** argv) { NonlinearFactorGraph::shared_ptr graph; Values::shared_ptr initial; SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0)); - boost::tie(graph, initial) = load2D("../../examples/Data/w100.graph", model); + string graph_file = findExampleDataFile("w100.graph"); + boost::tie(graph, initial) = load2D(graph_file, model); initial->print("Initial estimate:\n"); // Add a Gaussian prior on first poses diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index 1f5d80d7b..bb434f3ce 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -32,7 +32,7 @@ int main(const int argc, const char *argv[]) { // Read graph from file string g2oFile; if (argc < 2) - g2oFile = "../../examples/Data/noisyToyGraph.txt"; + g2oFile = findExampleDataFile("noisyToyGraph.txt"); else g2oFile = argv[1]; diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 29b02a271..252372f39 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -40,6 +40,7 @@ #include #include #include +#include // Standard headers, added last, so we know headers above work on their own #include @@ -59,9 +60,8 @@ namespace NM = gtsam::noiseModel; typedef pair TimedOdometry; list readOdometry() { list odometryList; - ifstream is("../../examples/Data/Plaza2_DR.txt"); - if (!is) - throw runtime_error("../../examples/Data/Plaza2_DR.txt file not found"); + string data_file = findExampleDataFile("Plaza2_DR.txt"); + ifstream is(data_file.c_str()); while (is) { double t, distance_traveled, delta_heading; @@ -78,9 +78,8 @@ list readOdometry() { typedef boost::tuple RangeTriple; vector readTriples() { vector triples; - ifstream is("../../examples/Data/Plaza2_TD.txt"); - if (!is) - throw runtime_error("../../examples/Data/Plaza2_TD.txt file not found"); + string data_file = findExampleDataFile("Plaza2_TD.txt"); + ifstream is(data_file.c_str()); while (is) { double t, sender, receiver, range; diff --git a/examples/StereoVOExample.cpp b/examples/StereoVOExample.cpp index 8a04d13a3..19725798c 100644 --- a/examples/StereoVOExample.cpp +++ b/examples/StereoVOExample.cpp @@ -24,47 +24,48 @@ */ #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){ + //create graph object, add first pose at origin with key '1' NonlinearFactorGraph graph; - Pose3 first_pose = Pose3(); - graph.push_back(NonlinearEquality(1, first_pose)); + Pose3 first_pose; + graph.push_back(NonlinearEquality(1, Pose3())); //create factor noise model with 3 sigmas of value 1 const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); //create stereo camera calibration object with .2m between cameras const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)); + //create and add stereo factors between first pose (key value 1) and the three landmarks graph.push_back(GenericStereoFactor(StereoPoint2(520, 480, 440), model, 1, 3, K)); graph.push_back(GenericStereoFactor(StereoPoint2(120, 80, 440), model, 1, 4, K)); graph.push_back(GenericStereoFactor(StereoPoint2(320, 280, 140), model, 1, 5, K)); + //create and add stereo factors between second pose and the three landmarks graph.push_back(GenericStereoFactor(StereoPoint2(570, 520, 490), model, 2, 3, K)); graph.push_back(GenericStereoFactor(StereoPoint2(70, 20, 490), model, 2, 4, K)); graph.push_back(GenericStereoFactor(StereoPoint2(320, 270, 115), model, 2, 5, K)); + //create Values object to contain initial estimates of camera poses and landmark locations - Values initial_estimate = Values(); + Values initial_estimate; + //create and add iniital estimates initial_estimate.insert(1, first_pose); initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1))); initial_estimate.insert(3, Point3(1, 1, 5)); initial_estimate.insert(4, Point3(-1, 1, 5)); initial_estimate.insert(5, Point3(0, -0.5, 5)); + //create Levenberg-Marquardt optimizer for resulting factor graph, optimize LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate); Values result = optimizer.optimize(); @@ -72,4 +73,4 @@ int main(int argc, char** argv){ result.print("Final result:\n"); return 0; -} \ No newline at end of file +} diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index eb45ea60f..f8b3e7fa4 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -26,7 +26,7 @@ #include #include #include -#include +//#include #include #include @@ -234,7 +234,7 @@ TEST(Unit3, localCoordinates_retract) { for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). - boost::this_thread::sleep(boost::posix_time::milliseconds(0)); +// boost::this_thread::sleep(boost::posix_time::milliseconds(0)); // Create the two Unit3s. // NOTE: You can not create two totally random Unit3's because you cannot always compute @@ -264,10 +264,10 @@ TEST(Unit3, localCoordinates_retract_expmap) { for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). - boost::this_thread::sleep(boost::posix_time::milliseconds(0)); +// boost::this_thread::sleep(boost::posix_time::milliseconds(0)); // Create the two Unit3s. - // Unlike the above case, we can use any two sphers. + // Unlike the above case, we can use any two Unit3's. Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); // Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); Vector v12 = randomVector(minXiLimit, maxXiLimit); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 29b482861..6b849ba5a 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -288,7 +288,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ // result.print("results of 3 camera, 3 landmark optimization \n"); if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3,result.at(x3),1e-8)); if(isDebugTest) tictoc_print_(); } @@ -365,7 +365,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ // result.print("results of 3 camera, 3 landmark optimization \n"); if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3,result.at(x3), 1e-7)); if(isDebugTest) tictoc_print_(); } @@ -428,7 +428,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){ Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); } /* *************************************************************************/ @@ -631,7 +631,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ){ Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); } /* *************************************************************************/ @@ -697,7 +697,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ Values result = optimizer.optimize(); if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3,result.at(x3), 1e-7)); } /* *************************************************************************/ diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index a33a4548c..628a9058b 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -28,7 +28,7 @@ set (excluded_headers # "") "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h" ) -# assemble core libaries +# assemble core libraries foreach(subdir ${gtsam_unstable_subdirs}) # Build convenience libraries file(GLOB subdir_srcs "${subdir}/*.cpp") @@ -63,7 +63,7 @@ set(gtsam_unstable_soversion ${GTSAM_VERSION_MAJOR}) message(STATUS "GTSAM_UNSTABLE Version: ${gtsam_unstable_version}") message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") -# build shared and static versions of the library +# build shared or static versions of the library if (GTSAM_BUILD_STATIC_LIBRARY) message(STATUS "Building GTSAM_UNSTABLE - static") add_library(gtsam_unstable STATIC ${gtsam_unstable_srcs}) @@ -116,8 +116,6 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX) wrap_and_install_library(gtsam_unstable.h "gtsam" "" "${mexFlags}") endif(GTSAM_INSTALL_MATLAB_TOOLBOX) - # Build examples -if (GTSAM_BUILD_EXAMPLES) - add_subdirectory(examples) -endif(GTSAM_BUILD_EXAMPLES) +add_subdirectory(examples) + diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index 40a0a8725..01c2ab3e1 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -108,9 +108,13 @@ int main(int argc, char** argv){ // QR is much slower than Cholesky, but numerically more stable graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + LevenbergMarquardtParams params; + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + params.verbosity = NonlinearOptimizerParams::ERROR; + cout << "Optimizing" << endl; //create Levenberg-Marquardt optimizer to optimize the factor graph - LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate); + LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params); Values result = optimizer.optimize(); cout << "Final result sample:" << endl;