Merge remote-tracking branch 'origin/develop' into feature/BAD_custom_chart

Conflicts:
	gtsam_unstable/nonlinear/ExpressionFactor.h

CMakeLists.txt

@@ -59,7 +59,7 @@ option(GTSAM_BUILD_STATIC_LIBRARY        "Build a static gtsam library, instead
 option(GTSAM_USE_QUATERNIONS             "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
 option(GTSAM_POSE3_EXPMAP 			 	 "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF)
 option(GTSAM_ROT3_EXPMAP 			 	 "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
-option(GTSAM_ENABLE_CONSISTENCY_CHECKS   "Enable/Disable expensive consistency checks"       OFF) 
+option(GTSAM_ENABLE_CONSISTENCY_CHECKS   "Enable/Disable expensive consistency checks"       OFF)
 option(GTSAM_WITH_TBB                    "Use Intel Threaded Building Blocks (TBB) if available" ON)
 option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available" ON)
 option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON)
@@ -203,13 +203,13 @@ set(GTSAM_USE_SYSTEM_EIGEN OFF)
 if(GTSAM_USE_SYSTEM_EIGEN)
 	# Use generic Eigen include paths e.g. <Eigen/Core>
 	set(GTSAM_EIGEN_INCLUDE_PREFIX "")
-	
+
 	find_package(Eigen3 REQUIRED)
 	include_directories(AFTER "${EIGEN3_INCLUDE_DIR}")
 else()
 	# Use bundled Eigen include paths e.g. <gtsam/3rdparty/Eigen/Eigen/Core>
 	set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/")
-	
+
 	# Clear any variables set by FindEigen3
 	if(EIGEN3_INCLUDE_DIR)
 		set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
@@ -222,7 +222,6 @@ configure_file(gtsam/3rdparty/gtsam_eigen_includes.h.in gtsam/3rdparty/gtsam_eig
 # Install the configuration file for Eigen
 install(FILES ${PROJECT_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINATION include/gtsam/3rdparty)
 
-
 ###############################################################################
 # Global compile options
 
@@ -269,18 +268,18 @@ include_directories(BEFORE ${Boost_INCLUDE_DIR})
 # paths so that the compiler uses GTSAM headers in our source directory instead
 # of any previously installed GTSAM headers.
 include_directories(BEFORE
-  gtsam/3rdparty/UFconfig 
+  gtsam/3rdparty/UFconfig
   gtsam/3rdparty/CCOLAMD/Include
-  gtsam/3rdparty/metis-5.1.0/include
-  gtsam/3rdparty/metis-5.1.0/libmetis
-  gtsam/3rdparty/metis-5.1.0/GKlib
+  gtsam/3rdparty/metis/include
+  gtsam/3rdparty/metis/libmetis
+  gtsam/3rdparty/metis/GKlib
   ${PROJECT_SOURCE_DIR}
   ${PROJECT_BINARY_DIR} # So we can include generated config header files
   CppUnitLite)
 
 if(MSVC)
 	add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS)
-	add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344) # Disable non-DLL-exported base class and other warnings
+	add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
 endif()
 
 # GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
@@ -331,6 +330,7 @@ endif(GTSAM_BUILD_UNSTABLE)
 GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in")
 export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake)
 
+
 # Check for doxygen availability - optional dependency
 find_package(Doxygen)
 

examples/METISOrderingExample.cpp

@@ -0,0 +1,64 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 METISOrdering.cpp
+ * @brief Simple robot motion example, with prior and two odometry measurements, using a METIS ordering
+ * @author Frank Dellaert
+ * @author Andrew Melim
+ */
+
+/**
+ * Example of a simple 2D localization example optimized using METIS ordering
+ * - For more details on the full optimization pipeline, see OdometryExample.cpp
+ */
+
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/Values.h>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char** argv) {
+  NonlinearFactorGraph graph;
+
+  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
+  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
+
+  Pose2 odometry(2.0, 0.0, 0.0);
+  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
+  graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
+  graph.print("\nFactor Graph:\n"); // print
+
+  Values initial;
+  initial.insert(1, Pose2(0.5, 0.0, 0.2));
+  initial.insert(2, Pose2(2.3, 0.1, -0.2));
+  initial.insert(3, Pose2(4.1, 0.1, 0.1));
+  initial.print("\nInitial Estimate:\n"); // print
+
+  // optimize using Levenberg-Marquardt optimization
+  LevenbergMarquardtParams params;
+  // In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType"
+  // By default this parameter is set to OrderingType::COLAMD
+  params.orderingType = Ordering::METIS;
+  LevenbergMarquardtOptimizer optimizer(graph, initial, params);
+  Values result = optimizer.optimize();
+  result.print("Final Result:\n");
+
+  return 0;
+}

examples/SFMExample_SmartFactor.cpp

@@ -61,8 +61,7 @@ using namespace std;
 using namespace gtsam;
 
 // Make the typename short so it looks much cleaner
-typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3,
-    gtsam::Cal3_S2> SmartFactor;
+typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartFactor;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {

examples/SolverComparer.cpp

@@ -589,7 +589,7 @@ void runStats()
 {
   cout << "Gathering statistics..." << endl;
   GaussianFactorGraph linear = *datasetMeasurements.linearize(initial);
-  GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::COLAMD(linear)));
+  GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::colamd(linear)));
   treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt);
 
   ofstream file;

