Merge branch 'develop' of git@bitbucket.org:gtborg/gtsam.git into develop

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()

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}")

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];
 

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

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];
 

examples/RangeISAMExample_plaza2.cpp

@@ -40,6 +40,7 @@
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/RangeFactor.h>
+#include <gtsam/slam/dataset.h>
 
 // Standard headers, added last, so we know headers above work on their own
 #include <boost/foreach.hpp>
@@ -59,9 +60,8 @@ namespace NM = gtsam::noiseModel;
 typedef pair<double, Pose2> TimedOdometry;
 list<TimedOdometry> readOdometry() {
   list<TimedOdometry> odometryList;
-  ifstream is("../../examples/Data/Plaza2_DR.txt");
+  string data_file = findExampleDataFile("Plaza2_DR.txt");
-  if (!is)
+  ifstream is(data_file.c_str());
-    throw runtime_error("../../examples/Data/Plaza2_DR.txt file not found");
 
   while (is) {
     double t, distance_traveled, delta_heading;
@@ -78,9 +78,8 @@ list<TimedOdometry> readOdometry() {
 typedef boost::tuple<double, size_t, double> RangeTriple;
 vector<RangeTriple> readTriples() {
   vector<RangeTriple> triples;
-  ifstream is("../../examples/Data/Plaza2_TD.txt");
+  string data_file = findExampleDataFile("Plaza2_TD.txt");
-  if (!is)
+  ifstream is(data_file.c_str());
-    throw runtime_error("../../examples/Data/Plaza2_TD.txt file not found");
 
   while (is) {
     double t, sender, receiver, range;

examples/StereoVOExample.cpp

@@ -24,47 +24,48 @@
  */
  
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/inference/Key.h>
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/nonlinear/Marginals.h>
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/slam/StereoFactor.h>
-#include <gtsam/nonlinear/NonlinearEquality.h>
-#include <gtsam/inference/Symbol.h>
-
-
 
 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();
+  Pose3 first_pose;
-  graph.push_back(NonlinearEquality<Pose3>(1, first_pose));
+  graph.push_back(NonlinearEquality<Pose3>(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<Pose3,Point3>(StereoPoint2(520, 480, 440), model, 1, 3, K));
   graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(120, 80, 440), model, 1, 4, K));
   graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 280, 140), model, 1, 5, K));
+
   //create and add stereo factors between second pose and the three landmarks
   graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(570, 520, 490), model, 2, 3, K));
   graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(70, 20, 490), model, 2, 4, K));
   graph.push_back(GenericStereoFactor<Pose3,Point3>(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();

gtsam/geometry/tests/testUnit3.cpp

@@ -26,7 +26,7 @@
 #include <boost/bind.hpp>
 #include <boost/foreach.hpp>
 #include <boost/random.hpp>
-#include <boost/thread.hpp>
+//#include <boost/thread.hpp>
 #include <boost/assign/std/vector.hpp>
 #include <cmath>
 
@@ -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);

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<Pose3>(x3).print("Smart: Pose3 after optimization: ");
-  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
+  EXPECT(assert_equal(pose3,result.at<Pose3>(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<Pose3>(x3).print("Smart: Pose3 after optimization: ");
-  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
+  EXPECT(assert_equal(pose3,result.at<Pose3>(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<Pose3>(x3)));
+  EXPECT(assert_equal(pose3,result.at<Pose3>(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<Pose3>(x3)));
+  EXPECT(assert_equal(pose3,result.at<Pose3>(x3), 1e-8));
 }
 
 /* *************************************************************************/
@@ -697,7 +697,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){
   Values result = optimizer.optimize();
 
   if(isDebugTest)  result.at<Pose3>(x3).print("Pose3 after optimization: ");
-  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
+  EXPECT(assert_equal(pose3,result.at<Pose3>(x3), 1e-7));
 }
 
 /* *************************************************************************/

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)
-    add_subdirectory(examples)
+
-endif(GTSAM_BUILD_EXAMPLES)

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<Pose3>(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;