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

Merge branch 'borglab:develop' into hotfix/stack_overflow

release/4.3a0
wanmeihuali 2023-02-06 12:58:52 -08:00 committed by GitHub
parent 50d3f9c2f5 2856282932
commit 46c311b5b9
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
362 changed files with 3427 additions and 2936 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.github/scripts/unix.sh vendored
View File

@ -71,7 +71,7 @@ function configure()
-DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \
-DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DGTSAM_SINGLE_TEST_EXE=ON \
-DGTSAM_SINGLE_TEST_EXE=OFF \
-DBOOST_ROOT=$BOOST_ROOT \
-DBoost_NO_SYSTEM_PATHS=ON \
-DBoost_ARCHITECTURE=-x64

24
.github/workflows/build-linux.yml vendored
View File

@ -20,19 +20,15 @@ jobs:
# Github Actions requires a single row to be added to the build matrix.
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [
ubuntu-20.04-gcc-7,
ubuntu-20.04-gcc-9,
ubuntu-20.04-clang-9,
ubuntu-22.04-gcc-11,
ubuntu-22.04-clang-14,
]
build_type: [Debug, Release]
build_unstable: [ON]
include:
- name: ubuntu-20.04-gcc-7
os: ubuntu-20.04
compiler: gcc
version: "7"
- name: ubuntu-20.04-gcc-9
os: ubuntu-20.04
compiler: gcc
@ -43,14 +39,24 @@ jobs:
compiler: clang
version: "9"
- name: ubuntu-22.04-gcc-11
os: ubuntu-22.04
compiler: gcc
version: "11"
- name: ubuntu-22.04-clang-14
os: ubuntu-22.04
compiler: clang
version: "14"
steps:
- name: Checkout
uses: actions/checkout@v2
uses: actions/checkout@v3
- name: Install Dependencies
run: |
# LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository.
if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
# LLVM (clang) 9/14 is not in Bionic's repositories so we add the official LLVM repository.
if [ "${{ matrix.compiler }}" = "clang" ]; then
# (ipv4|ha).pool.sks-keyservers.net is the SKS GPG global keyserver pool
# ipv4 avoids potential timeouts because of crappy IPv6 infrastructure
# 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository

10
.github/workflows/build-macos.yml vendored
View File

@ -14,7 +14,7 @@ jobs:
GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
strategy:
fail-fast: false
fail-fast: true
matrix:
# Github Actions requires a single row to be added to the build matrix.
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
@ -32,20 +32,14 @@ jobs:
steps:
- name: Checkout
uses: actions/checkout@v2
uses: actions/checkout@v3
- name: Install Dependencies
run: |
brew install cmake ninja
brew install boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else
sudo xcode-select -switch /Applications/Xcode.app
echo "CC=clang" >> $GITHUB_ENV
echo "CXX=clang++" >> $GITHUB_ENV
fi
- name: Build and Test
run: bash .github/scripts/unix.sh -t

37
.github/workflows/build-python.yml vendored
View File

@ -14,58 +14,45 @@ jobs:
PYTHON_VERSION: ${{ matrix.python_version }}
strategy:
fail-fast: false
fail-fast: true
matrix:
# Github Actions requires a single row to be added to the build matrix.
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [
ubuntu-20.04-gcc-7,
name:
[
ubuntu-20.04-gcc-9,
ubuntu-20.04-gcc-9-tbb,
ubuntu-20.04-clang-9,
macOS-11-xcode-13.4.1,
ubuntu-20.04-gcc-7-tbb,
]
build_type: [Debug, Release]
build_type: [Release]
python_version: [3]
include:
- name: ubuntu-20.04-gcc-7
os: ubuntu-20.04
compiler: gcc
version: "7"
- name: ubuntu-20.04-gcc-9
os: ubuntu-20.04
compiler: gcc
version: "9"
- name: ubuntu-20.04-clang-9
- name: ubuntu-20.04-gcc-9-tbb
os: ubuntu-20.04
compiler: clang
compiler: gcc
version: "9"
flag: tbb
# NOTE temporarily added this as it is a required check.
- name: ubuntu-20.04-clang-9
os: ubuntu-20.04
compiler: clang
version: "9"
build_type: Debug
python_version: "3"
- name: macOS-11-xcode-13.4.1
os: macOS-11
compiler: xcode
version: "13.4.1"
- name: ubuntu-20.04-gcc-7-tbb
os: ubuntu-20.04
compiler: gcc
version: "7"
flag: tbb
steps:
- name: Checkout
uses: actions/checkout@v2
uses: actions/checkout@v3
- name: Install (Linux)
if: runner.os == 'Linux'
run: |
@ -98,15 +85,9 @@ jobs:
brew tap ProfFan/robotics
brew install cmake ninja
brew install boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else
sudo xcode-select -switch /Applications/Xcode.app
echo "CC=clang" >> $GITHUB_ENV
echo "CXX=clang++" >> $GITHUB_ENV
fi
- name: Set GTSAM_WITH_TBB Flag
if: matrix.flag == 'tbb'
run: |

101
.github/workflows/build-special.yml vendored
View File

@ -22,71 +22,80 @@ jobs:
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name:
[
ubuntu-gcc-deprecated,
ubuntu-gcc-quaternions,
ubuntu-gcc-tbb,
ubuntu-cayleymap,
ubuntu-clang-deprecated,
ubuntu-clang-quaternions,
ubuntu-clang-tbb,
ubuntu-clang-cayleymap,
ubuntu-clang-system-libs,
ubuntu-no-boost,
]
build_type: [Debug, Release]
include:
- name: ubuntu-gcc-deprecated
os: ubuntu-20.04
compiler: gcc
version: "9"
- name: ubuntu-clang-deprecated
os: ubuntu-22.04
compiler: clang
version: "14"
flag: deprecated
- name: ubuntu-gcc-quaternions
os: ubuntu-20.04
compiler: gcc
version: "9"
- name: ubuntu-clang-quaternions
os: ubuntu-22.04
compiler: clang
version: "14"
flag: quaternions
- name: ubuntu-gcc-tbb
os: ubuntu-20.04
compiler: gcc
version: "9"
- name: ubuntu-clang-tbb
os: ubuntu-22.04
compiler: clang
version: "14"
flag: tbb
- name: ubuntu-cayleymap
os: ubuntu-20.04
compiler: gcc
version: "9"
- name: ubuntu-clang-cayleymap
os: ubuntu-22.04
compiler: clang
version: "14"
flag: cayley
- name: ubuntu-system-libs
os: ubuntu-20.04
compiler: gcc
version: "9"
flag: system-libs
- name: ubuntu-clang-system-libs
os: ubuntu-22.04
compiler: clang
version: "14"
flag: system
- name: ubuntu-no-boost
os: ubuntu-22.04
compiler: clang
version: "14"
flag: no_boost
steps:
- name: Checkout
uses: actions/checkout@v2
uses: actions/checkout@v3
- name: Install (Linux)
if: runner.os == 'Linux'
run: |
# LLVM 9 is not in Bionic's repositories so we add the official LLVM repository.
if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
sudo apt-get -y update
sudo apt-get -y install software-properties-common
# LLVM (clang) 9/14 is not in 22.04 (jammy)'s repositories so we add the official LLVM repository.
if [ "${{ matrix.compiler }}" = "clang" ]; then
# (ipv4|ha).pool.sks-keyservers.net is the SKS GPG global keyserver pool
# ipv4 avoids potential timeouts because of crappy IPv6 infrastructure
# 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository
# This key is not in the keystore by default for Ubuntu so we need to add it.
LLVM_KEY=15CF4D18AF4F7421
gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add -
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
gpg -a --export $LLVM_KEY | sudo apt-key add -
sudo add-apt-repository "deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy main"
fi
sudo apt-get -y update
sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
fi
- name: Install Boost
if: runner.os == 'Linux'
@ -97,15 +106,9 @@ jobs:
if: runner.os == 'macOS'
run: |
brew install cmake ninja boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
echo "CC=clang" >> $GITHUB_ENV
echo "CXX=clang++" >> $GITHUB_ENV
fi
- name: Set Allow Deprecated Flag
if: matrix.flag == 'deprecated'
@ -135,8 +138,18 @@ jobs:
- name: Use system versions of 3rd party libraries
if: matrix.flag == 'system'
run: |
sudo apt-get install libeigen3-dev
echo "GTSAM_USE_SYSTEM_EIGEN=ON" >> $GITHUB_ENV
echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV
# TODO(dellaert): This does not work yet?
# sudo apt-get install metis
# echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV
- name: Turn off boost
if: matrix.flag == 'no_boost'
run: |
echo "GTSAM_ENABLE_BOOST_SERIALIZATION=OFF" >> $GITHUB_ENV
echo "GTSAM_USE_BOOST_FEATURES=OFF" >> $GITHUB_ENV
echo "GTSAM will not use BOOST"
- name: Build & Test
run: |

4
.github/workflows/build-windows.yml vendored
View File

@ -16,7 +16,7 @@ jobs:
BOOST_EXE: boost_1_72_0-msvc-14.2
strategy:
fail-fast: false
fail-fast: true
matrix:
# Github Actions requires a single row to be added to the build matrix.
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
@ -94,7 +94,7 @@ jobs:
echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV
- name: Checkout
uses: actions/checkout@v2
uses: actions/checkout@v3
- name: Configuration
run: |

20
CMakeLists.txt
View File

@ -50,8 +50,24 @@ endif()
include(cmake/HandleGeneralOptions.cmake) # CMake build options
# Libraries:
include(cmake/HandleBoost.cmake) # Boost
############### Decide on BOOST ######################################
# Enable or disable serialization with GTSAM_ENABLE_BOOST_SERIALIZATION
option(GTSAM_ENABLE_BOOST_SERIALIZATION "Enable Boost serialization" ON)
if(GTSAM_ENABLE_BOOST_SERIALIZATION)
add_definitions(-DGTSAM_ENABLE_BOOST_SERIALIZATION)
endif()
option(GTSAM_USE_BOOST_FEATURES "Enable Features that use Boost" ON)
if(GTSAM_USE_BOOST_FEATURES)
add_definitions(-DGTSAM_USE_BOOST_FEATURES)
endif()
if(GTSAM_ENABLE_BOOST_SERIALIZATION OR GTSAM_USE_BOOST_FEATURES)
include(cmake/HandleBoost.cmake)
endif()
######################################################################
# Other Libraries:
include(cmake/HandleCCache.cmake) # ccache
include(cmake/HandleCPack.cmake) # CPack
include(cmake/HandleEigen.cmake) # Eigen3

1
CppUnitLite/CMakeLists.txt
View File

@ -6,7 +6,6 @@ file(GLOB cppunitlite_src "*.cpp")
add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers})
list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
target_link_libraries(CppUnitLite PUBLIC Boost::boost) # boost/lexical_cast.h
gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure

10
CppUnitLite/Test.cpp
View File

@ -15,8 +15,6 @@
#include "TestResult.h"
#include "Failure.h"
#include <boost/lexical_cast.hpp>
Test::Test (const std::string& testName)
: name_ (testName), next_(0), lineNumber_(-1), safeCheck_(true)
{
@ -47,10 +45,10 @@ bool Test::check(long expected, long actual, TestResult& result, const std::stri
result.addFailure (
Failure (
name_,
boost::lexical_cast<std::string> (__FILE__),
std::string(__FILE__),
__LINE__,
boost::lexical_cast<std::string> (expected),
boost::lexical_cast<std::string> (actual)));
std::to_string(expected),
std::to_string(actual)));
return false;
@ -64,7 +62,7 @@ bool Test::check(const std::string& expected, const std::string& actual, TestRes
result.addFailure (
Failure (
name_,
boost::lexical_cast<std::string> (__FILE__),
std::string(__FILE__),
__LINE__,
expected,
actual));

22
CppUnitLite/Test.h
View File

@ -23,7 +23,7 @@
#include <cmath>
#include <boost/lexical_cast.hpp>
#include <string>
class TestResult;
@ -112,17 +112,17 @@ protected:
#define THROWS_EXCEPTION(condition)\
{ try { condition; \
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast<std::string>(#condition))); \
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + std::string(#condition))); \
return; } \
catch (...) {} }
#define CHECK_EXCEPTION(condition, exception_name)\
{ try { condition; \
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast<std::string>(#condition))); \
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + std::string(#condition))); \
return; } \
catch (exception_name&) {} \
catch (...) { \
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + boost::lexical_cast<std::string>(#condition) + boost::lexical_cast<std::string>(", expected: ") + boost::lexical_cast<std::string>(#exception_name))); \
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + std::string(#condition) + std::string(", expected: ") + std::string(#exception_name))); \
return; } }
#define EQUALITY(expected,actual)\
@ -130,21 +130,21 @@ protected:
result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); }
#define CHECK_EQUAL(expected,actual)\
{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, boost::lexical_cast<std::string>(expected), boost::lexical_cast<std::string>(actual))); }
{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); }
#define LONGS_EQUAL(expected,actual)\
{ long actualTemp = actual; \
long expectedTemp = expected; \
if ((expectedTemp) != (actualTemp)) \
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, boost::lexical_cast<std::string>(expectedTemp), \
boost::lexical_cast<std::string>(actualTemp))); return; } }
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, std::to_string(expectedTemp), \
std::to_string(actualTemp))); return; } }
#define DOUBLES_EQUAL(expected,actual,threshold)\
{ double actualTemp = actual; \
double expectedTemp = expected; \
if (!std::isfinite(actualTemp) || !std::isfinite(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, \
boost::lexical_cast<std::string>((double)expectedTemp), boost::lexical_cast<std::string>((double)actualTemp))); return; } }
std::to_string((double)expectedTemp), std::to_string((double)actualTemp))); return; } }
/* EXPECTs: tests will continue running after a failure */
@ -156,15 +156,15 @@ boost::lexical_cast<std::string>((double)expectedTemp), boost::lexical_cast<std:
{ long actualTemp = actual; \
long expectedTemp = expected; \
if ((expectedTemp) != (actualTemp)) \
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, boost::lexical_cast<std::string>(expectedTemp), \
boost::lexical_cast<std::string>(actualTemp))); } }
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, std::to_string(expectedTemp), \
std::to_string(actualTemp))); } }
#define EXPECT_DOUBLES_EQUAL(expected,actual,threshold)\
{ double actualTemp = actual; \
double expectedTemp = expected; \
if (!std::isfinite(actualTemp) || !std::isfinite(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, \
boost::lexical_cast<std::string>((double)expectedTemp), boost::lexical_cast<std::string>((double)actualTemp))); } }
std::to_string((double)expectedTemp), std::to_string((double)actualTemp))); } }
#define FAIL(text) \

2
GTSAM-Concepts.md
View File

@ -166,7 +166,7 @@ Concept Checks
Boost provides a nice way to check whether a given type satisfies a concept. For example, the following
BOOST_CONCEPT_ASSERT(IsVectorSpace<Point2>)
GTSAM_CONCEPT_ASSERT(IsVectorSpace<Point2>)
asserts that Point2 indeed is a model for the VectorSpace concept.

4
cmake/GtsamBuildTypes.cmake
View File

@ -88,6 +88,9 @@ if(MSVC)
WINDOWS_LEAN_AND_MEAN
NOMINMAX
)
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC
_ENABLE_EXTENDED_ALIGNED_STORAGE
)
# Avoid literally hundreds to thousands of warnings:
list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC
/wd4267 # warning C4267: 'initializing': conversion from 'size_t' to 'int', possible loss of data
@ -126,6 +129,7 @@ else()
-fPIC # ensure proper code generation for shared libraries
$<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr -Werror=return-local-addr> # Error: return local address
$<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address -Werror=return-stack-address> # Error: return local address
$<$<CXX_COMPILER_ID:Clang>:-Wno-misleading-indentation> # Eigen triggers a ton!
-Wreturn-type -Werror=return-type # Error on missing return()
-Wformat -Werror=format-security # Error on wrong printf() arguments
$<$<COMPILE_LANGUAGE:CXX>:${flag_override_}> # Enforce the use of the override keyword

