12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'develop' into feature/fixExpressionFactorKeys

release/4.3a0
Frank Dellaert 2017-12-02 15:55:25 -08:00
parent 2c05664731 9f09069b65
commit 843682cd7e
583 changed files with 34358 additions and 8803 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -1,4 +1,5 @@
/build*
.idea
*.pyc
*.DS_Store
/examples/Data/dubrovnik-3-7-pre-rewritten.txt

30
CMakeLists.txt
View File

@ -66,8 +66,9 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON)
option(GTSAM_USE_VECTOR3_POINTS "Simply typdef Point3 to eigen::Vector3d" OFF)
option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
# Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries
@ -90,8 +91,8 @@ if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4)
message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.")
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_USE_VECTOR3_POINTS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_USE_VECTOR3_POINTS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.")
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.")
endif()
# Flags for choosing default packaging tools
@ -115,11 +116,11 @@ if(MSVC)
endif()
endif()
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono)
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono)
# Required components
if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR
NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY OR NOT Boost_REGEX_LIBRARY)
NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY)
message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.")
endif()
@ -127,7 +128,7 @@ option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
set(GTSAM_BOOST_LIBRARIES
${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}
${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY})
${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY})
if (GTSAM_DISABLE_NEW_TIMERS)
message("WARNING: GTSAM timing instrumentation manually disabled")
add_definitions(-DGTSAM_DISABLE_NEW_TIMERS)
@ -305,7 +306,7 @@ endif()
# paths so that the compiler uses GTSAM headers in our source directory instead
# of any previously installed GTSAM headers.
include_directories(BEFORE
gtsam/3rdparty/UFconfig
gtsam/3rdparty/SuiteSparse_config
gtsam/3rdparty/CCOLAMD/Include
${METIS_INCLUDE_DIRECTORIES}
${PROJECT_SOURCE_DIR}
@ -485,13 +486,14 @@ message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}")
message(STATUS " CPack Generator : ${CPACK_GENERATOR}")
message(STATUS "GTSAM flags ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ")
print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ")
print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")
message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")

6
CppUnitLite/Test.h
View File

@ -68,6 +68,12 @@ protected:
testGroup##testName##Instance; \
void testGroup##testName##Test::run (TestResult& result_)
/**
* Declare friend in a class to test its private methods
*/
#define FRIEND_TEST(testGroup, testName) \
friend class testGroup##testName##Test;
/**
* For debugging only: use TEST_UNSAFE to allow debuggers to have access to exceptions, as this
* will not wrap execution with a try/catch block

2
CppUnitLite/TestResult.h
View File

@ -18,8 +18,6 @@
//
///////////////////////////////////////////////////////////////////////////////
#ifndef TESTRESULT_H
#define TESTRESULT_H

6
LICENSE
View File

@ -4,11 +4,11 @@ LICENSE.BSD in this directory.
GTSAM contains two third party libraries, with documentation of licensing and
modifications as follows:
- CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree
- CCOLAMD 2.9.3: Tim Davis' constrained column approximate minimum degree
ordering library
- Included unmodified in gtsam/3rdparty/CCOLAMD and gtsam/3rdparty/UFconfig
- http://www.cise.ufl.edu/research/sparse
- Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt
- http://faculty.cse.tamu.edu/davis/suitesparse.html
- Licenced under BSD-3, provided in gtsam/3rdparty/CCOLAMD/Doc/License.txt
- Eigen 3.2: General C++ matrix and linear algebra library
- Modified with 3 patches that have been contributed back to the Eigen team:
- http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection)

134
README.md
View File

@ -1,59 +1,77 @@
README - Georgia Tech Smoothing and Mapping library
===================================================
What is GTSAM?
--------------
GTSAM is a library of C++ classes that implement smoothing and
mapping (SAM) in robotics and vision, using factor graphs and Bayes
networks as the underlying computing paradigm rather than sparse
matrices.
On top of the C++ library, GTSAM includes a MATLAB interface (enable
GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
is under development.
Quickstart
----------
In the root library folder execute:
```
#!bash
$ mkdir build
$ cd build
$ cmake ..
$ make check (optional, runs unit tests)
$ make install
```
Prerequisites:
- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
Optional prerequisites - used automatically if findable by CMake:
- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
GTSAM 4 Compatibility
---------------------
GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect.
Additional Information
----------------------
Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here.
See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
README - Georgia Tech Smoothing and Mapping library
===================================================
What is GTSAM?
--------------
GTSAM is a library of C++ classes that implement smoothing and
mapping (SAM) in robotics and vision, using factor graphs and Bayes
networks as the underlying computing paradigm rather than sparse
matrices.
On top of the C++ library, GTSAM includes a MATLAB interface (enable
GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
is under development.
Quickstart
----------
In the root library folder execute:
```
#!bash
$ mkdir build
$ cd build
$ cmake ..
$ make check (optional, runs unit tests)
$ make install
```
Prerequisites:
- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
Optional prerequisites - used automatically if findable by CMake:
- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
GTSAM 4 Compatibility
---------------------
GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect.
The Preintegrated IMU Factor
----------------------------
GTSAM includes a state of the art IMU handling scheme based on
- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
Our implementation improves on this using integration on the manifold, as detailed in
- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014.
- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015.
If you are using the factor in academic work, please cite the publications above.
In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
Additional Information
----------------------
Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here.
See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).

16
bitbucket-pipelines.yml Normal file
View File

@ -0,0 +1,16 @@
# This is a sample build configuration for C++ – Make.
# Check our guides at https://confluence.atlassian.com/x/5Q4SMw for more examples.
# Only use spaces to indent your .yml configuration.
# -----
# You can specify a custom docker image from Docker Hub as your build environment.
image: ilssim/cmake-boost-qt5
pipelines:
default:
- step:
script: # Modify the commands below to build your repository.
- mkdir build
- cd build
- cmake ..
- make
- make check

27
cmake/GtsamBuildTypes.cmake
View File

@ -6,7 +6,8 @@ list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}")
if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING
"Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo.")
set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE})
set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE} CACHE STRING
"Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo." FORCE)
endif()
# Add option for using build type postfixes to allow installing multiple build modes
@ -60,18 +61,18 @@ mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_C
# Apply the gtsam specific build flags as normal variables. This makes it so that they only
# apply to the gtsam part of the build if gtsam is built as a subproject
set(CMAKE_C_FLAGS ${GTSAM_CMAKE_C_FLAGS})
set(CMAKE_CXX_FLAGS ${GTSAM_CMAKE_CXX_FLAGS})
set(CMAKE_C_FLAGS_DEBUG ${GTSAM_CMAKE_C_FLAGS_DEBUG})
set(CMAKE_CXX_FLAGS_DEBUG ${GTSAM_CMAKE_CXX_FLAGS_DEBUG})
set(CMAKE_C_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO})
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO})
set(CMAKE_C_FLAGS_RELEASE ${GTSAM_CMAKE_C_FLAGS_RELEASE})
set(CMAKE_CXX_FLAGS_RELEASE ${GTSAM_CMAKE_CXX_FLAGS_RELEASE})
set(CMAKE_C_FLAGS_PROFILING ${GTSAM_CMAKE_C_FLAGS_PROFILING})
set(CMAKE_CXX_FLAGS_PROFILING ${GTSAM_CMAKE_CXX_FLAGS_PROFILING})
set(CMAKE_C_FLAGS_TIMING ${GTSAM_CMAKE_C_FLAGS_TIMING})
set(CMAKE_CXX_FLAGS_TIMING ${GTSAM_CMAKE_CXX_FLAGS_TIMING})
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${GTSAM_CMAKE_C_FLAGS}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GTSAM_CMAKE_CXX_FLAGS}")
set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} ${GTSAM_CMAKE_C_FLAGS_DEBUG}")
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${GTSAM_CMAKE_CXX_FLAGS_DEBUG}")
set(CMAKE_C_FLAGS_RELWITHDEBINFO "${CMAKE_C_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}")
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO}")
set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} ${GTSAM_CMAKE_C_FLAGS_RELEASE}")
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${GTSAM_CMAKE_CXX_FLAGS_RELEASE}")
set(CMAKE_C_FLAGS_PROFILING "${CMAKE_C_FLAGS_PROFILING} ${GTSAM_CMAKE_C_FLAGS_PROFILING}")
set(CMAKE_CXX_FLAGS_PROFILING "${CMAKE_CXX_FLAGS_PROFILING} ${GTSAM_CMAKE_CXX_FLAGS_PROFILING}")
set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_TIMING} ${GTSAM_CMAKE_C_FLAGS_TIMING}")
set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_TIMING} ${GTSAM_CMAKE_CXX_FLAGS_TIMING}")
set(CMAKE_SHARED_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING})
set(CMAKE_MODULE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING})

3
cmake/GtsamMatlabWrap.cmake
View File

@ -128,8 +128,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
## This needs to be fixed!!
if(UNIX AND NOT APPLE)
list(APPEND automaticDependencies ${Boost_SERIALIZATION_LIBRARY_RELEASE} ${Boost_FILESYSTEM_LIBRARY_RELEASE}
${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE}
${Boost_REGEX_LIBRARY_RELEASE})
${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE})
if(Boost_TIMER_LIBRARY_RELEASE AND NOT GTSAM_DISABLE_NEW_TIMERS) # Only present in Boost >= 1.48.0
list(APPEND automaticDependencies ${Boost_TIMER_LIBRARY_RELEASE} ${Boost_CHRONO_LIBRARY_RELEASE})
if(GTSAM_MEX_BUILD_STATIC_MODULE)

15
cmake/GtsamTesting.cmake
View File

@ -101,6 +101,9 @@ mark_as_advanced(GTSAM_SINGLE_TEST_EXE)
# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
if(GTSAM_BUILD_TESTS)
add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
# Add target to build tests without running
add_custom_target(all.tests)
endif()
# Add examples target
@ -109,8 +112,6 @@ add_custom_target(examples)
# Add timing target
add_custom_target(timing)
# Add target to build tests without running
add_custom_target(all.tests)
# Implementations of this file's macros:
@ -179,11 +180,17 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests/${groupName}")
endforeach()
else()
#skip folders which don't have any tests
if(NOT script_srcs)
return()
endif()
# Default on MSVC and XCode - combine test group into a single exectuable
set(target_name check_${groupName}_program)
# Add executable
add_executable(${target_name} ${script_srcs} ${script_headers})
add_executable(${target_name} "${script_srcs}" ${script_headers})
target_link_libraries(${target_name} CppUnitLite ${linkLibraries})
# Only have a main function in one script - use preprocessor
@ -196,7 +203,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
add_dependencies(check.${groupName} ${target_name})
add_dependencies(check ${target_name})
if(NOT XCODE_VERSION)
add_dependencies(all.tests ${script_name})
add_dependencies(all.tests ${target_name})
endif()
# Add TOPSRCDIR

23
examples/CameraResectioning.cpp
View File

@ -48,8 +48,7 @@ public:
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const {
SimpleCamera camera(pose, *K_);
Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_);
return reprojectionError.vector();
return camera.project(P_, H, boost::none, boost::none) - p_;
}
};
@ -72,18 +71,14 @@ int main(int argc, char* argv[]) {
// add measurement factors
SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5));
boost::shared_ptr<ResectioningFactor> factor;
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(55, 45), Point3(10, 10, 0)));
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(45, 45), Point3(-10, 10, 0)));
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(45, 55), Point3(-10, -10, 0)));
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(55, 55), Point3(10, -10, 0)));
graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(55, 45), Point3(10, 10, 0));
graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(45, 45), Point3(-10, 10, 0));
graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(45, 55), Point3(-10, -10, 0));
graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(55, 55), Point3(10, -10, 0));
/* 3. Create an initial estimate for the camera pose */
Values initial;

3
examples/CreateSFMExampleData.cpp
View File

@ -18,7 +18,6 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
@ -41,7 +40,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
data.cameras.push_back(SfM_Camera(pose1, K));
data.cameras.push_back(SfM_Camera(pose2, K));
BOOST_FOREACH(const Point3& p, P) {
for(const Point3& p: P) {
// Create the track
SfM_Track track;

21
examples/Data/QPExample.QPS Normal file
View File

@ -0,0 +1,21 @@
NAME QP example
ROWS
N obj
G r1
L r2
COLUMNS
c1 r1 2.0 r2 -1.0
c1 obj 1.5
c2 r1 1.0 r2 2.0
c2 obj -2.0
RHS
rhs1 obj -4.0
rhs1 r1 2.0 r2 6.0
RANGES
BOUNDS
UP BOUNDS c1 20.0
QUADOBJ
c1 c1 8.0
c1 c2 2.0
c2 c2 10.0
ENDATA

20101
examples/Data/imuAndGPSdata.csv Normal file
View File

File diff suppressed because it is too large Load Diff

269
examples/ImuFactorsExample.cpp Normal file
View File

@ -0,0 +1,269 @@
/* ----------------------------------------------------------------------------
* 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 imuFactorsExample
* @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor navigation code.
* @author Garrett (ghemann@gmail.com), Luca Carlone
*/
/**
* Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS
* - you can test imuFactor (resp. combinedImuFactor) by commenting (resp. uncommenting)
* the line #define USE_COMBINED (few lines below)
* - we read IMU and GPS data from a CSV file, with the following format:
* A row starting with "i" is the first initial position formatted with
* N, E, D, qx, qY, qZ, qW, velN, velE, velD
* A row starting with "0" is an imu measurement
* linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
* A row starting with "1" is a gps correction formatted with
* N, E, D, qX, qY, qZ, qW
* Note that for GPS correction, we're only using the position not the rotation. The
* rotation is provided in the file for ground truth comparison.
*/
// GTSAM related includes.
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <fstream>
#include <iostream>
// Uncomment line below to use the CombinedIMUFactor as opposed to the standard ImuFactor.
// #define USE_COMBINED
using namespace gtsam;
using namespace std;
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
const string output_filename = "imuFactorExampleResults.csv";
// This will either be PreintegratedImuMeasurements (for ImuFactor) or
// PreintegratedCombinedMeasurements (for CombinedImuFactor).
PreintegrationType *imu_preintegrated_;
int main(int argc, char* argv[])
{
string data_filename;
if (argc < 2) {
printf("using default CSV file\n");
data_filename = findExampleDataFile("imuAndGPSdata.csv");
} else {
data_filename = argv[1];
}
// Set up output file for plotting errors
FILE* fp_out = fopen(output_filename.c_str(), "w+");
fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,gt_qy,gt_qz,gt_qw\n");
// Begin parsing the CSV file. Input the first line for initialization.
// From there, we'll iterate through the file and we'll preintegrate the IMU
// or add in the GPS given the input.
ifstream file(data_filename.c_str());
string value;
// Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
Eigen::Matrix<double,10,1> initial_state = Eigen::Matrix<double,10,1>::Zero();
getline(file, value, ','); // i
for (int i=0; i<9; i++) {
getline(file, value, ',');
initial_state(i) = atof(value.c_str());
}
getline(file, value, '\n');
initial_state(9) = atof(value.c_str());
cout << "initial state:\n" << initial_state.transpose() << "\n\n";
// Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
initial_state(4), initial_state(5));
Point3 prior_point(initial_state.head<3>());
Pose3 prior_pose(prior_rotation, prior_point);
Vector3 prior_velocity(initial_state.tail<3>());
imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
Values initial_values;
int correction_count = 0;
initial_values.insert(X(correction_count), prior_pose);
initial_values.insert(V(correction_count), prior_velocity);
initial_values.insert(B(correction_count), prior_imu_bias);
// Assemble prior noise model and add it the graph.
noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()); // rad,rad,rad,m, m, m
noiseModel::Diagonal::shared_ptr velocity_noise_model = noiseModel::Isotropic::Sigma(3,0.1); // m/s
noiseModel::Diagonal::shared_ptr bias_noise_model = noiseModel::Isotropic::Sigma(6,1e-3);
// Add all prior factors (pose, velocity, bias) to the graph.
NonlinearFactorGraph *graph = new NonlinearFactorGraph();
graph->add(PriorFactor<Pose3>(X(correction_count), prior_pose, pose_noise_model));
graph->add(PriorFactor<Vector3>(V(correction_count), prior_velocity,velocity_noise_model));
graph->add(PriorFactor<imuBias::ConstantBias>(B(correction_count), prior_imu_bias,bias_noise_model));
// We use the sensor specs to build the noise model for the IMU factor.
double accel_noise_sigma = 0.0003924;
double gyro_noise_sigma = 0.000205689024915;
double accel_bias_rw_sigma = 0.004905;
double gyro_bias_rw_sigma = 0.000001454441043;
Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2);
Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2);
Matrix33 integration_error_cov = Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities
Matrix33 bias_acc_cov = Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2);
Matrix33 bias_omega_cov = Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2);
Matrix66 bias_acc_omega_int = Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration
boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
// PreintegrationBase params:
p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
// should be using 2nd order integration
// PreintegratedRotation params:
p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
// PreintegrationCombinedMeasurements params:
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
p->biasAccOmegaInt = bias_acc_omega_int;
#ifdef USE_COMBINED
imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias);
#else
imu_preintegrated_ = new PreintegratedImuMeasurements(p, prior_imu_bias);
#endif
// Store previous state for the imu integration and the latest predicted outcome.
NavState prev_state(prior_pose, prior_velocity);
NavState prop_state = prev_state;
imuBias::ConstantBias prev_bias = prior_imu_bias;
// Keep track of the total error over the entire run for a simple performance metric.
double current_position_error = 0.0, current_orientation_error = 0.0;
double output_time = 0.0;
double dt = 0.005; // The real system has noise, but here, results are nearly
// exactly the same, so keeping this for simplicity.
// All priors have been set up, now iterate through the data file.
while (file.good()) {
// Parse out first value
getline(file, value, ',');
int type = atoi(value.c_str());
if (type == 0) { // IMU measurement
Eigen::Matrix<double,6,1> imu = Eigen::Matrix<double,6,1>::Zero();
for (int i=0; i<5; ++i) {
getline(file, value, ',');
imu(i) = atof(value.c_str());
}
getline(file, value, '\n');
imu(5) = atof(value.c_str());
// Adding the IMU preintegration.
imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
} else if (type == 1) { // GPS measurement
Eigen::Matrix<double,7,1> gps = Eigen::Matrix<double,7,1>::Zero();
for (int i=0; i<6; ++i) {
getline(file, value, ',');
gps(i) = atof(value.c_str());
}
getline(file, value, '\n');
gps(6) = atof(value.c_str());
correction_count++;
// Adding IMU factor and GPS factor and optimizing.
#ifdef USE_COMBINED
PreintegratedCombinedMeasurements *preint_imu_combined = dynamic_cast<PreintegratedCombinedMeasurements*>(imu_preintegrated_);
CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
X(correction_count ), V(correction_count ),
B(correction_count-1), B(correction_count ),
*preint_imu_combined);
graph->add(imu_factor);
#else
PreintegratedImuMeasurements *preint_imu = dynamic_cast<PreintegratedImuMeasurements*>(imu_preintegrated_);
ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
X(correction_count ), V(correction_count ),
B(correction_count-1),
*preint_imu);
graph->add(imu_factor);
imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
B(correction_count ),
zero_bias, bias_noise_model));
#endif
noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3,1.0);
GPSFactor gps_factor(X(correction_count),
Point3(gps(0), // N,
gps(1), // E,
gps(2)), // D,
correction_noise);
graph->add(gps_factor);
// Now optimize and compare results.
prop_state = imu_preintegrated_->predict(prev_state, prev_bias);
initial_values.insert(X(correction_count), prop_state.pose());
initial_values.insert(V(correction_count), prop_state.v());
initial_values.insert(B(correction_count), prev_bias);
LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
Values result = optimizer.optimize();
// Overwrite the beginning of the preintegration for the next step.
prev_state = NavState(result.at<Pose3>(X(correction_count)),
result.at<Vector3>(V(correction_count)));
prev_bias = result.at<imuBias::ConstantBias>(B(correction_count));
// Reset the preintegration object.
imu_preintegrated_->resetIntegrationAndSetBias(prev_bias);
// Print out the position and orientation error for comparison.
Vector3 gtsam_position = prev_state.pose().translation();
Vector3 position_error = gtsam_position - gps.head<3>();
current_position_error = position_error.norm();
Quaternion gtsam_quat = prev_state.pose().rotation().toQuaternion();
Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
Quaternion quat_error = gtsam_quat * gps_quat.inverse();
quat_error.normalize();
Vector3 euler_angle_error(quat_error.x()*2,
quat_error.y()*2,
quat_error.z()*2);
current_orientation_error = euler_angle_error.norm();
// display statistics
cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n";
fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2),
gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(),
gps(0), gps(1), gps(2),
gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());
output_time += 1.0;
} else {
cerr << "ERROR parsing file\n";
return 1;
}
}
fclose(fp_out);
cout << "Complete, results written to " << output_filename << "\n\n";;
return 0;
}