examples/StereoVOExample_large.cpp

@@ -103,7 +103,9 @@ int main(int argc, char** argv){
 
   cout << "Optimizing" << endl;
   //create Levenberg-Marquardt optimizer to optimize the factor graph
-  LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
+  LevenbergMarquardtParams params;
+  params.orderingType = Ordering::METIS;
+  LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params);
   Values result = optimizer.optimize();
 
   cout << "Final result sample:" << endl;

gtsam.h

@@ -2272,7 +2272,7 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
 };
 
 #include <gtsam/slam/SmartProjectionPoseFactor.h>
-template<POSE, LANDMARK, CALIBRATION>
+template<POSE, CALIBRATION>
 virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
 
   SmartProjectionPoseFactor(double rankTol, double linThreshold,
@@ -2288,7 +2288,7 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
   //void serialize() const;
 };
 
-typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
+typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
 
 
 #include <gtsam/slam/StereoFactor.h>
@@ -2438,8 +2438,8 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
 #include <gtsam/navigation/AHRSFactor.h>
 class AHRSFactorPreintegratedMeasurements {
   // Standard Constructor
-  AHRSFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredOmegaCovariance);
-  AHRSFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredOmegaCovariance);
+  AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
+  AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
   AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs);
 
   // Testable
@@ -2468,8 +2468,8 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
   // Standard Interface
   gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const;
   Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
-        const gtsam::imuBias::ConstantBias& bias) const;
-  gtsam::Rot3 predict(const gtsam::Rot3& rot_i, const gtsam::imuBias::ConstantBias& bias,
+      Vector bias) const;
+  gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias,
       const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements,
       Vector omegaCoriolis) const;
 };

gtsam/3rdparty/CMakeLists.txt

@@ -18,7 +18,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 
 	# do the same for the unsupported eigen folder
 	file(GLOB_RECURSE  unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h")
-	
+
 	file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*")
 	foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all})
 		get_filename_component(filename ${unsupported_eigen_dir} NAME)
@@ -36,17 +36,15 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 	install(DIRECTORY Eigen/Eigen
 		DESTINATION include/gtsam/3rdparty/Eigen
 		FILES_MATCHING PATTERN "*.h")
-	
+
 	install(DIRECTORY Eigen/unsupported/Eigen
 		DESTINATION include/gtsam/3rdparty/Eigen/unsupported/
 		FILES_MATCHING PATTERN "*.h")
 endif()
 