2
cmake/example_cmake_find_gtsam/CMakeLists.txt
View File

@ -12,6 +12,6 @@ add_executable(example
)
# By using CMake exported targets, a simple "link" dependency introduces the
# include directories (-I) flags, links against Boost, and add any other
# include directories (-I) flags, and add any other
# required build flags (e.g. C++11, etc.)
target_link_libraries(example PRIVATE gtsam)

10
cmake/obsolete/GtsamTestingObsolete.cmake
View File

@ -2,7 +2,7 @@
# Macro for adding categorized tests in a "tests" folder, with
# optional exclusion of tests and convenience library linking options
#
# By default, all tests are linked with CppUnitLite and boost
# By default, all tests are linked with CppUnitLite
# Arguments:
# - subdir The name of the category for this test
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
@ -32,7 +32,6 @@ endfunction()
# Macro for adding categorized timing scripts in a "tests" folder, with
# optional exclusion of tests and convenience library linking options
#
# By default, all tests are linked with boost
# Arguments:
# - subdir The name of the category for this timing script
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
@ -51,8 +50,7 @@ macro(gtsam_add_subdir_timing subdir local_libs full_libs excluded_srcs)
endmacro()
# Macro for adding executables matching a pattern - builds one executable for
# each file matching the pattern. These exectuables are automatically linked
# with boost.
# each file matching the pattern.
# Arguments:
# - pattern The glob pattern to match source files
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
@ -138,9 +136,9 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l
# Linking and dependendencies
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES})
target_link_libraries(${script_bin} ${local_libs})
else()
target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES})
target_link_libraries(${script_bin} ${full_libs})
endif()
# Add .run target

18
examples/CMakeLists.txt
View File