10
examples/LocalizationExample.cpp
View File

@ -120,15 +120,15 @@ int main(int argc, char** argv) {
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise);
// 2b. Add "GPS-like" measurements
// We will use our custom UnaryFactor for this.
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution

6
examples/METISOrderingExample.cpp
View File

@ -37,12 +37,12 @@ int main(int argc, char** argv) {
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
graph.emplace_shared<PriorFactor<Pose2> >(1, priorMean, priorNoise);
Pose2 odometry(2.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
graph.print("\nFactor Graph:\n"); // print
Values initial;

6
examples/OdometryExample.cpp
View File

@ -65,15 +65,15 @@ int main(int argc, char** argv) {
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
graph.emplace_shared<PriorFactor<Pose2> >(1, priorMean, priorNoise);
// Add odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
graph.print("\nFactor Graph:\n"); // print
// Create the data structure to hold the initialEstimate estimate to the solution

12
examples/PlanarSLAMExample.cpp
View File

@ -81,13 +81,13 @@ int main(int argc, char** argv) {
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.add(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
graph.emplace_shared<PriorFactor<Pose2> >(x1, prior, priorNoise); // add directly to graph
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
graph.emplace_shared<BetweenFactor<Pose2> >(x1, x2, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(x2, x3, odometry, odometryNoise);
// Add Range-Bearing measurements to two different landmarks
// create a noise model for the landmark measurements
@ -101,9 +101,9 @@ int main(int argc, char** argv) {
range32 = 2.0;
// Add Bearing-Range factors
graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x2, l1, bearing21, range21, measurementNoise);
graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x3, l2, bearing32, range32, measurementNoise);
// Print
graph.print("Factor Graph:\n");

12
examples/Pose2SLAMExample.cpp
View File

@ -72,23 +72,23 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0 ), model);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
// 2c. Add the loop closure constraint
// This factor encodes the fact that we have returned to the same pose. In real systems,
// these constraints may be identified in many ways, such as appearance-based techniques
// with camera images. We will use another Between Factor to enforce this constraint:
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
graph.emplace_shared<BetweenFactor<Pose2> >(5, 2, Pose2(2, 0, M_PI_2), model);
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution

12
examples/Pose2SLAMExample_graphviz.cpp
View File

@ -33,19 +33,19 @@ int main (int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0 ), model);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
// 2c. Add the loop closure constraint
graph.push_back(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
graph.emplace_shared<BetweenFactor<Pose2> >(5, 2, Pose2(2, 0, M_PI_2), model);
// 3. Create the data structure to hold the initial estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values

12
examples/Pose2SLAMwSPCG.cpp
View File

@ -36,18 +36,18 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, prior, priorNoise));
graph.emplace_shared<PriorFactor<Pose2> >(1, prior, priorNoise);
// 2b. Add odometry factors
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 2c. Add the loop closure constraint
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.push_back(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0), model);
graph.print("\nFactor Graph:\n"); // print

4
examples/Pose3SLAMExample_changeKeys.cpp
View File

@ -56,7 +56,7 @@ int main(const int argc, const char *argv[]) {
std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
// Additional: rewrite input with simplified keys 0,1,...
Values simpleInitial;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
Key key;
if(add)
key = key_value.key + firstKey;
@ -66,7 +66,7 @@ int main(const int argc, const char *argv[]) {
simpleInitial.insert(key, initial->at(key_value.key));
}
NonlinearFactorGraph simpleGraph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
for(const boost::shared_ptr<NonlinearFactor>& factor: *graph) {
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){

2
examples/Pose3SLAMExample_g2o.cpp
View File

@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) {
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

2
examples/Pose3SLAMExample_initializePose3Chordal.cpp
View File

@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) {
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

2
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) {
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

37
examples/README
View File

@ -1,37 +0,0 @@
This directory contains a number of examples that illustrate the use of GTSAM:
SimpleRotation: a super-simple example of optimizing a single rotation according to a single prior
Kalman Filter Examples
======================
elaboratePoint2KalmanFilter: simple linear Kalman filter on a moving 2D point, but done using factor graphs
easyPoint2KalmanFilter: uses the cool generic templated Kalman filter class to do the same
fullStateKalmanFilter: simple 1D example with a full-state filter
errorStateKalmanFilter: simple 1D example of a moving target measured by a accelerometer, incl. drift-rate bias
2D Pose SLAM
============
LocalizationExample.cpp: modeling robot motion
LocalizationExample2.cpp: example with GPS like measurements
Pose2SLAMExample: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
Pose2SLAMExample_advanced: same, but uses an Optimizer object
Pose2SLAMwSPCG: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
Planar SLAM with landmarks
==========================
PlanarSLAMExample: simple robotics example using the pre-built planar SLAM domain
PlanarSLAMExample_selfcontained: simple robotics example with all typedefs internal to this script.
Visual SLAM
===========
CameraResectioning.cpp: An example of gtsam for solving the camera resectioning problem
The directory vSLAMexample includes 2 simple examples using GTSAM:
- vSFMexample using visualSLAM in for structure-from-motion (SFM), and
- vISAMexample using visualSLAM and ISAM for incremental SLAM updates
See the separate README file there.
Undirected Graphical Models (UGM)
=================================
The best representation for a Markov Random Field is a factor graph :-)
This is illustrated with some discrete examples from the UGM MATLAB toolbox, which
can be found at http://www.di.ens.fr/~mschmidt/Software/UGM

96
examples/README.md Normal file
View File

@ -0,0 +1,96 @@
# GTSAM Examples
This directory contains all GTSAM C++ examples GTSAM pertaining to SFM
## Basic Examples:
* **SimpleRotation**: a simple example of optimizing a single rotation according to a single prior
* **CameraResectioning**: resection camera from some known points
* **SFMExample**: basic structure from motion
* **SFMExample_bal**: same, but read data from read from BAL file
* **SelfCalibrationExample**: Do SFM while also optimizing for calibration
## Stereo Visual Odometry Examples
Visual odometry using a stereo rig:
* **StereoVOExample**: basic example of stereo VO
* **StereoVOExample_large**: larger, with a snippet of Kitti data
## More Advanced Examples
The following examples illustrate some concepts from Georgia Tech's research papers, listed in the references section at the end:
* **VisualISAMExample**: uses iSAM [TRO08]
* **VisualISAM2Example**: uses iSAM2 [IJRR12]
* **SFMExample_SmartFactor**: uses smartFactors [ICRA14]
## Kalman Filter Examples
* **elaboratePoint2KalmanFilter**: simple linear Kalman filter on a moving 2D point, but done using factor graphs
* **easyPoint2KalmanFilter**: uses the generic templated Kalman filter class to do the same
* **fullStateKalmanFilter**: simple 1D example with a full-state filter
* **errorStateKalmanFilter**: simple 1D example of a moving target measured by a accelerometer, incl. drift-rate bias
## 2D Pose SLAM
* **LocalizationExample.cpp**: modeling robot motion
* **LocalizationExample2.cpp**: example with GPS like measurements
* **Pose2SLAMExample**: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
* **Pose2SLAMExample_advanced**: same, but uses an Optimizer object
* **Pose2SLAMwSPCG**: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
## Planar SLAM with landmarks
* **PlanarSLAMExample**: simple robotics example using the pre-built planar SLAM domain
* **PlanarSLAMExample_selfcontained**: simple robotics example with all typedefs internal to this script.
## Visual SLAM
The directory **vSLAMexample** includes 2 simple examples using GTSAM:
- **vSFMexample** using visual SLAM for structure-from-motion (SFM)
- **vISAMexample** using visual SLAM and ISAM for incremental SLAM updates
See the separate README file there.
##Undirected Graphical Models (UGM)
The best representation for a Markov Random Field is a factor graph :-) This is illustrated with some discrete examples from the UGM MATLAB toolbox, which
can be found at <http://www.di.ens.fr/~mschmidt/Software/UGM>
##Building and Running
To build, cd into the directory and do:
```
mkdir build
cd build
cmake ..
```
For each .cpp file in this directory two make targets are created, one to build the executable, and one to build and run it. For example, the file `CameraResectioning.cpp` contains simple example to resection a camera from 4 known points. You can build it using
```
make CameraResectioning
```
or build and run it immediately with
```
make CameraResectioning.run
```
which should output:
```
Final result:
Values with 1 values:
Value x1: R:
[
1, 0.0, 0.0,
0.0, -1, 0.0,
0.0, 0.0, -1,
];
t: [0, 0, 2]';
```
## References
- [TRO08]: [iSAM: Incremental Smoothing and Mapping, Michael Kaess](http://frank.dellaert.com/pub/Kaess08tro.pdf), Michael Kaess, Ananth Ranganathan, and Frank Dellaert, IEEE Transactions on Robotics, 2008
- [IJRR12]: [iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree](http://www.cc.gatech.edu/~dellaert/pub/Kaess12ijrr.pdf), Michael Kaess, Hordur Johannsson, Richard Roberts, Viorela Ila, John Leonard, and Frank Dellaert, International Journal of Robotics Research, 2012
- [ICRA14]: [Eliminating Conditionally Independent Sets in Factor Graphs: A Unifying Perspective based on Smart Factors](http://frank.dellaert.com/pub/Carlone14icra.pdf), Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, IEEE International Conference on Robotics and Automation (ICRA), 2014

5
examples/RangeISAMExample_plaza2.cpp
View File

@ -43,7 +43,6 @@
#include <gtsam/slam/dataset.h>
// Standard headers, added last, so we know headers above work on their own
#include <boost/foreach.hpp>
#include <fstream>
#include <iostream>
@ -151,7 +150,7 @@ int main (int argc, char** argv) {
// Loop over odometry
gttic_(iSAM);
BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
for(const TimedOdometry& timedOdometry: odometry) {
//--------------------------------- odometry loop -----------------------------------------
double t;
Pose2 odometry;
@ -196,7 +195,7 @@ int main (int argc, char** argv) {
}
i += 1;
//--------------------------------- odometry loop -----------------------------------------
} // BOOST_FOREACH
} // end for
gttoc_(iSAM);
// Print timings

6
examples/SFMExample.cpp
View File

@ -75,14 +75,14 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x1. This indirectly specifies where the origin is.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) {
SimpleCamera camera(poses[i], *K);
for (size_t j = 0; j < points.size(); ++j) {
Point2 measurement = camera.project(points[j]);
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
}
}
@ -90,7 +90,7 @@ int main(int argc, char* argv[]) {
// Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
// between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
graph.print("Factor Graph:\n");
// Create the data structure to hold the initial estimate to the solution

8
examples/SFMExampleExpressions_bal.cpp
View File

@ -78,10 +78,10 @@ int main(int argc, char* argv[]) {
// Simulated measurements from each camera pose, adding them to the factor graph
size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
for(const SfM_Track& track: mydata.tracks) {
// Leaf expression for j^th point
Point3_ point_('p', j);
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
for(const SfM_Measurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
// Leaf expression for i^th camera
@ -98,9 +98,9 @@ int main(int argc, char* argv[]) {
Values initial;
size_t i = 0;
j = 0;
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras)
for(const SfM_Camera& camera: mydata.cameras)
initial.insert(C(i++), camera);
BOOST_FOREACH(const SfM_Track& track, mydata.tracks)
for(const SfM_Track& track: mydata.tracks)
initial.insert(P(j++), track.p);
/* Optimize the graph and print results */

6
examples/SFMExample_SmartFactor.cpp
View File

@ -82,13 +82,13 @@ int main(int argc, char* argv[]) {
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
// Because the structure-from-motion problem has a scale ambiguity, the problem is
// still under-constrained. Here we add a prior on the second pose x1, so this will
// fix the scale by indicating the distance between x0 and x1.
// Because these two are fixed, the rest of the poses will be also be fixed.
graph.push_back(PriorFactor<Camera>(1, Camera(poses[1],K), noise)); // add directly to graph
graph.emplace_shared<PriorFactor<Pose3> >(1, poses[1], noise); // add directly to graph
graph.print("Factor Graph:\n");
@ -97,7 +97,7 @@ int main(int argc, char* argv[]) {
Values initialEstimate;
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, Camera(poses[i].compose(delta), K));
initialEstimate.insert(i, poses[i].compose(delta));
initialEstimate.print("Initial Estimates:\n");
// Optimize the graph and print results

6
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -74,16 +74,16 @@ int main(int argc, char* argv[]) {
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
// Fix the scale ambiguity by adding a prior
graph.push_back(PriorFactor<Camera>(1, Camera(poses[0],K), noise));
graph.emplace_shared<PriorFactor<Pose3> >(1, poses[0], noise);
// Create the initial estimate to the solution
Values initialEstimate;
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, Camera(poses[i].compose(delta),K));
initialEstimate.insert(i, poses[i].compose(delta));
// We will use LM in the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used.

14
examples/SFMExample_bal.cpp
View File

@ -55,25 +55,25 @@ int main (int argc, char* argv[]) {
// Add measurements to the factor graph
size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
for(const SfM_Track& track: mydata.tracks) {
for(const SfM_Measurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
}
j += 1;
}
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
graph.emplace_shared<PriorFactor<SfM_Camera> >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.emplace_shared<PriorFactor<Point3> > (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
// Create initial estimate
Values initial;
size_t i = 0; j = 0;
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p);
for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera);
for(const SfM_Track& track: mydata.tracks) initial.insert(P(j++), track.p);
/* Optimize the graph and print results */
Values result;

14
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

@ -60,25 +60,25 @@ int main (int argc, char* argv[]) {
// Add measurements to the factor graph
size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
for(const SfM_Track& track: mydata.tracks) {
for(const SfM_Measurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
}
j += 1;
}
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
graph.emplace_shared<PriorFactor<SfM_Camera> >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.emplace_shared<PriorFactor<Point3> >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
// Create initial estimate
Values initial;
size_t i = 0; j = 0;
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p);
for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera);
for(const SfM_Track& track: mydata.tracks) initial.insert(P(j++), track.p);
/** --------------- COMPARISON -----------------------**/
/** ----------------------------------------------------**/

8
examples/SelfCalibrationExample.cpp
View File

@ -54,7 +54,7 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x1.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
// Simulated measurements from each camera pose, adding them to the factor graph
Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
@ -65,17 +65,17 @@ int main(int argc, char* argv[]) {
Point2 measurement = camera.project(points[j]);
// The only real difference with the Visual SLAM example is that here we use a
// different factor type, that also calculates the Jacobian with respect to calibration
graph.push_back(GeneralSFMFactor2<Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0)));
graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0));
}
}
// Add a prior on the position of the first landmark.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
// Add a prior on the calibration.
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished());
graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));
graph.emplace_shared<PriorFactor<Cal3_S2> >(Symbol('K', 0), K, calNoise);
// Create the initial estimate to the solution
// now including an estimate on the camera calibration parameters

13
examples/SolverComparer.cpp
View File