-option(GTSAM_BUILD_METIS "Build metis library" ON)
 option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF)
-if(GTSAM_BUILD_METIS)
-	add_subdirectory(metis-5.1.0)
-endif(GTSAM_BUILD_METIS)
+add_subdirectory(metis)
+
 ############ NOTE:  When updating GeographicLib be sure to disable building their examples
 ############        and unit tests by commenting out their lines:
 # add_subdirectory (examples)
@@ -73,3 +71,5 @@ endif()
 if(GTSAM_INSTALL_GEOGRAPHICLIB)
   add_subdirectory(GeographicLib)
 endif()
+
+set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)

gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt

@@ -1,16 +0,0 @@
-# Add this directory for internal users.
-include_directories(.)
-# Find sources.
-file(GLOB metis_sources *.c)
-# Build libmetis.
-add_library(metis ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources})
-if(UNIX)
-  target_link_libraries(metis m)
-endif()
-
-
-install(TARGETS metis
-  LIBRARY DESTINATION include/gtsam/3rdparty/metis/lib
-  RUNTIME DESTINATION include/gtsam/3rdparty/metis/lib
-  ARCHIVE DESTINATION include/gtsam/3rdparty/metis/lib)
-

gtsam/3rdparty/metis/CMakeLists.txt

@@ -46,3 +46,6 @@ add_subdirectory("libmetis")
 if(GTSAM_BUILD_METIS_EXECUTABLES)
   add_subdirectory("programs")
 endif()
+
+set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
+

gtsam/3rdparty/metis/GKlib/ms_stdint.h

@@ -1,7 +1,7 @@
 // ISO C9x  compliant stdint.h for Microsoft Visual Studio
 // Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124 
 // 
-//  Copyright (c) 2006 Alexander Chemeris
+//  Copyright (c) 2006-2013 Alexander Chemeris
 // 
 // Redistribution and use in source and binary forms, with or without
 // modification, are permitted provided that the following conditions are met:
@@ -13,8 +13,9 @@
 //      notice, this list of conditions and the following disclaimer in the
 //      documentation and/or other materials provided with the distribution.
 // 
-//   3. The name of the author may be used to endorse or promote products
-//      derived from this software without specific prior written permission.
+//   3. Neither the name of the product nor the names of its contributors may
+//      be used to endorse or promote products derived from this software
+//      without specific prior written permission.
 // 
 // THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
 // WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
@@ -40,30 +41,59 @@
 #pragma once
 #endif
 
+#if _MSC_VER >= 1600 // [
+#include <stdint.h>
+#else // ] _MSC_VER >= 1600 [
+
 #include <limits.h>
 
-// For Visual Studio 6 in C++ mode wrap <wchar.h> include with 'extern "C++" {}'
+// For Visual Studio 6 in C++ mode and for many Visual Studio versions when
+// compiling for ARM we should wrap <wchar.h> include with 'extern "C++" {}'
 // or compiler give many errors like this:
 //   error C2733: second C linkage of overloaded function 'wmemchr' not allowed
-#if (_MSC_VER < 1300) && defined(__cplusplus)
-   extern "C++" {
-#endif 
-#     include <wchar.h>
-#if (_MSC_VER < 1300) && defined(__cplusplus)
-   }
+#ifdef __cplusplus
+extern "C" {
 #endif
+#  include <wchar.h>
+#ifdef __cplusplus
+}
+#endif
+
+// Define _W64 macros to mark types changing their size, like intptr_t.
+#ifndef _W64
+#  if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300
+#     define _W64 __w64
+#  else
+#     define _W64
+#  endif
+#endif
+
 
 // 7.18.1 Integer types
 
 // 7.18.1.1 Exact-width integer types
