diff --git a/.cproject b/.cproject index 97e36d81f..af6b32cd2 100644 --- a/.cproject +++ b/.cproject @@ -1,17 +1,19 @@ - + + + + + - - @@ -60,13 +62,13 @@ + + - - @@ -116,13 +118,13 @@ + + - - @@ -540,14 +542,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -718,62 +712,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -790,30 +728,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - make -j5 @@ -854,70 +768,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 @@ -990,34 +840,98 @@ true true - + make - -j2 - check + -j5 + testGaussianFactorGraphUnordered.run true true true - + make - -j2 - timeCalibratedCamera.run + -j5 + testGaussianBayesNetUnordered.run true true true - + make - -j2 - timeRot3.run + -j5 + testGaussianConditional.run true true true - + make - -j2 - clean + -j5 + testGaussianDensity.run + true + true + true + + + make + -j5 + testGaussianJunctionTree.run + true + true + true + + + make + -j5 + testHessianFactor.run + true + true + true + + + make + -j5 + testJacobianFactor.run + true + true + true + + + make + -j5 + testKalmanFilter.run + true + true + true + + + make + -j5 + testNoiseModel.run + true + true + true + + + make + -j5 + testSampler.run + true + true + true + + + make + -j5 + testSerializationLinear.run + true + true + true + + + make + -j5 + testVectorValues.run true true true @@ -1086,319 +1000,10 @@ true true - + make -j5 - testDiscreteFactor.run - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j5 - testInference.run - true - true - true - - - make - -j1 - testSymbolicSequentialSolver.run - true - false - true - - - make - -j5 - testEliminationTree.run - true - true - true - - - make - -j1 - testSymbolicBayesTree.run - true - false - true - - - make - -j2 - vSFMexample.run - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j2 - testVSLAMGraph - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testGaussianISAM2.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - -j5 - timing.tests - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - testGraph.run - true - false - true - - - make - testJunctionTree.run - true - false - true - - - make - testSymbolicBayesNetB.run - true - false - true - - - make - -j5 - testGaussianISAM.run - true - true - true - - - make - -j5 - testDoglegOptimizer.run - true - true - true - - - make - -j5 - testNonlinearFactorGraph.run - true - true - true - - - make - -j5 - testIterative.run - true - true - true - - - make - -j5 - testSubgraphSolver.run - true - true - true - - - make - -j5 - testGaussianFactorGraphB.run - true - true - true - - - make - -j5 - testSummarization.run + testTSAMFactors.run true true true @@ -1554,22 +1159,6 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1650,6 +1239,14 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + make -j2 @@ -1746,54 +1343,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -1802,150 +1351,6 @@ true true - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j2 @@ -2090,282 +1495,58 @@ true true - + make -j5 - testVector.run - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - - - make - -j5 - testVectorValues.run - true - true - true - - - make - -j5 - testNoiseModel.run - true - true - true - - - make - -j5 - testHessianFactor.run - true - true - true - - - make - -j5 - testGaussianConditional.run - true - true - true - - - make - -j5 - testGaussianFactorGraphUnordered.run - true - true - true - - - make - -j5 - testGaussianJunctionTree.run - true - true - true - - - make - -j5 - testKalmanFilter.run - true - true - true - - - make - -j5 - testGaussianDensity.run - true - true - true - - - make - -j5 - testSerializationLinear.run - true - true - true - - - make - -j5 - testJacobianFactor.run - true - true - true - - - make - -j5 - testSampler.run - true - true - true - - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - -j5 - SelfCalibrationExample.run - true - true - true - - - make - -j5 - SFMExample.run - true - true - true - - - make - -j5 - VisualISAMExample.run - true - true - true - - - make - -j5 - VisualISAM2Example.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graphviz.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graph.run - true - true - true - - - make - -j5 - SFMExample_bal.run - true - true - true - - - make - -j4 testImuFactor.run true true true - + make - -j2 - check + -j5 + testCombinedImuFactor.run true true true - + make - - tests/testGaussianISAM2 + -j5 + testEliminationTree.run + true + true + true + + + make + -j5 + testInference.run + true + true + true + + + make + -j5 + testKey.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + + + make + -j1 + testSymbolicSequentialSolver.run true false true @@ -2386,102 +1567,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2877,38 +1962,6 @@ true true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap - true - true - true - make -j2 @@ -2948,6 +2001,979 @@ false true + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all + true + 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 + check + true + true + true + + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j2 + testVSLAMGraph + true + true + true + + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testGaussianISAM2.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + -j5 + timing.tests + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + testGraph.run + true + false + true + + + make + testJunctionTree.run + true + false + true + + + make + testSymbolicBayesNetB.run + true + false + true + + + make + -j5 + testGaussianISAM.run + true + true + true + + + make + -j5 + testDoglegOptimizer.run + true + true + true + + + make + -j5 + testNonlinearFactorGraph.run + true + true + true + + + make + -j5 + testIterative.run + true + true + true + + + make + -j5 + testSubgraphSolver.run + true + true + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + + + make + -j5 + testSummarization.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + SimpleRotation.run + true + true + true + + + make + -j5 + CameraResectioning.run + true + true + true + + + make + -j5 + PlanarSLAMExample.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + easyPoint2KalmanFilter.run + true + true + true + + + make + -j2 + elaboratePoint2KalmanFilter.run + true + true + true + + + make + -j5 + Pose2SLAMExample.run + true + true + true + + + make + -j2 + Pose2SLAMwSPCG_easy.run + true + true + true + + + make + -j5 + UGM_small.run + true + true + true + + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + OdometryExample.run + true + true + true + + + make + -j5 + RangeISAMExample_plaza2.run + true + true + true + + + make + -j5 + SelfCalibrationExample.run + true + true + true + + + make + -j5 + SFMExample.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + + + make + -j5 + SFMExample_bal.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + + tests/testGaussianISAM2 + true + false + true + + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/.gitignore b/.gitignore index 030d51b09..6d274af09 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ /build* +/doc* *.pyc *.DS_Store \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index d03f11ede..196b7e4df 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -91,10 +91,10 @@ set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") # If using Boost shared libs, disable auto linking if(MSVC) # Some libraries, at least Boost Program Options, rely on this to export DLL symbols - add_definitions(-DBOOST_ALL_DYN_LINK) # Disable autolinking if(NOT Boost_USE_STATIC_LIBS) add_definitions(-DBOOST_ALL_NO_LIB) + add_definitions(-DBOOST_ALL_DYN_LINK) endif() endif() diff --git a/DEVELOP b/DEVELOP new file mode 100644 index 000000000..483197bc8 --- /dev/null +++ b/DEVELOP @@ -0,0 +1,19 @@ +Information for developers + +Coding Conventions: + +* Classes are Uppercase, methods and functions lowerMixedCase +* We use a modified K&R Style, with 2-space tabs, inserting spaces for tabs +* Use meaningful variable names, e.g., measurement not msm + + +Windows: + +On Windows it is necessary to explicitly export all functions from the library +which should be externally accessible. To do this, include the macro +GTSAM_EXPORT in your class or function definition. + +For example: +class GTSAM_EXPORT MyClass { ... }; + +GTSAM_EXPORT myFunction(); \ No newline at end of file diff --git a/INSTALL b/INSTALL index 75277e815..c71dcd4f9 100644 --- a/INSTALL +++ b/INSTALL @@ -24,7 +24,7 @@ Optional dependent libraries: may be installed from the Ubuntu repositories, and for other platforms it may be downloaded from https://www.threadingbuildingblocks.org/ -Tested compilers +Tested compilers: - GCC 4.2-4.7 - OSX Clang 2.9-5.0 @@ -35,7 +35,12 @@ Tested systems: - Ubuntu 11.04 - 13.10 - MacOS 10.6 - 10.9 -- Windows 7, 8 +- Windows 7, 8, 8.1 + +Known issues: + +- MSVC 2013 is not yet supported because it cannot build the serialization module + of Boost 1.55 (or earlier). 2) GTSAM makes extensive use of debug assertions, and we highly recommend you work diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index ee2bbd42a..9a0b297ab 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -56,7 +56,7 @@ endif() # Clang on Mac uses a template depth that is less than standard and is too small if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0") - add_definitions(-ftemplate-depth=1024) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") endif() endif() diff --git a/cmake/example_project/CMakeLists.txt b/cmake/example_project/CMakeLists.txt index 8dc124b2f..548d56acd 100644 --- a/cmake/example_project/CMakeLists.txt +++ b/cmake/example_project/CMakeLists.txt @@ -1,173 +1,43 @@ # This file should be used as a template for creating new projects using the CMake tools # This project has the following features # - GTSAM linking -# - Boost linking # - Unit tests via CppUnitLite -# - Automatic detection of sources and headers in subfolders -# - Installation of library and headers -# - Matlab wrap interface with within-project building -# - Use of GTSAM cmake macros +# - Scripts +# - Automatic MATLAB wrapper generation ################################################################################### -# To create your own project, replace "myproject" with the actual name of your project +# To create your own project, replace "example" with the actual name of your project cmake_minimum_required(VERSION 2.6) -enable_testing() -project(myproject CXX C) +project(example CXX C) -# Add the cmake subfolder to the cmake module path - necessary to use macros -set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${PROJECT_SOURCE_DIR}/cmake") +# Include GTSAM CMake tools +find_package(GTSAMCMakeTools) +include(GtsamBuildTypes) # Load build type flags and default to Debug mode +include(GtsamTesting) # Easy functions for creating unit tests and scripts +include(GtsamMatlabWrap) # Automatic MATLAB wrapper generation # Ensure that local folder is searched before library folders include_directories(BEFORE "${PROJECT_SOURCE_DIR}") -# Load build type flags and default to Debug mode -include(GtsamBuildTypes) - ################################################################################### -# Create a list of library dependencies -# These will be linked with executables -set(library_deps "") -set(linking_mode "static") - # Find GTSAM components find_package(GTSAM REQUIRED) # Uses installed package -list(APPEND library_deps gtsam-${linking_mode} gtsam_unstable-${linking_mode}) - -# Include ransac -find_package(ransac REQUIRED) # Uses installed package -list(APPEND library_deps ransac-${linking_mode}) - -# Boost - same requirement as gtsam -find_package(Boost 1.43 COMPONENTS - serialization - system - filesystem - thread - date_time - REQUIRED) -list(APPEND library_deps - ${Boost_SERIALIZATION_LIBRARY} - ${Boost_SYSTEM_LIBRARY} - ${Boost_FILESYSTEM_LIBRARY} - ${Boost_THREAD_LIBRARY} - ${Boost_DATE_TIME_LIBRARY}) - -include_directories(${Boost_INCLUDE_DIR} ${GTSAM_INCLUDE_DIR} ${ransac_INCLUDE_DIR}) +include_directories(${GTSAM_INCLUDE_DIR}) ################################################################################### -# List subdirs to process tests/sources -# Each of these will be scanned for new files -set (myproject_subdirs - "." # ensure root folder gets included - stuff - things - ) - -# loop through subdirs to install and build up source lists -set(myproject_lib_source "") -set(myproject_tests_source "") -set(myproject_scripts_source "") -foreach(subdir ${myproject_subdirs}) - # Installing headers - message(STATUS "Installing ${subdir}") - file(GLOB sub_myproject_headers "myproject/${subdir}/*.h") - install(FILES ${sub_myproject_headers} DESTINATION include/myproject/${subdir}) - - # add sources to main sources list - file(GLOB subdir_srcs "myproject/${subdir}/*.cpp") - list(APPEND myproject_lib_source ${subdir_srcs}) - - # add tests to main tests list - file(GLOB subdir_test_srcs "myproject/${subdir}/tests/*.cpp") - list(APPEND myproject_tests_source ${subdir_test_srcs}) - - # add scripts to main tests list - file(GLOB subdir_scripts_srcs "myproject/${subdir}/scripts/*.cpp") - list(APPEND myproject_scripts_source ${subdir_scripts_srcs}) -endforeach(subdir) - -set(myproject_version ${myproject_VERSION_MAJOR}.${myproject_VERSION_MINOR}.${myproject_VERSION_PATCH}) -set(myproject_soversion ${myproject_VERSION_MAJOR}) -message(STATUS "GTSAM Version: ${gtsam_version}") -message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") - -# Build library (static and shared versions) -# Include installed versions -SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) -add_library(${PROJECT_NAME}-shared SHARED ${myproject_lib_source}) -set_target_properties(${PROJECT_NAME}-shared PROPERTIES - OUTPUT_NAME ${PROJECT_NAME} - CLEAN_DIRECT_OUTPUT 1) -install(TARGETS myproject-shared EXPORT myproject-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) -list(APPEND myproject_EXPORTED_TARGETS myproject-shared) - -add_library(${PROJECT_NAME}-static STATIC ${myproject_lib_source}) -set_target_properties(${PROJECT_NAME}-static PROPERTIES - OUTPUT_NAME ${PROJECT_NAME} - CLEAN_DIRECT_OUTPUT 1) -install(TARGETS myproject-static EXPORT myproject-exports ARCHIVE DESTINATION lib) -list(APPEND myproject_EXPORTED_TARGETS myproject-static) - -install(TARGETS ${PROJECT_NAME}-shared LIBRARY DESTINATION lib ) -install(TARGETS ${PROJECT_NAME}-static ARCHIVE DESTINATION lib ) - -# Disabled tests - subtract these from the test files -# Note the need for a full path -set(disabled_tests - "dummy" - #"${PROJECT_SOURCE_DIR}/myproject/geometry/tests/testCovarianceEllipse.cpp" -) -list(REMOVE_ITEM myproject_tests_source ${disabled_tests}) +# Build static library from common sources +set(CONVENIENCE_LIB_NAME ${PROJECT_NAME}) +add_library(${CONVENIENCE_LIB_NAME} STATIC example/PrintExamples.h example/PrintExamples.cpp) +target_link_libraries(${CONVENIENCE_LIB_NAME} gtsam) ################################################################################### -# Build tests -add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND}) -foreach(test_src_file ${myproject_tests_source}) - get_filename_component(test_base ${test_src_file} NAME_WE) - message(STATUS "Adding test ${test_src_file} with base name ${test_base}" ) - add_executable(${test_base} ${test_src_file}) - target_link_libraries(${test_base} ${PROJECT_NAME}-${linking_mode} ${library_deps} CppUnitLite) - add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}/${test_base}) - add_custom_target(${test_base}.run ${test_base} ${ARGN}) - add_dependencies(check ${test_base}) -endforeach(test_src_file) - -# Build scripts -foreach(script_src_file ${myproject_scripts_source}) - get_filename_component(script_base ${script_src_file} NAME_WE) - message(STATUS "Adding script ${script_src_file} with base name ${script_base}" ) - add_executable(${script_base} ${script_src_file}) - target_link_libraries(${script_base} ${PROJECT_NAME}-${linking_mode} ${library_deps} CppUnitLite) - add_custom_target(${script_base}.run ${script_base} ${ARGN}) -endforeach(script_src_file) +# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library) +gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}") ################################################################################### -# Matlab wrapping -include(GtsamMatlabWrap) -set(MEX_COMMAND "mex" CACHE STRING "Command to use for executing mex (if on path, 'mex' will work)") -set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation") -set(MYPROJECT_TOOLBOX_DIR "../matlab/myproject" CACHE PATH "Install folder for matlab toolbox - defaults to inside project") -set(WRAP_HEADER_PATH "${GTSAM_DIR}/../../../include") -set(MYPROJECT_TOOLBOX_FLAGS - ${GTSAM_BUILD_MEX_BINARY_FLAGS} -I${PROJECT_SOURCE_DIR} -I${PROJECT_SOURCE_DIR}/myproject -I${Boost_INCLUDE_DIR} -I${MEX_INCLUDE_ROOT} -I${GTSAM_INCLUDE_DIR} -I${WRAP_HEADER_PATH} -Wl,-rpath,${CMAKE_BINARY_DIR}:${CMAKE_INSTALL_PREFIX}/lib) -set(MYPROJECT_LIBRARY_DEPS gtsam gtsam_unstable ransac myproject) -set(GTSAM_BUILD_MEX_BIN ON) - -# Function to setup codegen, building and installation of the wrap toolbox -# This wrap setup function assumes that the toolbox will be installed directly, -# with predictable matlab.h sourcing -# params: -# moduleName : the name of the module, interface file must be called moduleName.h -# mexFlags : Compilation flags to be passed to the mex compiler -# modulePath : relative path to module markup header file (called moduleName.h) -# otherLibraries : list of library targets this should depend on -# toolboxPath : the directory in which to generate/build wrappers -# wrap_header_path : path to the installed wrap header -wrap_library_generic(myproject "${MYPROJECT_TOOLBOX_FLAGS}" "" "${MYPROJECT_LIBRARY_DEPS}" "${MYPROJECT_TOOLBOX_DIR}" "${WRAP_HEADER_PATH}") +# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library) +gtsamAddExamplesGlob("*.cpp" "" "${CONVENIENCE_LIB_NAME}") ################################################################################### -# Create Install config and export files -# This config file takes the place of FindXXX.cmake scripts -include(GtsamMakeConfigFile) -GtsamMakeConfigFile(myproject) -export(TARGETS ${myproject_EXPORTED_TARGETS} FILE myproject-exports.cmake) \ No newline at end of file +# Build MATLAB wrapper (CMake tracks the dependecy to link with GTSAM through our project's static library) +wrap_and_install_library("example.h" "${CONVENIENCE_LIB_NAME}" "" "") diff --git a/cmake/example_project/SayGoodbye.cpp b/cmake/example_project/SayGoodbye.cpp new file mode 100644 index 000000000..be1165ef6 --- /dev/null +++ b/cmake/example_project/SayGoodbye.cpp @@ -0,0 +1,23 @@ +/* ---------------------------------------------------------------------------- + + * 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 SayGoodbye.cpp + * @brief Example script for example project + * @author Richard Roberts + */ + +#include + +int main(int argc, char *argv[]) { + example::PrintExamples().sayGoodbye(); + return 0; +} diff --git a/cmake/example_project/SayHello.cpp b/cmake/example_project/SayHello.cpp new file mode 100644 index 000000000..2da06ab32 --- /dev/null +++ b/cmake/example_project/SayHello.cpp @@ -0,0 +1,23 @@ +/* ---------------------------------------------------------------------------- + + * 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 SayHello.cpp + * @brief Example script for example project + * @author Richard Roberts + */ + +#include + +int main(int argc, char *argv[]) { + example::PrintExamples().sayHello(); + return 0; +} diff --git a/cmake/example_project/example.h b/cmake/example_project/example.h new file mode 100644 index 000000000..47b9a0aa2 --- /dev/null +++ b/cmake/example_project/example.h @@ -0,0 +1,28 @@ +/* ---------------------------------------------------------------------------- + + * 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 example.h + * @brief Example wrapper interface file + * @author Richard Roberts + */ + +// This is an interface file for automatic MATLAB wrapper generation. See +// gtsam.h for full documentation and more examples. + +namespace example { + +class PrintExamples { + void sayHello() const; + void sayGoodbye() const; +}; + +} diff --git a/cmake/example_project/example/PrintExamples.cpp b/cmake/example_project/example/PrintExamples.cpp new file mode 100644 index 000000000..1e9f10713 --- /dev/null +++ b/cmake/example_project/example/PrintExamples.cpp @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + + * 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 print_examples.cpp + * @brief Example library file + * @author Richard Roberts + */ + +#include + +#include + +namespace example { + +void PrintExamples::sayHello() const { + std::cout << internal::getHelloString() << std::endl; +} + +void PrintExamples::sayGoodbye() const { + std::cout << internal::getGoodbyeString() << std::endl; +} + +namespace internal { + +std::string getHelloString() { + return "Hello!"; +} + +std::string getGoodbyeString() { + return "See you soon!"; +} + +} // namespace internal + +} // namespace example diff --git a/cmake/example_project/example/PrintExamples.h b/cmake/example_project/example/PrintExamples.h new file mode 100644 index 000000000..b151dfbc7 --- /dev/null +++ b/cmake/example_project/example/PrintExamples.h @@ -0,0 +1,41 @@ +/* ---------------------------------------------------------------------------- + + * 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 print_examples.h + * @brief Example library file + * @author Richard Roberts + */ + +#pragma once + +#include + +namespace example { + +class PrintExamples { +public: + /// Print a greeting + void sayHello() const; + + /// Print a farewell + void sayGoodbye() const; +}; + +namespace internal { + +std::string getHelloString(); + +std::string getGoodbyeString(); + +} // namespace internal + +} // namespace example diff --git a/cmake/example_project/tests/testExample.cpp b/cmake/example_project/tests/testExample.cpp new file mode 100644 index 000000000..c2a5a173b --- /dev/null +++ b/cmake/example_project/tests/testExample.cpp @@ -0,0 +1,43 @@ +/* ---------------------------------------------------------------------------- + + * 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 testExample.cpp + * @brief Unit tests for example + * @author Richard Roberts + */ + +#include + +#include + +#include + +using namespace gtsam; + +TEST(Example, HelloString) { + const std::string expectedString = "Hello!"; + EXPECT(assert_equal(expectedString, example::internal::getHelloString())); +} + +TEST(Example, GoodbyeString) { + const std::string expectedString = "See you soon!"; + EXPECT(assert_equal(expectedString, example::internal::getGoodbyeString())); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + + diff --git a/cmake/example_project_simple/CMakeLists.txt b/cmake/example_project_simple/CMakeLists.txt deleted file mode 100644 index e8bea909c..000000000 --- a/cmake/example_project_simple/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -# This file should be used as a template for creating new projects using the CMake tools -# This project has the following features -# - GTSAM linking -# - Unit tests via CppUnitLite -# - Scripts - -################################################################################### -# To create your own project, replace "myproject" with the actual name of your project -cmake_minimum_required(VERSION 2.6) -enable_testing() -project(myproject CXX C) - -# Add the cmake subfolder to the cmake module path - necessary to use macros -list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake") - -# Ensure that local folder is searched before library folders -include_directories(BEFORE "${PROJECT_SOURCE_DIR}") - -# Load build type flags and default to Debug mode -include(GtsamBuildTypes) - -################################################################################### -# Find GTSAM components -find_package(GTSAM REQUIRED) # Uses installed package -include_directories(${GTSAM_INCLUDE_DIR}) - -################################################################################### -# Build static library from common sources -add_library(${PROJECT_NAME} STATIC ${PROJECT_NAME}/MySourceFiles.cpp) -target_link_libraries(${PROJECT_NAME} gtsam-shared) - -################################################################################### -# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library) -gtsam_add_subdir_tests(${PROJECT_NAME} "${PROJECT_NAME}" "${PROJECT_NAME}" "") - -################################################################################### -# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library) -gtsam_add_executables("${PROJECT_NAME}/myScripts.cpp" "${PROJECT_NAME}" "${PROJECT_NAME}" "") diff --git a/examples/Data/dubrovnik-3-7-pre-rewritten.txt b/examples/Data/dubrovnik-3-7-pre-rewritten.txt index 12c9f4db4..7dea43c9e 100644 --- a/examples/Data/dubrovnik-3-7-pre-rewritten.txt +++ b/examples/Data/dubrovnik-3-7-pre-rewritten.txt @@ -20,9 +20,9 @@ 1 6 543.18011474609375 294.80999755859375 2 6 -58.419979095458984375 110.8300018310546875 -0.29656188120312942935 + 0.29656188120312942935 -0.035318354384285870207 -0.31252101755032046793 + 0.31252101755032046793 0.47230274932665988752 -0.3572340863744113415 -2.0517704282499575896 @@ -30,21 +30,21 @@ -7.5572756941255647689e-08 3.2377570134516087119e-14 -0.28532097381985194184 + 0.28532097381985194184 -0.27699838370789808817 0.048601169984112867206 --1.2598695987143850861 + -1.2598695987143850861 -0.049063798188844320869 --1.9586867140445654023 + -1.9586867140445654023 1432.137451171875 -7.3171918302250560373e-08 3.1759419042137054801e-14 0.057491325683772541433 -0.34853090049579965592 -0.47985129303736057116 -8.1963904289063389541 -6.5146840788718787252 + 0.34853090049579965592 + 0.47985129303736057116 + 8.1963904289063389541 + 6.5146840788718787252 -3.8392804395897406344 1572.047119140625 -1.5962623223231275915e-08 diff --git a/gtsam.h b/gtsam.h index de4c1381d..96a778acf 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1423,6 +1423,7 @@ virtual class GaussianBayesNet { void push_back(const gtsam::GaussianBayesNet& bayesNet); gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; gtsam::VectorValues optimizeGradientSearch() const; gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; gtsam::VectorValues gradientAtZero() const; @@ -1550,8 +1551,12 @@ char symbolChr(size_t key); size_t symbolIndex(size_t key); // Default keyformatter -void printKeySet(const gtsam::KeySet& keys); -void printKeySet(const gtsam::KeySet& keys, string s); +void printKeyList (const gtsam::KeyList& keys); +void printKeyList (const gtsam::KeyList& keys, string s); +void printKeyVector(const gtsam::KeyVector& keys); +void printKeyVector(const gtsam::KeyVector& keys, string s); +void printKeySet (const gtsam::KeySet& keys); +void printKeySet (const gtsam::KeySet& keys, string s); #include class LabeledSymbol { @@ -1725,6 +1730,7 @@ class KeySet { // structure specific methods void insert(size_t key); + void merge(gtsam::KeySet& other); bool erase(size_t key); // returns true if value was removed bool count(size_t key) const; // returns true if value exists @@ -2140,6 +2146,8 @@ template virtual class BearingRangeFactor : gtsam::NoiseModelFactor { BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); + pair measured() const; + // enabling serialization functionality void serialize() const; }; @@ -2355,17 +2363,26 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor { namespace utilities { #include + gtsam::KeyList createKeyList(Vector I); + gtsam::KeyList createKeyList(string s, Vector I); + gtsam::KeyVector createKeyVector(Vector I); + gtsam::KeyVector createKeyVector(string s, Vector I); + gtsam::KeySet createKeySet(Vector I); + gtsam::KeySet createKeySet(string s, Vector I); Matrix extractPoint2(const gtsam::Values& values); Matrix extractPoint3(const gtsam::Values& values); Matrix extractPose2(const gtsam::Values& values); gtsam::Values allPose3s(gtsam::Values& values); Matrix extractPose3(const gtsam::Values& values); void perturbPoint2(gtsam::Values& values, double sigma, int seed); + void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); void perturbPoint3(gtsam::Values& values, double sigma, int seed); void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); } //\namespace utilities diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 5822a51f5..576da93bd 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -16,13 +16,30 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endif() endforeach(eigen_dir) + # 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) + if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn"))) + list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}") + install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/unsupported/Eigen) + endif() + endforeach(unsupported_eigen_dir) + # Add to project source set(eigen_headers ${eigen_headers} PARENT_SCOPE) + # set(unsupported_eigen_headers ${unsupported_eigen_headers} PARENT_SCOPE) # install Eigen - only the headers in our 3rdparty directory 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) diff --git a/gtsam/3rdparty/Eigen/CMakeLists.txt b/gtsam/3rdparty/Eigen/CMakeLists.txt index ad0269ea6..76a11b9d2 100644 --- a/gtsam/3rdparty/Eigen/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/CMakeLists.txt @@ -204,7 +204,7 @@ if(NOT MSVC) option(EIGEN_TEST_NEON "Enable/Disable Neon in tests/examples" OFF) if(EIGEN_TEST_NEON) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a"8) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a8") message(STATUS "Enabling NEON in tests/examples") endif() diff --git a/gtsam/3rdparty/Eigen/Eigen/Eigen2Support b/gtsam/3rdparty/Eigen/Eigen/Eigen2Support index 36156d29a..6aa009d20 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Eigen2Support +++ b/gtsam/3rdparty/Eigen/Eigen/Eigen2Support @@ -14,12 +14,25 @@ #error Eigen2 support must be enabled by defining EIGEN2_SUPPORT before including any Eigen header #endif +#ifndef EIGEN_NO_EIGEN2_DEPRECATED_WARNING + +#if defined(__GNUC__) || defined(__INTEL_COMPILER) || defined(__clang__) +#warning "Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)" +#else +#pragma message ("Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)") +#endif + +#endif // EIGEN_NO_EIGEN2_DEPRECATED_WARNING + #include "src/Core/util/DisableStupidWarnings.h" /** \ingroup Support_modules * \defgroup Eigen2Support_Module Eigen2 support module - * This module provides a couple of deprecated functions improving the compatibility with Eigen2. * + * \warning Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. + * + * This module provides a couple of deprecated functions improving the compatibility with Eigen2. + * * To use it, define EIGEN2_SUPPORT before including any Eigen header * \code * #define EIGEN2_SUPPORT diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h index d19cb3968..d026418f8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h @@ -16,7 +16,10 @@ namespace Eigen { namespace internal { -template struct LDLT_Traits; + template struct LDLT_Traits; + + // PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef + enum SignMatrix { PositiveSemiDef, NegativeSemiDef, ZeroSign, Indefinite }; } /** \ingroup Cholesky_Module @@ -69,7 +72,12 @@ template class LDLT * The default constructor is useful in cases in which the user intends to * perform decompositions via LDLT::compute(const MatrixType&). */ - LDLT() : m_matrix(), m_transpositions(), m_isInitialized(false) {} + LDLT() + : m_matrix(), + m_transpositions(), + m_sign(internal::ZeroSign), + m_isInitialized(false) + {} /** \brief Default Constructor with memory preallocation * @@ -81,6 +89,7 @@ template class LDLT : m_matrix(size, size), m_transpositions(size), m_temporary(size), + m_sign(internal::ZeroSign), m_isInitialized(false) {} @@ -93,6 +102,7 @@ template class LDLT : m_matrix(matrix.rows(), matrix.cols()), m_transpositions(matrix.rows()), m_temporary(matrix.rows()), + m_sign(internal::ZeroSign), m_isInitialized(false) { compute(matrix); @@ -139,7 +149,7 @@ template class LDLT inline bool isPositive() const { eigen_assert(m_isInitialized && "LDLT is not initialized."); - return m_sign == 1; + return m_sign == internal::PositiveSemiDef || m_sign == internal::ZeroSign; } #ifdef EIGEN2_SUPPORT @@ -153,7 +163,7 @@ template class LDLT inline bool isNegative(void) const { eigen_assert(m_isInitialized && "LDLT is not initialized."); - return m_sign == -1; + return m_sign == internal::NegativeSemiDef || m_sign == internal::ZeroSign; } /** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A. @@ -235,7 +245,7 @@ template class LDLT MatrixType m_matrix; TranspositionType m_transpositions; TmpMatrixType m_temporary; - int m_sign; + internal::SignMatrix m_sign; bool m_isInitialized; }; @@ -246,7 +256,7 @@ template struct ldlt_inplace; template<> struct ldlt_inplace { template - static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0) + static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign) { using std::abs; typedef typename MatrixType::Scalar Scalar; @@ -258,8 +268,9 @@ template<> struct ldlt_inplace if (size <= 1) { transpositions.setIdentity(); - if(sign) - *sign = numext::real(mat.coeff(0,0))>0 ? 1:-1; + if (numext::real(mat.coeff(0,0)) > 0) sign = PositiveSemiDef; + else if (numext::real(mat.coeff(0,0)) < 0) sign = NegativeSemiDef; + else sign = ZeroSign; return true; } @@ -284,7 +295,6 @@ template<> struct ldlt_inplace if(biggest_in_corner < cutoff) { for(Index i = k; i < size; i++) transpositions.coeffRef(i) = i; - if(sign) *sign = 0; break; } @@ -325,15 +335,15 @@ template<> struct ldlt_inplace } if((rs>0) && (abs(mat.coeffRef(k,k)) > cutoff)) A21 /= mat.coeffRef(k,k); - - if(sign) - { - // LDLT is not guaranteed to work for indefinite matrices, but let's try to get the sign right - int newSign = numext::real(mat.diagonal().coeff(index_of_biggest_in_corner)) > 0; - if(k == 0) - *sign = newSign; - else if(*sign != newSign) - *sign = 0; + + RealScalar realAkk = numext::real(mat.coeffRef(k,k)); + if (sign == PositiveSemiDef) { + if (realAkk < 0) sign = Indefinite; + } else if (sign == NegativeSemiDef) { + if (realAkk > 0) sign = Indefinite; + } else if (sign == ZeroSign) { + if (realAkk > 0) sign = PositiveSemiDef; + else if (realAkk < 0) sign = NegativeSemiDef; } } @@ -399,7 +409,7 @@ template<> struct ldlt_inplace template<> struct ldlt_inplace { template - static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0) + static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign) { Transpose matt(mat); return ldlt_inplace::unblocked(matt, transpositions, temp, sign); @@ -445,7 +455,7 @@ LDLT& LDLT::compute(const MatrixType& a) m_isInitialized = false; m_temporary.resize(size); - internal::ldlt_inplace::unblocked(m_matrix, m_transpositions, m_temporary, &m_sign); + internal::ldlt_inplace::unblocked(m_matrix, m_transpositions, m_temporary, m_sign); m_isInitialized = true; return *this; @@ -473,7 +483,7 @@ LDLT& LDLT::rankUpdate(const MatrixBase=0 ? 1 : -1; + m_sign = sigma>=0 ? internal::PositiveSemiDef : internal::NegativeSemiDef; m_isInitialized = true; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h index 783324b0b..c449960de 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h @@ -58,10 +58,12 @@ cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat) res.p = mat.outerIndexPtr(); res.i = mat.innerIndexPtr(); res.x = mat.valuePtr(); + res.z = 0; res.sorted = 1; if(mat.isCompressed()) { res.packed = 1; + res.nz = 0; } else { @@ -170,6 +172,7 @@ class CholmodBase : internal::noncopyable CholmodBase() : m_cholmodFactor(0), m_info(Success), m_isInitialized(false) { + m_shiftOffset[0] = m_shiftOffset[1] = RealScalar(0.0); cholmod_start(&m_cholmod); } @@ -241,7 +244,7 @@ class CholmodBase : internal::noncopyable return internal::sparse_solve_retval(*this, b.derived()); } - /** Performs a symbolic decomposition on the sparcity of \a matrix. + /** Performs a symbolic decomposition on the sparsity pattern of \a matrix. * * This function is particularly useful when solving for several problems having the same structure. * @@ -265,7 +268,7 @@ class CholmodBase : internal::noncopyable /** Performs a numeric decomposition of \a matrix * - * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed. + * The given matrix must have the same sparsity pattern as the matrix on which the symbolic decomposition has been performed. * * \sa analyzePattern() */ @@ -302,7 +305,7 @@ class CholmodBase : internal::noncopyable { this->m_info = NumericalIssue; } - // TODO optimize this copy by swapping when possible (be carreful with alignment, etc.) + // TODO optimize this copy by swapping when possible (be careful with alignment, etc.) dest = Matrix::Map(reinterpret_cast(x_cd->x),b.rows(),b.cols()); cholmod_free_dense(&x_cd, &m_cholmod); } @@ -323,7 +326,7 @@ class CholmodBase : internal::noncopyable { this->m_info = NumericalIssue; } - // TODO optimize this copy by swapping when possible (be carreful with alignment, etc.) + // TODO optimize this copy by swapping when possible (be careful with alignment, etc.) dest = viewAsEigen(*x_cs); cholmod_free_sparse(&x_cs, &m_cholmod); } @@ -365,8 +368,8 @@ class CholmodBase : internal::noncopyable * * This class allows to solve for A.X = B sparse linear problems via a simplicial LL^T Cholesky factorization * using the Cholmod library. - * This simplicial variant is equivalent to Eigen's built-in SimplicialLLT class. Thefore, it has little practical interest. - * The sparse matrix A must be selfajoint and positive definite. The vectors or matrices + * This simplicial variant is equivalent to Eigen's built-in SimplicialLLT class. Therefore, it has little practical interest. + * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices * X and B can be either dense or sparse. * * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> @@ -412,8 +415,8 @@ class CholmodSimplicialLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimpl * * This class allows to solve for A.X = B sparse linear problems via a simplicial LDL^T Cholesky factorization * using the Cholmod library. - * This simplicial variant is equivalent to Eigen's built-in SimplicialLDLT class. Thefore, it has little practical interest. - * The sparse matrix A must be selfajoint and positive definite. The vectors or matrices + * This simplicial variant is equivalent to Eigen's built-in SimplicialLDLT class. Therefore, it has little practical interest. + * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices * X and B can be either dense or sparse. * * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> @@ -458,7 +461,7 @@ class CholmodSimplicialLDLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimp * This class allows to solve for A.X = B sparse linear problems via a supernodal LL^T Cholesky factorization * using the Cholmod library. * This supernodal variant performs best on dense enough problems, e.g., 3D FEM, or very high order 2D FEM. - * The sparse matrix A must be selfajoint and positive definite. The vectors or matrices + * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices * X and B can be either dense or sparse. * * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> @@ -501,7 +504,7 @@ class CholmodSupernodalLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSuper * \brief A general Cholesky factorization and solver based on Cholmod * * This class allows to solve for A.X = B sparse linear problems via a LL^T or LDL^T Cholesky factorization - * using the Cholmod library. The sparse matrix A must be selfajoint and positive definite. The vectors or matrices + * using the Cholmod library. The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices * X and B can be either dense or sparse. * * This variant permits to change the underlying Cholesky method at runtime. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h index 497efff66..0ab03eff0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h @@ -210,7 +210,7 @@ class Array : Base(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols()) { Base::_check_template_params(); - Base::resize(other.rows(), other.cols()); + Base::_resize_to_match(other); *this = other; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h index 6e37e031a..be9f48a8c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h @@ -29,9 +29,9 @@ struct all_unroller }; template -struct all_unroller +struct all_unroller { - static inline bool run(const Derived &mat) { return mat.coeff(0, 0); } + static inline bool run(const Derived &/*mat*/) { return true; } }; template @@ -55,9 +55,9 @@ struct any_unroller }; template -struct any_unroller +struct any_unroller { - static inline bool run(const Derived &mat) { return mat.coeff(0, 0); } + static inline bool run(const Derived & /*mat*/) { return false; } }; template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h index 2b8dd1b70..fadb45852 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h @@ -126,36 +126,6 @@ Derived& DenseBase::operator-=(const EigenBase &other) return derived(); } -/** replaces \c *this by \c *this * \a other. - * - * \returns a reference to \c *this - */ -template -template -inline Derived& -MatrixBase::operator*=(const EigenBase &other) -{ - other.derived().applyThisOnTheRight(derived()); - return derived(); -} - -/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=(). - */ -template -template -inline void MatrixBase::applyOnTheRight(const EigenBase &other) -{ - other.derived().applyThisOnTheRight(derived()); -} - -/** replaces \c *this by \c *this * \a other. */ -template -template -inline void MatrixBase::applyOnTheLeft(const EigenBase &other) -{ - other.derived().applyThisOnTheLeft(derived()); -} - } // end namespace Eigen #endif // EIGEN_EIGENBASE_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h index c8d5f6379..8d4bc59e9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h @@ -185,21 +185,22 @@ std::ostream & print_matrix(std::ostream & s, const Derived& _m, const IOFormat& explicit_precision = fmt.precision; } + std::streamsize old_precision = 0; + if(explicit_precision) old_precision = s.precision(explicit_precision); + bool align_cols = !(fmt.flags & DontAlignCols); if(align_cols) { // compute the largest width - for(Index j = 1; j < m.cols(); ++j) + for(Index j = 0; j < m.cols(); ++j) for(Index i = 0; i < m.rows(); ++i) { std::stringstream sstr; - if(explicit_precision) sstr.precision(explicit_precision); + sstr.copyfmt(s); sstr << m.coeff(i,j); width = std::max(width, Index(sstr.str().length())); } } - std::streamsize old_precision = 0; - if(explicit_precision) old_precision = s.precision(explicit_precision); s << fmt.matPrefix; for(Index i = 0; i < m.rows(); ++i) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h index 0ba5d90cc..d7d0b5b9a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h @@ -304,7 +304,7 @@ class Matrix : Base(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols()) { Base::_check_template_params(); - Base::resize(other.rows(), other.cols()); + Base::_resize_to_match(other); // FIXME/CHECK: isn't *this = other.derived() more efficient. it allows to // go for pure _set() implementations, right? *this = other; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h index 9193b6abb..344b38f2f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h @@ -510,6 +510,51 @@ template class MatrixBase {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;} }; + +/*************************************************************************** +* Implementation of matrix base methods +***************************************************************************/ + +/** replaces \c *this by \c *this * \a other. + * + * \returns a reference to \c *this + * + * Example: \include MatrixBase_applyOnTheRight.cpp + * Output: \verbinclude MatrixBase_applyOnTheRight.out + */ +template +template +inline Derived& +MatrixBase::operator*=(const EigenBase &other) +{ + other.derived().applyThisOnTheRight(derived()); + return derived(); +} + +/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=(). + * + * Example: \include MatrixBase_applyOnTheRight.cpp + * Output: \verbinclude MatrixBase_applyOnTheRight.out + */ +template +template +inline void MatrixBase::applyOnTheRight(const EigenBase &other) +{ + other.derived().applyThisOnTheRight(derived()); +} + +/** replaces \c *this by \a other * \c *this. + * + * Example: \include MatrixBase_applyOnTheLeft.cpp + * Output: \verbinclude MatrixBase_applyOnTheLeft.out + */ +template +template +inline void MatrixBase::applyOnTheLeft(const EigenBase &other) +{ + other.derived().applyThisOnTheLeft(derived()); +} + } // end namespace Eigen #endif // EIGEN_MATRIXBASE_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h index 4fc5dd318..1297b8413 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h @@ -553,7 +553,8 @@ struct permut_matrix_product_retval template inline void evalTo(Dest& dst) const { const Index n = Side==OnTheLeft ? rows() : cols(); - + // FIXME we need an is_same for expression that is not sensitive to constness. For instance + // is_same_xpr, Block >::value should be true. if(is_same::value && extract_data(dst) == extract_data(m_matrix)) { // apply the permutation inplace diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h index af0a479c7..dd34b59e5 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h @@ -47,7 +47,10 @@ template<> struct check_rows_cols_for_overflow { } }; -template struct conservative_resize_like_impl; +template +struct conservative_resize_like_impl; template struct matrix_swap_impl; @@ -668,8 +671,10 @@ private: enum { ThisConstantIsPrivateInPlainObjectBase }; }; +namespace internal { + template -struct internal::conservative_resize_like_impl +struct conservative_resize_like_impl { typedef typename Derived::Index Index; static void run(DenseBase& _this, Index rows, Index cols) @@ -729,11 +734,14 @@ struct internal::conservative_resize_like_impl } }; -namespace internal { - +// Here, the specialization for vectors inherits from the general matrix case +// to allow calling .conservativeResize(rows,cols) on vectors. template struct conservative_resize_like_impl + : conservative_resize_like_impl { + using conservative_resize_like_impl::run; + typedef typename Derived::Index Index; static void run(DenseBase& _this, Index size) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h index aba795bdb..00d9e6d2b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h @@ -94,7 +94,8 @@ struct traits > typedef _PlainObjectType PlainObjectType; typedef _StrideType StrideType; enum { - Options = _Options + Options = _Options, + Flags = traits >::Flags | NestByRefBit }; template struct match { @@ -111,7 +112,7 @@ struct traits > }; typedef typename internal::conditional::type type; }; - + }; template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h index c83e955ee..389d94275 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h @@ -17,16 +17,29 @@ namespace internal { template inline void stable_norm_kernel(const ExpressionType& bl, Scalar& ssq, Scalar& scale, Scalar& invScale) { - Scalar max = bl.cwiseAbs().maxCoeff(); - if (max>scale) + using std::max; + Scalar maxCoeff = bl.cwiseAbs().maxCoeff(); + + if (maxCoeff>scale) { - ssq = ssq * numext::abs2(scale/max); - scale = max; - invScale = Scalar(1)/scale; + ssq = ssq * numext::abs2(scale/maxCoeff); + Scalar tmp = Scalar(1)/maxCoeff; + if(tmp > NumTraits::highest()) + { + invScale = NumTraits::highest(); + scale = Scalar(1)/invScale; + } + else + { + scale = maxCoeff; + invScale = tmp; + } } - // TODO if the max is much much smaller than the current scale, + + // TODO if the maxCoeff is much much smaller than the current scale, // then we can neglect this sub vector - ssq += (bl*invScale).squaredNorm(); + if(scale>Scalar(0)) // if scale==0, then bl is 0 + ssq += (bl*invScale).squaredNorm(); } template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h index f21b3aa65..22096ea2f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h @@ -284,7 +284,8 @@ struct inplace_transpose_selector { // non square matrix * Notice however that this method is only useful if you want to replace a matrix by its own transpose. * If you just need the transpose of a matrix, use transpose(). * - * \note if the matrix is not square, then \c *this must be a resizable matrix. + * \note if the matrix is not square, then \c *this must be a resizable matrix. + * This excludes (non-square) fixed-size matrices, block-expressions and maps. * * \sa transpose(), adjoint(), adjointInPlace() */ template @@ -315,6 +316,7 @@ inline void DenseBase::transposeInPlace() * If you just need the adjoint of a matrix, use adjoint(). * * \note if the matrix is not square, then \c *this must be a resizable matrix. + * This excludes (non-square) fixed-size matrices, block-expressions and maps. * * \sa transpose(), adjoint(), transposeInPlace() */ template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h index 511564875..d5ab03664 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h @@ -50,7 +50,7 @@ struct traits > MaxColsAtCompileTime = Direction==Horizontal ? 1 : MatrixType::MaxColsAtCompileTime, Flags0 = (unsigned int)_MatrixTypeNested::Flags & HereditaryBits, Flags = (Flags0 & ~RowMajorBit) | (RowsAtCompileTime == 1 ? RowMajorBit : 0), - TraversalSize = Direction==Vertical ? RowsAtCompileTime : ColsAtCompileTime + TraversalSize = Direction==Vertical ? MatrixType::RowsAtCompileTime : MatrixType::ColsAtCompileTime }; #if EIGEN_GNUC_AT_LEAST(3,4) typedef typename MemberOp::template Cost CostOpType; @@ -58,7 +58,8 @@ struct traits > typedef typename MemberOp::template Cost CostOpType; #endif enum { - CoeffReadCost = TraversalSize * traits<_MatrixTypeNested>::CoeffReadCost + int(CostOpType::value) + CoeffReadCost = TraversalSize==Dynamic ? Dynamic + : TraversalSize * traits<_MatrixTypeNested>::CoeffReadCost + int(CostOpType::value) }; }; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h index 3376a984e..99cbd0d95 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h @@ -442,8 +442,11 @@ Packet4f pcos(const Packet4f& _x) return _mm_xor_ps(y, sign_bit); } +#if EIGEN_FAST_MATH + // This is based on Quake3's fast inverse square root. // For detail see here: http://www.beyond3d.com/content/articles/8/ +// It lacks 1 (or 2 bits in some rare cases) of precision, and does not handle negative, +inf, or denormalized numbers correctly. template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f psqrt(const Packet4f& _x) { @@ -457,6 +460,14 @@ Packet4f psqrt(const Packet4f& _x) return pmul(_x,x); } +#else + +template<> EIGEN_STRONG_INLINE Packet4f psqrt(const Packet4f& x) { return _mm_sqrt_ps(x); } + +#endif + +template<> EIGEN_STRONG_INLINE Packet2d psqrt(const Packet2d& x) { return _mm_sqrt_pd(x); } + } // end namespace internal } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h index e256f4bac..fc8ae50fe 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h @@ -83,7 +83,8 @@ template<> struct packet_traits : default_packet_traits size=2, HasDiv = 1, - HasExp = 1 + HasExp = 1, + HasSqrt = 1 }; }; template<> struct packet_traits : default_packet_traits @@ -507,8 +508,8 @@ template<> EIGEN_STRONG_INLINE int predux_min(const Packet4i& a) // for GCC (eg., it does not like using std::min after the pstore !!) EIGEN_ALIGN16 int aux[4]; pstore(aux, a); - register int aux0 = aux[0] EIGEN_STRONG_INLINE int predux_max(const Packet4i& a) // for GCC (eg., it does not like using std::min after the pstore !!) EIGEN_ALIGN16 int aux[4]; pstore(aux, a); - register int aux0 = aux[0]>aux[1] ? aux[0] : aux[1]; - register int aux2 = aux[2]>aux[3] ? aux[2] : aux[3]; + int aux0 = aux[0]>aux[1] ? aux[0] : aux[1]; + int aux2 = aux[2]>aux[3] ? aux[2] : aux[3]; return aux0>aux2 ? aux0 : aux2; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h index 780fa74d3..bcdca5b0d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h @@ -1128,6 +1128,8 @@ EIGEN_DONT_INLINE void gemm_pack_lhs::size }; EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK LHS"); + EIGEN_UNUSED_VARIABLE(stride) + EIGEN_UNUSED_VARIABLE(offset) eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride)); eigen_assert( (StorageOrder==RowMajor) || ((Pack1%PacketSize)==0 && Pack1<=4*PacketSize) ); conj_if::IsComplex && Conjugate> cj; @@ -1215,6 +1217,8 @@ EIGEN_DONT_INLINE void gemm_pack_rhs=depth && offset<=stride)); conj_if::IsComplex && Conjugate> cj; Index packet_cols = (cols/nr) * nr; @@ -1266,6 +1270,8 @@ EIGEN_DONT_INLINE void gemm_pack_rhs=depth && offset<=stride)); conj_if::IsComplex && Conjugate> cj; Index packet_cols = (cols/nr) * nr; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h index c1cb78498..09387703e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h @@ -52,11 +52,7 @@ EIGEN_DONT_INLINE static void run( Index rows, Index cols, const LhsScalar* lhs, Index lhsStride, const RhsScalar* rhs, Index rhsIncr, - ResScalar* res, Index - #ifdef EIGEN_INTERNAL_DEBUGGING - resIncr - #endif - , RhsScalar alpha); + ResScalar* res, Index resIncr, RhsScalar alpha); }; template @@ -64,12 +60,9 @@ EIGEN_DONT_INLINE void general_matrix_vector_product(&lhs0[i]), ptmp0, pload(&res[i]))); + pstore(&res[i], pcj.pmadd(pload(&lhs0[i]), ptmp0, pload(&res[i]))); else for (Index i = alignedStart;i(&lhs0[i]), ptmp0, pload(&res[i]))); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h index c40e80f53..f698f67f9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h @@ -79,8 +79,8 @@ EIGEN_DONT_INLINE void selfadjoint_matrix_vector_product(t0); @@ -147,7 +147,7 @@ EIGEN_DONT_INLINE void selfadjoint_matrix_vector_productx || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ @@ -238,7 +238,12 @@ #endif // Suppresses 'unused variable' warnings. -#define EIGEN_UNUSED_VARIABLE(var) (void)var; +namespace Eigen { + namespace internal { + template void ignore_unused_variable(const T&) {} + } +} +#define EIGEN_UNUSED_VARIABLE(var) Eigen::internal::ignore_unused_variable(var); #if !defined(EIGEN_ASM_COMMENT) #if (defined __GNUC__) && ( defined(__i386__) || defined(__x86_64__) ) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h index 451535a0c..cacbd02fd 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h @@ -578,7 +578,7 @@ template class aligned_stack_memory_handler */ #ifdef EIGEN_ALLOCA - #ifdef __arm__ + #if defined(__arm__) || defined(_WIN32) #define EIGEN_ALIGNED_ALLOCA(SIZE) reinterpret_cast((reinterpret_cast(EIGEN_ALLOCA(SIZE+16)) & ~(size_t(15))) + 16) #else #define EIGEN_ALIGNED_ALLOCA EIGEN_ALLOCA @@ -634,7 +634,9 @@ template class aligned_stack_memory_handler /* memory allocated we can safely let the default implementation handle */ \ /* this particular case. */ \ static void *operator new(size_t size, void *ptr) { return ::operator new(size,ptr); } \ + static void *operator new[](size_t size, void* ptr) { return ::operator new[](size,ptr); } \ void operator delete(void * memory, void *ptr) throw() { return ::operator delete(memory,ptr); } \ + void operator delete[](void * memory, void *ptr) throw() { return ::operator delete[](memory,ptr); } \ /* nothrow-new (returns zero instead of std::bad_alloc) */ \ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \ void operator delete(void *ptr, const std::nothrow_t&) throw() { \ @@ -729,15 +731,6 @@ public: ::new( p ) T( value ); } - // Support for c++11 -#if (__cplusplus >= 201103L) - template - void construct(pointer p, Args&&... args) - { - ::new(p) T(std::forward(args)...); - } -#endif - void destroy( pointer p ) { p->~T(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/SVD.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/SVD.h index 077d26d54..3d03d2288 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/SVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/SVD.h @@ -512,8 +512,7 @@ template template bool SVD::solve(const MatrixBase &b, ResultType* result) const { - const int rows = m_matU.rows(); - ei_assert(b.rows() == rows); + ei_assert(b.rows() == m_matU.rows()); Scalar maxVal = m_sigma.cwise().abs().maxCoeff(); for (int j=0; j conjugate() const; - /** \returns an interpolation for a constant motion between \a other and \c *this - * \a t in [0;1] - * see http://en.wikipedia.org/wiki/Slerp - */ template Quaternion slerp(const Scalar& t, const QuaternionBase& other) const; /** \returns \c true if \c *this is approximately equal to \a other, within the precision @@ -194,11 +190,11 @@ public: * \brief The quaternion class used to represent 3D orientations and rotations * * \tparam _Scalar the scalar type, i.e., the type of the coefficients - * \tparam _Options controls the memory alignement of the coeffecients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign. + * \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign. * * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of * orientations and rotations of objects in three dimensions. Compared to other representations - * like Euler angles or 3x3 matrices, quatertions offer the following advantages: + * like Euler angles or 3x3 matrices, quaternions offer the following advantages: * \li \b compact storage (4 scalars) * \li \b efficient to compose (28 flops), * \li \b stable spherical interpolation @@ -385,7 +381,7 @@ class Map, _Options > /** Constructs a Mapped Quaternion object from the pointer \a coeffs * - * The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order: + * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order: * \code *coeffs == {x, y, z, w} \endcode * * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ @@ -399,16 +395,16 @@ class Map, _Options > }; /** \ingroup Geometry_Module - * Map an unaligned array of single precision scalar as a quaternion */ + * Map an unaligned array of single precision scalars as a quaternion */ typedef Map, 0> QuaternionMapf; /** \ingroup Geometry_Module - * Map an unaligned array of double precision scalar as a quaternion */ + * Map an unaligned array of double precision scalars as a quaternion */ typedef Map, 0> QuaternionMapd; /** \ingroup Geometry_Module - * Map a 16-bits aligned array of double precision scalars as a quaternion */ + * Map a 16-byte aligned array of single precision scalars as a quaternion */ typedef Map, Aligned> QuaternionMapAlignedf; /** \ingroup Geometry_Module - * Map a 16-bits aligned array of double precision scalars as a quaternion */ + * Map a 16-byte aligned array of double precision scalars as a quaternion */ typedef Map, Aligned> QuaternionMapAlignedd; /*************************************************************************** @@ -579,7 +575,7 @@ inline Derived& QuaternionBase::setFromTwoVectors(const MatrixBase accuraletly compute the rotation axis by computing the + // => accurately compute the rotation axis by computing the // intersection of the two planes. This is done by solving: // x^T v0 = 0 // x^T v1 = 0 @@ -677,8 +673,13 @@ QuaternionBase::angularDistance(const QuaternionBase& oth return static_cast(2 * acos(d)); } + + /** \returns the spherical linear interpolation between the two quaternions - * \c *this and \a other at the parameter \a t + * \c *this and \a other at the parameter \a t in [0;1]. + * + * This represents an interpolation for a constant motion between \c *this and \a other, + * see also http://en.wikipedia.org/wiki/Slerp. */ template template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h index 887e718d6..498fea41a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h @@ -530,9 +530,9 @@ public: inline Transform& operator=(const UniformScaling& t); inline Transform& operator*=(const UniformScaling& s) { return scale(s.factor()); } - inline Transform operator*(const UniformScaling& s) const + inline Transform operator*(const UniformScaling& s) const { - Transform res = *this; + Transform res = *this; res.scale(s.factor()); return res; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h index 8b01f8179..bec85810c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h @@ -349,7 +349,7 @@ template class ColPivHouseholderQR return m_usePrescribedThreshold ? m_prescribedThreshold // this formula comes from experimenting (see "LU precision tuning" thread on the list) // and turns out to be identical to Higham's formula used already in LDLt. - : NumTraits::epsilon() * m_qr.diagonalSize(); + : NumTraits::epsilon() * RealScalar(m_qr.diagonalSize()); } /** \returns the number of nonzero pivots in the QR decomposition. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h index 0dd5ad347..6168e7abf 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h @@ -63,9 +63,10 @@ template class FullPivHouseholderQR typedef typename MatrixType::Index Index; typedef internal::FullPivHouseholderQRMatrixQReturnType MatrixQReturnType; typedef typename internal::plain_diag_type::type HCoeffsType; - typedef Matrix IntRowVectorType; + typedef Matrix IntDiagSizeVectorType; typedef PermutationMatrix PermutationType; - typedef typename internal::plain_col_type::type IntColVectorType; typedef typename internal::plain_row_type::type RowVectorType; typedef typename internal::plain_col_type::type ColVectorType; @@ -93,10 +94,10 @@ template class FullPivHouseholderQR FullPivHouseholderQR(Index rows, Index cols) : m_qr(rows, cols), m_hCoeffs((std::min)(rows,cols)), - m_rows_transpositions(rows), - m_cols_transpositions(cols), + m_rows_transpositions((std::min)(rows,cols)), + m_cols_transpositions((std::min)(rows,cols)), m_cols_permutation(cols), - m_temp((std::min)(rows,cols)), + m_temp(cols), m_isInitialized(false), m_usePrescribedThreshold(false) {} @@ -115,10 +116,10 @@ template class FullPivHouseholderQR FullPivHouseholderQR(const MatrixType& matrix) : m_qr(matrix.rows(), matrix.cols()), m_hCoeffs((std::min)(matrix.rows(), matrix.cols())), - m_rows_transpositions(matrix.rows()), - m_cols_transpositions(matrix.cols()), + m_rows_transpositions((std::min)(matrix.rows(), matrix.cols())), + m_cols_transpositions((std::min)(matrix.rows(), matrix.cols())), m_cols_permutation(matrix.cols()), - m_temp((std::min)(matrix.rows(), matrix.cols())), + m_temp(matrix.cols()), m_isInitialized(false), m_usePrescribedThreshold(false) { @@ -126,11 +127,12 @@ template class FullPivHouseholderQR } /** This method finds a solution x to the equation Ax=b, where A is the matrix of which - * *this is the QR decomposition, if any exists. + * \c *this is the QR decomposition. * * \param b the right-hand-side of the equation to solve. * - * \returns a solution. + * \returns the exact or least-square solution if the rank is greater or equal to the number of columns of A, + * and an arbitrary solution otherwise. * * \note The case where b is a matrix is not yet implemented. Also, this * code is space inefficient. @@ -172,7 +174,7 @@ template class FullPivHouseholderQR } /** \returns a const reference to the vector of indices representing the rows transpositions */ - const IntColVectorType& rowsTranspositions() const + const IntDiagSizeVectorType& rowsTranspositions() const { eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized."); return m_rows_transpositions; @@ -344,7 +346,7 @@ template class FullPivHouseholderQR return m_usePrescribedThreshold ? m_prescribedThreshold // this formula comes from experimenting (see "LU precision tuning" thread on the list) // and turns out to be identical to Higham's formula used already in LDLt. - : NumTraits::epsilon() * m_qr.diagonalSize(); + : NumTraits::epsilon() * RealScalar(m_qr.diagonalSize()); } /** \returns the number of nonzero pivots in the QR decomposition. @@ -368,8 +370,8 @@ template class FullPivHouseholderQR protected: MatrixType m_qr; HCoeffsType m_hCoeffs; - IntColVectorType m_rows_transpositions; - IntRowVectorType m_cols_transpositions; + IntDiagSizeVectorType m_rows_transpositions; + IntDiagSizeVectorType m_cols_transpositions; PermutationType m_cols_permutation; RowVectorType m_temp; bool m_isInitialized, m_usePrescribedThreshold; @@ -415,10 +417,10 @@ FullPivHouseholderQR& FullPivHouseholderQR::compute(cons m_temp.resize(cols); - m_precision = NumTraits::epsilon() * size; + m_precision = NumTraits::epsilon() * RealScalar(size); - m_rows_transpositions.resize(matrix.rows()); - m_cols_transpositions.resize(matrix.cols()); + m_rows_transpositions.resize(size); + m_cols_transpositions.resize(size); Index number_of_transpositions = 0; RealScalar biggest(0); @@ -516,17 +518,6 @@ struct solve_retval, Rhs> dec().hCoeffs().coeff(k), &temp.coeffRef(0)); } - if(!dec().isSurjective()) - { - // is c is in the image of R ? - RealScalar biggest_in_upper_part_of_c = c.topRows( dec().rank() ).cwiseAbs().maxCoeff(); - RealScalar biggest_in_lower_part_of_c = c.bottomRows(rows-dec().rank()).cwiseAbs().maxCoeff(); - // FIXME brain dead - const RealScalar m_precision = NumTraits::epsilon() * (std::min)(rows,cols); - // this internal:: prefix is needed by at least gcc 3.4 and ICC - if(!internal::isMuchSmallerThan(biggest_in_lower_part_of_c, biggest_in_upper_part_of_c, m_precision)) - return; - } dec().matrixQR() .topLeftCorner(dec().rank(), dec().rank()) .template triangularView() @@ -548,14 +539,14 @@ template struct FullPivHouseholderQRMatrixQReturnType { public: typedef typename MatrixType::Index Index; - typedef typename internal::plain_col_type::type IntColVectorType; + typedef typename FullPivHouseholderQR::IntDiagSizeVectorType IntDiagSizeVectorType; typedef typename internal::plain_diag_type::type HCoeffsType; typedef Matrix WorkVectorType; FullPivHouseholderQRMatrixQReturnType(const MatrixType& qr, const HCoeffsType& hCoeffs, - const IntColVectorType& rowsTranspositions) + const IntDiagSizeVectorType& rowsTranspositions) : m_qr(qr), m_hCoeffs(hCoeffs), m_rowsTranspositions(rowsTranspositions) @@ -595,7 +586,7 @@ public: protected: typename MatrixType::Nested m_qr; typename HCoeffsType::Nested m_hCoeffs; - typename IntColVectorType::Nested m_rowsTranspositions; + typename IntDiagSizeVectorType::Nested m_rowsTranspositions; }; } // end namespace internal diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h index aa41f434c..a2cc2a9e2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h @@ -64,7 +64,8 @@ class SPQR typedef PermutationMatrix PermutationType; public: SPQR() - : m_ordering(SPQR_ORDERING_DEFAULT), + : m_isInitialized(false), + m_ordering(SPQR_ORDERING_DEFAULT), m_allow_tol(SPQR_DEFAULT_TOL), m_tolerance (NumTraits::epsilon()) { @@ -72,7 +73,8 @@ class SPQR } SPQR(const _MatrixType& matrix) - : m_ordering(SPQR_ORDERING_DEFAULT), + : m_isInitialized(false), + m_ordering(SPQR_ORDERING_DEFAULT), m_allow_tol(SPQR_DEFAULT_TOL), m_tolerance (NumTraits::epsilon()) { @@ -82,16 +84,22 @@ class SPQR ~SPQR() { - // Calls SuiteSparseQR_free() + SPQR_free(); + cholmod_l_finish(&m_cc); + } + void SPQR_free() + { cholmod_l_free_sparse(&m_H, &m_cc); cholmod_l_free_sparse(&m_cR, &m_cc); cholmod_l_free_dense(&m_HTau, &m_cc); std::free(m_E); std::free(m_HPinv); - cholmod_l_finish(&m_cc); } + void compute(const _MatrixType& matrix) { + if(m_isInitialized) SPQR_free(); + MatrixType mat(matrix); cholmod_sparse A; A = viewAsCholmod(mat); @@ -139,7 +147,7 @@ class SPQR eigen_assert(b.cols()==1 && "This method is for vectors only"); //Compute Q^T * b - Dest y; + typename Dest::PlainObject y; y = matrixQ().transpose() * b; // Solves with the triangular matrix R Index rk = this->rank(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h index 4786768ff..f44995cd3 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h @@ -380,7 +380,10 @@ struct svd_precondition_2x2_block_to_be_real z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q); work_matrix.row(p) *= z; if(svd.computeU()) svd.m_matrixU.col(p) *= conj(z); - z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q); + if(work_matrix.coeff(q,q)!=Scalar(0)) + z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q); + else + z = Scalar(0); work_matrix.row(q) *= z; if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h index 4b13f08d4..5c320e2d2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h @@ -66,9 +66,9 @@ static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& r } // unordered insertion - for(int k=0; k1) std::sort(indices.data(),indices.data()+nnz); - for(int k=0; k RowMajorMatrix; - typedef SparseMatrix ColMajorMatrix; + typedef SparseMatrix RowMajorMatrix; + typedef SparseMatrix ColMajorMatrix; ColMajorMatrix resCol(lhs.rows(),rhs.cols()); internal::conservative_sparse_sparse_product_impl(lhs, rhs, resCol); // sort the non zeros: @@ -149,7 +149,7 @@ struct conservative_sparse_sparse_product_selector RowMajorMatrix; + typedef SparseMatrix RowMajorMatrix; RowMajorMatrix rhsRow = rhs; RowMajorMatrix resRow(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(rhsRow, lhs, resRow); @@ -162,7 +162,7 @@ struct conservative_sparse_sparse_product_selector RowMajorMatrix; + typedef SparseMatrix RowMajorMatrix; RowMajorMatrix lhsRow = lhs; RowMajorMatrix resRow(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(rhs, lhsRow, resRow); @@ -175,7 +175,7 @@ struct conservative_sparse_sparse_product_selector RowMajorMatrix; + typedef SparseMatrix RowMajorMatrix; RowMajorMatrix resRow(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(rhs, lhs, resRow); res = resRow; @@ -190,7 +190,7 @@ struct conservative_sparse_sparse_product_selector ColMajorMatrix; + typedef SparseMatrix ColMajorMatrix; ColMajorMatrix resCol(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(lhs, rhs, resCol); res = resCol; @@ -202,7 +202,7 @@ struct conservative_sparse_sparse_product_selector ColMajorMatrix; + typedef SparseMatrix ColMajorMatrix; ColMajorMatrix lhsCol = lhs; ColMajorMatrix resCol(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(lhsCol, rhs, resCol); @@ -215,7 +215,7 @@ struct conservative_sparse_sparse_product_selector ColMajorMatrix; + typedef SparseMatrix ColMajorMatrix; ColMajorMatrix rhsCol = rhs; ColMajorMatrix resCol(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(lhs, rhsCol, resCol); @@ -228,8 +228,8 @@ struct conservative_sparse_sparse_product_selector RowMajorMatrix; - typedef SparseMatrix ColMajorMatrix; + typedef SparseMatrix RowMajorMatrix; + typedef SparseMatrix ColMajorMatrix; RowMajorMatrix resRow(lhs.rows(),rhs.cols()); internal::conservative_sparse_sparse_product_impl(rhs, lhs, resRow); // sort the non zeros: diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h index 93cd4832d..ab1a266a9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h @@ -50,6 +50,8 @@ class MappedSparseMatrix inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; } inline Index innerSize() const { return m_innerSize; } inline Index outerSize() const { return m_outerSize; } + + bool isCompressed() const { return true; } //---------------------------------------- // direct access interface diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h index 0b3e193db..16a20a574 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h @@ -66,6 +66,8 @@ public: typename XprType::Nested m_matrix; Index m_outerStart; const internal::variable_if_dynamic m_outerSize; + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) }; @@ -391,6 +393,8 @@ public: protected: friend class InnerIterator; friend class ReverseInnerIterator; + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) typename XprType::Nested m_matrix; const internal::variable_if_dynamic m_startRow; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h index f89ca3814..f8745f461 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h @@ -63,6 +63,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl typedef typename MatrixType::Index Index; Index nc = mat.cols(); // Number of columns Index m = mat.rows(); + Index diagSize = (std::min)(nc,m); IndexVector root(nc); // root of subtree of etree root.setZero(); IndexVector pp(nc); // disjoint sets @@ -72,7 +73,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl Index row,col; firstRowElt.resize(m); firstRowElt.setConstant(nc); - firstRowElt.segment(0, nc).setLinSpaced(nc, 0, nc-1); + firstRowElt.segment(0, diagSize).setLinSpaced(diagSize, 0, diagSize-1); bool found_diag; for (col = 0; col < nc; col++) { @@ -91,7 +92,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl Index rset, cset, rroot; for (col = 0; col < nc; col++) { - found_diag = false; + found_diag = col>=m; pp(col) = col; cset = col; root(cset) = col; @@ -105,6 +106,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl Index i = col; if(it) i = it.index(); if (i == col) found_diag = true; + row = firstRowElt(i); if (row >= col) continue; rset = internal::etree_find(row, pp); // Find the name of the set containing row diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h index 30975c29c..54fd633a1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h @@ -125,7 +125,7 @@ class SparseDenseOuterProduct::InnerIterator : public _LhsNes inline Scalar value() const { return Base::value() * m_factor; } protected: - int m_outer; + Index m_outer; Scalar m_factor; }; @@ -155,7 +155,7 @@ struct sparse_time_dense_product_impl::Constant(outerSize(), 2)); } return insertUncompressed(row,col); } @@ -402,7 +402,7 @@ class SparseMatrix * \sa insertBack, insertBackByOuterInner */ inline void startVec(Index outer) { - eigen_assert(m_outerIndex[outer]==int(m_data.size()) && "You must call startVec for each inner vector sequentially"); + eigen_assert(m_outerIndex[outer]==Index(m_data.size()) && "You must call startVec for each inner vector sequentially"); eigen_assert(m_outerIndex[outer+1]==0 && "You must call startVec for each inner vector sequentially"); m_outerIndex[outer+1] = m_outerIndex[outer]; } @@ -480,7 +480,7 @@ class SparseMatrix if(m_innerNonZeros != 0) return; m_innerNonZeros = static_cast(std::malloc(m_outerSize * sizeof(Index))); - for (int i = 0; i < m_outerSize; i++) + for (Index i = 0; i < m_outerSize; i++) { m_innerNonZeros[i] = m_outerIndex[i+1] - m_outerIndex[i]; } @@ -752,8 +752,8 @@ class SparseMatrix else for (Index i=0; i trMat(mat.rows(),mat.cols()); - if(begin wi(trMat.outerSize()); wi.setZero(); for(InputIterator it(begin); it!=end; ++it) { @@ -1018,11 +1019,11 @@ void SparseMatrix::sumupDuplicates() { eigen_assert(!isCompressed()); // TODO, in practice we should be able to use m_innerNonZeros for that task - VectorXi wi(innerSize()); + Matrix wi(innerSize()); wi.fill(-1); Index count = 0; // for each inner-vector, wi[inner_index] will hold the position of first element into the index/value buffers - for(int j=0; j& SparseMatrix positions(dest.outerSize()); for (Index j=0; j class SparseMatrixBase : public EigenBase } else { - SparseMatrix trans = m; - s << static_cast >&>(trans); + SparseMatrix trans = m; + s << static_cast >&>(trans); } } return s; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h index b897b7595..b85be93f6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h @@ -57,7 +57,7 @@ struct permut_sparsematrix_product_retval if(MoveOuter) { SparseMatrix tmp(m_matrix.rows(), m_matrix.cols()); - VectorXi sizes(m_matrix.outerSize()); + Matrix sizes(m_matrix.outerSize()); for(Index j=0; j tmp(m_matrix.rows(), m_matrix.cols()); - VectorXi sizes(tmp.outerSize()); + Matrix sizes(tmp.outerSize()); sizes.setZero(); PermutationMatrix perm; if((Side==OnTheLeft) ^ Transposed) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseProduct.h index 70b6480ef..cf7663070 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseProduct.h @@ -16,6 +16,7 @@ template struct SparseSparseProductReturnType { typedef typename internal::traits::Scalar Scalar; + typedef typename internal::traits::Index Index; enum { LhsRowMajor = internal::traits::Flags & RowMajorBit, RhsRowMajor = internal::traits::Flags & RowMajorBit, @@ -24,11 +25,11 @@ struct SparseSparseProductReturnType }; typedef typename internal::conditional, + SparseMatrix, typename internal::nested::type>::type LhsNested; typedef typename internal::conditional, + SparseMatrix, typename internal::nested::type>::type RhsNested; typedef SparseSparseProduct Type; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h index 70857c7b6..fcc18f5c9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h @@ -27,7 +27,7 @@ static void sparse_sparse_product_with_pruning_impl(const Lhs& lhs, const Rhs& r // make sure to call innerSize/outerSize since we fake the storage order. Index rows = lhs.innerSize(); Index cols = rhs.outerSize(); - //int size = lhs.outerSize(); + //Index size = lhs.outerSize(); eigen_assert(lhs.outerSize() == rhs.innerSize()); // allocate a temporary buffer @@ -100,7 +100,7 @@ struct sparse_sparse_product_with_pruning_selector SparseTemporaryType; + typedef SparseMatrix SparseTemporaryType; SparseTemporaryType _res(res.rows(), res.cols()); internal::sparse_sparse_product_with_pruning_impl(lhs, rhs, _res, tolerance); res = _res; @@ -126,10 +126,11 @@ struct sparse_sparse_product_with_pruning_selector ColMajorMatrix; - ColMajorMatrix colLhs(lhs); - ColMajorMatrix colRhs(rhs); - internal::sparse_sparse_product_with_pruning_impl(colLhs, colRhs, res, tolerance); + typedef SparseMatrix ColMajorMatrixLhs; + typedef SparseMatrix ColMajorMatrixRhs; + ColMajorMatrixLhs colLhs(lhs); + ColMajorMatrixRhs colRhs(rhs); + internal::sparse_sparse_product_with_pruning_impl(colLhs, colRhs, res, tolerance); // let's transpose the product to get a column x column product // typedef SparseMatrix SparseTemporaryType; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h index 064a40707..05023858b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h @@ -143,7 +143,7 @@ template struct plain_matrix_type * * \sa SparseMatrix::setFromTriplets() */ -template +template::Index > class Triplet { public: diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h index dd9eab2c2..1d592f2c8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h @@ -219,9 +219,9 @@ class SparseLU : public internal::SparseLUImpl - bool _solve(const MatrixBase &B, MatrixBase &_X) const + bool _solve(const MatrixBase &B, MatrixBase &X_base) const { - Dest& X(_X.derived()); + Dest& X(X_base.derived()); eigen_assert(m_factorizationIsOk && "The matrix should be factorized first"); EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0, THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES); @@ -229,8 +229,10 @@ class SparseLU : public internal::SparseLUImplmatrixL().solveInPlace(X); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h index a5158025c..1ffa7d54e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h @@ -70,23 +70,30 @@ Index SparseLUImpl::expand(VectorType& vec, Index& length, Index if(num_expansions == 0 || keep_prev) new_len = length ; // First time allocate requested else - new_len = Index(alpha * length); + new_len = (std::max)(length+1,Index(alpha * length)); VectorType old_vec; // Temporary vector to hold the previous values if (nbElts > 0 ) old_vec = vec.segment(0,nbElts); //Allocate or expand the current vector - try +#ifdef EIGEN_EXCEPTIONS + try +#endif { vec.resize(new_len); } +#ifdef EIGEN_EXCEPTIONS catch(std::bad_alloc& ) +#else + if(!vec.size()) +#endif { - if ( !num_expansions ) + if (!num_expansions) { // First time to allocate from LUMemInit() - throw; // Pass the exception to LUMemInit() which has a try... catch block + // Let LUMemInit() deals with it. + return -1; } if (keep_prev) { @@ -100,12 +107,18 @@ Index SparseLUImpl::expand(VectorType& vec, Index& length, Index do { alpha = (alpha + 1)/2; - new_len = Index(alpha * length); + new_len = (std::max)(length+1,Index(alpha * length)); +#ifdef EIGEN_EXCEPTIONS try +#endif { vec.resize(new_len); } +#ifdef EIGEN_EXCEPTIONS catch(std::bad_alloc& ) +#else + if (!vec.size()) +#endif { tries += 1; if ( tries > 10) return new_len; @@ -139,10 +152,9 @@ template Index SparseLUImpl::memInit(Index m, Index n, Index annz, Index lwork, Index fillratio, Index panel_size, GlobalLU_t& glu) { Index& num_expansions = glu.num_expansions; //No memory expansions so far - num_expansions = 0; - glu.nzumax = glu.nzlumax = (std::max)(fillratio * annz, m*n); // estimated number of nonzeros in U + num_expansions = 0; + glu.nzumax = glu.nzlumax = (std::min)(fillratio * annz / n, m) * n; // estimated number of nonzeros in U glu.nzlmax = (std::max)(Index(4), fillratio) * annz / 4; // estimated nnz in L factor - // Return the estimated size to the user if necessary Index tempSpace; tempSpace = (2*panel_size + 4 + LUNoMarker) * m * sizeof(Index) + (panel_size + 1) * m * sizeof(Scalar); @@ -166,14 +178,10 @@ Index SparseLUImpl::memInit(Index m, Index n, Index annz, Index lw // Reserve memory for L/U factors do { - try - { - expand(glu.lusup, glu.nzlumax, 0, 0, num_expansions); - expand(glu.ucol,glu.nzumax, 0, 0, num_expansions); - expand(glu.lsub,glu.nzlmax, 0, 0, num_expansions); - expand(glu.usub,glu.nzumax, 0, 1, num_expansions); - } - catch(std::bad_alloc& ) + if( (expand(glu.lusup, glu.nzlumax, 0, 0, num_expansions)<0) + || (expand(glu.ucol, glu.nzumax, 0, 0, num_expansions)<0) + || (expand (glu.lsub, glu.nzlmax, 0, 0, num_expansions)<0) + || (expand (glu.usub, glu.nzumax, 0, 1, num_expansions)<0) ) { //Reduce the estimated size and retry glu.nzlumax /= 2; @@ -181,10 +189,7 @@ Index SparseLUImpl::memInit(Index m, Index n, Index annz, Index lw glu.nzlmax /= 2; if (glu.nzlumax < annz ) return glu.nzlumax; } - } while (!glu.lusup.size() || !glu.ucol.size() || !glu.lsub.size() || !glu.usub.size()); - - ++num_expansions; return 0; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h index 07c46e4b9..afda43bfc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h @@ -161,8 +161,9 @@ class SparseQR b = y; // Solve with the triangular matrix R + y.resize((std::max)(cols(),Index(y.rows())),y.cols()); y.topRows(rank) = this->matrixR().topLeftCorner(rank, rank).template triangularView().solve(b.topRows(rank)); - y.bottomRows(y.size()-rank).setZero(); + y.bottomRows(y.rows()-rank).setZero(); // Apply the column permutation if (m_perm_c.size()) dest.topRows(cols()) = colsPermutation() * y.topRows(cols()); @@ -246,7 +247,7 @@ class SparseQR Index m_nonzeropivots; // Number of non zero pivots found IndexVector m_etree; // Column elimination tree IndexVector m_firstRowElt; // First element in each row - bool m_isQSorted; // whether Q is sorted or not + bool m_isQSorted; // whether Q is sorted or not template friend struct SparseQR_QProduct; template friend struct SparseQRMatrixQReturnType; @@ -338,7 +339,7 @@ void SparseQR::factorize(const MatrixType& mat) Index nonzeroCol = 0; // Record the number of valid pivots // Left looking rank-revealing QR factorization: compute a column of R and Q at a time - for (Index col = 0; col < n; ++col) + for (Index col = 0; col < (std::min)(n,m); ++col) { mark.setConstant(-1); m_R.startVec(col); @@ -346,7 +347,7 @@ void SparseQR::factorize(const MatrixType& mat) mark(nonzeroCol) = col; Qidx(0) = nonzeroCol; nzcolR = 0; nzcolQ = 1; - found_diag = false; + found_diag = col>=m; tval.setZero(); // Symbolic factorization: find the nonzero locations of the column k of the factors R and Q, i.e., @@ -571,6 +572,7 @@ struct SparseQR_QProduct : ReturnByValue topRightCorner() const /** \returns an expression of a top-right corner of *this. * - * \tparam CRows number of rows in corner as specified at compile time - * \tparam CCols number of columns in corner as specified at compile time - * \param cRows number of rows in corner as specified at run time - * \param cCols number of columns in corner as specified at run time + * \tparam CRows number of rows in corner as specified at compile-time + * \tparam CCols number of columns in corner as specified at compile-time + * \param cRows number of rows in corner as specified at run-time + * \param cCols number of columns in corner as specified at run-time * - * This function is mainly useful for corners where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for corners where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a cRows should equal \a CRows unless * \a CRows is \a Dynamic, and the same for the number of columns. * @@ -188,13 +188,13 @@ inline const Block topLeftCorner() const /** \returns an expression of a top-left corner of *this. * - * \tparam CRows number of rows in corner as specified at compile time - * \tparam CCols number of columns in corner as specified at compile time - * \param cRows number of rows in corner as specified at run time - * \param cCols number of columns in corner as specified at run time + * \tparam CRows number of rows in corner as specified at compile-time + * \tparam CCols number of columns in corner as specified at compile-time + * \param cRows number of rows in corner as specified at run-time + * \param cCols number of columns in corner as specified at run-time * - * This function is mainly useful for corners where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for corners where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a cRows should equal \a CRows unless * \a CRows is \a Dynamic, and the same for the number of columns. * @@ -263,13 +263,13 @@ inline const Block bottomRightCorner() const /** \returns an expression of a bottom-right corner of *this. * - * \tparam CRows number of rows in corner as specified at compile time - * \tparam CCols number of columns in corner as specified at compile time - * \param cRows number of rows in corner as specified at run time - * \param cCols number of columns in corner as specified at run time + * \tparam CRows number of rows in corner as specified at compile-time + * \tparam CCols number of columns in corner as specified at compile-time + * \param cRows number of rows in corner as specified at run-time + * \param cCols number of columns in corner as specified at run-time * - * This function is mainly useful for corners where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for corners where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a cRows should equal \a CRows unless * \a CRows is \a Dynamic, and the same for the number of columns. * @@ -338,13 +338,13 @@ inline const Block bottomLeftCorner() const /** \returns an expression of a bottom-left corner of *this. * - * \tparam CRows number of rows in corner as specified at compile time - * \tparam CCols number of columns in corner as specified at compile time - * \param cRows number of rows in corner as specified at run time - * \param cCols number of columns in corner as specified at run time + * \tparam CRows number of rows in corner as specified at compile-time + * \tparam CCols number of columns in corner as specified at compile-time + * \param cRows number of rows in corner as specified at run-time + * \param cCols number of columns in corner as specified at run-time * - * This function is mainly useful for corners where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for corners where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a cRows should equal \a CRows unless * \a CRows is \a Dynamic, and the same for the number of columns. * @@ -390,7 +390,11 @@ inline ConstRowsBlockXpr topRows(Index n) const /** \returns a block consisting of the top rows of *this. * - * \tparam N the number of rows in the block + * \tparam N the number of rows in the block as specified at compile-time + * \param n the number of rows in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_topRows.cpp * Output: \verbinclude MatrixBase_template_int_topRows.out @@ -398,16 +402,16 @@ inline ConstRowsBlockXpr topRows(Index n) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NRowsBlockXpr::Type topRows() +inline typename NRowsBlockXpr::Type topRows(Index n = N) { - return typename NRowsBlockXpr::Type(derived(), 0, 0, N, cols()); + return typename NRowsBlockXpr::Type(derived(), 0, 0, n, cols()); } /** This is the const version of topRows().*/ template -inline typename ConstNRowsBlockXpr::Type topRows() const +inline typename ConstNRowsBlockXpr::Type topRows(Index n = N) const { - return typename ConstNRowsBlockXpr::Type(derived(), 0, 0, N, cols()); + return typename ConstNRowsBlockXpr::Type(derived(), 0, 0, n, cols()); } @@ -434,7 +438,11 @@ inline ConstRowsBlockXpr bottomRows(Index n) const /** \returns a block consisting of the bottom rows of *this. * - * \tparam N the number of rows in the block + * \tparam N the number of rows in the block as specified at compile-time + * \param n the number of rows in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_bottomRows.cpp * Output: \verbinclude MatrixBase_template_int_bottomRows.out @@ -442,16 +450,16 @@ inline ConstRowsBlockXpr bottomRows(Index n) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NRowsBlockXpr::Type bottomRows() +inline typename NRowsBlockXpr::Type bottomRows(Index n = N) { - return typename NRowsBlockXpr::Type(derived(), rows() - N, 0, N, cols()); + return typename NRowsBlockXpr::Type(derived(), rows() - n, 0, n, cols()); } /** This is the const version of bottomRows().*/ template -inline typename ConstNRowsBlockXpr::Type bottomRows() const +inline typename ConstNRowsBlockXpr::Type bottomRows(Index n = N) const { - return typename ConstNRowsBlockXpr::Type(derived(), rows() - N, 0, N, cols()); + return typename ConstNRowsBlockXpr::Type(derived(), rows() - n, 0, n, cols()); } @@ -459,28 +467,32 @@ inline typename ConstNRowsBlockXpr::Type bottomRows() const /** \returns a block consisting of a range of rows of *this. * * \param startRow the index of the first row in the block - * \param numRows the number of rows in the block + * \param n the number of rows in the block * * Example: \include DenseBase_middleRows_int.cpp * Output: \verbinclude DenseBase_middleRows_int.out * * \sa class Block, block(Index,Index,Index,Index) */ -inline RowsBlockXpr middleRows(Index startRow, Index numRows) +inline RowsBlockXpr middleRows(Index startRow, Index n) { - return RowsBlockXpr(derived(), startRow, 0, numRows, cols()); + return RowsBlockXpr(derived(), startRow, 0, n, cols()); } /** This is the const version of middleRows(Index,Index).*/ -inline ConstRowsBlockXpr middleRows(Index startRow, Index numRows) const +inline ConstRowsBlockXpr middleRows(Index startRow, Index n) const { - return ConstRowsBlockXpr(derived(), startRow, 0, numRows, cols()); + return ConstRowsBlockXpr(derived(), startRow, 0, n, cols()); } /** \returns a block consisting of a range of rows of *this. * - * \tparam N the number of rows in the block + * \tparam N the number of rows in the block as specified at compile-time * \param startRow the index of the first row in the block + * \param n the number of rows in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include DenseBase_template_int_middleRows.cpp * Output: \verbinclude DenseBase_template_int_middleRows.out @@ -488,16 +500,16 @@ inline ConstRowsBlockXpr middleRows(Index startRow, Index numRows) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NRowsBlockXpr::Type middleRows(Index startRow) +inline typename NRowsBlockXpr::Type middleRows(Index startRow, Index n = N) { - return typename NRowsBlockXpr::Type(derived(), startRow, 0, N, cols()); + return typename NRowsBlockXpr::Type(derived(), startRow, 0, n, cols()); } /** This is the const version of middleRows().*/ template -inline typename ConstNRowsBlockXpr::Type middleRows(Index startRow) const +inline typename ConstNRowsBlockXpr::Type middleRows(Index startRow, Index n = N) const { - return typename ConstNRowsBlockXpr::Type(derived(), startRow, 0, N, cols()); + return typename ConstNRowsBlockXpr::Type(derived(), startRow, 0, n, cols()); } @@ -524,7 +536,11 @@ inline ConstColsBlockXpr leftCols(Index n) const /** \returns a block consisting of the left columns of *this. * - * \tparam N the number of columns in the block + * \tparam N the number of columns in the block as specified at compile-time + * \param n the number of columns in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_leftCols.cpp * Output: \verbinclude MatrixBase_template_int_leftCols.out @@ -532,16 +548,16 @@ inline ConstColsBlockXpr leftCols(Index n) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NColsBlockXpr::Type leftCols() +inline typename NColsBlockXpr::Type leftCols(Index n = N) { - return typename NColsBlockXpr::Type(derived(), 0, 0, rows(), N); + return typename NColsBlockXpr::Type(derived(), 0, 0, rows(), n); } /** This is the const version of leftCols().*/ template -inline typename ConstNColsBlockXpr::Type leftCols() const +inline typename ConstNColsBlockXpr::Type leftCols(Index n = N) const { - return typename ConstNColsBlockXpr::Type(derived(), 0, 0, rows(), N); + return typename ConstNColsBlockXpr::Type(derived(), 0, 0, rows(), n); } @@ -568,7 +584,11 @@ inline ConstColsBlockXpr rightCols(Index n) const /** \returns a block consisting of the right columns of *this. * - * \tparam N the number of columns in the block + * \tparam N the number of columns in the block as specified at compile-time + * \param n the number of columns in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_rightCols.cpp * Output: \verbinclude MatrixBase_template_int_rightCols.out @@ -576,16 +596,16 @@ inline ConstColsBlockXpr rightCols(Index n) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NColsBlockXpr::Type rightCols() +inline typename NColsBlockXpr::Type rightCols(Index n = N) { - return typename NColsBlockXpr::Type(derived(), 0, cols() - N, rows(), N); + return typename NColsBlockXpr::Type(derived(), 0, cols() - n, rows(), n); } /** This is the const version of rightCols().*/ template -inline typename ConstNColsBlockXpr::Type rightCols() const +inline typename ConstNColsBlockXpr::Type rightCols(Index n = N) const { - return typename ConstNColsBlockXpr::Type(derived(), 0, cols() - N, rows(), N); + return typename ConstNColsBlockXpr::Type(derived(), 0, cols() - n, rows(), n); } @@ -613,8 +633,12 @@ inline ConstColsBlockXpr middleCols(Index startCol, Index numCols) const /** \returns a block consisting of a range of columns of *this. * - * \tparam N the number of columns in the block + * \tparam N the number of columns in the block as specified at compile-time * \param startCol the index of the first column in the block + * \param n the number of columns in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include DenseBase_template_int_middleCols.cpp * Output: \verbinclude DenseBase_template_int_middleCols.out @@ -622,16 +646,16 @@ inline ConstColsBlockXpr middleCols(Index startCol, Index numCols) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NColsBlockXpr::Type middleCols(Index startCol) +inline typename NColsBlockXpr::Type middleCols(Index startCol, Index n = N) { - return typename NColsBlockXpr::Type(derived(), 0, startCol, rows(), N); + return typename NColsBlockXpr::Type(derived(), 0, startCol, rows(), n); } /** This is the const version of middleCols().*/ template -inline typename ConstNColsBlockXpr::Type middleCols(Index startCol) const +inline typename ConstNColsBlockXpr::Type middleCols(Index startCol, Index n = N) const { - return typename ConstNColsBlockXpr::Type(derived(), 0, startCol, rows(), N); + return typename ConstNColsBlockXpr::Type(derived(), 0, startCol, rows(), n); } @@ -667,15 +691,15 @@ inline const Block block(Index startRow, In /** \returns an expression of a block in *this. * - * \tparam BlockRows number of rows in block as specified at compile time - * \tparam BlockCols number of columns in block as specified at compile time + * \tparam BlockRows number of rows in block as specified at compile-time + * \tparam BlockCols number of columns in block as specified at compile-time * \param startRow the first row in the block * \param startCol the first column in the block - * \param blockRows number of rows in block as specified at run time - * \param blockCols number of columns in block as specified at run time + * \param blockRows number of rows in block as specified at run-time + * \param blockCols number of columns in block as specified at run-time * - * This function is mainly useful for blocks where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for blocks where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a blockRows should equal \a BlockRows unless * \a BlockRows is \a Dynamic, and the same for the number of columns. * @@ -738,7 +762,7 @@ inline ConstRowXpr row(Index i) const * \only_for_vectors * * \param start the first coefficient in the segment - * \param vecSize the number of coefficients in the segment + * \param n the number of coefficients in the segment * * Example: \include MatrixBase_segment_int_int.cpp * Output: \verbinclude MatrixBase_segment_int_int.out @@ -749,25 +773,25 @@ inline ConstRowXpr row(Index i) const * * \sa class Block, segment(Index) */ -inline SegmentReturnType segment(Index start, Index vecSize) +inline SegmentReturnType segment(Index start, Index n) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return SegmentReturnType(derived(), start, vecSize); + return SegmentReturnType(derived(), start, n); } /** This is the const version of segment(Index,Index).*/ -inline ConstSegmentReturnType segment(Index start, Index vecSize) const +inline ConstSegmentReturnType segment(Index start, Index n) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return ConstSegmentReturnType(derived(), start, vecSize); + return ConstSegmentReturnType(derived(), start, n); } /** \returns a dynamic-size expression of the first coefficients of *this. * * \only_for_vectors * - * \param vecSize the number of coefficients in the block + * \param n the number of coefficients in the segment * * Example: \include MatrixBase_start_int.cpp * Output: \verbinclude MatrixBase_start_int.out @@ -778,25 +802,24 @@ inline ConstSegmentReturnType segment(Index start, Index vecSize) const * * \sa class Block, block(Index,Index) */ -inline SegmentReturnType head(Index vecSize) +inline SegmentReturnType head(Index n) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return SegmentReturnType(derived(), 0, vecSize); + return SegmentReturnType(derived(), 0, n); } /** This is the const version of head(Index).*/ -inline ConstSegmentReturnType - head(Index vecSize) const +inline ConstSegmentReturnType head(Index n) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return ConstSegmentReturnType(derived(), 0, vecSize); + return ConstSegmentReturnType(derived(), 0, n); } /** \returns a dynamic-size expression of the last coefficients of *this. * * \only_for_vectors * - * \param vecSize the number of coefficients in the block + * \param n the number of coefficients in the segment * * Example: \include MatrixBase_end_int.cpp * Output: \verbinclude MatrixBase_end_int.out @@ -807,95 +830,106 @@ inline ConstSegmentReturnType * * \sa class Block, block(Index,Index) */ -inline SegmentReturnType tail(Index vecSize) +inline SegmentReturnType tail(Index n) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return SegmentReturnType(derived(), this->size() - vecSize, vecSize); + return SegmentReturnType(derived(), this->size() - n, n); } /** This is the const version of tail(Index).*/ -inline ConstSegmentReturnType tail(Index vecSize) const +inline ConstSegmentReturnType tail(Index n) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return ConstSegmentReturnType(derived(), this->size() - vecSize, vecSize); + return ConstSegmentReturnType(derived(), this->size() - n, n); } /** \returns a fixed-size expression of a segment (i.e. a vector block) in \c *this * * \only_for_vectors * - * The template parameter \a Size is the number of coefficients in the block + * \tparam N the number of coefficients in the segment as specified at compile-time + * \param start the index of the first element in the segment + * \param n the number of coefficients in the segment as specified at compile-time * - * \param start the index of the first element of the sub-vector + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_segment.cpp * Output: \verbinclude MatrixBase_template_int_segment.out * * \sa class Block */ -template -inline typename FixedSegmentReturnType::Type segment(Index start) +template +inline typename FixedSegmentReturnType::Type segment(Index start, Index n = N) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename FixedSegmentReturnType::Type(derived(), start); + return typename FixedSegmentReturnType::Type(derived(), start, n); } /** This is the const version of segment(Index).*/ -template -inline typename ConstFixedSegmentReturnType::Type segment(Index start) const +template +inline typename ConstFixedSegmentReturnType::Type segment(Index start, Index n = N) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename ConstFixedSegmentReturnType::Type(derived(), start); + return typename ConstFixedSegmentReturnType::Type(derived(), start, n); } /** \returns a fixed-size expression of the first coefficients of *this. * * \only_for_vectors * - * The template parameter \a Size is the number of coefficients in the block + * \tparam N the number of coefficients in the segment as specified at compile-time + * \param n the number of coefficients in the segment as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_start.cpp * Output: \verbinclude MatrixBase_template_int_start.out * * \sa class Block */ -template -inline typename FixedSegmentReturnType::Type head() +template +inline typename FixedSegmentReturnType::Type head(Index n = N) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename FixedSegmentReturnType::Type(derived(), 0); + return typename FixedSegmentReturnType::Type(derived(), 0, n); } /** This is the const version of head().*/ -template -inline typename ConstFixedSegmentReturnType::Type head() const +template +inline typename ConstFixedSegmentReturnType::Type head(Index n = N) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename ConstFixedSegmentReturnType::Type(derived(), 0); + return typename ConstFixedSegmentReturnType::Type(derived(), 0, n); } /** \returns a fixed-size expression of the last coefficients of *this. * * \only_for_vectors * - * The template parameter \a Size is the number of coefficients in the block + * \tparam N the number of coefficients in the segment as specified at compile-time + * \param n the number of coefficients in the segment as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_end.cpp * Output: \verbinclude MatrixBase_template_int_end.out * * \sa class Block */ -template -inline typename FixedSegmentReturnType::Type tail() +template +inline typename FixedSegmentReturnType::Type tail(Index n = N) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename FixedSegmentReturnType::Type(derived(), size() - Size); + return typename FixedSegmentReturnType::Type(derived(), size() - n); } /** This is the const version of tail.*/ -template -inline typename ConstFixedSegmentReturnType::Type tail() const +template +inline typename ConstFixedSegmentReturnType::Type tail(Index n = N) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename ConstFixedSegmentReturnType::Type(derived(), size() - Size); + return typename ConstFixedSegmentReturnType::Type(derived(), size() - n); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h b/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h index 3a737df7b..7f62149e0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h @@ -83,7 +83,7 @@ cwiseMin(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const EIGEN_STRONG_INLINE const CwiseBinaryOp, const Derived, const ConstantReturnType> cwiseMin(const Scalar &other) const { - return cwiseMin(Derived::PlainObject::Constant(rows(), cols(), other)); + return cwiseMin(Derived::Constant(rows(), cols(), other)); } /** \returns an expression of the coefficient-wise max of *this and \a other @@ -107,7 +107,7 @@ cwiseMax(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const EIGEN_STRONG_INLINE const CwiseBinaryOp, const Derived, const ConstantReturnType> cwiseMax(const Scalar &other) const { - return cwiseMax(Derived::PlainObject::Constant(rows(), cols(), other)); + return cwiseMax(Derived::Constant(rows(), cols(), other)); } diff --git a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt index c35a2fdbe..a9bc05137 100644 --- a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt @@ -7,6 +7,9 @@ workaround_9220(Fortran EIGEN_Fortran_COMPILER_WORKS) if(EIGEN_Fortran_COMPILER_WORKS) enable_language(Fortran OPTIONAL) + if(NOT CMAKE_Fortran_COMPILER) + set(EIGEN_Fortran_COMPILER_WORKS OFF) + endif() endif() add_custom_target(blas) diff --git a/gtsam/3rdparty/Eigen/cmake/language_support.cmake b/gtsam/3rdparty/Eigen/cmake/language_support.cmake index 2ca303c92..d687b71f6 100644 --- a/gtsam/3rdparty/Eigen/cmake/language_support.cmake +++ b/gtsam/3rdparty/Eigen/cmake/language_support.cmake @@ -23,7 +23,7 @@ function(workaround_9220 language language_works) #message("DEBUG: language = ${language}") set(text "project(test NONE) - cmake_minimum_required(VERSION 2.6.0) + cmake_minimum_required(VERSION 2.8.0) set (CMAKE_Fortran_FLAGS \"${CMAKE_Fortran_FLAGS}\") set (CMAKE_EXE_LINKER_FLAGS \"${CMAKE_EXE_LINKER_FLAGS}\") enable_language(${language} OPTIONAL) diff --git a/gtsam/3rdparty/Eigen/doc/A05_PortingFrom2To3.dox b/gtsam/3rdparty/Eigen/doc/A05_PortingFrom2To3.dox index d885b4f6d..4d5f3ae1f 100644 --- a/gtsam/3rdparty/Eigen/doc/A05_PortingFrom2To3.dox +++ b/gtsam/3rdparty/Eigen/doc/A05_PortingFrom2To3.dox @@ -2,6 +2,8 @@ namespace Eigen { /** \page Eigen2ToEigen3 Porting from Eigen2 to Eigen3 +
Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3.
+ This page lists the most important API changes between Eigen2 and Eigen3, and gives tips to help porting your application from Eigen2 to Eigen3. diff --git a/gtsam/3rdparty/Eigen/doc/A10_Eigen2SupportModes.dox b/gtsam/3rdparty/Eigen/doc/A10_Eigen2SupportModes.dox index abfdb4683..f3df91515 100644 --- a/gtsam/3rdparty/Eigen/doc/A10_Eigen2SupportModes.dox +++ b/gtsam/3rdparty/Eigen/doc/A10_Eigen2SupportModes.dox @@ -2,6 +2,8 @@ namespace Eigen { /** \page Eigen2SupportModes Eigen 2 support modes +
Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3.
+ This page documents the Eigen2 support modes, a powerful tool to help migrating your project from Eigen 2 to Eigen 3. Don't miss our page on \ref Eigen2ToEigen3 "API changes" between Eigen 2 and Eigen 3. diff --git a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt index 6b0a7cd6a..694d32484 100644 --- a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt +++ b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt @@ -16,8 +16,8 @@ double s; // Basic usage // Eigen // Matlab // comments x.size() // length(x) // vector size -C.rows() // size(C)(1) // number of rows -C.cols() // size(C)(2) // number of columns +C.rows() // size(C,1) // number of rows +C.cols() // size(C,2) // number of columns x(i) // x(i+1) // Matlab is 1-based C(i,j) // C(i+1,j+1) // @@ -51,20 +51,34 @@ v.setLinSpace(size,low,high) // v = linspace(low,high,size)' // Eigen // Matlab x.head(n) // x(1:n) x.head() // x(1:n) -x.tail(n) // N = rows(x); x(N - n: N) -x.tail() // N = rows(x); x(N - n: N) +x.tail(n) // x(end - n + 1: end) +x.tail() // x(end - n + 1: end) x.segment(i, n) // x(i+1 : i+n) x.segment(i) // x(i+1 : i+n) P.block(i, j, rows, cols) // P(i+1 : i+rows, j+1 : j+cols) P.block(i, j) // P(i+1 : i+rows, j+1 : j+cols) +P.row(i) // P(i+1, :) +P.col(j) // P(:, j+1) +P.leftCols() // P(:, 1:cols) +P.leftCols(cols) // P(:, 1:cols) +P.middleCols(j) // P(:, j+1:j+cols) +P.middleCols(j, cols) // P(:, j+1:j+cols) +P.rightCols() // P(:, end-cols+1:end) +P.rightCols(cols) // P(:, end-cols+1:end) +P.topRows() // P(1:rows, :) +P.topRows(rows) // P(1:rows, :) +P.middleRows(i) // P(:, i+1:i+rows) +P.middleRows(i, rows) // P(:, i+1:i+rows) +P.bottomRows() // P(:, end-rows+1:end) +P.bottomRows(rows) // P(:, end-rows+1:end) P.topLeftCorner(rows, cols) // P(1:rows, 1:cols) -P.topRightCorner(rows, cols) // [m n]=size(P); P(1:rows, n-cols+1:n) -P.bottomLeftCorner(rows, cols) // [m n]=size(P); P(m-rows+1:m, 1:cols) -P.bottomRightCorner(rows, cols) // [m n]=size(P); P(m-rows+1:m, n-cols+1:n) +P.topRightCorner(rows, cols) // P(1:rows, end-cols+1:end) +P.bottomLeftCorner(rows, cols) // P(end-rows+1:end, 1:cols) +P.bottomRightCorner(rows, cols) // P(end-rows+1:end, end-cols+1:end) P.topLeftCorner() // P(1:rows, 1:cols) -P.topRightCorner() // [m n]=size(P); P(1:rows, n-cols+1:n) -P.bottomLeftCorner() // [m n]=size(P); P(m-rows+1:m, 1:cols) -P.bottomRightCorner() // [m n]=size(P); P(m-rows+1:m, n-cols+1:n) +P.topRightCorner() // P(1:rows, end-cols+1:end) +P.bottomLeftCorner() // P(end-rows+1:end, 1:cols) +P.bottomRightCorner() // P(end-rows+1:end, end-cols+1:end) // Of particular note is Eigen's swap function which is highly optimized. // Eigen // Matlab @@ -125,10 +139,8 @@ int r, c; // Eigen // Matlab R.minCoeff() // min(R(:)) R.maxCoeff() // max(R(:)) -s = R.minCoeff(&r, &c) // [aa, bb] = min(R); [cc, dd] = min(aa); - // r = bb(dd); c = dd; s = cc -s = R.maxCoeff(&r, &c) // [aa, bb] = max(R); [cc, dd] = max(aa); - // row = bb(dd); col = dd; s = cc +s = R.minCoeff(&r, &c) // [s, i] = min(R(:)); [r, c] = ind2sub(size(R), i); +s = R.maxCoeff(&r, &c) // [s, i] = max(R(:)); [r, c] = ind2sub(size(R), i); R.sum() // sum(R(:)) R.colwise().sum() // sum(R) R.rowwise().sum() // sum(R, 2) or sum(R')' @@ -150,13 +162,25 @@ x.squaredNorm() // dot(x, x) Note the equivalence is not true for co x.dot(y) // dot(x, y) x.cross(y) // cross(x, y) Requires #include +//// Type conversion +// Eigen // Matlab +A.cast(); // double(A) +A.cast(); // single(A) +A.cast(); // int32(A) +// if the original type equals destination type, no work is done + +// Note that for most operations Eigen requires all operands to have the same type: +MatrixXf F = MatrixXf::Zero(3,3); +A += F; // illegal in Eigen. In Matlab A = A+F is allowed +A += F.cast(); // F converted to double and then added (generally, conversion happens on-the-fly) + // Eigen can map existing memory into Eigen matrices. float array[3]; -Map(array, 3).fill(10); -int data[4] = 1, 2, 3, 4; -Matrix2i mat2x2(data); -MatrixXi mat2x2 = Map(data); -MatrixXi mat2x2 = Map(data, 2, 2); +Vector3f::Map(array).fill(10); // create a temporary Map over array and sets entries to 10 +int data[4] = {1, 2, 3, 4}; +Matrix2i mat2x2(data); // copies data into mat2x2 +Matrix2i::Map(data) = 2*mat2x2; // overwrite elements of data with 2*mat2x2 +MatrixXi::Map(data, 2, 2) += mat2x2; // adds mat2x2 to elements of data (alternative syntax if size is not know at compile time) // Solve Ax = b. Result stored in x. Matlab: x = A \ b. x = A.ldlt().solve(b)); // A sym. p.s.d. #include diff --git a/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox b/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox index eedd5524a..981083e96 100644 --- a/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox +++ b/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox @@ -64,9 +64,9 @@ run time. However, these assertions do cost time and can thus be turned off. \c EIGEN_DONT_ALIGN is defined. - \b EIGEN_DONT_VECTORIZE - disables explicit vectorization when defined. Not defined by default, unless alignment is disabled by %Eigen's platform test or the user defining \c EIGEN_DONT_ALIGN. - - \b EIGEN_FAST_MATH - enables some optimizations which might affect the accuracy of the result. The only - optimization this currently includes is single precision sin() and cos() in the present of SSE - vectorization. Defined by default. + - \b EIGEN_FAST_MATH - enables some optimizations which might affect the accuracy of the result. This currently + enables the SSE vectorization of sin() and cos(), and speedups sqrt() for single precision. Defined to 1 by default. + Define it to 0 to disable. - \b EIGEN_UNROLLING_LIMIT - defines the size of a loop to enable meta unrolling. Set it to zero to disable unrolling. The size of a loop here is expressed in %Eigen's own notion of "number of FLOPS", it does not correspond to the number of iterations or the number of instructions. The default is value 100. diff --git a/gtsam/3rdparty/Eigen/doc/QuickReference.dox b/gtsam/3rdparty/Eigen/doc/QuickReference.dox index 5e0168649..a4be0f68a 100644 --- a/gtsam/3rdparty/Eigen/doc/QuickReference.dox +++ b/gtsam/3rdparty/Eigen/doc/QuickReference.dox @@ -405,19 +405,19 @@ array1 == array2 array1 != array2 array1 == scalar array1 != scalar array1.min(array2) array1.max(array2) array1.abs2() -array1.abs() std::abs(array1) -array1.sqrt() std::sqrt(array1) -array1.log() std::log(array1) -array1.exp() std::exp(array1) -array1.pow(exponent) std::pow(array1,exponent) +array1.abs() abs(array1) +array1.sqrt() sqrt(array1) +array1.log() log(array1) +array1.exp() exp(array1) +array1.pow(exponent) pow(array1,exponent) array1.square() array1.cube() array1.inverse() -array1.sin() std::sin(array1) -array1.cos() std::cos(array1) -array1.tan() std::tan(array1) -array1.asin() std::asin(array1) -array1.acos() std::acos(array1) +array1.sin() sin(array1) +array1.cos() cos(array1) +array1.tan() tan(array1) +array1.asin() asin(array1) +array1.acos() acos(array1) \endcode diff --git a/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox b/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox index 81aeec978..372a275de 100644 --- a/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox +++ b/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox @@ -127,7 +127,7 @@ VectorNf vec1, vec2; vec2 = t.linear() * vec1;\endcode Apply a \em general transformation \n to a \b normal \b vector -(explanations)\code +(explanations)\code VectorNf n1, n2; MatrixNf normalMatrix = t.linear().inverse().transpose(); n2 = (normalMatrix * n1).normalized();\endcode diff --git a/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox b/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox index dbfb4a9eb..835c59354 100644 --- a/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox +++ b/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox @@ -83,7 +83,7 @@ There is no notion of compressed/uncompressed mode for a SparseVector. \section TutorialSparseExample First example -Before describing each individual class, let's start with the following typical example: solving the Lapace equation \f$ \nabla u = 0 \f$ on a regular 2D grid using a finite difference scheme and Dirichlet boundary conditions. +Before describing each individual class, let's start with the following typical example: solving the Laplace equation \f$ \nabla u = 0 \f$ on a regular 2D grid using a finite difference scheme and Dirichlet boundary conditions. Such problem can be mathematically expressed as a linear problem of the form \f$ Ax=b \f$ where \f$ x \f$ is the vector of \c m unknowns (in our case, the values of the pixels), \f$ b \f$ is the right hand side vector resulting from the boundary conditions, and \f$ A \f$ is an \f$ m \times m \f$ matrix containing only a few non-zero elements resulting from the discretization of the Laplacian operator. diff --git a/gtsam/3rdparty/Eigen/doc/eigendoxy.css b/gtsam/3rdparty/Eigen/doc/eigendoxy.css index 60243d870..efaeb46dc 100644 --- a/gtsam/3rdparty/Eigen/doc/eigendoxy.css +++ b/gtsam/3rdparty/Eigen/doc/eigendoxy.css @@ -199,3 +199,13 @@ h3.version { td.width20em p.endtd { width: 20em; } + +.bigwarning { + font-size:2em; + font-weight:bold; + margin:1em; + padding:1em; + color:red; + border:solid; +} + diff --git a/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in b/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in index e62af8ed5..878244a19 100644 --- a/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in +++ b/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in @@ -17,8 +17,7 @@ $generatedby   - - ---> diff --git a/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheLeft.cpp b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheLeft.cpp new file mode 100644 index 000000000..6398c873a --- /dev/null +++ b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheLeft.cpp @@ -0,0 +1,7 @@ +Matrix3f A = Matrix3f::Random(3,3), B; +B << 0,1,0, + 0,0,1, + 1,0,0; +cout << "At start, A = " << endl << A << endl; +A.applyOnTheLeft(B); +cout << "After applyOnTheLeft, A = " << endl << A << endl; diff --git a/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheRight.cpp b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheRight.cpp new file mode 100644 index 000000000..e4b71b2d8 --- /dev/null +++ b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheRight.cpp @@ -0,0 +1,9 @@ +Matrix3f A = Matrix3f::Random(3,3), B; +B << 0,1,0, + 0,0,1, + 1,0,0; +cout << "At start, A = " << endl << A << endl; +A *= B; +cout << "After A *= B, A = " << endl << A << endl; +A.applyOnTheRight(B); // equivalent to A *= B +cout << "After applyOnTheRight, A = " << endl << A << endl; diff --git a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt index 138a2f6ef..0c9b3c3ba 100644 --- a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt @@ -10,12 +10,12 @@ endif(NOT EIGEN_TEST_NOQT) if(QT4_FOUND) add_executable(Tutorial_sparse_example Tutorial_sparse_example.cpp Tutorial_sparse_example_details.cpp) target_link_libraries(Tutorial_sparse_example ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}) - + add_custom_command( TARGET Tutorial_sparse_example POST_BUILD - COMMAND Tutorial_sparse_example - ARGS ${CMAKE_CURRENT_BINARY_DIR}/../html/Tutorial_sparse_example.jpeg + COMMAND Tutorial_sparse_example ARGS ${CMAKE_CURRENT_BINARY_DIR}/../html/Tutorial_sparse_example.jpeg ) + add_dependencies(all_examples Tutorial_sparse_example) endif(QT4_FOUND) diff --git a/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example_details.cpp b/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example_details.cpp index 8c3020b63..7d820b44a 100644 --- a/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example_details.cpp +++ b/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example_details.cpp @@ -11,8 +11,8 @@ void insertCoefficient(int id, int i, int j, double w, std::vector& coeffs, int n = boundary.size(); int id1 = i+j*n; - if(i==-1 || i==n) b(id) -= w * boundary(j); // constrained coeffcieint - else if(j==-1 || j==n) b(id) -= w * boundary(i); // constrained coeffcieint + if(i==-1 || i==n) b(id) -= w * boundary(j); // constrained coefficient + else if(j==-1 || j==n) b(id) -= w * boundary(i); // constrained coefficient else coeffs.push_back(T(id,id1,w)); // unknown coefficient } diff --git a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt index 7e7444326..9883d4c72 100644 --- a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt @@ -7,6 +7,9 @@ workaround_9220(Fortran EIGEN_Fortran_COMPILER_WORKS) if(EIGEN_Fortran_COMPILER_WORKS) enable_language(Fortran OPTIONAL) + if(NOT CMAKE_Fortran_COMPILER) + set(EIGEN_Fortran_COMPILER_WORKS OFF) + endif() endif() add_custom_target(lapack) diff --git a/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs b/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs index 9b71cd8e0..0e6f9ada2 100644 --- a/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs +++ b/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs @@ -4,6 +4,7 @@ # You should call this script with USER set as you want, else some default # will be used USER=${USER:-'orzel'} +UPLOAD_DIR=dox #ulimit -v 1024000 @@ -14,10 +15,10 @@ mkdir build -p #step 2 : upload # (the '/' at the end of path is very important, see rsync documentation) -rsync -az --no-p --delete build/doc/html/ $USER@ssh.tuxfamily.org:eigen/eigen.tuxfamily.org-web/htdocs/dox-devel/ || { echo "upload failed"; exit 1; } +rsync -az --no-p --delete build/doc/html/ $USER@ssh.tuxfamily.org:eigen/eigen.tuxfamily.org-web/htdocs/$UPLOAD_DIR/ || { echo "upload failed"; exit 1; } #step 3 : fix the perm -ssh $USER@ssh.tuxfamily.org 'chmod -R g+w /home/eigen/eigen.tuxfamily.org-web/htdocs/dox-devel' || { echo "perm failed"; exit 1; } +ssh $USER@ssh.tuxfamily.org "chmod -R g+w /home/eigen/eigen.tuxfamily.org-web/htdocs/$UPLOAD_DIR" || { echo "perm failed"; exit 1; } echo "Uploaded successfully" diff --git a/gtsam/3rdparty/Eigen/test/array.cpp b/gtsam/3rdparty/Eigen/test/array.cpp index 8960e49f8..c607da631 100644 --- a/gtsam/3rdparty/Eigen/test/array.cpp +++ b/gtsam/3rdparty/Eigen/test/array.cpp @@ -167,21 +167,14 @@ template void array_real(const ArrayType& m) Scalar s1 = internal::random(); // these tests are mostly to check possible compilation issues. -// VERIFY_IS_APPROX(m1.sin(), std::sin(m1)); VERIFY_IS_APPROX(m1.sin(), sin(m1)); -// VERIFY_IS_APPROX(m1.cos(), std::cos(m1)); VERIFY_IS_APPROX(m1.cos(), cos(m1)); -// VERIFY_IS_APPROX(m1.asin(), std::asin(m1)); VERIFY_IS_APPROX(m1.asin(), asin(m1)); -// VERIFY_IS_APPROX(m1.acos(), std::acos(m1)); VERIFY_IS_APPROX(m1.acos(), acos(m1)); -// VERIFY_IS_APPROX(m1.tan(), std::tan(m1)); VERIFY_IS_APPROX(m1.tan(), tan(m1)); VERIFY_IS_APPROX(cos(m1+RealScalar(3)*m2), cos((m1+RealScalar(3)*m2).eval())); -// VERIFY_IS_APPROX(std::cos(m1+RealScalar(3)*m2), std::cos((m1+RealScalar(3)*m2).eval())); -// VERIFY_IS_APPROX(m1.abs().sqrt(), std::sqrt(std::abs(m1))); VERIFY_IS_APPROX(m1.abs().sqrt(), sqrt(abs(m1))); VERIFY_IS_APPROX(m1.abs(), sqrt(numext::abs2(m1))); @@ -190,9 +183,10 @@ template void array_real(const ArrayType& m) if(!NumTraits::IsComplex) VERIFY_IS_APPROX(numext::real(m1), m1); - VERIFY_IS_APPROX(m1.abs().log() , log(abs(m1))); + // shift argument of logarithm so that it is not zero + Scalar smallNumber = NumTraits::dummy_precision(); + VERIFY_IS_APPROX((m1.abs() + smallNumber).log() , log(abs(m1) + smallNumber)); -// VERIFY_IS_APPROX(m1.exp(), std::exp(m1)); VERIFY_IS_APPROX(m1.exp() * m2.exp(), exp(m1+m2)); VERIFY_IS_APPROX(m1.exp(), exp(m1)); VERIFY_IS_APPROX(m1.exp() / m2.exp(),(m1-m2).exp()); @@ -236,7 +230,6 @@ template void array_complex(const ArrayType& m) m2(i,j) = sqrt(m1(i,j)); VERIFY_IS_APPROX(m1.sqrt(), m2); -// VERIFY_IS_APPROX(m1.sqrt(), std::sqrt(m1)); VERIFY_IS_APPROX(m1.sqrt(), Eigen::sqrt(m1)); } diff --git a/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp b/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp index 1250c9ff5..9a50f99ab 100644 --- a/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp +++ b/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp @@ -163,10 +163,14 @@ template void cwise_min_max(const MatrixType& m) // min/max with scalar input VERIFY_IS_APPROX(MatrixType::Constant(rows,cols, minM1), m1.cwiseMin( minM1)); - VERIFY_IS_APPROX(m1, m1.cwiseMin( maxM1)); + VERIFY_IS_APPROX(m1, m1.cwiseMin(maxM1)); + VERIFY_IS_APPROX(-m1, (-m1).cwiseMin(-minM1)); + VERIFY_IS_APPROX(-m1.array(), ((-m1).array().min)( -minM1)); VERIFY_IS_APPROX(MatrixType::Constant(rows,cols, maxM1), m1.cwiseMax( maxM1)); - VERIFY_IS_APPROX(m1, m1.cwiseMax( minM1)); + VERIFY_IS_APPROX(m1, m1.cwiseMax(minM1)); + VERIFY_IS_APPROX(-m1, (-m1).cwiseMax(-maxM1)); + VERIFY_IS_APPROX(-m1.array(), ((-m1).array().max)(-maxM1)); VERIFY_IS_APPROX(MatrixType::Constant(rows,cols, minM1).array(), (m1.array().min)( minM1)); VERIFY_IS_APPROX(m1.array(), (m1.array().min)( maxM1)); @@ -202,6 +206,12 @@ template void resize(const MatrixTraits& t) VERIFY(a1.size()==cols); } +void regression_bug_654() +{ + ArrayXf a = RowVectorXf(3); + VectorXf v = Array(3); +} + void test_array_for_matrix() { for(int i = 0; i < g_repeat; i++) { @@ -239,4 +249,5 @@ void test_array_for_matrix() CALL_SUBTEST_5( resize(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_6( resize(MatrixXi(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); } + CALL_SUBTEST_6( regression_bug_654() ); } diff --git a/gtsam/3rdparty/Eigen/test/cholesky.cpp b/gtsam/3rdparty/Eigen/test/cholesky.cpp index 378525a83..b980dc572 100644 --- a/gtsam/3rdparty/Eigen/test/cholesky.cpp +++ b/gtsam/3rdparty/Eigen/test/cholesky.cpp @@ -263,8 +263,8 @@ template void cholesky_bug241(const MatrixType& m) // LDLT is not guaranteed to work for indefinite matrices, but happens to work fine if matrix is diagonal. // This test checks that LDLT reports correctly that matrix is indefinite. -// See http://forum.kde.org/viewtopic.php?f=74&t=106942 -template void cholesky_indefinite(const MatrixType& m) +// See http://forum.kde.org/viewtopic.php?f=74&t=106942 and bug 736 +template void cholesky_definiteness(const MatrixType& m) { eigen_assert(m.rows() == 2 && m.cols() == 2); MatrixType mat; @@ -280,6 +280,24 @@ template void cholesky_indefinite(const MatrixType& m) VERIFY(!ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } + { + mat << 0, 0, 0, 0; + LDLT ldlt(mat); + VERIFY(ldlt.isNegative()); + VERIFY(ldlt.isPositive()); + } + { + mat << 0, 0, 0, 1; + LDLT ldlt(mat); + VERIFY(!ldlt.isNegative()); + VERIFY(ldlt.isPositive()); + } + { + mat << -1, 0, 0, 0; + LDLT ldlt(mat); + VERIFY(ldlt.isNegative()); + VERIFY(!ldlt.isPositive()); + } } template void cholesky_verify_assert() @@ -309,7 +327,7 @@ void test_cholesky() CALL_SUBTEST_1( cholesky(Matrix()) ); CALL_SUBTEST_3( cholesky(Matrix2d()) ); CALL_SUBTEST_3( cholesky_bug241(Matrix2d()) ); - CALL_SUBTEST_3( cholesky_indefinite(Matrix2d()) ); + CALL_SUBTEST_3( cholesky_definiteness(Matrix2d()) ); CALL_SUBTEST_4( cholesky(Matrix3f()) ); CALL_SUBTEST_5( cholesky(Matrix4d()) ); s = internal::random(1,EIGEN_TEST_MAX_SIZE); diff --git a/gtsam/3rdparty/Eigen/test/conservative_resize.cpp b/gtsam/3rdparty/Eigen/test/conservative_resize.cpp index 2d1ab3f03..498421b4c 100644 --- a/gtsam/3rdparty/Eigen/test/conservative_resize.cpp +++ b/gtsam/3rdparty/Eigen/test/conservative_resize.cpp @@ -60,34 +60,51 @@ void run_matrix_tests() template void run_vector_tests() { - typedef Matrix MatrixType; + typedef Matrix VectorType; - MatrixType m, n; + VectorType m, n; // boundary cases ... - m = n = MatrixType::Random(50); + m = n = VectorType::Random(50); m.conservativeResize(1); VERIFY_IS_APPROX(m, n.segment(0,1)); - m = n = MatrixType::Random(50); + m = n = VectorType::Random(50); m.conservativeResize(50); VERIFY_IS_APPROX(m, n.segment(0,50)); + + m = n = VectorType::Random(50); + m.conservativeResize(m.rows(),1); + VERIFY_IS_APPROX(m, n.segment(0,1)); + + m = n = VectorType::Random(50); + m.conservativeResize(m.rows(),50); + VERIFY_IS_APPROX(m, n.segment(0,50)); // random shrinking ... for (int i=0; i<50; ++i) { const int size = internal::random(1,50); - m = n = MatrixType::Random(50); + m = n = VectorType::Random(50); m.conservativeResize(size); VERIFY_IS_APPROX(m, n.segment(0,size)); + + m = n = VectorType::Random(50); + m.conservativeResize(m.rows(), size); + VERIFY_IS_APPROX(m, n.segment(0,size)); } // random growing with zeroing ... for (int i=0; i<50; ++i) { const int size = internal::random(50,100); - m = n = MatrixType::Random(50); - m.conservativeResizeLike(MatrixType::Zero(size)); + m = n = VectorType::Random(50); + m.conservativeResizeLike(VectorType::Zero(size)); + VERIFY_IS_APPROX(m.segment(0,50), n); + VERIFY( size<=50 || m.segment(50,size-50).sum() == Scalar(0) ); + + m = n = VectorType::Random(50); + m.conservativeResizeLike(Matrix::Zero(1,size)); VERIFY_IS_APPROX(m.segment(0,50), n); VERIFY( size<=50 || m.segment(50,size-50).sum() == Scalar(0) ); } diff --git a/gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp b/gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp index 5445cd81a..b4830bd41 100644 --- a/gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp @@ -12,36 +12,48 @@ #include #include -template void check_all_var(const Matrix& ea) + +template +void verify_euler(const Matrix& ea, int i, int j, int k) { typedef Matrix Matrix3; typedef Matrix Vector3; typedef AngleAxis AngleAxisx; using std::abs; + Matrix3 m(AngleAxisx(ea[0], Vector3::Unit(i)) * AngleAxisx(ea[1], Vector3::Unit(j)) * AngleAxisx(ea[2], Vector3::Unit(k))); + Vector3 eabis = m.eulerAngles(i, j, k); + Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) * AngleAxisx(eabis[2], Vector3::Unit(k))); + VERIFY_IS_APPROX(m, mbis); + /* If I==K, and ea[1]==0, then there no unique solution. */ + /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ + if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(M_PI/2),test_precision())) ) + VERIFY((ea-eabis).norm() <= test_precision()); - #define VERIFY_EULER(I,J,K, X,Y,Z) { \ - Matrix3 m(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \ - Vector3 eabis = m.eulerAngles(I,J,K); \ - Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit##X()) * AngleAxisx(eabis[1], Vector3::Unit##Y()) * AngleAxisx(eabis[2], Vector3::Unit##Z())); \ - VERIFY_IS_APPROX(m, mbis); \ - /* If I==K, and ea[1]==0, then there no unique solution. */ \ - /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ \ - if( (I!=K || ea[1]!=0) && (I==K || !internal::isApprox(abs(ea[1]),Scalar(M_PI/2),test_precision())) ) VERIFY((ea-eabis).norm() <= test_precision()); \ - } - VERIFY_EULER(0,1,2, X,Y,Z); - VERIFY_EULER(0,1,0, X,Y,X); - VERIFY_EULER(0,2,1, X,Z,Y); - VERIFY_EULER(0,2,0, X,Z,X); + // approx_or_less_than does not work for 0 + VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1))); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(M_PI)); + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(M_PI), eabis[1]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(M_PI)); + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(M_PI), eabis[2]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(M_PI)); +} - VERIFY_EULER(1,2,0, Y,Z,X); - VERIFY_EULER(1,2,1, Y,Z,Y); - VERIFY_EULER(1,0,2, Y,X,Z); - VERIFY_EULER(1,0,1, Y,X,Y); +template void check_all_var(const Matrix& ea) +{ + verify_euler(ea, 0,1,2); + verify_euler(ea, 0,1,0); + verify_euler(ea, 0,2,1); + verify_euler(ea, 0,2,0); - VERIFY_EULER(2,0,1, Z,X,Y); - VERIFY_EULER(2,0,2, Z,X,Z); - VERIFY_EULER(2,1,0, Z,Y,X); - VERIFY_EULER(2,1,2, Z,Y,Z); + verify_euler(ea, 1,2,0); + verify_euler(ea, 1,2,1); + verify_euler(ea, 1,0,2); + verify_euler(ea, 1,0,1); + + verify_euler(ea, 2,0,1); + verify_euler(ea, 2,0,2); + verify_euler(ea, 2,1,0); + verify_euler(ea, 2,1,2); } template void eulerangles() @@ -63,7 +75,16 @@ template void eulerangles() ea = m.eulerAngles(0,1,0); check_all_var(ea); - ea = (Array3::Random() + Array3(1,1,0))*Scalar(M_PI)*Array3(0.5,0.5,1); + // Check with purely random Quaternion: + q1.coeffs() = Quaternionx::Coefficients::Random().normalized(); + m = q1; + ea = m.eulerAngles(0,1,2); + check_all_var(ea); + ea = m.eulerAngles(0,1,0); + check_all_var(ea); + + // Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi]. + ea = (Array3::Random() + Array3(1,0,0))*Scalar(M_PI)*Array3(0.5,1,1); check_all_var(ea); ea[2] = ea[0] = internal::random(0,Scalar(M_PI)); diff --git a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp index 35ae67ebe..ee3030b5d 100644 --- a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp @@ -279,6 +279,13 @@ template void transformations() t1 = Eigen::Scaling(s0,s0,s0) * t1; VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + t0 = t3; + t0.scale(s0); + t1 = t3 * Eigen::Scaling(s0); + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + t0.prescale(s0); + t1 = Eigen::Scaling(s0) * t1; + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); t0.setIdentity(); t0.prerotate(q1).prescale(v0).pretranslate(v0); diff --git a/gtsam/3rdparty/Eigen/test/product_trmm.cpp b/gtsam/3rdparty/Eigen/test/product_trmm.cpp index 506a1aeb9..d715b9a36 100644 --- a/gtsam/3rdparty/Eigen/test/product_trmm.cpp +++ b/gtsam/3rdparty/Eigen/test/product_trmm.cpp @@ -51,6 +51,7 @@ void trmm(int rows=internal::random(1,EIGEN_TEST_MAX_SIZE), ge_xs_save = ge_xs; VERIFY_IS_APPROX( (ge_xs_save + s1*triTr.conjugate() * (s2*ge_left.adjoint())).eval(), ge_xs.noalias() += (s1*mat.adjoint()).template triangularView() * (s2*ge_left.adjoint()) ); + ge_sx.setRandom(); ge_sx_save = ge_sx; VERIFY_IS_APPROX( ge_sx_save - (ge_right.adjoint() * (-s1 * triTr).conjugate()).eval(), ge_sx.noalias() -= (ge_right.adjoint() * (-s1 * mat).adjoint().template triangularView()).eval()); diff --git a/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp b/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp index 15d7299d7..511f2473f 100644 --- a/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp +++ b/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp @@ -130,4 +130,8 @@ void test_qr_fullpivoting() // Test problem size constructors CALL_SUBTEST_7(FullPivHouseholderQR(10, 20)); + CALL_SUBTEST_7((FullPivHouseholderQR >(10,20))); + CALL_SUBTEST_7((FullPivHouseholderQR >(Matrix::Random()))); + CALL_SUBTEST_7((FullPivHouseholderQR >(20,10))); + CALL_SUBTEST_7((FullPivHouseholderQR >(Matrix::Random()))); } diff --git a/gtsam/3rdparty/Eigen/test/ref.cpp b/gtsam/3rdparty/Eigen/test/ref.cpp index 65b4f5a3e..f639d900b 100644 --- a/gtsam/3rdparty/Eigen/test/ref.cpp +++ b/gtsam/3rdparty/Eigen/test/ref.cpp @@ -14,9 +14,9 @@ static int nb_temporaries; -inline void on_temporary_creation(int size) { +inline void on_temporary_creation(int) { // here's a great place to set a breakpoint when debugging failures in this test! - if(size!=0) nb_temporaries++; + nb_temporaries++; } diff --git a/gtsam/3rdparty/Eigen/test/sparse.h b/gtsam/3rdparty/Eigen/test/sparse.h index a09c65e5f..e19a76316 100644 --- a/gtsam/3rdparty/Eigen/test/sparse.h +++ b/gtsam/3rdparty/Eigen/test/sparse.h @@ -154,16 +154,16 @@ initSparse(double density, sparseMat.finalize(); } -template void +template void initSparse(double density, Matrix& refVec, - SparseVector& sparseVec, + SparseVector& sparseVec, std::vector* zeroCoords = 0, std::vector* nonzeroCoords = 0) { sparseVec.reserve(int(refVec.size()*density)); sparseVec.setZero(); - for(int i=0; i(0,1) < density) ? internal::random() : Scalar(0); if (v!=Scalar(0)) @@ -178,10 +178,10 @@ initSparse(double density, } } -template void +template void initSparse(double density, Matrix& refVec, - SparseVector& sparseVec, + SparseVector& sparseVec, std::vector* zeroCoords = 0, std::vector* nonzeroCoords = 0) { diff --git a/gtsam/3rdparty/Eigen/test/sparse_product.cpp b/gtsam/3rdparty/Eigen/test/sparse_product.cpp index 664e33887..a2ea9d5b7 100644 --- a/gtsam/3rdparty/Eigen/test/sparse_product.cpp +++ b/gtsam/3rdparty/Eigen/test/sparse_product.cpp @@ -13,8 +13,9 @@ template struct test_outer { static void run(SparseMatrixType& m2, SparseMatrixType& m4, DenseMatrix& refMat2, DenseMatrix& refMat4) { - int c = internal::random(0,m2.cols()-1); - int c1 = internal::random(0,m2.cols()-1); + typedef typename SparseMatrixType::Index Index; + Index c = internal::random(0,m2.cols()-1); + Index c1 = internal::random(0,m2.cols()-1); VERIFY_IS_APPROX(m4=m2.col(c)*refMat2.col(c1).transpose(), refMat4=refMat2.col(c)*refMat2.col(c1).transpose()); VERIFY_IS_APPROX(m4=refMat2.col(c1)*m2.col(c).transpose(), refMat4=refMat2.col(c1)*refMat2.col(c).transpose()); } @@ -22,8 +23,9 @@ template struct test_outer struct test_outer { static void run(SparseMatrixType& m2, SparseMatrixType& m4, DenseMatrix& refMat2, DenseMatrix& refMat4) { - int r = internal::random(0,m2.rows()-1); - int c1 = internal::random(0,m2.cols()-1); + typedef typename SparseMatrixType::Index Index; + Index r = internal::random(0,m2.rows()-1); + Index c1 = internal::random(0,m2.cols()-1); VERIFY_IS_APPROX(m4=m2.row(r).transpose()*refMat2.col(c1).transpose(), refMat4=refMat2.row(r).transpose()*refMat2.col(c1).transpose()); VERIFY_IS_APPROX(m4=refMat2.col(c1)*m2.row(r), refMat4=refMat2.col(c1)*refMat2.row(r)); } @@ -37,9 +39,9 @@ template void sparse_product() { typedef typename SparseMatrixType::Index Index; Index n = 100; - const Index rows = internal::random(1,n); - const Index cols = internal::random(1,n); - const Index depth = internal::random(1,n); + const Index rows = internal::random(1,n); + const Index cols = internal::random(1,n); + const Index depth = internal::random(1,n); typedef typename SparseMatrixType::Scalar Scalar; enum { Flags = SparseMatrixType::Flags }; @@ -244,6 +246,7 @@ void test_sparse_product() CALL_SUBTEST_1( (sparse_product >()) ); CALL_SUBTEST_2( (sparse_product, ColMajor > >()) ); CALL_SUBTEST_2( (sparse_product, RowMajor > >()) ); + CALL_SUBTEST_3( (sparse_product >()) ); CALL_SUBTEST_4( (sparse_product_regression_test, Matrix >()) ); } } diff --git a/gtsam/3rdparty/Eigen/test/sparse_vector.cpp b/gtsam/3rdparty/Eigen/test/sparse_vector.cpp index ec5877b6a..0c9476803 100644 --- a/gtsam/3rdparty/Eigen/test/sparse_vector.cpp +++ b/gtsam/3rdparty/Eigen/test/sparse_vector.cpp @@ -9,14 +9,14 @@ #include "sparse.h" -template void sparse_vector(int rows, int cols) +template void sparse_vector(int rows, int cols) { double densityMat = (std::max)(8./(rows*cols), 0.01); double densityVec = (std::max)(8./float(rows), 0.1); typedef Matrix DenseMatrix; typedef Matrix DenseVector; - typedef SparseVector SparseVectorType; - typedef SparseMatrix SparseMatrixType; + typedef SparseVector SparseVectorType; + typedef SparseMatrix SparseMatrixType; Scalar eps = 1e-6; SparseMatrixType m1(rows,rows); @@ -101,9 +101,10 @@ template void sparse_vector(int rows, int cols) void test_sparse_vector() { for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( sparse_vector(8, 8) ); - CALL_SUBTEST_2( sparse_vector >(16, 16) ); - CALL_SUBTEST_1( sparse_vector(299, 535) ); + CALL_SUBTEST_1(( sparse_vector(8, 8) )); + CALL_SUBTEST_2(( sparse_vector, int>(16, 16) )); + CALL_SUBTEST_1(( sparse_vector(299, 535) )); + CALL_SUBTEST_1(( sparse_vector(299, 535) )); } } diff --git a/gtsam/3rdparty/Eigen/test/stable_norm.cpp b/gtsam/3rdparty/Eigen/test/stable_norm.cpp index c09fc17b7..549f91fbf 100644 --- a/gtsam/3rdparty/Eigen/test/stable_norm.cpp +++ b/gtsam/3rdparty/Eigen/test/stable_norm.cpp @@ -55,8 +55,16 @@ template void stable_norm(const MatrixType& m) Index rows = m.rows(); Index cols = m.cols(); - Scalar big = internal::random() * ((std::numeric_limits::max)() * RealScalar(1e-4)); - Scalar small = internal::random() * ((std::numeric_limits::min)() * RealScalar(1e4)); + // get a non-zero random factor + Scalar factor = internal::random(); + while(numext::abs2(factor)(); + Scalar big = factor * ((std::numeric_limits::max)() * RealScalar(1e-4)); + + factor = internal::random(); + while(numext::abs2(factor)(); + Scalar small = factor * ((std::numeric_limits::min)() * RealScalar(1e4)); MatrixType vzero = MatrixType::Zero(rows, cols), vrand = MatrixType::Random(rows, cols), @@ -91,7 +99,7 @@ template void stable_norm(const MatrixType& m) VERIFY_IS_APPROX(vsmall.blueNorm(), sqrt(size)*abs(small)); VERIFY_IS_APPROX(vsmall.hypotNorm(), sqrt(size)*abs(small)); -// Test compilation of cwise() version + // Test compilation of cwise() version VERIFY_IS_APPROX(vrand.colwise().stableNorm(), vrand.colwise().norm()); VERIFY_IS_APPROX(vrand.colwise().blueNorm(), vrand.colwise().norm()); VERIFY_IS_APPROX(vrand.colwise().hypotNorm(), vrand.colwise().norm()); diff --git a/gtsam/3rdparty/Eigen/test/umeyama.cpp b/gtsam/3rdparty/Eigen/test/umeyama.cpp index 738d0af70..2e8092434 100644 --- a/gtsam/3rdparty/Eigen/test/umeyama.cpp +++ b/gtsam/3rdparty/Eigen/test/umeyama.cpp @@ -130,10 +130,11 @@ void run_fixed_size_test(int num_elements) // MUST be positive because in any other case det(cR_t) may become negative for // odd dimensions! - const Scalar c = abs(internal::random()); + // Also if c is to small compared to t.norm(), problem is ill-posed (cf. Bug 744) + const Scalar c = internal::random(0.5, 2.0); FixedMatrix R = randMatrixSpecialUnitary(dim); - FixedVector t = Scalar(50)*FixedVector::Random(dim,1); + FixedVector t = Scalar(32)*FixedVector::Random(dim,1); HomMatrix cR_t = HomMatrix::Identity(dim+1,dim+1); cR_t.block(0,0,dim,dim) = c*R; @@ -149,9 +150,9 @@ void run_fixed_size_test(int num_elements) HomMatrix cR_t_umeyama = umeyama(src_block, dst_block); - const Scalar error = ( cR_t_umeyama*src - dst ).array().square().sum(); + const Scalar error = ( cR_t_umeyama*src - dst ).squaredNorm(); - VERIFY(error < Scalar(10)*std::numeric_limits::epsilon()); + VERIFY(error < Scalar(16)*std::numeric_limits::epsilon()); } void test_umeyama() diff --git a/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp b/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp index 9d60b0388..6cd1acdda 100644 --- a/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp @@ -101,6 +101,16 @@ template void vectorwiseop_array(const ArrayType& m) VERIFY_RAISES_ASSERT(m2.rowwise() /= rowvec.transpose()); VERIFY_RAISES_ASSERT(m1.rowwise() / rowvec.transpose()); + + m2 = m1; + // yes, there might be an aliasing issue there but ".rowwise() /=" + // is suppposed to evaluate " m2.colwise().sum()" into to temporary to avoid + // evaluating the reducions multiple times + if(ArrayType::RowsAtCompileTime>2 || ArrayType::RowsAtCompileTime==Dynamic) + { + m2.rowwise() /= m2.colwise().sum(); + VERIFY_IS_APPROX(m2, m1.rowwise() / m1.colwise().sum()); + } } template void vectorwiseop_matrix(const MatrixType& m) diff --git a/gtsam/3rdparty/Eigen/test/zerosized.cpp b/gtsam/3rdparty/Eigen/test/zerosized.cpp index 6905e584e..da7dd0481 100644 --- a/gtsam/3rdparty/Eigen/test/zerosized.cpp +++ b/gtsam/3rdparty/Eigen/test/zerosized.cpp @@ -9,12 +9,26 @@ #include "main.h" + +template void zeroReduction(const MatrixType& m) { + // Reductions that must hold for zero sized objects + VERIFY(m.all()); + VERIFY(!m.any()); + VERIFY(m.prod()==1); + VERIFY(m.sum()==0); + VERIFY(m.count()==0); + VERIFY(m.allFinite()); + VERIFY(!m.hasNaN()); +} + + template void zeroSizedMatrix() { MatrixType t1; - if (MatrixType::SizeAtCompileTime == Dynamic) + if (MatrixType::SizeAtCompileTime == Dynamic || MatrixType::SizeAtCompileTime == 0) { + zeroReduction(t1); if (MatrixType::RowsAtCompileTime == Dynamic) VERIFY(t1.rows() == 0); if (MatrixType::ColsAtCompileTime == Dynamic) @@ -22,9 +36,13 @@ template void zeroSizedMatrix() if (MatrixType::RowsAtCompileTime == Dynamic && MatrixType::ColsAtCompileTime == Dynamic) { + MatrixType t2(0, 0); VERIFY(t2.rows() == 0); VERIFY(t2.cols() == 0); + + zeroReduction(t2); + VERIFY(t1==t2); } } } @@ -33,11 +51,15 @@ template void zeroSizedVector() { VectorType t1; - if (VectorType::SizeAtCompileTime == Dynamic) + if (VectorType::SizeAtCompileTime == Dynamic || VectorType::SizeAtCompileTime==0) { + zeroReduction(t1); VERIFY(t1.size() == 0); VectorType t2(DenseIndex(0)); // DenseIndex disambiguates with 0-the-null-pointer (error with gcc 4.4 and MSVC8) VERIFY(t2.size() == 0); + zeroReduction(t2); + + VERIFY(t1==t2); } } @@ -51,9 +73,12 @@ void test_zerosized() zeroSizedMatrix >(); zeroSizedMatrix >(); zeroSizedMatrix >(); - + zeroSizedMatrix >(); + zeroSizedMatrix >(); + zeroSizedVector(); zeroSizedVector(); zeroSizedVector(); zeroSizedVector >(); + zeroSizedVector >(); } diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport b/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport index 4454bf820..c4090ab11 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport @@ -11,7 +11,12 @@ #define EIGEN_OPENGL_MODULE #include -#include + +#if defined(__APPLE_CC__) + #include +#else + #include +#endif namespace Eigen { diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h index 3f18beeeb..dc0093eb9 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h @@ -58,7 +58,9 @@ void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV) Scalar rho, rho_1, alpha; d.setZero(); - CINV.startFill(); // FIXME estimate the number of non-zeros + typedef Triplet T; + std::vector tripletList; + for (Index i = 0; i < rows; ++i) { d[i] = 1.0; @@ -84,11 +86,12 @@ void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV) // FIXME add a generic "prune/filter" expression for both dense and sparse object to sparse for (Index j=0; j TmpVec; diff --git a/gtsam/3rdparty/Eigen/unsupported/test/FFTW.cpp b/gtsam/3rdparty/Eigen/unsupported/test/FFTW.cpp index a07bf274b..d3718e2d2 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/FFTW.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/FFTW.cpp @@ -16,9 +16,6 @@ std::complex RandomCpx() { return std::complex( (T)(rand()/(T)RAND_MAX - . using namespace std; using namespace Eigen; -float norm(float x) {return x*x;} -double norm(double x) {return x*x;} -long double norm(long double x) {return x*x;} template < typename T> complex promote(complex x) { return complex(x.real(),x.imag()); } @@ -40,11 +37,11 @@ complex promote(long double x) { return complex( x); for (size_t k1=0;k1<(size_t)timebuf.size();++k1) { acc += promote( timebuf[k1] ) * exp( complex(0,k1*phinc) ); } - totalpower += norm(acc); + totalpower += numext::abs2(acc); complex x = promote(fftbuf[k0]); complex dif = acc - x; - difpower += norm(dif); - //cerr << k0 << "\t" << acc << "\t" << x << "\t" << sqrt(norm(dif)) << endl; + difpower += numext::abs2(dif); + //cerr << k0 << "\t" << acc << "\t" << x << "\t" << sqrt(numext::abs2(dif)) << endl; } cerr << "rmse:" << sqrt(difpower/totalpower) << endl; return sqrt(difpower/totalpower); @@ -57,8 +54,8 @@ complex promote(long double x) { return complex( x); long double difpower=0; size_t n = (min)( buf1.size(),buf2.size() ); for (size_t k=0;k 0.02. + + * GeodSolve.cgi allows ellipsoid to be set (and uses the -E option + for GeodSolve). + + * Set title in HTML versions of man pages for the utility programs. + + * Changes in cmake support: + + add _d to names of executables in debug mode of Visual Studio; + + add support for Android (cmake-only), thanks to Pullan Yu; + + check CPACK version numbers supplied on command line; + + configured version of project-config.cmake.in is + project-config.cmake (instead of geographiclib-config.cmake), to + prevent find_package incorrectly using this file; + + fix tests with multi-line output; + + this release includes a file, pom.xml, which is used by an + experimental build system (based on maven) at SRI. Changes between 1.34 (released 2013-12-11) and 1.33 versions: diff --git a/gtsam/3rdparty/GeographicLib/cmake/CMakeLists.txt b/gtsam/3rdparty/GeographicLib/cmake/CMakeLists.txt index 51e6c44b2..bbb5192cf 100644 --- a/gtsam/3rdparty/GeographicLib/cmake/CMakeLists.txt +++ b/gtsam/3rdparty/GeographicLib/cmake/CMakeLists.txt @@ -31,14 +31,17 @@ endif () # it to prevent the source and build paths appearing in the installed # config files set (PROJECT_INCLUDE_DIRS) -configure_file (project-config.cmake.in - ${PROJECT_NAME_LOWER}-config.cmake @ONLY) +configure_file (project-config.cmake.in project-config.cmake @ONLY) configure_file (project-config-version.cmake.in - ${PROJECT_NAME_LOWER}-config-version.cmake @ONLY) + project-config-version.cmake @ONLY) install (FILES - "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME_LOWER}-config.cmake" - "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME_LOWER}-config-version.cmake" - DESTINATION "${INSTALL_CMAKE_DIR}") + "${CMAKE_CURRENT_BINARY_DIR}/project-config.cmake" + DESTINATION "${INSTALL_CMAKE_DIR}" + RENAME "${PROJECT_NAME_LOWER}-config.cmake") +install (FILES + "${CMAKE_CURRENT_BINARY_DIR}/project-config-version.cmake" + DESTINATION "${INSTALL_CMAKE_DIR}" + RENAME "${PROJECT_NAME_LOWER}-config-version.cmake") # Make information about the cmake targets (the library and the tools) # available. install (EXPORT depends diff --git a/gtsam/3rdparty/GeographicLib/cmake/project-config.cmake.in b/gtsam/3rdparty/GeographicLib/cmake/project-config.cmake.in index a8b15a098..9bd50c24d 100644 --- a/gtsam/3rdparty/GeographicLib/cmake/project-config.cmake.in +++ b/gtsam/3rdparty/GeographicLib/cmake/project-config.cmake.in @@ -30,7 +30,7 @@ if (IS_ABSOLUTE "@PROJECT_ROOT_DIR@") set (_ROOT "@PROJECT_ROOT_DIR@") set (@PROJECT_NAME@_INCLUDE_DIRS "@PROJECT_INCLUDE_DIRS@") set (@PROJECT_NAME@_LIBRARY_DIRS "${_ROOT}/src") - set (@PROJECT_NAME@_BINARY_DIRS "${_ROOT}/src") + set (@PROJECT_NAME@_BINARY_DIRS "${_ROOT}/tools") else () # This is an installed package; figure out the paths relative to the # current directory. diff --git a/gtsam/3rdparty/GeographicLib/configure b/gtsam/3rdparty/GeographicLib/configure index cc81327f5..a9a70828a 100755 --- a/gtsam/3rdparty/GeographicLib/configure +++ b/gtsam/3rdparty/GeographicLib/configure @@ -1,6 +1,6 @@ #! /bin/sh # Guess values for system-dependent variables and create Makefiles. -# Generated by GNU Autoconf 2.69 for GeographicLib 1.34. +# Generated by GNU Autoconf 2.69 for GeographicLib 1.35. # # Report bugs to . # @@ -590,8 +590,8 @@ MAKEFLAGS= # Identity of this package. PACKAGE_NAME='GeographicLib' PACKAGE_TARNAME='geographiclib' -PACKAGE_VERSION='1.34' -PACKAGE_STRING='GeographicLib 1.34' +PACKAGE_VERSION='1.35' +PACKAGE_STRING='GeographicLib 1.35' PACKAGE_BUGREPORT='charles@karney.com' PACKAGE_URL='' @@ -1339,7 +1339,7 @@ if test "$ac_init_help" = "long"; then # Omit some internal or obsolete options to make the list less imposing. # This message is too long to be a string in the A/UX 3.1 sh. cat <<_ACEOF -\`configure' configures GeographicLib 1.34 to adapt to many kinds of systems. +\`configure' configures GeographicLib 1.35 to adapt to many kinds of systems. Usage: $0 [OPTION]... [VAR=VALUE]... @@ -1410,7 +1410,7 @@ fi if test -n "$ac_init_help"; then case $ac_init_help in - short | recursive ) echo "Configuration of GeographicLib 1.34:";; + short | recursive ) echo "Configuration of GeographicLib 1.35:";; esac cat <<\_ACEOF @@ -1519,7 +1519,7 @@ fi test -n "$ac_init_help" && exit $ac_status if $ac_init_version; then cat <<\_ACEOF -GeographicLib configure 1.34 +GeographicLib configure 1.35 generated by GNU Autoconf 2.69 Copyright (C) 2012 Free Software Foundation, Inc. @@ -2063,7 +2063,7 @@ cat >config.log <<_ACEOF This file contains any messages produced by compilers while running configure, to aid debugging if configure makes a mistake. -It was created by GeographicLib $as_me 1.34, which was +It was created by GeographicLib $as_me 1.35, which was generated by GNU Autoconf 2.69. Invocation command line was $ $0 $@ @@ -3001,7 +3001,7 @@ fi # Define the identity of the package. PACKAGE='geographiclib' - VERSION='1.34' + VERSION='1.35' cat >>confdefs.h <<_ACEOF @@ -3049,7 +3049,7 @@ am__tar='$${TAR-tar} chof - "$$tardir"' am__untar='$${TAR-tar} xf -' GEOGRAPHICLIB_VERSION_MAJOR=1 -GEOGRAPHICLIB_VERSION_MINOR=34 +GEOGRAPHICLIB_VERSION_MINOR=35 GEOGRAPHICLIB_VERSION_PATCH=0 cat >>confdefs.h <<_ACEOF @@ -3098,7 +3098,7 @@ ac_config_headers="$ac_config_headers include/GeographicLib/Config-ac.h" LT_CURRENT=11 -LT_REVISION=1 +LT_REVISION=2 LT_AGE=1 @@ -16672,7 +16672,7 @@ cat >>$CONFIG_STATUS <<\_ACEOF || ac_write_fail=1 # report actual input values of CONFIG_FILES etc. instead of their # values after options handling. ac_log=" -This file was extended by GeographicLib $as_me 1.34, which was +This file was extended by GeographicLib $as_me 1.35, which was generated by GNU Autoconf 2.69. Invocation command line was CONFIG_FILES = $CONFIG_FILES @@ -16738,7 +16738,7 @@ _ACEOF cat >>$CONFIG_STATUS <<_ACEOF || ac_write_fail=1 ac_cs_config="`$as_echo "$ac_configure_args" | sed 's/^ //; s/[\\""\`\$]/\\\\&/g'`" ac_cs_version="\\ -GeographicLib config.status 1.34 +GeographicLib config.status 1.35 configured by $0, generated by GNU Autoconf 2.69, with options \\"\$ac_cs_config\\" diff --git a/gtsam/3rdparty/GeographicLib/configure.ac b/gtsam/3rdparty/GeographicLib/configure.ac index e9fb7b6f2..c0b539c5c 100644 --- a/gtsam/3rdparty/GeographicLib/configure.ac +++ b/gtsam/3rdparty/GeographicLib/configure.ac @@ -1,7 +1,7 @@ dnl dnl Copyright (C) 2009, Francesco P. Lovergine -AC_INIT([GeographicLib],[1.34],[charles@karney.com]) +AC_INIT([GeographicLib],[1.35],[charles@karney.com]) AC_CANONICAL_SYSTEM AC_PREREQ(2.61) AC_CONFIG_SRCDIR(src/Geodesic.cpp) @@ -9,7 +9,7 @@ AC_CONFIG_MACRO_DIR(m4) AM_INIT_AUTOMAKE GEOGRAPHICLIB_VERSION_MAJOR=1 -GEOGRAPHICLIB_VERSION_MINOR=34 +GEOGRAPHICLIB_VERSION_MINOR=35 GEOGRAPHICLIB_VERSION_PATCH=0 AC_DEFINE_UNQUOTED([GEOGRAPHICLIB_VERSION_MAJOR], [$GEOGRAPHICLIB_VERSION_MAJOR],[major version number]) @@ -35,7 +35,7 @@ dnl Interfaces changed/added/removed: CURRENT++ REVISION=0 dnl Interfaces added: AGE++ dnl Interfaces removed: AGE=0 LT_CURRENT=11 -LT_REVISION=1 +LT_REVISION=2 LT_AGE=1 AC_SUBST(LT_CURRENT) AC_SUBST(LT_REVISION) diff --git a/gtsam/3rdparty/GeographicLib/doc/GeographicLib.dox b/gtsam/3rdparty/GeographicLib/doc/GeographicLib.dox index b0f980d19..7fd60e992 100644 --- a/gtsam/3rdparty/GeographicLib/doc/GeographicLib.dox +++ b/gtsam/3rdparty/GeographicLib/doc/GeographicLib.dox @@ -11,8 +11,8 @@ /** \mainpage GeographicLib library \author Charles F. F. Karney (charles@karney.com) -\version 1.34 -\date 2013-12-11 +\version 1.35 +\date 2014-03-13 \section abstract Abstract @@ -45,22 +45,22 @@ The main project page is at http://sourceforge.net/projects/geographiclib . The code is available for download at -- - GeographicLib-1.34.tar.gz -- - GeographicLib-1.34.zip +- + GeographicLib-1.35.tar.gz +- + GeographicLib-1.35.zip . as either a compressed tar file (tar.gz) or a zip file. (The two archives have identical contents, except that the zip file has DOS line endings.) Alternatively you can get the latest release using git \verbatim - git clone -b r1.34 git://git.code.sf.net/p/geographiclib/code geographiclib + git clone -b r1.35 git://git.code.sf.net/p/geographiclib/code geographiclib \endverbatim There are also binary installers for Windows available at -- - GeographicLib-1.34-win32.exe -- - GeographicLib-1.34-win64.exe +- + GeographicLib-1.35-win32.exe +- + GeographicLib-1.35-win64.exe . It is licensed under the MIT/X11 License; @@ -180,14 +180,14 @@ Back to \ref intro. Forward to \ref start. Up to \ref contents. (versions 4.0 and later) and under Windows with Visual Studio 2005, 2008, and 2010. Earlier versions were tested also under Darwin and Solaris. It should compile on a wide range of other systems. First download either - -GeographicLib-1.34.tar.gz or - -GeographicLib-1.34.zip (or - -GeographicLib-1.34-win32.exe or - -GeographicLib-1.34-win64.exe for binary installation under Windows). + +GeographicLib-1.35.tar.gz or + +GeographicLib-1.35.zip (or + +GeographicLib-1.35-win32.exe or + +GeographicLib-1.35-win64.exe for binary installation under Windows). Then pick one of the first five options below: - \ref cmake. This is the preferred installation method as it will work on the widest range of platforms. However it requires that you have @@ -262,10 +262,10 @@ g++ on Linux and with the Visual Studio IDE on Windows. Here are the steps to compile and install %GeographicLib: - Unpack the source, running one of \verbatim - tar xfpz GeographicLib-1.34.tar.gz - unzip -q GeographicLib-1.34.zip \endverbatim + tar xfpz GeographicLib-1.35.tar.gz + unzip -q GeographicLib-1.35.zip \endverbatim then enter the directory created with one of \verbatim - cd GeographicLib-1.34 \endverbatim + cd GeographicLib-1.35 \endverbatim - Create a separate build directory and enter it, for example, \verbatim mkdir BUILD cd BUILD \endverbatim @@ -273,8 +273,8 @@ Here are the steps to compile and install %GeographicLib: and MacOSX systems, the command is \verbatim cmake .. \endverbatim For Windows, the command is typically one of \verbatim - cmake -G "Visual Studio 10" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc10/GeographicLib-1.34 .. - cmake -G "Visual Studio 9 2008" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc9/GeographicLib-1.34 .. + cmake -G "Visual Studio 10" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc10/GeographicLib-1.35 .. + cmake -G "Visual Studio 9 2008" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc9/GeographicLib-1.35 .. \endverbatim The definitions of CMAKE_INSTALL_PREFIX are optional (see below). The settings given above are recommended to ensure that packages that use @@ -290,7 +290,7 @@ Here are the steps to compile and install %GeographicLib: convention. If it is on ON (the Linux default), the installation is to a common directory, e.g., /usr/local. If it is OFF (the Windows default), the installation directory contains the package - name, e.g., C:/pkg/GeographicLib-1.34. The installation + name, e.g., C:/pkg/GeographicLib-1.35. The installation directories for the documentation, cmake configuration, python and matlab interfaces all depend on the variable with deeper paths relative to CMAKE_INSTALL_PREFIX being used when it's ON: @@ -305,7 +305,7 @@ Here are the steps to compile and install %GeographicLib: For windows systems, it is recommended to use a prefix which includes the compiler version, as shown above (and also, possibly, whether this is a 64-bit build, e.g., cmake -G "Visual Studio - 10 Win64" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc10-x64/GeographicLib-1.34 + 10 Win64" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc10-x64/GeographicLib-1.35 ..). If you just want to try the library to see if it suits your needs, pick, for example, CMAKE_INSTALL_PREFIX=/tmp/geographic. @@ -394,9 +394,9 @@ Here are the steps to compile and install %GeographicLib: The method works on most Unix-like systems including Linux and Mac OS X. Here are the steps to compile and install %GeographicLib: - Unpack the source, running \verbatim - tar xfpz GeographicLib-1.34.tar.gz \endverbatim + tar xfpz GeographicLib-1.35.tar.gz \endverbatim then enter the directory created \verbatim - cd GeographicLib-1.34 \endverbatim + cd GeographicLib-1.35 \endverbatim - Create a separate build directory and enter it, for example, \verbatim mkdir BUILD cd BUILD \endverbatim @@ -425,9 +425,9 @@ and g++. This builds a static library and the examples. Here are the steps to compile and install %GeographicLib: - Unpack the source, running \verbatim - tar xfpz GeographicLib-1.34.tar.gz \endverbatim + tar xfpz GeographicLib-1.35.tar.gz \endverbatim then enter the directory created \verbatim - cd GeographicLib-1.34 \endverbatim + cd GeographicLib-1.35 \endverbatim - Edit \verbatim include/GeographicLib/Config.h \endverbatim If your C++ compiler does not recognize the long double type @@ -458,8 +458,8 @@ static library and the utilities. If you only have Visual Studio 2003, use cmake to create the necessary solution file, see \ref cmake. (cmake is needed to build the Matlab interface and to run the tests.) - Unpack the source, running \verbatim - unzip -q GeographicLib-1.34.zip \endverbatim -- Open GeographicLib-1.34/windows/GeographicLib-vc10.sln in Visual Studio + unzip -q GeographicLib-1.35.zip \endverbatim +- Open GeographicLib-1.35/windows/GeographicLib-vc10.sln in Visual Studio 2010 (for Visual Studio 2005 and 2008, replace -vc10 by -vc8 or -vc9). - Pick the build type (e.g., Release), and select "Build Solution". - The library and the compiled examples are in the windows/Release. @@ -486,14 +486,14 @@ be advisable to build it with the compiler you are using for your own code using either \ref cmake or \ref windows. Download and run - -GeographicLib-1.34-win32.exe or - -GeographicLib-1.34-win64.exe: + +GeographicLib-1.35-win32.exe or + +GeographicLib-1.35-win64.exe: - read the MIT/X11 License agreement, - select whether you want your PATH modified, - select the installation folder, by default - C:\\pkg-vc10\\GeographicLib-1.34 or C:\\pkg-vc10-x64\\GeographicLib-1.34, + C:\\pkg-vc10\\GeographicLib-1.35 or C:\\pkg-vc10-x64\\GeographicLib-1.35, - select the start menu folder, - and install. . @@ -501,7 +501,7 @@ GeographicLib-1.34-win64.exe: given in \ref cmake.) The start menu will now include links to the documentation for the library and for the utilities (and a link for uninstalling the library). If you ask for your PATH to be modified, it -will include C:/pkg-vc10/GeographicLib-1.34/bin where the utilities are +will include C:/pkg-vc10/GeographicLib-1.35/bin where the utilities are installed. The headers and library are installed in the include/GeographicLib and lib folders. With the 64-bit installer, the Matlab interface is installed in the matlab folder. Add this to your @@ -552,7 +552,7 @@ Check the code out of git with \verbatim Here the "master" branch is checked out. There are three branches in the git repository: - master: the main branch for code maintainence. Releases are - tagged on this branch as, e.g., v1.34. + tagged on this branch as, e.g., v1.35. - devel: the development branch; changes made here are merged into master. - release: the release branch created by unpacking the source @@ -562,7 +562,7 @@ the git repository: specifying a branch). This differs from the master branch in that some administrative files are excluded while some intermediate files are included (in order to aid building on as many platforms as - possible). Releases are tagged on this branch as, e.g., r1.34. + possible). Releases are tagged on this branch as, e.g., r1.35. . The autoconf configuration script and the formatted man pages are not checked into master branch of the repository. In order to create the @@ -578,8 +578,8 @@ In the case of cmake, you then run \verbatim which will copy the man pages from the build directory back into the source tree and package the resulting source tree for distribution as \verbatim - GeographicLib-1.34.tar.gz - GeographicLib-1.34.zip \endverbatim + GeographicLib-1.35.tar.gz + GeographicLib-1.35.zip \endverbatim Finally, \verbatim make package \endverbatim or building PACKAGE in Visual Studio will create a binary installer for @@ -605,7 +605,7 @@ With configure, run \verbatim make dist-gzip \endverbatim which will create the additional files and packages the results ready for distribution as \verbatim - geographiclib-1.34.tar.gz \endverbatim + geographiclib-1.35.tar.gz \endverbatim
Back to \ref intro. Forward to \ref start. Up to \ref contents. @@ -695,7 +695,7 @@ In order to use %GeographicLib from C++ code, you will need to If %GeographicLib is found, then the following cmake variables are set: - GeographicLib_FOUND = 1 - - GeographicLib_VERSION = 1.34 + - GeographicLib_VERSION = 1.35 - GeographicLib_INCLUDE_DIRS - GeographicLib_LIBRARIES = one of the following two: - GeographicLib_SHARED_LIBRARIES = %GeographicLib @@ -1120,9 +1120,9 @@ feature of %GeographicLib, but want your code still to work with older versions. In that case, you can test the values of the macros GEOGRAPHICLIB_VERSION_MAJOR, GEOGRAPHICLIB_VERSION_MINOR, and GEOGRAPHICLIB_VERSION_PATCH; these expand to numbers (and the last one -is usually 0); these macros appeared starting in version 1.34. There's +is usually 0); these macros appeared starting in version 1.31. There's also a macro GEOGRAPHICLIB_VERSION_STRING which expands to, e.g., -"1.34"; this macro has been defined since version 1.9. +"1.35"; this macro has been defined since version 1.9.
Back to \ref utilities. Forward to \ref other. Up to \ref contents. @@ -1274,7 +1274,7 @@ The matlab directory contains - Native Matlab implementations of the geodesic routines. To use these, start Matlab or Octave and run one of (for example) \verbatim addpath /usr/local/libexec/GeographicLib/matlab - addpath 'C:/pkg-vc10-x64/GeographicLib-1.34/libexec/GeographicLib/matlab' + addpath 'C:/pkg-vc10-x64/GeographicLib-1.35/libexec/GeographicLib/matlab' \endverbatim The available functions are: - geoddoc: briefly descibe the routines @@ -1345,9 +1345,9 @@ There are two ways of compiling the interface code: (1) using cmake and - Invoking the compiler from Matlab or Octave: Start Matlab or Octave and run, e.g., \code mex -setup - cd 'C:/pkg-vc10-x64/GeographicLib-1.34/matlab' + cd 'C:/pkg-vc10-x64/GeographicLib-1.35/matlab' help geographiclibinterface - geographiclibinterface('C:/pkg-vc10/GeographicLib-1.34'); + geographiclibinterface('C:/pkg-vc10/GeographicLib-1.35'); addpath(pwd); \endcode The first command allows you to select the compiler to use (which @@ -1356,7 +1356,7 @@ There are two ways of compiling the interface code: (1) using cmake and To use the interface routines for %GeographicLib, run one of (for example) \verbatim addpath /usr/local/libexec/GeographicLib/matlab - addpath 'C:/pkg-vc10-x64/GeographicLib-1.34/libexec/GeographicLib/matlab' + addpath 'C:/pkg-vc10-x64/GeographicLib-1.35/libexec/GeographicLib/matlab' \endverbatim in Octave or Matlab. The available functions are: - geodesicdirect: solve direct geodesic problem @@ -4048,7 +4048,7 @@ starting point of this geodesic is \f$\beta_1 = 87.48^\circ\f$, \f$\omega_1 = 0^\circ\f$, and \f$\alpha_1 = 90^\circ\f$. If the starting point is \f$\beta_1 = 90^\circ\f$, \f$\omega_1 \in -(0^\circ, 180^\circ)\f$, and \f$\alpha_1 = 0^\circ\f$, then the geodesic +(0^\circ, 180^\circ)\f$, and \f$\alpha_1 = 180^\circ\f$, then the geodesic encircles the ellipsoid in a "transpolar" sense. The geodesic oscillates east and west of the ellipse \f$x = 0\f$; on each oscillation it completes slightly more that a full circuit around the ellipsoid @@ -4067,7 +4067,7 @@ Fig. 4 Fig. 4: Example of a transpolar geodesic on a triaxial ellipsoid. The starting point of this geodesic is \f$\beta_1 = 90^\circ\f$, \f$\omega_1 = -39.9^\circ\f$, and \f$\alpha_1 = 0^\circ\f$. +39.9^\circ\f$, and \f$\alpha_1 = 180^\circ\f$.
). - If only one point is an umbilicial point, the azimuth at the non-umbilical point is found using the generalization of Clairaut's equation (given above) with \f$\gamma = 0\f$. - - If both points lie on the equator \f$\beta = 0\f$, then determine the - reduced length \f$m_{12}\f$ for the geodesic which is the shorter - path along the ellipse \f$z = 0\f$. If \f$m_{12} \ge 0\f$, then this - is the shortest path on the ellipsoid; otherwise proceed to the - general case (next). + - Treat the cases where the geodesic might follow a line of constant + \f$\beta\f$. There are two such cases: (a) the points lie on the + ellipse \f$z = 0\f$ on a general ellipsoid and (b) the points lie on + an ellipse whose major axis is the \f$x\f$ axis on a prolate ellipsoid + (\f$a = b > c\f$). Determine the reduced length \f$m_{12}\f$ for the + geodesic which is the shorter path along the ellipse. If \f$m_{12} + \ge 0\f$, then this is the shortest path on the ellipsoid; otherwise + proceed to the general case (next). - Swap the points, if necessary, so that the first point is the one closest to a pole. Estimate \f$\alpha_1\f$ (by some means) and solve the \e hybrid problem, i.e., determine the longitude \f$\omega_2\f$ @@ -4238,6 +4241,12 @@ The shortest path found by this method is unique unless: - The points are opposite umbilical points. In this case, \f$\alpha_1\f$ can take on any value and \f$\alpha_2\f$ needs to be adjusted to maintain the value of \f$\tan\alpha_1 / \tan\alpha_2\f$. + Note that \f$\alpha\f$ increases by \f$\pm 90^\circ\f$ as the + geodesic passes through an umbilical point, depending on whether the + geodesic is considered as passing to the right or left of the point. + Here \f$\alpha_2\f$ is the \e forward azimuth at the second umbilical + point, i.e., its azimuth immediately \e after passage through the + umbilical point. - \f$\beta_1 + \beta_2 = 0\f$ and \f$\cos\alpha_1\f$ and \f$\cos\alpha_2\f$ have opposite signs. In this case, there another shortest geodesic with azimuths \f$\pi - \alpha_1\f$ and @@ -4757,6 +4766,26 @@ been migrated to the archive subdirectory). All the releases are available as tags “rm.nn” in the the "release" branch of the git repository for %GeographicLib. + - Version 1.35 + (released 2014-03-13) + - Fix blunder in GeographicLib::UTMUPS::EncodeEPSG (found by Ben + Adler). + - Matlab wrapper routines geodesic{direct,inverse,line} switch to + "exact" routes if |f| > 0.02. + - GeodSolve.cgi allows ellipsoid to be set (and uses the -E option + for GeodSolve). + - Set title in HTML versions of man pages for the \ref utilities. + - Changes in cmake support: + - add _d to names of executables in debug mode of Visual Studio; + - add support for Android (cmake-only), thanks to Pullan Yu; + - check CPACK version numbers supplied on command line; + - configured version of project-config.cmake.in is + project-config.cmake (instead of geographiclib-config.cmake), to + prevent find_package incorrectly using this file; + - fix tests with multi-line output; + - this release includes a file, pom.xml, which is used by an + experimental build system (based on maven) at SRI. + - Version 1.34 (released 2013-12-11) - Many changes in cmake support: diff --git a/gtsam/3rdparty/GeographicLib/doc/NETGeographicLib.dox b/gtsam/3rdparty/GeographicLib/doc/NETGeographicLib.dox index 89b934a7d..fd8f5e057 100644 --- a/gtsam/3rdparty/GeographicLib/doc/NETGeographicLib.dox +++ b/gtsam/3rdparty/GeographicLib/doc/NETGeographicLib.dox @@ -11,8 +11,8 @@ /** \mainpage NETGeographicLib library \author Scott Heiman (mrmtdew2@outlook.com) -\version 1.34 -\date 2013-12-11 +\version 1.35 +\date 2014-03-13 \section abstract Abstract @@ -26,14 +26,14 @@ to the GeographicLib classes. GeographicLib and NETGeographicLib is an integrated product. The NETGeographic project in the GeographicLib-vc10.sln file located in -\/GeographicLib-1.34/windows will create the NETGeographicLib +\/GeographicLib-1.35/windows will create the NETGeographicLib DLL. The source code for NETGeographicLib is located in -\/GeographicLib-1.34/dotnet/NETGeographicLib. NETGeographicLib +\/GeographicLib-1.35/dotnet/NETGeographicLib. NETGeographicLib is not available for older versions of Microsoft Visual Studio. NETGeographicLib has been tested with C#, Managed C++, and Visual Basic. Sample code snippets can be found in -\/GeographicLib-1.34/dotnet/examples. +\/GeographicLib-1.35/dotnet/examples. \section differences Differences between NETGeographicLib and GeographicLib @@ -135,7 +135,7 @@ to any Visual Basic source that uses NETGeographicLib classes. A C# sample application is provided that demonstrates NETGeographicLib classes. The source code for the sample application is located in -\/GeographicLib-1.34/dotnet/Projections. The sample +\/GeographicLib-1.35/dotnet/Projections. The sample application creates a tabbed dialog. Each tab provides data entry fields that allow the user to exercise one or more NETGeographicLib classes. @@ -200,7 +200,7 @@ code using the installed library: \verbatim project (geodesictest) cmake_minimum_required (VERSION 2.8.7) # required for VS_DOTNET_REFERENCES -find_package (GeographicLib 1.34 REQUIRED COMPONENTS NETGeographicLib) +find_package (GeographicLib 1.35 REQUIRED COMPONENTS NETGeographicLib) add_executable (${PROJECT_NAME} example-Geodesic-small.cpp) set_target_properties (${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "/clr") diff --git a/gtsam/3rdparty/GeographicLib/doc/scripts/GeographicLib/Geodesic.js b/gtsam/3rdparty/GeographicLib/doc/scripts/GeographicLib/Geodesic.js index 09d22593a..cf059126f 100644 --- a/gtsam/3rdparty/GeographicLib/doc/scripts/GeographicLib/Geodesic.js +++ b/gtsam/3rdparty/GeographicLib/doc/scripts/GeographicLib/Geodesic.js @@ -753,7 +753,7 @@ GeographicLib.GeodesicLine = {}; // Add the check for sig12 since zero length geodesics might yield // m12 < 0. Test case was // - // echo 20.001 0 20.001 0 | Geod -i + // echo 20.001 0 20.001 0 | GeodSolve -i // // In fact, we will have sig12 > pi/2 for meridional geodesic // which is not a shortest path. diff --git a/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google-instructions.html b/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google-instructions.html index 8d6f25c56..89c2e5605 100644 --- a/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google-instructions.html +++ b/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google-instructions.html @@ -101,8 +101,8 @@ In putting together this Google Maps demonstration, I started with the sample code - - geometry-headings.html. + + geometry-headings.

Charles Karney diff --git a/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google.html b/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google.html index 35576cf87..5a48d7e2e 100644 --- a/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google.html +++ b/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google.html @@ -24,13 +24,18 @@ Google Maps, WGS84 ellipsoid, GeographicLib" /> - + -
diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Config.h b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Config.h index 1e5e30b9b..2249dd127 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Config.h +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Config.h @@ -1,8 +1,8 @@ // This will be overwritten by ./configure -#define GEOGRAPHICLIB_VERSION_STRING "1.34" +#define GEOGRAPHICLIB_VERSION_STRING "1.35" #define GEOGRAPHICLIB_VERSION_MAJOR 1 -#define GEOGRAPHICLIB_VERSION_MINOR 34 +#define GEOGRAPHICLIB_VERSION_MINOR 35 #define GEOGRAPHICLIB_VERSION_PATCH 0 // Undefine HAVE_LONG_DOUBLE if this type is unknown to the compiler diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Constants.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Constants.hpp index e37ec62ce..99baff31f 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Constants.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Constants.hpp @@ -21,6 +21,17 @@ # elif defined(__GXX_EXPERIMENTAL_CXX0X__) # define STATIC_ASSERT static_assert # elif defined(_MSC_VER) && _MSC_VER >= 1600 +// For reference, here is a table of Visual Studio and _MSC_VER +// correspondences: +// +// _MSC_VER Visual Studio +// 1300 vc7 +// 1311 vc7.1 (2003) +// 1400 vc8 (2005) +// 1500 vc9 (2008) +// 1600 vc10 (2010) +// 1700 vc11 (2012) +// 1800 vc12 (2013) # define STATIC_ASSERT static_assert # else # define STATIC_ASSERT(cond,reason) \ diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Geodesic.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Geodesic.hpp index cd687acce..6cec279a2 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Geodesic.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Geodesic.hpp @@ -162,8 +162,8 @@ namespace GeographicLib { * Example of use: * \include example-Geodesic.cpp * - * Geod is a command-line utility providing access - * to the functionality of Geodesic and GeodesicLine. + * GeodSolve is a command-line utility + * providing access to the functionality of Geodesic and GeodesicLine. **********************************************************************/ class GEOGRAPHICLIB_EXPORT Geodesic { diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicExact.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicExact.hpp index c0444f70e..e76daee4c 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicExact.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicExact.hpp @@ -25,7 +25,7 @@ namespace GeographicLib { class GeodesicLineExact; /** - * \brief Exact %Geodesic calculations + * \brief Exact geodesic calculations * * The equations for geodesics on an ellipsoid can be expressed in terms of * incomplete elliptic integrals. The Geodesic class expands these integrals @@ -67,14 +67,14 @@ namespace GeographicLib { * about 8 decimal digits for \e b/\e a ∈ [1/4, 4]. * * See \ref geodellip for the formulation. See the documentation on the - * Geodesic class for additional information on the geodesics problems. + * Geodesic class for additional information on the geodesic problems. * * Example of use: * \include example-GeodesicExact.cpp * - * Geod is a command-line utility providing access - * to the functionality of GeodesicExact and GeodesicLineExact (via the -E - * option). + * GeodSolve is a command-line utility + * providing access to the functionality of GeodesicExact and + * GeodesicLineExact (via the -E option). **********************************************************************/ class GEOGRAPHICLIB_EXPORT GeodesicExact { diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLine.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLine.hpp index fd968e2cf..c901b3007 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLine.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLine.hpp @@ -52,8 +52,8 @@ namespace GeographicLib { * Example of use: * \include example-GeodesicLine.cpp * - * Geod is a command-line utility providing access - * to the functionality of Geodesic and GeodesicLine. + * GeodSolve is a command-line utility + * providing access to the functionality of Geodesic and GeodesicLine. **********************************************************************/ class GEOGRAPHICLIB_EXPORT GeodesicLine { diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLineExact.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLineExact.hpp index ca7d0d211..a2e19f845 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLineExact.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLineExact.hpp @@ -27,9 +27,9 @@ namespace GeographicLib { * Example of use: * \include example-GeodesicLineExact.cpp * - * Geod is a command-line utility providing access - * to the functionality of GeodesicExact and GeodesicLineExact (via the -E - * option). + * GeodSolve is a command-line utility + * providing access to the functionality of GeodesicExact and + * GeodesicLineExact (via the -E option). **********************************************************************/ class GEOGRAPHICLIB_EXPORT GeodesicLineExact { diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Math.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Math.hpp index 9971ebdf0..2ef649e96 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Math.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Math.hpp @@ -18,8 +18,11 @@ * Are C++11 math functions available? **********************************************************************/ #if !defined(GEOGRAPHICLIB_CPLUSPLUS11_MATH) -# if defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ >= 8 \ - && __cplusplus >= 201103 +# if defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ >= 7 \ + && __cplusplus >= 201103 && !(defined(__ANDROID__) || defined(ANDROID)) +// The android toolchain uses g++ and supports C++11, but not, apparently, the +// new mathematical functions introduced with C++11. Android toolchains might +// define __ANDROID__ or ANDROID; so need to check both. # define GEOGRAPHICLIB_CPLUSPLUS11_MATH 1 # elif defined(_MSC_VER) && _MSC_VER >= 1800 # define GEOGRAPHICLIB_CPLUSPLUS11_MATH 1 diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/UTMUPS.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/UTMUPS.hpp index 512ba5c43..9fc19c66e 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/UTMUPS.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/UTMUPS.hpp @@ -70,7 +70,7 @@ namespace GeographicLib { static const int epsg01N = 32601; // EPSG code for UTM 01N static const int epsg60N = 32660; // EPSG code for UTM 60N static const int epsgN = 32661; // EPSG code for UPS N - static const int epsg01S = 32701; // EPSG code for UTM 01N + static const int epsg01S = 32701; // EPSG code for UTM 01S static const int epsg60S = 32760; // EPSG code for UTM 60S static const int epsgS = 32761; // EPSG code for UPS S static real CentralMeridian(int zone) throw() diff --git a/gtsam/3rdparty/GeographicLib/legacy/C/geodesic.c b/gtsam/3rdparty/GeographicLib/legacy/C/geodesic.c index bd9fc960f..f46a6ff41 100644 --- a/gtsam/3rdparty/GeographicLib/legacy/C/geodesic.c +++ b/gtsam/3rdparty/GeographicLib/legacy/C/geodesic.c @@ -700,7 +700,7 @@ real geod_geninverse(const struct geod_geodesic* g, /* Add the check for sig12 since zero length geodesics might yield m12 < * 0. Test case was * - * echo 20.001 0 20.001 0 | Geod -i + * echo 20.001 0 20.001 0 | GeodSolve -i * * In fact, we will have sig12 > pi/2 for meridional geodesic which is * not a shortest path. */ diff --git a/gtsam/3rdparty/GeographicLib/legacy/Fortran/geodesic.for b/gtsam/3rdparty/GeographicLib/legacy/Fortran/geodesic.for index 6501363c1..aaef081c5 100644 --- a/gtsam/3rdparty/GeographicLib/legacy/Fortran/geodesic.for +++ b/gtsam/3rdparty/GeographicLib/legacy/Fortran/geodesic.for @@ -685,7 +685,7 @@ * Add the check for sig12 since zero length geodesics might yield m12 < * 0. Test case was * -* echo 20.001 0 20.001 0 | Geod -i +* echo 20.001 0 20.001 0 | GeodSolve -i * * In fact, we will have sig12 > pi/2 for meridional geodesic which is * not a shortest path. diff --git a/gtsam/3rdparty/GeographicLib/man/CMakeLists.txt b/gtsam/3rdparty/GeographicLib/man/CMakeLists.txt index d8af12998..ccdf213c3 100644 --- a/gtsam/3rdparty/GeographicLib/man/CMakeLists.txt +++ b/gtsam/3rdparty/GeographicLib/man/CMakeLists.txt @@ -34,7 +34,7 @@ foreach (TOOL ${TOOLS}) MAIN_DEPENDENCY ${TOOL}.pod) add_custom_command (OUTPUT ${TOOL}.1.html COMMAND - pod2html --noindex ${CMAKE_CURRENT_SOURCE_DIR}/${TOOL}.pod | + pod2html --title "'${TOOL}(1)'" --noindex ${CMAKE_CURRENT_SOURCE_DIR}/${TOOL}.pod | sed -e 's%%%' -e 's%\\\([^<>]*\\\)\(\\\(.\\\)\)%&%'g > ${TOOL}.1.html && cp ${TOOL}.1.html ../doc/html-stage/ diff --git a/gtsam/3rdparty/GeographicLib/man/CartConvert.1 b/gtsam/3rdparty/GeographicLib/man/CartConvert.1 index 02dfbfa5a..9a2f74660 100644 --- a/gtsam/3rdparty/GeographicLib/man/CartConvert.1 +++ b/gtsam/3rdparty/GeographicLib/man/CartConvert.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "CARTCONVERT 1" -.TH CARTCONVERT 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH CARTCONVERT 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/CartConvert.1.html b/gtsam/3rdparty/GeographicLib/man/CartConvert.1.html index 38dcd6af1..17ec77507 100644 --- a/gtsam/3rdparty/GeographicLib/man/CartConvert.1.html +++ b/gtsam/3rdparty/GeographicLib/man/CartConvert.1.html @@ -2,7 +2,7 @@ - +CartConvert(1) diff --git a/gtsam/3rdparty/GeographicLib/man/CartConvert.usage b/gtsam/3rdparty/GeographicLib/man/CartConvert.usage index f9f6f05ac..57374a78f 100644 --- a/gtsam/3rdparty/GeographicLib/man/CartConvert.usage +++ b/gtsam/3rdparty/GeographicLib/man/CartConvert.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " CartConvert --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/CartConvert.1.html\n"; +" http://geographiclib.sf.net/1.35/CartConvert.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/ConicProj.1 b/gtsam/3rdparty/GeographicLib/man/ConicProj.1 index 85f65f9a6..997017d7f 100644 --- a/gtsam/3rdparty/GeographicLib/man/ConicProj.1 +++ b/gtsam/3rdparty/GeographicLib/man/ConicProj.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "CONICPROJ 1" -.TH CONICPROJ 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH CONICPROJ 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/ConicProj.1.html b/gtsam/3rdparty/GeographicLib/man/ConicProj.1.html index e4af0e6b5..9de3fd683 100644 --- a/gtsam/3rdparty/GeographicLib/man/ConicProj.1.html +++ b/gtsam/3rdparty/GeographicLib/man/ConicProj.1.html @@ -2,7 +2,7 @@ - +ConicProj(1) diff --git a/gtsam/3rdparty/GeographicLib/man/ConicProj.usage b/gtsam/3rdparty/GeographicLib/man/ConicProj.usage index 3fbcc4481..a84aa4fde 100644 --- a/gtsam/3rdparty/GeographicLib/man/ConicProj.usage +++ b/gtsam/3rdparty/GeographicLib/man/ConicProj.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " ConicProj --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/ConicProj.1.html\n"; +" http://geographiclib.sf.net/1.35/ConicProj.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/GeoConvert.1 b/gtsam/3rdparty/GeographicLib/man/GeoConvert.1 index 0dfa11283..747b8cd3f 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoConvert.1 +++ b/gtsam/3rdparty/GeographicLib/man/GeoConvert.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GEOCONVERT 1" -.TH GEOCONVERT 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GEOCONVERT 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/GeoConvert.1.html b/gtsam/3rdparty/GeographicLib/man/GeoConvert.1.html index c46e2e7c0..5c117b49e 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoConvert.1.html +++ b/gtsam/3rdparty/GeographicLib/man/GeoConvert.1.html @@ -2,7 +2,7 @@ - +GeoConvert(1) diff --git a/gtsam/3rdparty/GeographicLib/man/GeoConvert.usage b/gtsam/3rdparty/GeographicLib/man/GeoConvert.usage index 7a0f8a852..19d78a3ef 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoConvert.usage +++ b/gtsam/3rdparty/GeographicLib/man/GeoConvert.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " GeoConvert --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/GeoConvert.1.html\n"; +" http://geographiclib.sf.net/1.35/GeoConvert.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/GeodSolve.1 b/gtsam/3rdparty/GeographicLib/man/GeodSolve.1 index 06ca388e4..3bd025aa4 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodSolve.1 +++ b/gtsam/3rdparty/GeographicLib/man/GeodSolve.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GEODSOLVE 1" -.TH GEODSOLVE 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GEODSOLVE 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/GeodSolve.1.html b/gtsam/3rdparty/GeographicLib/man/GeodSolve.1.html index 262bccf3b..8af2af9e7 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodSolve.1.html +++ b/gtsam/3rdparty/GeographicLib/man/GeodSolve.1.html @@ -2,7 +2,7 @@ - +GeodSolve(1) diff --git a/gtsam/3rdparty/GeographicLib/man/GeodSolve.usage b/gtsam/3rdparty/GeographicLib/man/GeodSolve.usage index 48bc7261e..78ec73ec7 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodSolve.usage +++ b/gtsam/3rdparty/GeographicLib/man/GeodSolve.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " GeodSolve --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/GeodSolve.1.html\n"; +" http://geographiclib.sf.net/1.35/GeodSolve.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1 b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1 index a4cc6680e..9b87078a3 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1 +++ b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GEODESICPROJ 1" -.TH GEODESICPROJ 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GEODESICPROJ 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1.html b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1.html index c40ca5a4d..2b63cf7de 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1.html +++ b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1.html @@ -2,7 +2,7 @@ - +GeodesicProj(1) diff --git a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.usage b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.usage index 9a5ab2517..013fb7acf 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.usage +++ b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " GeodesicProj --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/GeodesicProj.1.html\n"; +" http://geographiclib.sf.net/1.35/GeodesicProj.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/GeoidEval.1 b/gtsam/3rdparty/GeographicLib/man/GeoidEval.1 index 681a096f0..7c5086e1c 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoidEval.1 +++ b/gtsam/3rdparty/GeographicLib/man/GeoidEval.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GEOIDEVAL 1" -.TH GEOIDEVAL 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GEOIDEVAL 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/GeoidEval.1.html b/gtsam/3rdparty/GeographicLib/man/GeoidEval.1.html index 91f8f3610..ea131d8a7 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoidEval.1.html +++ b/gtsam/3rdparty/GeographicLib/man/GeoidEval.1.html @@ -2,7 +2,7 @@ - +GeoidEval(1) diff --git a/gtsam/3rdparty/GeographicLib/man/GeoidEval.usage b/gtsam/3rdparty/GeographicLib/man/GeoidEval.usage index 727811108..4fa144ca7 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoidEval.usage +++ b/gtsam/3rdparty/GeographicLib/man/GeoidEval.usage @@ -10,7 +10,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " GeoidEval --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/GeoidEval.1.html\n"; +" http://geographiclib.sf.net/1.35/GeoidEval.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/Gravity.1 b/gtsam/3rdparty/GeographicLib/man/Gravity.1 index 97e247b7f..96462b244 100644 --- a/gtsam/3rdparty/GeographicLib/man/Gravity.1 +++ b/gtsam/3rdparty/GeographicLib/man/Gravity.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GRAVITY 1" -.TH GRAVITY 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GRAVITY 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/Gravity.1.html b/gtsam/3rdparty/GeographicLib/man/Gravity.1.html index 3fbdba19b..cca8ad941 100644 --- a/gtsam/3rdparty/GeographicLib/man/Gravity.1.html +++ b/gtsam/3rdparty/GeographicLib/man/Gravity.1.html @@ -2,7 +2,7 @@ - +Gravity(1) diff --git a/gtsam/3rdparty/GeographicLib/man/Gravity.usage b/gtsam/3rdparty/GeographicLib/man/Gravity.usage index 20b522307..d60547f09 100644 --- a/gtsam/3rdparty/GeographicLib/man/Gravity.usage +++ b/gtsam/3rdparty/GeographicLib/man/Gravity.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " Gravity --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/Gravity.1.html\n"; +" http://geographiclib.sf.net/1.35/Gravity.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/MagneticField.1 b/gtsam/3rdparty/GeographicLib/man/MagneticField.1 index 0659f2325..3d520cb6b 100644 --- a/gtsam/3rdparty/GeographicLib/man/MagneticField.1 +++ b/gtsam/3rdparty/GeographicLib/man/MagneticField.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "MAGNETICFIELD 1" -.TH MAGNETICFIELD 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH MAGNETICFIELD 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/MagneticField.1.html b/gtsam/3rdparty/GeographicLib/man/MagneticField.1.html index cac6794c8..dd1884602 100644 --- a/gtsam/3rdparty/GeographicLib/man/MagneticField.1.html +++ b/gtsam/3rdparty/GeographicLib/man/MagneticField.1.html @@ -2,7 +2,7 @@ - +MagneticField(1) diff --git a/gtsam/3rdparty/GeographicLib/man/MagneticField.usage b/gtsam/3rdparty/GeographicLib/man/MagneticField.usage index b6b89d0de..9f1ca8b9b 100644 --- a/gtsam/3rdparty/GeographicLib/man/MagneticField.usage +++ b/gtsam/3rdparty/GeographicLib/man/MagneticField.usage @@ -10,7 +10,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " MagneticField --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/MagneticField.1.html\n"; +" http://geographiclib.sf.net/1.35/MagneticField.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/Makefile.am b/gtsam/3rdparty/GeographicLib/man/Makefile.am index 8fae122f6..cad9e45ce 100644 --- a/gtsam/3rdparty/GeographicLib/man/Makefile.am +++ b/gtsam/3rdparty/GeographicLib/man/Makefile.am @@ -62,7 +62,7 @@ if HAVE_PODPROGS $(POD2MAN) $^ > $@ .pod.1.html: - pod2html --noindex $^ | $(PODFIX) > $@ + pod2html --noindex --tile "$*(1)" $^ | $(PODFIX) > $@ else diff --git a/gtsam/3rdparty/GeographicLib/man/Makefile.in b/gtsam/3rdparty/GeographicLib/man/Makefile.in index b434fd523..3399f753c 100644 --- a/gtsam/3rdparty/GeographicLib/man/Makefile.in +++ b/gtsam/3rdparty/GeographicLib/man/Makefile.in @@ -567,7 +567,7 @@ htmlman: $(HTMLMAN) @HAVE_PODPROGS_TRUE@ $(POD2MAN) $^ > $@ @HAVE_PODPROGS_TRUE@.pod.1.html: -@HAVE_PODPROGS_TRUE@ pod2html --noindex $^ | $(PODFIX) > $@ +@HAVE_PODPROGS_TRUE@ pod2html --noindex --tile "$*(1)" $^ | $(PODFIX) > $@ @HAVE_PODPROGS_FALSE@CartConvert.usage: @HAVE_PODPROGS_FALSE@ $(USAGECMD) diff --git a/gtsam/3rdparty/GeographicLib/man/Planimeter.1 b/gtsam/3rdparty/GeographicLib/man/Planimeter.1 index 28bbbf46f..07e1563cd 100644 --- a/gtsam/3rdparty/GeographicLib/man/Planimeter.1 +++ b/gtsam/3rdparty/GeographicLib/man/Planimeter.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "PLANIMETER 1" -.TH PLANIMETER 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH PLANIMETER 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l @@ -200,7 +200,7 @@ is allowed for \fIf\fR. (Also, if \fIf\fR > 1, the flattening is set to 1/\fIf\fR.) By default, the \s-1WGS84\s0 ellipsoid is used, \fIa\fR = 6378137 m, \&\fIf\fR = 1/298.257223563. If entering vertices as \s-1UTM/UPS\s0 or \s-1MGRS\s0 coordinates, use the default ellipsoid, since the conversion of these -coordinates to latitude and longitude uses the \s-1WGS84\s0 parameters. +coordinates to latitude and longitude always uses the \s-1WGS84\s0 parameters. .IP "\fB\-\-comment\-delimiter\fR" 4 .IX Item "--comment-delimiter" set the comment delimiter to \fIcommentdelim\fR (e.g., \*(L"#\*(R" or \*(L"//\*(R"). If diff --git a/gtsam/3rdparty/GeographicLib/man/Planimeter.1.html b/gtsam/3rdparty/GeographicLib/man/Planimeter.1.html index b86a5ecd7..7a6eb52c4 100644 --- a/gtsam/3rdparty/GeographicLib/man/Planimeter.1.html +++ b/gtsam/3rdparty/GeographicLib/man/Planimeter.1.html @@ -2,7 +2,7 @@ - +Planimeter(1) @@ -56,7 +56,7 @@
-e
-

specify the ellipsoid via a f; the equatorial radius is a and the flattening is f. Setting f = 0 results in a sphere. Specify f < 0 for a prolate ellipsoid. A simple fraction, e.g., 1/297, is allowed for f. (Also, if f > 1, the flattening is set to 1/f.) By default, the WGS84 ellipsoid is used, a = 6378137 m, f = 1/298.257223563. If entering vertices as UTM/UPS or MGRS coordinates, use the default ellipsoid, since the conversion of these coordinates to latitude and longitude uses the WGS84 parameters.

+

specify the ellipsoid via a f; the equatorial radius is a and the flattening is f. Setting f = 0 results in a sphere. Specify f < 0 for a prolate ellipsoid. A simple fraction, e.g., 1/297, is allowed for f. (Also, if f > 1, the flattening is set to 1/f.) By default, the WGS84 ellipsoid is used, a = 6378137 m, f = 1/298.257223563. If entering vertices as UTM/UPS or MGRS coordinates, use the default ellipsoid, since the conversion of these coordinates to latitude and longitude always uses the WGS84 parameters.

--comment-delimiter
diff --git a/gtsam/3rdparty/GeographicLib/man/Planimeter.pod b/gtsam/3rdparty/GeographicLib/man/Planimeter.pod index 828ddc100..d78afca8c 100644 --- a/gtsam/3rdparty/GeographicLib/man/Planimeter.pod +++ b/gtsam/3rdparty/GeographicLib/man/Planimeter.pod @@ -78,7 +78,7 @@ is allowed for I. (Also, if I E 1, the flattening is set to 1/I.) By default, the WGS84 ellipsoid is used, I = 6378137 m, I = 1/298.257223563. If entering vertices as UTM/UPS or MGRS coordinates, use the default ellipsoid, since the conversion of these -coordinates to latitude and longitude uses the WGS84 parameters. +coordinates to latitude and longitude always uses the WGS84 parameters. =item B<--comment-delimiter> diff --git a/gtsam/3rdparty/GeographicLib/man/Planimeter.usage b/gtsam/3rdparty/GeographicLib/man/Planimeter.usage index 3a45ad85d..b96b12f9f 100644 --- a/gtsam/3rdparty/GeographicLib/man/Planimeter.usage +++ b/gtsam/3rdparty/GeographicLib/man/Planimeter.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " Planimeter --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/Planimeter.1.html\n"; +" http://geographiclib.sf.net/1.35/Planimeter.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" @@ -76,7 +76,7 @@ int usage(int retval, bool brief) { " default, the WGS84 ellipsoid is used, a = 6378137 m, f =\n" " 1/298.257223563. If entering vertices as UTM/UPS or MGRS\n" " coordinates, use the default ellipsoid, since the conversion of\n" -" these coordinates to latitude and longitude uses the WGS84\n" +" these coordinates to latitude and longitude always uses the WGS84\n" " parameters.\n" "\n" " --comment-delimiter\n" diff --git a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1 b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1 index 0b03ff6ca..3a5d3d115 100644 --- a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1 +++ b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "TRANSVERSEMERCATORPROJ 1" -.TH TRANSVERSEMERCATORPROJ 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH TRANSVERSEMERCATORPROJ 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1.html b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1.html index c43bf74da..8b28a7d87 100644 --- a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1.html +++ b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1.html @@ -2,7 +2,7 @@ - +TransverseMercatorProj(1) diff --git a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.usage b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.usage index 189744e8b..62d6045a7 100644 --- a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.usage +++ b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " TransverseMercatorProj --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/TransverseMercatorProj.1.html\n"; +" http://geographiclib.sf.net/1.35/TransverseMercatorProj.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/matlab/geodesicdirect.cpp b/gtsam/3rdparty/GeographicLib/matlab/geodesicdirect.cpp index 770feecd0..06a35e607 100644 --- a/gtsam/3rdparty/GeographicLib/matlab/geodesicdirect.cpp +++ b/gtsam/3rdparty/GeographicLib/matlab/geodesicdirect.cpp @@ -2,7 +2,7 @@ * \file geodesicdirect.cpp * \brief Matlab mex file for geographic to UTM/UPS conversions * - * Copyright (c) Charles Karney (2010-2011) and licensed + * Copyright (c) Charles Karney (2010-2013) and licensed * under the MIT/X11 License. For more information, see * http://geographiclib.sourceforge.net/ **********************************************************************/ @@ -17,11 +17,51 @@ #include #include +#include #include using namespace std; using namespace GeographicLib; +template void +compute(double a, double f, mwSize m, const double* geodesic, + double* latlong, double* aux) { + const double* lat1 = geodesic; + const double* lon1 = geodesic + m; + const double* azi1 = geodesic + 2*m; + const double* s12 = geodesic + 3*m; + double* lat2 = latlong; + double* lon2 = latlong + m; + double* azi2 = latlong + 2*m; + double* a12 = NULL; + double* m12 = NULL; + double* M12 = NULL; + double* M21 = NULL; + double* S12 = NULL; + if (aux) { + a12 = aux; + m12 = aux + m; + M12 = aux + 2*m; + M21 = aux + 3*m; + S12 = aux + 4*m; + } + + const G g(a, f); + for (mwIndex i = 0; i < m; ++i) { + if (abs(lat1[i]) <= 90 && + lon1[i] >= -540 && lon1[i] < 540 && + azi1[i] >= -540 && azi1[i] < 540) { + if (aux) + a12[i] = g.Direct(lat1[i], lon1[i], azi1[i], s12[i], + lat2[i], lon2[i], azi2[i], + m12[i], M12[i], M21[i], S12[i]); + else + g.Direct(lat1[i], lon1[i], azi1[i], s12[i], + lat2[i], lon2[i], azi2[i]); + } + } +} + void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] ) { @@ -54,48 +94,22 @@ void mexFunction( int nlhs, mxArray* plhs[], mwSize m = mxGetM(prhs[0]); - double* lat1 = mxGetPr(prhs[0]); - double* lon1 = lat1 + m; - double* azi1 = lat1 + 2*m; - double* s12 = lat1 + 3*m; + const double* geodesic = mxGetPr(prhs[0]); - plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL); - double* lat2 = mxGetPr(plhs[0]); - std::fill(lat2, lat2 + 3*m, Math::NaN()); - double* lon2 = lat2 + m; - double* azi2 = lat2 + 2*m; - double* a12 = NULL; - double* m12 = NULL; - double* M12 = NULL; - double* M21 = NULL; - double* S12 = NULL; - bool aux = nlhs == 2; + double* latlong = mxGetPr(plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL)); + std::fill(latlong, latlong + 3*m, Math::NaN()); - if (aux) { - plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL); - a12 = mxGetPr(plhs[1]); - std::fill(a12, a12 + 5*m, Math::NaN()); - m12 = a12 + m; - M12 = a12 + 2*m; - M21 = a12 + 3*m; - S12 = a12 + 4*m; - } + double* aux = + nlhs == 2 ? mxGetPr(plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL)) : + NULL; + if (aux) + std::fill(aux, aux + 5*m, Math::NaN()); try { - const Geodesic g(a, f); - for (mwIndex i = 0; i < m; ++i) { - if (abs(lat1[i]) <= 90 && - lon1[i] >= -540 && lon1[i] < 540 && - azi1[i] >= -540 && azi1[i] < 540) { - if (aux) - a12[i] = g.Direct(lat1[i], lon1[i], azi1[i], s12[i], - lat2[i], lon2[i], azi2[i], m12[i], - M12[i], M21[i], S12[i]); - else - g.Direct(lat1[i], lon1[i], azi1[i], s12[i], - lat2[i], lon2[i], azi2[i]); - } - } + if (std::abs(f) <= 0.02) + compute(a, f, m, geodesic, latlong, aux); + else + compute(a, f, m, geodesic, latlong, aux); } catch (const std::exception& e) { mexErrMsgTxt(e.what()); diff --git a/gtsam/3rdparty/GeographicLib/matlab/geodesicinverse.cpp b/gtsam/3rdparty/GeographicLib/matlab/geodesicinverse.cpp index c3b1b2ace..f03877f18 100644 --- a/gtsam/3rdparty/GeographicLib/matlab/geodesicinverse.cpp +++ b/gtsam/3rdparty/GeographicLib/matlab/geodesicinverse.cpp @@ -2,7 +2,7 @@ * \file geodesicinverse.cpp * \brief Matlab mex file for geographic to UTM/UPS conversions * - * Copyright (c) Charles Karney (2010-2011) and licensed + * Copyright (c) Charles Karney (2010-2013) and licensed * under the MIT/X11 License. For more information, see * http://geographiclib.sourceforge.net/ **********************************************************************/ @@ -17,11 +17,50 @@ #include #include +#include #include using namespace std; using namespace GeographicLib; +template void +compute(double a, double f, mwSize m, const double* latlong, + double* geodesic, double* aux) { + const double* lat1 = latlong; + const double* lon1 = latlong + m; + const double* lat2 = latlong + 2*m; + const double* lon2 = latlong + 3*m; + double* azi1 = geodesic; + double* azi2 = geodesic + m; + double* s12 = geodesic + 2*m; + double* a12 = NULL; + double* m12 = NULL; + double* M12 = NULL; + double* M21 = NULL; + double* S12 = NULL; + if (aux) { + a12 = aux; + m12 = aux + m; + M12 = aux + 2*m; + M21 = aux + 3*m; + S12 = aux + 4*m; + } + + const G g(a, f); + for (mwIndex i = 0; i < m; ++i) { + if (abs(lat1[i]) <= 90 && lon1[i] >= -540 && lon1[i] < 540 && + abs(lat2[i]) <= 90 && lon2[i] >= -540 && lon2[i] < 540) { + if (aux) + a12[i] = g.Inverse(lat1[i], lon1[i], lat2[i], lon2[i], + s12[i], azi1[i], azi2[i], + m12[i], M12[i], M21[i], S12[i]); + else + g.Inverse(lat1[i], lon1[i], lat2[i], lon2[i], + s12[i], azi1[i], azi2[i]); + } + } +} + void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] ) { @@ -54,47 +93,22 @@ void mexFunction( int nlhs, mxArray* plhs[], mwSize m = mxGetM(prhs[0]); - double* lat1 = mxGetPr(prhs[0]); - double* lon1 = lat1 + m; - double* lat2 = lat1 + 2*m; - double* lon2 = lat1 + 3*m; + const double* latlong = mxGetPr(prhs[0]); - plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL); - double* azi1 = mxGetPr(plhs[0]); - std::fill(azi1, azi1 + 3*m, Math::NaN()); - double* azi2 = azi1 + m; - double* s12 = azi1 + 2*m; - double* a12 = NULL; - double* m12 = NULL; - double* M12 = NULL; - double* M21 = NULL; - double* S12 = NULL; - bool aux = nlhs == 2; + double* geodesic = mxGetPr(plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL)); + std::fill(geodesic, geodesic + 3*m, Math::NaN()); - if (aux) { - plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL); - a12 = mxGetPr(plhs[1]); - std::fill(a12, a12 + 5*m, Math::NaN()); - m12 = a12 + m; - M12 = a12 + 2*m; - M21 = a12 + 3*m; - S12 = a12 + 4*m; - } + double* aux = + nlhs == 2 ? mxGetPr(plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL)) : + NULL; + if (aux) + std::fill(aux, aux + 5*m, Math::NaN()); try { - const Geodesic g(a, f); - for (mwIndex i = 0; i < m; ++i) { - if (abs(lat1[i]) <= 90 && lon1[i] >= -540 && lon1[i] < 540 && - abs(lat2[i]) <= 90 && lon2[i] >= -540 && lon2[i] < 540) { - if (aux) - a12[i] = g.Inverse(lat1[i], lon1[i], lat2[i], lon2[i], - s12[i], azi1[i], azi2[i], - m12[i], M12[i], M21[i], S12[i]); - else - g.Inverse(lat1[i], lon1[i], lat2[i], lon2[i], - s12[i], azi1[i], azi2[i]); - } - } + if (std::abs(f) <= 0.02) + compute(a, f, m, latlong, geodesic, aux); + else + compute(a, f, m, latlong, geodesic, aux); } catch (const std::exception& e) { mexErrMsgTxt(e.what()); diff --git a/gtsam/3rdparty/GeographicLib/matlab/geodesicline.cpp b/gtsam/3rdparty/GeographicLib/matlab/geodesicline.cpp index 4de50e476..7382cc03a 100644 --- a/gtsam/3rdparty/GeographicLib/matlab/geodesicline.cpp +++ b/gtsam/3rdparty/GeographicLib/matlab/geodesicline.cpp @@ -2,7 +2,7 @@ * \file geodesicline.cpp * \brief Matlab mex file for geographic to UTM/UPS conversions * - * Copyright (c) Charles Karney (2010-2011) and licensed + * Copyright (c) Charles Karney (2010-2013) and licensed * under the MIT/X11 License. For more information, see * http://geographiclib.sourceforge.net/ **********************************************************************/ @@ -16,11 +16,40 @@ // -lGeographic geodesicline.cpp #include +#include #include using namespace std; using namespace GeographicLib; +template void +compute(double a, double f, double lat1, double lon1, double azi1, + mwSize m, const double* s12, double* latlong, double* aux) { + double* lat2 = latlong; + double* lon2 = latlong + m; + double* azi2 = latlong + 2*m; + double* a12 = NULL; + double* m12 = NULL; + double* M12 = NULL; + double* M21 = NULL; + double* S12 = NULL; + if (aux) { + a12 = aux; + m12 = aux + m; + M12 = aux + 2*m; + M21 = aux + 3*m; + S12 = aux + 4*m; + } + const G g(a, f); + const L l(g, lat1, lon1, azi1); + for (mwIndex i = 0; i < m; ++i) + if (aux) + a12[i] = l.Position(s12[i], lat2[i], lon2[i], azi2[i], + m12[i], M12[i], M21[i], S12[i]); + else + l.Position(s12[i], lat2[i], lon2[i], azi2[i]); +} + void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] ) { @@ -70,41 +99,24 @@ void mexFunction( int nlhs, mxArray* plhs[], double* s12 = mxGetPr(prhs[3]); - plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL); - double* lat2 = mxGetPr(plhs[0]); - double* lon2 = lat2 + m; - double* azi2 = lat2 + 2*m; - double* a12 = NULL; - double* m12 = NULL; - double* M12 = NULL; - double* M21 = NULL; - double* S12 = NULL; - bool aux = nlhs == 2; - - if (aux) { - plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL); - a12 = mxGetPr(plhs[1]); - m12 = a12 + m; - M12 = a12 + 2*m; - M21 = a12 + 3*m; - S12 = a12 + 4*m; - } + double* latlong = mxGetPr(plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL)); + double* aux = + nlhs == 2 ? mxGetPr(plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL)) : + NULL; try { - const Geodesic g(a, f); if (!(abs(lat1) <= 90)) throw GeographicErr("Invalid latitude"); if (!(lon1 >= -540 || lon1 < 540)) throw GeographicErr("Invalid longitude"); if (!(azi1 >= -540 || azi1 < 540)) throw GeographicErr("Invalid azimuth"); - const GeodesicLine l(g, lat1, lon1, azi1); - for (mwIndex i = 0; i < m; ++i) - if (aux) - a12[i] = l.Position(s12[i], lat2[i], lon2[i], azi2[i], - m12[i], M12[i], M21[i], S12[i]); - else - l.Position(s12[i], lat2[i], lon2[i], azi2[i]); + if (std::abs(f) <= 0.02) + compute + (a, f, lat1, lon1, azi1, m, s12, latlong, aux); + else + compute + (a, f, lat1, lon1, azi1, m, s12, latlong, aux); } catch (const std::exception& e) { mexErrMsgTxt(e.what()); diff --git a/gtsam/3rdparty/GeographicLib/pom.xml b/gtsam/3rdparty/GeographicLib/pom.xml new file mode 100644 index 000000000..2d40845a3 --- /dev/null +++ b/gtsam/3rdparty/GeographicLib/pom.xml @@ -0,0 +1,82 @@ + + 4.0.0 + + + com.sri.vt.majic + majic-parent + 0.1.9-SNAPSHOT + + + com.sri.vt + geographiclib + 1.35-SNAPSHOT + majic-cmake + GeographicLib + + + + os-windows + + + Windows + + + + ON + + + + os-linux + + + Linux + + + + OFF + + + + + + + artifactory-vt + SRI VT Repository + https://artifactory-vt.sarnoff.internal/artifactory/repo + + + + + + + com.sri.vt.majic + build-helper-maven-plugin + + + default-cmake-configure + + + ON + BOTH + OFF + OFF + ${build.netgeographiclib} + + + + + default-cmake-test + + + Release + + + + + + + + diff --git a/gtsam/3rdparty/GeographicLib/src/UTMUPS.cpp b/gtsam/3rdparty/GeographicLib/src/UTMUPS.cpp index 2abf43e11..88c151a6a 100644 --- a/gtsam/3rdparty/GeographicLib/src/UTMUPS.cpp +++ b/gtsam/3rdparty/GeographicLib/src/UTMUPS.cpp @@ -269,21 +269,19 @@ namespace GeographicLib { } void UTMUPS::DecodeEPSG(int epsg, int& zone, bool& northp) throw() { + northp = false; if (epsg >= epsg01N && epsg <= epsg60N) { - zone = epsg - epsg01N + 1; + zone = (epsg - epsg01N) + MINUTMZONE; northp = true; } else if (epsg == epsgN) { zone = UPS; northp = true; } else if (epsg >= epsg01S && epsg <= epsg60S) { - zone = epsg - epsg01S + 1; - northp = false; + zone = (epsg - epsg01S) + MINUTMZONE; } else if (epsg == epsgS) { zone = UPS; - northp = false; } else { zone = INVALID; - northp = false; } } @@ -292,7 +290,7 @@ namespace GeographicLib { if (zone == UPS) epsg = epsgS; else if (zone >= MINUTMZONE && zone <= MAXUTMZONE) - epsg = epsg + (zone - MINUTMZONE) + epsg01S; + epsg = (zone - MINUTMZONE) + epsg01S; if (epsg >= 0 && northp) epsg += epsgN - epsgS; return epsg; diff --git a/gtsam/3rdparty/GeographicLib/tools/CMakeLists.txt b/gtsam/3rdparty/GeographicLib/tools/CMakeLists.txt index 98233fe29..987505817 100644 --- a/gtsam/3rdparty/GeographicLib/tools/CMakeLists.txt +++ b/gtsam/3rdparty/GeographicLib/tools/CMakeLists.txt @@ -22,6 +22,12 @@ foreach (TOOL ${TOOLS}) endforeach () +if (MSVC OR CMAKE_CONFIGURATION_TYPES) + # Add _d suffix for your debug versions of the tools + set_target_properties (${TOOLS} PROPERTIES + DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) +endif () + if (MSVC) get_target_property (_LIBTYPE ${PROJECT_LIBRARIES} TYPE) if (_LIBTYPE STREQUAL "SHARED_LIBRARY") @@ -62,21 +68,23 @@ enable_testing () # Here are the tests. They consists of calling the various tools with # --input-string and matching the output against regular expressions. -add_test (GeoConvert0 GeoConvert -p -3 -m --input-string "33.3 44.4") +add_test (NAME GeoConvert0 + COMMAND GeoConvert -p -3 -m --input-string "33.3 44.4") set_tests_properties (GeoConvert0 PROPERTIES PASS_REGULAR_EXPRESSION "38SMB4484") -add_test (GeoConvert1 GeoConvert -d --input-string "38smb") +add_test (NAME GeoConvert1 COMMAND GeoConvert -d --input-string "38smb") set_tests_properties (GeoConvert1 PROPERTIES PASS_REGULAR_EXPRESSION "32d59'14\\.1\"N 044d27'53\\.4\"E") -add_test (GeoConvert2 GeoConvert -p -2 --input-string "30d30'30\" 30.50833") +add_test (NAME GeoConvert2 + COMMAND GeoConvert -p -2 --input-string "30d30'30\" 30.50833") set_tests_properties (GeoConvert2 PROPERTIES PASS_REGULAR_EXPRESSION "30\\.508 30\\.508") -add_test (GeoConvert3 GeoConvert --junk) +add_test (NAME GeoConvert3 COMMAND GeoConvert --junk) set_tests_properties (GeoConvert3 PROPERTIES WILL_FAIL ON) -add_test (GeoConvert4 GeoConvert --input-string garbage) +add_test (NAME GeoConvert4 COMMAND GeoConvert --input-string garbage) set_tests_properties (GeoConvert4 PROPERTIES WILL_FAIL ON) # Check fix for DMS::Decode bug fixed on 2011-03-22 -add_test (GeoConvert5 GeoConvert --input-string "5d. 0") +add_test (NAME GeoConvert5 COMMAND GeoConvert --input-string "5d. 0") set_tests_properties (GeoConvert5 PROPERTIES WILL_FAIL ON) if (NOT (MSVC AND MSVC_VERSION MATCHES "1[78]..")) # Check fix for DMS::Decode double rounding bug fixed on 2012-11-15 @@ -84,48 +92,49 @@ if (NOT (MSVC AND MSVC_VERSION MATCHES "1[78]..")) # http://connect.microsoft.com/VisualStudio/feedback/details/776287 # OK to skip this test for these compilers because this is a question # of accuracy of the least significant bit. - add_test (GeoConvert6 GeoConvert -p 9 + add_test (NAME GeoConvert6 COMMAND GeoConvert -p 9 --input-string "0 179.99999999999998578") set_tests_properties (GeoConvert6 PROPERTIES PASS_REGULAR_EXPRESSION "179\\.9999999999999[7-9]") endif () -add_test (GeodSolve0 +add_test (NAME GeodSolve0 COMMAND GeodSolve -i -p 0 --input-string "40.6 -73.8 49d01'N 2d33'E") set_tests_properties (GeodSolve0 PROPERTIES PASS_REGULAR_EXPRESSION "53\\.47022 111\\.59367 5853226") -add_test (GeodSolve1 +add_test (NAME GeodSolve1 COMMAND GeodSolve -p 0 --input-string "40d38'23\"N 073d46'44\"W 53d30' 5850e3") set_tests_properties (GeodSolve1 PROPERTIES PASS_REGULAR_EXPRESSION "49\\.01467 2\\.56106 111\\.62947") # Check fix for antipodal prolate bug found 2010-09-04 -add_test (GeodSolve2 +add_test (NAME GeodSolve2 COMMAND GeodSolve -i -p 0 -e 6.4e6 -1/150 --input-string "0.07476 0 -0.07476 180") set_tests_properties (GeodSolve2 PROPERTIES PASS_REGULAR_EXPRESSION "90\\.00078 90\\.00078 20106193") # Another check for similar bug -add_test (GeodSolve3 +add_test (NAME GeodSolve3 COMMAND GeodSolve -i -p 0 -e 6.4e6 -1/150 --input-string "0.1 0 -0.1 180") set_tests_properties (GeodSolve3 PROPERTIES PASS_REGULAR_EXPRESSION "90\\.00105 90\\.00105 20106193") # Check fix for short line bug found 2010-05-21 -add_test (GeodSolve4 +add_test (NAME GeodSolve4 COMMAND GeodSolve -i --input-string "36.493349428792 0 36.49334942879201 .0000008") set_tests_properties (GeodSolve4 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 0\\.072") # Check fix for point2=pole bug found 2010-05-03 (but only with long double) -add_test (GeodSolve5 GeodSolve -p 0 --input-string "0.01777745589997 30 0 10e6") +add_test (NAME GeodSolve5 + COMMAND GeodSolve -p 0 --input-string "0.01777745589997 30 0 10e6") set_tests_properties (GeodSolve5 PROPERTIES PASS_REGULAR_EXPRESSION "90\\.00000 -150\\.00000 -180\\.00000;90\\.00000 30\\.00000 0\\.00000") # Check fix for volatile sbet12a bug found 2011-06-25 (gcc 4.4.4 x86 -O3) # Found again on 2012-03-27 with tdm-mingw32 (g++ 4.6.1). -add_test (GeodSolve6 GeodSolve -i --input-string +add_test (NAME GeodSolve6 COMMAND GeodSolve -i --input-string "88.202499451857 0 -88.202499451857 179.981022032992859592") -add_test (GeodSolve7 GeodSolve -i --input-string +add_test (NAME GeodSolve7 COMMAND GeodSolve -i --input-string "89.262080389218 0 -89.262080389218 179.992207982775375662") -add_test (GeodSolve8 GeodSolve -i --input-string +add_test (NAME GeodSolve8 COMMAND GeodSolve -i --input-string "89.333123580033 0 -89.333123580032997687 179.99295812360148422") set_tests_properties (GeodSolve6 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 20003898.214") @@ -135,39 +144,42 @@ set_tests_properties (GeodSolve8 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 20003926.881") # Check fix for volatile x bug found 2011-06-25 (gcc 4.4.4 x86 -O3) -add_test (GeodSolve9 GeodSolve -i --input-string +add_test (NAME GeodSolve9 COMMAND GeodSolve -i --input-string "56.320923501171 0 -56.320923501171 179.664747671772880215") set_tests_properties (GeodSolve9 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 19993558.287") # Check fix for adjust tol1_ bug found 2011-06-25 (Visual Studio 10 rel + debug) -add_test (GeodSolve10 GeodSolve -i --input-string +add_test (NAME GeodSolve10 COMMAND GeodSolve -i --input-string "52.784459512564 0 -52.784459512563990912 179.634407464943777557") set_tests_properties (GeodSolve10 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 19991596.095") # Check fix for bet2 = -bet1 bug found 2011-06-25 (Visual Studio 10 rel + debug) -add_test (GeodSolve11 GeodSolve -i --input-string +add_test (NAME GeodSolve11 COMMAND GeodSolve -i --input-string "48.522876735459 0 -48.52287673545898293 179.599720456223079643") set_tests_properties (GeodSolve11 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 19989144.774") # Check fix for inverse geodesics on extreme prolate/oblate ellipsoids # Reported 2012-08-29 Stefan Guenther ; fixed 2012-10-07 -add_test (GeodSolve12 +add_test (NAME GeodSolve12 COMMAND GeodSolve -i -e 89.8 -1.83 -p 0 --input-string "0 0 -10 160") -add_test (GeodSolve13 +add_test (NAME GeodSolve13 COMMAND GeodSolve -i -e 89.8 -1.83 -p 0 --input-string "0 0 -10 160" -E) set_tests_properties (GeodSolve12 GeodSolve13 PROPERTIES PASS_REGULAR_EXPRESSION "120\\.27.* 105\\.15.* 267") # Check fix for pole-encircling bug found 2011-03-16 -add_test (Planimeter0 Planimeter --input-string "89 0;89 90;89 180;89 270") -add_test (Planimeter1 +add_test (NAME Planimeter0 + COMMAND Planimeter --input-string "89 0;89 90;89 180;89 270") +add_test (NAME Planimeter1 COMMAND Planimeter -r --input-string "-89 0;-89 90;-89 180;-89 270") -add_test (Planimeter2 Planimeter --input-string "0 -1;-1 0;0 1;1 0") -add_test (Planimeter3 Planimeter --input-string "90 0; 0 0; 0 90") -add_test (Planimeter4 Planimeter -l --input-string "90 0; 0 0; 0 90") +add_test (NAME Planimeter2 + COMMAND Planimeter --input-string "0 -1;-1 0;0 1;1 0") +add_test (NAME Planimeter3 COMMAND Planimeter --input-string "90 0; 0 0; 0 90") +add_test (NAME Planimeter4 + COMMAND Planimeter -l --input-string "90 0; 0 0; 0 90") set_tests_properties (Planimeter0 PROPERTIES PASS_REGULAR_EXPRESSION "4 631819\\.8745[0-9]+ 2495230567[78]\\.[0-9]+") @@ -182,56 +194,65 @@ set_tests_properties (Planimeter3 set_tests_properties (Planimeter4 PROPERTIES PASS_REGULAR_EXPRESSION "3 20020719\\.[0-9]+") # Check fix for Planimeter pole crossing bug found 2011-06-24 -add_test (Planimeter5 Planimeter --input-string "89,0.1;89,90.1;89,-179.9") +add_test (NAME Planimeter5 + COMMAND Planimeter --input-string "89,0.1;89,90.1;89,-179.9") set_tests_properties (Planimeter5 PROPERTIES PASS_REGULAR_EXPRESSION "3 539297\\.[0-9]+ 1247615283[89]\\.[0-9]+") # Check fix for Planimeter lon12 rounding bug found 2012-12-03 -add_test (Planimeter6 Planimeter --input-string "9 -0.00000000000001;9 180;9 0") -add_test (Planimeter7 Planimeter --input-string "9 0.00000000000001;9 0;9 180") -add_test (Planimeter8 Planimeter --input-string "9 0.00000000000001;9 180;9 0") -add_test (Planimeter9 Planimeter --input-string "9 -0.00000000000001;9 0;9 180") +add_test (NAME Planimeter6 + COMMAND Planimeter --input-string "9 -0.00000000000001;9 180;9 0") +add_test (NAME Planimeter7 + COMMAND Planimeter --input-string "9 0.00000000000001;9 0;9 180") +add_test (NAME Planimeter8 + COMMAND Planimeter --input-string "9 0.00000000000001;9 180;9 0") +add_test (NAME Planimeter9 + COMMAND Planimeter --input-string "9 -0.00000000000001;9 0;9 180") set_tests_properties (Planimeter6 Planimeter7 Planimeter8 Planimeter9 PROPERTIES PASS_REGULAR_EXPRESSION "3 36026861\\.[0-9]+ -?0.0[0-9]+") # Check fix for AlbersEqualArea::Reverse bug found 2011-05-01 -add_test (ConicProj0 +add_test (NAME ConicProj0 COMMAND ConicProj -a 40d58 39d56 -l 77d45W -r --input-string "220e3 -52e3") set_tests_properties (ConicProj0 PROPERTIES PASS_REGULAR_EXPRESSION "39\\.95[0-9]+ -75\\.17[0-9]+ 1\\.67[0-9]+ 0\\.99[0-9]+") # Check fix for AlbersEqualArea prolate bug found 2012-05-15 -add_test (ConicProj1 +add_test (NAME ConicProj1 COMMAND ConicProj -a 0 0 -e 6.4e6 -0.5 -r --input-string "0 8605508") set_tests_properties (ConicProj1 PROPERTIES PASS_REGULAR_EXPRESSION "^85\\.00") # Check fix for LambertConformalConic::Forward bug found 2012-07-14 -add_test (ConicProj2 ConicProj -c -30 -30 --input-string "-30 0") +add_test (NAME ConicProj2 COMMAND ConicProj -c -30 -30 --input-string "-30 0") set_tests_properties (ConicProj2 PROPERTIES PASS_REGULAR_EXPRESSION "^-?0\\.0+ -?0\\.0+ -?0\\.0+ 1\\.0+") # Check fixes for LambertConformalConic::Reverse overflow bugs found 2012-07-14 -add_test (ConicProj3 ConicProj -r -c 0 0 --input-string "1113195 -1e10") +add_test (NAME ConicProj3 + COMMAND ConicProj -r -c 0 0 --input-string "1113195 -1e10") set_tests_properties (ConicProj3 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ 10\\.00[0-9]+ ") -add_test (ConicProj4 ConicProj -r -c 0 0 --input-string "1113195 inf") +add_test (NAME ConicProj4 + COMMAND ConicProj -r -c 0 0 --input-string "1113195 inf") set_tests_properties (ConicProj4 PROPERTIES PASS_REGULAR_EXPRESSION "^90\\.0+ 10\\.00[0-9]+ ") -add_test (ConicProj5 ConicProj -r -c 45 45 --input-string "0 -1e100") +add_test (NAME ConicProj5 + COMMAND ConicProj -r -c 45 45 --input-string "0 -1e100") set_tests_properties (ConicProj5 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ -?0\\.00[0-9]+ ") -add_test (ConicProj6 ConicProj -r -c 45 45 --input-string "0 -inf") +add_test (NAME ConicProj6 COMMAND ConicProj -r -c 45 45 --input-string "0 -inf") set_tests_properties (ConicProj6 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ -?0\\.00[0-9]+ ") -add_test (ConicProj7 ConicProj -r -c 90 90 --input-string "0 -1e150") +add_test (NAME ConicProj7 + COMMAND ConicProj -r -c 90 90 --input-string "0 -1e150") set_tests_properties (ConicProj7 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ -?0\\.00[0-9]+ ") -add_test (ConicProj8 ConicProj -r -c 90 90 --input-string "0 -inf") +add_test (NAME ConicProj8 COMMAND ConicProj -r -c 90 90 --input-string "0 -inf") set_tests_properties (ConicProj8 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ -?0\\.00[0-9]+ ") -add_test (CartConvert0 +add_test (NAME CartConvert0 COMMAND CartConvert -e 6.4e6 1/100 -r --input-string "10e3 0 1e3") -add_test (CartConvert1 +add_test (NAME CartConvert1 COMMAND CartConvert -e 6.4e6 -1/100 -r --input-string "1e3 0 10e3") set_tests_properties (CartConvert0 PROPERTIES PASS_REGULAR_EXPRESSION @@ -242,60 +263,60 @@ set_tests_properties (CartConvert1 # Test fix to bad meridian convergence at pole with # TransverseMercatorExact found 2013-06-26 -add_test (TransverseMercatorProj0 +add_test (NAME TransverseMercatorProj0 COMMAND TransverseMercatorProj -k 1 --input-string "90 75") set_tests_properties (TransverseMercatorProj0 PROPERTIES PASS_REGULAR_EXPRESSION "^0\\.0+ 10001965\\.7293[0-9]+ 75\\.0+ 1\\.0+") # Test fix to bad scale at pole with TransverseMercatorExact # found 2013-06-30 (quarter meridian = 10001965.7293127228128889202m) -add_test (TransverseMercatorProj1 +add_test (NAME TransverseMercatorProj1 COMMAND TransverseMercatorProj -k 1 -r --input-string "0 10001965.7293127228") set_tests_properties (TransverseMercatorProj1 PROPERTIES PASS_REGULAR_EXPRESSION "90\\.0+ 0\\.0+ 0\\.0+ (1\\.0+|0\\.9999+)") if (EXISTS ${GEOGRAPHICLIB_DATA}/geoids/egm96-5.pgm) # Check fix for single-cell cache bug found 2010-11-23 - add_test (GeoidEval0 GeoidEval -n egm96-5 --input-string "0d1 0d1;0d4 0d4") + add_test (NAME GeoidEval0 + COMMAND GeoidEval -n egm96-5 --input-string "0d1 0d1;0d4 0d4") set_tests_properties (GeoidEval0 - PROPERTIES PASS_REGULAR_EXPRESSION "^17\\.1[56].. -17\\.1[45]..") + PROPERTIES PASS_REGULAR_EXPRESSION "^17\\.1[56]..\n17\\.1[45]..") endif () if (EXISTS ${GEOGRAPHICLIB_DATA}/magnetic/wmm2010.wmm) # Test case from WMM2010_Report.pdf, Sec 1.5, pp 14-15: # t = 2012.5, lat = -80, lon = 240, h = 100e3 - add_test (MagneticField0 + add_test (NAME MagneticField0 COMMAND MagneticField -n wmm2010 -p 10 -r --input-string "2012.5 -80 240 100e3") - add_test (MagneticField1 + add_test (NAME MagneticField1 COMMAND MagneticField -n wmm2010 -p 10 -r -t 2012.5 --input-string "-80 240 100e3") - add_test (MagneticField2 + add_test (NAME MagneticField2 COMMAND MagneticField -n wmm2010 -p 10 -r -c 2012.5 -80 100e3 --input-string "240") set_tests_properties (MagneticField0 PROPERTIES PASS_REGULAR_EXPRESSION - " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .* -.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") + " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .*\n.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") set_tests_properties (MagneticField1 PROPERTIES PASS_REGULAR_EXPRESSION - " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .* -.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") + " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .*\n.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") set_tests_properties (MagneticField2 PROPERTIES PASS_REGULAR_EXPRESSION - " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .* -.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") + " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .*\n.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") endif () if (EXISTS ${GEOGRAPHICLIB_DATA}/gravity/egm2008.egm) # Verify no overflow at poles with high degree model - add_test (Gravity0 Gravity -n egm2008 -p 6 --input-string "90 110 0") + add_test (NAME Gravity0 + COMMAND Gravity -n egm2008 -p 6 --input-string "90 110 0") set_tests_properties (Gravity0 PROPERTIES PASS_REGULAR_EXPRESSION "-0\\.000146 0\\.000078 -9\\.832294") # Check fix for invR bug in GravityCircle found by Mathieu Peyrega on # 2013-04-09 - add_test (Gravity1 Gravity -n egm2008 -A -c -18 4000 --input-string "-86") + add_test (NAME Gravity1 + COMMAND Gravity -n egm2008 -A -c -18 4000 --input-string "-86") set_tests_properties (Gravity1 PROPERTIES PASS_REGULAR_EXPRESSION "-7\\.438 1\\.305 -1\\.563") - add_test (Gravity2 Gravity -n egm2008 -D -c -18 4000 --input-string "-86") + add_test (NAME Gravity2 + COMMAND Gravity -n egm2008 -D -c -18 4000 --input-string "-86") set_tests_properties (Gravity2 PROPERTIES PASS_REGULAR_EXPRESSION "7\\.404 -6\\.168 7\\.616") endif () diff --git a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt b/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt index 827e50b34..4c4e2c03f 100644 --- a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt +++ b/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt @@ -4,11 +4,18 @@ project(METIS) # Add flags for currect directory and below if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") add_definitions(-Wno-unused-variable) - add_definitions(-Wno-sometimes-uninitialized) + if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 5.0 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 5.0) + add_definitions(-Wno-sometimes-uninitialized) + endif() endif() add_definitions(-Wno-unknown-pragmas) -add_definitions(-Wunused-but-set-variable) + +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.6 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 4.6) + add_definitions(-Wno-unused-but-set-variable) + endif() +endif() set(GKLIB_PATH ${PROJECT_SOURCE_DIR}/GKlib CACHE PATH "path to GKlib") set(SHARED FALSE CACHE BOOL "build a shared library") diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 7ed6c7cd7..69e841e57 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -103,6 +103,11 @@ public: /** Check for equality within tolerance to implement Testable */ bool equals(const FastSet& other, double tol = 1e-9) const { return FastSetTestableHelper::equals(*this, other, tol); } + /** insert another set: handy for MATLAB access */ + void merge(const FastSet& other) { + Base::insert(other.begin(),other.end()); + } + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 3b16beb51..f51197cf2 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -591,41 +591,17 @@ Matrix3 skewSymmetric(double wx, double wy, double wz) } /* ************************************************************************* */ -/** Numerical Recipes in C wrappers - * create Numerical Recipes in C structure - * pointers are subtracted by one to provide base 1 access - */ -/* ************************************************************************* */ -// FIXME: assumes row major, rather than column major -//double** createNRC(Matrix& A) { -// const size_t m=A.rows(); -// double** a = new double* [m]; -// for(size_t i = 0; i < m; i++) -// a[i] = &A(i,0)-1; -// return a; -//} - -/* ****************************************** - * - * Modified from Justin's codebase - * - * Idea came from other public domain code. Takes a S.P.D. matrix - * and computes the LL^t decomposition. returns L, which is lower - * triangular. Note this is the opposite convention from Matlab, - * which calculates Q'Q where Q is upper triangular. - * - * ******************************************/ Matrix LLt(const Matrix& A) { - Matrix L = zeros(A.rows(), A.rows()); - Eigen::LLT llt; - llt.compute(A); + Eigen::LLT llt(A); return llt.matrixL(); } +/* ************************************************************************* */ Matrix RtR(const Matrix &A) { - return LLt(A).transpose(); + Eigen::LLT llt(A); + return llt.matrixU(); } /* @@ -633,46 +609,21 @@ Matrix RtR(const Matrix &A) */ Matrix cholesky_inverse(const Matrix &A) { - // FIXME: replace with real algorithm - return A.inverse(); - -// Matrix L = LLt(A); -// Matrix inv(eye(A.rows())); -// inplace_solve (L, inv, BNU::lower_tag ()); -// return BNU::prod(trans(inv), inv); + Eigen::LLT llt(A); + Matrix inv = eye(A.rows()); + llt.matrixU().solveInPlace(inv); + return inv*inv.transpose(); } -#if 0 /* ************************************************************************* */ -// TODO, would be faster with Cholesky -Matrix inverse_square_root(const Matrix& A) { - size_t m = A.cols(), n = A.rows(); - if (m!=n) - throw invalid_argument("inverse_square_root: A must be square"); - - // Perform SVD, TODO: symmetric SVD? - Matrix U,V; - Vector S; - svd(A,U,S,V); - - // invert and sqrt diagonal of S - // We also arbitrarily choose sign to make result have positive signs - for(size_t i = 0; i llt(A); Matrix inv = eye(A.rows()); - R.triangularView().solveInPlace(inv); + llt.matrixU().solveInPlace(inv); return inv.transpose(); } @@ -717,7 +668,7 @@ Matrix expm(const Matrix& A, size_t K) { /* ************************************************************************* */ Matrix Cayley(const Matrix& A) { - int n = A.cols(); + size_t n = A.cols(); assert(A.rows() == n); // original diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index caee66851..6386bd526 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -418,7 +418,7 @@ GTSAM_EXPORT Matrix3 skewSymmetric(double wx, double wy, double wz); template inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { return skewSymmetric(w(0),w(1),w(2));} -/** Use SVD to calculate inverse square root of a matrix */ +/** Use Cholesky to calculate inverse square root of a matrix */ GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A); /** Calculate the LL^t decomposition of a S.P.D matrix */ diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index d95fe84b7..546b6a7f1 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -53,7 +53,7 @@ SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf( VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial( DenseIndex nFrontals) { // Do dense elimination - if (!blockStart() == 0) + if (blockStart() != 0) throw std::invalid_argument( "Can only do Cholesky when the SymmetricBlockMatrix is not a restricted view, i.e. when blockStart == 0."); if (!gtsam::choleskyPartial(matrix_, offset(nFrontals))) diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 45edbf6af..5dcc79a68 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -236,6 +236,10 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { + // Fill in the lower triangle part of the matrix, so boost::serialization won't + // complain about uninitialized data with an input_stream_error exception + // http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/exceptions.html#stream_error + matrix_.triangularView() = matrix_.triangularView().transpose(); ar & BOOST_SERIALIZATION_NVP(matrix_); ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); ar & BOOST_SERIALIZATION_NVP(blockStart_); diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 828208361..4e857b143 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -459,6 +459,22 @@ TEST( matrix, scale_rows_mask ) EXPECT(assert_equal(actual, expected)); } +/* ************************************************************************* */ +TEST( matrix, skewSymmetric ) +{ + double wx = 1, wy = 2, wz = 3; + Matrix3 actual = skewSymmetric(wx,wy,wz); + + Matrix expected(3,3); + expected << 0, -3, 2, + 3, 0, -1, + -2, 1, 0; + + EXPECT(assert_equal(actual, expected)); + +} + + /* ************************************************************************* */ TEST( matrix, equal ) { @@ -1006,22 +1022,32 @@ TEST( matrix, inverse_square_root ) /* *********************************************************************** */ // M was generated as the covariance of a set of random numbers. L that // we are checking against was generated via chol(M)' on octave +namespace cholesky { +Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463, + 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, + 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, + 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, + -0.0021113, 0.0036415, 0.0909464); + +Matrix expected = (Matrix(5, 5) << + 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, + -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, + 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, + 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, + 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); +} TEST( matrix, LLt ) { - Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463, - 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, - 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, - 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, - -0.0021113, 0.0036415, 0.0909464); + EQUALITY(cholesky::expected, LLt(cholesky::M)); +} +TEST( matrix, RtR ) +{ + EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M)); +} - Matrix expected = (Matrix(5, 5) << - 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, - -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, - 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, - 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, - 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); - - EQUALITY(expected, LLt(M)); +TEST( matrix, cholesky_inverse ) +{ + EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 5453e1481..ced05086b 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -30,7 +30,7 @@ namespace gtsam { */ class GTSAM_EXPORT Cal3DS2 : public DerivedValue { -private: +protected: double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point double k1_, k2_ ; // radial 2nd-order and 4th-order diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp new file mode 100644 index 000000000..808cb84a4 --- /dev/null +++ b/gtsam/geometry/Cal3Unified.cpp @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3Unified.cpp + * @date Mar 8, 2014 + * @author Jing Dong + */ + +#include +#include +#include +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +Cal3Unified::Cal3Unified(const Vector &v): + Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} + +/* ************************************************************************* */ +Vector Cal3Unified::vector() const { + return (Vector(10) << Base::vector(), xi_); +} + +/* ************************************************************************* */ +void Cal3Unified::print(const std::string& s) const { + Base::print(s); + gtsam::print((Vector)(Vector(1) << xi_), s + ".xi"); +} + +/* ************************************************************************* */ +bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { + if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || + fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || + fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol || + fabs(xi_ - K.xi_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +Point2 Cal3Unified::uncalibrate(const Point2& p, + boost::optional H1, + boost::optional H2) const { + + // this part of code is modified from Cal3DS2, + // since the second part of this model (after project to normalized plane) + // is same as Cal3DS2 + + // parameters + const double xi = xi_; + + // Part1: project 3D space to NPlane + const double xs = p.x(), ys = p.y(); // normalized points in 3D space + const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0); + const double xi_sqrt_nx = 1.0 / (1 + xi * sqrt_nx); + const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx; + const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane + + // Part2: project NPlane point to pixel plane: use Cal3DS2 + Point2 m(x,y); + Matrix H1base, H2base; // jacobians from Base class + Point2 puncalib = Base::uncalibrate(m, H1base, H2base); + + // Inlined derivative for calibration + if (H1) { + // part1 + Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2, + -ys * sqrt_nx * xi_sqrt_nx2); + Matrix DDS2U = H2base * DU; + + *H1 = collect(2, &H1base, &DDS2U); + } + // Inlined derivative for points + if (H2) { + // part1 + const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx; + const double mid = -(xi * xs*ys) * denom; + Matrix DU = (Matrix(2, 2) << + (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, + mid, (sqrt_nx + xi*(xs*xs + 1)) * denom); + + *H2 = H2base * DU; + } + + return puncalib; +} + +/* ************************************************************************* */ +Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const { + + // calibrate point to Nplane use base class::calibrate() + Point2 pnplane = Base::calibrate(pi, tol); + + // call nplane to space + return this->nPlaneToSpace(pnplane); +} +/* ************************************************************************* */ +Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const { + + const double x = p.x(), y = p.y(); + const double xy2 = x * x + y * y; + const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1); + + return Point2((sq_xy * x / (sq_xy - xi_)), (sq_xy * y / (sq_xy - xi_))); +} + +/* ************************************************************************* */ +Point2 Cal3Unified::spaceToNPlane(const Point2& p) const { + + const double x = p.x(), y = p.y(); + const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1); + + return Point2((x / sq_xy), (y / sq_xy)); +} + +/* ************************************************************************* */ +Cal3Unified Cal3Unified::retract(const Vector& d) const { + return Cal3Unified(vector() + d); +} + +/* ************************************************************************* */ +Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const { + return T2.vector() - vector(); +} + +} +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h new file mode 100644 index 000000000..a1d47332a --- /dev/null +++ b/gtsam/geometry/Cal3Unified.h @@ -0,0 +1,146 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3Unified.h + * @brief Unified Calibration Model, see Mei07icra for details + * @date Mar 8, 2014 + * @author Jing Dong + */ + +/** + * @addtogroup geometry + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * @brief Calibration of a omni-directional camera with mirror + lens radial distortion + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Cal3Unified : public Cal3DS2 { + + typedef Cal3Unified This; + typedef Cal3DS2 Base; + +private: + + double xi_; // mirror parameter + + // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] + // Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})] + // rr = Pn.x^2 + Pn.y^2 + // \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; + // k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] + // pi = K*pn + +public: + //Matrix K() const ; + //Eigen::Vector4d k() const { return Base::k(); } + Vector vector() const ; + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3Unified() : Base(), xi_(0) {} + + Cal3Unified(double fx, double fy, double s, double u0, double v0, + double k1, double k2, double k3 = 0.0, double k4 = 0.0, double xi = 0.0) : + Base(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {} + + /// @} + /// @name Advanced Constructors + /// @{ + + Cal3Unified(const Vector &v) ; + + /// @} + /// @name Testable + /// @{ + + /// print with optional string + void print(const std::string& s = "") const ; + + /// assert equality up to a tolerance + bool equals(const Cal3Unified& K, double tol = 10e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// mirror parameter + inline double xi() const { return xi_;} + + /** + * convert intrinsic coordinates xy to image coordinates uv + * @param p point in intrinsic coordinates + * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p, + boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const ; + + /// Conver a pixel coordinate to ideal coordinate + Point2 calibrate(const Point2& p, const double tol=1e-5) const; + + /// Convert a 3D point to normalized unit plane + Point2 spaceToNPlane(const Point2& p) const; + + /// Convert a normalized unit plane point to 3D space + Point2 nPlaneToSpace(const Point2& p) const; + + /// @} + /// @name Manifold + /// @{ + + /// Given delta vector, update calibration + Cal3Unified retract(const Vector& d) const ; + + /// Given a different calibration, calculate update to obtain it + Vector localCoordinates(const Cal3Unified& T2) const ; + + /// Return dimensions of calibration manifold object + virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + + /// Return dimensions of calibration manifold object + static size_t Dim() { return 10; } //TODO: make a final dimension variable + +private: + + /// @} + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & boost::serialization::make_nvp("Cal3Unified", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(xi_); + } + + /// @} + +}; + +} + diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 7de68c7a1..3c9568c3d 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -20,7 +20,7 @@ namespace gtsam { * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ -class EssentialMatrix: public DerivedValue { +class GTSAM_EXPORT EssentialMatrix: public DerivedValue { private: @@ -150,10 +150,37 @@ public: /// @{ /// stream to stream - friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E); + GTSAM_EXPORT friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E); /// stream from stream - friend std::istream& operator >>(std::istream& is, EssentialMatrix& E); + GTSAM_EXPORT friend std::istream& operator >>(std::istream& is, EssentialMatrix& E); + + /// @} + +private: + + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("EssentialMatrix", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(aRb_); + ar & BOOST_SERIALIZATION_NVP(aTb_); + + ar & boost::serialization::make_nvp("E11", E_(0,0)); + ar & boost::serialization::make_nvp("E12", E_(0,1)); + ar & boost::serialization::make_nvp("E13", E_(0,2)); + ar & boost::serialization::make_nvp("E21", E_(1,0)); + ar & boost::serialization::make_nvp("E22", E_(1,1)); + ar & boost::serialization::make_nvp("E23", E_(1,2)); + ar & boost::serialization::make_nvp("E31", E_(2,0)); + ar & boost::serialization::make_nvp("E32", E_(2,1)); + ar & boost::serialization::make_nvp("E33", E_(2,2)); + } /// @} diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 88359e0f8..db4c8da83 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -151,9 +151,9 @@ Vector Unit3::localCoordinates(const Unit3& y) const { double dot = p.dot(q); // Check for special cases - if (std::abs(dot - 1.0) < 1e-20) + if (std::abs(dot - 1.0) < 1e-16) return (Vector(2) << 0, 0); - else if (std::abs(dot + 1.0) < 1e-20) + else if (std::abs(dot + 1.0) < 1e-16) return (Vector(2) << M_PI, 0); else { // no special case diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 9e2ef0945..39e2c9799 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -27,7 +27,7 @@ namespace gtsam { /// Represents a 3D point on a unit sphere. -class Unit3: public DerivedValue { +class GTSAM_EXPORT Unit3: public DerivedValue { private: @@ -136,6 +136,24 @@ public: Vector localCoordinates(const Unit3& s) const; /// @} + +private: + + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Unit3", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(B_); + } + + /// @} + }; } // namespace gtsam diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp new file mode 100644 index 000000000..b260415f1 --- /dev/null +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -0,0 +1,102 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCal3Unify.cpp + * @brief Unit tests for transform derivatives + */ + +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unified) + +/* +ground truth from matlab, code : +X = [0.5 0.7 1]'; +V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; +[P, J] = spaceToImgPlane(X, V); +matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox +*/ + +static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1); +static Point2 p(0.5, 0.7); + +/* ************************************************************************* */ +TEST( Cal3Unified, uncalibrate) +{ + Point2 p_i(364.7791831734982, 305.6677211952602) ; + Point2 q = K.uncalibrate(p); + CHECK(assert_equal(q,p_i)); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, spaceNplane) +{ + Point2 q = K.spaceToNPlane(p); + CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q)); + CHECK(assert_equal(p, K.nPlaneToSpace(q))); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, calibrate) +{ + Point2 pi = K.uncalibrate(p); + Point2 pn_hat = K.calibrate(pi); + CHECK( p.equals(pn_hat, 1e-8)); +} + +Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); } + +/* ************************************************************************* */ +TEST( Cal3Unified, Duncalibrate1) +{ + Matrix computed; + K.uncalibrate(p, computed, boost::none); + Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); + CHECK(assert_equal(numerical,computed,1e-6)); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, Duncalibrate2) +{ + Matrix computed; + K.uncalibrate(p, boost::none, computed); + Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); + CHECK(assert_equal(numerical,computed,1e-6)); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, assert_equal) +{ + CHECK(assert_equal(K,K,1e-9)); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, retract) +{ + Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1); + Vector d(10); + d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1; + Cal3Unified actual = K.retract(d); + CHECK(assert_equal(expected,actual,1e-9)); + CHECK(assert_equal(d,K.localCoordinates(actual),1e-9)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index b3ff907d9..9ad30bc41 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -125,7 +125,7 @@ TEST (EssentialMatrix, rotate) { } // avoid exception Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), // expH2 = numericalDerivative22(rotate_, bodyE, cRb); - EXPECT(assert_equal(expH1, actH1, 1e-8)); + EXPECT(assert_equal(expH1, actH1, 1e-7)); // Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8)); } @@ -149,7 +149,7 @@ TEST (EssentialMatrix, FromPose3_b) { EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); - EXPECT(assert_equal(expectedH, actualH, 1e-8)); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } //************************************************************************* diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index dcf3b2596..6ed49d0d9 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -183,12 +183,11 @@ TEST( PinholeCamera, Dproject) } /* ************************************************************************* */ -static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) { - Point3 point(point2D.x(), point2D.y(), 1.0); - return Camera(pose,cal).projectPointAtInfinity(point); -} - -/* ************************************************************************* */ +//static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) { +// Point3 point(point2D.x(), point2D.y(), 1.0); +// return Camera(pose,cal).projectPointAtInfinity(point); +//} +// //TEST( PinholeCamera, Dproject_Infinity) //{ // Matrix Dpose, Dpoint, Dcal; @@ -202,7 +201,7 @@ static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const C // CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); // CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); //} - +// /* ************************************************************************* */ static Point2 project4(const Camera& camera, const Point3& point) { return camera.project2(point); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index ab0d12b9e..65c610c25 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -81,7 +81,7 @@ TEST( Point3, stream) { TEST (Point3, normalize) { Matrix actualH; Point3 point(1, -2, 3); // arbitrary point - Point3 expected(point / sqrt(14)); + Point3 expected(point / sqrt(14.0)); EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8)); Matrix expectedH = numericalDerivative11( boost::bind(&Point3::normalize, _1, boost::none), point); diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index 2725df061..eb45ea60f 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -43,7 +44,7 @@ Point3 point3_(const Unit3& p) { TEST(Unit3, point3) { vector ps; ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0) - / sqrt(2); + / sqrt(2.0); Matrix actualH, expectedH; BOOST_FOREACH(Point3 p,ps) { Unit3 s(p); @@ -233,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). - sleep(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 @@ -263,7 +264,7 @@ 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). - sleep(0); + boost::this_thread::sleep(boost::posix_time::milliseconds(0)); // Create the two Unit3s. // Unlike the above case, we can use any two sphers. diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp index 3b73725bc..0b9be2f1c 100644 --- a/gtsam/inference/Key.cpp +++ b/gtsam/inference/Key.cpp @@ -27,31 +27,48 @@ namespace gtsam { - /* ************************************************************************* */ -std::string _multirobotKeyFormatter(gtsam::Key key) { +std::string _multirobotKeyFormatter(Key key) { const LabeledSymbol asLabeledSymbol(key); - if(asLabeledSymbol.chr() > 0 && asLabeledSymbol.label() > 0) - return (std::string)asLabeledSymbol; + if (asLabeledSymbol.chr() > 0 && asLabeledSymbol.label() > 0) + return (std::string) asLabeledSymbol; - const gtsam::Symbol asSymbol(key); + const Symbol asSymbol(key); if (asLabeledSymbol.chr() > 0) - return (std::string)asSymbol; + return (std::string) asSymbol; else return boost::lexical_cast(key); } /* ************************************************************************* */ -void printKeySet(const gtsam::KeySet& keys, const std::string& s, const KeyFormatter& keyFormatter) { +template +static void print(const CONTAINER& keys, const std::string& s, + const KeyFormatter& keyFormatter) { std::cout << s << " "; if (keys.empty()) std::cout << "(none)" << std::endl; else { - BOOST_FOREACH(const gtsam::Key& key, keys) + BOOST_FOREACH(const Key& key, keys) std::cout << keyFormatter(key) << " "; std::cout << std::endl; } } + +/* ************************************************************************* */ +void printKeyList(const KeyList& keys, const std::string& s, + const KeyFormatter& keyFormatter) { + print(keys, s, keyFormatter); +} +/* ************************************************************************* */ +void printKeyVector(const KeyVector& keys, const std::string& s, + const KeyFormatter& keyFormatter) { + print(keys, s, keyFormatter); +} +/* ************************************************************************* */ +void printKeySet(const KeySet& keys, const std::string& s, + const KeyFormatter& keyFormatter) { + print(keys, s, keyFormatter); +} /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index 3f573fcce..e2be79dc7 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -46,6 +46,14 @@ namespace gtsam { typedef FastSet KeySet; typedef FastMap KeyGroupMap; + /// Utility function to print sets of keys with optional prefix + GTSAM_EXPORT void printKeyList(const KeyList& keys, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + + /// Utility function to print sets of keys with optional prefix + GTSAM_EXPORT void printKeyVector(const KeyVector& keys, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + /// Utility function to print sets of keys with optional prefix GTSAM_EXPORT void printKeySet(const KeySet& keys, const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index d393b61b1..9a16ca788 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -55,7 +55,7 @@ class VariableSlots : public FastMap > { public: typedef FastMap > Base; - static const size_t Empty; + GTSAM_EXPORT static const size_t Empty; /// @name Standard Constructors /// @{ diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 8b3dc0084..b3b8b9a41 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -42,7 +42,14 @@ namespace gtsam { /* ************************************************************************* */ VectorValues GaussianBayesNet::optimize() const { - VectorValues soln; + VectorValues soln; // no missing variables -> just create an empty vector + return optimize(soln); + } + + /* ************************************************************************* */ + VectorValues GaussianBayesNet::optimize( + const VectorValues& solutionForMissing) const { + VectorValues soln(solutionForMissing); // possibly empty // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) /** solve each node in turn in topological sort order (parents first)*/ BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) { diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 26b187369..69a70a5e4 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -68,12 +68,12 @@ namespace gtsam { /// @name Standard Interface /// @{ - /** - * Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by - * back-substitution. - */ + /// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by back-substitution VectorValues optimize() const; + /// Version of optimize for incomplete BayesNet, needs solution for missing variables + VectorValues optimize(const VectorValues& solutionForMissing) const; + ///@} ///@name Linear Algebra diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index c6bd3967b..a5c651a44 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -119,14 +119,20 @@ namespace gtsam { /* ************************************************************************* */ VectorValues GaussianConditional::solve(const VectorValues& x) const { - // Solve matrix + // Concatenate all vector values that correspond to parent variables Vector xS = x.vector(FastVector(beginParents(), endParents())); + + // Update right-hand-side xS = getb() - get_S() * xS; + + // Solve matrix Vector soln = get_R().triangularView().solve(xS); // Check for indeterminant solution if(soln.hasNaN()) throw IndeterminantLinearSystemException(keys().front()); + // TODO, do we not have to scale by sigmas here? Copy/paste bug + // Insert solution into a VectorValues VectorValues result; DenseIndex vectorPosition = 0; @@ -142,9 +148,14 @@ namespace gtsam { VectorValues GaussianConditional::solveOtherRHS( const VectorValues& parents, const VectorValues& rhs) const { + // Concatenate all vector values that correspond to parent variables Vector xS = parents.vector(FastVector(beginParents(), endParents())); + + // Instead of updating getb(), update the right-hand-side from the given rhs const Vector rhsR = rhs.vector(FastVector(beginFrontals(), endFrontals())); xS = rhsR - get_S() * xS; + + // Solve Matrix Vector soln = get_R().triangularView().solve(xS); // Scale by sigmas diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 8d76c7651..6f40b9bea 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -367,13 +367,12 @@ void HessianFactor::hessianDiagonal(double* d) const { typedef Eigen::Map DMap; // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { - Key j = keys_[pos]; - // Get the diagonal block, and insert its diagonal - Matrix B = info_(pos, pos).selfadjointView(); - DVector dj = B.diagonal(); - DMap(d + 9 * j) += dj; - } + for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + const Matrix& B = info_(pos, pos).selfadjointView(); + DMap(d + 9 * j) += B.diagonal(); + } } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 641b47640..4bce597a1 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -47,22 +47,50 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) { Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose(); } +/* ************************************************************************* */ +// check *above the diagonal* for non-zero entries +static boost::optional checkIfDiagonal(const Matrix M) { + size_t m = M.rows(), n = M.cols(); + // check all non-diagonal entries + bool full = false; + size_t i, j; + for (i = 0; i < m; i++) + if (!full) + for (j = i + 1; j < n; j++) + if (fabs(M(i, j)) > 1e-9) { + full = true; + break; + } + if (full) { + return boost::none; + } else { + Vector diagonal(n); + for (j = 0; j < n; j++) + diagonal(j) = M(j, j); + return diagonal; + } +} + +/* ************************************************************************* */ +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"); + 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)); +} /* ************************************************************************* */ 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 (smart) { - // check all non-diagonal entries - size_t i,j; - for (i = 0; i < m; i++) - for (j = 0; j < n; j++) - if (i != j && fabs(covariance(i, j)) > 1e-9) goto full; - Vector variances(n); - for (j = 0; j < n; j++) variances(j) = covariance(j,j); - return Diagonal::Variances(variances,true); - } - full: return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); + 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))); } /* ************************************************************************* */ @@ -166,7 +194,7 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const // Diagonal /* ************************************************************************* */ Diagonal::Diagonal() : - Gaussian(1)//, sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1)) + Gaussian(1) // TODO: Frank asks: really sure about this? { } @@ -191,12 +219,17 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { /* ************************************************************************* */ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) { if (smart) { + DenseIndex j, n = sigmas.size(); // look for zeros to make a constraint - for (size_t i=0; i< (size_t) sigmas.size(); ++i) - if (sigmas(i)<1e-8) + for (j=0; j< n; ++j) + if (sigmas(j)<1e-8) return Constrained::MixedSigmas(sigmas); + // check whether all the same entry + for (j = 1; j < n; j++) + if (sigmas(j) != sigmas(0)) goto full; + return Isotropic::Sigma(n, sigmas(0), true); } - return Diagonal::shared_ptr(new Diagonal(sigmas)); + full: return Diagonal::shared_ptr(new Diagonal(sigmas)); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index a87f426fa..e709ea543 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -159,10 +159,10 @@ namespace gtsam { /** * A Gaussian noise model created by specifying a square root information matrix. + * @param R The (upper-triangular) square root information matrix + * @param smart check if can be simplified to derived class */ - static shared_ptr SqrtInformation(const Matrix& R) { - return shared_ptr(new Gaussian(R.rows(),R)); - } + static shared_ptr SqrtInformation(const Matrix& R, bool smart = true); /** * A Gaussian noise model created by specifying a covariance matrix. diff --git a/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp similarity index 91% rename from gtsam/linear/tests/testGaussianBayesNetUnordered.cpp rename to gtsam/linear/tests/testGaussianBayesNet.cpp index d87007d0b..608e9b1e1 100644 --- a/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -35,7 +35,7 @@ using namespace boost::assign; using namespace std; using namespace gtsam; -static const Key _x_=0, _y_=1, _z_=2; +static const Key _x_=0, _y_=1; static GaussianBayesNet smallBayesNet = list_of (GaussianConditional(_x_, (Vector(1) << 9.0), (Matrix(1, 1) << 1.0), _y_, (Matrix(1, 1) << 1.0))) @@ -69,6 +69,24 @@ TEST( GaussianBayesNet, optimize ) EXPECT(assert_equal(expected,actual)); } +/* ************************************************************************* */ +TEST( GaussianBayesNet, optimizeIncomplete ) +{ + static GaussianBayesNet incompleteBayesNet = list_of + (GaussianConditional(_x_, (Vector(1) << 9.0), (Matrix(1, 1) << 1.0), _y_, (Matrix(1, 1) << 1.0))); + + VectorValues solutionForMissing = map_list_of + (_y_, (Vector(1) << 5.0)); + + VectorValues actual = incompleteBayesNet.optimize(solutionForMissing); + + VectorValues expected = map_list_of + (_x_, (Vector(1) << 4.0)) + (_y_, (Vector(1) << 5.0)); + + EXPECT(assert_equal(expected,actual)); +} + /* ************************************************************************* */ TEST( GaussianBayesNet, optimize3 ) { diff --git a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp similarity index 100% rename from gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp rename to gtsam/linear/tests/testGaussianBayesTree.cpp diff --git a/gtsam/linear/tests/testGaussianConditionalUnordered.cpp b/gtsam/linear/tests/testGaussianConditional.cpp similarity index 100% rename from gtsam/linear/tests/testGaussianConditionalUnordered.cpp rename to gtsam/linear/tests/testGaussianConditional.cpp diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp similarity index 100% rename from gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp rename to gtsam/linear/tests/testGaussianFactorGraph.cpp diff --git a/gtsam/linear/tests/testHessianFactorUnordered.cpp b/gtsam/linear/tests/testHessianFactor.cpp similarity index 100% rename from gtsam/linear/tests/testHessianFactorUnordered.cpp rename to gtsam/linear/tests/testHessianFactor.cpp diff --git a/gtsam/linear/tests/testJacobianFactorUnordered.cpp b/gtsam/linear/tests/testJacobianFactor.cpp similarity index 100% rename from gtsam/linear/tests/testJacobianFactorUnordered.cpp rename to gtsam/linear/tests/testJacobianFactor.cpp diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 9d87a163b..d68caeabe 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -267,6 +267,24 @@ TEST(NoiseModel, QRNan ) EXPECT(assert_equal(expectedAb,Ab)); } +/* ************************************************************************* */ +TEST(NoiseModel, SmartSqrtInformation ) +{ + bool smart = true; + gtsam::SharedGaussian expected = Unit::Create(3); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(eye(3), smart); + EXPECT(assert_equal(*expected,*actual)); +} + +/* ************************************************************************* */ +TEST(NoiseModel, SmartSqrtInformation2 ) +{ + bool smart = true; + gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*eye(3), smart); + EXPECT(assert_equal(*expected,*actual)); +} + /* ************************************************************************* */ TEST(NoiseModel, SmartCovariance ) { diff --git a/gtsam/linear/tests/testVectorValuesUnordered.cpp b/gtsam/linear/tests/testVectorValues.cpp similarity index 100% rename from gtsam/linear/tests/testVectorValuesUnordered.cpp rename to gtsam/linear/tests/testVectorValues.cpp diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 44dd4183a..64603bd99 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -61,7 +61,7 @@ public: * Version of AttitudeFactor for Rot3 * @addtogroup Navigation */ -class Rot3AttitudeFactor: public NoiseModelFactor1, public AttitudeFactor { +class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactor1, public AttitudeFactor { typedef NoiseModelFactor1 Base; @@ -129,7 +129,7 @@ private: * Version of AttitudeFactor for Pose3 * @addtogroup Navigation */ -class Pose3AttitudeFactor: public NoiseModelFactor1, +class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactor1, public AttitudeFactor { typedef NoiseModelFactor1 Base; diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index f4ef6c49a..6902f81f1 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -31,7 +31,7 @@ namespace gtsam { * See Farrell08book or e.g. http://www.dirsig.org/docs/new/coordinates.html * @addtogroup Navigation */ -class GPSFactor: public NoiseModelFactor1 { +class GTSAM_EXPORT GPSFactor: public NoiseModelFactor1 { private: diff --git a/gtsam/navigation/tests/CMakeLists.txt b/gtsam/navigation/tests/CMakeLists.txt index 2f3c0883a..b03b8167c 100644 --- a/gtsam/navigation/tests/CMakeLists.txt +++ b/gtsam/navigation/tests/CMakeLists.txt @@ -7,12 +7,22 @@ set(tests_excluded "") if(GTSAM_INSTALL_GEOGRAPHICLIB) # If we're installing GeographicLib, use the one we're compiling include_directories(${PROJECT_SOURCE_DIR}/gtsam/3rdparty/GeographicLib/include) - list(APPEND test_link_libraries GeographicLib) + if(MSVC) + list(APPEND test_link_libraries GeographicLib_STATIC) + else() + list(APPEND test_link_libraries GeographicLib) + endif() + else() if(GEOGRAPHICLIB_FOUND) # If we're not installing, but it's already installed, use the installed one include_directories(${GeographicLib_INCLUDE_DIRS}) - list(APPEND test_link_libraries ${GeographicLib_LIBRARIES}) + list(APPEND test_link_libraries ) + #if(MSVC) + # list(APPEND test_link_libraries GeographicLib_STATIC) + #else() + list(APPEND test_link_libraries ${GeographicLib_LIBRARIES}) + #endif() else() # We don't have GeographicLib set(tests_excluded testGeographicLib.cpp testGPSFactor.cpp testMagFactor.cpp) diff --git a/gtsam/navigation/tests/testGeographicLib.cpp b/gtsam/navigation/tests/testGeographicLib.cpp index 44964c569..aaa01b54d 100644 --- a/gtsam/navigation/tests/testGeographicLib.cpp +++ b/gtsam/navigation/tests/testGeographicLib.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index a6c04681b..dea4113f7 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -191,7 +191,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY) stay = false; // If not searching, just return with the new Delta else if(mode == SEARCH_EACH_ITERATION) { - if(newDelta == Delta || lastAction == DECREASED_DELTA) + if(fabs(newDelta - Delta) < 1e-15 || lastAction == DECREASED_DELTA) stay = false; // Searching, but Newton's solution is within trust region so keep the same trust region else { stay = true; // Searching and increased Delta, so try again to increase Delta diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index f2eb76e2d..0e4fb48cf 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -27,7 +27,7 @@ namespace gtsam { * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement * @addtogroup SLAM */ -class EssentialMatrixConstraint: public NoiseModelFactor2 { +class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactor2 { private: diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 60c17ec9e..47a0aca2a 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -112,14 +112,14 @@ pair load2D( // Create a sampler with random number generator Sampler sampler(42u); - // load the factors + // Parse the pose constraints + int id1, id2; bool haveLandmark = false; while (is) { if(! (is >> tag)) break; if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { - int id1, id2; double x, y, yaw; double v1, v2, v3, v4, v5, v6; @@ -168,68 +168,75 @@ pair load2D( new BetweenFactor(id1, id2, l1Xl2, *model)); graph->push_back(factor); } + + // Parse measurements + double bearing, range, bearing_std, range_std; + + // A bearing-range measurement 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); - } } + + // A landmark measurement, TODO Frank says: don't know why is converted to bearing-range if (tag == "LANDMARK") { - int id1, id2; double lmx, lmy; double v1, v2, v3; is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3; // Convert x,y to bearing,range - double bearing = std::atan2(lmy, lmx); - double range = std::sqrt(lmx*lmx + lmy*lmy); + bearing = std::atan2(lmy, lmx); + range = std::sqrt(lmx*lmx + lmy*lmy); // In our experience, the x-y covariance on landmark sightings is not very good, so assume - // that it describes the uncertainty at a range of 10m, and convert that to bearing/range - // uncertainty. - SharedDiagonal measurementNoise; + // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty. if(std::abs(v1 - v3) < 1e-4) { - double rangeVar = v1; - double bearingVar = v1 / 10.0; - measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearingVar, rangeVar)); + bearing_std = sqrt(v1 / 10.0); + range_std = sqrt(v1); } 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; haveLandmark = true; } } + } + + // Do some common stuff for bearing-range measurements + if (tag == "LANDMARK" || tag == "BR") { + + // optional filter + if (maxID && id1 >= maxID) + continue; + + // Create noise model + noiseModel::Diagonal::shared_ptr measurementNoise = + noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); // Add to graph - *graph += BearingRangeFactor(id1, L(id2), bearing, range, measurementNoise); + *graph += BearingRangeFactor(id1, L(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(L(id2))) { + Pose2 pose = initial->at(id1); + Point2 local(cos(bearing)*range,sin(bearing)*range); + Point2 global = pose.transform_from(local); + initial->insert(L(id2), global); + } } is.ignore(LINESIZE, '\n'); } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index fac2164d8..d0dbe7908 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -35,13 +35,13 @@ TEST(BetweenFactor, Rot3) { boost::function(boost::bind( &BetweenFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), R1, R2, 1e-5); - EXPECT(assert_equal(numericalH1,actualH1)); + EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); Matrix numericalH2 = numericalDerivative22( boost::function(boost::bind( &BetweenFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), R1, R2, 1e-5); - EXPECT(assert_equal(numericalH2,actualH2)); + EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 8fcb5b6e5..5184393ac 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -63,8 +63,8 @@ TEST( EssentialMatrixConstraint, test ) { factor.evaluateError(pose1, pose2, actualH1, actualH2); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); - CHECK(assert_equal(expectedH2, actualH2, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); + CHECK(assert_equal(expectedH2, actualH2, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 0766dbf8f..1e5674599 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -128,7 +128,11 @@ TEST (EssentialMatrixFactor, minimization) { EssentialMatrix initialE = trueE.retract( (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1)); initial.insert(1, initialE); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); +#else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); +#endif // Optimize LevenbergMarquardtParams parameters; @@ -339,7 +343,12 @@ TEST (EssentialMatrixFactor, extraMinimization) { EssentialMatrix initialE = trueE.retract( (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1)); initial.insert(1, initialE); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); +#else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); +#endif // Optimize LevenbergMarquardtParams parameters; diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index ca924aaae..f36405318 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -59,7 +59,11 @@ TEST (RotateFactor, test) { EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8)); Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + Vector expectedE = (Vector(3) << -0.0248752, 0.202981, -0.0890529); +#else Vector expectedE = (Vector(3) << -0.0246305, 0.20197, -0.08867); +#endif EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); Matrix actual, expected; @@ -97,7 +101,12 @@ TEST (RotateFactor, minimization) { double degree = M_PI / 180; Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20)); initial.insert(1, initialE); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(3545.40, graph.error(initial), 1); +#else EXPECT_DOUBLES_EQUAL(3349, graph.error(initial), 1); +#endif // Optimize LevenbergMarquardtParams parameters; @@ -120,7 +129,13 @@ TEST (RotateDirectionsFactor, test) { EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8)); Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + Vector expectedE = (Vector(2) << -0.0890529, -0.202981); +#else Vector expectedE = (Vector(2) << -0.08867, -0.20197); +#endif + EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); Matrix actual, expected; @@ -160,7 +175,12 @@ TEST (RotateDirectionsFactor, minimization) { double degree = M_PI / 180; Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20)); initial.insert(1, initialE); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(3335.9, graph.error(initial), 1); +#else EXPECT_DOUBLES_EQUAL(3162, graph.error(initial), 1); +#endif // Optimize LevenbergMarquardtParams parameters; diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index 610e98c58..99a09adc9 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -31,7 +31,6 @@ static const Key _A_ = 1; static const Key _B_ = 2; static const Key _C_ = 3; static const Key _D_ = 4; -static const Key _E_ = 5; static SymbolicConditional::shared_ptr B(new SymbolicConditional(_B_)), diff --git a/gtsam/symbolic/tests/testVariableIndexUnordered.cpp b/gtsam/symbolic/tests/testVariableIndex.cpp similarity index 100% rename from gtsam/symbolic/tests/testVariableIndexUnordered.cpp rename to gtsam/symbolic/tests/testVariableIndex.cpp diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 25d996772..82197302b 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -14,8 +14,8 @@ using namespace gtsam::symbol_shorthand; const double tol=1e-5; const double h = 0.01; -const double deg2rad = M_PI/180.0; +//const double deg2rad = M_PI/180.0; //Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); //LieVector v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 80ee41b22..1aa840825 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -15,8 +15,11 @@ virtual class gtsam::Pose3; virtual class gtsam::noiseModel::Base; virtual class gtsam::noiseModel::Gaussian; virtual class gtsam::imuBias::ConstantBias; -virtual class gtsam::NoiseModelFactor; virtual class gtsam::NonlinearFactor; +virtual class gtsam::NoiseModelFactor; +virtual class gtsam::NoiseModelFactor2; +virtual class gtsam::NoiseModelFactor3; +virtual class gtsam::NoiseModelFactor4; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; virtual class gtsam::JacobianFactor; @@ -709,4 +712,32 @@ class AHRS { void print(string s) const; }; +// Tectonic SAM Factors + +#include +#include + +//typedef gtsam::NoiseModelFactor2 NLPosePose; +virtual class DeltaFactor : gtsam::NoiseModelFactor { + DeltaFactor(size_t i, size_t j, const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel); + void print(string s) const; +}; + +//typedef gtsam::NoiseModelFactor4 NLPosePosePosePoint; +virtual class DeltaFactorBase : gtsam::NoiseModelFactor { + DeltaFactorBase(size_t b1, size_t i, size_t b2, size_t j, + const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel); + void print(string s) const; +}; + +//typedef gtsam::NoiseModelFactor4 NLPosePosePosePose; +virtual class OdometryFactorBase : gtsam::NoiseModelFactor { + OdometryFactorBase(size_t b1, size_t i, size_t b2, size_t j, + const gtsam::Pose2& measured, const gtsam::noiseModel::Base* noiseModel); + void print(string s) const; +}; + } //\namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 2761e3af6..8606538bf 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -350,7 +350,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { std::cout << "using lambda = " << lambda << std::endl; result.iterations++; - } while(result.iterations < parameters_.maxIterations && + } while(result.iterations < (size_t)parameters_.maxIterations && !checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol, parameters_.errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT)); return result; diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index 6eb39cfaa..dacbebc76 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -24,8 +24,6 @@ extern "C" { #include "FindSeparator.h" -using namespace std; - namespace gtsam { namespace partition { typedef boost::shared_array sharedInts; @@ -37,7 +35,7 @@ namespace gtsam { namespace partition { * whether node j is in the left part of the graph, the right part, or the * separator, respectively */ - pair separatorMetis(idx_t n, const sharedInts& xadj, + std::pair separatorMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) { // control parameters @@ -72,7 +70,7 @@ namespace gtsam { namespace partition { printf("**********************************************************************\n"); } - return make_pair(sepsize, part_); + return std::make_pair(sepsize, part_); } /* ************************************************************************* */ @@ -112,7 +110,7 @@ namespace gtsam { namespace partition { // *edgecut = graph->mincut; // *sepsize = graph.pwgts[2]; icopy(*nvtxs, graph->where, part); - cout << "Finished bisection:" << *edgecut << endl; + std::cout << "Finished bisection:" << *edgecut << std::endl; FreeGraph(&graph); FreeCtrl(&ctrl); @@ -124,7 +122,7 @@ namespace gtsam { namespace partition { * Part [j] is 0 or 1, depending on * whether node j is in the left part of the graph or the right part respectively */ - pair edgeMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy, + std::pair edgeMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) { // control parameters @@ -163,7 +161,7 @@ namespace gtsam { namespace partition { printf("**********************************************************************\n"); } - return make_pair(edgecut, part_); + return std::make_pair(edgecut, part_); } /* ************************************************************************* */ @@ -173,13 +171,13 @@ namespace gtsam { namespace partition { * adjncy always has the size equal to two times of the no. of the edges in the Metis graph */ template - void prepareMetisGraph(const GenericGraph& graph, const vector& keys, WorkSpace& workspace, + void prepareMetisGraph(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) { typedef int Weight; - typedef vector Weights; - typedef vector Neighbors; - typedef pair NeighborsInfo; + typedef std::vector Weights; + typedef std::vector Neighbors; + typedef std::pair NeighborsInfo; // set up dictionary std::vector& dictionary = workspace.dictionary; @@ -188,27 +186,27 @@ namespace gtsam { namespace partition { // prepare for {adjacencyMap}, a pair of neighbor indices and the correponding edge weights int numNodes = keys.size(); int numEdges = 0; - vector adjacencyMap; + std::vector adjacencyMap; adjacencyMap.resize(numNodes); - cout << "Number of nodes: " << adjacencyMap.size() << endl; + std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl; int index1, index2; BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ index1 = dictionary[factor->key1.index]; index2 = dictionary[factor->key2.index]; - cout << "index1: " << index1 << endl; - cout << "index2: " << index2 << endl; + std::cout << "index1: " << index1 << std::endl; + std::cout << "index2: " << index2 << std::endl; // if both nodes are in the current graph, i.e. not a joint factor between frontal and separator if (index1 >= 0 && index2 >= 0) { - pair& adjacencyMap1 = adjacencyMap[index1]; - pair& adjacencyMap2 = adjacencyMap[index2]; + std::pair& adjacencyMap1 = adjacencyMap[index1]; + std::pair& adjacencyMap2 = adjacencyMap[index2]; try{ adjacencyMap1.first.push_back(index2); adjacencyMap1.second.push_back(factor->weight); adjacencyMap2.first.push_back(index1); adjacencyMap2.second.push_back(factor->weight); }catch(std::exception& e){ - cout << e.what() << endl; + std::cout << e.what() << std::endl; } numEdges++; } @@ -237,11 +235,11 @@ namespace gtsam { namespace partition { /* ************************************************************************* */ template boost::optional separatorPartitionByMetis(const GenericGraph& graph, - const vector& keys, WorkSpace& workspace, bool verbose) { + const std::vector& keys, WorkSpace& workspace, bool verbose) { // create a metis graph size_t numKeys = keys.size(); if (verbose) - cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << endl; + std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; sharedInts xadj, adjncy, adjwgt; @@ -260,29 +258,29 @@ namespace gtsam { namespace partition { result.A.reserve(numKeys - sepsize); result.B.reserve(numKeys - sepsize); int* ptr_part = part.get(); - vector::const_iterator itKey = keys.begin(); - vector::const_iterator itKeyLast = keys.end(); + std::vector::const_iterator itKey = keys.begin(); + std::vector::const_iterator itKeyLast = keys.end(); while(itKey != itKeyLast) { switch(*(ptr_part++)) { case 0: result.A.push_back(*(itKey++)); break; case 1: result.B.push_back(*(itKey++)); break; case 2: result.C.push_back(*(itKey++)); break; - default: throw runtime_error("separatorPartitionByMetis: invalid results from Metis ND!"); + default: throw std::runtime_error("separatorPartitionByMetis: invalid results from Metis ND!"); } } if (verbose) { - cout << "total key: " << keys.size() + std::cout << "total key: " << keys.size() << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " - << result.C.size() << "; sepsize from Metis = " << sepsize << endl; + << result.C.size() << "; sepsize from Metis = " << sepsize << std::endl; //throw runtime_error("separatorPartitionByMetis:stop for debug"); } if(result.C.size() != sepsize) { - cout << "total key: " << keys.size() + std::cout << "total key: " << keys.size() << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size() - << "; sepsize from Metis = " << sepsize << endl; - throw runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!"); + << "; sepsize from Metis = " << sepsize << std::endl; + throw std::runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!"); } return boost::make_optional(result); @@ -291,7 +289,7 @@ namespace gtsam { namespace partition { /* *************************************************************************/ template boost::optional edgePartitionByMetis(const GenericGraph& graph, - const vector& keys, WorkSpace& workspace, bool verbose) { + const std::vector& keys, WorkSpace& workspace, bool verbose) { // a small hack for handling the camera1-camera2 case used in the unit tests if (graph.size() == 1 && keys.size() == 2) { @@ -303,7 +301,7 @@ namespace gtsam { namespace partition { // create a metis graph size_t numKeys = keys.size(); - if (verbose) cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << endl; + if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; sharedInts xadj, adjncy, adjwgt; prepareMetisGraph(graph, keys, workspace, &xadj, &adjncy, &adjwgt); @@ -317,35 +315,35 @@ namespace gtsam { namespace partition { result.A.reserve(numKeys); result.B.reserve(numKeys); int* ptr_part = part.get(); - vector::const_iterator itKey = keys.begin(); - vector::const_iterator itKeyLast = keys.end(); + std::vector::const_iterator itKey = keys.begin(); + std::vector::const_iterator itKeyLast = keys.end(); while(itKey != itKeyLast) { if (*ptr_part != 0 && *ptr_part != 1) - cout << *ptr_part << "!!!" << endl; + std::cout << *ptr_part << "!!!" << std::endl; switch(*(ptr_part++)) { case 0: result.A.push_back(*(itKey++)); break; case 1: result.B.push_back(*(itKey++)); break; - default: throw runtime_error("edgePartitionByMetis: invalid results from Metis ND!"); + default: throw std::runtime_error("edgePartitionByMetis: invalid results from Metis ND!"); } } if (verbose) { - cout << "the size of two submaps in the reduced graph: " << result.A.size() - << " " << result.B.size() << endl; + std::cout << "the size of two submaps in the reduced graph: " << result.A.size() + << " " << result.B.size() << std::endl; int edgeCut = 0; BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ int key1 = factor->key1.index; int key2 = factor->key2.index; // print keys and their subgraph assignment - cout << key1; - if (std::find(result.A.begin(), result.A.end(), key1) != result.A.end()) cout <<"A "; - if (std::find(result.B.begin(), result.B.end(), key1) != result.B.end()) cout <<"B "; + std::cout << key1; + if (std::find(result.A.begin(), result.A.end(), key1) != result.A.end()) std::cout <<"A "; + if (std::find(result.B.begin(), result.B.end(), key1) != result.B.end()) std::cout <<"B "; - cout << key2; - if (std::find(result.A.begin(), result.A.end(), key2) != result.A.end()) cout <<"A "; - if (std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) cout <<"B "; - cout << "weight " << factor->weight;; + std::cout << key2; + if (std::find(result.A.begin(), result.A.end(), key2) != result.A.end()) std::cout <<"A "; + if (std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) std::cout <<"B "; + std::cout << "weight " << factor->weight;; // find vertices that were assigned to sets A & B. Their edge will be cut if ((std::find(result.A.begin(), result.A.end(), key1) != result.A.end() && @@ -353,48 +351,48 @@ namespace gtsam { namespace partition { (std::find(result.B.begin(), result.B.end(), key1) != result.B.end() && std::find(result.A.begin(), result.A.end(), key2) != result.A.end())){ edgeCut ++; - cout << " CUT "; + std::cout << " CUT "; } - cout << endl; + std::cout << std::endl; } - cout << "edgeCut: " << edgeCut << endl; + std::cout << "edgeCut: " << edgeCut << std::endl; } return boost::make_optional(result); } /* ************************************************************************* */ - bool isLargerIsland(const vector& island1, const vector& island2) { + bool isLargerIsland(const std::vector& island1, const std::vector& island2) { return island1.size() > island2.size(); } /* ************************************************************************* */ // debug functions - void printIsland(const vector& island) { - cout << "island: "; + void printIsland(const std::vector& island) { + std::cout << "island: "; BOOST_FOREACH(const size_t key, island) - cout << key << " "; - cout << endl; + std::cout << key << " "; + std::cout << std::endl; } - void printIslands(const list >& islands) { - BOOST_FOREACH(const vector& island, islands) + void printIslands(const std::list >& islands) { + BOOST_FOREACH(const std::vector& island, islands) printIsland(island); } - void printNumCamerasLandmarks(const vector& keys, const vector& int2symbol) { + void printNumCamerasLandmarks(const std::vector& keys, const std::vector& int2symbol) { int numCamera = 0, numLandmark = 0; BOOST_FOREACH(const size_t key, keys) if (int2symbol[key].chr() == 'x') numCamera++; else numLandmark++; - cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << endl; + std::cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << std::endl; } /* ************************************************************************* */ template - void addLandmarkToPartitionResult(const GenericGraph& graph, const vector& landmarkKeys, + void addLandmarkToPartitionResult(const GenericGraph& graph, const std::vector& landmarkKeys, MetisResult& partitionResult, WorkSpace& workspace) { // set up cameras in the dictionary @@ -408,7 +406,7 @@ namespace gtsam { namespace partition { BOOST_FOREACH(const size_t b, B) dictionary[b] = 2; if (!C.empty()) - throw runtime_error("addLandmarkToPartitionResult: C is not empty"); + throw std::runtime_error("addLandmarkToPartitionResult: C is not empty"); // set up landmarks size_t i,j; @@ -424,7 +422,7 @@ namespace gtsam { namespace partition { dictionary[j] = 0; } // if (j == 67980) -// cout << "dictionary[67980]" << dictionary[j] << endl; +// std::cout << "dictionary[67980]" << dictionary[j] << std::endl; } BOOST_FOREACH(const size_t j, landmarkKeys) { @@ -432,8 +430,8 @@ namespace gtsam { namespace partition { case 0: C.push_back(j); break; case 1: A.push_back(j); break; case 2: B.push_back(j); break; - default: cout << j << ": " << dictionary[j] << endl; - throw runtime_error("addLandmarkToPartitionResult: wrong status for landmark"); + default: std::cout << j << ": " << dictionary[j] << std::endl; + throw std::runtime_error("addLandmarkToPartitionResult: wrong status for landmark"); } } } @@ -442,13 +440,13 @@ namespace gtsam { namespace partition { /* ************************************************************************* */ template - boost::optional findPartitoning(const GenericGraph& graph, const vector& keys, + boost::optional findPartitoning(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, bool verbose, - const boost::optional >& int2symbol, const bool reduceGraph) { + const boost::optional >& int2symbol, const bool reduceGraph) { boost::optional result; GenericGraph reducedGraph; - vector keyToPartition; - vector cameraKeys, landmarkKeys; + std::vector keyToPartition; + std::vector cameraKeys, landmarkKeys; if (reduceGraph) { if (!int2symbol.is_initialized()) throw std::invalid_argument("findSeparator: int2symbol must be valid!"); @@ -467,20 +465,20 @@ namespace gtsam { namespace partition { workspace.prepareDictionary(keyToPartition); const std::vector& dictionary = workspace.dictionary; reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph); - cout << "original graph: V" << keys.size() << ", E" << graph.size() - << " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << endl; + std::cout << "original graph: V" << keys.size() << ", E" << graph.size() + << " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << std::endl; result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose); } else // call Metis to partition the graph to A, B, C result = separatorPartitionByMetis(graph, keys, workspace, verbose); if (!result.is_initialized()) { - cout << "metis failed!" << endl; + std::cout << "metis failed!" << std::endl; return 0; } if (reduceGraph) { addLandmarkToPartitionResult(graph, landmarkKeys, *result, workspace); - cout << "the separator size: " << result->C.size() << " landmarks" << endl; + std::cout << "the separator size: " << result->C.size() << " landmarks" << std::endl; } return result; @@ -488,22 +486,22 @@ namespace gtsam { namespace partition { /* ************************************************************************* */ template - int findSeparator(const GenericGraph& graph, const vector& keys, + int findSeparator(const GenericGraph& graph, const std::vector& keys, const int minNodesPerMap, WorkSpace& workspace, bool verbose, - const boost::optional >& int2symbol, const bool reduceGraph, + const boost::optional >& int2symbol, const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { boost::optional result = findPartitoning(graph, keys, workspace, verbose, int2symbol, reduceGraph); // find the island in A and B, and make them separated submaps - typedef vector Island; - list islands; + typedef std::vector Island; + std::list islands; - list islands_in_A = findIslands(graph, result->A, workspace, + std::list islands_in_A = findIslands(graph, result->A, workspace, minNrConstraintsPerCamera, minNrConstraintsPerLandmark); - list islands_in_B = findIslands(graph, result->B, workspace, + std::list islands_in_B = findIslands(graph, result->B, workspace, minNrConstraintsPerCamera, minNrConstraintsPerLandmark); islands.insert(islands.end(), islands_in_A.begin(), islands_in_A.end()); @@ -514,13 +512,13 @@ namespace gtsam { namespace partition { #ifdef NDEBUG // verbose = true; // if (!int2symbol) throw std::invalid_argument("findSeparator: int2symbol is not set!"); -// cout << "sep size: " << result->C.size() << "; "; +// std::cout << "sep size: " << result->C.size() << "; "; // printNumCamerasLandmarks(result->C, *int2symbol); -// cout << "no. of island: " << islands.size() << "; "; -// cout << "island size: "; +// std::cout << "no. of island: " << islands.size() << "; "; +// std::cout << "island size: "; // BOOST_FOREACH(const Island& island, islands) -// cout << island.size() << " "; -// cout << endl; +// std::cout << island.size() << " "; +// std::cout << std::endl; // BOOST_FOREACH(const Island& island, islands) { // printNumCamerasLandmarks(island, int2symbol); @@ -531,20 +529,20 @@ namespace gtsam { namespace partition { size_t oldSize = islands.size(); while(true) { if (islands.size() < 2) { - cout << "numIsland: " << numIsland0 << endl; - throw runtime_error("findSeparator: found fewer than 2 submaps!"); + std::cout << "numIsland: " << numIsland0 << std::endl; + throw std::runtime_error("findSeparator: found fewer than 2 submaps!"); } - list::reference island = islands.back(); + std::list::reference island = islands.back(); if ((int)island.size() >= minNodesPerMap) break; result->C.insert(result->C.end(), island.begin(), island.end()); islands.pop_back(); } if (islands.size() != oldSize){ - if (verbose) cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << endl; + if (verbose) std::cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << std::endl; } else{ - if (verbose) cout << oldSize << " submap(s);\t" << endl; + if (verbose) std::cout << oldSize << " submap(s);\t" << std::endl; } // generate the node map diff --git a/gtsam_unstable/partition/NestedDissection-inl.h b/gtsam_unstable/partition/NestedDissection-inl.h index e8f3fa34a..d6dafce49 100644 --- a/gtsam_unstable/partition/NestedDissection-inl.h +++ b/gtsam_unstable/partition/NestedDissection-inl.h @@ -14,8 +14,6 @@ #include "OrderedSymbols.h" #include "NestedDissection.h" -using namespace std; - namespace gtsam { namespace partition { /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/JacobianFactorQ.h b/gtsam_unstable/slam/JacobianFactorQ.h index fdbe0e231..f4dfb9422 100644 --- a/gtsam_unstable/slam/JacobianFactorQ.h +++ b/gtsam_unstable/slam/JacobianFactorQ.h @@ -23,6 +23,19 @@ public: JacobianFactorQ() { } + /// Empty constructor with keys + JacobianFactorQ(const FastVector& keys, + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + Matrix zeroMatrix = Matrix::Zero(0,D); + Vector zeroVector = Vector::Zero(0); + typedef std::pair KeyMatrix; + std::vector QF; + QF.reserve(keys.size()); + BOOST_FOREACH(const Key& key, keys) + QF.push_back(KeyMatrix(key, zeroMatrix)); + JacobianFactor::fillTerms(QF, zeroVector, model); + } + /// Constructor JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, diff --git a/gtsam_unstable/slam/JacobianFactorSVD.h b/gtsam_unstable/slam/JacobianFactorSVD.h index 752a9f48e..e8ade3b1b 100644 --- a/gtsam_unstable/slam/JacobianFactorSVD.h +++ b/gtsam_unstable/slam/JacobianFactorSVD.h @@ -18,10 +18,23 @@ public: typedef Eigen::Matrix Matrix2D; typedef std::pair KeyMatrix2D; + typedef std::pair KeyMatrix; /// Default constructor JacobianFactorSVD() {} + /// Empty constructor with keys + JacobianFactorSVD(const FastVector& keys, + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + Matrix zeroMatrix = Matrix::Zero(0,D); + Vector zeroVector = Vector::Zero(0); + std::vector QF; + QF.reserve(keys.size()); + BOOST_FOREACH(const Key& key, keys) + QF.push_back(KeyMatrix(key, zeroMatrix)); + JacobianFactor::fillTerms(QF, zeroVector, model); + } + /// Constructor JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { @@ -32,7 +45,6 @@ public: // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // JacobianFactor factor(QF, Q * b); - typedef std::pair KeyMatrix; std::vector QF; QF.reserve(numKeys); BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index e60b7e930..93931549f 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -20,6 +20,7 @@ #pragma once #include "JacobianFactorQ.h" +#include "JacobianFactorSVD.h" #include "ImplicitSchurFactor.h" #include "RegularHessianFactor.h" @@ -135,12 +136,12 @@ public: } /** return the measurements */ - const Vector& measured() const { + const std::vector& measured() const { return measured_; } /** return the noise model */ - const SharedNoiseModel& noise() const { + const std::vector& noise() const { return noise_; } @@ -218,8 +219,9 @@ public: const Point2& zi = this->measured_.at(i); try { Point2 reprojectionError(camera.project(point) - zi); - overallError += 0.5 * reprojectionError.vector().squaredNorm(); - } catch (CheiralityException& e) { + overallError += 0.5 + * this->noise_.at(i)->distance(reprojectionError.vector()); + } catch (CheiralityException&) { std::cout << "Cheirality exception " << std::endl; exit(EXIT_FAILURE); } @@ -259,7 +261,7 @@ public: double computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, const Cameras& cameras, const Point3& point) const { - int numKeys = this->keys_.size(); + size_t numKeys = this->keys_.size(); E = zeros(2 * numKeys, 3); b = zero(2 * numKeys); double f = 0; @@ -271,7 +273,7 @@ public: try { bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); - } catch (CheiralityException& e) { + } catch (CheiralityException&) { std::cout << "Cheirality exception " << std::endl; exit(EXIT_FAILURE); } @@ -646,6 +648,17 @@ public: return boost::make_shared >(Fblocks, E, PointCov, b); } + // **************************************************************************************************** + boost::shared_ptr createJacobianSVDFactor( + const Cameras& cameras, const Point3& point, double lambda = 0.0) const { + size_t numKeys = this->keys_.size(); + std::vector < KeyMatrix2D > Fblocks; + Vector b; + Matrix Enull(2*numKeys, 2*numKeys-3); + computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); + return boost::make_shared< JacobianFactorSVD<6> >(Fblocks, Enull, b); + } + private: /// Serialization function diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index a1b85e2c3..2124dd6f6 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -54,6 +54,10 @@ public: double f; }; +enum linearizationType { + HESSIAN, JACOBIAN_SVD, JACOBIAN_Q +}; + /** * SmartProjectionFactor: triangulates point * TODO: why LANDMARK parameter? @@ -91,6 +95,13 @@ protected: /// shorthand for base class type typedef SmartFactorBase Base; + double landmarkDistanceThreshold_; // if the landmark is triangulated at a + // distance larger than that the factor is considered degenerate + + double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the + // average reprojection error is smaller than this threshold after triangulation, + // and the factor is disregarded if the error is large + /// shorthand for this class typedef SmartProjectionFactor This; @@ -115,12 +126,15 @@ public: SmartProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, boost::optional body_P_sensor = boost::none, - SmartFactorStatePtr state = SmartFactorStatePtr( - new SmartProjectionFactorState())) : + double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1, + SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false), state_(state) { + false), verboseCheirality_(false), state_(state), + landmarkDistanceThreshold_(landmarkDistanceThreshold), + dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { } /** Virtual destructor */ @@ -234,14 +248,39 @@ public: rankTolerance_, enableEPI_); degenerate_ = false; cheiralityException_ = false; - } catch (TriangulationUnderconstrainedException& e) { + + // Check landmark distance and reprojection errors to avoid outliers + double totalReprojError = 0.0; + size_t i=0; + BOOST_FOREACH(const Camera& camera, cameras) { + Point3 cameraTranslation = camera.pose().translation(); + // we discard smart factors corresponding to points that are far away + if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){ + degenerate_ = true; + break; + } + const Point2& zi = this->measured_.at(i); + try { + Point2 reprojectionError(camera.project(point_) - zi); + totalReprojError += reprojectionError.vector().norm(); + } catch (CheiralityException& e) { + cheiralityException_ = true; + } + i += 1; + } + // we discard smart factors that have large reprojection error + if(dynamicOutlierRejectionThreshold_ > 0 && + totalReprojError/m > dynamicOutlierRejectionThreshold_) + degenerate_ = true; + + } catch (TriangulationUnderconstrainedException&) { // if TriangulationUnderconstrainedException can be // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) // in the second case we want to use a rotation-only smart factor degenerate_ = true; cheiralityException_ = false; - } catch (TriangulationCheiralityException& e) { + } catch (TriangulationCheiralityException&) { // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint cheiralityException_ = true; @@ -279,7 +318,7 @@ public: const Cameras& cameras, const double lambda = 0.0) const { bool isDebug = false; - int numKeys = this->keys_.size(); + size_t numKeys = this->keys_.size(); // Create structures for Hessian Factors std::vector < Key > js; std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); @@ -351,10 +390,10 @@ public: // Populate Gs and gs int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera + for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera DenseIndex i1D = i1 * D; gs.at(i1) = gs_vector.segment < D > (i1D); - for (DenseIndex i2 = 0; i2 < numKeys; i2++) { + for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) { if (i2 >= i1) { Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D); GsCount2++; @@ -385,7 +424,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianQFactor(cameras, point_, lambda); else - return boost::shared_ptr >(); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// Create a factor, takes values @@ -397,7 +436,16 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::shared_ptr >(); + return boost::make_shared< JacobianFactorQ >(this->keys_); + } + + /// different (faster) way to compute Jacobian factor + boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, + double lambda) const { + if (triangulateForLinearize(cameras)) + return Base::createJacobianSVDFactor(cameras, point_, lambda); + else + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactor.h b/gtsam_unstable/slam/SmartProjectionPoseFactor.h index 8f9617561..66497d28d 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactor.h @@ -41,6 +41,8 @@ template class SmartProjectionPoseFactor: public SmartProjectionFactor { protected: + linearizationType linearizeTo_; + // Known calibration std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) @@ -66,8 +68,11 @@ 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) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor) {} + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + linearizationType linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1) : + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, + landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {} /** Virtual destructor */ virtual ~SmartProjectionPoseFactor() {} @@ -149,11 +154,22 @@ public: } /** - * linearize returns a Hessian factor contraining the poses + * linear factor on the poses */ virtual boost::shared_ptr linearize( const Values& values) const { - return this->createHessianFactor(cameras(values)); + // depending on flag set on construction we may linearize to different linear factors + switch(linearizeTo_){ + case JACOBIAN_SVD : + return this->createJacobianSVDFactor(cameras(values), 0.0); + break; + case JACOBIAN_Q : + return this->createJacobianQFactor(cameras(values), 0.0); + break; + default: + return this->createHessianFactor(cameras(values)); + break; + } } /** diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h new file mode 100644 index 000000000..d4c5f8172 --- /dev/null +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -0,0 +1,167 @@ +/* ---------------------------------------------------------------------------- + + * 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 TSAMFactors.h + * @brief TSAM 1 Factors, simpler than the hierarchical TSAM 2 + * @author Frank Dellaert + * @date May 2014 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * DeltaFactor: relative 2D measurement between Pose2 and Point2 + */ +class DeltaFactor: public NoiseModelFactor2 { + +public: + typedef DeltaFactor This; + typedef NoiseModelFactor2 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Point2 measured_; ///< the measurement + +public: + + /// Constructor + DeltaFactor(Key i, Key j, const Point2& measured, + const SharedNoiseModel& model) : + Base(model, i, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& pose, const Point2& point, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + Point2 d = pose.transform_to(point, H1, H2); + Point2 e = measured_.between(d); + return e.vector(); + } +}; + +/** + * DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes + */ +class DeltaFactorBase: public NoiseModelFactor4 { + +public: + typedef DeltaFactorBase This; + typedef NoiseModelFactor4 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Point2 measured_; ///< the measurement + +public: + + /// Constructor + DeltaFactorBase(Key b1, Key i, Key b2, Key j, const Point2& measured, + const SharedNoiseModel& model) : + Base(model, b1, i, b2, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& base1, const Pose2& pose, + const Pose2& base2, const Point2& point, // + boost::optional H1 = boost::none, // + boost::optional H2 = boost::none, // + boost::optional H3 = boost::none, // + boost::optional H4 = boost::none) const { + if (H1 || H2 || H3 || H4) { + // TODO use fixed-size matrices + Matrix D_pose_g_base1, D_pose_g_pose; + Pose2 pose_g = base1.compose(pose, D_pose_g_base1, D_pose_g_pose); + Matrix D_point_g_base2, D_point_g_point; + Point2 point_g = base2.transform_from(point, D_point_g_base2, + D_point_g_point); + Matrix D_e_pose_g, D_e_point_g; + Point2 d = pose_g.transform_to(point_g, D_e_pose_g, D_e_point_g); + if (H1) + *H1 = D_e_pose_g * D_pose_g_base1; + if (H2) + *H2 = D_e_pose_g * D_pose_g_pose; + if (H3) + *H3 = D_e_point_g * D_point_g_base2; + if (H4) + *H4 = D_e_point_g * D_point_g_point; + return measured_.localCoordinates(d); + } else { + Pose2 pose_g = base1.compose(pose); + Point2 point_g = base2.transform_from(point); + Point2 d = pose_g.transform_to(point_g); + return measured_.localCoordinates(d); + } + } +}; + +/** + * OdometryFactorBase: Pose2 odometry, with Basenodes + */ +class OdometryFactorBase: public NoiseModelFactor4 { + +public: + typedef OdometryFactorBase This; + typedef NoiseModelFactor4 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Pose2 measured_; ///< the measurement + +public: + + /// Constructor + OdometryFactorBase(Key b1, Key i, Key b2, Key j, const Pose2& measured, + const SharedNoiseModel& model) : + Base(model, b1, i, b2, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& base1, const Pose2& pose1, + const Pose2& base2, const Pose2& pose2, // + boost::optional H1 = boost::none, // + boost::optional H2 = boost::none, // + boost::optional H3 = boost::none, // + boost::optional H4 = boost::none) const { + if (H1 || H2 || H3 || H4) { + // TODO use fixed-size matrices + Matrix D_pose1_g_base1, D_pose1_g_pose1; + Pose2 pose1_g = base1.compose(pose1, D_pose1_g_base1, D_pose1_g_pose1); + Matrix D_pose2_g_base2, D_pose2_g_pose2; + Pose2 pose2_g = base2.compose(pose2, D_pose2_g_base2, D_pose2_g_pose2); + Matrix D_e_pose1_g, D_e_pose2_g; + Pose2 d = pose1_g.between(pose2_g, D_e_pose1_g, D_e_pose2_g); + if (H1) + *H1 = D_e_pose1_g * D_pose1_g_base1; + if (H2) + *H2 = D_e_pose1_g * D_pose1_g_pose1; + if (H3) + *H3 = D_e_pose2_g * D_pose2_g_base2; + if (H4) + *H4 = D_e_pose2_g * D_pose2_g_pose2; + return measured_.localCoordinates(d); + } else { + Pose2 pose1_g = base1.compose(pose1); + Pose2 pose2_g = base2.compose(pose2); + Pose2 d = pose1_g.between(pose2_g); + return measured_.localCoordinates(d); + } + } +}; + +} + diff --git a/tests/testOccupancyGrid.cpp b/gtsam_unstable/slam/tests/testOccupancyGrid.cpp similarity index 100% rename from tests/testOccupancyGrid.cpp rename to gtsam_unstable/slam/tests/testOccupancyGrid.cpp diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index 2fc471eab..bf9dc0e8e 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -95,12 +95,21 @@ TEST( PoseBetweenFactor, Error ) { // The expected error Vector expectedError(6); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + expectedError << -0.0298135267953815, + 0.0131341515747393, + 0.0968868439682154, + -0.145701634472172, + -0.134898525569125, + -0.0421026389164264; +#else expectedError << -0.029839512616488, 0.013145599455949, 0.096971291682578, -0.139187549519821, -0.142346243063553, -0.039088532100977; +#endif // Create a factor and calculate the error Key poseKey1(1); @@ -123,12 +132,23 @@ TEST( PoseBetweenFactor, ErrorWithTransform ) { // The expected error Vector expectedError(6); + // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) + expectedError << 0.0173358202010741, + 0.0222210698409755, + -0.0125032003886145, + 0.0263800787416566, + 0.00540285006310398, + 0.000175859555693563; +#else expectedError << 0.017337193670445, 0.022222830355243, -0.012504190982804, 0.026413288603739, 0.005237695303536, -0.000071612703633; +#endif + // Create a factor and calculate the error Key poseKey1(1); @@ -165,8 +185,8 @@ TEST( PoseBetweenFactor, Jacobian ) { factor.evaluateError(pose1, pose2, actualH1, actualH2); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); - CHECK(assert_equal(expectedH2, actualH2, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); + CHECK(assert_equal(expectedH2, actualH2, 1e-6)); } /* ************************************************************************* */ @@ -194,8 +214,8 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) { Vector error = factor.evaluateError(pose1, pose2, actualH1, actualH2); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); - CHECK(assert_equal(expectedH2, actualH2, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-6)); + CHECK(assert_equal(expectedH2, actualH2, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index 27bcd55ce..cbfa45431 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -90,12 +90,23 @@ TEST( PosePriorFactor, Error ) { // The expected error Vector expectedError(6); + // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) + expectedError << -0.182948257976108, + 0.13851858011118, + -0.157375974517456, + 0.766913166076379, + -1.22976117053126, + 0.949345561430261; +#else expectedError << -0.184137861505414, 0.139419283914526, -0.158399296722242, 0.740211733817804, -1.198210282560946, 1.008156093015192; +#endif + // Create a factor and calculate the error Key poseKey(1); @@ -116,12 +127,22 @@ TEST( PosePriorFactor, ErrorWithTransform ) { // The expected error Vector expectedError(6); + // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) + expectedError << -0.0224998729281528, + 0.191947887288328, + 0.273826035236257, + 1.36483391560855, + -0.754590051075035, + 0.585710674473659; +#else expectedError << -0.022712885347328, 0.193765110165872, 0.276418420819283, 1.497519863757366, -0.549375791422721, 0.452761203187666; +#endif // Create a factor and calculate the error Key poseKey(1); @@ -152,7 +173,7 @@ TEST( PosePriorFactor, Jacobian ) { factor.evaluateError(pose, actualH1); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } /* ************************************************************************* */ @@ -176,7 +197,7 @@ TEST( PosePriorFactor, JacobianWithTransform ) { factor.evaluateError(pose, actualH1); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp similarity index 79% rename from gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp rename to gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp index 9a3fe7f62..ee8db1267 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp @@ -369,6 +369,271 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ if(isDebugTest) tictoc_print_(); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, jacobianSVD ){ + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, landmarkDistance ){ + + double excludeLandmarksFutherThanDist = 2; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + // All factors are disabled and pose should remain where it is + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3),result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + Point3 landmark4(5, -0.5, 1); + + vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10,10); // add outlier + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, + JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor3->add(measurements_cam3, views, model, K); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor4->add(measurements_cam4, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); + + // All factors are disabled and pose should remain where it is + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, jacobianQ ){ + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp new file mode 100644 index 000000000..44dca9b8e --- /dev/null +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -0,0 +1,149 @@ +/* ---------------------------------------------------------------------------- + + * 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 testTSAMFactors.cpp + * @brief Unit tests for TSAM 1 Factors + * @author Frank Dellaert + * @date May 2014 + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +Key i(1), j(2); // Key for pose and point + +//************************************************************************* +TEST( DeltaFactor, all ) { + // Create a factor + Point2 measurement(1, 1); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + DeltaFactor factor(i, j, measurement, model); + + // Set the linearization point + Pose2 pose(1, 2, 0); + Point2 point(4, 11); + Vector2 expected(4 - 1 - 1, 11 - 2 - 1); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual; + Vector actual = factor.evaluateError(pose, point, H1Actual, H2Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected; + H1Expected = numericalDerivative11( + boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, + boost::none), pose); + H2Expected = numericalDerivative11( + boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none, + boost::none), point); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); +} + +//************************************************************************* +TEST( DeltaFactorBase, all ) { + // Create a factor + Key b1(10), b2(20); + Point2 measurement(1, 1); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + DeltaFactorBase factor(b1, i, b2, j, measurement, model); + + // Set the linearization point + Pose2 base1, base2(1, 0, 0); + Pose2 pose(1, 2, 0); + Point2 point(4, 11); + Vector2 expected(4 + 1 - 1 - 1, 11 - 2 - 1); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual, H4Actual; + Vector actual = factor.evaluateError(base1, pose, base2, point, H1Actual, + H2Actual, H3Actual, H4Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected, H3Expected, H4Expected; + H1Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, + point, boost::none, boost::none, boost::none, boost::none), base1); + H2Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, + point, boost::none, boost::none, boost::none, boost::none), pose); + H3Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, + point, boost::none, boost::none, boost::none, boost::none), base2); + H4Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, + _1, boost::none, boost::none, boost::none, boost::none), point); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); + EXPECT(assert_equal(H3Expected, H3Actual, 1e-9)); + EXPECT(assert_equal(H4Expected, H4Actual, 1e-9)); +} + +//************************************************************************* +TEST( OdometryFactorBase, all ) { + // Create a factor + Key b1(10), b2(20); + Pose2 measurement(1, 1, 0); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + OdometryFactorBase factor(b1, i, b2, j, measurement, model); + + // Set the linearization pose2 + Pose2 base1, base2(1, 0, 0); + Pose2 pose1(1, 2, 0), pose2(4, 11, 0); + Vector3 expected(4 + 1 - 1 - 1, 11 - 2 - 1, 0); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual, H4Actual; + Vector actual = factor.evaluateError(base1, pose1, base2, pose2, H1Actual, + H2Actual, H3Actual, H4Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected, H3Expected, H4Expected; + H1Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, + pose2, boost::none, boost::none, boost::none, boost::none), base1); + H2Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, + pose2, boost::none, boost::none, boost::none, boost::none), pose1); + H3Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, + pose2, boost::none, boost::none, boost::none, boost::none), base2); + H4Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, + base2, _1, boost::none, boost::none, boost::none, boost::none), + pose2); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); + EXPECT(assert_equal(H3Expected, H3Actual, 1e-9)); + EXPECT(assert_equal(H4Expected, H4Actual, 1e-9)); +} + +//************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//************************************************************************* + diff --git a/matlab.h b/matlab.h index 6c2b137d7..c4891a615 100644 --- a/matlab.h +++ b/matlab.h @@ -28,120 +28,228 @@ #include #include +#include + #include namespace gtsam { - namespace utilities { - - /// Extract all Point2 values into a single matrix [x y] - Matrix extractPoint2(const Values& values) { - size_t j=0; - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(),2); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) - result.row(j++) = key_value.value.vector(); - return result; - } - - /// Extract all Point3 values into a single matrix [x y z] - Matrix extractPoint3(const Values& values) { - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(),3); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) - result.row(j++) = key_value.value.vector(); - return result; - } - - /// Extract all Pose2 values into a single matrix [x y theta] - Matrix extractPose2(const Values& values) { - Values::ConstFiltered poses = values.filter(); - Matrix result(poses.size(),3); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) - result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); - return result; - } - - /// Extract all Pose3 values - Values allPose3s(const Values& values) { - return values.filter(); - } - - /// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] - Matrix extractPose3(const Values& values) { - Values::ConstFiltered poses = values.filter(); - Matrix result(poses.size(),12); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { - result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); - result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); - result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); - result.row(j).tail(3) = key_value.value.translation().vector(); - j++; - } - return result; - } - - /// perturb all Point2 using normally distributed noise - void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// perturb all Point3 using normally distributed noise - void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// insert a number of initial point values by backprojecting - void insertBackprojections(Values& values, const SimpleCamera& camera, const Vector& J, const Matrix& Z, double depth) { - if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); - if (Z.cols() != J.size()) throw std::invalid_argument("insertBackProjections: J and Z must have same number of entries"); - for(int k=0;k > - (Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); - } - } - - /// calculate the errors of all projection factors in a graph - Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) { - // first count - size_t K = 0, k=0; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) - if (boost::dynamic_pointer_cast >(f)) ++K; - // now fill - Matrix errors(2,K); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { - boost::shared_ptr > p = boost::dynamic_pointer_cast >(f); - if (p) errors.col(k++) = p->unwhitenedError(values); - } - return errors; - } - - } +namespace utilities { +// Create a KeyList from indices +FastList createKeyList(const Vector& I) { + FastList set; + for (int i = 0; i < I.size(); i++) + set.push_back(I[i]); + return set; +} + +// Create a KeyList from indices using symbol +FastList createKeyList(string s, const Vector& I) { + FastList set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.push_back(Symbol(c, I[i])); + return set; +} + +// Create a KeyVector from indices +FastVector createKeyVector(const Vector& I) { + FastVector set; + for (int i = 0; i < I.size(); i++) + set.push_back(I[i]); + return set; +} + +// Create a KeyVector from indices using symbol +FastVector createKeyVector(string s, const Vector& I) { + FastVector set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.push_back(Symbol(c, I[i])); + return set; +} + +// Create a KeySet from indices +FastSet createKeySet(const Vector& I) { + FastSet set; + for (int i = 0; i < I.size(); i++) + set.insert(I[i]); + return set; +} + +// Create a KeySet from indices using symbol +FastSet createKeySet(string s, const Vector& I) { + FastSet set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.insert(symbol(c, I[i])); + return set; +} + +/// Extract all Point2 values into a single matrix [x y] +Matrix extractPoint2(const Values& values) { + size_t j = 0; + Values::ConstFiltered points = values.filter(); + Matrix result(points.size(), 2); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; +} + +/// Extract all Point3 values into a single matrix [x y z] +Matrix extractPoint3(const Values& values) { + Values::ConstFiltered points = values.filter(); + Matrix result(points.size(), 3); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; +} + +/// Extract all Pose2 values into a single matrix [x y theta] +Matrix extractPose2(const Values& values) { + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(), 3); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) + result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); + return result; +} + +/// Extract all Pose3 values +Values allPose3s(const Values& values) { + return values.filter(); +} + +/// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] +Matrix extractPose3(const Values& values) { + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(), 12); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { + result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); + result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); + result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); + result.row(j).tail(3) = key_value.value.translation().vector(); + j++; + } + return result; +} + +/// Perturb all Point2 values using normally distributed noise +void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, + sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Perturb all Pose2 values using normally distributed noise +void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = + 42u) { + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( + Vector3(sigmaT, sigmaT, sigmaR)); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Perturb all Point3 values using normally distributed noise +void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3, + sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Insert a number of initial point values by backprojecting +void insertBackprojections(Values& values, const SimpleCamera& camera, + const Vector& J, const Matrix& Z, double depth) { + if (Z.rows() != 2) + throw std::invalid_argument("insertBackProjections: Z must be 2*K"); + if (Z.cols() != J.size()) + throw std::invalid_argument( + "insertBackProjections: J and Z must have same number of entries"); + for (int k = 0; k < Z.cols(); k++) { + Point2 p(Z(0, k), Z(1, k)); + Point3 P = camera.backproject(p, depth); + values.insert(J(k), P); + } +} + +/// Insert multiple projection factors for a single pose key +void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, + const Vector& J, const Matrix& Z, const SharedNoiseModel& model, + const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { + if (Z.rows() != 2) + throw std::invalid_argument("addMeasurements: Z must be 2*K"); + if (Z.cols() != J.size()) + throw std::invalid_argument( + "addMeasurements: J and Z must have same number of entries"); + for (int k = 0; k < Z.cols(); k++) { + graph.push_back( + boost::make_shared >( + Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); + } +} + +/// Calculate the errors of all projection factors in a graph +Matrix reprojectionErrors(const NonlinearFactorGraph& graph, + const Values& values) { + // first count + size_t K = 0, k = 0; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) + if (boost::dynamic_pointer_cast >( + f)) + ++K; + // now fill + Matrix errors(2, K); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { + boost::shared_ptr > p = + boost::dynamic_pointer_cast >( + f); + if (p) + errors.col(k++) = p->unwhitenedError(values); + } + return errors; +} + +/// Convert from local to world coordinates +Values localToWorld(const Values& local, const Pose2& base, + const FastVector user_keys = FastVector()) { + + Values world; + + // if no keys given, get all keys from local values + FastVector keys(user_keys); + if (keys.size()==0) + keys = FastVector(local.keys()); + + // Loop over all keys + BOOST_FOREACH(Key key, keys) { + try { + // if value is a Pose2, compose it with base pose + Pose2 pose = local.at(key); + world.insert(key, base.compose(pose)); + } catch (std::exception e1) { + try { + // if value is a Point2, transform it from base pose + Point2 point = local.at(key); + world.insert(key, base.transform_from(point)); + } catch (std::exception e2) { + // if not Pose2 or Point2, do nothing + } + } + } + return world; +} + +} } diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 8d15bc913..023c61dbe 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -173,3 +173,20 @@ % symbol - create a Symbol key % symbolChr - get character from a symbol key % symbolIndex - get index from a symbol key +% +%% Wrapped C++ Convenience Functions for use within MATLAB +% utilities.createKeyList - Create KeyList from indices +% utilities.createKeyVector - Create KeyVector from indices +% utilities.createKeySet - Create KeySet from indices +% utilities.extractPoint2 - Extract all Point2 values into a single matrix [x y] +% utilities.extractPoint3 - Extract all Point3 values into a single matrix [x y z] +% utilities.extractPose2 - Extract all Pose2 values into a single matrix [x y theta] +% utilities.allPose3s - Extract all Pose3 values +% utilities.extractPose3 - Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] +% utilities.perturbPoint2 - Perturb all Point2 values using normally distributed noise +% utilities.perturbPose2 - Perturb all Pose2 values using normally distributed noise +% utilities.perturbPoint3 - Perturb all Point3 values using normally distributed noise +% utilities.insertBackprojections - Insert a number of initial point values by backprojecting +% utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key +% utilities.reprojectionErrors - Calculate the errors of all projection factors in a graph +% utilities.localToWorld - Convert from local to world coordinates diff --git a/matlab/+gtsam/plot2DTrajectory.m b/matlab/+gtsam/plot2DTrajectory.m index c6b0f85aa..45e7fe467 100644 --- a/matlab/+gtsam/plot2DTrajectory.m +++ b/matlab/+gtsam/plot2DTrajectory.m @@ -8,53 +8,41 @@ function plot2DTrajectory(values, linespec, marginals) import gtsam.* if ~exist('linespec', 'var') || isempty(linespec) - linespec = 'k*-'; + linespec = 'k*-'; end haveMarginals = exist('marginals', 'var'); -keys = KeyVector(values.keys); holdstate = ishold; hold on -% Plot poses and covariance matrices -lastIndex = []; -for i = 0:keys.size-1 +% Do something very efficient to draw trajectory +poses = utilities.extractPose2(values); +X = poses(:,1); +Y = poses(:,2); +theta = poses(:,3); +plot(X,Y,linespec); + +% Quiver can also be vectorized if no marginals asked +if ~haveMarginals + C = cos(theta); + S = sin(theta); + quiver(X,Y,C,S,0.1,linespec); +else + % plotPose2 does both quiver and covariance matrix + keys = KeyVector(values.keys); + for i = 0:keys.size-1 key = keys.at(i); x = values.at(key); if isa(x, 'gtsam.Pose2') - if ~isempty(lastIndex) - % Draw line from last pose then covariance ellipse on top of - % last pose. - lastKey = keys.at(lastIndex); - lastPose = values.at(lastKey); - plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - gtsam.plotPose2(lastPose, 'g', P); - else - gtsam.plotPose2(lastPose, 'g', []); - end - - end - lastIndex = i; - end -end - -% Draw final covariance ellipse -if ~isempty(lastIndex) - lastKey = keys.at(lastIndex); - lastPose = values.at(lastKey); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - gtsam.plotPose2(lastPose, 'g', P); - else - gtsam.plotPose2(lastPose, 'g', []); + P = marginals.marginalCovariance(key); + gtsam.plotPose2(x,linespec(1), P); end + end end if ~holdstate - hold off + hold off end end diff --git a/matlab/gtsam_examples/Pose2SLAMExample_advanced.m b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m index 343dee05b..64cda5afc 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_advanced.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m @@ -59,16 +59,16 @@ params.setAbsoluteErrorTol(1e-15); params.setRelativeErrorTol(1e-15); params.setVerbosity('ERROR'); params.setVerbosityDL('VERBOSE'); -params.setOrdering(graph.orderingCOLAMD(initialEstimate)); +params.setOrdering(graph.orderingCOLAMD()); optimizer = DoglegOptimizer(graph, initialEstimate, params); result = optimizer.optimizeSafely(); result.print('final result'); %% Get the corresponding dense matrix -ord = graph.orderingCOLAMD(result); -gfg = graph.linearize(result,ord); -denseAb = gfg.denseJacobian; +ord = graph.orderingCOLAMD(); +gfg = graph.linearize(result); +denseAb = gfg.augmentedJacobian; %% Get sparse matrix A and RHS b IJS = gfg.sparseJacobian_(); diff --git a/matlab/gtsam_examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m index b4957cce3..83ec949cc 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m @@ -36,7 +36,9 @@ toc hold on; plot2DTrajectory(result, 'b-*'); %% Plot Covariance Ellipses +tic marginals = Marginals(graph, result); +toc P={}; for i=1:result.size()-1 pose_i = result.at(i); diff --git a/matlab/gtsam_examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m index 01df4fc33..39e48c204 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose3SLAMExample_graph.m @@ -12,6 +12,8 @@ import gtsam.* +%% PLEASE NOTE THAT PLOTTING TAKES A VERY LONG TIME HERE + %% Find data file N = 2500; % dataset = 'sphere_smallnoise.graph'; diff --git a/matlab/gtsam_tests/testJacobianFactor.m b/matlab/gtsam_tests/testJacobianFactor.m index 3bc8f4edd..bba6ca5ac 100644 --- a/matlab/gtsam_tests/testJacobianFactor.m +++ b/matlab/gtsam_tests/testJacobianFactor.m @@ -36,7 +36,9 @@ model4 = noiseModel.Diagonal.Sigmas(sigmas); combined = JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4); % eliminate the first variable (x2) in the combined factor, destructive ! -actualCG = combined.eliminateFirst(); +ord=Ordering; +ord.push_back(x2); +actualCG = combined.eliminate(ord); % create expected Conditional Gaussian R11 = [ @@ -52,7 +54,7 @@ S13 = [ +0.00,-8.94427 ]; d=[2.23607;-1.56525]; -expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13,[1;1]); +expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13); % check if the result matches CHECK('actualCG.equals(expectedCG,1e-5)',actualCG.equals(expectedCG,1e-4)); diff --git a/matlab/gtsam_tests/testUtilities.m b/matlab/gtsam_tests/testUtilities.m new file mode 100644 index 000000000..da8dec789 --- /dev/null +++ b/matlab/gtsam_tests/testUtilities.m @@ -0,0 +1,47 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 +% +% @brief Checks for results of functions in utilities namespace +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +import gtsam.* + +%% Create keys for variables +x1 = symbol('x',1); x2 = symbol('x',2); x3 = symbol('x',3); + +actual = utilities.createKeyList([1;2;3]); +CHECK('KeyList', isa(actual,'gtsam.KeyList')); +CHECK('size==3', actual.size==3); +CHECK('actual.front==1', actual.front==1); + +actual = utilities.createKeyList('x',[1;2;3]); +CHECK('KeyList', isa(actual,'gtsam.KeyList')); +CHECK('size==3', actual.size==3); +CHECK('actual.front==x1', actual.front==x1); + +actual = utilities.createKeyVector([1;2;3]); +CHECK('KeyVector', isa(actual,'gtsam.KeyVector')); +CHECK('size==3', actual.size==3); +CHECK('actual.at(0)==1', actual.at(0)==1); + +actual = utilities.createKeyVector('x',[1;2;3]); +CHECK('KeyVector', isa(actual,'gtsam.KeyVector')); +CHECK('size==3', actual.size==3); +CHECK('actual.at(0)==x1', actual.at(0)==x1); + +actual = utilities.createKeySet([1;2;3]); +CHECK('KeySet', isa(actual,'gtsam.KeySet')); +CHECK('size==3', actual.size==3); +CHECK('actual.count(1)', actual.count(1)); + +actual = utilities.createKeySet('x',[1;2;3]); +CHECK('KeySet', isa(actual,'gtsam.KeySet')); +CHECK('size==3', actual.size==3); +CHECK('actual.count(x1)', actual.count(x1)); + diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 2cad40df8..c379179c5 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -33,5 +33,8 @@ testVisualISAMExample display 'Starting: testSerialization' testSerialization +display 'Starting: testUtilities' +testUtilities + % end of tests display 'Tests complete!' diff --git a/matlab/unstable_examples/.gitignore b/matlab/unstable_examples/.gitignore new file mode 100644 index 000000000..6d725d9bc --- /dev/null +++ b/matlab/unstable_examples/.gitignore @@ -0,0 +1 @@ +*.m~ diff --git a/matlab/unstable_examples/testTSAMFactors.m b/matlab/unstable_examples/testTSAMFactors.m new file mode 100644 index 000000000..abdfc5922 --- /dev/null +++ b/matlab/unstable_examples/testTSAMFactors.m @@ -0,0 +1,62 @@ +% TSAMFactorsExample +% Frank Dellaert, May 2014 + +import gtsam.*; + +% noise models +noisePrior = noiseModel.Diagonal.Sigmas([100; 100; 100]); +noiseDelta = noiseModel.Isotropic.Sigma(2, 0.1); +noiseOdom = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]); + +% Example is 2 landmarks 1 and 2, 2 poses 10 and 20, 2 base nodes 100 and 200 +% l1 l2 +% + + +% - b - p - p - b +% +---+---+---+ + +% True values +b1 = Pose2(0,0,0); +b2 = Pose2(2,0,0); +l1 = Point2(0,1); +l2 = Point2(2,1); + +% Create a graph +graph = NonlinearFactorGraph; +origin = Pose2; +graph.add(gtsam.PriorFactorPose2(10, origin, noisePrior)) +graph.add(gtsam.PriorFactorPose2(20, origin, noisePrior)) +graph.add(gtsam.PriorFactorPose2(100, origin, noisePrior)) +graph.add(DeltaFactor(10, 1, b1.transform_to(l1), noiseDelta)) +graph.add(DeltaFactor(20, 1, b2.transform_to(l2), noiseDelta)) +graph.add(DeltaFactorBase(100,10, 200,2, b1.transform_to(l2), noiseDelta)) +graph.add(DeltaFactorBase(200,20, 100,1, b2.transform_to(l1), noiseDelta)) +graph.add(OdometryFactorBase(100,10,200,20, Pose2(2,0,0), noiseOdom)) + +% Initial values +initial = Values; +initial.insert(100,origin); +initial.insert(10,origin); +initial.insert(1,l1); +initial.insert(2,l2); +initial.insert(20,origin); +initial.insert(200,origin); + +% optimize +params = LevenbergMarquardtParams; +% params.setVerbosity('ERROR'); +optimizer = LevenbergMarquardtOptimizer(graph, initial, params); +result = optimizer.optimize(); + +% Check result +CHECK('error',result.at(100).equals(b1,1e-5)) +CHECK('error',result.at(10).equals(origin,1e-5)) +CHECK('error',result.at(1).equals(Point2(0,1),1e-5)) +CHECK('error',result.at(2).equals(Point2(0,1),1e-5)) +CHECK('error',result.at(20).equals(origin,1e-5)) +CHECK('error',result.at(200).equals(b2,1e-5)) + +% Check error +CHECK('error',abs(graph.error(result))<1e-9) +for i=0:7 + CHECK('error',abs(graph.at(i).error(result))<1e-9) +end diff --git a/tests/testGaussianBayesTree.cpp b/tests/testGaussianBayesTreeB.cpp similarity index 100% rename from tests/testGaussianBayesTree.cpp rename to tests/testGaussianBayesTreeB.cpp diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 3bcb313b0..55329d8e9 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -35,8 +35,6 @@ using namespace std; using namespace gtsam; using boost::shared_ptr; -const double tol = 1e-4; - // SETDEBUG("ISAM2 update", true); // SETDEBUG("ISAM2 update verbose", true); // SETDEBUG("ISAM2 recalculate", true); diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index c3798e5ce..d76556e4a 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -16,13 +16,14 @@ * @author Richard Roberts **/ -#include -#include -#include +#include "Argument.h" + #include #include -#include "Argument.h" +#include +#include +#include using namespace std; using namespace wrap; @@ -31,18 +32,24 @@ using namespace wrap; string Argument::matlabClass(const string& delim) const { string result; BOOST_FOREACH(const string& ns, namespaces) - result += ns + delim; - if (type=="string" || type=="unsigned char" || type=="char") + result += ns + delim; + if (type == "string" || type == "unsigned char" || type == "char") return result + "char"; - if (type=="Vector" || type=="Matrix") + if (type == "Vector" || type == "Matrix") return result + "double"; - if (type=="int" || type=="size_t") + if (type == "int" || type == "size_t") return result + "numeric"; - if (type=="bool") + if (type == "bool") return result + "logical"; return result + type; } +/* ************************************************************************* */ +bool Argument::isScalar() const { + return (type == "bool" || type == "char" || type == "unsigned char" + || type == "int" || type == "size_t" || type == "double"); +} + /* ************************************************************************* */ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << " "; @@ -52,7 +59,8 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { if (is_ptr) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer - file.oss << "boost::shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; + file.oss << "boost::shared_ptr<" << cppType << "> " << name + << " = unwrap_shared_ptr< "; else if (is_ref) // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< "; @@ -65,23 +73,28 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << cppType << " " << name << " = unwrap< "; file.oss << cppType << " >(" << matlabName; - if (is_ptr || is_ref) file.oss << ", \"ptr_" << matlabUniqueType << "\""; + if (is_ptr || is_ref) + file.oss << ", \"ptr_" << matlabUniqueType << "\""; file.oss << ");" << endl; } /* ************************************************************************* */ string Argument::qualifiedType(const string& delim) const { string result; - BOOST_FOREACH(const string& ns, namespaces) result += ns + delim; + BOOST_FOREACH(const string& ns, namespaces) + result += ns + delim; return result + type; } /* ************************************************************************* */ string ArgumentList::types() const { string str; - bool first=true; + bool first = true; BOOST_FOREACH(Argument arg, *this) { - if (!first) str += ","; str += arg.type; first=false; + if (!first) + str += ","; + str += arg.type; + first = false; } return str; } @@ -89,16 +102,17 @@ string ArgumentList::types() const { /* ************************************************************************* */ string ArgumentList::signature() const { string sig; - bool cap=false; + bool cap = false; BOOST_FOREACH(Argument arg, *this) { BOOST_FOREACH(char ch, arg.type) - if(isupper(ch)) { - sig += ch; - //If there is a capital letter, we don't want to read it below - cap=true; - } - if(!cap) sig += arg.type[0]; + if (isupper(ch)) { + sig += ch; + //If there is a capital letter, we don't want to read it below + cap = true; + } + if (!cap) + sig += arg.type[0]; //Reset to default cap = false; } @@ -109,23 +123,77 @@ string ArgumentList::signature() const { /* ************************************************************************* */ string ArgumentList::names() const { string str; - bool first=true; + bool first = true; BOOST_FOREACH(Argument arg, *this) { - if (!first) str += ","; str += arg.name; first=false; + if (!first) + str += ","; + str += arg.name; + first = false; } return str; } +/* ************************************************************************* */ +bool ArgumentList::allScalar() const { + BOOST_FOREACH(Argument arg, *this) + if (!arg.isScalar()) return false; + return true; +} + /* ************************************************************************* */ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const { int index = start; BOOST_FOREACH(Argument arg, *this) { stringstream buf; buf << "in[" << index << "]"; - arg.matlab_unwrap(file,buf.str()); + arg.matlab_unwrap(file, buf.str()); index++; } } /* ************************************************************************* */ +void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { + file.oss << name << "("; + bool first = true; + BOOST_FOREACH(Argument arg, *this) { + if (!first) + file.oss << ", "; + file.oss << arg.type << " " << arg.name; + first = false; + } + file.oss << ")"; +} +/* ************************************************************************* */ +void ArgumentList::emit_call(FileWriter& file, const ReturnValue& returnVal, + const string& wrapperName, int id, bool staticMethod) const { + returnVal.emit_matlab(file); + file.oss << wrapperName << "(" << id; + if (!staticMethod) + file.oss << ", this"; + file.oss << ", varargin{:});\n"; +} +/* ************************************************************************* */ +void ArgumentList::emit_conditional_call(FileWriter& file, + const ReturnValue& returnVal, const string& wrapperName, int id, + bool staticMethod) const { + // Check nr of arguments + file.oss << "if length(varargin) == " << size(); + if (size() > 0) + file.oss << " && "; + // ...and their types + bool first = true; + for (size_t i = 0; i < size(); i++) { + if (!first) + file.oss << " && "; + file.oss << "isa(varargin{" << i + 1 << "},'" << (*this)[i].matlabClass(".") + << "')"; + first = false; + } + file.oss << "\n"; + + // output call to C++ wrapper + file.oss << " "; + emit_call(file, returnVal, wrapperName, id, staticMethod); +} +/* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index f46eaa427..6f791978a 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -19,11 +19,12 @@ #pragma once +#include "FileWriter.h" +#include "ReturnValue.h" + #include #include -#include "FileWriter.h" - namespace wrap { /// Argument class @@ -40,6 +41,9 @@ struct Argument { /// return MATLAB class for use in isa(x,class) std::string matlabClass(const std::string& delim = "") const; + /// Check if will be unwrapped using scalar login in wrap/matlab.h + bool isScalar() const; + /// adds namespaces to type std::string qualifiedType(const std::string& delim = "") const; @@ -59,6 +63,9 @@ struct ArgumentList: public std::vector { /// create a comma-separated string listing all argument names, used in m-files std::string names() const; + /// Check if all arguments scalar + bool allScalar() const; + // MATLAB code generation: /** @@ -68,6 +75,32 @@ struct ArgumentList: public std::vector { */ void matlab_unwrap(FileWriter& file, int start = 0) const; // MATLAB to C++ + /** + * emit MATLAB prototype + * @param file output stream + * @param name of method or function + */ + void emit_prototype(FileWriter& file, const std::string& name) const; + + /** + * emit emit MATLAB call to wrapper + * @param file output stream + * @param returnVal the return value + * @param wrapperName of method or function + * @param staticMethod flag to emit "this" in call + */ + void emit_call(FileWriter& file, const ReturnValue& returnVal, + const std::string& wrapperName, int id, bool staticMethod = false) const; + + /** + * emit conditional MATLAB call to wrapper (checking arguments first) + * @param file output stream + * @param returnVal the return value + * @param wrapperName of method or function + * @param staticMethod flag to emit "this" in call + */ + void emit_conditional_call(FileWriter& file, const ReturnValue& returnVal, + const std::string& wrapperName, int id, bool staticMethod = false) const; }; } // \namespace wrap diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 3b8c41041..075c98811 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -7,62 +7,62 @@ * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - + * -------------------------------------------------------------------------- */ + /** * @file Class.cpp * @author Frank Dellaert * @author Andrew Melim * @author Richard Roberts - **/ - + **/ + +#include "Class.h" +#include "utilities.h" +#include "Argument.h" + +#include +#include + #include #include #include //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC - -#include -#include - -#include "Class.h" -#include "utilities.h" -#include "Argument.h" - -using namespace std; -using namespace wrap; - -/* ************************************************************************* */ -void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, - const TypeAttributesTable& typeAttributes, - FileWriter& wrapperFile, vector& functionNames) const { - +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, + const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, + vector& functionNames) const { + // Create namespace folders - createNamespaceStructure(namespaces, toolboxPath); - + createNamespaceStructure(namespaces, toolboxPath); + // open destination classFile - string classFile = toolboxPath; - if(!namespaces.empty()) - classFile += "/+" + wrap::qualifiedName("/+", namespaces); - classFile += "/" + name + ".m"; - FileWriter proxyFile(classFile, verbose_, "%"); - + string classFile = toolboxPath; + if (!namespaces.empty()) + classFile += "/+" + wrap::qualifiedName("/+", namespaces); + classFile += "/" + name + ".m"; + FileWriter proxyFile(classFile, verbose_, "%"); + // get the name of actual matlab object - const string matlabQualName = qualifiedName("."), matlabUniqueName = qualifiedName(), cppName = qualifiedName("::"); - const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent); - const string cppBaseName = wrap::qualifiedName("::", qualifiedParent); - + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppName = qualifiedName("::"); + const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent); + const string cppBaseName = wrap::qualifiedName("::", qualifiedParent); + // emit class proxy code // we want our class to inherit the handle class for memory purposes - const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; + const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; comment_fragment(proxyFile); - proxyFile.oss << "classdef " << name << " < " << parent << endl; + proxyFile.oss << "classdef " << name << " < " << parent << endl; proxyFile.oss << " properties\n"; proxyFile.oss << " ptr_" << matlabUniqueName << " = 0\n"; proxyFile.oss << " end\n"; proxyFile.oss << " methods\n"; - + // Constructor proxyFile.oss << " function obj = " << name << "(varargin)\n"; // Special pointer constructors - one in MATLAB to create an object and @@ -72,284 +72,325 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, // other wrap modules - to add these to their collectors the pointer is // passed from one C++ module into matlab then back into the other C++ // module. - pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, functionNames); - wrapperFile.oss << "\n"; + pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, + functionNames); + wrapperFile.oss << "\n"; // Regular constructors - BOOST_FOREACH(ArgumentList a, constructor.args_list) - { - const int id = (int)functionNames.size(); - constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), id, a); - const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, - cppName, matlabUniqueName, cppBaseName, id, a); - wrapperFile.oss << "\n"; - functionNames.push_back(wrapFunctionName); - } - proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of " << matlabQualName << " constructor');\n"; - proxyFile.oss << " end\n"; - if(!qualifiedParent.empty()) - proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" << ptr_constructor_key << "), base_ptr);\n"; - proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; - proxyFile.oss << " end\n\n"; - + BOOST_FOREACH(ArgumentList a, constructor.args_list) { + const int id = (int) functionNames.size(); + constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), + id, a); + const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, + cppName, matlabUniqueName, cppBaseName, id, a); + wrapperFile.oss << "\n"; + functionNames.push_back(wrapFunctionName); + } + proxyFile.oss << " else\n"; + proxyFile.oss << " error('Arguments do not match any overload of " + << matlabQualName << " constructor');\n"; + proxyFile.oss << " end\n"; + if (!qualifiedParent.empty()) + proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" + << ptr_constructor_key << "), base_ptr);\n"; + proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; + proxyFile.oss << " end\n\n"; + // Deconstructor - { - const int id = (int)functionNames.size(); - deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id); - proxyFile.oss << "\n"; - const string functionName = deconstructor.wrapper_fragment(wrapperFile, cppName, matlabUniqueName, id); - wrapperFile.oss << "\n"; - functionNames.push_back(functionName); - } - proxyFile.oss << " function display(obj), obj.print(''); end\n %DISPLAY Calls print on the object\n"; - proxyFile.oss << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; - + { + const int id = (int) functionNames.size(); + deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id); + proxyFile.oss << "\n"; + const string functionName = deconstructor.wrapper_fragment(wrapperFile, + cppName, matlabUniqueName, id); + wrapperFile.oss << "\n"; + functionNames.push_back(functionName); + } + proxyFile.oss + << " function display(obj), obj.print(''); end\n %DISPLAY Calls print on the object\n"; + proxyFile.oss + << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; + // Methods - BOOST_FOREACH(const Methods::value_type& name_m, methods) { - const Method& m = name_m.second; - m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); - proxyFile.oss << "\n"; - wrapperFile.oss << "\n"; - } + BOOST_FOREACH(const Methods::value_type& name_m, methods) { + const Method& m = name_m.second; + m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, + matlabUniqueName, wrapperName, typeAttributes, functionNames); + proxyFile.oss << "\n"; + wrapperFile.oss << "\n"; + } if (hasSerialization) serialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames); - - proxyFile.oss << " end\n"; - proxyFile.oss << "\n"; - proxyFile.oss << " methods(Static = true)\n"; - + + proxyFile.oss << " end\n"; + proxyFile.oss << "\n"; + proxyFile.oss << " methods(Static = true)\n"; + // Static methods - BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { - const StaticMethod& m = name_m.second; - m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); - proxyFile.oss << "\n"; - wrapperFile.oss << "\n"; - } + BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { + const StaticMethod& m = name_m.second; + m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, + matlabUniqueName, wrapperName, typeAttributes, functionNames); + proxyFile.oss << "\n"; + wrapperFile.oss << "\n"; + } if (hasSerialization) - deserialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames); + deserialization_fragments(proxyFile, wrapperFile, wrapperName, + functionNames); proxyFile.oss << " end\n"; proxyFile.oss << "end\n"; - + // Close file - proxyFile.emit(true); -} - -/* ************************************************************************* */ -string Class::qualifiedName(const string& delim) const { - return ::wrap::qualifiedName(delim, namespaces, name); -} - -/* ************************************************************************* */ -void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const string& wrapperName, vector& functionNames) const { - - const string matlabUniqueName = qualifiedName(), cppName = qualifiedName("::"); - const string baseCppName = wrap::qualifiedName("::", qualifiedParent); - - const int collectorInsertId = (int)functionNames.size(); - const string collectorInsertFunctionName = matlabUniqueName + "_collectorInsertAndMakeBase_" + boost::lexical_cast(collectorInsertId); - functionNames.push_back(collectorInsertFunctionName); - - int upcastFromVoidId; - string upcastFromVoidFunctionName; - if(isVirtual) { - upcastFromVoidId = (int)functionNames.size(); - upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + boost::lexical_cast(upcastFromVoidId); - functionNames.push_back(upcastFromVoidFunctionName); - } - + proxyFile.emit(true); +} + +/* ************************************************************************* */ +string Class::qualifiedName(const string& delim) const { + return ::wrap::qualifiedName(delim, namespaces, name); +} + +/* ************************************************************************* */ +void Class::pointer_constructor_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const string& wrapperName, + vector& functionNames) const { + + const string matlabUniqueName = qualifiedName(), cppName = qualifiedName( + "::"); + const string baseCppName = wrap::qualifiedName("::", qualifiedParent); + + const int collectorInsertId = (int) functionNames.size(); + const string collectorInsertFunctionName = matlabUniqueName + + "_collectorInsertAndMakeBase_" + + boost::lexical_cast(collectorInsertId); + functionNames.push_back(collectorInsertFunctionName); + + int upcastFromVoidId; + string upcastFromVoidFunctionName; + if (isVirtual) { + upcastFromVoidId = (int) functionNames.size(); + upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + + boost::lexical_cast(upcastFromVoidId); + functionNames.push_back(upcastFromVoidFunctionName); + } + // MATLAB constructor that assigns pointer to matlab object then calls c++ // function to add the object to the collector. - if(isVirtual) { - proxyFile.oss << " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))"; - } else { - proxyFile.oss << " if nargin == 2"; - } - proxyFile.oss << " && isa(varargin{1}, 'uint64') && varargin{1} == uint64(" << ptr_constructor_key << ")\n"; - if(isVirtual) { - proxyFile.oss << " if nargin == 2\n"; - proxyFile.oss << " my_ptr = varargin{2};\n"; - proxyFile.oss << " else\n"; - proxyFile.oss << " my_ptr = " << wrapperName << "(" << upcastFromVoidId << ", varargin{2});\n"; - proxyFile.oss << " end\n"; - } else { - proxyFile.oss << " my_ptr = varargin{2};\n"; - } - if(qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back - proxyFile.oss << " "; - else - proxyFile.oss << " base_ptr = "; + if (isVirtual) { + proxyFile.oss + << " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))"; + } else { + proxyFile.oss << " if nargin == 2"; + } + proxyFile.oss << " && isa(varargin{1}, 'uint64') && varargin{1} == uint64(" + << ptr_constructor_key << ")\n"; + if (isVirtual) { + proxyFile.oss << " if nargin == 2\n"; + proxyFile.oss << " my_ptr = varargin{2};\n"; + proxyFile.oss << " else\n"; + proxyFile.oss << " my_ptr = " << wrapperName << "(" + << upcastFromVoidId << ", varargin{2});\n"; + proxyFile.oss << " end\n"; + } else { + proxyFile.oss << " my_ptr = varargin{2};\n"; + } + if (qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back + proxyFile.oss << " "; + else + proxyFile.oss << " base_ptr = "; proxyFile.oss << wrapperName << "(" << collectorInsertId << ", my_ptr);\n"; // Call collector insert and get base class ptr - + // C++ function to add pointer from MATLAB to collector. The pointer always // comes from a C++ return value; this mechanism allows the object to be added // to a collector in a different wrap module. If this class has a base class, // a new pointer to the base class is allocated and returned. - wrapperFile.oss << "void " << collectorInsertFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - wrapperFile.oss << "{\n"; - wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\n"; + wrapperFile.oss << "void " << collectorInsertFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "{\n"; + wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\n"; // Typedef boost::shared_ptr - wrapperFile.oss << " typedef boost::shared_ptr<" << cppName << "> Shared;\n"; - wrapperFile.oss << "\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppName << "> Shared;\n"; + wrapperFile.oss << "\n"; // Get self pointer passed in - wrapperFile.oss << " Shared *self = *reinterpret_cast (mxGetData(in[0]));\n"; + wrapperFile.oss + << " Shared *self = *reinterpret_cast (mxGetData(in[0]));\n"; // Add to collector - wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; + wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) - if(!qualifiedParent.empty()) { - wrapperFile.oss << "\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName << "> SharedBase;\n"; - wrapperFile.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; - wrapperFile.oss << " *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self);\n"; - } - wrapperFile.oss << "}\n"; - + if (!qualifiedParent.empty()) { + wrapperFile.oss << "\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName + << "> SharedBase;\n"; + wrapperFile.oss + << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; + wrapperFile.oss + << " *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self);\n"; + } + wrapperFile.oss << "}\n"; + // If this is a virtual function, C++ function to dynamic upcast it from a // shared_ptr. This mechanism allows automatic dynamic creation of the // real underlying derived-most class when a C++ method returns a virtual // base class. - if(isVirtual) - wrapperFile.oss << - "\n" - "void " << upcastFromVoidFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {\n" - " mexAtExit(&_deleteAllObjects);\n" - " typedef boost::shared_ptr<" << cppName << "> Shared;\n" - " boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0]));\n" - " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n" - " Shared *self = new Shared(boost::static_pointer_cast<" << cppName << ">(*asVoid));\n" - " *reinterpret_cast(mxGetData(out[0])) = self;\n" - "}\n"; -} - -/* ************************************************************************* */ -vector expandArgumentListsTemplate(const vector& argLists, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - vector result; - BOOST_FOREACH(const ArgumentList& argList, argLists) { - ArgumentList instArgList; - BOOST_FOREACH(const Argument& arg, argList) { - Argument instArg = arg; - if(arg.type == templateArg) { - instArg.namespaces.assign(instName.begin(), instName.end()-1); - instArg.type = instName.back(); - } else if(arg.type == "This") { - instArg.namespaces.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instArg.type = expandedClassName; - } - instArgList.push_back(instArg); - } - result.push_back(instArgList); - } - return result; -} - -/* ************************************************************************* */ -template -map expandMethodTemplate(const map& methods, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - map result; - typedef pair Name_Method; - BOOST_FOREACH(const Name_Method& name_method, methods) { - const METHOD& method = name_method.second; - METHOD instMethod = method; - instMethod.argLists = expandArgumentListsTemplate(method.argLists, templateArg, instName, expandedClassNamespace, expandedClassName); - instMethod.returnVals.clear(); - BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) { - ReturnValue instRetVal = retVal; - if(retVal.type1 == templateArg) { - instRetVal.namespaces1.assign(instName.begin(), instName.end()-1); - instRetVal.type1 = instName.back(); - } else if(retVal.type1 == "This") { - instRetVal.namespaces1.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instRetVal.type1 = expandedClassName; - } - if(retVal.type2 == templateArg) { - instRetVal.namespaces2.assign(instName.begin(), instName.end()-1); - instRetVal.type2 = instName.back(); - } else if(retVal.type1 == "This") { - instRetVal.namespaces2.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instRetVal.type2 = expandedClassName; - } - instMethod.returnVals.push_back(instRetVal); - } - result.insert(make_pair(name_method.first, instMethod)); - } - return result; -} - -/* ************************************************************************* */ -Class expandClassTemplate(const Class& cls, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - Class inst; - inst.name = cls.name; - inst.templateArgs = cls.templateArgs; - inst.typedefName = cls.typedefName; - inst.isVirtual = cls.isVirtual; + if (isVirtual) + wrapperFile.oss << "\n" + "void " << upcastFromVoidFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {\n" + " mexAtExit(&_deleteAllObjects);\n" + " typedef boost::shared_ptr<" << cppName + << "> Shared;\n" + " boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0]));\n" + " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n" + " Shared *self = new Shared(boost::static_pointer_cast<" << cppName + << ">(*asVoid));\n" + " *reinterpret_cast(mxGetData(out[0])) = self;\n" + "}\n"; +} + +/* ************************************************************************* */ +vector expandArgumentListsTemplate( + const vector& argLists, const string& templateArg, + const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + vector result; + BOOST_FOREACH(const ArgumentList& argList, argLists) { + ArgumentList instArgList; + BOOST_FOREACH(const Argument& arg, argList) { + Argument instArg = arg; + if (arg.type == templateArg) { + instArg.namespaces.assign(instName.begin(), instName.end() - 1); + instArg.type = instName.back(); + } else if (arg.type == "This") { + instArg.namespaces.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instArg.type = expandedClassName; + } + instArgList.push_back(instArg); + } + result.push_back(instArgList); + } + return result; +} + +/* ************************************************************************* */ +template +map expandMethodTemplate(const map& methods, + const string& templateArg, const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + map result; + typedef pair Name_Method; + BOOST_FOREACH(const Name_Method& name_method, methods) { + const METHOD& method = name_method.second; + METHOD instMethod = method; + instMethod.argLists = expandArgumentListsTemplate(method.argLists, + templateArg, instName, expandedClassNamespace, expandedClassName); + instMethod.returnVals.clear(); + BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) { + ReturnValue instRetVal = retVal; + if (retVal.type1 == templateArg) { + instRetVal.namespaces1.assign(instName.begin(), instName.end() - 1); + instRetVal.type1 = instName.back(); + } else if (retVal.type1 == "This") { + instRetVal.namespaces1.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instRetVal.type1 = expandedClassName; + } + if (retVal.type2 == templateArg) { + instRetVal.namespaces2.assign(instName.begin(), instName.end() - 1); + instRetVal.type2 = instName.back(); + } else if (retVal.type1 == "This") { + instRetVal.namespaces2.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instRetVal.type2 = expandedClassName; + } + instMethod.returnVals.push_back(instRetVal); + } + result.insert(make_pair(name_method.first, instMethod)); + } + return result; +} + +/* ************************************************************************* */ +Class expandClassTemplate(const Class& cls, const string& templateArg, + const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + Class inst; + inst.name = cls.name; + inst.templateArgs = cls.templateArgs; + inst.typedefName = cls.typedefName; + inst.isVirtual = cls.isVirtual; inst.isSerializable = cls.isSerializable; - inst.qualifiedParent = cls.qualifiedParent; - inst.methods = expandMethodTemplate(cls.methods, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.namespaces = cls.namespaces; - inst.constructor = cls.constructor; - inst.constructor.args_list = expandArgumentListsTemplate(cls.constructor.args_list, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.constructor.name = inst.name; - inst.deconstructor = cls.deconstructor; - inst.deconstructor.name = inst.name; - inst.verbose_ = cls.verbose_; - return inst; -} - -/* ************************************************************************* */ -vector Class::expandTemplate(const string& templateArg, const vector >& instantiations) const { - vector result; - BOOST_FOREACH(const vector& instName, instantiations) { - const string expandedName = name + instName.back(); - Class inst = expandClassTemplate(*this, templateArg, instName, this->namespaces, expandedName); - inst.name = expandedName; - inst.templateArgs.clear(); - inst.typedefName = qualifiedName("::") + "<" + wrap::qualifiedName("::", instName) + ">"; - result.push_back(inst); - } - return result; -} - -/* ************************************************************************* */ -Class Class::expandTemplate(const string& templateArg, const vector& instantiation, const std::vector& expandedClassNamespace, const string& expandedClassName) const { - return expandClassTemplate(*this, templateArg, instantiation, expandedClassNamespace, expandedClassName); -} - -/* ************************************************************************* */ -std::string Class::getTypedef() const { - string result; - BOOST_FOREACH(const string& namesp, namespaces) { - result += ("namespace " + namesp + " { "); - } - result += ("typedef " + typedefName + " " + name + ";"); - for (size_t i = 0; i Class::expandTemplate(const string& templateArg, + const vector >& instantiations) const { + vector result; + BOOST_FOREACH(const vector& instName, instantiations) { + const string expandedName = name + instName.back(); + Class inst = expandClassTemplate(*this, templateArg, instName, + this->namespaces, expandedName); + inst.name = expandedName; + inst.templateArgs.clear(); + inst.typedefName = qualifiedName("::") + "<" + + wrap::qualifiedName("::", instName) + ">"; + result.push_back(inst); + } + return result; +} + +/* ************************************************************************* */ +Class Class::expandTemplate(const string& templateArg, + const vector& instantiation, + const std::vector& expandedClassNamespace, + const string& expandedClassName) const { + return expandClassTemplate(*this, templateArg, instantiation, + expandedClassNamespace, expandedClassName); +} + +/* ************************************************************************* */ +std::string Class::getTypedef() const { + string result; + BOOST_FOREACH(const string& namesp, namespaces) { + result += ("namespace " + namesp + " { "); + } + result += ("typedef " + typedefName + " " + name + ";"); + for (size_t i = 0; i < namespaces.size(); ++i) { + result += " }"; + } + return result; +} + +/* ************************************************************************* */ void Class::comment_fragment(FileWriter& proxyFile) const { - proxyFile.oss << "%class " << name - << ", see Doxygen page for details\n"; + proxyFile.oss << "%class " << name << ", see Doxygen page for details\n"; proxyFile.oss << "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; if (!constructor.args_list.empty()) proxyFile.oss << "%\n%-------Constructors-------\n"; BOOST_FOREACH(ArgumentList argList, constructor.args_list) { - string up_name = boost::to_upper_copy(name); - proxyFile.oss << "%" << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")\n"; + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, name); + proxyFile.oss << "\n"; } if (!methods.empty()) @@ -357,17 +398,9 @@ void Class::comment_fragment(FileWriter& proxyFile) const { BOOST_FOREACH(const Methods::value_type& name_m, methods) { const Method& m = name_m.second; BOOST_FOREACH(ArgumentList argList, m.argLists) { - string up_name = boost::to_upper_copy(m.name); - proxyFile.oss << "%" << m.name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ") : returns " + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, m.name); + proxyFile.oss << " : returns " << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl; } } @@ -377,18 +410,9 @@ void Class::comment_fragment(FileWriter& proxyFile) const { BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { const StaticMethod& m = name_m.second; BOOST_FOREACH(ArgumentList argList, m.argLists) { - string up_name = boost::to_upper_copy(m.name); - proxyFile.oss << "%" << m.name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - - proxyFile.oss << ") : returns " + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, m.name); + proxyFile.oss << " : returns " << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl; } } @@ -396,15 +420,17 @@ void Class::comment_fragment(FileWriter& proxyFile) const { if (hasSerialization) { proxyFile.oss << "%\n%-------Serialization Interface-------\n"; proxyFile.oss << "%string_serialize() : returns string\n"; - proxyFile.oss << "%string_deserialize(string serialized) : returns " << this->name << "\n"; + proxyFile.oss << "%string_deserialize(string serialized) : returns " + << this->name << "\n"; } proxyFile.oss << "%\n"; } -/* ************************************************************************* */ -void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& wrapperName, std::vector& functionNames) const { +/* ************************************************************************* */ +void Class::serialization_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const std::string& wrapperName, + std::vector& functionNames) const { //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //{ @@ -418,30 +444,34 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi //} int serialize_id = functionNames.size(); - const string - matlabQualName = qualifiedName("."), - matlabUniqueName = qualifiedName(), - cppClassName = qualifiedName("::"); - const string wrapFunctionNameSerialize = matlabUniqueName + "_string_serialize_" + boost::lexical_cast(serialize_id); + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppClassName = qualifiedName("::"); + const string wrapFunctionNameSerialize = matlabUniqueName + + "_string_serialize_" + boost::lexical_cast(serialize_id); functionNames.push_back(wrapFunctionNameSerialize); // call //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) - wrapperFile.oss << "void " << wrapFunctionNameSerialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "void " << wrapFunctionNameSerialize + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; wrapperFile.oss << "{\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; // check arguments - for serialize, no arguments // example: checkArguments("string_serialize",nargout,nargin-1,0); - wrapperFile.oss << " checkArguments(\"string_serialize\",nargout,nargin-1,0);\n"; + wrapperFile.oss + << " checkArguments(\"string_serialize\",nargout,nargin-1,0);\n"; // get class pointer // example: Shared obj = unwrap_shared_ptr(in[0], "ptr_Point3"); - wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; + wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName + << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // Serialization boilerplate wrapperFile.oss << " std::ostringstream out_archive_stream;\n"; - wrapperFile.oss << " boost::archive::text_oarchive out_archive(out_archive_stream);\n"; + wrapperFile.oss + << " boost::archive::text_oarchive out_archive(out_archive_stream);\n"; wrapperFile.oss << " out_archive << *obj;\n"; wrapperFile.oss << " out[0] = wrap< string >(out_archive_stream.str());\n"; @@ -459,13 +489,19 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi // end // end - proxyFile.oss << " function varargout = string_serialize(this, varargin)\n"; - proxyFile.oss << " % STRING_SERIALIZE usage: string_serialize() : returns string\n"; - proxyFile.oss << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; + proxyFile.oss + << " function varargout = string_serialize(this, varargin)\n"; + proxyFile.oss + << " % STRING_SERIALIZE usage: string_serialize() : returns string\n"; + proxyFile.oss + << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; proxyFile.oss << " if length(varargin) == 0\n"; - proxyFile.oss << " varargout{1} = " << wrapperName << "(" << boost::lexical_cast(serialize_id) << ", this, varargin{:});\n"; + proxyFile.oss << " varargout{1} = " << wrapperName << "(" + << boost::lexical_cast(serialize_id) << ", this, varargin{:});\n"; proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << ".string_serialize');\n"; + proxyFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << ".string_serialize');\n"; proxyFile.oss << " end\n"; proxyFile.oss << " end\n\n"; @@ -476,14 +512,16 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi // end proxyFile.oss << " function sobj = saveobj(obj)\n"; - proxyFile.oss << " % SAVEOBJ Saves the object to a matlab-readable format\n"; + proxyFile.oss + << " % SAVEOBJ Saves the object to a matlab-readable format\n"; proxyFile.oss << " sobj = obj.string_serialize();\n"; proxyFile.oss << " end\n"; } /* ************************************************************************* */ -void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& wrapperName, std::vector& functionNames) const { +void Class::deserialization_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const std::string& wrapperName, + std::vector& functionNames) const { //void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //{ // typedef boost::shared_ptr Shared; @@ -495,32 +533,36 @@ void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapper // in_archive >> *output; // out[0] = wrap_shared_ptr(output,"Point3", false); //} - int deserialize_id = functionNames.size(); - const string - matlabQualName = qualifiedName("."), - matlabUniqueName = qualifiedName(), - cppClassName = qualifiedName("::"); - const string wrapFunctionNameDeserialize = matlabUniqueName + "_string_deserialize_" + boost::lexical_cast(deserialize_id); - functionNames.push_back(wrapFunctionNameDeserialize); + int deserialize_id = functionNames.size(); + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppClassName = qualifiedName("::"); + const string wrapFunctionNameDeserialize = matlabUniqueName + + "_string_deserialize_" + boost::lexical_cast(deserialize_id); + functionNames.push_back(wrapFunctionNameDeserialize); - // call - wrapperFile.oss << "void " << wrapFunctionNameDeserialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - wrapperFile.oss << "{\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + // call + wrapperFile.oss << "void " << wrapFunctionNameDeserialize + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "{\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; - // check arguments - for deserialize, 1 string argument - wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << ".string_deserialize\",nargout,nargin,1);\n"; + // check arguments - for deserialize, 1 string argument + wrapperFile.oss << " checkArguments(\"" << matlabUniqueName + << ".string_deserialize\",nargout,nargin,1);\n"; - // string argument with deserialization boilerplate - wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n"; - wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n"; - wrapperFile.oss << " boost::archive::text_iarchive in_archive(in_archive_stream);\n"; - wrapperFile.oss << " Shared output(new " << cppClassName << "());\n"; - wrapperFile.oss << " in_archive >> *output;\n"; - wrapperFile.oss << " out[0] = wrap_shared_ptr(output,\"" << matlabQualName << "\", false);\n"; - wrapperFile.oss << "}\n"; + // string argument with deserialization boilerplate + wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n"; + wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n"; + wrapperFile.oss + << " boost::archive::text_iarchive in_archive(in_archive_stream);\n"; + wrapperFile.oss << " Shared output(new " << cppClassName << "());\n"; + wrapperFile.oss << " in_archive >> *output;\n"; + wrapperFile.oss << " out[0] = wrap_shared_ptr(output,\"" << matlabQualName + << "\", false);\n"; + wrapperFile.oss << "}\n"; - // Generate matlab function + // Generate matlab function // function varargout = string_deserialize(varargin) // % STRING_DESERIALIZE usage: string_deserialize() : returns Point3 // % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html @@ -531,32 +573,40 @@ void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapper // end // end - proxyFile.oss << " function varargout = string_deserialize(varargin)\n"; - proxyFile.oss << " % STRING_DESERIALIZE usage: string_deserialize() : returns " << matlabQualName << "\n"; - proxyFile.oss << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; - proxyFile.oss << " if length(varargin) == 1\n"; - proxyFile.oss << " varargout{1} = " << wrapperName << "(" << boost::lexical_cast(deserialize_id) << ", varargin{:});\n"; - proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << ".string_deserialize');\n"; - proxyFile.oss << " end\n"; - proxyFile.oss << " end\n\n"; + proxyFile.oss << " function varargout = string_deserialize(varargin)\n"; + proxyFile.oss + << " % STRING_DESERIALIZE usage: string_deserialize() : returns " + << matlabQualName << "\n"; + proxyFile.oss + << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; + proxyFile.oss << " if length(varargin) == 1\n"; + proxyFile.oss << " varargout{1} = " << wrapperName << "(" + << boost::lexical_cast(deserialize_id) << ", varargin{:});\n"; + proxyFile.oss << " else\n"; + proxyFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << ".string_deserialize');\n"; + proxyFile.oss << " end\n"; + proxyFile.oss << " end\n\n"; - - // Generate matlab load function + // Generate matlab load function // function obj = loadobj(sobj) // % LOADOBJ Saves the object to a matlab-readable format // obj = Point3.string_deserialize(sobj); // end - proxyFile.oss << " function obj = loadobj(sobj)\n"; - proxyFile.oss << " % LOADOBJ Saves the object to a matlab-readable format\n"; - proxyFile.oss << " obj = " << matlabQualName << ".string_deserialize(sobj);\n"; - proxyFile.oss << " end" << endl; + proxyFile.oss << " function obj = loadobj(sobj)\n"; + proxyFile.oss + << " % LOADOBJ Saves the object to a matlab-readable format\n"; + proxyFile.oss << " obj = " << matlabQualName + << ".string_deserialize(sobj);\n"; + proxyFile.oss << " end" << endl; } /* ************************************************************************* */ std::string Class::getSerializationExport() const { //BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal"); - return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + qualifiedName() + "\");"; + return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + + qualifiedName() + "\");"; } /* ************************************************************************* */ diff --git a/wrap/Class.h b/wrap/Class.h index f5267cee2..2ca976f66 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -27,7 +27,6 @@ #include "Method.h" #include "StaticMethod.h" #include "TypeAttributesTable.h" -#include namespace wrap { diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 96d1030a1..5438c515c 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -18,11 +18,11 @@ #pragma once +#include "Argument.h" + #include #include -#include "Argument.h" - namespace wrap { // Constructor class @@ -34,7 +34,7 @@ struct Constructor { } // Then the instance variables are set directly by the Module constructor - std::vector args_list; + std::vector args_list; std::string name; bool verbose_; @@ -50,21 +50,18 @@ struct Constructor { * if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end */ void proxy_fragment(FileWriter& file, const std::string& wrapperName, - bool hasParent, const int id, const ArgumentList args) const; + bool hasParent, const int id, const ArgumentList args) const; /// cpp wrapper std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, - const std::string& matlabUniqueName, - const std::string& cppBaseClassName, - int id, - const ArgumentList& al) const; + const std::string& cppClassName, const std::string& matlabUniqueName, + const std::string& cppBaseClassName, int id, + const ArgumentList& al) const; /// constructor function void generate_construct(FileWriter& file, const std::string& cppClassName, - std::vector& args_list) const; - + std::vector& args_list) const; + }; - } // \namespace wrap diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 16f6d48f1..25e1dcedb 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -17,7 +17,8 @@ using namespace std; /* ************************************************************************* */ void GlobalFunction::addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal, const StrVec& ns_stack) { + const ArgumentList& args, const ReturnValue& retVal, + const StrVec& ns_stack) { this->verbose_ = verbose; this->name = name; this->argLists.push_back(args); @@ -26,16 +27,16 @@ void GlobalFunction::addOverload(bool verbose, const std::string& name, } /* ************************************************************************* */ -void GlobalFunction::matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const { +void GlobalFunction::matlab_proxy(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const { // cluster overloads with same namespace // create new GlobalFunction structures around namespaces - same namespaces and names are overloads // map of namespace to global function typedef map GlobalFunctionMap; GlobalFunctionMap grouped_functions; - for (size_t i=0; i& functionNames) const { +void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const { // create the folder for the namespace const StrVec& ns = namespaces.front(); @@ -68,84 +70,68 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons // open destination mfunctionFileName string mfunctionFileName = toolboxPath; - if(!ns.empty()) + if (!ns.empty()) mfunctionFileName += "/+" + wrap::qualifiedName("/+", ns); mfunctionFileName += "/" + name + ".m"; FileWriter mfunctionFile(mfunctionFileName, verbose_, "%"); // get the name of actual matlab object - const string - matlabQualName = qualifiedName(".", ns, name), - matlabUniqueName = qualifiedName("", ns, name), - cppName = qualifiedName("::", ns, name); + const string matlabQualName = qualifiedName(".", ns, name), matlabUniqueName = + qualifiedName("", ns, name), cppName = qualifiedName("::", ns, name); mfunctionFile.oss << "function varargout = " << name << "(varargin)\n"; - for(size_t overload = 0; overload < argLists.size(); ++overload) { + for (size_t overload = 0; overload < argLists.size(); ++overload) { const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); const int id = functionNames.size(); // Output proxy matlab code - - // check for number of arguments... - mfunctionFile.oss << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) mfunctionFile.oss << " && "; - // ...and their types - bool first = true; - for(size_t i=0;i(id); + const string wrapFunctionName = matlabUniqueName + "_" + + boost::lexical_cast(id); // call - wrapperFile.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start - wrapperFile.oss << "{\n"; + file.oss << "{\n"; - returnVal.wrapTypeUnwrap(wrapperFile); + returnVal.wrapTypeUnwrap(file); // check arguments // NOTE: for static functions, there is no object passed - wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << "\",nargout,nargin," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << matlabUniqueName + << "\",nargout,nargin," << args.size() << ");\n"; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(wrapperFile,0); // We start at 0 because there is no self object + args.matlab_unwrap(file, 0); // We start at 0 because there is no self object // call method with default type and wrap result - if (returnVal.type1!="void") - returnVal.wrap_result(cppName+"("+args.names()+")", wrapperFile, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result(cppName + "(" + args.names() + ")", file, + typeAttributes); else - wrapperFile.oss << cppName+"("+args.names()+");\n"; + file.oss << cppName + "(" + args.names() + ");\n"; // finish - wrapperFile.oss << "}\n"; + file.oss << "}\n"; // Add to function list functionNames.push_back(wrapFunctionName); } - mfunctionFile.oss << "else\n"; - mfunctionFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << "');" << endl; - mfunctionFile.oss << "end" << endl; + mfunctionFile.oss << " else\n"; + mfunctionFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << "');" << endl; + mfunctionFile.oss << " end" << endl; // Close file mfunctionFile.emit(true); @@ -153,9 +139,5 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons /* ************************************************************************* */ - } // \namespace wrap - - - diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 2ecaf4998..6c8ad0c86 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -22,37 +22,38 @@ struct GlobalFunction { std::string name; // each overload, regardless of namespace - std::vector argLists; ///< arugments for each overload - std::vector returnVals; ///< returnVals for each overload - std::vector namespaces; ///< Stack of namespaces + std::vector argLists; ///< arugments for each overload + std::vector returnVals; ///< returnVals for each overload + std::vector namespaces; ///< Stack of namespaces // Constructor only used in Module - GlobalFunction(bool verbose = true) : verbose_(verbose) {} + GlobalFunction(bool verbose = true) : + verbose_(verbose) { + } // Used to reconstruct - GlobalFunction(const std::string& name_, bool verbose = true) - : verbose_(verbose), name(name_) {} + GlobalFunction(const std::string& name_, bool verbose = true) : + verbose_(verbose), name(name_) { + } // adds an overloaded version of this function void addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal, const StrVec& ns_stack); + const ArgumentList& args, const ReturnValue& retVal, + const StrVec& ns_stack); // codegen function called from Module to build the cpp and matlab versions of the function - void matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const; + void matlab_proxy(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const; private: // Creates a single global function - all in same namespace - void generateSingleFunction(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const; + void generateSingleFunction(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const; }; } // \namespace wrap - - - diff --git a/wrap/Method.cpp b/wrap/Method.cpp index b327ac7dc..7b88b9cdc 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -15,14 +15,15 @@ * @author Richard Roberts **/ -#include -#include +#include "Method.h" +#include "utilities.h" #include #include +#include -#include "Method.h" -#include "utilities.h" +#include +#include using namespace std; using namespace wrap; @@ -38,155 +39,140 @@ void Method::addOverload(bool verbose, bool is_const, const std::string& name, } /* ************************************************************************* */ -void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const string& cppClassName, - const std::string& matlabQualName, - const std::string& matlabUniqueName, - const string& wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { +void Method::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile, + const string& cppClassName, const std::string& matlabQualName, + const std::string& matlabUniqueName, const string& wrapperName, + const TypeAttributesTable& typeAttributes, + vector& functionNames) const { - proxyFile.oss << " function varargout = " << name << "(this, varargin)\n"; - //Comments for documentation - string up_name = boost::to_upper_copy(name); - proxyFile.oss << " % " << up_name << " usage:"; + // Create function header + file.oss << " function varargout = " << name << "(this, varargin)\n"; + + // Emit comments for documentation + string up_name = boost::to_upper_copy(name); + file.oss << " % " << up_name << " usage: "; unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - if(argLCount != argLists.size()-1) - proxyFile.oss << "), "; - else - proxyFile.oss << ") : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; - argLCount++; - } - - proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; - proxyFile.oss << " % " << "" << endl; - proxyFile.oss << " % " << "Method Overloads" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " % " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")" << endl; - } - - for(size_t overload = 0; overload < argLists.size(); ++overload) { - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); - - const int id = functionNames.size(); - - // Output proxy matlab code - - // check for number of arguments... - proxyFile.oss << " " << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) proxyFile.oss << " && "; - // ...and their types - bool first = true; - for(size_t i=0;i 1) { + file.oss << " % " << "" << endl; + file.oss << " % " << "Method Overloads" << endl; + BOOST_FOREACH(ArgumentList argList, argLists) { + file.oss << " % "; + argList.emit_prototype(file, name); + file.oss << endl; + } + } + + // Handle special case of single overload with all numeric arguments + if (argLists.size() == 1 && argLists[0].allScalar()) { + // Output proxy matlab code + file.oss << " "; + const int id = (int) functionNames.size(); + argLists[0].emit_call(file, returnVals[0], wrapperName, id); // Output C++ wrapper code - - const string wrapFunctionName = wrapper_fragment( - wrapperFile, cppClassName, matlabUniqueName, overload, id, typeAttributes); + const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, + matlabUniqueName, 0, id, typeAttributes); // Add to function list functionNames.push_back(wrapFunctionName); + } else { + // Check arguments for all overloads + for (size_t overload = 0; overload < argLists.size(); ++overload) { + // Output proxy matlab code + file.oss << " " << (overload == 0 ? "" : "else"); + const int id = (int) functionNames.size(); + argLists[overload].emit_conditional_call(file, returnVals[overload], + wrapperName, id); + + // Output C++ wrapper code + const string wrapFunctionName = wrapper_fragment(wrapperFile, + cppClassName, matlabUniqueName, overload, id, typeAttributes); + + // Add to function list + functionNames.push_back(wrapFunctionName); + } + file.oss << " else\n"; + file.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << "." << name << "');" << endl; + file.oss << " end\n"; } - - proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of function " << - matlabQualName << "." << name << "');" << endl; - proxyFile.oss << " end\n"; - proxyFile.oss << " end\n"; + file.oss << " end\n"; } /* ************************************************************************* */ -string Method::wrapper_fragment(FileWriter& file, - const string& cppClassName, - const string& matlabUniqueName, - int overload, - int id, - const TypeAttributesTable& typeAttributes) const { +string Method::wrapper_fragment(FileWriter& file, const string& cppClassName, + const string& matlabUniqueName, int overload, int id, + const TypeAttributesTable& typeAttributes) const { // generate code - const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + boost::lexical_cast(id); + const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + + boost::lexical_cast(id); const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; // call - file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start file.oss << "{\n"; - if(returnVal.isPair) - { - if(returnVal.category1 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; - if(returnVal.category2 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 << ";"<< endl; - } - else - if(returnVal.category1 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; + if (returnVal.isPair) { + if (returnVal.category1 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" + << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 + << ";" << endl; + if (returnVal.category2 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" + << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 + << ";" << endl; + } else if (returnVal.category1 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") + << "> Shared" << returnVal.type1 << ";" << endl; - file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" + << endl; // check arguments // extra argument obj -> nargin-1 is passed ! // example: checkArguments("equals",nargout,nargin-1,2); - file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1," + << args.size() << ");\n"; // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); - file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; + file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName + << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(file,1); + args.matlab_unwrap(file, 1); // call method and wrap result // example: out[0]=wrap(self->return_field(t)); - if (returnVal.type1!="void") - returnVal.wrap_result("obj->"+name+"("+args.names()+")", file, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result("obj->" + name + "(" + args.names() + ")", file, + typeAttributes); else - file.oss << " obj->"+name+"("+args.names()+");\n"; + file.oss << " obj->" + name + "(" + args.names() + ");\n"; // finish file.oss << "}\n"; diff --git a/wrap/Method.h b/wrap/Method.h index f2a7ed190..9926a5179 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -18,13 +18,12 @@ #pragma once -#include -#include - #include "Argument.h" #include "ReturnValue.h" #include "TypeAttributesTable.h" -#include + +#include +#include namespace wrap { diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 6870d5178..2cb5ea7ed 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -98,7 +98,7 @@ void Module::parseMarkup(const std::string& data) { // The one with postfix 0 are used to reset the variables after parse. string methodName, methodName0; bool isConst, isConst0 = false; - ReturnValue retVal0(verbose), retVal(verbose); + ReturnValue retVal0, retVal; Argument arg0, arg; ArgumentList args0, args; vector arg_dup; ///keep track of duplicates diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 3cb68181b..78e36d4da 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -141,5 +141,13 @@ void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const { } } /* ************************************************************************* */ +void ReturnValue::emit_matlab(FileWriter& file) const { + string output; + if (isPair) + file.oss << "[ varargout{1} varargout{2} ] = "; + else if (category1 != ReturnValue::VOID) + file.oss << "varargout{1} = "; +} +/* ************************************************************************* */ diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 7a677532f..989f81109 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -8,36 +8,36 @@ * @author Richard Roberts */ -#include -#include - #include "FileWriter.h" #include "TypeAttributesTable.h" +#include + #pragma once namespace wrap { +/** + * Encapsulates return value of a method or function + */ struct ReturnValue { + /// the different supported return value categories typedef enum { - CLASS = 1, - EIGEN = 2, - BASIS = 3, - VOID = 4 + CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 } return_category; - ReturnValue(bool enable_verbosity = true) - : verbose(enable_verbosity), isPtr1(false), isPtr2(false), - isPair(false), category1(CLASS), category2(CLASS) - {} - - bool verbose; bool isPtr1, isPtr2, isPair; return_category category1, category2; std::string type1, type2; std::vector namespaces1, namespaces2; + /// Constructor + ReturnValue() : + isPtr1(false), isPtr2(false), isPair(false), category1(CLASS), category2( + CLASS) { + } + typedef enum { arg1, arg2, pair } pairing; @@ -49,10 +49,12 @@ struct ReturnValue { std::string matlab_returnType() const; - void wrap_result(const std::string& result, FileWriter& file, const TypeAttributesTable& typeAttributes) const; + void wrap_result(const std::string& result, FileWriter& file, + const TypeAttributesTable& typeAttributes) const; void wrapTypeUnwrap(FileWriter& wrapperFile) const; + void emit_matlab(FileWriter& file) const; }; } // \namespace wrap diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index e5c10b4c8..0c4cc7d75 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -16,19 +16,19 @@ * @author Richard Roberts **/ -#include -#include - -#include -#include - #include "StaticMethod.h" #include "utilities.h" +#include +#include +#include + +#include +#include + using namespace std; using namespace wrap; - /* ************************************************************************* */ void StaticMethod::addOverload(bool verbose, const std::string& name, const ArgumentList& args, const ReturnValue& retVal) { @@ -39,144 +39,103 @@ void StaticMethod::addOverload(bool verbose, const std::string& name, } /* ************************************************************************* */ -void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const string& cppClassName, - const std::string& matlabQualName, - const std::string& matlabUniqueName, - const string& wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { +void StaticMethod::proxy_wrapper_fragments(FileWriter& file, + FileWriter& wrapperFile, const string& cppClassName, + const std::string& matlabQualName, const std::string& matlabUniqueName, + const string& wrapperName, const TypeAttributesTable& typeAttributes, + vector& functionNames) const { - string upperName = name; upperName[0] = std::toupper(upperName[0], std::locale()); + string upperName = name; + upperName[0] = std::toupper(upperName[0], std::locale()); - proxyFile.oss << " function varargout = " << upperName << "(varargin)\n"; + file.oss << " function varargout = " << upperName << "(varargin)\n"; //Comments for documentation - string up_name = boost::to_upper_copy(name); - proxyFile.oss << " % " << up_name << " usage:"; + string up_name = boost::to_upper_copy(name); + file.oss << " % " << up_name << " usage: "; unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - if(argLCount != argLists.size()-1) - proxyFile.oss << "), "; - else - proxyFile.oss << ") : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; - argLCount++; - } + BOOST_FOREACH(ArgumentList argList, argLists) { + argList.emit_prototype(file, name); + if (argLCount != argLists.size() - 1) + file.oss << ", "; + else + file.oss << " : returns " + << returnVals[0].return_type(false, returnVals[0].pair) << endl; + argLCount++; + } - proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; - proxyFile.oss << " % " << "" << endl; - proxyFile.oss << " % " << "Usage" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " % " << up_name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")" << endl; - } + file.oss << " % " + << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" + << endl; + file.oss << " % " << "" << endl; + file.oss << " % " << "Usage" << endl; + BOOST_FOREACH(ArgumentList argList, argLists) { + file.oss << " % "; + argList.emit_prototype(file, up_name); + file.oss << endl; + } - - for(size_t overload = 0; overload < argLists.size(); ++overload) { - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); - - const int id = (int)functionNames.size(); + // Check arguments for all overloads + for (size_t overload = 0; overload < argLists.size(); ++overload) { // Output proxy matlab code - - // check for number of arguments... - proxyFile.oss << " " << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) proxyFile.oss << " && "; - // ...and their types - bool first = true; - for(size_t i=0;i(id); + const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + + boost::lexical_cast(id); const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; // call - file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start file.oss << "{\n"; returnVal.wrapTypeUnwrap(file); - file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" + << endl; // check arguments // NOTE: for static functions, there is no object passed - file.oss << " checkArguments(\"" << matlabUniqueName << "." << name << "\",nargout,nargin," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << matlabUniqueName << "." << name + << "\",nargout,nargin," << args.size() << ");\n"; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(file,0); // We start at 0 because there is no self object + args.matlab_unwrap(file, 0); // We start at 0 because there is no self object // call method with default type and wrap result - if (returnVal.type1!="void") - returnVal.wrap_result(cppClassName+"::"+name+"("+args.names()+")", file, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result(cppClassName + "::" + name + "(" + args.names() + ")", + file, typeAttributes); else - file.oss << cppClassName+"::"+name+"("+args.names()+");\n"; + file.oss << cppClassName + "::" + name + "(" + args.names() + ");\n"; // finish file.oss << "}\n"; diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 3e8dc08cf..e1855f4c2 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -19,13 +19,9 @@ #pragma once -#include -#include - #include "Argument.h" #include "ReturnValue.h" #include "TypeAttributesTable.h" -#include namespace wrap { @@ -34,7 +30,8 @@ struct StaticMethod { /// Constructor creates empty object StaticMethod(bool verbosity = true) : - verbose(verbosity) {} + verbose(verbosity) { + } // Then the instance variables are set directly by the Module constructor bool verbose; @@ -46,22 +43,20 @@ struct StaticMethod { // with those in rhs, but in subsequent calls it adds additional argument // lists as function overloads. void addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal); + const ArgumentList& args, const ReturnValue& retVal); // MATLAB code generation // classPath is class directory, e.g., ../matlab/@Point2 void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName, - const std::string& wrapperName, const TypeAttributesTable& typeAttributes, - std::vector& functionNames) const; + const std::string& cppClassName, const std::string& matlabQualName, + const std::string& matlabUniqueName, const std::string& wrapperName, + const TypeAttributesTable& typeAttributes, + std::vector& functionNames) const; private: std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, - const std::string& matlabUniqueName, - int overload, - int id, - const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper + const std::string& cppClassName, const std::string& matlabUniqueName, + int overload, int id, const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper }; } // \namespace wrap diff --git a/wrap/tests/expected/.gitignore b/wrap/tests/expected/.gitignore new file mode 100644 index 000000000..6d725d9bc --- /dev/null +++ b/wrap/tests/expected/.gitignore @@ -0,0 +1 @@ +*.m~ diff --git a/wrap/tests/expected/Point2.m b/wrap/tests/expected/Point2.m index 22dec9641..9f64f2d10 100644 --- a/wrap/tests/expected/Point2.m +++ b/wrap/tests/expected/Point2.m @@ -44,92 +44,43 @@ classdef Point2 < handle function varargout = argChar(this, varargin) % ARGCHAR usage: argChar(char a) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % argChar(char a) - if length(varargin) == 1 && isa(varargin{1},'char') - geometry_wrapper(4, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.argChar'); - end + geometry_wrapper(4, this, varargin{:}); end function varargout = argUChar(this, varargin) % ARGUCHAR usage: argUChar(unsigned char a) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % argUChar(unsigned char a) - if length(varargin) == 1 && isa(varargin{1},'char') - geometry_wrapper(5, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.argUChar'); - end + geometry_wrapper(5, this, varargin{:}); end function varargout = dim(this, varargin) % DIM usage: dim() : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % dim() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(6, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.dim'); - end + varargout{1} = geometry_wrapper(6, this, varargin{:}); end function varargout = returnChar(this, varargin) % RETURNCHAR usage: returnChar() : returns char % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % returnChar() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(7, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.returnChar'); - end + varargout{1} = geometry_wrapper(7, this, varargin{:}); end function varargout = vectorConfusion(this, varargin) % VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % vectorConfusion() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(8, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.vectorConfusion'); - end + varargout{1} = geometry_wrapper(8, this, varargin{:}); end function varargout = x(this, varargin) % X usage: x() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % x() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(9, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.x'); - end + varargout{1} = geometry_wrapper(9, this, varargin{:}); end function varargout = y(this, varargin) % Y usage: y() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % y() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(10, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.y'); - end + varargout{1} = geometry_wrapper(10, this, varargin{:}); end end diff --git a/wrap/tests/expected/Point3.m b/wrap/tests/expected/Point3.m index 94d9c25b8..8a43987b9 100644 --- a/wrap/tests/expected/Point3.m +++ b/wrap/tests/expected/Point3.m @@ -43,14 +43,7 @@ classdef Point3 < handle function varargout = norm(this, varargin) % NORM usage: norm() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % norm() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(14, this, varargin{:}); - else - error('Arguments do not match any overload of function Point3.norm'); - end + varargout{1} = geometry_wrapper(14, this, varargin{:}); end function varargout = string_serialize(this, varargin) diff --git a/wrap/tests/expected/Test.m b/wrap/tests/expected/Test.m index 8e56df6fc..1afd15efe 100644 --- a/wrap/tests/expected/Test.m +++ b/wrap/tests/expected/Test.m @@ -56,9 +56,6 @@ classdef Test < handle function varargout = arg_EigenConstRef(this, varargin) % ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % arg_EigenConstRef(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') geometry_wrapper(23, this, varargin{:}); else @@ -69,61 +66,30 @@ classdef Test < handle function varargout = create_MixedPtrs(this, varargin) % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % create_MixedPtrs() - if length(varargin) == 0 - [ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.create_MixedPtrs'); - end + [ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:}); end function varargout = create_ptrs(this, varargin) % CREATE_PTRS usage: create_ptrs() : returns pair< SharedTest, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % create_ptrs() - if length(varargin) == 0 - [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.create_ptrs'); - end + [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:}); end function varargout = print(this, varargin) % PRINT usage: print() : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % print() - if length(varargin) == 0 - geometry_wrapper(26, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.print'); - end + geometry_wrapper(26, this, varargin{:}); end function varargout = return_Point2Ptr(this, varargin) % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2 % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_Point2Ptr(bool value) - if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = geometry_wrapper(27, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_Point2Ptr'); - end + varargout{1} = geometry_wrapper(27, this, varargin{:}); end function varargout = return_Test(this, varargin) % RETURN_TEST usage: return_Test(Test value) : returns Test % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_Test(Test value) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(28, this, varargin{:}); else @@ -134,9 +100,6 @@ classdef Test < handle function varargout = return_TestPtr(this, varargin) % RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_TestPtr(Test value) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(29, this, varargin{:}); else @@ -147,35 +110,18 @@ classdef Test < handle function varargout = return_bool(this, varargin) % RETURN_BOOL usage: return_bool(bool value) : returns bool % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_bool(bool value) - if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = geometry_wrapper(30, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_bool'); - end + varargout{1} = geometry_wrapper(30, this, varargin{:}); end function varargout = return_double(this, varargin) % RETURN_DOUBLE usage: return_double(double value) : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_double(double value) - if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = geometry_wrapper(31, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_double'); - end + varargout{1} = geometry_wrapper(31, this, varargin{:}); end function varargout = return_field(this, varargin) % RETURN_FIELD usage: return_field(Test t) : returns bool % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_field(Test t) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(32, this, varargin{:}); else @@ -186,22 +132,12 @@ classdef Test < handle function varargout = return_int(this, varargin) % RETURN_INT usage: return_int(int value) : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_int(int value) - if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = geometry_wrapper(33, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_int'); - end + varargout{1} = geometry_wrapper(33, this, varargin{:}); end function varargout = return_matrix1(this, varargin) % RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_matrix1(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(34, this, varargin{:}); else @@ -212,9 +148,6 @@ classdef Test < handle function varargout = return_matrix2(this, varargin) % RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_matrix2(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(35, this, varargin{:}); else @@ -225,9 +158,6 @@ classdef Test < handle function varargout = return_pair(this, varargin) % RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_pair(Vector v, Matrix A) if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double') [ varargout{1} varargout{2} ] = geometry_wrapper(36, this, varargin{:}); else @@ -238,9 +168,6 @@ classdef Test < handle function varargout = return_ptrs(this, varargin) % RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< SharedTest, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_ptrs(Test p1, Test p2) if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test') [ varargout{1} varargout{2} ] = geometry_wrapper(37, this, varargin{:}); else @@ -251,22 +178,12 @@ classdef Test < handle function varargout = return_size_t(this, varargin) % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_size_t(size_t value) - if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = geometry_wrapper(38, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_size_t'); - end + varargout{1} = geometry_wrapper(38, this, varargin{:}); end function varargout = return_string(this, varargin) % RETURN_STRING usage: return_string(string value) : returns string % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_string(string value) if length(varargin) == 1 && isa(varargin{1},'char') varargout{1} = geometry_wrapper(39, this, varargin{:}); else @@ -277,9 +194,6 @@ classdef Test < handle function varargout = return_vector1(this, varargin) % RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_vector1(Vector value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(40, this, varargin{:}); else @@ -290,9 +204,6 @@ classdef Test < handle function varargout = return_vector2(this, varargin) % RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_vector2(Vector value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(41, this, varargin{:}); else diff --git a/wrap/tests/expected/aGlobalFunction.m b/wrap/tests/expected/aGlobalFunction.m index 09ece0b83..94e9b4a67 100644 --- a/wrap/tests/expected/aGlobalFunction.m +++ b/wrap/tests/expected/aGlobalFunction.m @@ -1,6 +1,6 @@ function varargout = aGlobalFunction(varargin) -if length(varargin) == 0 - varargout{1} = geometry_wrapper(42, varargin{:}); -else - error('Arguments do not match any overload of function aGlobalFunction'); -end + if length(varargin) == 0 + varargout{1} = geometry_wrapper(42, varargin{:}); + else + error('Arguments do not match any overload of function aGlobalFunction'); + end diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index e00004d57..b34112718 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -502,6 +502,19 @@ void aGlobalFunction_42(int nargout, mxArray *out[], int nargin, const mxArray * checkArguments("aGlobalFunction",nargout,nargin,0); out[0] = wrap< Vector >(aGlobalFunction()); } +void overloadedGlobalFunction_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("overloadedGlobalFunction",nargout,nargin,1); + int a = unwrap< int >(in[0]); + out[0] = wrap< Vector >(overloadedGlobalFunction(a)); +} +void overloadedGlobalFunction_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("overloadedGlobalFunction",nargout,nargin,2); + int a = unwrap< int >(in[0]); + double b = unwrap< double >(in[1]); + out[0] = wrap< Vector >(overloadedGlobalFunction(a,b)); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { @@ -643,6 +656,12 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) case 42: aGlobalFunction_42(nargout, out, nargin-1, in+1); break; + case 43: + overloadedGlobalFunction_43(nargout, out, nargin-1, in+1); + break; + case 44: + overloadedGlobalFunction_44(nargout, out, nargin-1, in+1); + break; } } catch(const std::exception& e) { mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); diff --git a/wrap/tests/expected/overloadedGlobalFunction.m b/wrap/tests/expected/overloadedGlobalFunction.m new file mode 100644 index 000000000..5b086b15e --- /dev/null +++ b/wrap/tests/expected/overloadedGlobalFunction.m @@ -0,0 +1,8 @@ +function varargout = overloadedGlobalFunction(varargin) + if length(varargin) == 1 && isa(varargin{1},'numeric') + varargout{1} = geometry_wrapper(43, varargin{:}); + elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double') + varargout{1} = geometry_wrapper(44, varargin{:}); + else + error('Arguments do not match any overload of function overloadedGlobalFunction'); + end diff --git a/wrap/tests/expected_namespaces/+ns2/ClassA.m b/wrap/tests/expected_namespaces/+ns2/ClassA.m index c7ab901ae..9c064734e 100644 --- a/wrap/tests/expected_namespaces/+ns2/ClassA.m +++ b/wrap/tests/expected_namespaces/+ns2/ClassA.m @@ -40,22 +40,12 @@ classdef ClassA < handle function varargout = memberFunction(this, varargin) % MEMBERFUNCTION usage: memberFunction() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % memberFunction() - if length(varargin) == 0 - varargout{1} = testNamespaces_wrapper(9, this, varargin{:}); - else - error('Arguments do not match any overload of function ns2.ClassA.memberFunction'); - end + varargout{1} = testNamespaces_wrapper(9, this, varargin{:}); end function varargout = nsArg(this, varargin) % NSARG usage: nsArg(ClassB arg) : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % nsArg(ClassB arg) if length(varargin) == 1 && isa(varargin{1},'ns1.ClassB') varargout{1} = testNamespaces_wrapper(10, this, varargin{:}); else @@ -66,14 +56,7 @@ classdef ClassA < handle function varargout = nsReturn(this, varargin) % NSRETURN usage: nsReturn(double q) : returns ns2::ns3::ClassB % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % nsReturn(double q) - if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = testNamespaces_wrapper(11, this, varargin{:}); - else - error('Arguments do not match any overload of function ns2.ClassA.nsReturn'); - end + varargout{1} = testNamespaces_wrapper(11, this, varargin{:}); end end diff --git a/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m b/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m new file mode 100644 index 000000000..84f3b8f47 --- /dev/null +++ b/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m @@ -0,0 +1,8 @@ +function varargout = overloadedGlobalFunction(varargin) + if length(varargin) == 1 && isa(varargin{1},'ns1.ClassA') + varargout{1} = testNamespaces_wrapper(24, varargin{:}); + elseif length(varargin) == 2 && isa(varargin{1},'ns1.ClassA') && isa(varargin{2},'double') + varargout{1} = testNamespaces_wrapper(25, varargin{:}); + else + error('Arguments do not match any overload of function ns2.overloadedGlobalFunction'); + end diff --git a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp index 29739a2f6..6d22e1625 100644 --- a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp +++ b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp @@ -342,6 +342,21 @@ void ns2aGlobalFunction_23(int nargout, mxArray *out[], int nargin, const mxArra checkArguments("ns2aGlobalFunction",nargout,nargin,0); out[0] = wrap< Vector >(ns2::aGlobalFunction()); } +void ns2overloadedGlobalFunction_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedClassA; + checkArguments("ns2overloadedGlobalFunction",nargout,nargin,1); + ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA"); + out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a))),"ns1.ClassA", false); +} +void ns2overloadedGlobalFunction_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedClassA; + checkArguments("ns2overloadedGlobalFunction",nargout,nargin,2); + ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA"); + double b = unwrap< double >(in[1]); + out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a,b))),"ns1.ClassA", false); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { @@ -426,6 +441,12 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) case 23: ns2aGlobalFunction_23(nargout, out, nargin-1, in+1); break; + case 24: + ns2overloadedGlobalFunction_24(nargout, out, nargin-1, in+1); + break; + case 25: + ns2overloadedGlobalFunction_25(nargout, out, nargin-1, in+1); + break; } } catch(const std::exception& e) { mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index bdced45ed..bc233763d 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -87,6 +87,10 @@ class Test { Vector aGlobalFunction(); +// An overloaded global function +Vector overloadedGlobalFunction(int a); +Vector overloadedGlobalFunction(int a, double b); + // comments at the end! // even more comments at the end! diff --git a/wrap/tests/testNamespaces.h b/wrap/tests/testNamespaces.h index 8e6ef51d6..a9b23cad1 100644 --- a/wrap/tests/testNamespaces.h +++ b/wrap/tests/testNamespaces.h @@ -47,6 +47,10 @@ class ClassC { // separate namespace global function, same name Vector aGlobalFunction(); +// An overloaded global function +ns1::ClassA overloadedGlobalFunction(const ns1::ClassA& a); +ns1::ClassA overloadedGlobalFunction(const ns1::ClassA& a, double b); + } //\namespace ns2 class ClassD { diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 8bf2c1412..c6ce0903a 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -15,16 +15,18 @@ * @author Frank Dellaert **/ +#include +#include + +#include + +#include +#include + #include #include #include #include -#include -#include -#include - -#include -#include using namespace std; using namespace boost::assign; @@ -305,8 +307,8 @@ TEST( wrap, parse_geometry ) { } // evaluate global functions -// Vector aGlobalFunction(); - LONGS_EQUAL(1, module.global_functions.size()); + // Vector aGlobalFunction(); + LONGS_EQUAL(2, module.global_functions.size()); CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end()); { GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); @@ -380,7 +382,7 @@ TEST( wrap, parse_namespaces ) { // evaluate global functions // Vector ns1::aGlobalFunction(); // Vector ns2::aGlobalFunction(); - LONGS_EQUAL(1, module.global_functions.size()); + LONGS_EQUAL(2, module.global_functions.size()); CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end()); { GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); @@ -415,13 +417,17 @@ TEST( wrap, matlab_code_namespaces ) { module.matlab_code("actual_namespaces", headerPath); - EXPECT(files_equal(exp_path + "ClassD.m" , act_path + "ClassD.m" )); - EXPECT(files_equal(exp_path + "+ns1/ClassA.m" , act_path + "+ns1/ClassA.m" )); - EXPECT(files_equal(exp_path + "+ns1/ClassB.m" , act_path + "+ns1/ClassB.m" )); - EXPECT(files_equal(exp_path + "+ns2/ClassA.m" , act_path + "+ns2/ClassA.m" )); - EXPECT(files_equal(exp_path + "+ns2/ClassC.m" , act_path + "+ns2/ClassC.m" )); - EXPECT(files_equal(exp_path + "+ns2/+ns3/ClassB.m" , act_path + "+ns2/+ns3/ClassB.m" )); - EXPECT(files_equal(exp_path + "testNamespaces_wrapper.cpp" , act_path + "testNamespaces_wrapper.cpp" )); + EXPECT(files_equal(exp_path + "ClassD.m", act_path + "ClassD.m" )); + EXPECT(files_equal(exp_path + "+ns1/ClassA.m", act_path + "+ns1/ClassA.m" )); + EXPECT(files_equal(exp_path + "+ns1/ClassB.m", act_path + "+ns1/ClassB.m" )); + EXPECT(files_equal(exp_path + "+ns2/ClassA.m", act_path + "+ns2/ClassA.m" )); + EXPECT(files_equal(exp_path + "+ns2/ClassC.m", act_path + "+ns2/ClassC.m" )); + EXPECT( + files_equal(exp_path + "+ns2/overloadedGlobalFunction.m", exp_path + "+ns2/overloadedGlobalFunction.m" )); + EXPECT( + files_equal(exp_path + "+ns2/+ns3/ClassB.m", act_path + "+ns2/+ns3/ClassB.m" )); + EXPECT( + files_equal(exp_path + "testNamespaces_wrapper.cpp", act_path + "testNamespaces_wrapper.cpp" )); } /* ************************************************************************* */ @@ -445,6 +451,7 @@ TEST( wrap, matlab_code_geometry ) { EXPECT(files_equal(epath + "Point3.m" , apath + "Point3.m" )); EXPECT(files_equal(epath + "Test.m" , apath + "Test.m" )); EXPECT(files_equal(epath + "aGlobalFunction.m" , apath + "aGlobalFunction.m" )); + EXPECT(files_equal(epath + "overloadedGlobalFunction.m" , apath + "overloadedGlobalFunction.m" )); } /* ************************************************************************* */ diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 1acc50db1..47f7d10a6 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -88,9 +88,11 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) string actual_contents = file_contents(actual, skipheader); bool equal = actual_contents == expected_contents; if (!equal) { + cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n"; stringstream command; - command << "diff " << actual << " " << expected << endl; + command << "diff --ignore-all-space " << expected << " " << actual << endl; system(command.str().c_str()); + cerr << ">>>\n"; } return equal; }