@ -50,6 +50,7 @@
#include <boost/archive/binary_oarchive.hpp>
#include <boost/program_options.hpp>
#include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/random.hpp>
#include <boost/serialization/export.hpp>
@ -80,7 +81,7 @@ double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& c
// the factor graph already includes a factor for the prior/equality constraint.
// double dof = graph.size() - config.size();
int graph_dim = 0;
BOOST_FOREACH(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf, graph) {
for(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf: graph) {
graph_dim += (int)nlf->dim();
}
double dof = double(graph_dim) - double(config.dim()); // kaess: changed to dim
@ -421,9 +422,9 @@ void runIncremental()
//try {
// Marginals marginals(graph, values);
// int i=0;
// BOOST_REVERSE_FOREACH(Key key1, values.keys()) {
// for (Key key1: boost::adaptors::reverse(values.keys())) {
// int j=0;
// BOOST_REVERSE_FOREACH(Key key2, values.keys()) {
// for (Key key12: boost::adaptors::reverse(values.keys())) {
// if(i != j) {
// gttic_(jointMarginalInformation);
// std::vector<Key> keys(2);
@ -442,7 +443,7 @@ void runIncremental()
// break;
// }
// tictoc_print_();
// BOOST_FOREACH(Key key, values.keys()) {
// for(Key key: values.keys()) {
// gttic_(marginalInformation);
// Matrix info = marginals.marginalInformation(key);
// gttoc_(marginalInformation);
@ -535,7 +536,7 @@ void runCompare()
vector<Key> commonKeys;
br::set_intersection(soln1.keys(), soln2.keys(), std::back_inserter(commonKeys));
double maxDiff = 0.0;
BOOST_FOREACH(Key j, commonKeys)
for(Key j: commonKeys)
maxDiff = std::max(maxDiff, soln1.at(j).localCoordinates_(soln2.at(j)).norm());
cout << " Maximum solution difference (norm of logmap): " << maxDiff << endl;
}
@ -549,7 +550,7 @@ void runPerturb()
// Perturb values
VectorValues noise;
BOOST_FOREACH(const Values::KeyValuePair& key_val, initial)
for(const Values::KeyValuePair& key_val: initial)
{
Vector noisev(key_val.value.dim());
for(Vector::Index i = 0; i < noisev.size(); ++i)

16
examples/StereoVOExample.cpp
View File

@ -39,7 +39,7 @@ int main(int argc, char** argv){
//create graph object, add first pose at origin with key '1'
NonlinearFactorGraph graph;
Pose3 first_pose;
graph.push_back(NonlinearEquality<Pose3>(1, Pose3()));
graph.emplace_shared<NonlinearEquality<Pose3> >(1, Pose3());
//create factor noise model with 3 sigmas of value 1
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
@ -47,14 +47,14 @@ int main(int argc, char** argv){
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
//create and add stereo factors between first pose (key value 1) and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(520, 480, 440), model, 1, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(120, 80, 440), model, 1, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 280, 140), model, 1, 5, K));
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(520, 480, 440), model, 1, 3, K);
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(120, 80, 440), model, 1, 4, K);
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(320, 280, 140), model, 1, 5, K);
//create and add stereo factors between second pose and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(570, 520, 490), model, 2, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(70, 20, 490), model, 2, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 270, 115), model, 2, 5, K));
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(570, 520, 490), model, 2, 3, K);
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(70, 20, 490), model, 2, 4, K);
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(320, 270, 115), model, 2, 5, K);
//create Values object to contain initial estimates of camera poses and landmark locations
Values initial_estimate;
@ -67,7 +67,7 @@ int main(int argc, char** argv){
initial_estimate.insert(5, Point3(0, -0.5, 5));
//create Levenberg-Marquardt optimizer for resulting factor graph, optimize
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
LevenbergMarquardtOptimizer optimizer(graph, initial_estimate);
Values result = optimizer.optimize();
result.print("Final result:\n");

10
examples/StereoVOExample_large.cpp
View File

@ -83,9 +83,9 @@ int main(int argc, char** argv){
cout << "Reading stereo factors" << endl;
//read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
graph.push_back(
GenericStereoFactor<Pose3, Point3>(StereoPoint2(uL, uR, v), model,
Symbol('x', x), Symbol('l', l), K));
graph.emplace_shared<
GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model,
Symbol('x', x), Symbol('l', l), K);
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
if (!initial_estimate.exists(Symbol('l', l))) {
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
@ -99,13 +99,13 @@ int main(int argc, char** argv){
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtParams params;
params.orderingType = Ordering::METIS;
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params);
LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params);
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;

14
examples/TimeTBB.cpp
View File

@ -19,8 +19,8 @@
#include <gtsam/base/Matrix.h>
#include <boost/assign/list_of.hpp>
#include <boost/foreach.hpp>
#include <map>
#include <iostream>
using namespace std;
using namespace gtsam;
@ -76,7 +76,7 @@ map<int, double> testWithoutMemoryAllocation()
const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
map<int, double> timingResults;
BOOST_FOREACH(size_t grainSize, grainSizes)
for(size_t grainSize: grainSizes)
{
tbb::tick_count t0 = tbb::tick_count::now();
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithoutAllocation(results));
@ -129,7 +129,7 @@ map<int, double> testWithMemoryAllocation()
const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
map<int, double> timingResults;
BOOST_FOREACH(size_t grainSize, grainSizes)
for(size_t grainSize: grainSizes)
{
tbb::tick_count t0 = tbb::tick_count::now();
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
@ -150,7 +150,7 @@ int main(int argc, char* argv[])
const vector<int> numThreads = list_of(1)(4)(8);
Results results;
BOOST_FOREACH(size_t n, numThreads)
for(size_t n: numThreads)
{
cout << "With " << n << " threads:" << endl;
tbb::task_scheduler_init init((int)n);
@ -160,19 +160,19 @@ int main(int argc, char* argv[])
}
cout << "Summary of results:" << endl;
BOOST_FOREACH(const Results::value_type& threads_result, results)
for(const Results::value_type& threads_result: results)
{
const int threads = threads_result.first;
const ResultWithThreads& result = threads_result.second;
if(threads != 1)
{
BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithoutAllocation)
for(const ResultWithThreads::value_type& grainsize_time: result.grainSizesWithoutAllocation)
{
const int grainsize = grainsize_time.first;
const double speedup = results[1].grainSizesWithoutAllocation[grainsize] / grainsize_time.second;
cout << threads << " threads, without allocation, grain size = " << grainsize << ", speedup = " << speedup << endl;
}
BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithAllocation)
for(const ResultWithThreads::value_type& grainsize_time: result.grainSizesWithAllocation)
{
const int grainsize = grainsize_time.first;
const double speedup = results[1].grainSizesWithAllocation[grainsize] / grainsize_time.second;

6
examples/VisualISAM2Example.cpp
View File

@ -90,7 +90,7 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
}
// Add an initial guess for the current pose
@ -104,11 +104,11 @@ int main(int argc, char* argv[]) {
if( i == 0) {
// Add a prior on pose x0
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth

9
examples/VisualISAMExample.cpp
View File

@ -89,9 +89,8 @@ int main(int argc, char* argv[]) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
// Add measurement
graph.add(
GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, noise,
Symbol('x', i), Symbol('l', j), K));
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, noise,
Symbol('x', i), Symbol('l', j), K);
}
// Intentionally initialize the variables off from the ground truth
@ -109,12 +108,12 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise =
noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise));
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise);
// Add initial guesses to all observed landmarks
Point3 noise(-0.25, 0.20, 0.15);

12
examples/easyPoint2KalmanFilter.cpp
View File