-typedef __int8            int8_t;
-typedef __int16           int16_t;
-typedef __int32           int32_t;
-typedef __int64           int64_t;
+
+// Visual Studio 6 and Embedded Visual C++ 4 doesn't
+// realize that, e.g. char has the same size as __int8
+// so we give up on __intX for them.
+#if (_MSC_VER < 1300)
+typedef signed char       int8_t;
+typedef signed short      int16_t;
+typedef signed int        int32_t;
+typedef unsigned char     uint8_t;
+typedef unsigned short    uint16_t;
+typedef unsigned int      uint32_t;
+#else
+typedef signed __int8     int8_t;
+typedef signed __int16    int16_t;
+typedef signed __int32    int32_t;
 typedef unsigned __int8   uint8_t;
 typedef unsigned __int16  uint16_t;
 typedef unsigned __int32  uint32_t;
-typedef unsigned __int64  uint64_t;
+#endif
+typedef signed __int64       int64_t;
+typedef unsigned __int64     uint64_t;
+
 
 // 7.18.1.2 Minimum-width integer types
 typedef int8_t    int_least8_t;
@@ -87,11 +117,11 @@ typedef uint64_t  uint_fast64_t;
 
 // 7.18.1.4 Integer types capable of holding object pointers
 #ifdef _WIN64 // [
-   typedef __int64           intptr_t;
-   typedef unsigned __int64  uintptr_t;
+typedef signed __int64    intptr_t;
+typedef unsigned __int64  uintptr_t;
 #else // _WIN64 ][
-   typedef int               intptr_t;
-   typedef unsigned int      uintptr_t;
+typedef _W64 signed int   intptr_t;
+typedef _W64 unsigned int uintptr_t;
 #endif // _WIN64 ]
 
 // 7.18.1.5 Greatest-width integer types
@@ -213,10 +243,17 @@ typedef uint64_t  uintmax_t;
 #define UINT64_C(val) val##ui64
 
 // 7.18.4.2 Macros for greatest-width integer constants
-#define INTMAX_C   INT64_C
-#define UINTMAX_C  UINT64_C
+// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
+// Check out Issue 9 for the details.
+#ifndef INTMAX_C //   [
+#  define INTMAX_C   INT64_C
+#endif // INTMAX_C    ]
+#ifndef UINTMAX_C //  [
+#  define UINTMAX_C  UINT64_C
+#endif // UINTMAX_C   ]
 
 #endif // __STDC_CONSTANT_MACROS ]
 
+#endif // _MSC_VER >= 1600 ]
 
-#endif // _MSC_STDINT_H_ ]
+#endif // _MSC_STDINT_H_ ]

gtsam/3rdparty/metis/include/metis.h

@@ -65,17 +65,29 @@
 #ifndef _GKLIB_H_
 #ifdef COMPILER_MSC
 #include <limits.h>
-
 typedef __int32 int32_t;
 typedef __int64 int64_t;
 #define PRId32       "I32d"
 #define PRId64       "I64d"
 #define SCNd32       "ld"
 #define SCNd64       "I64d"
+
+#ifndef INT32_MIN
 #define INT32_MIN    ((int32_t)_I32_MIN)
+#endif
+
+#ifndef INT32_MAX
 #define INT32_MAX    _I32_MAX
+#endif
+
+#ifndef INT64_MIN
 #define INT64_MIN    ((int64_t)_I64_MIN)
+#endif
+
+#ifndef INT64_MAX
 #define INT64_MAX    _I64_MAX
+#endif
+
 #else
 #include <inttypes.h>
 #endif

gtsam/3rdparty/metis/libmetis/CMakeLists.txt

@@ -0,0 +1,19 @@
+# Add this directory for internal users.
+include_directories(.)
+# Find sources.
+file(GLOB metis_sources *.c)
+# Build libmetis.
+add_library(metis ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources})
+if(UNIX)
+  target_link_libraries(metis m)
+endif()
+
+if(WIN32)
+		set_target_properties(metis PROPERTIES
+			PREFIX ""
+			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin")
+endif()
+
+install(TARGETS metis EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
+list(APPEND GTSAM_EXPORTED_TARGETS metis)
+set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)