@ -1,12 +1,22 @@
set (excluded_examples
elaboratePoint2KalmanFilter.cpp
"elaboratePoint2KalmanFilter.cpp"
)
# if GTSAM_ENABLE_BOOST_SERIALIZATION is not set then SolverComparer.cpp will not compile
if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION)
set (excluded_examples
${excluded_examples}
SolverComparer.cpp
list (APPEND excluded_examples
"SolverComparer.cpp"
)
endif()
# Add examples to exclude if GTSAM_USE_BOOST_FEATURES is not set
if (NOT GTSAM_USE_BOOST_FEATURES)
# add to excluded examples
list (APPEND excluded_examples
"CombinedImuFactorsExample.cpp"
"ImuFactorsExample.cpp"
"ShonanAveragingCLI.cpp"
"SolverComparer.cpp"
)
endif()

136
examples/Data/randomGrid3D.xml
View File

@ -1,13 +1,13 @@
<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
<!DOCTYPE boost_serialization>
<boost_serialization signature="serialization::archive" version="17">
<graph class_id="0" tracking_level="0" version="0">
<boost_serialization signature="serialization::archive" version="19">
<data class_id="0" tracking_level="0" version="0">
<Base class_id="1" tracking_level="0" version="0">
<factors_ class_id="2" tracking_level="0" version="0">
<count>32</count>
<item_version>1</item_version>
<item class_id="3" tracking_level="0" version="1">
<px class_id="4" class_name="gtsam::JacobianFactor" tracking_level="1" version="0" object_id="_0">
<px class_id="4" class_name="gtsam::JacobianFactor" tracking_level="1" version="1" object_id="_0">
<Base class_id="5" tracking_level="0" version="0">
<Base class_id="6" tracking_level="0" version="0">
<keys_>
@ -100,9 +100,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_ class_id="11" tracking_level="0" version="1">
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -199,9 +197,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -298,9 +294,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -397,9 +391,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -496,9 +488,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -595,9 +585,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -694,9 +682,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -793,9 +779,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -892,9 +876,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -991,9 +973,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -1090,9 +1070,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -1189,9 +1167,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -1288,9 +1264,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -1387,9 +1361,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -1486,9 +1458,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -1585,9 +1555,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -1684,9 +1652,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -1783,9 +1749,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -1882,9 +1846,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -1981,9 +1943,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -2080,9 +2040,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -2179,9 +2137,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -2278,9 +2234,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -2377,9 +2331,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -2476,9 +2428,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -2575,9 +2525,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -2674,9 +2622,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -2773,9 +2719,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -2872,9 +2816,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -2971,9 +2913,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -3070,9 +3010,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -3402,13 +3340,11 @@
<rowEnd_>3</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
</factors_>
</Base>
</graph>
</data>
</boost_serialization>

16
examples/Data/toy3D.xml
View File

@ -1,13 +1,13 @@
<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
<!DOCTYPE boost_serialization>
<boost_serialization signature="serialization::archive" version="17">
<graph class_id="0" tracking_level="0" version="0">
<boost_serialization signature="serialization::archive" version="19">
<data class_id="0" tracking_level="0" version="0">
<Base class_id="1" tracking_level="0" version="0">
<factors_ class_id="2" tracking_level="0" version="0">
<count>2</count>
<item_version>1</item_version>
<item class_id="3" tracking_level="0" version="1">
<px class_id="4" class_name="gtsam::JacobianFactor" tracking_level="1" version="0" object_id="_0">
<px class_id="4" class_name="gtsam::JacobianFactor" tracking_level="1" version="1" object_id="_0">
<Base class_id="5" tracking_level="0" version="0">
<Base class_id="6" tracking_level="0" version="0">
<keys_>
@ -100,9 +100,7 @@
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_ class_id="11" tracking_level="0" version="1">
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
<item>
@ -157,13 +155,11 @@
<rowEnd_>3</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
<model_null>1</model_null>
</px>
</item>
</factors_>
</Base>
</graph>
</data>
</boost_serialization>

3
examples/DiscreteBayesNetExample.cpp
View File

@ -51,8 +51,7 @@ int main(int argc, char **argv) {
DiscreteFactorGraph fg(asia);
// Create solver and eliminate
Ordering ordering;
ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7);
const Ordering ordering{0, 1, 2, 3, 4, 5, 6, 7};
// solve
auto mpe = fg.optimize();

5
examples/ISAM2Example_SmartFactor.cpp
View File

@ -87,9 +87,8 @@ int main(int argc, char* argv[]) {
result.print();
cout << "Detailed results:" << endl;
for (auto keyedStatus : result.detail->variableStatus) {
const auto& status = keyedStatus.second;
PrintKey(keyedStatus.first);
for (auto& [key, status] : result.detail->variableStatus) {
PrintKey(key);
cout << " {" << endl;
cout << "reeliminated: " << status.isReeliminated << endl;
cout << "relinearized above thresh: " << status.isAboveRelinThreshold

8
examples/Pose2SLAMExample_g2o.cpp
View File

@ -48,16 +48,16 @@ int main(const int argc, const char *argv[]) {
Values::shared_ptr initial;
bool is3D = false;
if (kernelType.compare("none") == 0) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
std::tie(graph, initial) = readG2o(g2oFile, is3D);
}
if (kernelType.compare("huber") == 0) {
std::cout << "Using robust kernel: huber " << std::endl;
boost::tie(graph, initial) =
std::tie(graph, initial) =
readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
}
if (kernelType.compare("tukey") == 0) {
std::cout << "Using robust kernel: tukey " << std::endl;
boost::tie(graph, initial) =
std::tie(graph, initial) =
readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
}
@ -90,7 +90,7 @@ int main(const int argc, const char *argv[]) {
std::cout << "Writing results to file: " << outputFile << std::endl;
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, result, outputFile);
std::cout << "done! " << std::endl;
}

2
examples/Pose2SLAMExample_graph.cpp
View File

@ -36,7 +36,7 @@ int main (int argc, char** argv) {
Values::shared_ptr initial;
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished());
string graph_file = findExampleDataFile("w100.graph");
boost::tie(graph, initial) = load2D(graph_file, model);
std::tie(graph, initial) = load2D(graph_file, model);
initial->print("Initial estimate:\n");
// Add a Gaussian prior on first poses

4
examples/Pose2SLAMExample_lago.cpp
View File

@ -37,7 +37,7 @@ int main(const int argc, const char *argv[]) {
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
boost::tie(graph, initial) = readG2o(g2oFile);
std::tie(graph, initial) = readG2o(g2oFile);
// Add prior on the pose having index (key) = 0
auto priorModel = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
@ -55,7 +55,7 @@ int main(const int argc, const char *argv[]) {
std::cout << "Writing results to file: " << outputFile << std::endl;
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, estimateLago, outputFile);
std::cout << "done! " << std::endl;
}

8
examples/Pose3Localization.cpp
View File

@ -37,7 +37,7 @@ int main(const int argc, const char* argv[]) {
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
std::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
auto priorModel = noiseModel::Diagonal::Variances(
@ -67,15 +67,15 @@ int main(const int argc, const char* argv[]) {
std::cout << "Writing results to file: " << outputFile << std::endl;
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, result, outputFile);
std::cout << "done! " << std::endl;
}
// Calculate and print marginal covariances for all variables
Marginals marginals(*graph, result);
for (const auto& key_pose : result.extract<Pose3>()) {
std::cout << marginals.marginalCovariance(key_pose.first) << endl;
for (const auto& [key, pose] : result.extract<Pose3>()) {
std::cout << marginals.marginalCovariance(key) << endl;
}
return 0;
}

2
examples/Pose3SLAMExample_changeKeys.cpp
View File

@ -36,7 +36,7 @@ int main(const int argc, const char *argv[]) {
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
std::tie(graph, initial) = readG2o(g2oFile, is3D);
bool add = false;
Key firstKey = 8646911284551352320;

4
examples/Pose3SLAMExample_g2o.cpp
View File

@ -36,7 +36,7 @@ int main(const int argc, const char* argv[]) {
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
std::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
auto priorModel = noiseModel::Diagonal::Variances(
@ -66,7 +66,7 @@ int main(const int argc, const char* argv[]) {
std::cout << "Writing results to file: " << outputFile << std::endl;
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, result, outputFile);
std::cout << "done! " << std::endl;
}

4
examples/Pose3SLAMExample_initializePose3Chordal.cpp
View File

@ -36,7 +36,7 @@ int main(const int argc, const char* argv[]) {
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
std::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
auto priorModel = noiseModel::Diagonal::Variances(
@ -60,7 +60,7 @@ int main(const int argc, const char* argv[]) {
std::cout << "Writing results to file: " << outputFile << std::endl;
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, initialization, outputFile);
std::cout << "done! " << std::endl;
}

4
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -36,7 +36,7 @@ int main(const int argc, const char* argv[]) {
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
std::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
auto priorModel = noiseModel::Diagonal::Variances(
@ -66,7 +66,7 @@ int main(const int argc, const char* argv[]) {
std::cout << "Writing results to file: " << outputFile << std::endl;
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, initialization, outputFile);
std::cout << "done! " << std::endl;
}

10
examples/RangeISAMExample_plaza2.cpp
View File

@ -92,7 +92,7 @@ std::list<TimedOdometry> readOdometry() {
// load the ranges from TD
// Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
using RangeTriple = boost::tuple<double, size_t, double>;
using RangeTriple = std::tuple<double, size_t, double>;
std::vector<RangeTriple> readTriples() {
std::vector<RangeTriple> triples;
std::string data_file = gtsam::findExampleDataFile("Plaza2_TD.txt");
@ -166,7 +166,7 @@ int main(int argc, char** argv) {
//--------------------------------- odometry loop --------------------------
double t;
Pose2 odometry;
boost::tie(t, odometry) = timedOdometry;
std::tie(t, odometry) = timedOdometry;
// add odometry factor
newFactors.emplace_shared<gtsam::BetweenFactor<Pose2>>(i - 1, i, odometry,
@ -178,10 +178,10 @@ int main(int argc, char** argv) {
initial.insert(i, predictedPose);
// Check if there are range factors to be added
while (k < K && t >= boost::get<0>(triples[k])) {
size_t j = boost::get<1>(triples[k]);
while (k < K && t >= std::get<0>(triples[k])) {
size_t j = std::get<1>(triples[k]);
Symbol landmark_key('L', j);
double range = boost::get<2>(triples[k]);
double range = std::get<2>(triples[k]);
newFactors.emplace_shared<gtsam::RangeFactor<Pose2, Point2>>(
i, landmark_key, range, rangeNoise);
if (initializedLandmarks.count(landmark_key) == 0) {

8
examples/SFMExampleExpressions_bal.cpp
View File

@ -31,7 +31,6 @@
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <boost/format.hpp>
#include <vector>
using namespace std;
@ -50,8 +49,7 @@ int main(int argc, char* argv[]) {
// Load the SfM data from file
SfmData mydata = SfmData::FromBalFile(filename);
cout << boost::format("read %1% tracks on %2% cameras\n") %
mydata.numberTracks() % mydata.numberCameras();
cout << "read " << mydata.numberTracks() << " tracks on " << mydata.numberCameras() << " cameras" << endl;
// Create a factor graph
ExpressionFactorGraph graph;
@ -79,9 +77,7 @@ int main(int argc, char* argv[]) {
for (const SfmTrack& track : mydata.tracks) {
// Leaf expression for j^th point
Point3_ point_('p', j);
for (const SfmMeasurement& m : track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
for (const auto& [i, uv] : track.measurements) {
// Leaf expression for i^th camera
Expression<SfmCamera> camera_(C(i));
// Below an expression for the prediction of the measurement:

7
examples/SFMExample_bal.cpp
View File

@ -23,7 +23,6 @@
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <boost/format.hpp>
#include <vector>
using namespace std;
@ -45,7 +44,7 @@ int main (int argc, char* argv[]) {
// Load the SfM data from file
SfmData mydata = SfmData::FromBalFile(filename);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras();
cout << "read " << mydata.numberTracks() << " tracks on " << mydata.numberCameras() << " cameras" << endl;
// Create a factor graph
NonlinearFactorGraph graph;
@ -57,9 +56,7 @@ int main (int argc, char* argv[]) {
// Add measurements to the factor graph
size_t j = 0;
for(const SfmTrack& track: mydata.tracks) {
for(const SfmMeasurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
for (const auto& [i, uv] : track.measurements) {
graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
}
j += 1;

14
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

@ -26,7 +26,6 @@
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/timing.h>
#include <boost/format.hpp>
#include <vector>
using namespace std;
@ -47,8 +46,7 @@ int main(int argc, char* argv[]) {
// Load the SfM data from file
SfmData mydata = SfmData::FromBalFile(filename);
cout << boost::format("read %1% tracks on %2% cameras\n") %
mydata.numberTracks() % mydata.numberCameras();
cout << "read " << mydata.numberTracks() << " tracks on " << mydata.numberCameras() << " cameras" << endl;
// Create a factor graph
NonlinearFactorGraph graph;
@ -59,9 +57,7 @@ int main(int argc, char* argv[]) {
// Add measurements to the factor graph
size_t j = 0;
for (const SfmTrack& track : mydata.tracks) {
for (const SfmMeasurement& m : track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
for (const auto& [i, uv] : track.measurements) {
graph.emplace_shared<MyFactor>(
uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
}
@ -130,9 +126,9 @@ int main(int argc, char* argv[]) {
cout << endl << endl;
cout << "Time comparison by solving " << filename << " results:" << endl;
cout << boost::format("%1% point tracks and %2% cameras\n") %
mydata.numberTracks() % mydata.numberCameras()
<< endl;
cout << mydata.numberTracks() << " point tracks and " << mydata.numberCameras()
<< " cameras" << endl;
tictoc_print_();
}

4
examples/ShonanAveragingCLI.cpp
View File

@ -103,7 +103,7 @@ int main(int argc, char* argv[]) {
auto result = shonan.run(initial, pMin);
// Parse file again to set up translation problem, adding a prior
boost::tie(inputGraph, posesInFile) = load2D(inputFile);
std::tie(inputGraph, posesInFile) = load2D(inputFile);
auto priorModel = noiseModel::Unit::Create(3);
inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);
@ -119,7 +119,7 @@ int main(int argc, char* argv[]) {
auto result = shonan.run(initial, pMin);
// Parse file again to set up translation problem, adding a prior
boost::tie(inputGraph, posesInFile) = load3D(inputFile);
std::tie(inputGraph, posesInFile) = load3D(inputFile);
auto priorModel = noiseModel::Unit::Create(6);
inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);

8
examples/SolverComparer.cpp
View File

@ -49,8 +49,6 @@
#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/serialization/export.hpp>
#include <fstream>
#include <iostream>
@ -559,12 +557,12 @@ void runPerturb()
// Perturb values
VectorValues noise;
for(const auto& key_dim: initial.dims())
for(const auto& [key, dim]: initial.dims())
{
Vector noisev(key_dim.second);
Vector noisev(dim);
for(Vector::Index i = 0; i < noisev.size(); ++i)
noisev(i) = normal(rng);
noise.insert(key_dim.first, noisev);
noise.insert(key, noisev);
}
Values perturbed = initial.retract(noise);

17
gtsam/CMakeLists.txt
View File

@ -48,6 +48,23 @@ else()
set(excluded_sources ${excluded_sources} "${CMAKE_CURRENT_SOURCE_DIR}/geometry/Rot3Q.cpp")
endif()
# if GTSAM_ENABLE_BOOST_SERIALIZATION is not set, then we need to exclude the following:
if(NOT GTSAM_ENABLE_BOOST_SERIALIZATION)
list (APPEND excluded_headers
"${CMAKE_CURRENT_SOURCE_DIR}/gtsam/base/serializationTestHelpers.h"
"${CMAKE_CURRENT_SOURCE_DIR}/gtsam/base/serialization.h"
)
endif()
# if GTSAM_USE_BOOST_FEATURES is not set, then we need to exclude the following:
if(NOT GTSAM_USE_BOOST_FEATURES)
list (APPEND excluded_sources
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/GncOptimizer.h"
"${CMAKE_CURRENT_SOURCE_DIR}/inference/graph.h"
"${CMAKE_CURRENT_SOURCE_DIR}/inference/graph-inl.h"
)
endif()
# Common headers
file(GLOB gtsam_core_headers "*.h")
install(FILES ${gtsam_core_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam)

1
gtsam/base/ConcurrentMap.h
View File

@ -52,7 +52,6 @@ using ConcurrentMapBase = gtsam::FastMap<KEY, VALUE>;
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#endif
#include <boost/static_assert.hpp>
#include <gtsam/base/FastVector.h>

2
gtsam/base/DSFMap.h
View File

@ -50,7 +50,7 @@ class DSFMap {
iterator it = entries_.find(key);
// if key does not exist, create and return itself
if (it == entries_.end()) {
it = entries_.insert(std::make_pair(key, empty)).first;
it = entries_.insert({key, empty}).first;
it->second.parent_ = it;
it->second.rank_ = 0;
}

4
gtsam/base/FastList.h
View File

@ -20,10 +20,12 @@
#include <gtsam/base/FastDefaultAllocator.h>
#include <list>
#include <boost/utility/enable_if.hpp>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/version.hpp>
#if BOOST_VERSION >= 107400
#include <boost/serialization/library_version_type.hpp>
#endif
#include <boost/serialization/list.hpp>
#endif

2
gtsam/base/FastMap.h
View File

@ -63,7 +63,7 @@ public:
}
/** Handy 'insert' function for Matlab wrapper */
bool insert2(const KEY& key, const VALUE& val) { return Base::insert(std::make_pair(key, val)).second; }
bool insert2(const KEY& key, const VALUE& val) { return Base::insert({key, val}).second; }
/** Handy 'exists' function */
bool exists(const KEY& e) const { return this->find(e) != this->end(); }

4
gtsam/base/FastSet.h
View File

@ -18,8 +18,8 @@
#pragma once
#include <boost/version.hpp>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/version.hpp>
#if BOOST_VERSION >= 107400
#include <boost/serialization/library_version_type.hpp>
#endif
@ -51,7 +51,7 @@ template<typename VALUE>
class FastSet: public std::set<VALUE, std::less<VALUE>,
typename internal::FastDefaultAllocator<VALUE>::type> {
BOOST_CONCEPT_ASSERT ((IsTestable<VALUE> ));
GTSAM_CONCEPT_ASSERT(IsTestable<VALUE>);
public:

2
gtsam/base/GenericValue.h
View File

@ -23,8 +23,6 @@
#include <gtsam/base/types.h>
#include <gtsam/base/Value.h>
#include <boost/pool/pool_alloc.hpp>
#include <cmath>
#include <iostream>
#include <typeinfo> // operator typeid

19
gtsam/base/Group.h
View File

@ -22,10 +22,13 @@
#include <gtsam/base/Testable.h>
#ifdef GTSAM_USE_BOOST_FEATURES
#include <boost/concept_check.hpp>
#include <boost/concept/requires.hpp>
#include <boost/type_traits/is_base_of.hpp>
#include <boost/static_assert.hpp>
#endif
#include <utility>
namespace gtsam {
@ -50,8 +53,8 @@ public:
//typedef typename traits<G>::identity::value_type identity_value_type;
BOOST_CONCEPT_USAGE(IsGroup) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<group_tag, structure_category_tag>::value),
static_assert(
(std::is_base_of<group_tag, structure_category_tag>::value),
"This type's structure_category trait does not assert it as a group (or derived)");
e = traits<G>::Identity();
e = traits<G>::Compose(g, h);
@ -79,7 +82,7 @@ private:
/// Check invariants
template<typename G>
BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(bool)) //
GTSAM_CONCEPT_REQUIRES(IsGroup<G>,bool) //
check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
G e = traits<G>::Identity();
return traits<G>::Equals(traits<G>::Compose(a, traits<G>::Inverse(a)), e, tol)
@ -125,7 +128,7 @@ struct AdditiveGroup : AdditiveGroupTraits<Class>, Testable<Class> {};
/// compose multiple times
template<typename G>
BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(G)) //
GTSAM_CONCEPT_REQUIRES(IsGroup<G>,G) //
compose_pow(const G& g, size_t n) {
if (n == 0) return traits<G>::Identity();
else if (n == 1) return g;
@ -136,8 +139,8 @@ compose_pow(const G& g, size_t n) {
/// Assumes nothing except group structure and Testable from G and H
template<typename G, typename H>
class DirectProduct: public std::pair<G, H> {
BOOST_CONCEPT_ASSERT((IsGroup<G>));
BOOST_CONCEPT_ASSERT((IsGroup<H>));
GTSAM_CONCEPT_ASSERT1(IsGroup<G>);
GTSAM_CONCEPT_ASSERT2(IsGroup<H>);
public:
/// Default constructor yields identity
@ -167,8 +170,8 @@ struct traits<DirectProduct<G, H> > :
/// Assumes existence of three additive operators for both groups
template<typename G, typename H>
class DirectSum: public std::pair<G, H> {
BOOST_CONCEPT_ASSERT((IsGroup<G>)); // TODO(frank): check additive
BOOST_CONCEPT_ASSERT((IsGroup<H>)); // TODO(frank): check additive
GTSAM_CONCEPT_ASSERT1(IsGroup<G>); // TODO(frank): check additive
GTSAM_CONCEPT_ASSERT2(IsGroup<H>); // TODO(frank): check additive
const G& g() const { return this->first; }
const H& h() const { return this->second;}

4
gtsam/base/Lie.h
View File

@ -265,8 +265,8 @@ public:
typedef typename traits<T>::ChartJacobian ChartJacobian;
BOOST_CONCEPT_USAGE(IsLieGroup) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<lie_group_tag, structure_category_tag>::value),
static_assert(
(std::is_base_of<lie_group_tag, structure_category_tag>::value),
"This type's trait does not assert it is a Lie group (or derived)");
// group opertations with Jacobians

17
gtsam/base/Manifold.h
View File

@ -22,10 +22,7 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/concept_check.hpp>
#include <boost/concept/requires.hpp>
#include <boost/type_traits/is_base_of.hpp>
#include <gtsam/base/concepts.h>
namespace gtsam {
@ -95,7 +92,7 @@ template<class Class>
struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
// Check that Class has the necessary machinery
BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
GTSAM_CONCEPT_ASSERT(HasManifoldPrereqs<Class>);
// Dimension of the manifold
enum { dimension = Class::dimension };
@ -123,7 +120,7 @@ template<class Class> struct Manifold: ManifoldTraits<Class>, Testable<Class> {}
/// Check invariants for Manifold type
template<typename T>
BOOST_CONCEPT_REQUIRES(((IsTestable<T>)),(bool)) //
GTSAM_CONCEPT_REQUIRES(IsTestable<T>, bool) //
check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
typename traits<T>::TangentVector v0 = traits<T>::Local(a,a);
typename traits<T>::TangentVector v = traits<T>::Local(a,b);
@ -143,10 +140,10 @@ public:
typedef typename traits<T>::TangentVector TangentVector;
BOOST_CONCEPT_USAGE(IsManifold) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<manifold_tag, structure_category_tag>::value),
static_assert(
(std::is_base_of<manifold_tag, structure_category_tag>::value),
"This type's structure_category trait does not assert it as a manifold (or derived)");
BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);
static_assert(TangentVector::SizeAtCompileTime == dim);
// make sure Chart methods are defined
v = traits<T>::Local(p, q);
@ -164,7 +161,7 @@ template<typename T>
struct FixedDimension {
typedef const int value_type;
static const int value = traits<T>::dimension;
BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic,
static_assert(value != Eigen::Dynamic,
"FixedDimension instantiated for dymanically-sized type.");
};
} // \ namespace gtsam

31
gtsam/base/Matrix.cpp
View File

@ -23,10 +23,6 @@
#include <Eigen/SVD>
#include <Eigen/LU>
#include <boost/tuple/tuple.hpp>
#include <boost/tokenizer.hpp>
#include <boost/format.hpp>
#include <cstdarg>
#include <cstring>
#include <iomanip>
@ -129,8 +125,10 @@ bool linear_dependent(const Matrix& A, const Matrix& B, double tol) {
/* ************************************************************************* */
Vector operator^(const Matrix& A, const Vector & v) {
if (A.rows()!=v.size()) throw std::invalid_argument(
boost::str(boost::format("Matrix operator^ : A.m(%d)!=v.size(%d)") % A.rows() % v.size()));
if (A.rows()!=v.size()) {
throw std::invalid_argument("Matrix operator^ : A.m(" + std::to_string(A.rows()) + ")!=v.size(" +
std::to_string(v.size()) + ")");
}
// Vector vt = v.transpose();
// Vector vtA = vt * A;
// return vtA.transpose();
@ -250,8 +248,7 @@ pair<Matrix,Matrix> qr(const Matrix& A) {
xjm(k) = R(j+k, j);
// calculate the Householder vector v
double beta; Vector vjm;
boost::tie(beta,vjm) = house(xjm);
const auto [beta, vjm] = house(xjm);
// pad with zeros to get m-dimensional vector v
for(size_t k = 0 ; k < m; k++)
@ -269,13 +266,13 @@ pair<Matrix,Matrix> qr(const Matrix& A) {
}
/* ************************************************************************* */
list<boost::tuple<Vector, double, double> >
list<std::tuple<Vector, double, double> >
weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
size_t m = A.rows(), n = A.cols(); // get size(A)
size_t maxRank = min(m,n);
// create list
list<boost::tuple<Vector, double, double> > results;
list<std::tuple<Vector, double, double> > results;
Vector pseudo(m); // allocate storage for pseudo-inverse
Vector weights = sigmas.array().square().inverse(); // calculate weights once
@ -304,7 +301,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
// construct solution (r, d, sigma)
// TODO: avoid sqrt, store precision or at least variance
results.push_back(boost::make_tuple(r, d, 1./sqrt(precision)));
results.push_back(std::make_tuple(r, d, 1./sqrt(precision)));
// exit after rank exhausted
if (results.size()>=maxRank) break;
@ -565,7 +562,7 @@ void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V) {
}
/* ************************************************************************* */
boost::tuple<int, double, Vector> DLT(const Matrix& A, double rank_tol) {
std::tuple<int, double, Vector> DLT(const Matrix& A, double rank_tol) {
// Check size of A
size_t n = A.rows(), p = A.cols(), m = min(n,p);
@ -582,7 +579,7 @@ boost::tuple<int, double, Vector> DLT(const Matrix& A, double rank_tol) {
// Return rank, error, and corresponding column of V
double error = m<p ? 0 : s(m-1);
return boost::tuple<int, double, Vector>((int)rank, error, Vector(column(V, p-1)));
return std::tuple<int, double, Vector>((int)rank, error, Vector(column(V, p-1)));
}
/* ************************************************************************* */
@ -613,11 +610,12 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix,
else
matrixPrinted << matrix;
const std::string matrixStr = matrixPrinted.str();
boost::tokenizer<boost::char_separator<char> > tok(matrixStr, boost::char_separator<char>("\n"));
// Split the matrix string into lines and indent them
std::string line;
std::istringstream iss(matrixStr);
DenseIndex row = 0;
for(const std::string& line: tok)
{
while (std::getline(iss, line)) {
assert(row < effectiveRows);
if(row > 0)
ss << padding;
@ -626,6 +624,7 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix,
ss << "\n";
++ row;
}
} else {
ss << "Empty (" << matrix.rows() << "x" << matrix.cols() << ")";
}

7
gtsam/base/Matrix.h
View File

@ -26,7 +26,6 @@
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h>
#include <boost/tuple/tuple.hpp>
#include <vector>
@ -280,7 +279,7 @@ struct Reshape<N, M, InOptions, M, N, InOptions> {
template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
inline typename Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & m){
BOOST_STATIC_ASSERT(InM * InN == OutM * OutN);
static_assert(InM * InN == OutM * OutN);
return Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::reshape(m);
}
@ -307,7 +306,7 @@ GTSAM_EXPORT void inplace_QR(Matrix& A);
* @param sigmas is a vector of the measurement standard deviation
* @return list of r vectors, d and sigma
*/
GTSAM_EXPORT std::list<boost::tuple<Vector, double, double> >
GTSAM_EXPORT std::list<std::tuple<Vector, double, double> >
weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas);
/**
@ -434,7 +433,7 @@ GTSAM_EXPORT void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V);
* Returns rank of A, minimum error (singular value),
* and corresponding eigenvector (column of V, with A=U*S*V')
*/
GTSAM_EXPORT boost::tuple<int, double, Vector>
GTSAM_EXPORT std::tuple<int, double, Vector>
DLT(const Matrix& A, double rank_tol = 1e-9);
/**

4
gtsam/base/ProductLieGroup.h
View File

@ -27,8 +27,8 @@ namespace gtsam {
/// Assumes Lie group structure for G and H
template<typename G, typename H>
class ProductLieGroup: public std::pair<G, H> {
BOOST_CONCEPT_ASSERT((IsLieGroup<G>));
BOOST_CONCEPT_ASSERT((IsLieGroup<H>));
GTSAM_CONCEPT_ASSERT1(IsLieGroup<G>);
GTSAM_CONCEPT_ASSERT2(IsLieGroup<H>);
typedef std::pair<G, H> Base;
protected:

7
gtsam/base/Testable.h
View File

@ -33,7 +33,8 @@
#pragma once
#include <boost/concept_check.hpp>
#include <gtsam/base/concepts.h>
#include <functional>
#include <iostream>
#include <memory>
@ -151,7 +152,7 @@ namespace gtsam {
struct Testable {
// Check that T has the necessary methods
BOOST_CONCEPT_ASSERT((HasTestablePrereqs<T>));
GTSAM_CONCEPT_ASSERT(HasTestablePrereqs<T>);
static void Print(const T& m, const std::string& str = "") {
m.print(str);
@ -170,7 +171,7 @@ namespace gtsam {
*
* NOTE: intentionally not in the gtsam namespace to allow for classes not in
* the gtsam namespace to be more easily enforced as testable
* @deprecated please use BOOST_CONCEPT_ASSERT and
* @deprecated please use GTSAM_CONCEPT_ASSERT and
*/
#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable<T>;
#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable<T>;

2
gtsam/base/ThreadsafeException.h
View File

@ -46,9 +46,11 @@ private:
protected:
typedef std::basic_string<char, std::char_traits<char>,
tbb::tbb_allocator<char> > String;
typedef tbb::tbb_allocator<char> Allocator;
#else
protected:
typedef std::string String;
typedef std::allocator<char> Allocator;
#endif
protected:

6
gtsam/base/VectorSpace.h
View File

@ -185,7 +185,7 @@ template<class Class>
struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
// Check that Class has the necessary machinery
BOOST_CONCEPT_ASSERT((HasVectorSpacePrereqs<Class>));
GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs<Class>);
typedef vector_space_tag structure_category;
@ -473,8 +473,8 @@ public:
typedef typename traits<T>::structure_category structure_category_tag;
BOOST_CONCEPT_USAGE(IsVectorSpace) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<vector_space_tag, structure_category_tag>::value),
static_assert(
(std::is_base_of<vector_space_tag, structure_category_tag>::value),
"This type's trait does not assert it as a vector space (or derived)");
r = p + q;
r = -p;

4
gtsam/base/WeightedSampler.h
View File

@ -69,7 +69,7 @@ class WeightedSampler {
static const double kexp1 = std::exp(1.0);
for (auto it = weights.begin(); it != weights.begin() + numSamples; ++it) {
const double k_i = kexp1 / *it;
reservoir.push(std::make_pair(k_i, it - weights.begin() + 1));
reservoir.push({k_i, it - weights.begin() + 1});
}
// Step 4: Repeat Steps 5–10 until the population is exhausted
@ -110,7 +110,7 @@ class WeightedSampler {
// Step 8: The item in reservoir with the minimum key is replaced by
// item v_i
reservoir.pop();
reservoir.push(std::make_pair(k_i, it - weights.begin() + 1));
reservoir.push({k_i, it - weights.begin() + 1});
}
}

2
gtsam/base/chartTesting.h
View File

@ -39,7 +39,7 @@ void testDefaultChart(TestResult& result_,
// First, check the basic chart concept. This checks that the interface is satisfied.
// The rest of the function is even more detailed, checking the correctness of the chart.
BOOST_CONCEPT_ASSERT((ChartConcept<Chart>));
GTSAM_CONCEPT_ASSERT(ChartConcept<Chart>);
T other = value;

1
gtsam/base/cholesky.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/base/cholesky.h>
#include <gtsam/base/timing.h>
#include <boost/format.hpp>
#include <cmath>
using namespace std;

40
gtsam/base/concepts.h
View File

@ -8,25 +8,27 @@
#pragma once
// This is a helper to ease the transition to the new traits defined in this file.
// Uncomment this if you want all methods that are tagged as not implemented
// to cause compilation errors.
#ifdef COMPILE_ERROR_NOT_IMPLEMENTED
#include <boost/static_assert.hpp>
#define CONCEPT_NOT_IMPLEMENTED BOOST_STATIC_ASSERT_MSG(boost::false_type, \
"This method is required by the new concepts framework but has not been implemented yet.");
#ifdef GTSAM_USE_BOOST_FEATURES
#include <boost/concept_check.hpp>
#include <boost/concept/assert.hpp>
#include <boost/concept/requires.hpp>
#include <boost/concept_check.hpp>
#define GTSAM_CONCEPT_ASSERT(concept) BOOST_CONCEPT_ASSERT((concept))
#define GTSAM_CONCEPT_ASSERT1(concept) BOOST_CONCEPT_ASSERT((concept))
#define GTSAM_CONCEPT_ASSERT2(concept) BOOST_CONCEPT_ASSERT((concept))
#define GTSAM_CONCEPT_ASSERT3(concept) BOOST_CONCEPT_ASSERT((concept))
#define GTSAM_CONCEPT_ASSERT4(concept) BOOST_CONCEPT_ASSERT((concept))
#define GTSAM_CONCEPT_REQUIRES(concept, return_type) BOOST_CONCEPT_REQUIRES(((concept)), (return_type))
#else
#include <exception>
#define CONCEPT_NOT_IMPLEMENTED \
throw std::runtime_error("This method is required by the new concepts framework but has not been implemented yet.");
// These do something sensible:
#define BOOST_CONCEPT_USAGE(concept) void check##concept()
// TODO(dellaert): would be nice if it was a single macro...
#define GTSAM_CONCEPT_ASSERT(concept) concept checkConcept [[maybe_unused]];
#define GTSAM_CONCEPT_ASSERT1(concept) concept checkConcept1 [[maybe_unused]];
#define GTSAM_CONCEPT_ASSERT2(concept) concept checkConcept2 [[maybe_unused]];
#define GTSAM_CONCEPT_ASSERT3(concept) concept checkConcept3 [[maybe_unused]];
#define GTSAM_CONCEPT_ASSERT4(concept) concept checkConcept4 [[maybe_unused]];
// This one just ignores concept for now:
#define GTSAM_CONCEPT_REQUIRES(concept, return_type) return_type
#endif
namespace gtsam {
template <typename T> struct traits;
}

106
gtsam/base/kruskal-inl.h Normal file
View File

@ -0,0 +1,106 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, 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 kruskal-inl.h
* @date Dec 31, 2009
* @author Frank Dellaert
* @author Yong-Dian Jian
* @author Ankur Roy Chowdhury
*/
#pragma once
#include <gtsam/base/DSFMap.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/types.h>
#include <gtsam/inference/VariableIndex.h>
#include <algorithm>
#include <numeric>
#include <vector>
namespace gtsam::utils {
/*****************************************************************************/
/* Sort the 'weights' in increasing order and return the sorted order of its
* indices. */
inline std::vector<size_t> sortedIndices(const std::vector<double> &weights) {
// Get the number of elements in the 'weights' vector.
const size_t n = weights.size();
// Create a vector of 'indices'.
std::vector<size_t> indices(n);
std::iota(indices.begin(), indices.end(), 0);
// Sort the 'indices' based on the values in 'weights'.
std::stable_sort(indices.begin(), indices.end(),
[&weights](const size_t i0, const size_t i1) {
return weights[i0] < weights[i1];
});
return indices;
}
/****************************************************************/
template <class FACTOR>
std::vector<size_t> kruskal(const FactorGraph<FACTOR> &fg,
const std::vector<double> &weights) {
if (fg.size() != weights.size()) {
throw std::runtime_error(
"kruskal() failure: fg.size() != weights.size(), all factors need to "
"assigned a weight");
}
// Create an index from variables to factor indices.
const VariableIndex variableIndex(fg);
// Get indices in sort-order of the weights.
const std::vector<size_t> sortedIndices =
gtsam::utils::sortedIndices(weights);
// Create a vector to hold MST indices.
const size_t n = variableIndex.size();
std::vector<size_t> treeIndices;
treeIndices.reserve(n - 1);
// Initialize disjoint-set forest to keep track of merged Keys.
DSFMap<Key> dsf;
// Loop over all edges in order of increasing weight.
size_t count = 0;
for (const size_t index : sortedIndices) {
const auto factor = fg[index];
// Ignore non-binary edges.
if (factor->size() != 2) continue;
// Find the representatives of the sets for both the Keys in the binary
// factor.
const auto u = dsf.find(factor->front()), v = dsf.find(factor->back());
// Check if there is a potential loop.
const bool loop = (u == v);
if (!loop) {
// Merge the sets.
dsf.merge(u, v);
// Add the current index to the tree.
treeIndices.push_back(index);
// Break if all the Keys have been added to the tree.
if (++count == n - 1) break;
}
}
return treeIndices;
}
} // namespace gtsam::utils

41
gtsam/base/kruskal.h Normal file
View File

@ -0,0 +1,41 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, 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 kruskal.h
* @date Dec 31, 2009
* @author Frank Dellaert
* @author Yong-Dian Jian
* @author Ankur Roy Chowdhury
*/
#pragma once
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/FactorGraph.h>
#include <vector>
namespace gtsam::utils {
/**
* Compute the minimum spanning tree (MST) using Kruskal's algorithm
* @param fg Factor graph
* @param weights Weights of the edges/factors in the factor graph
* @return Edge/factor indices spanning the MST
* @note Only binary factors are considered while constructing the spanning tree
* @note The indices of 'weights' should match the indices of the edges in the factor graph
*/
template <class FACTOR>
std::vector<size_t> kruskal(const FactorGraph<FACTOR> &fg,
const std::vector<double> &weights);
} // namespace gtsam::utils
#include <gtsam/base/kruskal-inl.h>

94
gtsam/base/numericalDerivative.h
View File

@ -71,10 +71,10 @@ typename Eigen::Matrix<double, N, 1> numericalGradient(
std::function<double(const X&)> h, const X& x, double delta = 1e-5) {
double factor = 1.0 / (2.0 * delta);
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<manifold_tag, typename traits<X>::structure_category>::value),
static_assert(
(std::is_base_of<manifold_tag, typename traits<X>::structure_category>::value),
"Template argument X must be a manifold type.");
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified.");
static_assert(N>0, "Template argument X must be fixed-size type or N must be specified.");
// Prepare a tangent vector to perturb x with, only works for fixed size
typename traits<X>::TangentVector d;
@ -111,13 +111,13 @@ typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
std::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
typedef traits<Y> TraitsY;
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
"Template argument X must be a manifold type.");
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified.");
static_assert(N>0, "Template argument X must be fixed-size type or N must be specified.");
typedef traits<X> TraitsX;
// get value at x, and corresponding chart
@ -165,9 +165,9 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const
template<class Y, class X1, class X2, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const std::function<Y(const X1&, const X2&)>& h,
const X1& x1, const X2& x2, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2)), x1, delta);
@ -194,9 +194,9 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(cons
template<class Y, class X1, class X2, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(std::function<Y(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta = 1e-5) {
// BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
// static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
// "Template argument X1 must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1), x2, delta);
@ -226,9 +226,9 @@ template<class Y, class X1, class X2, class X3, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3)),
@ -259,9 +259,9 @@ template<class Y, class X1, class X2, class X3, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3)),
@ -292,9 +292,9 @@ template<class Y, class X1, class X2, class X3, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
"Template argument X3 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1),
@ -325,9 +325,9 @@ template<class Y, class X1, class X2, class X3, class X4, int N = traits<X1>::di
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
@ -359,9 +359,9 @@ template<class Y, class X1, class X2, class X3, class X4, int N = traits<X2>::di
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
@ -393,9 +393,9 @@ template<class Y, class X1, class X2, class X3, class X4, int N = traits<X3>::di
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
"Template argument X3 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
@ -427,9 +427,9 @@ template<class Y, class X1, class X2, class X3, class X4, int N = traits<X4>::di
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
"Template argument X4 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
@ -462,9 +462,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, int N = trai
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
@ -498,9 +498,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, int N = trai
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
@ -534,9 +534,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, int N = trai
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
@ -570,9 +570,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, int N = trai
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
@ -606,9 +606,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, int N = trai
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X5, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
@ -643,9 +643,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, in
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
@ -680,9 +680,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, in
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
@ -717,9 +717,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, in
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
@ -754,9 +754,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, in
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
@ -791,9 +791,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, in
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X5, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
@ -829,9 +829,9 @@ typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X6, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
@ -860,7 +860,7 @@ inline typename internal::FixedSizeMatrix<Y,X6>::type numericalDerivative66(Y (*
template<class X>
inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(std::function<double(const X&)> f, const X& x,
double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
"Template argument X must be a manifold type.");
typedef Eigen::Matrix<double, traits<X>::dimension, 1> VectorD;
typedef std::function<double(const X&)> F;

2
gtsam/base/std_optional_serialization.h
View File

@ -67,7 +67,7 @@ void save(Archive& ar, const std::optional<T>& t, const unsigned int /*version*/
// It is an inherent limitation to the serialization of optional.hpp
// that the underlying type must be either a pointer or must have a
// default constructor.
BOOST_STATIC_ASSERT(boost::serialization::detail::is_default_constructible<T>::value || boost::is_pointer<T>::value);
static_assert(boost::serialization::detail::is_default_constructible<T>::value || boost::is_pointer<T>::value);
const bool tflag = t.has_value();
ar << boost::serialization::make_nvp("initialized", tflag);
if (tflag) {

6
gtsam/base/tests/testGroup.cpp
View File

@ -80,7 +80,7 @@ using namespace gtsam;
typedef Symmetric<2> S2;
TEST(Group, S2) {
S2 e, s1 = S2::Transposition(0, 1);
BOOST_CONCEPT_ASSERT((IsGroup<S2>));
GTSAM_CONCEPT_ASSERT(IsGroup<S2>);
EXPECT(check_group_invariants(e, s1));
}
@ -88,7 +88,7 @@ TEST(Group, S2) {
typedef Symmetric<3> S3;
TEST(Group, S3) {
S3 e, s1 = S3::Transposition(0, 1), s2 = S3::Transposition(1, 2);
BOOST_CONCEPT_ASSERT((IsGroup<S3>));
GTSAM_CONCEPT_ASSERT(IsGroup<S3>);
EXPECT(check_group_invariants(e, s1));
EXPECT(assert_equal(s1, s1 * e));
EXPECT(assert_equal(s1, e * s1));
@ -127,7 +127,7 @@ struct traits<Dih6> : internal::MultiplicativeGroupTraits<Dih6> {
TEST(Group, Dih6) {
Dih6 e, g(S2::Transposition(0, 1),
S3::Transposition(0, 1) * S3::Transposition(1, 2));
BOOST_CONCEPT_ASSERT((IsGroup<Dih6>));
GTSAM_CONCEPT_ASSERT(IsGroup<Dih6>);
EXPECT(check_group_invariants(e, g));
EXPECT(assert_equal(e, compose_pow(g, 0)));
EXPECT(assert_equal(g, compose_pow(g, 1)));

105
gtsam/base/tests/testKruskal.cpp Normal file
View File

@ -0,0 +1,105 @@
/* ----------------------------------------------------------------------------
* 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 testKruskal
* @brief Unit tests for Kruskal's minimum spanning tree algorithm
* @author Ankur Roy Chowdhury
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/kruskal.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <list>
#include <memory>
#include <vector>
gtsam::GaussianFactorGraph makeTestGaussianFactorGraph() {
using namespace gtsam;
using namespace symbol_shorthand;
GaussianFactorGraph gfg;
Matrix I = I_2x2;
Vector2 b(0, 0);
const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));
gfg.emplace_shared<JacobianFactor>(X(1), I, X(2), I, b, model);
gfg.emplace_shared<JacobianFactor>(X(1), I, X(3), I, b, model);
gfg.emplace_shared<JacobianFactor>(X(1), I, X(4), I, b, model);
gfg.emplace_shared<JacobianFactor>(X(2), I, X(3), I, b, model);
gfg.emplace_shared<JacobianFactor>(X(2), I, X(4), I, b, model);
gfg.emplace_shared<JacobianFactor>(X(3), I, X(4), I, b, model);
return gfg;
}
gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph() {
using namespace gtsam;
using namespace symbol_shorthand;
NonlinearFactorGraph nfg;
const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));
nfg.emplace_shared<BetweenFactor<Rot3>>(X(1), X(2), Rot3(), model);
nfg.emplace_shared<BetweenFactor<Rot3>>(X(1), X(3), Rot3(), model);
nfg.emplace_shared<BetweenFactor<Rot3>>(X(1), X(4), Rot3(), model);
nfg.emplace_shared<BetweenFactor<Rot3>>(X(2), X(3), Rot3(), model);
nfg.emplace_shared<BetweenFactor<Rot3>>(X(2), X(4), Rot3(), model);
nfg.emplace_shared<BetweenFactor<Rot3>>(X(3), X(4), Rot3(), model);
return nfg;
}
/* ************************************************************************* */
TEST(kruskal, GaussianFactorGraph) {
using namespace gtsam;
// Create factor graph.
const auto g = makeTestGaussianFactorGraph();
// Assign weights to all the edges in the graph.
const auto weights = std::vector<double>(g.size(), 1.0);
const auto mstEdgeIndices = utils::kruskal(g, weights);
EXPECT(mstEdgeIndices[0] == 0);
EXPECT(mstEdgeIndices[1] == 1);
EXPECT(mstEdgeIndices[2] == 2);
}
/* ************************************************************************* */
TEST(kruskal, NonlinearFactorGraph) {
using namespace gtsam;
// Create factor graph.
const auto g = makeTestNonlinearFactorGraph();
// Assign weights to all the edges in the graph.
const auto weights = std::vector<double>(g.size(), 1.0);
const auto mstEdgeIndices = utils::kruskal(g, weights);
EXPECT(mstEdgeIndices[0] == 0);
EXPECT(mstEdgeIndices[1] == 1);
EXPECT(mstEdgeIndices[2] == 2);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

47
gtsam/base/tests/testMatrix.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <iostream>
#include <sstream>
#include <optional>
@ -858,8 +857,7 @@ TEST(Matrix, qr )
Matrix expectedR = (Matrix(6, 4) << 15, 0, -8.3333, 0, 00, 11.1803, 0, -2.2361, 00, 0,
7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0).finished();
Matrix Q, R;
boost::tie(Q, R) = qr(A);
const auto [Q, R] = qr(A);
EXPECT(assert_equal(expectedQ, Q, 1e-4));
EXPECT(assert_equal(expectedR, R, 1e-4));
EXPECT(assert_equal(A, Q*R, 1e-14));
@ -911,15 +909,12 @@ TEST(Matrix, weighted_elimination )
// perform elimination
Matrix A1 = A;
Vector b1 = b;
std::list<boost::tuple<Vector, double, double> > solution =
std::list<std::tuple<Vector, double, double> > solution =
weighted_eliminate(A1, b1, sigmas);
// unpack and verify
size_t i = 0;
for (const auto& tuple : solution) {
Vector r;
double di, sigma;
boost::tie(r, di, sigma) = tuple;
for (const auto& [r, di, sigma] : solution) {
EXPECT(assert_equal(r, expectedR.row(i))); // verify r
DOUBLES_EQUAL(d(i), di, 1e-8); // verify d
DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma
@ -1143,10 +1138,7 @@ TEST(Matrix, DLT )
1.89, 2.24, 3.99, 3.24, 3.84, 6.84, 18.09, 21.44, 38.19,
2.24, 2.48, 6.24, 3.08, 3.41, 8.58, 24.64, 27.28, 68.64
).finished();
int rank;
double error;
Vector actual;
boost::tie(rank,error,actual) = DLT(A);
const auto [rank,error,actual] = DLT(A);
Vector expected = (Vector(9) << -0.0, 0.2357, 0.4714, -0.2357, 0.0, - 0.4714,-0.4714, 0.4714, 0.0).finished();
EXPECT_LONGS_EQUAL(8,rank);
EXPECT_DOUBLES_EQUAL(0,error,1e-8);
@ -1154,17 +1146,30 @@ TEST(Matrix, DLT )
}
//******************************************************************************
TEST(Matrix , IsVectorSpace) {
BOOST_CONCEPT_ASSERT((IsVectorSpace<Matrix24>));
typedef Eigen::Matrix<double,2,3,Eigen::RowMajor> RowMajor;
BOOST_CONCEPT_ASSERT((IsVectorSpace<RowMajor>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Matrix>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector>));
typedef Eigen::Matrix<double,1,-1> RowVector;
BOOST_CONCEPT_ASSERT((IsVectorSpace<RowVector>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>));
TEST(Matrix, Matrix24IsVectorSpace) {
GTSAM_CONCEPT_ASSERT(IsVectorSpace<Matrix24>);
}
TEST(Matrix, RowMajorIsVectorSpace) {
typedef Eigen::Matrix<double, 2, 3, Eigen::RowMajor> RowMajor;
GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowMajor>);
}
TEST(Matrix, MatrixIsVectorSpace) {
GTSAM_CONCEPT_ASSERT(IsVectorSpace<Matrix>);
}
TEST(Matrix, VectorIsVectorSpace) {
GTSAM_CONCEPT_ASSERT(IsVectorSpace<Vector>);
}
TEST(Matrix, RowVectorIsVectorSpace) {
typedef Eigen::Matrix<double, 1, -1> RowVector;
GTSAM_CONCEPT_ASSERT1(IsVectorSpace<RowVector>);
GTSAM_CONCEPT_ASSERT2(IsVectorSpace<Vector5>);
}
//******************************************************************************
TEST(Matrix, AbsoluteError) {
double a = 2000, b = 1997, tol = 1e-1;
bool isEqual;

21
gtsam/base/tests/testVector.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <iostream>
using namespace std;
@ -155,8 +154,7 @@ TEST(Vector, weightedPseudoinverse )
Vector weights = sigmas.array().square().inverse();
// perform solve
Vector actual; double precision;
boost::tie(actual, precision) = weightedPseudoinverse(x, weights);
const auto [actual, precision] = weightedPseudoinverse(x, weights);
// construct expected
Vector expected(2);
@ -180,8 +178,7 @@ TEST(Vector, weightedPseudoinverse_constraint )
sigmas(0) = 0.0; sigmas(1) = 0.2;
Vector weights = sigmas.array().square().inverse();
// perform solve
Vector actual; double precision;
boost::tie(actual, precision) = weightedPseudoinverse(x, weights);
const auto [actual, precision] = weightedPseudoinverse(x, weights);
// construct expected
Vector expected(2);
@ -198,8 +195,7 @@ TEST(Vector, weightedPseudoinverse_nan )
Vector a = (Vector(4) << 1., 0., 0., 0.).finished();
Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished();
Vector weights = sigmas.array().square().inverse();
Vector pseudo; double precision;
boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);
const auto [pseudo, precision] = weightedPseudoinverse(a, weights);
Vector expected = (Vector(4) << 1., 0., 0.,0.).finished();
EXPECT(assert_equal(expected, pseudo));
@ -270,11 +266,14 @@ TEST(Vector, linear_dependent3 )
}
//******************************************************************************
TEST(Vector, IsVectorSpace) {
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector>));
TEST(Vector, VectorIsVectorSpace) {
GTSAM_CONCEPT_ASSERT1(IsVectorSpace<Vector5>);
GTSAM_CONCEPT_ASSERT2(IsVectorSpace<Vector>);
}
TEST(Vector, RowVectorIsVectorSpace) {
typedef Eigen::Matrix<double,1,-1> RowVector;
BOOST_CONCEPT_ASSERT((IsVectorSpace<RowVector>));
GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowVector>);
}
/* ************************************************************************* */

58
gtsam/base/timing.cpp
View File

@ -19,9 +19,6 @@
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <boost/algorithm/string/replace.hpp>
#include <boost/format.hpp>
#include <cmath>
#include <cstddef>
#include <cassert>
@ -34,6 +31,9 @@
namespace gtsam {
namespace internal {
// a static shared_ptr to TimingOutline with nullptr as the pointer
const static std::shared_ptr<TimingOutline> nullTimingOutline;
GTSAM_EXPORT std::shared_ptr<TimingOutline> gTimingRoot(
new TimingOutline("Total", getTicTocID("Total")));
GTSAM_EXPORT std::weak_ptr<TimingOutline> gCurrentTimer(gTimingRoot);
@ -56,13 +56,16 @@ void TimingOutline::add(size_t usecs, size_t usecsWall) {
TimingOutline::TimingOutline(const std::string& label, size_t id) :
id_(id), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_(
0), lastChildOrder_(0), label_(label) {
#ifdef GTSAM_USE_BOOST_FEATURES
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
timer_.stop();
#endif
#endif
}
/* ************************************************************************* */
size_t TimingOutline::time() const {
#ifdef GTSAM_USE_BOOST_FEATURES
size_t time = 0;
bool hasChildren = false;
for(const ChildMap::value_type& child: children_) {
@ -73,12 +76,16 @@ size_t TimingOutline::time() const {
return time;
else
return t_;
#else
return 0;
#endif
}
/* ************************************************************************* */
void TimingOutline::print(const std::string& outline) const {
#ifdef GTSAM_USE_BOOST_FEATURES
std::string formattedLabel = label_;
boost::replace_all(formattedLabel, "_", " ");
std::replace(formattedLabel.begin(), formattedLabel.end(), '_', ' ');
std::cout << outline << "-" << formattedLabel << ": " << self() << " CPU ("
<< n_ << " times, " << wall() << " wall, " << secs() << " children, min: "
<< min() << " max: " << max() << ")\n";
@ -95,11 +102,12 @@ void TimingOutline::print(const std::string& outline) const {
order_child.second->print(childOutline);
}
std::cout.flush();
#endif
}
void TimingOutline::print2(const std::string& outline,
const double parentTotal) const {
#ifdef GTSAM_USE_BOOST_FEATURES
const int w1 = 24, w2 = 2, w3 = 6, w4 = 8, precision = 2;
const double selfTotal = self(), selfMean = selfTotal / double(n_);
const double childTotal = secs();
@ -138,11 +146,13 @@ void TimingOutline::print2(const std::string& outline,
child.second->print2(childOutline, selfTotal);
}
}
#endif
}
/* ************************************************************************* */
const std::shared_ptr<TimingOutline>& TimingOutline::child(size_t child,
const std::string& label, const std::weak_ptr<TimingOutline>& thisPtr) {
#ifdef GTSAM_USE_BOOST_FEATURES
assert(thisPtr.lock().get() == this);
std::shared_ptr<TimingOutline>& result = children_[child];
if (!result) {
@ -153,10 +163,15 @@ const std::shared_ptr<TimingOutline>& TimingOutline::child(size_t child,
result->parent_ = thisPtr;
}
return result;
#else
return nullTimingOutline;
#endif
}
/* ************************************************************************* */
void TimingOutline::tic() {
// Disable this entire function if we are not using boost
#ifdef GTSAM_USE_BOOST_FEATURES
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
assert(timer_.is_stopped());
timer_.start();
@ -169,10 +184,14 @@ void TimingOutline::tic() {
#ifdef GTSAM_USE_TBB
tbbTimer_ = tbb::tick_count::now();
#endif
#endif
}
/* ************************************************************************* */
void TimingOutline::toc() {
// Disable this entire function if we are not using boost
#ifdef GTSAM_USE_BOOST_FEATURES
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
assert(!timer_.is_stopped());
@ -200,10 +219,12 @@ void TimingOutline::toc() {
#endif
add(cpuTime, wallTime);
#endif
}
/* ************************************************************************* */
void TimingOutline::finishedIteration() {
#ifdef GTSAM_USE_BOOST_FEATURES
if (tIt_ > tMax_)
tMax_ = tIt_;
if (tMin_ == 0 || tIt_ < tMin_)
@ -212,55 +233,64 @@ void TimingOutline::finishedIteration() {
for(ChildMap::value_type& child: children_) {
child.second->finishedIteration();
}
#endif
}
/* ************************************************************************* */
size_t getTicTocID(const char *descriptionC) {
// disable anything which refers to TimingOutline as well, for good measure
#ifdef GTSAM_USE_BOOST_FEATURES
const std::string description(descriptionC);
// Global (static) map from strings to ID numbers and current next ID number
static size_t nextId = 0;
static gtsam::FastMap<std::string, size_t> idMap;
// Retrieve or add this string
gtsam::FastMap<std::string, size_t>::const_iterator it = idMap.find(
description);
auto it = idMap.find(description);
if (it == idMap.end()) {
it = idMap.insert(std::make_pair(description, nextId)).first;
it = idMap.insert({description, nextId}).first;
++nextId;
}
// Return ID
return it->second;
#else
return 0;
#endif
}
/* ************************************************************************* */
void tic(size_t id, const char *labelC) {
// disable anything which refers to TimingOutline as well, for good measure
#ifdef GTSAM_USE_BOOST_FEATURES
const std::string label(labelC);
std::shared_ptr<TimingOutline> node = //
gCurrentTimer.lock()->child(id, label, gCurrentTimer);
gCurrentTimer = node;
node->tic();
#endif
}
/* ************************************************************************* */
void toc(size_t id, const char *label) {
// disable anything which refers to TimingOutline as well, for good measure
#ifdef GTSAM_USE_BOOST_FEATURES
std::shared_ptr<TimingOutline> current(gCurrentTimer.lock());
if (id != current->id_) {
gTimingRoot->print();
throw std::invalid_argument(
(boost::format(
"gtsam timing: Mismatched tic/toc: gttoc(\"%s\") called when last tic was \"%s\".")
% label % current->label_).str());
"gtsam timing: Mismatched tic/toc: gttoc(\"" + std::string(label) +
"\") called when last tic was \"" + current->label_ + "\".");
}
if (!current->parent_.lock()) {
gTimingRoot->print();
throw std::invalid_argument(
(boost::format(
"gtsam timing: Mismatched tic/toc: extra gttoc(\"%s\"), already at the root")
% label).str());
"gtsam timing: Mismatched tic/toc: extra gttoc(\"" + std::string(label) +
"\"), already at the root");
}
current->toc();
gCurrentTimer = current->parent_;
#endif
}
} // namespace internal

16
gtsam/base/timing.h
View File

@ -21,7 +21,9 @@
#include <gtsam/dllexport.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#ifdef GTSAM_USE_BOOST_FEATURES
#include <boost/version.hpp>
#endif
#include <memory>
#include <cstddef>
@ -105,6 +107,7 @@
// have matching gttic/gttoc statments. You may want to consider reorganizing your timing
// outline to match the scope of your code.
#ifdef GTSAM_USE_BOOST_FEATURES
// Automatically use the new Boost timers if version is recent enough.
#if BOOST_VERSION >= 104800
# ifndef GTSAM_DISABLE_NEW_TIMERS
@ -118,6 +121,7 @@
# include <boost/timer.hpp>
# include <gtsam/base/types.h>
#endif
#endif
#ifdef GTSAM_USE_TBB
# include <tbb/tick_count.h>
@ -160,6 +164,8 @@ namespace gtsam {
typedef FastMap<size_t, std::shared_ptr<TimingOutline> > ChildMap;
ChildMap children_; ///< subtrees
// disable all timers if not using boost
#ifdef GTSAM_USE_BOOST_FEATURES
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
boost::timer::cpu_timer timer_;
#else
@ -168,6 +174,7 @@ namespace gtsam {
#endif
#ifdef GTSAM_USE_TBB
tbb::tick_count tbbTimer_;
#endif
#endif
void add(size_t usecs, size_t usecsWall);
@ -176,11 +183,20 @@ namespace gtsam {
GTSAM_EXPORT TimingOutline(const std::string& label, size_t myId);
GTSAM_EXPORT size_t time() const; ///< time taken, including children
double secs() const { return double(time()) / 1000000.0;} ///< time taken, in seconds, including children
#ifdef GTSAM_USE_BOOST_FEATURES
double self() const { return double(t_) / 1000000.0;} ///< self time only, in seconds
double wall() const { return double(tWall_) / 1000000.0;} ///< wall time, in seconds
double min() const { return double(tMin_) / 1000000.0;} ///< min time, in seconds
double max() const { return double(tMax_) / 1000000.0;} ///< max time, in seconds
double mean() const { return self() / double(n_); } ///< mean self time, in seconds
#else
// make them no-ops if not using boost
double self() const { return -1.; } ///< self time only, in seconds
double wall() const { return -1.; } ///< wall time, in seconds
double min() const { return -1.; } ///< min time, in seconds
double max() const { return -1.; } ///< max time, in seconds
double mean() const { return -1.; } ///< mean self time, in seconds
#endif
GTSAM_EXPORT void print(const std::string& outline = "") const;
GTSAM_EXPORT void print2(const std::string& outline = "", const double parentTotal = -1.0) const;
GTSAM_EXPORT const std::shared_ptr<TimingOutline>&

44
gtsam/base/types.h
View File

@ -20,8 +20,10 @@
#pragma once
#include <gtsam/dllexport.h>
#ifdef GTSAM_USE_BOOST_FEATURES
#include <boost/concept/assert.hpp>
#include <boost/range/concepts.hpp>
#endif
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <cstddef>
@ -153,32 +155,6 @@ namespace gtsam {
operator T() const { return value; }
};
/* ************************************************************************* */
/** A helper class that behaves as a container with one element, and works with
* boost::range */
template<typename T>
class ListOfOneContainer {
T element_;
public:
typedef T value_type;
typedef const T* const_iterator;
typedef T* iterator;
ListOfOneContainer(const T& element) : element_(element) {}
const T* begin() const { return &element_; }
const T* end() const { return &element_ + 1; }
T* begin() { return &element_; }
T* end() { return &element_ + 1; }
size_t size() const { return 1; }
};
BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept<ListOfOneContainer<int> >));
/** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */
template<typename T>
ListOfOneContainer<T> ListOfOne(const T& element) {
return ListOfOneContainer<T>(element);
}
/* ************************************************************************* */
#ifdef __clang__
# pragma clang diagnostic push
@ -234,27 +210,27 @@ namespace gtsam {
#if (_MSC_VER < 1800)
#include <boost/math/special_functions/fpclassify.hpp>
#include <cmath>
namespace std {
template<typename T> inline int isfinite(T a) {
return (int)boost::math::isfinite(a); }
return (int)std::isfinite(a); }
template<typename T> inline int isnan(T a) {
return (int)boost::math::isnan(a); }
return (int)std::isnan(a); }
template<typename T> inline int isinf(T a) {
return (int)boost::math::isinf(a); }
return (int)std::isinf(a); }
}
#endif
#include <boost/math/constants/constants.hpp>
#include <cmath>
#ifndef M_PI
#define M_PI (boost::math::constants::pi<double>())
#define M_PI (3.14159265358979323846)
#endif
#ifndef M_PI_2
#define M_PI_2 (boost::math::constants::pi<double>() / 2.0)
#define M_PI_2 (M_PI / 2.0)
#endif
#ifndef M_PI_4
#define M_PI_4 (boost::math::constants::pi<double>() / 4.0)
#define M_PI_4 (M_PI / 4.0)
#endif
#endif

21
gtsam/base/utilities.h
View File

@ -28,16 +28,9 @@ private:
}
// boost::index_sequence was introduced in 1.66, so we'll manually define an
// implementation if user has 1.65. boost::index_sequence is used to get array
// indices that align with a parameter pack.
#include <boost/version.hpp>
#if BOOST_VERSION >= 106600
#include <boost/mp11/integer_sequence.hpp>
#else
namespace boost {
namespace mp11 {
namespace gtsam {
// Adapted from https://stackoverflow.com/a/32223343/9151520
// An adaptation of boost::mp11::index_sequence
template <size_t... Ints>
struct index_sequence {
using type = index_sequence;
@ -49,13 +42,11 @@ template <class Sequence1, class Sequence2>
struct _merge_and_renumber;
template <size_t... I1, size_t... I2>
struct _merge_and_renumber<index_sequence<I1...>, index_sequence<I2...> >
struct _merge_and_renumber<index_sequence<I1...>, index_sequence<I2...>>
: index_sequence<I1..., (sizeof...(I1) + I2)...> {};
} // namespace detail
template <size_t N>
struct make_index_sequence
: detail::_merge_and_renumber<
typename make_index_sequence<N / 2>::type,
struct make_index_sequence : detail::_merge_and_renumber<typename make_index_sequence<N / 2>::type,
typename make_index_sequence<N - N / 2>::type> {};
template <>
struct make_index_sequence<0> : index_sequence<> {};
@ -63,6 +54,4 @@ template <>
struct make_index_sequence<1> : index_sequence<0> {};
template <class... T>
using index_sequence_for = make_index_sequence<sizeof...(T)>;
} // namespace mp11
} // namespace boost
#endif
} // namespace gtsam

12
gtsam/basis/tests/testChebyshev2.cpp
View File

@ -25,10 +25,10 @@
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <functional>
using namespace std;
using namespace gtsam;
using namespace boost::placeholders;
namespace {
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
@ -119,8 +119,8 @@ TEST(Chebyshev2, InterpolateVector) {
EXPECT(assert_equal(expected, fx(X, actualH), 1e-9));
// Check derivative
std::function<Vector2(ParameterMatrix<2>)> f = boost::bind(
&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, nullptr);
std::function<Vector2(ParameterMatrix<2>)> f = std::bind(
&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, std::placeholders::_1, nullptr);
Matrix numericalH =
numericalDerivative11<Vector2, ParameterMatrix<2>, 2 * N>(f, X);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
@ -378,7 +378,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor) {
// Test Jacobian
Matrix expectedH = numericalDerivative11<Vector2, ParameterMatrix<M>, M * N>(
boost::bind(&VecD::operator(), fx, _1, nullptr), X);
std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-7));
}
@ -410,7 +410,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) {
VecD vecd(N, points(0), 0, T);
vecd(X, actualH);
Matrix expectedH = numericalDerivative11<Vector1, ParameterMatrix<M>, M * N>(
boost::bind(&VecD::operator(), vecd, _1, nullptr), X);
std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-6));
}
@ -427,7 +427,7 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) {
EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);
Matrix expectedH = numericalDerivative11<double, ParameterMatrix<M>, M * N>(
boost::bind(&CompFunc::operator(), fx, _1, nullptr), X);
std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-7));
}

5
gtsam/discrete/AlgebraicDecisionTree.h
View File

@ -24,6 +24,7 @@
#include <algorithm>
#include <map>
#include <string>
#include <iomanip>
#include <vector>
namespace gtsam {
@ -162,7 +163,9 @@ namespace gtsam {
const typename Base::LabelFormatter& labelFormatter =
&DefaultFormatter) const {
auto valueFormatter = [](const double& v) {
return (boost::format("%4.8g") % v).str();
std::stringstream ss;
ss << std::setw(4) << std::setprecision(8) << v;
return ss.str();
};
Base::print(s, labelFormatter, valueFormatter);
}

19
gtsam/discrete/DecisionTree-inl.h
View File

@ -22,7 +22,6 @@
#include <gtsam/discrete/DecisionTree.h>
#include <algorithm>
#include <boost/format.hpp>
#include <cmath>
#include <fstream>
@ -33,6 +32,7 @@
#include <string>
#include <vector>
#include <optional>
#include <cassert>
namespace gtsam {
@ -192,8 +192,8 @@ namespace gtsam {
~Choice() override {
#ifdef DT_DEBUG_MEMORY
std::std::cout << Node::nrNodes << " destructing (Choice) " << this->id()
<< std::std::endl;
std::cout << Node::nrNodes << " destructing (Choice) " << this->id()
<< std::endl;
#endif
}
@ -282,9 +282,9 @@ namespace gtsam {
const ValueFormatter& valueFormatter) const override {
std::cout << s << " Choice(";
std::cout << labelFormatter(label_) << ") " << std::endl;
for (size_t i = 0; i < branches_.size(); i++)
branches_[i]->print((boost::format("%s %d") % s % i).str(),
labelFormatter, valueFormatter);
for (size_t i = 0; i < branches_.size(); i++) {
branches_[i]->print(s + " " + std::to_string(i), labelFormatter, valueFormatter);
}
}
/** output to graphviz (as a a graph) */
@ -643,11 +643,8 @@ namespace gtsam {
// Create a simple choice node with values as leaves.
if (size != nrChoices) {
std::cout << "Trying to create DD on " << begin->first << std::endl;
std::cout << boost::format(
"DecisionTree::create: expected %d values but got %d "
"instead") %
nrChoices % size
<< std::endl;
std::cout << "DecisionTree::create: expected " << nrChoices
<< " values but got " << size << " instead" << std::endl;
throw std::invalid_argument("DecisionTree::create invalid argument");
}
auto choice = std::make_shared<Choice>(begin->first, endY - beginY);

6
gtsam/discrete/DecisionTree.h
View File

@ -424,10 +424,10 @@ namespace gtsam {
template <typename L, typename T1, typename T2>
std::pair<DecisionTree<L, T1>, DecisionTree<L, T2> > unzip(
const DecisionTree<L, std::pair<T1, T2> >& input) {
return std::make_pair(
return {
DecisionTree<L, T1>(input, [](std::pair<T1, T2> i) { return i.first; }),
DecisionTree<L, T2>(input,
[](std::pair<T1, T2> i) { return i.second; }));
DecisionTree<L, T2>(input, [](std::pair<T1, T2> i) { return i.second; })
};
}
} // namespace gtsam

29
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -22,7 +22,6 @@
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <boost/format.hpp>
#include <utility>
using namespace std;
@ -79,8 +78,9 @@ namespace gtsam {
const KeyFormatter& formatter) const {
cout << s;
cout << " f[";
for (auto&& key : keys())
cout << boost::format(" (%1%,%2%),") % formatter(key) % cardinality(key);
for (auto&& key : keys()) {
cout << " (" << formatter(key) << "," << cardinality(key) << "),";
}
cout << " ]" << endl;
ADT::print("", formatter);
}
@ -104,13 +104,12 @@ namespace gtsam {
/* ************************************************************************ */
DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
size_t nrFrontals, ADT::Binary op) const {
if (nrFrontals > size())
if (nrFrontals > size()) {
throw invalid_argument(
(boost::format(
"DecisionTreeFactor::combine: invalid number of frontal "
"keys %d, nr.keys=%d") %
nrFrontals % size())
.str());
"keys " +
std::to_string(nrFrontals) + ", nr.keys=" + std::to_string(size()));
}
// sum over nrFrontals keys
size_t i;
@ -132,13 +131,13 @@ namespace gtsam {
/* ************************************************************************ */
DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
const Ordering& frontalKeys, ADT::Binary op) const {
if (frontalKeys.size() > size())
if (frontalKeys.size() > size()) {
throw invalid_argument(
(boost::format(
"DecisionTreeFactor::combine: invalid number of frontal "
"keys %d, nr.keys=%d") %
frontalKeys.size() % size())
.str());
"keys " +
std::to_string(frontalKeys.size()) + ", nr.keys=" +
std::to_string(size()));
}
// sum over nrFrontals keys
size_t i;
@ -193,7 +192,9 @@ namespace gtsam {
/* ************************************************************************ */
static std::string valueFormatter(const double& v) {
return (boost::format("%4.2g") % v).str();
std::stringstream ss;
ss << std::setw(4) << std::setprecision(2) << std::fixed << v;
return ss.str();
}
/** output to graphviz format, stream version */

3
gtsam/discrete/DecisionTreeFactor.h
View File

@ -104,7 +104,7 @@ namespace gtsam {
return ADT::operator()(values);
}
/// Evaluate probability density, sugar.
/// Evaluate probability distribution, sugar.
double operator()(const DiscreteValues& values) const override {
return ADT::operator()(values);
}
@ -268,5 +268,4 @@ namespace gtsam {
// traits
template <>
struct traits<DecisionTreeFactor> : public Testable<DecisionTreeFactor> {};
} // namespace gtsam

7
gtsam/discrete/DiscreteBayesNet.cpp
View File

@ -20,8 +20,6 @@
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <boost/range/adaptor/reversed.hpp>
namespace gtsam {
// Instantiate base class
@ -58,8 +56,9 @@ DiscreteValues DiscreteBayesNet::sample() const {
DiscreteValues DiscreteBayesNet::sample(DiscreteValues result) const {
// sample each node in turn in topological sort order (parents first)
for (auto conditional : boost::adaptors::reverse(*this))
conditional->sampleInPlace(&result);
for (auto it = std::make_reverse_iterator(end()); it != std::make_reverse_iterator(begin()); ++it) {
(*it)->sampleInPlace(&result);
}
return result;
}

2
gtsam/discrete/DiscreteBayesTree.h
View File

@ -63,7 +63,7 @@ class GTSAM_EXPORT DiscreteBayesTreeClique
/* ************************************************************************* */
/**
* @brief A Bayes tree representing a Discrete density.
* @brief A Bayes tree representing a Discrete distribution.
* @ingroup discrete
*/
class GTSAM_EXPORT DiscreteBayesTree

2
gtsam/discrete/DiscreteConditional.cpp
View File

@ -266,7 +266,7 @@ void DiscreteConditional::sampleInPlace(DiscreteValues* values) const {
size_t DiscreteConditional::sample(const DiscreteValues& parentsValues) const {
static mt19937 rng(2); // random number generator
// Get the correct conditional density
// Get the correct conditional distribution
ADT pFS = choose(parentsValues, true); // P(F|S=parentsValues)
// TODO(Duy): only works for one key now, seems horribly slow this way

5
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -140,8 +140,7 @@ namespace gtsam {
orderedKeys, product);
gttoc(lookup);
return std::make_pair(
std::dynamic_pointer_cast<DiscreteConditional>(lookup), max);
return {std::dynamic_pointer_cast<DiscreteConditional>(lookup), max};
}
/* ************************************************************************ */
@ -223,7 +222,7 @@ namespace gtsam {
std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
gttoc(divide);
return std::make_pair(conditional, sum);
return {conditional, sum};
}
/* ************************************************************************ */

3
gtsam/discrete/DiscreteKey.cpp
View File

@ -17,7 +17,6 @@
*/
#include <iostream>
#include <boost/format.hpp> // for key names
#include "DiscreteKey.h"
namespace gtsam {
@ -26,7 +25,7 @@ namespace gtsam {
DiscreteKeys::DiscreteKeys(const vector<int>& cs) {
for (size_t i = 0; i < cs.size(); i++) {
string name = boost::str(boost::format("v%1%") % i);
string name = "v" + std::to_string(i);
push_back(DiscreteKey(i, cs[i]));
}
}

7
gtsam/discrete/DiscreteLookupDAG.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/discrete/DiscreteLookupDAG.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <iterator>
#include <string>
#include <utility>
@ -118,8 +119,10 @@ DiscreteLookupDAG DiscreteLookupDAG::FromBayesNet(
DiscreteValues DiscreteLookupDAG::argmax(DiscreteValues result) const {
// Argmax each node in turn in topological sort order (parents first).
for (auto lookupTable : boost::adaptors::reverse(*this))
lookupTable->argmaxInPlace(&result);
for (auto it = std::make_reverse_iterator(end()); it != std::make_reverse_iterator(begin()); ++it) {
// dereference to get the sharedFactor to the lookup table
(*it)->argmaxInPlace(&result);
}
return result;
}
/* ************************************************************************** */

2
gtsam/discrete/DiscreteMarginals.h
View File

@ -41,7 +41,7 @@ class DiscreteMarginals {
DiscreteMarginals() {}
/** Construct a marginals class.
* @param graph The factor graph defining the full joint density on all variables.
* @param graph The factor graph defining the full joint distribution on all variables.
*/
DiscreteMarginals(const DiscreteFactorGraph& graph) {
bayesTree_ = graph.eliminateMultifrontal();

7
gtsam/discrete/DiscreteValues.cpp
View File

@ -17,7 +17,6 @@
#include <gtsam/discrete/DiscreteValues.h>
#include <boost/range/combine.hpp>
#include <sstream>
using std::cout;
@ -39,8 +38,10 @@ void DiscreteValues::print(const string& s,
/* ************************************************************************ */
bool DiscreteValues::equals(const DiscreteValues& x, double tol) const {
if (this->size() != x.size()) return false;
for (const auto values : boost::combine(*this, x)) {
if (values.get<0>() != values.get<1>()) return false;
auto it1 = x.begin();
auto it2 = this->begin();
for (; it1 != x.end(); ++it1, ++it2) {
if (it1->first != it2->first || it1->second != it2->second) return false;
}
return true;
}

145
gtsam/discrete/Signature.cpp
View File

@ -16,100 +16,57 @@
* @date Feb 27, 2011
*/
#include <sstream>
#include "Signature.h"
#include <boost/spirit/include/qi.hpp> // for parsing
#include <boost/spirit/include/phoenix.hpp> // for qi::_val
#include <cassert>
#include <sstream>
#include "gtsam/discrete/SignatureParser.h"
namespace gtsam {
using namespace std;
using namespace std;
namespace qi = boost::spirit::qi;
namespace ph = boost::phoenix;
// parser for strings of form "99/1 80/20" etc...
namespace parser {
typedef string::const_iterator It;
using boost::phoenix::val;
using boost::phoenix::ref;
using boost::phoenix::push_back;
// Special rows, true and false
Signature::Row F{1, 0}, T{0, 1};
// Special tables (inefficient, but do we care for user input?)
Signature::Table logic(bool ff, bool ft, bool tf, bool tt) {
Signature::Table t(4);
t[0] = ff ? T : F;
t[1] = ft ? T : F;
t[2] = tf ? T : F;
t[3] = tt ? T : F;
return t;
}
struct Grammar {
qi::rule<It, qi::space_type, Signature::Table()> table, or_, and_, rows;
qi::rule<It, Signature::Row()> true_, false_, row;
Grammar() {
table = or_ | and_ | rows;
or_ = qi::lit("OR")[qi::_val = logic(false, true, true, true)];
and_ = qi::lit("AND")[qi::_val = logic(false, false, false, true)];
rows = +(row | true_ | false_);
row = qi::double_ >> +("/" >> qi::double_);
true_ = qi::lit("T")[qi::_val = T];
false_ = qi::lit("F")[qi::_val = F];
}
} grammar;
} // \namespace parser
ostream& operator <<(ostream &os, const Signature::Row &row) {
ostream& operator<<(ostream& os, const Signature::Row& row) {
os << row[0];
for (size_t i = 1; i < row.size(); i++)
os << " " << row[i];
for (size_t i = 1; i < row.size(); i++) os << " " << row[i];
return os;
}
}
ostream& operator <<(ostream &os, const Signature::Table &table) {
for (size_t i = 0; i < table.size(); i++)
os << table[i] << endl;
ostream& operator<<(ostream& os, const Signature::Table& table) {
for (size_t i = 0; i < table.size(); i++) os << table[i] << endl;
return os;
}
}
Signature::Signature(const DiscreteKey& key, const DiscreteKeys& parents,
Signature::Signature(const DiscreteKey& key, const DiscreteKeys& parents,
const Table& table)
: key_(key), parents_(parents) {
operator=(table);
}
}
Signature::Signature(const DiscreteKey& key, const DiscreteKeys& parents,
Signature::Signature(const DiscreteKey& key, const DiscreteKeys& parents,
const std::string& spec)
: key_(key), parents_(parents) {
operator=(spec);
}
}
Signature::Signature(const DiscreteKey& key) :
key_(key) {
}
Signature::Signature(const DiscreteKey& key) : key_(key) {}
DiscreteKeys Signature::discreteKeys() const {
DiscreteKeys Signature::discreteKeys() const {
DiscreteKeys keys;
keys.push_back(key_);
for (const DiscreteKey& key : parents_) keys.push_back(key);
return keys;
}
}
KeyVector Signature::indices() const {
KeyVector Signature::indices() const {
KeyVector js;
js.push_back(key_.first);
for (const DiscreteKey& key : parents_) js.push_back(key.first);
return js;
}
}
vector<double> Signature::cpt() const {
vector<double> Signature::cpt() const {
vector<double> cpt;
if (table_) {
const size_t nrStates = table_->at(0).size();
@ -121,43 +78,37 @@ namespace gtsam {
}
}
return cpt;
}
}
Signature& Signature::operator,(const DiscreteKey& parent) {
Signature &Signature::operator,(const DiscreteKey& parent) {
parents_.push_back(parent);
return *this;
}
}
static void normalize(Signature::Row& row) {
static void normalize(Signature::Row& row) {
double sum = 0;
for (size_t i = 0; i < row.size(); i++)
sum += row[i];
for (size_t i = 0; i < row.size(); i++)
row[i] /= sum;
}
for (size_t i = 0; i < row.size(); i++) sum += row[i];
for (size_t i = 0; i < row.size(); i++) row[i] /= sum;
}
Signature& Signature::operator=(const string& spec) {
Signature& Signature::operator=(const string& spec) {
spec_ = spec;
Table table;
parser::It f = spec.begin(), l = spec.end();
bool success =
qi::phrase_parse(f, l, parser::grammar.table, qi::space, table);
if (success) {
auto table = SignatureParser::Parse(spec);
if (table) {
for (Row& row : *table) normalize(row);
table_ = *table;
}
return *this;
}
Signature& Signature::operator=(const Table& t) {
Table table = t;
for (Row& row : table) normalize(row);
table_ = table;
}
return *this;
}
}
Signature& Signature::operator=(const Table& t) {
Table table = t;
for(Row& row: table)
normalize(row);
table_ = table;
return *this;
}
ostream& operator <<(ostream &os, const Signature &s) {
ostream& operator<<(ostream& os, const Signature& s) {
os << s.key_.first;
if (s.parents_.empty()) {
os << " % ";
@ -173,21 +124,21 @@ namespace gtsam {
else
os << "spec could not be parsed" << endl;
return os;
}
}
Signature operator|(const DiscreteKey& key, const DiscreteKey& parent) {
Signature operator|(const DiscreteKey& key, const DiscreteKey& parent) {
Signature s(key);
return s, parent;
}
}
Signature operator%(const DiscreteKey& key, const string& parent) {
Signature operator%(const DiscreteKey& key, const string& parent) {
Signature s(key);
return s = parent;
}
}
Signature operator%(const DiscreteKey& key, const Signature::Table& parent) {
Signature operator%(const DiscreteKey& key, const Signature::Table& parent) {
Signature s(key);
return s = parent;
}
}
} // namespace gtsam

2
gtsam/discrete/Signature.h
View File

@ -25,7 +25,7 @@
namespace gtsam {
/**
* Signature for a discrete conditional density, used to construct conditionals.
* Signature for a discrete conditional distribution, used to construct conditionals.
*
* The format is (Key % string) for nodes with no parents,
* and (Key | Key, Key = string) for nodes with parents.

119
gtsam/discrete/SignatureParser.cpp Normal file
View File

@ -0,0 +1,119 @@
#include <gtsam/discrete/SignatureParser.h>
#include <algorithm>
#include <iterator>
#include <optional>
#include <sstream>
namespace gtsam {
using Row = std::vector<double>;
using Table = std::vector<Row>;
inline static Row ParseTrueRow() { return {0, 1}; }
inline static Row ParseFalseRow() { return {1, 0}; }
inline static Table ParseOr() {
return {ParseFalseRow(), ParseTrueRow(), ParseTrueRow(), ParseTrueRow()};
}
inline static Table ParseAnd() {
return {ParseFalseRow(), ParseFalseRow(), ParseFalseRow(), ParseTrueRow()};
}
std::optional<Row> static ParseConditional(const std::string& token) {
// Expect something like a/b/c
std::istringstream iss2(token);
Row row;
try {
// if the string has no / then return std::nullopt
if (std::count(token.begin(), token.end(), '/') == 0) return std::nullopt;
// split the word on the '/' character
for (std::string s; std::getline(iss2, s, '/');) {
// can throw exception
row.push_back(std::stod(s));
}
// if we ended with a '/' then return false
if (token.back() == '/') return std::nullopt;
} catch (...) {
return std::nullopt;
}
return row;
}
std::optional<Table> static ParseConditionalTable(
const std::vector<std::string>& tokens) {
Table table;
// loop over the words
// for each word, split it into doubles using a stringstream
for (const auto& word : tokens) {
// If the string word is F or T then the row is {0,1} or {1,0} respectively
if (word == "F") {
table.push_back(ParseFalseRow());
} else if (word == "T") {
table.push_back(ParseTrueRow());
} else {
// Expect something like a/b/c
if (auto row = ParseConditional(word)) {
table.push_back(*row);
} else {
// stop parsing if we encounter an error
return std::nullopt;
}
}
}
return table;
}
std::vector<std::string> static Tokenize(const std::string& str) {
std::istringstream iss(str);
std::vector<std::string> tokens;
for (std::string s; iss >> s;) {
tokens.push_back(s);
}
return tokens;
}
std::optional<Table> SignatureParser::Parse(const std::string& str) {
// check if string is just whitespace
if (std::all_of(str.begin(), str.end(), isspace)) {
return std::nullopt;
}
// return std::nullopt if the string is empty
if (str.empty()) {
return std::nullopt;
}
// tokenize the str on whitespace
std::vector<std::string> tokens = Tokenize(str);
// if the first token is "OR", return the OR table
if (tokens[0] == "OR") {
// if there are more tokens, return std::nullopt
if (tokens.size() > 1) {
return std::nullopt;
}
return ParseOr();
}
// if the first token is "AND", return the AND table
if (tokens[0] == "AND") {
// if there are more tokens, return std::nullopt
if (tokens.size() > 1) {
return std::nullopt;
}
return ParseAnd();
}
// otherwise then parse the conditional table
auto table = ParseConditionalTable(tokens);
// return std::nullopt if the table is empty
if (!table || table->empty()) {
return std::nullopt;
}
// the boost::phoenix parser did not return an error if we could not fully
// parse a string it just returned whatever it could parse
return table;
}
} // namespace gtsam

56
gtsam/discrete/SignatureParser.h Normal file
View File

@ -0,0 +1,56 @@
/* ----------------------------------------------------------------------------
* 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 SignatureParser.h
* @brief Parser for conditional distribution signatures.
* @author Kartik Arcot
* @date January 2023
*/
#pragma once
#include <optional>
#include <string>
#include <vector>
namespace gtsam {
/**
* @brief A simple parser that replaces the boost spirit parser.
*
* It is meant to parse strings like "1/1 2/3 1/4". Every word of the form
* "a/b/c/..." is parsed as a row, and the rows are stored in a table.
*
* A `Row` is a vector of doubles, and a `Table` is a vector of rows.
*
* Examples: the parser is able to parse the following strings:
* "1/1 2/3 1/4":
* {{1,1},{2,3},{1,4}}
* "1/1 2/3 1/4 1/1 2/3 1/4" :
* {{1,1},{2,3},{1,4},{1,1},{2,3},{1,4}}
* "1/2/3 2/3/4 1/2/3 2/3/4 1/2/3 2/3/4 1/2/3 2/3/4 1/2/3" :
* {{1,2,3},{2,3,4},{1,2,3},{2,3,4},{1,2,3},{2,3,4},{1,2,3},{2,3,4},{1,2,3}}
*
* If the string has un-parsable elements the parser will fail with nullopt:
* "1/2 sdf" : nullopt !
*
* It also fails if the string is empty:
* "": nullopt !
*
* Also fails if the rows are not of the same size.
*/
struct SignatureParser {
using Row = std::vector<double>;
using Table = std::vector<Row>;
static std::optional<Table> Parse(const std::string& str);
};
} // namespace gtsam

10
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
View File

@ -25,8 +25,6 @@
#include <gtsam/discrete/DecisionTree-inl.h> // for convert only
#define DISABLE_TIMING
#include <boost/tokenizer.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/timing.h>
#include <gtsam/discrete/Signature.h>
@ -81,8 +79,8 @@ void resetCounts() {
}
void printCounts(const string& s) {
#ifndef DISABLE_TIMING
cout << boost::format("%s: %3d muls, %3d adds, %g ms.") % s % muls % adds %
(1000 * elapsed)
cout << s << ": " << std::setw(3) << muls << " muls, " <<
std::setw(3) << adds << " adds, " << 1000 * elapsed << " ms."
<< endl;
#endif
resetCounts();
@ -133,7 +131,9 @@ ADT create(const Signature& signature) {
ADT p(signature.discreteKeys(), signature.cpt());
static size_t count = 0;
const DiscreteKey& key = signature.key();
string DOTfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str();
std::stringstream ss;
ss << "CPT-" << std::setw(3) << std::setfill('0') << ++count << "-" << key.first;
string DOTfile = ss.str();
dot(p, DOTfile);
return p;
}

12
gtsam/discrete/tests/testDecisionTree.cpp
View File

@ -26,6 +26,8 @@
#include <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/discrete/Signature.h>
#include <iomanip>
using std::vector;
using std::string;
using std::map;
@ -50,7 +52,9 @@ struct CrazyDecisionTree : public DecisionTree<string, Crazy> {
void print(const std::string& s = "") const {
auto keyFormatter = [](const std::string& s) { return s; };
auto valueFormatter = [](const Crazy& v) {
return (boost::format("{%d,%4.2g}") % v.a % v.b).str();
std::stringstream ss;
ss << "{" << v.a << "," << std::setw(4) << std::setprecision(2) << v.b << "}";
return ss.str();
};
DecisionTree<string, Crazy>::print("", keyFormatter, valueFormatter);
}
@ -86,7 +90,7 @@ struct DT : public DecisionTree<string, int> {
void print(const std::string& s = "") const {
auto keyFormatter = [](const std::string& s) { return s; };
auto valueFormatter = [](const int& v) {
return (boost::format("%d") % v).str();
return std::to_string(v);
};
std::cout << s;
Base::print("", keyFormatter, valueFormatter);
@ -458,9 +462,7 @@ TEST(DecisionTree, unzip) {
DTP tree(B, DTP(A, {0, "zero"}, {1, "one"}),
DTP(A, {2, "two"}, {1337, "l33t"}));
DT1 dt1;
DT2 dt2;
std::tie(dt1, dt2) = unzip(tree);
const auto [dt1, dt2] = unzip(tree);
DT1 tree1(B, DT1(A, 0, 1), DT1(A, 2, 1337));
DT2 tree2(B, DT2(A, "zero", "one"), DT2(A, "two", "l33t"));

3
gtsam/discrete/tests/testDiscreteBayesNet.cpp
View File

@ -94,8 +94,7 @@ TEST(DiscreteBayesNet, Asia) {
EXPECT(assert_equal(vs, marginals.marginalProbabilities(Smoking)));
// Create solver and eliminate
Ordering ordering;
ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7);
const Ordering ordering{0, 1, 2, 3, 4, 5, 6, 7};
DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering);
DiscreteConditional expected2(Bronchitis % "11/9");
EXPECT(assert_equal(expected2, *chordal->back()));

17
gtsam/discrete/tests/testDiscreteFactorGraph.cpp
View File

@ -107,11 +107,8 @@ TEST(DiscreteFactorGraph, test) {
graph.add(C & B, "3 1 1 3");
// Test EliminateDiscrete
Ordering frontalKeys;
frontalKeys += Key(0);
DiscreteConditional::shared_ptr conditional;
DecisionTreeFactor::shared_ptr newFactor;
boost::tie(conditional, newFactor) = EliminateDiscrete(graph, frontalKeys);
const Ordering frontalKeys{0};
const auto [conditional, newFactor] = EliminateDiscrete(graph, frontalKeys);
// Check Conditional
CHECK(conditional);
@ -125,12 +122,9 @@ TEST(DiscreteFactorGraph, test) {
EXPECT(assert_equal(expectedFactor, *newFactor));
// Test using elimination tree
Ordering ordering;
ordering += Key(0), Key(1), Key(2);
const Ordering ordering{0, 1, 2};
DiscreteEliminationTree etree(graph, ordering);
DiscreteBayesNet::shared_ptr actual;
DiscreteFactorGraph::shared_ptr remainingGraph;
boost::tie(actual, remainingGraph) = etree.eliminate(&EliminateDiscrete);
const auto [actual, remainingGraph] = etree.eliminate(&EliminateDiscrete);
// Check Bayes net
DiscreteBayesNet expectedBayesNet;
@ -235,8 +229,7 @@ TEST(DiscreteFactorGraph, testMPE_Darwiche09book_p244) {
EXPECT(assert_equal(mpe, actualMPE));
// Check Bayes Net
Ordering ordering;
ordering += Key(0), Key(1), Key(2), Key(3), Key(4);
const Ordering ordering{0, 1, 2, 3, 4};
auto chordal = graph.eliminateSequential(ordering);
EXPECT_LONGS_EQUAL(5, chordal->size());

10
gtsam/discrete/tests/testSignature.cpp
View File

@ -29,7 +29,7 @@ using namespace gtsam;
DiscreteKey X(0, 2), Y(1, 3), Z(2, 2);
/* ************************************************************************* */
TEST(testSignature, simple_conditional) {
TEST(testSignature, SimpleConditional) {
Signature sig(X, {Y}, "1/1 2/3 1/4");
CHECK(sig.table());
Signature::Table table = *sig.table();
@ -54,7 +54,7 @@ TEST(testSignature, simple_conditional) {
}
/* ************************************************************************* */
TEST(testSignature, simple_conditional_nonparser) {
TEST(testSignature, SimpleConditionalNonparser) {
Signature::Row row1{1, 1}, row2{2, 3}, row3{1, 4};
Signature::Table table{row1, row2, row3};
@ -77,7 +77,7 @@ TEST(testSignature, simple_conditional_nonparser) {
DiscreteKey A(0, 2), S(1, 2), T(2, 2), L(3, 2), B(4, 2), E(5, 2), D(7, 2);
// Make sure we can create all signatures for Asia network with constructor.
TEST(testSignature, all_examples) {
TEST(testSignature, AllExamples) {
DiscreteKey X(6, 2);
Signature a(A, {}, "99/1");
Signature s(S, {}, "50/50");
@ -89,7 +89,7 @@ TEST(testSignature, all_examples) {
}
// Make sure we can create all signatures for Asia network with operator magic.
TEST(testSignature, all_examples_magic) {
TEST(testSignature, AllExamplesMagic) {
DiscreteKey X(6, 2);
Signature a(A % "99/1");
Signature s(S % "50/50");
@ -101,7 +101,7 @@ TEST(testSignature, all_examples_magic) {
}
// Check example from docs.
TEST(testSignature, doxygen_example) {
TEST(testSignature, DoxygenExample) {
Signature::Table table{{0.9, 0.1}, {0.2, 0.8}, {0.3, 0.7}, {0.1, 0.9}};
Signature d1(D, {E, B}, table);
Signature d2((D | E, B) = "9/1 2/8 3/7 1/9");

103
gtsam/discrete/tests/testSignatureParser.cpp Normal file
View File

@ -0,0 +1,103 @@
/**
* Unit tests for the SimpleParser class.
* @file testSimpleParser.cpp
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/discrete/SignatureParser.h>
using namespace gtsam;
/* ************************************************************************* */
// Simple test case
bool compareTables(const SignatureParser::Table& table1,
const SignatureParser::Table& table2) {
if (table1.size() != table2.size()) {
return false;
}
for (size_t i = 0; i < table1.size(); ++i) {
if (table1[i].size() != table2[i].size()) {
return false;
}
for (size_t j = 0; j < table1[i].size(); ++j) {
if (table1[i][j] != table2[i][j]) {
return false;
}
}
}
return true;
}
/* ************************************************************************* */
// Simple test case
TEST(SimpleParser, Simple) {
SignatureParser::Table expectedTable{{1, 1}, {2, 3}, {1, 4}};
const auto table = SignatureParser::Parse("1/1 2/3 1/4");
CHECK(table);
// compare the tables
EXPECT(compareTables(*table, expectedTable));
}
/* ************************************************************************* */
// Test case with each row having 3 elements
TEST(SimpleParser, ThreeElements) {
SignatureParser::Table expectedTable{{1, 1, 1}, {2, 3, 2}, {1, 4, 3}};
const auto table = SignatureParser::Parse("1/1/1 2/3/2 1/4/3");
CHECK(table);
// compare the tables
EXPECT(compareTables(*table, expectedTable));
}
/* ************************************************************************* */
// A test case to check if we can parse a signature with 'T' and 'F'
TEST(SimpleParser, TAndF) {
SignatureParser::Table expectedTable{{1, 0}, {1, 0}, {1, 0}, {0, 1}};
const auto table = SignatureParser::Parse("F F F T");
CHECK(table);
// compare the tables
EXPECT(compareTables(*table, expectedTable));
}
/* ************************************************************************* */
// A test to parse {F F F 1}
TEST(SimpleParser, FFF1) {
SignatureParser::Table expectedTable{{1, 0}, {1, 0}, {1, 0}};
const auto table = SignatureParser::Parse("F F F");
CHECK(table);
// compare the tables
EXPECT(compareTables(*table, expectedTable));
}
/* ************************************************************************* */
// Expect false if the string is empty
TEST(SimpleParser, emptyString) {
const auto table = SignatureParser::Parse("");
EXPECT(!table);
}
/* ************************************************************************* */
// Expect false if gibberish
TEST(SimpleParser, Gibberish) {
const auto table = SignatureParser::Parse("sdf 22/3");
EXPECT(!table);
}
// If Gibberish is in the middle, it should not parse.
TEST(SimpleParser, GibberishInMiddle) {
const auto table = SignatureParser::Parse("1/1 2/3 sdf 1/4");
EXPECT(!table);
}
// A test with slash in the end
TEST(SimpleParser, SlashInEnd) {
const auto table = SignatureParser::Parse("1/1 2/");
EXPECT(!table);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}

1
gtsam/geometry/BearingRange.h
View File

@ -21,7 +21,6 @@
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/concept/assert.hpp>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#endif

2
gtsam/geometry/CalibratedCamera.h
View File

@ -223,7 +223,7 @@ public:
* @return a pair of [start, end] indices into the tangent space vector
*/
inline static std::pair<size_t, size_t> translationInterval() {
return std::make_pair(3, 5);
return {3, 5};
}
/// @}

4
gtsam/geometry/CameraSet.h
View File

@ -153,7 +153,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
* full matrices and vectors and pass it to the pointer
* version of the function
*/
template <class POINT, class... OptArgs>
template <class POINT, class... OptArgs, typename = std::enable_if_t<sizeof...(OptArgs)!=0>>
Vector reprojectionError(const POINT& point, const ZVector& measured,
OptArgs&... args) const {
// pass it to the pointer version of the function
@ -396,7 +396,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
FastMap<Key, size_t> KeySlotMap;
for (size_t slot = 0; slot < allKeys.size(); slot++)
KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
KeySlotMap.emplace(allKeys[slot], slot);
// Schur complement trick
// G = F' * F - F' * E * P * E' * F

4
gtsam/geometry/Pose2.h
View File

@ -309,14 +309,14 @@ public:
* exponential map parameterization
* @return a pair of [start, end] indices into the tangent space vector
*/
inline static std::pair<size_t, size_t> translationInterval() { return std::make_pair(0, 1); }
inline static std::pair<size_t, size_t> translationInterval() { return {0, 1}; }
/**
* Return the start and end indices (inclusive) of the rotation component of the
* exponential map parameterization
* @return a pair of [start, end] indices into the tangent space vector
*/
static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); }
static std::pair<size_t, size_t> rotationInterval() { return {2, 2}; }
/// Output stream operator
GTSAM_EXPORT

4
gtsam/geometry/Pose3.h
View File

@ -364,7 +364,7 @@ public:
* @return a pair of [start, end] indices into the tangent space vector
*/
inline static std::pair<size_t, size_t> translationInterval() {
return std::make_pair(3, 5);
return {3, 5};
}
/**
@ -373,7 +373,7 @@ public:
* @return a pair of [start, end] indices into the tangent space vector
*/
static std::pair<size_t, size_t> rotationInterval() {
return std::make_pair(0, 2);
return {0, 2};
}
/**

5
gtsam/geometry/Rot3.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <random>
@ -170,13 +169,13 @@ Vector3 Rot3::xyz(OptionalJacobian<3, 3> H) const {
#endif
Matrix39 qHm;
boost::tie(I, q) = RQ(m, qHm);
std::tie(I, q) = RQ(m, qHm);
// TODO : Explore whether this expression can be optimized as both
// qHm and mH are super-sparse
*H = qHm * mH;
} else
boost::tie(I, q) = RQ(matrix());
std::tie(I, q) = RQ(matrix());
return q;
}

1
gtsam/geometry/Rot3M.cpp
View File

@ -24,7 +24,6 @@
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>
using namespace std;

1
gtsam/geometry/Rot3Q.cpp
View File

@ -20,7 +20,6 @@
#ifdef GTSAM_USE_QUATERNIONS
#include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>
using namespace std;

18
gtsam/geometry/Similarity2.cpp
View File

@ -31,9 +31,9 @@ namespace internal {
static Point2Pairs SubtractCentroids(const Point2Pairs& abPointPairs,
const Point2Pair& centroids) {
Point2Pairs d_abPointPairs;
for (const Point2Pair& abPair : abPointPairs) {
Point2 da = abPair.first - centroids.first;
Point2 db = abPair.second - centroids.second;
for (const auto& [a, b] : abPointPairs) {
Point2 da = a - centroids.first;
Point2 db = b - centroids.second;
d_abPointPairs.emplace_back(da, db);
}
return d_abPointPairs;
@ -43,10 +43,8 @@ static Point2Pairs SubtractCentroids(const Point2Pairs& abPointPairs,
static double CalculateScale(const Point2Pairs& d_abPointPairs,
const Rot2& aRb) {
double x = 0, y = 0;
Point2 da, db;
for (const Point2Pair& d_abPair : d_abPointPairs) {
std::tie(da, db) = d_abPair;
for (const auto& [da, db] : d_abPointPairs) {
const Vector2 da_prime = aRb * db;
y += da.transpose() * da_prime;
x += da_prime.transpose() * da_prime;
@ -58,8 +56,8 @@ static double CalculateScale(const Point2Pairs& d_abPointPairs,
/// Form outer product H.
static Matrix2 CalculateH(const Point2Pairs& d_abPointPairs) {
Matrix2 H = Z_2x2;
for (const Point2Pair& d_abPair : d_abPointPairs) {
H += d_abPair.first * d_abPair.second.transpose();
for (const auto& [da, db] : d_abPointPairs) {
H += da * db.transpose();
}
return H;
}
@ -186,9 +184,7 @@ Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) {
abPointPairs.reserve(n);
// Below denotes the pose of the i'th object/camera/etc
// in frame "a" or frame "b".
Pose2 aTi, bTi;
for (const Pose2Pair& abPair : abPosePairs) {
std::tie(aTi, bTi) = abPair;
for (const auto& [aTi, bTi] : abPosePairs) {
const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse());
rotations.emplace_back(aRb);
abPointPairs.emplace_back(aTi.translation(), bTi.translation());

16
gtsam/geometry/Similarity3.cpp
View File

@ -31,9 +31,9 @@ namespace internal {
static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
const Point3Pair &centroids) {
Point3Pairs d_abPointPairs;
for (const Point3Pair& abPair : abPointPairs) {
Point3 da = abPair.first - centroids.first;
Point3 db = abPair.second - centroids.second;
for (const auto& [a, b] : abPointPairs) {
Point3 da = a - centroids.first;
Point3 db = b - centroids.second;
d_abPointPairs.emplace_back(da, db);
}
return d_abPointPairs;
@ -46,8 +46,7 @@ static double calculateScale(const Point3Pairs &d_abPointPairs,
const Rot3 &aRb) {
double x = 0, y = 0;
Point3 da, db;
for (const Point3Pair& d_abPair : d_abPointPairs) {
std::tie(da, db) = d_abPair;
for (const auto& [da, db] : d_abPointPairs) {
const Vector3 da_prime = aRb * db;
y += da.transpose() * da_prime;
x += da_prime.transpose() * da_prime;
@ -59,8 +58,8 @@ static double calculateScale(const Point3Pairs &d_abPointPairs,
/// Form outer product H.
static Matrix3 calculateH(const Point3Pairs &d_abPointPairs) {
Matrix3 H = Z_3x3;
for (const Point3Pair& d_abPair : d_abPointPairs) {
H += d_abPair.first * d_abPair.second.transpose();
for (const auto& [da, db] : d_abPointPairs) {
H += da * db.transpose();
}
return H;
}
@ -184,8 +183,7 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
abPointPairs.reserve(n);
// Below denotes the pose of the i'th object/camera/etc in frame "a" or frame "b"
Pose3 aTi, bTi;
for (const Pose3Pair &abPair : abPosePairs) {
std::tie(aTi, bTi) = abPair;
for (const auto &[aTi, bTi] : abPosePairs) {
const Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse());
rotations.emplace_back(aRb);
abPointPairs.emplace_back(aTi.translation(), bTi.translation());

4
gtsam/geometry/tests/testBearingRange.cpp
View File

@ -34,7 +34,7 @@ BearingRange3D br3D(Pose3().bearing(Point3(1, 0, 0)), 1);
//******************************************************************************
TEST(BearingRange2D, Concept) {
BOOST_CONCEPT_ASSERT((IsManifold<BearingRange2D>));
GTSAM_CONCEPT_ASSERT(IsManifold<BearingRange2D>);
}
/* ************************************************************************* */
@ -46,7 +46,7 @@ TEST(BearingRange, 2D) {
//******************************************************************************
TEST(BearingRange3D, Concept) {
BOOST_CONCEPT_ASSERT((IsManifold<BearingRange3D>));
GTSAM_CONCEPT_ASSERT(IsManifold<BearingRange3D>);
}
/* ************************************************************************* */

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