@ -70,7 +70,7 @@ int main() {
// Predict the new value with the EKF class
Point2 x1_predict = ekf.predict(factor1);
x1_predict.print("X1 Predict");
traits<Point2>::Print(x1_predict, "X1 Predict");
@ -91,7 +91,7 @@ int main() {
// Update the Kalman Filter with the measurement
Point2 x1_update = ekf.update(factor2);
x1_update.print("X1 Update");
traits<Point2>::Print(x1_update, "X1 Update");
@ -101,13 +101,13 @@ int main() {
difference = Point2(1,0);
BetweenFactor<Point2> factor3(x1, x2, difference, Q);
Point2 x2_predict = ekf.predict(factor1);
x2_predict.print("X2 Predict");
traits<Point2>::Print(x2_predict, "X2 Predict");
// Update
Point2 z2(2.0, 0.0);
PriorFactor<Point2> factor4(x2, z2, R);
Point2 x2_update = ekf.update(factor4);
x2_update.print("X2 Update");
traits<Point2>::Print(x2_update, "X2 Update");
@ -117,13 +117,13 @@ int main() {
difference = Point2(1,0);
BetweenFactor<Point2> factor5(x2, x3, difference, Q);
Point2 x3_predict = ekf.predict(factor5);
x3_predict.print("X3 Predict");
traits<Point2>::Print(x3_predict, "X3 Predict");
// Update
Point2 z3(3.0, 0.0);
PriorFactor<Point2> factor6(x3, z3, R);
Point2 x3_update = ekf.update(factor6);
x3_update.print("X3 Update");
traits<Point2>::Print(x3_update, "X3 Update");
return 0;
}

59
gtsam.h
View File

@ -266,23 +266,12 @@ class Point2 {
// Group
static gtsam::Point2 identity();
gtsam::Point2 inverse() const;
gtsam::Point2 compose(const gtsam::Point2& p2) const;
gtsam::Point2 between(const gtsam::Point2& p2) const;
// Manifold
gtsam::Point2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point2& p) const;
// Lie Group
static gtsam::Point2 Expmap(Vector v);
static Vector Logmap(const gtsam::Point2& p);
// Standard Interface
double x() const;
double y() const;
Vector vector() const;
double dist(const gtsam::Point2& p2) const;
double distance(const gtsam::Point2& p2) const;
double norm() const;
// enabling serialization functionality
@ -1368,7 +1357,7 @@ virtual class HessianFactor : gtsam::GaussianFactor {
//Standard Interface
size_t rows() const;
Matrix info() const;
Matrix information() const;
double constantTerm() const;
Vector linearTerm() const;
@ -1941,10 +1930,10 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor {
gtsam::JacobianFactor* toJacobian() const;
gtsam::HessianFactor* toHessian() const;
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
const gtsam::Values& linearizationPoint);
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph);
static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph);
// enabling serialization functionality
void serializable() const;
@ -2506,30 +2495,8 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
bool getUse2ndOrderCoriolis() const;
};
#include <gtsam/navigation/PreintegrationBase.h>
virtual class PreintegrationBase {
// Constructors
PreintegrationBase(const gtsam::PreintegrationParams* params);
PreintegrationBase(const gtsam::PreintegrationParams* params,
const gtsam::imuBias::ConstantBias& bias);
// Testable
void print(string s) const;
bool equals(const gtsam::PreintegrationBase& expected, double tol);
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
// Standard Interface
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
};
#include <gtsam/navigation/ImuFactor.h>
virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase {
class PreintegratedImuMeasurements {
// Constructors
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params);
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params,
@ -2544,6 +2511,13 @@ virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase {
double deltaT);
void resetIntegration();
Matrix preintMeasCov() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
};
virtual class ImuFactor: gtsam::NonlinearFactor {
@ -2559,7 +2533,7 @@ virtual class ImuFactor: gtsam::NonlinearFactor {
};
#include <gtsam/navigation/CombinedImuFactor.h>
virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase {
class PreintegratedCombinedMeasurements {
// Testable
void print(string s) const;
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
@ -2570,6 +2544,13 @@ virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase {
double deltaT);
void resetIntegration();
Matrix preintMeasCov() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
};
virtual class CombinedImuFactor: gtsam::NonlinearFactor {

49
gtsam/3rdparty/CCOLAMD/Demo/Makefile vendored Normal file
View File

@ -0,0 +1,49 @@
#-----------------------------------------------------------------------------
# compile the CCOLAMD demo
#-----------------------------------------------------------------------------
default: all
include ../../SuiteSparse_config/SuiteSparse_config.mk
I = -I../../include
C = $(CC) $(CF) $(I)
LIB2 = $(LDFLAGS) -L../../lib -lccolamd -lsuitesparseconfig $(LDLIBS)
all: library ccolamd_example ccolamd_l_example
library:
( cd ../../SuiteSparse_config ; $(MAKE) )
( cd ../Lib ; $(MAKE) )
#------------------------------------------------------------------------------
# Create the demo program, run it, and compare the output
#------------------------------------------------------------------------------
dist:
ccolamd_example: ccolamd_example.c
$(C) -o ccolamd_example ccolamd_example.c $(LIB2)
- ./ccolamd_example > my_ccolamd_example.out
- diff ccolamd_example.out my_ccolamd_example.out
ccolamd_l_example: ccolamd_l_example.c
$(C) -o ccolamd_l_example ccolamd_l_example.c $(LIB2)
- ./ccolamd_l_example > my_ccolamd_l_example.out
- diff ccolamd_l_example.out my_ccolamd_l_example.out
#------------------------------------------------------------------------------
# Remove all but the files in the original distribution
#------------------------------------------------------------------------------
clean:
- $(RM) -r $(CLEAN)
purge: distclean
distclean: clean
- $(RM) ccolamd_example ccolamd_l_example
- $(RM) my_ccolamd_example.out my_ccolamd_l_example.out
- $(RM) -r $(PURGE)

BIN
gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example vendored
View File

Binary file not shown.

2
gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c vendored
View File

@ -5,8 +5,6 @@
/* ----------------------------------------------------------------------------
* CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*

4
gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out vendored
View File

@ -15,7 +15,7 @@ Column 3, with 2 entries:
row 1
row 3
ccolamd version 2.7, Jan 25, 2011: OK.
ccolamd version 2.9, May 4, 2016: OK.
ccolamd: number of dense or empty rows ignored: 0
ccolamd: number of dense or empty columns ignored: 0
ccolamd: number of garbage collections performed: 0
@ -38,7 +38,7 @@ Column 3, with 1 entries:
row 4
Column 4, with 0 entries:
csymamd version 2.7, Jan 25, 2011: OK.
csymamd version 2.9, May 4, 2016: OK.
csymamd: number of dense or empty rows ignored: 0
csymamd: number of dense or empty columns ignored: 0
csymamd: number of garbage collections performed: 0

BIN
gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example vendored
View File

Binary file not shown.

22
gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c vendored
View File

@ -1,12 +1,10 @@
/* ========================================================================== */
/* === ccolamd and csymamd example (UF_long integer version) ================ */
/* === ccolamd and csymamd example (long integer version) =================== */
/* ========================================================================== */
/* ----------------------------------------------------------------------------
* CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*
@ -46,9 +44,6 @@
#define B_NNZ 4
#define B_N 5
/* define UF_long */
#include "UFconfig.h"
int main (void)
{
@ -56,14 +51,14 @@ int main (void)
/* input matrix A definition */
/* ====================================================================== */
UF_long A [ALEN] = {
SuiteSparse_long A [ALEN] = {
0, 1, 4, /* row indices of nonzeros in column 0 */
2, 4, /* row indices of nonzeros in column 1 */
0, 1, 2, 3, /* row indices of nonzeros in column 2 */
1, 3} ; /* row indices of nonzeros in column 3 */
UF_long p [ ] = {
SuiteSparse_long p [ ] = {
0, /* column 0 is in A [0..2] */
3, /* column 1 is in A [3..4] */
@ -75,7 +70,7 @@ int main (void)
/* input matrix B definition */
/* ====================================================================== */
UF_long B [ ] = { /* Note: only strictly lower triangular part */
SuiteSparse_long B [ ] = { /* Note: only strictly lower triangular part */
/* is included, since symamd ignores the */
/* diagonal and upper triangular part of B. */
@ -85,7 +80,7 @@ int main (void)
4 /* row indices of nonzeros in column 3 */
} ; /* row indices of nonzeros in column 4 (none) */
UF_long q [ ] = {
SuiteSparse_long q [ ] = {
0, /* column 0 is in B [0] */
1, /* column 1 is in B [1..2] */
@ -98,10 +93,9 @@ int main (void)
/* other variable definitions */
/* ====================================================================== */
UF_long perm [B_N+1] ; /* note the size is N+1 */
UF_long stats [CCOLAMD_STATS] ; /* for ccolamd and csymamd output stats */
UF_long row, col, pp, length, ok ;
SuiteSparse_long perm [B_N+1] ; /* note the size is N+1 */
SuiteSparse_long stats [CCOLAMD_STATS] ; /* ccolamd/csymamd output stats */
SuiteSparse_long row, col, pp, length, ok ;
/* ====================================================================== */
/* dump the input matrix A */

4
gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out vendored
View File

@ -15,7 +15,7 @@ Column 3, with 2 entries:
row 1
row 3
ccolamd version 2.7, Jan 25, 2011: OK.
ccolamd version 2.9, May 4, 2016: OK.
ccolamd: number of dense or empty rows ignored: 0
ccolamd: number of dense or empty columns ignored: 0
ccolamd: number of garbage collections performed: 0
@ -38,7 +38,7 @@ Column 3, with 1 entries:
row 4
Column 4, with 0 entries:
csymamd version 2.7, Jan 25, 2011: OK.
csymamd version 2.9, May 4, 2016: OK.
csymamd: number of dense or empty rows ignored: 0
csymamd: number of dense or empty columns ignored: 0
csymamd: number of garbage collections performed: 0

50
gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_example.out vendored
View File

@ -1,50 +0,0 @@
ccolamd 5-by-4 input matrix:
Column 0, with 3 entries:
row 0
row 1
row 4
Column 1, with 2 entries:
row 2
row 4
Column 2, with 4 entries:
row 0
row 1
row 2
row 3
Column 3, with 2 entries:
row 1
row 3
ccolamd version 2.7, Jan 25, 2011: OK.
ccolamd: number of dense or empty rows ignored: 0
ccolamd: number of dense or empty columns ignored: 0
ccolamd: number of garbage collections performed: 0
ccolamd column ordering:
1st column: 1
2nd column: 0
3rd column: 3
4th column: 2
csymamd 5-by-5 input matrix:
Entries in strictly lower triangular part:
Column 0, with 1 entries:
row 1
Column 1, with 2 entries:
row 2
row 3
Column 2, with 0 entries:
Column 3, with 1 entries:
row 4
Column 4, with 0 entries:
csymamd version 2.7, Jan 25, 2011: OK.
csymamd: number of dense or empty rows ignored: 0
csymamd: number of dense or empty columns ignored: 0
csymamd: number of garbage collections performed: 0
csymamd column ordering:
1st row/column: 0
2nd row/column: 2
3rd row/column: 1
4th row/column: 3
5th row/column: 4

50
gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_l_example.out vendored
View File

@ -1,50 +0,0 @@
ccolamd 5-by-4 input matrix:
Column 0, with 3 entries:
row 0
row 1
row 4
Column 1, with 2 entries:
row 2
row 4
Column 2, with 4 entries:
row 0
row 1
row 2
row 3
Column 3, with 2 entries:
row 1
row 3
ccolamd version 2.7, Jan 25, 2011: OK.
ccolamd: number of dense or empty rows ignored: 0
ccolamd: number of dense or empty columns ignored: 0
ccolamd: number of garbage collections performed: 0
ccolamd_l column ordering:
1st column: 1
2nd column: 0
3rd column: 3
4th column: 2
csymamd_l 5-by-5 input matrix:
Entries in strictly lower triangular part:
Column 0, with 1 entries:
row 1
Column 1, with 2 entries:
row 2
row 3
Column 2, with 0 entries:
Column 3, with 1 entries:
row 4
Column 4, with 0 entries:
csymamd version 2.7, Jan 25, 2011: OK.
csymamd: number of dense or empty rows ignored: 0
csymamd: number of dense or empty columns ignored: 0
csymamd: number of garbage collections performed: 0
csymamd_l column ordering:
1st row/column: 0
2nd row/column: 2
3rd row/column: 1
4th row/column: 3
5th row/column: 4

38
gtsam/3rdparty/CCOLAMD/Doc/ChangeLog vendored
View File

@ -1,3 +1,41 @@
May 4, 2016: version 2.9.6
* minor changes to Makefile
Apr 1, 2016: version 2.9.5
* licensing simplified (no other change); refer to CCOLAMD/Doc/License.txt
Feb 1, 2016: version 2.9.4
* update to Makefiles
Jan 30, 2016: version 2.9.3
* modifications to Makefiles
Jan 1, 2016: version 2.9.2
* modified Makefile to create shared libraries
No change to C code except version number.
The empty file ccolamd_global.c removed.
Oct 10, 2014: version 2.9.1
modified MATLAB/ccolamd_make.m. No change to C code except version number.
July 31, 2013: version 2.9.0
* changed malloc and printf pointers to use SuiteSparse_config
Jun 1, 2012: version 2.8.0
* changed from UFconfig to SuiteSparse_config
Dec 7, 2011: version 2.7.4
* fixed the Makefile to better align with CFLAGS and other standards
Jan 25, 2011: version 2.7.3
* minor fix to "make install"

33
gtsam/3rdparty/CCOLAMD/Doc/License.txt vendored Normal file
View File

@ -0,0 +1,33 @@
CCOLAMD: constrained column approximate minimum degree ordering
Copyright (C) 2005-2016, Univ. of Florida. Authors: Timothy A. Davis,
Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by
Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert.
http://www.suitesparse.com
--------------------------------------------------------------------------------
CCOLAMD license: BSD 3-clause:
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the organizations to which the authors are
affiliated, nor the names of its contributors may be used to endorse
or promote products derived from this software without specific prior
written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
DAMAGE.

283
gtsam/3rdparty/CCOLAMD/Include/ccolamd.h vendored
View File

@ -5,8 +5,6 @@
/* ----------------------------------------------------------------------------
* CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*
@ -32,24 +30,24 @@ extern "C" {
/* All versions of CCOLAMD will include the following definitions.
* As an example, to test if the version you are using is 1.3 or later:
*
* if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ...
* if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ...
*
* This also works during compile-time:
*
* #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)
* printf ("This is version 1.3 or later\n") ;
* #else
* printf ("This is an early version\n") ;
* #endif
* #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)
* printf ("This is version 1.3 or later\n") ;
* #else
* printf ("This is an early version\n") ;
* #endif
*/
#define CCOLAMD_DATE "Jan 25, 2011"
#define CCOLAMD_DATE "May 4, 2016"
#define CCOLAMD_VERSION_CODE(main,sub) ((main) * 1000 + (sub))
#define CCOLAMD_MAIN_VERSION 2
#define CCOLAMD_SUB_VERSION 7
#define CCOLAMD_SUBSUB_VERSION 3
#define CCOLAMD_SUB_VERSION 9
#define CCOLAMD_SUBSUB_VERSION 6
#define CCOLAMD_VERSION \
CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION)
CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION)
/* ========================================================================== */
/* === Knob and statistics definitions ====================================== */
@ -94,106 +92,105 @@ extern "C" {
#define CCOLAMD_NEWLY_EMPTY_COL 10
/* error codes returned in stats [3]: */
#define CCOLAMD_OK (0)
#define CCOLAMD_OK_BUT_JUMBLED (1)
#define CCOLAMD_ERROR_A_not_present (-1)
#define CCOLAMD_ERROR_p_not_present (-2)
#define CCOLAMD_ERROR_nrow_negative (-3)
#define CCOLAMD_ERROR_ncol_negative (-4)
#define CCOLAMD_ERROR_nnz_negative (-5)
#define CCOLAMD_ERROR_p0_nonzero (-6)
#define CCOLAMD_ERROR_A_too_small (-7)
#define CCOLAMD_ERROR_col_length_negative (-8)
#define CCOLAMD_ERROR_row_index_out_of_bounds (-9)
#define CCOLAMD_ERROR_out_of_memory (-10)
#define CCOLAMD_ERROR_invalid_cmember (-11)
#define CCOLAMD_ERROR_internal_error (-999)
#define CCOLAMD_OK (0)
#define CCOLAMD_OK_BUT_JUMBLED (1)
#define CCOLAMD_ERROR_A_not_present (-1)
#define CCOLAMD_ERROR_p_not_present (-2)
#define CCOLAMD_ERROR_nrow_negative (-3)
#define CCOLAMD_ERROR_ncol_negative (-4)
#define CCOLAMD_ERROR_nnz_negative (-5)
#define CCOLAMD_ERROR_p0_nonzero (-6)
#define CCOLAMD_ERROR_A_too_small (-7)
#define CCOLAMD_ERROR_col_length_negative (-8)
#define CCOLAMD_ERROR_row_index_out_of_bounds (-9)
#define CCOLAMD_ERROR_out_of_memory (-10)
#define CCOLAMD_ERROR_invalid_cmember (-11)
#define CCOLAMD_ERROR_internal_error (-999)
/* ========================================================================== */
/* === Prototypes of user-callable routines ================================= */
/* ========================================================================== */
/* define UF_long */
#include "UFconfig.h"
#include "SuiteSparse_config.h"
size_t ccolamd_recommended /* returns recommended value of Alen, */
/* or 0 if input arguments are erroneous */
size_t ccolamd_recommended /* returns recommended value of Alen, */
/* or 0 if input arguments are erroneous */
(
int nnz, /* nonzeros in A */
int n_row, /* number of rows in A */
int n_col /* number of columns in A */
int nnz, /* nonzeros in A */
int n_row, /* number of rows in A */
int n_col /* number of columns in A */
) ;
size_t ccolamd_l_recommended /* returns recommended value of Alen, */
/* or 0 if input arguments are erroneous */
size_t ccolamd_l_recommended /* returns recommended value of Alen, */
/* or 0 if input arguments are erroneous */
(
UF_long nnz, /* nonzeros in A */
UF_long n_row, /* number of rows in A */
UF_long n_col /* number of columns in A */
SuiteSparse_long nnz, /* nonzeros in A */
SuiteSparse_long n_row, /* number of rows in A */
SuiteSparse_long n_col /* number of columns in A */
) ;
void ccolamd_set_defaults /* sets default parameters */
( /* knobs argument is modified on output */
double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */
void ccolamd_set_defaults /* sets default parameters */
( /* knobs argument is modified on output */
double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */
) ;
void ccolamd_l_set_defaults /* sets default parameters */
( /* knobs argument is modified on output */
double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */
void ccolamd_l_set_defaults /* sets default parameters */
( /* knobs argument is modified on output */
double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */
) ;
int ccolamd /* returns (1) if successful, (0) otherwise*/
( /* A and p arguments are modified on output */
int n_row, /* number of rows in A */
int n_col, /* number of columns in A */
int Alen, /* size of the array A */
int A [ ], /* row indices of A, of size Alen */
int p [ ], /* column pointers of A, of size n_col+1 */
int ccolamd /* returns (1) if successful, (0) otherwise*/
( /* A and p arguments are modified on output */
int n_row, /* number of rows in A */
int n_col, /* number of columns in A */
int Alen, /* size of the array A */
int A [ ], /* row indices of A, of size Alen */
int p [ ], /* column pointers of A, of size n_col+1 */
double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */
int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */
int cmember [ ] /* Constraint set of A, of size n_col */
int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */
int cmember [ ] /* Constraint set of A, of size n_col */
) ;
UF_long ccolamd_l /* same as ccolamd, but with UF_long integers */
SuiteSparse_long ccolamd_l /* as ccolamd w/ SuiteSparse_long integers */
(
UF_long n_row,
UF_long n_col,
UF_long Alen,
UF_long A [ ],
UF_long p [ ],
SuiteSparse_long n_row,
SuiteSparse_long n_col,
SuiteSparse_long Alen,
SuiteSparse_long A [ ],
SuiteSparse_long p [ ],
double knobs [CCOLAMD_KNOBS],
UF_long stats [CCOLAMD_STATS],
UF_long cmember [ ]
SuiteSparse_long stats [CCOLAMD_STATS],
SuiteSparse_long cmember [ ]
) ;
int csymamd /* return (1) if OK, (0) otherwise */
int csymamd /* return (1) if OK, (0) otherwise */
(
int n, /* number of rows and columns of A */
int A [ ], /* row indices of A */
int p [ ], /* column pointers of A */
int perm [ ], /* output permutation, size n_col+1 */
int n, /* number of rows and columns of A */
int A [ ], /* row indices of A */
int p [ ], /* column pointers of A */
int perm [ ], /* output permutation, size n_col+1 */
double knobs [CCOLAMD_KNOBS],/* parameters (uses defaults if NULL) */
int stats [CCOLAMD_STATS], /* output statistics and error codes */
int stats [CCOLAMD_STATS], /* output statistics and error codes */
void * (*allocate) (size_t, size_t), /* pointer to calloc (ANSI C) or */
/* mxCalloc (for MATLAB mexFunction) */
void (*release) (void *), /* pointer to free (ANSI C) or */
/* mxFree (for MATLAB mexFunction) */
int cmember [ ], /* Constraint set of A */
int stype /* 0: use both parts, >0: upper, <0: lower */
/* mxCalloc (for MATLAB mexFunction) */
void (*release) (void *), /* pointer to free (ANSI C) or */
/* mxFree (for MATLAB mexFunction) */
int cmember [ ], /* Constraint set of A */
int stype /* 0: use both parts, >0: upper, <0: lower */
) ;
UF_long csymamd_l /* same as csymamd, but with UF_long integers */
SuiteSparse_long csymamd_l /* as csymamd, w/ SuiteSparse_long integers */
(
UF_long n,
UF_long A [ ],
UF_long p [ ],
UF_long perm [ ],
SuiteSparse_long n,
SuiteSparse_long A [ ],
SuiteSparse_long p [ ],
SuiteSparse_long perm [ ],
double knobs [CCOLAMD_KNOBS],
UF_long stats [CCOLAMD_STATS],
SuiteSparse_long stats [CCOLAMD_STATS],
void * (*allocate) (size_t, size_t),
void (*release) (void *),
UF_long cmember [ ],
UF_long stype
SuiteSparse_long cmember [ ],
SuiteSparse_long stype
) ;
void ccolamd_report
@ -203,7 +200,7 @@ void ccolamd_report
void ccolamd_l_report
(
UF_long stats [CCOLAMD_STATS]
SuiteSparse_long stats [CCOLAMD_STATS]
) ;
void csymamd_report
@ -213,7 +210,7 @@ void csymamd_report
void csymamd_l_report
(
UF_long stats [CCOLAMD_STATS]
SuiteSparse_long stats [CCOLAMD_STATS]
) ;
@ -227,42 +224,42 @@ void csymamd_l_report
*/
int ccolamd2
( /* A and p arguments are modified on output */
int n_row, /* number of rows in A */
int n_col, /* number of columns in A */
int Alen, /* size of the array A */
int A [ ], /* row indices of A, of size Alen */
int p [ ], /* column pointers of A, of size n_col+1 */
( /* A and p arguments are modified on output */
int n_row, /* number of rows in A */
int n_col, /* number of columns in A */
int Alen, /* size of the array A */
int A [ ], /* row indices of A, of size Alen */
int p [ ], /* column pointers of A, of size n_col+1 */
double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */
int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */
int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */
/* each Front_ array is of size n_col+1: */
int Front_npivcol [ ], /* # pivot cols in each front */
int Front_nrows [ ], /* # of rows in each front (incl. pivot rows) */
int Front_ncols [ ], /* # of cols in each front (incl. pivot cols) */
int Front_parent [ ], /* parent of each front */
int Front_cols [ ], /* link list of pivot columns for each front */
int *p_nfr, /* total number of frontal matrices */
int InFront [ ], /* InFront [row] = f if row in front f */
int cmember [ ] /* Constraint set of A */
int Front_npivcol [ ], /* # pivot cols in each front */
int Front_nrows [ ], /* # of rows in each front (incl. pivot rows) */
int Front_ncols [ ], /* # of cols in each front (incl. pivot cols) */
int Front_parent [ ], /* parent of each front */
int Front_cols [ ], /* link list of pivot columns for each front */
int *p_nfr, /* total number of frontal matrices */
int InFront [ ], /* InFront [row] = f if row in front f */
int cmember [ ] /* Constraint set of A */
) ;
UF_long ccolamd2_l /* same as ccolamd2, but with UF_long integers */
SuiteSparse_long ccolamd2_l /* as ccolamd2, w/ SuiteSparse_long integers */
(
UF_long n_row,
UF_long n_col,
UF_long Alen,
UF_long A [ ],
UF_long p [ ],
SuiteSparse_long n_row,
SuiteSparse_long n_col,
SuiteSparse_long Alen,
SuiteSparse_long A [ ],
SuiteSparse_long p [ ],
double knobs [CCOLAMD_KNOBS],
UF_long stats [CCOLAMD_STATS],
UF_long Front_npivcol [ ],
UF_long Front_nrows [ ],
UF_long Front_ncols [ ],
UF_long Front_parent [ ],
UF_long Front_cols [ ],
UF_long *p_nfr,
UF_long InFront [ ],
UF_long cmember [ ]
SuiteSparse_long stats [CCOLAMD_STATS],
SuiteSparse_long Front_npivcol [ ],
SuiteSparse_long Front_nrows [ ],
SuiteSparse_long Front_ncols [ ],
SuiteSparse_long Front_parent [ ],
SuiteSparse_long Front_cols [ ],
SuiteSparse_long *p_nfr,
SuiteSparse_long InFront [ ],
SuiteSparse_long cmember [ ]
) ;
void ccolamd_apply_order
@ -276,11 +273,11 @@ void ccolamd_apply_order
void ccolamd_l_apply_order
(
UF_long Front [ ],
const UF_long Order [ ],
UF_long Temp [ ],
UF_long nn,
UF_long nfr
SuiteSparse_long Front [ ],
const SuiteSparse_long Order [ ],
SuiteSparse_long Temp [ ],
SuiteSparse_long nn,
SuiteSparse_long nfr
) ;
@ -296,12 +293,12 @@ void ccolamd_fsize
void ccolamd_l_fsize
(
UF_long nn,
UF_long MaxFsize [ ],
UF_long Fnrows [ ],
UF_long Fncols [ ],
UF_long Parent [ ],
UF_long Npiv [ ]
SuiteSparse_long nn,
SuiteSparse_long MaxFsize [ ],
SuiteSparse_long Fnrows [ ],
SuiteSparse_long Fncols [ ],
SuiteSparse_long Parent [ ],
SuiteSparse_long Npiv [ ]
) ;
void ccolamd_postorder
@ -320,16 +317,16 @@ void ccolamd_postorder
void ccolamd_l_postorder
(
UF_long nn,
UF_long Parent [ ],
UF_long Npiv [ ],
UF_long Fsize [ ],
UF_long Order [ ],
UF_long Child [ ],
UF_long Sibling [ ],
UF_long Stack [ ],
UF_long Front_cols [ ],
UF_long cmember [ ]
SuiteSparse_long nn,
SuiteSparse_long Parent [ ],
SuiteSparse_long Npiv [ ],
SuiteSparse_long Fsize [ ],
SuiteSparse_long Order [ ],
SuiteSparse_long Child [ ],
SuiteSparse_long Sibling [ ],
SuiteSparse_long Stack [ ],
SuiteSparse_long Front_cols [ ],
SuiteSparse_long cmember [ ]
) ;
int ccolamd_post_tree
@ -342,22 +339,16 @@ int ccolamd_post_tree
int Stack [ ]
) ;
UF_long ccolamd_l_post_tree
SuiteSparse_long ccolamd_l_post_tree
(
UF_long root,
UF_long k,
UF_long Child [ ],
const UF_long Sibling [ ],
UF_long Order [ ],
UF_long Stack [ ]
SuiteSparse_long root,
SuiteSparse_long k,
SuiteSparse_long Child [ ],
const SuiteSparse_long Sibling [ ],
SuiteSparse_long Order [ ],
SuiteSparse_long Stack [ ]
) ;
#ifndef EXTERN
#define EXTERN extern
#endif
EXTERN int (*ccolamd_printf) (const char *, ...) ;
#ifdef __cplusplus
}
#endif

73
gtsam/3rdparty/CCOLAMD/Lib/Makefile vendored Normal file
View File

@ -0,0 +1,73 @@
#-------------------------------------------------------------------------------
# CCOLAMD Lib/Makefile
#-------------------------------------------------------------------------------
LIBRARY = libccolamd
VERSION = 2.9.6
SO_VERSION = 2
default: library
include ../../SuiteSparse_config/SuiteSparse_config.mk
# CCOLAMD depends on SuiteSparse_config
LDLIBS += -lsuitesparseconfig
# compile and install in SuiteSparse/lib
library:
$(MAKE) install INSTALL=$(SUITESPARSE)
I = -I../Include -I../../SuiteSparse_config
INC = ../Include/ccolamd.h ../../SuiteSparse_config/SuiteSparse_config.h
SRC = ../Source/ccolamd.c
OBJ = ccolamd.o ccolamd_l.o
ccolamd.o: $(SRC) $(INC)
$(CC) $(CF) $(I) -c ../Source/ccolamd.c
ccolamd_l.o: $(SRC) $(INC)
$(CC) $(CF) $(I) -c ../Source/ccolamd.c -DDLONG -o ccolamd_l.o
# creates libccolamd.a, a C-callable CCOLAMD library
static: $(AR_TARGET)
$(AR_TARGET): $(OBJ)
$(ARCHIVE) $@ $^
- $(RANLIB) $@
ccode: library
clean:
- $(RM) -r $(CLEAN)
purge: distclean
distclean: clean
- $(RM) -r $(PURGE)
# install CCOLAMD
install: $(AR_TARGET) $(INSTALL_LIB)/$(SO_TARGET)
$(INSTALL_LIB)/$(SO_TARGET): $(OBJ)
@mkdir -p $(INSTALL_LIB)
@mkdir -p $(INSTALL_INCLUDE)
@mkdir -p $(INSTALL_DOC)
$(CC) $(SO_OPTS) $^ -o $@ $(LDLIBS)
( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_PLAIN) )
( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_MAIN) )
$(CP) ../Include/ccolamd.h $(INSTALL_INCLUDE)
$(CP) ../README.txt $(INSTALL_DOC)/CCOLAMD_README.txt
chmod 755 $(INSTALL_LIB)/$(SO_TARGET)
chmod 644 $(INSTALL_INCLUDE)/ccolamd.h
chmod 644 $(INSTALL_DOC)/CCOLAMD_README.txt
uninstall:
$(RM) $(INSTALL_LIB)/$(SO_TARGET)
$(RM) $(INSTALL_LIB)/$(SO_PLAIN)
$(RM) $(INSTALL_LIB)/$(SO_MAIN)
$(RM) $(INSTALL_INCLUDE)/ccolamd.h
$(RM) $(INSTALL_DOC)/CCOLAMD_README.txt

BIN
gtsam/3rdparty/CCOLAMD/Lib/libccolamd.a vendored
View File

Binary file not shown.

29
gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m vendored
View File

@ -47,9 +47,8 @@ spparms ('default') ;
A = sprandn (n, n, 2/n) + speye (n) ;
b = (1:n)' ;
figure (1)
clf ;
subplot (2,2,1)
subplot (3,4,1)
spy (A)
title ('original matrix')
@ -62,7 +61,7 @@ fl = luflops (L, U) ;
x = Q * (U \ (L \ (P * b))) ;
fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ;
fprintf (1, 'residual: %e\n', norm (A*x-b));
subplot (2,2,2) ;
subplot (3,4,2) ;
spy (L|U) ;
title ('LU with ccolamd') ;
@ -76,7 +75,7 @@ fl = luflops (L, U) ;
x = Q * (U \ (L \ (P * b))) ;
fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ;
fprintf (1, 'residual: %e\n', norm (A*x-b));
subplot (2,2,3) ;
subplot (3,4,3) ;
spy (L|U) ;
title ('LU with colamd') ;
catch
@ -89,7 +88,7 @@ fl = luflops (L, U) ;
x = U \ (L \ (P * b)) ;
fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ;
fprintf (1, 'residual: %e\n', norm (A*x-b));
subplot (2,2,4) ;
subplot (3,4,4) ;
spy (L|U) ;
title ('LU with no ordering') ;
@ -111,9 +110,7 @@ n = 1000 ;
fprintf (1, 'Generating a random %d-by-%d sparse matrix.\n', n, n) ;
A = sprandn (n, n, 2/n) + speye (n) ;
figure (2)
clf ;
subplot (2,2,1)
subplot (3,4,5)
spy (A)
title ('original matrix')
@ -121,7 +118,7 @@ fprintf (1, '\n\nUnordered matrix:\n') ;
[lnz,h,parent,post,R] = symbfact (A, 'col') ;
fprintf (1, 'nz in Cholesky factors of A''A: %d\n', sum (lnz)) ;
fprintf (1, 'flop count for Cholesky of A''A: %d\n', sum (lnz.^2)) ;
subplot (2,2,4) ;
subplot (3,4,6) ;
spy (R) ;
title ('Cholesky with no ordering') ;
@ -133,7 +130,7 @@ fprintf (1, '\n\nccolamd run time: %f\n', t) ;
fprintf (1, 'ccolamd ordering quality: \n') ;
fprintf (1, 'nz in Cholesky factors of A(:,p)''A(:,p): %d\n', sum (lnz)) ;
fprintf (1, 'flop count for Cholesky of A(:,p)''A(:,p): %d\n', sum (lnz.^2)) ;
subplot (2,2,2) ;
subplot (3,4,7) ;
spy (R) ;
title ('Cholesky with ccolamd') ;
@ -146,7 +143,7 @@ fprintf (1, '\n\ncolamd run time: %f\n', t) ;
fprintf (1, 'colamd ordering quality: \n') ;
fprintf (1, 'nz in Cholesky factors of A(:,p)''A(:,p): %d\n', sum (lnz)) ;
fprintf (1, 'flop count for Cholesky of A(:,p)''A(:,p): %d\n', sum (lnz.^2)) ;
subplot (2,2,3) ;
subplot (3,4,8) ;
spy (R) ;
title ('Cholesky with colamd') ;
catch
@ -164,9 +161,7 @@ fprintf (1, '\n-----------------------------------------------------------\n') ;
fprintf (1, 'Generating a random symmetric %d-by-%d sparse matrix.\n', n, n) ;
A = A+A' ;
figure (3)
clf ;
subplot (2,2,1)
subplot (3,4,9) ;
spy (A)
title ('original matrix')
@ -174,7 +169,7 @@ fprintf (1, '\n\nUnordered matrix:\n') ;
[lnz,h,parent,post,R] = symbfact (A, 'sym') ;
fprintf (1, 'nz in Cholesky factors of A: %d\n', sum (lnz)) ;
fprintf (1, 'flop count for Cholesky of A: %d\n', sum (lnz.^2)) ;
subplot (2,2,4) ;
subplot (3,4,10) ;
spy (R) ;
title ('Cholesky with no ordering') ;
@ -186,7 +181,7 @@ fprintf (1, '\n\ncsymamd run time: %f\n', t) ;
fprintf (1, 'csymamd ordering quality: \n') ;
fprintf (1, 'nz in Cholesky factors of A(p,p): %d\n', sum (lnz)) ;
fprintf (1, 'flop count for Cholesky of A(p,p): %d\n', sum (lnz.^2)) ;
subplot (2,2,2) ;
subplot (3,4,11) ;
spy (R) ;
title ('Cholesky with csymamd') ;
@ -199,7 +194,7 @@ fprintf (1, '\n\nsymamd run time: %f\n', t) ;
fprintf (1, 'symamd ordering quality: \n') ;
fprintf (1, 'nz in Cholesky factors of A(p,p): %d\n', sum (lnz)) ;
fprintf (1, 'flop count for Cholesky of A(p,p): %d\n', sum (lnz.^2)) ;
subplot (2,2,3) ;
subplot (3,4,12) ;
spy (R) ;
title ('Cholesky with symamd') ;
catch

23
gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m vendored
View File

@ -14,14 +14,33 @@ d = '' ;
if (~isempty (strfind (computer, '64')))
d = '-largeArrayDims' ;
end
src = '../Source/ccolamd.c ../Source/ccolamd_global.c' ;
cmd = sprintf ('mex -DDLONG -O %s -I../../UFconfig -I../Include -output ', d) ;
% MATLAB 8.3.0 now has a -silent option to keep 'mex' from burbling too much
if (~verLessThan ('matlab', '8.3.0'))
d = ['-silent ' d] ;
end
src = '../Source/ccolamd.c ../../SuiteSparse_config/SuiteSparse_config.c' ;
cmd = sprintf ( ...
'mex -DDLONG -O %s -I../../SuiteSparse_config -I../Include -output ', d) ;
s = [cmd 'ccolamd ccolamdmex.c ' src] ;
if (~(ispc || ismac))
% for POSIX timing routine
s = [s ' -lrt'] ;
end
if (details)
fprintf ('%s\n', s) ;
end
eval (s) ;
s = [cmd 'csymamd csymamdmex.c ' src] ;
if (~(ispc || ismac))
% for POSIX timing routine
s = [s ' -lrt'] ;
end
if (details)
fprintf ('%s\n', s) ;
end

9
gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m vendored
View File

@ -22,8 +22,13 @@ csymamd_default_knobs = [10 1 0] ;
if (~isempty (strfind (computer, '64')))
d = '-largeArrayDims' ;
end
src = '../Source/ccolamd.c ../Source/ccolamd_global.c' ;
cmd = sprintf ('mex -DDLONG -O %s -I../../UFconfig -I../Include ', d) ;
cmd = sprintf ( ...
'mex -DDLONG -O %s -I../../SuiteSparse_config -I../Include ', d) ;
src = '../Source/ccolamd.c ../../SuiteSparse_config/SuiteSparse_config.c' ;
if (~(ispc || ismac))
% for POSIX timing routine
src = [src ' -lrt'] ;
end
eval ([cmd 'ccolamdtestmex.c ' src]) ;
eval ([cmd 'csymamdtestmex.c ' src]) ;
fprintf ('Done compiling.\n') ;

52
gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c vendored
View File

@ -5,8 +5,6 @@
/* ----------------------------------------------------------------------------
* CCOLAMD, Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*
@ -26,7 +24,7 @@
#include "matrix.h"
#include <stdlib.h>
#include <string.h>
#include "UFconfig.h"
#define Long SuiteSparse_long
/* ========================================================================== */
/* === ccolamd mexFunction ================================================== */
@ -44,24 +42,24 @@ void mexFunction
{
/* === Local variables ================================================== */
UF_long *A ; /* ccolamd's copy of the matrix and workspace */
UF_long *cmember ; /* ccolamd's copy of the constraint set */
double *in_cmember ; /* input constraint set */
UF_long *p ; /* ccolamd's copy of the column pointers */
UF_long Alen ; /* size of A */
UF_long cslen ; /* size of CS */
UF_long n_col ; /* number of columns of A */
UF_long n_row ; /* number of rows of A */
UF_long nnz ; /* number of entries in A */
UF_long full ; /* TRUE if input matrix full, FALSE if sparse */
Long *A ; /* ccolamd's copy of the matrix and workspace */
Long *cmember ; /* ccolamd's copy of the constraint set */
double *in_cmember ; /* input constraint set */
Long *p ; /* ccolamd's copy of the column pointers */
Long Alen ; /* size of A */
Long cslen ; /* size of CS */
Long n_col ; /* number of columns of A */
Long n_row ; /* number of rows of A */
Long nnz ; /* number of entries in A */
Long full ; /* TRUE if input matrix full, FALSE if sparse */
double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
UF_long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
UF_long spumoni ; /* verbosity variable */
UF_long stats [CCOLAMD_STATS] ; /* stats for ccolamd */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
Long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
Long spumoni ; /* verbosity variable */
Long stats [CCOLAMD_STATS] ;/* stats for ccolamd */
/* === Check inputs ===================================================== */
@ -80,11 +78,11 @@ void mexFunction
cslen = mxGetNumberOfElements (pargin [2]) ;
if (cslen != 0)
{
cmember = (UF_long *) mxCalloc (cslen, sizeof (UF_long)) ;
cmember = (Long *) mxCalloc (cslen, sizeof (Long)) ;
for (i = 0 ; i < cslen ; i++)
{
/* convert cmember from 1-based to 0-based */
cmember[i] = ((UF_long) in_cmember [i] - 1) ;
cmember[i] = ((Long) in_cmember [i] - 1) ;
}
}
}
@ -157,10 +155,10 @@ void mexFunction
n_col = mxGetN (Ainput) ;
/* get column pointer vector */
p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
(void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ;
p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
(void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ;
nnz = p [n_col] ;
Alen = (UF_long) ccolamd_l_recommended (nnz, n_row, n_col) ;
Alen = (Long) ccolamd_l_recommended (nnz, n_row, n_col) ;
if (Alen == 0)
{
mexErrMsgTxt ("ccolamd: problem too large") ;
@ -168,8 +166,8 @@ void mexFunction
/* === Copy input matrix into workspace ================================= */
A = (UF_long *) mxCalloc (Alen, sizeof (UF_long)) ;
(void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ;
A = (Long *) mxCalloc (Alen, sizeof (Long)) ;
(void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ;
if (full)
{

68
gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c vendored
View File

@ -5,8 +5,6 @@
/* ----------------------------------------------------------------------------
* CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*
@ -43,7 +41,7 @@
#include "matrix.h"
#include <stdlib.h>
#include <string.h>
#include "UFconfig.h"
#define Long SuiteSparse_long
/* Here only for testing */
#undef MIN
@ -61,15 +59,15 @@
static void dump_matrix
(
UF_long A [ ],
UF_long p [ ],
UF_long n_row,
UF_long n_col,
UF_long Alen,
UF_long limit
Long A [ ],
Long p [ ],
Long n_row,
Long n_col,
Long Alen,
Long limit
)
{
UF_long col, k, row ;
Long col, k, row ;
mexPrintf ("dump matrix: nrow %d ncol %d Alen %d\n", n_row, n_col, Alen) ;
@ -102,24 +100,24 @@ void mexFunction
{
/* === Local variables ================================================== */
UF_long *A ; /* ccolamd's copy of the matrix and workspace */
UF_long *p ; /* ccolamd's copy of the column pointers */
UF_long Alen ; /* size of A */
UF_long n_col ; /* number of columns of A */
UF_long n_row ; /* number of rows of A */
UF_long nnz ; /* number of entries in A */
UF_long full ; /* TRUE if input matrix full, FALSE if sparse */
Long *A ; /* ccolamd's copy of the matrix and workspace */
Long *p ; /* ccolamd's copy of the column pointers */
Long Alen ; /* size of A */
Long n_col ; /* number of columns of A */
Long n_row ; /* number of rows of A */
Long nnz ; /* number of entries in A */
Long full ; /* TRUE if input matrix full, FALSE if sparse */
double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
UF_long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
UF_long spumoni ; /* verbosity variable */
UF_long stats2 [CCOLAMD_STATS] ; /* stats for ccolamd */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
Long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
Long spumoni ; /* verbosity variable */
Long stats2 [CCOLAMD_STATS] ; /* stats for ccolamd */
UF_long *cp, *cp_end, result, col, length, ok ;
UF_long *stats ;
Long *cp, *cp_end, result, col, length, ok ;
Long *stats ;
stats = stats2 ;
/* === Check inputs ===================================================== */
@ -199,10 +197,10 @@ void mexFunction
n_col = mxGetN (Ainput) ;
/* get column pointer vector so we can find nnz */
p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
(void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ;
p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
(void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ;
nnz = p [n_col] ;
Alen = (UF_long) ccolamd_l_recommended (nnz, n_row, n_col) ;
Alen = (Long) ccolamd_l_recommended (nnz, n_row, n_col) ;
if (Alen == 0)
{
mexErrMsgTxt ("ccolamd: problem too large") ;
@ -230,8 +228,8 @@ void mexFunction
/* === Copy input matrix into workspace ================================= */
A = (UF_long *) mxCalloc (Alen, sizeof (UF_long)) ;
(void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ;
A = (Long *) mxCalloc (Alen, sizeof (Long)) ;
(void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ;
if (full)
{
@ -261,7 +259,7 @@ void mexFunction
*/
/* jumble appropriately */
switch ((UF_long) in_knobs [6])
switch ((Long) in_knobs [6])
{
case 0 :
@ -359,7 +357,7 @@ void mexFunction
mexPrintf ("ccolamdtest: A not present\n") ;
}
result = 0 ; /* A not present */
A = (UF_long *) NULL ;
A = (Long *) NULL ;
break ;
case 8 :
@ -368,7 +366,7 @@ void mexFunction
mexPrintf ("ccolamdtest: p not present\n") ;
}
result = 0 ; /* p not present */
p = (UF_long *) NULL ;
p = (Long *) NULL ;
break ;
case 9 :
@ -456,7 +454,7 @@ void mexFunction
mexPrintf ("ccolamdtest: stats not present\n") ;
}
result = 0 ; /* stats not present */
stats = (UF_long *) NULL ;
stats = (Long *) NULL ;
break ;
case 13 :

4
gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m vendored
View File

@ -34,10 +34,10 @@ function [p, stats] = csymamd (S, knobs, cmember) %#ok
% p = csymamd(S) is about the same as p = symamd(S). knobs and its default
% values differ.
%
% Authors: S. Larimore, T. Davis (Univ of Florida), and S. Rajamanickam, in
% Authors: S. Larimore, T. Davis, and S. Rajamanickam, in
% collaboration with J. Gilbert and E. Ng. Supported by the National
% Science Foundation (DMS-9504974, DMS-9803599, CCR-0203270), and a grant
% from Sandia National Lab. See http://www.cise.ufl.edu/research/sparse
% from Sandia National Lab. See http://www.suitesparse.com
% for ccolamd, csymamd, amd, colamd, symamd, and other related orderings.
%
% See also AMD, CCOLAMD, COLAMD, SYMAMD, SYMRCM.

46
gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c vendored
View File

@ -5,8 +5,6 @@
/* ----------------------------------------------------------------------------
* CCOLAMD, Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*
@ -25,7 +23,7 @@
#include "mex.h"
#include "matrix.h"
#include <stdlib.h>
#include "UFconfig.h"
#define Long SuiteSparse_long
/* ========================================================================== */
/* === csymamd mexFunction ================================================== */
@ -43,23 +41,23 @@ void mexFunction
{
/* === Local variables ================================================== */
UF_long *A ; /* row indices of input matrix A */
UF_long *perm ; /* column ordering of M and ordering of A */
UF_long *cmember ; /* csymamd's copy of the constraint set */
double *in_cmember ; /* input constraint set */
UF_long *p ; /* column pointers of input matrix A */
UF_long cslen ; /* size of constraint set */
UF_long n_col ; /* number of columns of A */
UF_long n_row ; /* number of rows of A */
UF_long full ; /* TRUE if input matrix full, FALSE if sparse */
Long *A ; /* row indices of input matrix A */
Long *perm ; /* column ordering of M and ordering of A */
Long *cmember ; /* csymamd's copy of the constraint set */
double *in_cmember ; /* input constraint set */
Long *p ; /* column pointers of input matrix A */
Long cslen ; /* size of constraint set */
Long n_col ; /* number of columns of A */
Long n_row ; /* number of rows of A */
Long full ; /* TRUE if input matrix full, FALSE if sparse */
double knobs [CCOLAMD_KNOBS] ; /* csymamd user-controllable parameters */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
UF_long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
UF_long spumoni ; /* verbosity variable */
UF_long stats [CCOLAMD_STATS] ; /* stats for symamd */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
Long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
Long spumoni ; /* verbosity variable */
Long stats [CCOLAMD_STATS] ;/* stats for symamd */
/* === Check inputs ===================================================== */
@ -78,11 +76,11 @@ void mexFunction
cslen = mxGetNumberOfElements (pargin [2]) ;
if (cslen != 0)
{
cmember = (UF_long *) mxCalloc (cslen, sizeof (UF_long)) ;
cmember = (Long *) mxCalloc (cslen, sizeof (Long)) ;
for (i = 0 ; i < cslen ; i++)
{
/* convert cmember from 1-based to 0-based */
cmember[i] = ((UF_long) in_cmember [i] - 1) ;
cmember[i] = ((Long) in_cmember [i] - 1) ;
}
}
}
@ -153,9 +151,9 @@ void mexFunction
mexErrMsgTxt ("csymamd: cmember must be of length equal to #cols of A");
}
A = (UF_long *) mxGetIr (Ainput) ;
p = (UF_long *) mxGetJc (Ainput) ;
perm = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
A = (Long *) mxGetIr (Ainput) ;
p = (Long *) mxGetJc (Ainput) ;
perm = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
/* === Order the rows and columns of A (does not destroy A) ============= */

66
gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c vendored
View File

@ -5,8 +5,6 @@
/* ----------------------------------------------------------------------------
* CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*
@ -37,7 +35,7 @@
#include "matrix.h"
#include <stdlib.h>
#include <string.h>
#include "UFconfig.h"
#define Long SuiteSparse_long
#ifdef MIN
#undef MIN
@ -47,15 +45,15 @@
static void dump_matrix
(
UF_long A [ ],
UF_long p [ ],
UF_long n_row,
UF_long n_col,
UF_long Alen,
UF_long limit
Long A [ ],
Long p [ ],
Long n_row,
Long n_col,
Long Alen,
Long limit
)
{
UF_long col, k, row ;
Long col, k, row ;
mexPrintf ("dump matrix: nrow %d ncol %d Alen %d\n", n_row, n_col, Alen) ;
@ -100,23 +98,23 @@ void mexFunction
{
/* === Local variables ================================================== */
UF_long *perm ; /* column ordering of M and ordering of A */
UF_long *A ; /* row indices of input matrix A */
UF_long *p ; /* column pointers of input matrix A */
UF_long n_col ; /* number of columns of A */
UF_long n_row ; /* number of rows of A */
UF_long full ; /* TRUE if input matrix full, FALSE if sparse */
Long *perm ; /* column ordering of M and ordering of A */
Long *A ; /* row indices of input matrix A */
Long *p ; /* column pointers of input matrix A */
Long n_col ; /* number of columns of A */
Long n_row ; /* number of rows of A */
Long full ; /* TRUE if input matrix full, FALSE if sparse */
double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
UF_long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
UF_long spumoni ; /* verbosity variable */
UF_long stats2 [CCOLAMD_STATS] ;/* stats for csymamd */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
Long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
Long spumoni ; /* verbosity variable */
Long stats2 [CCOLAMD_STATS] ;/* stats for csymamd */
UF_long *cp, *cp_end, result, nnz, col, length, ok ;
UF_long *stats ;
Long *cp, *cp_end, result, nnz, col, length, ok ;
Long *stats ;
stats = stats2 ;
/* === Check inputs ===================================================== */
@ -192,8 +190,8 @@ void mexFunction
}
/* p = mxGetJc (Ainput) ; */
p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
(void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ;
p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
(void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ;
nnz = p [n_col] ;
if (spumoni)
@ -202,10 +200,10 @@ void mexFunction
}
/* A = mxGetIr (Ainput) ; */
A = (UF_long *) mxCalloc (nnz+1, sizeof (UF_long)) ;
(void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ;
A = (Long *) mxCalloc (nnz+1, sizeof (Long)) ;
(void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ;
perm = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
perm = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
/* === Jumble matrix ==================================================== */
@ -230,7 +228,7 @@ void mexFunction
*/
/* jumble appropriately */
switch ((UF_long) in_knobs [3])
switch ((Long) in_knobs [3])
{
case 0 :
@ -321,7 +319,7 @@ void mexFunction
mexPrintf ("csymamdtest: A not present\n") ;
}
result = 0 ; /* A not present */
A = (UF_long *) NULL ;
A = (Long *) NULL ;
break ;
case 8 :
@ -330,7 +328,7 @@ void mexFunction
mexPrintf ("csymamdtest: p not present\n") ;
}
result = 0 ; /* p not present */
p = (UF_long *) NULL ;
p = (Long *) NULL ;
break ;
case 9 :
@ -418,7 +416,7 @@ void mexFunction
mexPrintf ("csymamdtest: stats not present\n") ;
}
result = 0 ; /* stats not present */
stats = (UF_long *) NULL ;
stats = (Long *) NULL ;
break ;
case 13 :

55
gtsam/3rdparty/CCOLAMD/Makefile vendored Normal file
View File

@ -0,0 +1,55 @@
#------------------------------------------------------------------------------
# CCOLAMD Makefile
#------------------------------------------------------------------------------
SUITESPARSE ?= $(realpath $(CURDIR)/..)
export SUITESPARSE
default: all
include ../SuiteSparse_config/SuiteSparse_config.mk
demos: all
# Compile all C code
all:
( cd Lib ; $(MAKE) )
( cd Demo ; $(MAKE) )
# compile just the C-callable libraries (not Demos)
library:
( cd Lib ; $(MAKE) )
# compile the static libraries only
static:
( cd Lib ; $(MAKE) static )
# remove object files, but keep the compiled programs and library archives
clean:
( cd Lib ; $(MAKE) clean )
( cd Demo ; $(MAKE) clean )
( cd MATLAB ; $(RM) $(CLEAN) )
# clean, and then remove compiled programs and library archives
purge:
( cd Lib ; $(MAKE) purge )
( cd Demo ; $(MAKE) purge )
( cd MATLAB ; $(RM) $(CLEAN) ; $(RM) *.mex* )
distclean: purge
# get ready for distribution
dist: purge
( cd Demo ; $(MAKE) dist )
ccode: library
lib: library
# install CCOLAMD
install:
( cd Lib ; $(MAKE) install )
# uninstall CCOLAMD
uninstall:
( cd Lib ; $(MAKE) uninstall )

58
gtsam/3rdparty/CCOLAMD/README.txt vendored
View File

@ -1,8 +1,8 @@
CCOLAMD: constrained column approximate minimum degree ordering
Copyright (C) 2005-2011, Univ. of Florida. Authors: Timothy A. Davis,
Copyright (C) 2005-2016, Univ. of Florida. Authors: Timothy A. Davis,
Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by
Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert.
http://www.cise.ufl.edu/research/sparse
http://www.suitesparse.com
-------------------------------------------------------------------------------
The CCOLAMD column approximate minimum degree ordering algorithm computes
@ -14,7 +14,8 @@ available as a MATLAB-callable function. It constructs a matrix M such
that M'*M has the same pattern as A, and then uses CCOLAMD to compute a column
ordering of M.
Requires UFconfig, in the ../UFconfig directory relative to this directory.
Requires SuiteSparse_config, in the ../SuiteSparse_config directory relative to
this directory.
To compile and install the ccolamd m-files and mexFunctions, just cd to
CCOLAMD/MATLAB and type ccolamd_install in the MATLAB command window.
@ -22,47 +23,27 @@ A short demo will run. Optionally, type ccolamd_test to run an extensive tests.
Type "make" in Unix in the CCOLAMD directory to compile the C-callable
library and to run a short demo.
If you have MATLAB 7.2 or earlier, you must first edit UFconfig/UFconfig.h to
remove the "-largeArrayDims" option from the MEX command (or just use
ccolamd_install.m inside MATLAB).
Other "make" targets:
make mex compiles MATLAB mexFunctions only
make libccolamd.a compiles a C-callable library containing ccolamd
make clean removes all files not in the distribution, except for
libccolamd.a
make library compiles a C-callable library containing ccolamd
make clean removes all files not in the distribution
but keeps the compiled libraries.
make distclean removes all files not in the distribution
make install installs the library in /usr/local/lib and
/usr/local/include
make uninstall uninstalls the library from /usr/local/lib and
/usr/local/include
To use ccolamd and csymamd within an application written in C, all you need are
ccolamd.c and ccolamd.h, which are the C-callable ccolamd/csymamd codes.
See ccolamd.c for more information on how to call ccolamd from a C program.
It contains a complete description of the C-interface to CCOLAMD and CSYMAMD.
Copyright (c) 1998-2007 by the University of Florida.
All Rights Reserved.
Licensed under the GNU LESSER GENERAL PUBLIC LICENSE.
See CCOLAMD/Doc/License.txt for the license.
-------------------------------------------------------------------------------
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
-------------------------------------------------------------------------------
Related papers:
T. A. Davis and W. W. Hager, Rajamanickam, Multiple-rank updates
@ -86,21 +67,18 @@ Related papers:
"An approximate minimum degree column ordering algorithm",
S. I. Larimore, MS Thesis, Dept. of Computer and Information
Science and Engineering, University of Florida, Gainesville, FL,
1998. CISE Tech Report TR-98-016. Available at
ftp://ftp.cise.ufl.edu/cis/tech-reports/tr98/tr98-016.ps
via anonymous ftp.
1998. CISE Tech Report TR-98-016.
Approximate Deficiency for Ordering the Columns of a Matrix,
J. L. Kern, Senior Thesis, Dept. of Computer and Information
Science and Engineering, University of Florida, Gainesville, FL,
1999. Available at http://www.cise.ufl.edu/~davis/Kern/kern.ps
1999.
Authors: Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore.
Closely based on COLAMD by Stefan I. Larimore and Timothy A. Davis,
University of Florida, in collaboration with John Gilbert, Xerox PARC
(now at UC Santa Barbara), and Esmong Ng, Lawrence Berkeley National
Laboratory (much of this work he did while at Oak Ridge National
Laboratory).
in collaboration with John Gilbert, Xerox PARC (now at UC Santa
Barbara), and Esmong Ng, Lawrence Berkeley National Laboratory (much of
this work he did while at Oak Ridge National Laboratory).
CCOLAMD files:
@ -122,7 +100,6 @@ CCOLAMD files:
./Doc:
ChangeLog change log
lesser.txt license
./Include:
ccolamd.h include file
@ -147,4 +124,3 @@ CCOLAMD files:
./Source:
ccolamd.c primary source code
ccolamd_global.c globally defined function pointers (malloc, free, ...)

180
gtsam/3rdparty/CCOLAMD/Source/ccolamd.c vendored
View File

@ -5,8 +5,6 @@
/* ----------------------------------------------------------------------------
* CCOLAMD, Copyright (C) Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*
@ -58,39 +56,13 @@
* COLAMD is also available under alternate licenses, contact T. Davis
* for details.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
* USA
*
* Permission is hereby granted to use or copy this program under the
* terms of the GNU LGPL, provided that the Copyright, this License,
* and the Availability of the original version is retained on all copies.
* User documentation of any code that uses this code or any modified
* version of this code must cite the Copyright, this License, the
* Availability note, and "Used by permission." Permission to modify
* the code and to distribute modified code is granted, provided the
* Copyright, this License, and the Availability note are retained,
* and a notice that the code was modified is included.
* See CCOLAMD/Doc/License.txt for the license.
*
* Availability:
*
* The CCOLAMD/CSYMAMD library is available at
*
* http://www.cise.ufl.edu/research/sparse/ccolamd/
*
* This is the http://www.cise.ufl.edu/research/sparse/ccolamd/ccolamd.c
* file.
* http://www.suitesparse.com
*
* See the ChangeLog file for changes since Version 1.0.
*/
@ -99,10 +71,10 @@
/* === Description of user-callable routines ================================ */
/* ========================================================================== */
/* CCOLAMD includes both int and UF_long versions of all its routines. The
* description below is for the int version. For UF_long, all int arguments
* become UF_long integers. UF_long is normally defined as long, except for
* WIN64 */
/* CCOLAMD includes both int and SuiteSparse_long versions of all its routines.
* The description below is for the int version. For SuiteSparse_long, all
* int arguments become SuiteSparse_long integers. SuiteSparse_long is
* normally defined as long, except for WIN64 */
/* ----------------------------------------------------------------------------
* ccolamd_recommended:
@ -112,8 +84,8 @@
*
* #include "ccolamd.h"
* size_t ccolamd_recommended (int nnz, int n_row, int n_col) ;
* size_t ccolamd_l_recommended (UF_long nnz, UF_long n_row,
* UF_long n_col) ;
* size_t ccolamd_l_recommended (SuiteSparse_long nnz,
* SuiteSparse_long n_row, SuiteSparse_long n_col) ;
*
* Purpose:
*
@ -209,9 +181,12 @@
* double knobs [CCOLAMD_KNOBS], int stats [CCOLAMD_STATS],
* int *cmember) ;
*
* UF_long ccolamd_l (UF_long n_row, UF_long n_col, UF_long Alen,
* UF_long *A, UF_long *p, double knobs [CCOLAMD_KNOBS],
* UF_long stats [CCOLAMD_STATS], UF_long *cmember) ;
* SuiteSparse_long ccolamd_l (SuiteSparse_long n_row,
* SuiteSparse_long n_col, SuiteSparse_long Alen,
* SuiteSparse_long *A, SuiteSparse_long *p,
* double knobs [CCOLAMD_KNOBS],
* SuiteSparse_long stats [CCOLAMD_STATS],
* SuiteSparse_long *cmember) ;
*
* Purpose:
*
@ -385,9 +360,7 @@
*
* Example:
*
* See
* http://www.cise.ufl.edu/research/sparse/ccolamd/ccolamd_example.c
* for a complete example.
* See ccolamd_example.c for a complete example.
*
* To order the columns of a 5-by-4 matrix with 11 nonzero entries in
* the following nonzero pattern
@ -423,10 +396,12 @@
* void (*allocate) (size_t, size_t), void (*release) (void *),
* int *cmember, int stype) ;
*
* UF_long csymamd_l (UF_long n, UF_long *A, UF_long *p, UF_long *perm,
* double knobs [CCOLAMD_KNOBS], UF_long stats [CCOLAMD_STATS],
* void (*allocate) (size_t, size_t), void (*release) (void *),
* UF_long *cmember, UF_long stype) ;
* SuiteSparse_long csymamd_l (SuiteSparse_long n,
* SuiteSparse_long *A, SuiteSparse_long *p,
* SuiteSparse_long *perm, double knobs [CCOLAMD_KNOBS],
* SuiteSparse_long stats [CCOLAMD_STATS], void (*allocate)
* (size_t, size_t), void (*release) (void *),
* SuiteSparse_long *cmember, SuiteSparse_long stype) ;
*
* Purpose:
*
@ -562,7 +537,7 @@
*
* #include "ccolamd.h"
* ccolamd_report (int stats [CCOLAMD_STATS]) ;
* ccolamd_l_report (UF_long stats [CCOLAMD_STATS]) ;
* ccolamd_l_report (SuiteSparse_long stats [CCOLAMD_STATS]) ;
*
* Purpose:
*
@ -583,7 +558,7 @@
*
* #include "ccolamd.h"
* csymamd_report (int stats [CCOLAMD_STATS]) ;
* csymamd_l_report (UF_long stats [CCOLAMD_STATS]) ;
* csymamd_l_report (SuiteSparse_long stats [CCOLAMD_STATS]) ;
*
* Purpose:
*
@ -617,12 +592,11 @@
#include "ccolamd.h"
#include <stdlib.h>
#include <math.h>
#include <limits.h>
#ifdef MATLAB_MEX_FILE
#include <stdint.h>
typedef uint16_t char16_t;
#include "mex.h"
#include "matrix.h"
#endif
@ -636,17 +610,14 @@ typedef uint16_t char16_t;
#endif
/* ========================================================================== */
/* === int or UF_long ======================================================= */
/* === int or SuiteSparse_long ============================================== */
/* ========================================================================== */
/* define UF_long */
#include "UFconfig.h"
#ifdef DLONG
#define Int UF_long
#define ID UF_long_id
#define Int_MAX UF_long_max
#define Int SuiteSparse_long
#define ID SuiteSparse_long_id
#define Int_MAX SuiteSparse_long_max
#define CCOLAMD_recommended ccolamd_l_recommended
#define CCOLAMD_set_defaults ccolamd_l_set_defaults
@ -811,9 +782,6 @@ typedef struct CColamd_Row_struct
#define INDEX(i) (i)
#endif
/* All output goes through the PRINTF macro. */
#define PRINTF(params) { if (ccolamd_printf != NULL) (void) ccolamd_printf params ; }
/* ========================================================================== */
/* === Debugging prototypes and definitions ================================= */
@ -827,11 +795,11 @@ typedef struct CColamd_Row_struct
PRIVATE Int ccolamd_debug ;
/* debug print statements */
#define DEBUG0(params) { PRINTF (params) ; }
#define DEBUG1(params) { if (ccolamd_debug >= 1) PRINTF (params) ; }
#define DEBUG2(params) { if (ccolamd_debug >= 2) PRINTF (params) ; }
#define DEBUG3(params) { if (ccolamd_debug >= 3) PRINTF (params) ; }
#define DEBUG4(params) { if (ccolamd_debug >= 4) PRINTF (params) ; }
#define DEBUG0(params) { SUITESPARSE_PRINTF (params) ; }
#define DEBUG1(params) { if (ccolamd_debug >= 1) SUITESPARSE_PRINTF (params) ; }
#define DEBUG2(params) { if (ccolamd_debug >= 2) SUITESPARSE_PRINTF (params) ; }
#define DEBUG3(params) { if (ccolamd_debug >= 3) SUITESPARSE_PRINTF (params) ; }
#define DEBUG4(params) { if (ccolamd_debug >= 4) SUITESPARSE_PRINTF (params) ; }
#ifdef MATLAB_MEX_FILE
#define ASSERT(expression) (mxAssert ((expression), ""))
@ -3752,12 +3720,12 @@ PRIVATE void print_report
Int i1, i2, i3 ;
PRINTF (("\n%s version %d.%d, %s: ", method,
SUITESPARSE_PRINTF (("\n%s version %d.%d, %s: ", method,
CCOLAMD_MAIN_VERSION, CCOLAMD_SUB_VERSION, CCOLAMD_DATE)) ;
if (!stats)
{
PRINTF (("No statistics available.\n")) ;
SUITESPARSE_PRINTF (("No statistics available.\n")) ;
return ;
}
@ -3767,11 +3735,11 @@ PRIVATE void print_report
if (stats [CCOLAMD_STATUS] >= 0)
{
PRINTF(("OK. ")) ;
SUITESPARSE_PRINTF(("OK. ")) ;
}
else
{
PRINTF(("ERROR. ")) ;
SUITESPARSE_PRINTF(("ERROR. ")) ;
}
switch (stats [CCOLAMD_STATUS])
@ -3779,91 +3747,105 @@ PRIVATE void print_report
case CCOLAMD_OK_BUT_JUMBLED:
PRINTF(("Matrix has unsorted or duplicate row indices.\n")) ;
SUITESPARSE_PRINTF((
"Matrix has unsorted or duplicate row indices.\n")) ;
PRINTF(("%s: duplicate or out-of-order row indices: "ID"\n",
method, i3)) ;
SUITESPARSE_PRINTF((
"%s: duplicate or out-of-order row indices: "ID"\n",
method, i3)) ;
PRINTF(("%s: last seen duplicate or out-of-order row: "ID"\n",
method, INDEX (i2))) ;
SUITESPARSE_PRINTF((
"%s: last seen duplicate or out-of-order row: "ID"\n",
method, INDEX (i2))) ;
PRINTF(("%s: last seen in column: "ID"",
method, INDEX (i1))) ;
SUITESPARSE_PRINTF((
"%s: last seen in column: "ID"",
method, INDEX (i1))) ;
/* no break - fall through to next case instead */
case CCOLAMD_OK:
PRINTF(("\n")) ;
SUITESPARSE_PRINTF(("\n")) ;
PRINTF(("%s: number of dense or empty rows ignored: "ID"\n",
method, stats [CCOLAMD_DENSE_ROW])) ;
SUITESPARSE_PRINTF((
"%s: number of dense or empty rows ignored: "ID"\n",
method, stats [CCOLAMD_DENSE_ROW])) ;
PRINTF(("%s: number of dense or empty columns ignored: "ID"\n",
method, stats [CCOLAMD_DENSE_COL])) ;
SUITESPARSE_PRINTF((
"%s: number of dense or empty columns ignored: "ID"\n",
method, stats [CCOLAMD_DENSE_COL])) ;
PRINTF(("%s: number of garbage collections performed: "ID"\n",
method, stats [CCOLAMD_DEFRAG_COUNT])) ;
SUITESPARSE_PRINTF((
"%s: number of garbage collections performed: "ID"\n",
method, stats [CCOLAMD_DEFRAG_COUNT])) ;
break ;
case CCOLAMD_ERROR_A_not_present:
PRINTF(("Array A (row indices of matrix) not present.\n")) ;
SUITESPARSE_PRINTF((
"Array A (row indices of matrix) not present.\n")) ;
break ;
case CCOLAMD_ERROR_p_not_present:
PRINTF(("Array p (column pointers for matrix) not present.\n")) ;
SUITESPARSE_PRINTF((
"Array p (column pointers for matrix) not present.\n")) ;
break ;
case CCOLAMD_ERROR_nrow_negative:
PRINTF(("Invalid number of rows ("ID").\n", i1)) ;
SUITESPARSE_PRINTF(("Invalid number of rows ("ID").\n", i1)) ;
break ;
case CCOLAMD_ERROR_ncol_negative:
PRINTF(("Invalid number of columns ("ID").\n", i1)) ;
SUITESPARSE_PRINTF(("Invalid number of columns ("ID").\n", i1)) ;
break ;
case CCOLAMD_ERROR_nnz_negative:
PRINTF(("Invalid number of nonzero entries ("ID").\n", i1)) ;
SUITESPARSE_PRINTF((
"Invalid number of nonzero entries ("ID").\n", i1)) ;
break ;
case CCOLAMD_ERROR_p0_nonzero:
PRINTF(("Invalid column pointer, p [0] = "ID", must be 0.\n", i1)) ;
SUITESPARSE_PRINTF((
"Invalid column pointer, p [0] = "ID", must be 0.\n", i1)) ;
break ;
case CCOLAMD_ERROR_A_too_small:
PRINTF(("Array A too small.\n")) ;
PRINTF((" Need Alen >= "ID", but given only Alen = "ID".\n",
i1, i2)) ;
SUITESPARSE_PRINTF(("Array A too small.\n")) ;
SUITESPARSE_PRINTF((
" Need Alen >= "ID", but given only Alen = "ID".\n",
i1, i2)) ;
break ;
case CCOLAMD_ERROR_col_length_negative:
PRINTF(("Column "ID" has a negative number of entries ("ID").\n",
INDEX (i1), i2)) ;
SUITESPARSE_PRINTF((
"Column "ID" has a negative number of entries ("ID").\n",
INDEX (i1), i2)) ;
break ;
case CCOLAMD_ERROR_row_index_out_of_bounds:
PRINTF(("Row index (row "ID") out of bounds ("ID" to "ID") in"
"column "ID".\n", INDEX (i2), INDEX (0), INDEX (i3-1),
INDEX (i1))) ;
SUITESPARSE_PRINTF((
"Row index (row "ID") out of bounds ("ID" to "ID") in"
"column "ID".\n", INDEX (i2), INDEX (0), INDEX (i3-1),
INDEX (i1))) ;
break ;
case CCOLAMD_ERROR_out_of_memory:
PRINTF(("Out of memory.\n")) ;
SUITESPARSE_PRINTF(("Out of memory.\n")) ;
break ;
case CCOLAMD_ERROR_invalid_cmember:
PRINTF(("cmember invalid\n")) ;
SUITESPARSE_PRINTF(("cmember invalid\n")) ;
break ;
}
}

28
gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c vendored
View File

@ -1,28 +0,0 @@
/* ========================================================================== */
/* === ccolamd_global.c ===================================================== */
/* ========================================================================== */
/* ----------------------------------------------------------------------------
* CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/* Global variables for CCOLAMD */
#ifndef NPRINT
#ifdef MATLAB_MEX_FILE
#include <stdlib.h>
#include <stdint.h>
typedef uint16_t char16_t;
#include "mex.h"
int (*ccolamd_printf) (const char *, ...) = mexPrintf ;
#else
#include <stdio.h>
int (*ccolamd_printf) (const char *, ...) = printf ;
#endif
#else
int (*ccolamd_printf) (const char *, ...) = ((void *) 0) ;
#endif

2
gtsam/3rdparty/CMakeLists.txt vendored
View File

@ -1,6 +1,6 @@
# install CCOLAMD headers
install(FILES CCOLAMD/Include/ccolamd.h DESTINATION include/gtsam/3rdparty/CCOLAMD)
install(FILES UFconfig/UFconfig.h DESTINATION include/gtsam/3rdparty/UFconfig)
install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION include/gtsam/3rdparty/SuiteSparse_config)
if(NOT GTSAM_USE_SYSTEM_EIGEN)
# Find plain .h files

4
gtsam/3rdparty/Eigen/.hg_archival.txt vendored
View File

@ -1,4 +1,4 @@
repo: 8a21fd850624c931e448cbcfb38168cb2717c790
node: 07105f7124f9aef00a68c85e0fc606e65d3d6c15
node: b9cd8366d4e8f49471c7afafc4c2a1b00e54a54d
branch: 3.2
tag: 3.2.8
tag: 3.2.10

2
gtsam/3rdparty/Eigen/.hgtags vendored
View File

@ -31,3 +31,5 @@ ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0
bdd17ee3b1b3a166cd5ec36dcad4fc1f3faf774a 3.2.5
c58038c56923e0fd86de3ded18e03df442e66dfb 3.2.6
b30b87236a1b1552af32ac34075ee5696a9b5a33 3.2.7
07105f7124f9aef00a68c85e0fc606e65d3d6c15 3.2.8
dc6cfdf9bcec5efc7b6593bddbbb3d675de53524 3.2.9

5
gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h vendored
View File

@ -66,9 +66,8 @@ struct traits<Block<XprType, BlockRows, BlockCols, InnerPanel> > : traits<XprTyp
: ColsAtCompileTime != Dynamic ? int(ColsAtCompileTime)
: int(traits<XprType>::MaxColsAtCompileTime),
XprTypeIsRowMajor = (int(traits<XprType>::Flags)&RowMajorBit) != 0,
IsDense = is_same<StorageKind,Dense>::value,
IsRowMajor = (IsDense&&MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1
: (IsDense&&MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0
IsRowMajor = (MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1
: (MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0
: XprTypeIsRowMajor,
HasSameStorageOrderAsXprType = (IsRowMajor == XprTypeIsRowMajor),
InnerSize = IsRowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime),

25
gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h vendored
View File

@ -76,9 +76,7 @@ struct CommaInitializer
template<typename OtherDerived>
CommaInitializer& operator,(const DenseBase<OtherDerived>& other)
{
if(other.cols()==0 || other.rows()==0)
return *this;
if (m_col==m_xpr.cols())
if (m_col==m_xpr.cols() && (other.cols()!=0 || other.rows()!=m_currentBlockRows))
{
m_row+=m_currentBlockRows;
m_col = 0;
@ -86,24 +84,18 @@ struct CommaInitializer
eigen_assert(m_row+m_currentBlockRows<=m_xpr.rows()
&& "Too many rows passed to comma initializer (operator<<)");
}
eigen_assert(m_col<m_xpr.cols()
eigen_assert((m_col + other.cols() <= m_xpr.cols())
&& "Too many coefficients passed to comma initializer (operator<<)");
eigen_assert(m_currentBlockRows==other.rows());
if (OtherDerived::SizeAtCompileTime != Dynamic)
m_xpr.template block<OtherDerived::RowsAtCompileTime != Dynamic ? OtherDerived::RowsAtCompileTime : 1,
OtherDerived::ColsAtCompileTime != Dynamic ? OtherDerived::ColsAtCompileTime : 1>
(m_row, m_col) = other;
else
m_xpr.block(m_row, m_col, other.rows(), other.cols()) = other;
m_xpr.template block<OtherDerived::RowsAtCompileTime, OtherDerived::ColsAtCompileTime>
(m_row, m_col, other.rows(), other.cols()) = other;
m_col += other.cols();
return *this;
}
inline ~CommaInitializer()
{
eigen_assert((m_row+m_currentBlockRows) == m_xpr.rows()
&& m_col == m_xpr.cols()
&& "Too few coefficients passed to comma initializer (operator<<)");
finished();
}
/** \returns the built matrix once all its coefficients have been set.
@ -113,7 +105,12 @@ struct CommaInitializer
* quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished());
* \endcode
*/
inline XprType& finished() { return m_xpr; }
inline XprType& finished() {
eigen_assert(((m_row+m_currentBlockRows) == m_xpr.rows() || m_xpr.cols() == 0)
&& m_col == m_xpr.cols()
&& "Too few coefficients passed to comma initializer (operator<<)");
return m_xpr;
}
XprType& m_xpr; // target expression
Index m_row; // current row id

6
gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h vendored
View File

@ -44,10 +44,10 @@ class DiagonalBase : public EigenBase<Derived>
template<typename DenseDerived>
void evalTo(MatrixBase<DenseDerived> &other) const;
template<typename DenseDerived>
void addTo(MatrixBase<DenseDerived> &other) const
inline void addTo(MatrixBase<DenseDerived> &other) const
{ other.diagonal() += diagonal(); }
template<typename DenseDerived>
void subTo(MatrixBase<DenseDerived> &other) const
inline void subTo(MatrixBase<DenseDerived> &other) const
{ other.diagonal() -= diagonal(); }
inline const DiagonalVectorType& diagonal() const { return derived().diagonal(); }
@ -98,7 +98,7 @@ class DiagonalBase : public EigenBase<Derived>
template<typename Derived>
template<typename DenseDerived>
void DiagonalBase<Derived>::evalTo(MatrixBase<DenseDerived> &other) const
inline void DiagonalBase<Derived>::evalTo(MatrixBase<DenseDerived> &other) const
{
other.setZero();
other.diagonal() = diagonal();

2
gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h vendored
View File

@ -59,7 +59,7 @@ struct dot_nocheck<T, U, true>
*/
template<typename Derived>
template<typename OtherDerived>
typename internal::scalar_product_traits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType
inline typename internal::scalar_product_traits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType
MatrixBase<Derived>::dot(const MatrixBase<OtherDerived>& other) const
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)

3
gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h vendored
View File

@ -969,6 +969,8 @@ template<typename T>
struct functor_traits<std::not_equal_to<T> >
{ enum { Cost = 1, PacketAccess = false }; };
#if(__cplusplus < 201103L)
// std::binder* are deprecated since c++11 and will be removed in c++17
template<typename T>
struct functor_traits<std::binder2nd<T> >
{ enum { Cost = functor_traits<T>::Cost, PacketAccess = false }; };
@ -976,6 +978,7 @@ struct functor_traits<std::binder2nd<T> >
template<typename T>
struct functor_traits<std::binder1st<T> >
{ enum { Cost = functor_traits<T>::Cost, PacketAccess = false }; };
#endif
template<typename T>
struct functor_traits<std::unary_negate<T> >

5
gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h vendored
View File

@ -205,9 +205,6 @@ class GeneralProduct<Lhs, Rhs, InnerProduct>
public:
GeneralProduct(const Lhs& lhs, const Rhs& rhs)
{
EIGEN_STATIC_ASSERT((internal::is_same<typename Lhs::RealScalar, typename Rhs::RealScalar>::value),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
Base::coeffRef(0,0) = (lhs.transpose().cwiseProduct(rhs)).sum();
}
@ -264,8 +261,6 @@ class GeneralProduct<Lhs, Rhs, OuterProduct>
GeneralProduct(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs)
{
EIGEN_STATIC_ASSERT((internal::is_same<typename Lhs::RealScalar, typename Rhs::RealScalar>::value),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
}
struct set { template<typename Dst, typename Src> void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() = src; } };

4
gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h vendored
View File

@ -183,8 +183,8 @@ template<typename Scalar, typename Packet> inline void pstoreu(Scalar* to, const
/** \internal tries to do cache prefetching of \a addr */
template<typename Scalar> inline void prefetch(const Scalar* addr)
{
#if !defined(_MSC_VER)
__builtin_prefetch(addr);
#if (!EIGEN_COMP_MSVC) && (EIGEN_COMP_GNUC || EIGEN_COMP_CLANG || EIGEN_COMP_ICC)
__builtin_prefetch(addr);
#endif
}

40
gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h vendored
View File

@ -218,8 +218,8 @@ struct conj_retval
* Implementation of abs2 *
****************************************************************************/
template<typename Scalar>
struct abs2_impl
template<typename Scalar,bool IsComplex>
struct abs2_impl_default
{
typedef typename NumTraits<Scalar>::Real RealScalar;
static inline RealScalar run(const Scalar& x)
@ -228,15 +228,26 @@ struct abs2_impl
}
};
template<typename RealScalar>
struct abs2_impl<std::complex<RealScalar> >
template<typename Scalar>
struct abs2_impl_default<Scalar, true> // IsComplex
{
static inline RealScalar run(const std::complex<RealScalar>& x)
typedef typename NumTraits<Scalar>::Real RealScalar;
static inline RealScalar run(const Scalar& x)
{
return real(x)*real(x) + imag(x)*imag(x);
}
};
template<typename Scalar>
struct abs2_impl
{
typedef typename NumTraits<Scalar>::Real RealScalar;
static inline RealScalar run(const Scalar& x)
{
return abs2_impl_default<Scalar,NumTraits<Scalar>::IsComplex>::run(x);
}
};
template<typename Scalar>
struct abs2_retval
{
@ -496,11 +507,24 @@ struct floor_log2<n, lower, upper, floor_log2_bogus>
template<typename Scalar>
struct random_default_impl<Scalar, false, true>
{
typedef typename NumTraits<Scalar>::NonInteger NonInteger;
static inline Scalar run(const Scalar& x, const Scalar& y)
{
return x + Scalar((NonInteger(y)-x+1) * std::rand() / (RAND_MAX + NonInteger(1)));
typedef typename conditional<NumTraits<Scalar>::IsSigned,std::ptrdiff_t,std::size_t>::type ScalarX;
if(y<x)
return x;
// the following difference might overflow on a 32 bits system,
// but since y>=x the result converted to an unsigned long is still correct.
std::size_t range = ScalarX(y)-ScalarX(x);
std::size_t offset = 0;
// rejection sampling
std::size_t divisor = 1;
std::size_t multiplier = 1;
if(range<RAND_MAX) divisor = (std::size_t(RAND_MAX)+1)/(range+1);
else multiplier = 1 + range/(std::size_t(RAND_MAX)+1);
do {
offset = (std::size_t(std::rand()) * multiplier) / divisor;
} while (offset > range);
return Scalar(ScalarX(x) + offset);
}
static inline Scalar run()

3
gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h vendored
View File

@ -584,10 +584,11 @@ struct permut_matrix_product_retval
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<const Matrix>, Block<Matrix> >::value should be true.
const typename Dest::Scalar *dst_data = internal::extract_data(dst);
if( is_same<MatrixTypeNestedCleaned,Dest>::value
&& blas_traits<MatrixTypeNestedCleaned>::HasUsableDirectAccess
&& blas_traits<Dest>::HasUsableDirectAccess
&& extract_data(dst) == extract_data(m_matrix))
&& dst_data!=0 && dst_data == extract_data(m_matrix))
{
// apply the permutation inplace
Matrix<bool,PermutationType::RowsAtCompileTime,1,0,PermutationType::MaxRowsAtCompileTime> mask(m_permutation.size());

6
gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h vendored
View File

@ -315,8 +315,8 @@ class PlainObjectBase : public internal::dense_xpr_base<Derived>::type
EIGEN_STRONG_INLINE void resizeLike(const EigenBase<OtherDerived>& _other)
{
const OtherDerived& other = _other.derived();
internal::check_rows_cols_for_overflow<MaxSizeAtCompileTime>::run(other.rows(), other.cols());
const Index othersize = other.rows()*other.cols();
internal::check_rows_cols_for_overflow<MaxSizeAtCompileTime>::run(Index(other.rows()), Index(other.cols()));
const Index othersize = Index(other.rows())*Index(other.cols());
if(RowsAtCompileTime == 1)
{
eigen_assert(other.rows() == 1 || other.cols() == 1);
@ -487,7 +487,7 @@ class PlainObjectBase : public internal::dense_xpr_base<Derived>::type
/** \sa MatrixBase::operator=(const EigenBase<OtherDerived>&) */
template<typename OtherDerived>
EIGEN_STRONG_INLINE PlainObjectBase(const EigenBase<OtherDerived> &other)
: m_storage(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols())
: m_storage(Index(other.derived().rows()) * Index(other.derived().cols()), other.derived().rows(), other.derived().cols())
{
_check_template_params();
internal::check_rows_cols_for_overflow<MaxSizeAtCompileTime>::run(other.derived().rows(), other.derived().cols());

20
gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h vendored
View File

@ -76,9 +76,23 @@ template<typename MatrixType, int Direction> class Reverse
EIGEN_DENSE_PUBLIC_INTERFACE(Reverse)
using Base::IsRowMajor;
// next line is necessary because otherwise const version of operator()
// is hidden by non-const version defined in this file
using Base::operator();
// The following two operators are provided to worarkound
// a MSVC 2013 issue. In theory, we could simply do:
// using Base::operator();
// to make const version of operator() visible.
// Otheriwse, they would be hidden by the non-const versions defined in this file
inline CoeffReturnType operator()(Index row, Index col) const
{
eigen_assert(row >= 0 && row < rows() && col >= 0 && col < cols());
return coeff(row, col);
}
inline CoeffReturnType operator()(Index index) const
{
eigen_assert(index >= 0 && index < m_matrix.size());
return coeff(index);
}
protected:
enum {

3
gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h vendored
View File

@ -243,7 +243,8 @@ template<int Side, typename TriangularType, typename Rhs> struct triangular_solv
template<typename Dest> inline void evalTo(Dest& dst) const
{
if(!(is_same<RhsNestedCleaned,Dest>::value && extract_data(dst) == extract_data(m_rhs)))
const typename Dest::Scalar *dst_data = internal::extract_data(dst);
if(!(is_same<RhsNestedCleaned,Dest>::value && dst_data!=0 && extract_data(dst) == extract_data(m_rhs)))
dst = m_rhs;
m_triangularMatrix.template solveInPlace<Side>(dst);
}

9
gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h vendored
View File

@ -331,11 +331,11 @@ inline void MatrixBase<Derived>::adjointInPlace()
namespace internal {
template<typename BinOp,typename NestedXpr,typename Rhs>
struct blas_traits<SelfCwiseBinaryOp<BinOp,NestedXpr,Rhs> >
: blas_traits<NestedXpr>
template<typename BinOp,typename Xpr,typename Rhs>
struct blas_traits<SelfCwiseBinaryOp<BinOp,Xpr,Rhs> >
: blas_traits<typename internal::remove_all<typename Xpr::Nested>::type>
{
typedef SelfCwiseBinaryOp<BinOp,NestedXpr,Rhs> XprType;
typedef SelfCwiseBinaryOp<BinOp,Xpr,Rhs> XprType;
static inline const XprType extract(const XprType& x) { return x; }
};
@ -392,7 +392,6 @@ struct checkTransposeAliasing_impl
::run(extract_data(dst), other))
&& "aliasing detected during transposition, use transposeInPlace() "
"or evaluate the rhs into a temporary using .eval()");
}
};

3
gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h vendored
View File

@ -376,7 +376,8 @@ struct transposition_matrix_product_retval
const int size = m_transpositions.size();
Index j = 0;
if(!(is_same<MatrixTypeNestedCleaned,Dest>::value && extract_data(dst) == extract_data(m_matrix)))
const typename Dest::Scalar *dst_data = internal::extract_data(dst);
if(!(is_same<MatrixTypeNestedCleaned,Dest>::value && dst_data!=0 && dst_data == extract_data(m_matrix)))
dst = m_matrix;
for(int k=(Transposed?size-1:0) ; Transposed?k>=0:k<size ; Transposed?--k:++k)

2
gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h vendored
View File

@ -81,7 +81,7 @@ EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheLeft,Mode,Conju
// coherence when accessing the rhs elements
std::ptrdiff_t l1, l2;
manage_caching_sizes(GetAction, &l1, &l2);
Index subcols = cols>0 ? l2/(4 * sizeof(Scalar) * otherStride) : 0;
Index subcols = cols>0 ? l2/(4 * sizeof(Scalar) * std::max<Index>(otherStride,size)) : 0;
subcols = std::max<Index>((subcols/Traits::nr)*Traits::nr, Traits::nr);
for(Index k2=IsLower ? 0 : size;

57
gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h vendored
View File

@ -42,16 +42,29 @@ template<bool Conjugate> struct conj_if;
template<> struct conj_if<true> {
template<typename T>
inline T operator()(const T& x) { return numext::conj(x); }
inline T operator()(const T& x) const { return numext::conj(x); }
template<typename T>
inline T pconj(const T& x) { return internal::pconj(x); }
inline T pconj(const T& x) const { return internal::pconj(x); }
};
template<> struct conj_if<false> {
template<typename T>
inline const T& operator()(const T& x) { return x; }
inline const T& operator()(const T& x) const { return x; }
template<typename T>
inline const T& pconj(const T& x) { return x; }
inline const T& pconj(const T& x) const { return x; }
};
// Generic implementation for custom complex types.
template<typename LhsScalar, typename RhsScalar, bool ConjLhs, bool ConjRhs>
struct conj_helper
{
typedef typename scalar_product_traits<LhsScalar,RhsScalar>::ReturnType Scalar;
EIGEN_STRONG_INLINE Scalar pmadd(const LhsScalar& x, const RhsScalar& y, const Scalar& c) const
{ return padd(c, pmul(x,y)); }
EIGEN_STRONG_INLINE Scalar pmul(const LhsScalar& x, const RhsScalar& y) const
{ return conj_if<ConjLhs>()(x) * conj_if<ConjRhs>()(y); }
};
template<typename Scalar> struct conj_helper<Scalar,Scalar,false,false>
@ -171,12 +184,13 @@ template<typename XprType> struct blas_traits
};
// pop conjugate
template<typename Scalar, typename NestedXpr>
struct blas_traits<CwiseUnaryOp<scalar_conjugate_op<Scalar>, NestedXpr> >
: blas_traits<NestedXpr>
template<typename Scalar, typename Xpr>
struct blas_traits<CwiseUnaryOp<scalar_conjugate_op<Scalar>, Xpr> >
: blas_traits<typename internal::remove_all<typename Xpr::Nested>::type>
{
typedef typename internal::remove_all<typename Xpr::Nested>::type NestedXpr;
typedef blas_traits<NestedXpr> Base;
typedef CwiseUnaryOp<scalar_conjugate_op<Scalar>, NestedXpr> XprType;
typedef CwiseUnaryOp<scalar_conjugate_op<Scalar>, Xpr> XprType;
typedef typename Base::ExtractType ExtractType;
enum {
@ -188,12 +202,13 @@ struct blas_traits<CwiseUnaryOp<scalar_conjugate_op<Scalar>, NestedXpr> >
};
// pop scalar multiple
template<typename Scalar, typename NestedXpr>
struct blas_traits<CwiseUnaryOp<scalar_multiple_op<Scalar>, NestedXpr> >
: blas_traits<NestedXpr>
template<typename Scalar, typename Xpr>
struct blas_traits<CwiseUnaryOp<scalar_multiple_op<Scalar>, Xpr> >
: blas_traits<typename internal::remove_all<typename Xpr::Nested>::type>
{
typedef typename internal::remove_all<typename Xpr::Nested>::type NestedXpr;
typedef blas_traits<NestedXpr> Base;
typedef CwiseUnaryOp<scalar_multiple_op<Scalar>, NestedXpr> XprType;
typedef CwiseUnaryOp<scalar_multiple_op<Scalar>, Xpr> XprType;
typedef typename Base::ExtractType ExtractType;
static inline ExtractType extract(const XprType& x) { return Base::extract(x.nestedExpression()); }
static inline Scalar extractScalarFactor(const XprType& x)
@ -201,12 +216,13 @@ struct blas_traits<CwiseUnaryOp<scalar_multiple_op<Scalar>, NestedXpr> >
};
// pop opposite
template<typename Scalar, typename NestedXpr>
struct blas_traits<CwiseUnaryOp<scalar_opposite_op<Scalar>, NestedXpr> >
: blas_traits<NestedXpr>
template<typename Scalar, typename Xpr>
struct blas_traits<CwiseUnaryOp<scalar_opposite_op<Scalar>, Xpr> >
: blas_traits<typename internal::remove_all<typename Xpr::Nested>::type>
{
typedef typename internal::remove_all<typename Xpr::Nested>::type NestedXpr;
typedef blas_traits<NestedXpr> Base;
typedef CwiseUnaryOp<scalar_opposite_op<Scalar>, NestedXpr> XprType;
typedef CwiseUnaryOp<scalar_opposite_op<Scalar>, Xpr> XprType;
typedef typename Base::ExtractType ExtractType;
static inline ExtractType extract(const XprType& x) { return Base::extract(x.nestedExpression()); }
static inline Scalar extractScalarFactor(const XprType& x)
@ -214,13 +230,14 @@ struct blas_traits<CwiseUnaryOp<scalar_opposite_op<Scalar>, NestedXpr> >
};
// pop/push transpose
template<typename NestedXpr>
struct blas_traits<Transpose<NestedXpr> >
: blas_traits<NestedXpr>
template<typename Xpr>
struct blas_traits<Transpose<Xpr> >
: blas_traits<typename internal::remove_all<typename Xpr::Nested>::type>
{
typedef typename internal::remove_all<typename Xpr::Nested>::type NestedXpr;
typedef typename NestedXpr::Scalar Scalar;
typedef blas_traits<NestedXpr> Base;
typedef Transpose<NestedXpr> XprType;
typedef Transpose<Xpr> XprType;
typedef Transpose<const typename Base::_ExtractType> ExtractType; // const to get rid of a compile error; anyway blas traits are only used on the RHS
typedef Transpose<const typename Base::_ExtractType> _ExtractType;
typedef typename conditional<bool(Base::HasUsableDirectAccess),

16
gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h vendored
View File

@ -162,7 +162,7 @@ const unsigned int HereditaryBits = RowMajorBit
/** \ingroup enums
* Enum containing possible values for the \p Mode parameter of
* MatrixBase::selfadjointView() and MatrixBase::triangularView(). */
enum {
enum UpLoType {
/** View matrix as a lower triangular matrix. */
Lower=0x1,
/** View matrix as an upper triangular matrix. */
@ -187,7 +187,7 @@ enum {
/** \ingroup enums
* Enum for indicating whether an object is aligned or not. */
enum {
enum AlignmentType {
/** Object is not correctly aligned for vectorization. */
Unaligned=0,
/** Object is aligned for vectorization. */
@ -217,7 +217,7 @@ enum DirectionType {
/** \internal \ingroup enums
* Enum to specify how to traverse the entries of a matrix. */
enum {
enum TraversalType {
/** \internal Default traversal, no vectorization, no index-based access */
DefaultTraversal,
/** \internal No vectorization, use index-based access to have only one for loop instead of 2 nested loops */
@ -239,7 +239,7 @@ enum {
/** \internal \ingroup enums
* Enum to specify whether to unroll loops when traversing over the entries of a matrix. */
enum {
enum UnrollingType {
/** \internal Do not unroll loops. */
NoUnrolling,
/** \internal Unroll only the inner loop, but not the outer loop. */
@ -251,7 +251,7 @@ enum {
/** \internal \ingroup enums
* Enum to specify whether to use the default (built-in) implementation or the specialization. */
enum {
enum SpecializedType {
Specialized,
BuiltIn
};
@ -259,7 +259,7 @@ enum {
/** \ingroup enums
* Enum containing possible values for the \p _Options template parameter of
* Matrix, Array and BandMatrix. */
enum {
enum StorageOptions {
/** Storage order is column major (see \ref TopicStorageOrders). */
ColMajor = 0,
/** Storage order is row major (see \ref TopicStorageOrders). */
@ -272,7 +272,7 @@ enum {
/** \ingroup enums
* Enum for specifying whether to apply or solve on the left or right. */
enum {
enum SideType {
/** Apply transformation on the left. */
OnTheLeft = 1,
/** Apply transformation on the right. */
@ -418,7 +418,7 @@ namespace Architecture
/** \internal \ingroup enums
* Enum used as template parameter in GeneralProduct. */
enum { CoeffBasedProductMode, LazyCoeffBasedProductMode, OuterProduct, InnerProduct, GemvProduct, GemmProduct };
enum ProductImplType { CoeffBasedProductMode, LazyCoeffBasedProductMode, OuterProduct, InnerProduct, GemvProduct, GemmProduct };
/** \internal \ingroup enums
* Enum used in experimental parallel implementation. */

8
gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h vendored
View File

@ -35,6 +35,14 @@
#pragma clang diagnostic push
#endif
#pragma clang diagnostic ignored "-Wconstant-logical-operand"
#elif defined __GNUC__ && __GNUC__>=6
#ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS
#pragma GCC diagnostic push
#endif
#pragma GCC diagnostic ignored "-Wignored-attributes"
#endif
#endif // not EIGEN_WARNINGS_DISABLED

2
gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h vendored
View File

@ -13,7 +13,7 @@
#define EIGEN_WORLD_VERSION 3
#define EIGEN_MAJOR_VERSION 2
#define EIGEN_MINOR_VERSION 8
#define EIGEN_MINOR_VERSION 10
#define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
(EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \

133
gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h vendored
View File

@ -507,7 +507,12 @@ template<typename T> void smart_copy(const T* start, const T* end, T* target)
template<typename T> struct smart_copy_helper<T,true> {
static inline void run(const T* start, const T* end, T* target)
{ memcpy(target, start, std::ptrdiff_t(end)-std::ptrdiff_t(start)); }
{
std::ptrdiff_t size = std::ptrdiff_t(end)-std::ptrdiff_t(start);
if(size==0) return;
eigen_internal_assert(start!=0 && end!=0 && target!=0);
memcpy(target, start, size);
}
};
template<typename T> struct smart_copy_helper<T,false> {
@ -515,7 +520,6 @@ template<typename T> struct smart_copy_helper<T,false> {
{ std::copy(start, end, target); }
};
/*****************************************************************************
*** Implementation of runtime stack allocation (falling back to malloc) ***
*****************************************************************************/
@ -655,99 +659,60 @@ template<typename T> class aligned_stack_memory_handler
/****************************************************************************/
/** \class aligned_allocator
* \ingroup Core_Module
*
* \brief STL compatible allocator to use with with 16 byte aligned types
*
* Example:
* \code
* // Matrix4f requires 16 bytes alignment:
* std::map< int, Matrix4f, std::less<int>,
* aligned_allocator<std::pair<const int, Matrix4f> > > my_map_mat4;
* // Vector3f does not require 16 bytes alignment, no need to use Eigen's allocator:
* std::map< int, Vector3f > my_map_vec3;
* \endcode
*
* \sa \ref TopicStlContainers.
*/
* \ingroup Core_Module
*
* \brief STL compatible allocator to use with with 16 byte aligned types
*
* Example:
* \code
* // Matrix4f requires 16 bytes alignment:
* std::map< int, Matrix4f, std::less<int>,
* aligned_allocator<std::pair<const int, Matrix4f> > > my_map_mat4;
* // Vector3f does not require 16 bytes alignment, no need to use Eigen's allocator:
* std::map< int, Vector3f > my_map_vec3;
* \endcode
*
* \sa \blank \ref TopicStlContainers.
*/
template<class T>
class aligned_allocator
class aligned_allocator : public std::allocator<T>
{
public:
typedef size_t size_type;
typedef std::ptrdiff_t difference_type;
typedef T* pointer;
typedef const T* const_pointer;
typedef T& reference;
typedef const T& const_reference;
typedef T value_type;
typedef size_t size_type;
typedef std::ptrdiff_t difference_type;
typedef T* pointer;
typedef const T* const_pointer;
typedef T& reference;
typedef const T& const_reference;
typedef T value_type;
template<class U>
struct rebind
{
typedef aligned_allocator<U> other;
};
template<class U>
struct rebind
{
typedef aligned_allocator<U> other;
};
pointer address( reference value ) const
{
return &value;
}
aligned_allocator() : std::allocator<T>() {}
const_pointer address( const_reference value ) const
{
return &value;
}
aligned_allocator(const aligned_allocator& other) : std::allocator<T>(other) {}
aligned_allocator()
{
}
template<class U>
aligned_allocator(const aligned_allocator<U>& other) : std::allocator<T>(other) {}
aligned_allocator( const aligned_allocator& )
{
}
~aligned_allocator() {}
template<class U>
aligned_allocator( const aligned_allocator<U>& )
{
}
pointer allocate(size_type num, const void* /*hint*/ = 0)
{
internal::check_size_for_overflow<T>(num);
return static_cast<pointer>( internal::aligned_malloc(num * sizeof(T)) );
}
~aligned_allocator()
{
}
size_type max_size() const
{
return (std::numeric_limits<size_type>::max)();
}
pointer allocate( size_type num, const void* hint = 0 )
{
EIGEN_UNUSED_VARIABLE(hint);
internal::check_size_for_overflow<T>(num);
return static_cast<pointer>( internal::aligned_malloc( num * sizeof(T) ) );
}
void construct( pointer p, const T& value )
{
::new( p ) T( value );
}
void destroy( pointer p )
{
p->~T();
}
void deallocate( pointer p, size_type /*num*/ )
{
internal::aligned_free( p );
}
bool operator!=(const aligned_allocator<T>& ) const
{ return false; }
bool operator==(const aligned_allocator<T>& ) const
{ return true; }
void deallocate(pointer p, size_type /*num*/)
{
internal::aligned_free(p);
}
};
//---------- Cache sizes ----------

3
gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h vendored
View File

@ -8,7 +8,10 @@
#pragma warning pop
#elif defined __clang__
#pragma clang diagnostic pop
#elif defined __GNUC__ && __GNUC__>=6
#pragma GCC diagnostic pop
#endif
#endif
#endif // EIGEN_WARNINGS_DISABLED

32
gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h vendored
View File

@ -327,13 +327,33 @@ GeneralizedEigenSolver<MatrixType>::compute(const MatrixType& A, const MatrixTyp
}
else
{
Scalar p = Scalar(0.5) * (m_matS.coeff(i, i) - m_matS.coeff(i+1, i+1));
Scalar z = sqrt(abs(p * p + m_matS.coeff(i+1, i) * m_matS.coeff(i, i+1)));
m_alphas.coeffRef(i) = ComplexScalar(m_matS.coeff(i+1, i+1) + p, z);
m_alphas.coeffRef(i+1) = ComplexScalar(m_matS.coeff(i+1, i+1) + p, -z);
// We need to extract the generalized eigenvalues of the pair of a general 2x2 block S and a triangular 2x2 block T
// From the eigen decomposition of T = U * E * U^-1,
// we can extract the eigenvalues of (U^-1 * S * U) / E
// Here, we can take advantage that E = diag(T), and U = [ 1 T_01 ; 0 T_11-T_00], and U^-1 = [1 -T_11/(T_11-T_00) ; 0 1/(T_11-T_00)].
// Then taking beta=T_00*T_11*(T_11-T_00), we can avoid any division, and alpha is the eigenvalues of A = (U^-1 * S * U) * diag(T_11,T_00) * (T_11-T_00):
// T = [a b ; 0 c]
// S = [e f ; g h]
RealScalar a = m_realQZ.matrixT().coeff(i, i), b = m_realQZ.matrixT().coeff(i, i+1), c = m_realQZ.matrixT().coeff(i+1, i+1);
RealScalar e = m_matS.coeff(i, i), f = m_matS.coeff(i, i+1), g = m_matS.coeff(i+1, i), h = m_matS.coeff(i+1, i+1);
RealScalar d = c-a;
RealScalar gb = g*b;
Matrix<RealScalar,2,2> A;
A << (e*d-gb)*c, ((e*b+f*d-h*b)*d-gb*b)*a,
g*c , (gb+h*d)*a;
// NOTE, we could also compute the SVD of T's block during the QZ factorization so that the respective T block is guaranteed to be diagonal,
// and then we could directly apply the formula below (while taking care of scaling S columns by T11,T00):
Scalar p = Scalar(0.5) * (A.coeff(i, i) - A.coeff(i+1, i+1));
Scalar z = sqrt(abs(p * p + A.coeff(i+1, i) * A.coeff(i, i+1)));
m_alphas.coeffRef(i) = ComplexScalar(A.coeff(i+1, i+1) + p, z);
m_alphas.coeffRef(i+1) = ComplexScalar(A.coeff(i+1, i+1) + p, -z);
m_betas.coeffRef(i) =
m_betas.coeffRef(i+1) = a*c*d;
m_betas.coeffRef(i) = m_realQZ.matrixT().coeff(i,i);
m_betas.coeffRef(i+1) = m_realQZ.matrixT().coeff(i,i);
i += 2;
}
}

4
gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h vendored
View File

@ -367,10 +367,10 @@ void tridiagonalization_inplace(MatrixType& matA, CoeffVectorType& hCoeffs)
hCoeffs.tail(n-i-1).noalias() = (matA.bottomRightCorner(remainingSize,remainingSize).template selfadjointView<Lower>()
* (conj(h) * matA.col(i).tail(remainingSize)));
hCoeffs.tail(n-i-1) += (conj(h)*Scalar(-0.5)*(hCoeffs.tail(remainingSize).dot(matA.col(i).tail(remainingSize)))) * matA.col(i).tail(n-i-1);
hCoeffs.tail(n-i-1) += (conj(h)*RealScalar(-0.5)*(hCoeffs.tail(remainingSize).dot(matA.col(i).tail(remainingSize)))) * matA.col(i).tail(n-i-1);
matA.bottomRightCorner(remainingSize, remainingSize).template selfadjointView<Lower>()
.rankUpdate(matA.col(i).tail(remainingSize), hCoeffs.tail(remainingSize), -1);
.rankUpdate(matA.col(i).tail(remainingSize), hCoeffs.tail(remainingSize), Scalar(-1));
matA.col(i).coeffRef(i+1) = beta;
hCoeffs.coeffRef(i) = h;

2
gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h vendored
View File

@ -75,7 +75,7 @@ template<typename MatrixType,int _Direction> class Homogeneous
inline Index rows() const { return m_matrix.rows() + (int(Direction)==Vertical ? 1 : 0); }
inline Index cols() const { return m_matrix.cols() + (int(Direction)==Horizontal ? 1 : 0); }
inline Scalar coeff(Index row, Index col) const
inline Scalar coeff(Index row, Index col=0) const
{
if( (int(Direction)==Vertical && row==m_matrix.rows())
|| (int(Direction)==Horizontal && col==m_matrix.cols()))

2
gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h vendored
View File

@ -276,7 +276,7 @@ public:
inline Coefficients& coeffs() { return m_coeffs;}
inline const Coefficients& coeffs() const { return m_coeffs;}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(IsAligned))
protected:
Coefficients m_coeffs;

8
gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h vendored
View File

@ -443,7 +443,7 @@ public:
operator * (const DiagonalBase<DiagonalDerived> &b) const
{
TransformTimeDiagonalReturnType res(*this);
res.linear() *= b;
res.linearExt() *= b;
return res;
}
@ -557,7 +557,7 @@ public:
return res;
}
inline Transform& operator*=(const DiagonalMatrix<Scalar,Dim>& s) { linear() *= s; return *this; }
inline Transform& operator*=(const DiagonalMatrix<Scalar,Dim>& s) { linearExt() *= s; return *this; }
template<typename Derived>
inline Transform& operator=(const RotationBase<Derived,Dim>& r);
@ -828,7 +828,7 @@ Transform<Scalar,Dim,Mode,Options>::prescale(const MatrixBase<OtherDerived> &oth
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
m_matrix.template block<Dim,HDim>(0,0).noalias() = (other.asDiagonal() * m_matrix.template block<Dim,HDim>(0,0));
affine().noalias() = (other.asDiagonal() * affine());
return *this;
}
@ -1048,7 +1048,7 @@ void Transform<Scalar,Dim,Mode,Options>::computeRotationScaling(RotationMatrixTy
}
}
/** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being
/** decomposes the linear part of the transformation as a product scaling x rotation, the scaling being
* not necessarily positive.
*
* If either pointer is zero, the corresponding computation is skipped.

6
gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h vendored
View File

@ -130,8 +130,10 @@ public:
}
/** Applies translation to vector */
inline VectorType operator* (const VectorType& other) const
{ return m_coeffs + other; }
template<typename Derived>
inline typename internal::enable_if<Derived::IsVectorAtCompileTime,VectorType>::type
operator* (const MatrixBase<Derived>& vec) const
{ return m_coeffs + vec.derived(); }
/** \returns the inverse translation (opposite) */
Translation inverse() const { return Translation(-m_coeffs); }

Some files were not shown because too many files have changed in this diff Show More