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

Merge branch 'develop' into imu-examples

release/4.3a0
Varun Agrawal 2020-08-17 00:38:14 -04:00
parent 45b5cd4011 ae4968328d
commit be70d1785c
25 changed files with 1415 additions and 875 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
.github/scripts/python.sh vendored Normal file
View File

@ -0,0 +1,94 @@
#!/bin/bash
##########################################################
# Build and test the GTSAM Python wrapper.
##########################################################
set -x -e
if [ -z ${PYTHON_VERSION+x} ]; then
echo "Please provide the Python version to build against!"
exit 127
fi
if [ -z ${WRAPPER+x} ]; then
echo "Please provide the wrapper to build!"
exit 126
fi
PYTHON="python${PYTHON_VERSION}"
if [[ $(uname) == "Darwin" ]]; then
brew install wget
else
# Install a system package required by our library
sudo apt-get install -y wget libicu-dev python3-pip python3-setuptools
fi
PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
case $WRAPPER in
"cython")
BUILD_CYTHON="ON"
BUILD_PYBIND="OFF"
TYPEDEF_POINTS_TO_VECTORS="OFF"
sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/cython/requirements.txt
;;
"pybind")
BUILD_CYTHON="OFF"
BUILD_PYBIND="ON"
TYPEDEF_POINTS_TO_VECTORS="ON"
sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
;;
*)
exit 126
;;
esac
git submodule update --init --recursive
mkdir $GITHUB_WORKSPACE/build
cd $GITHUB_WORKSPACE/build
cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=Release \
-DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_UNSTABLE=ON \
-DGTSAM_USE_QUATERNIONS=OFF \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DGTSAM_INSTALL_CYTHON_TOOLBOX=${BUILD_CYTHON} \
-DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \
-DGTSAM_TYPEDEF_POINTS_TO_VECTORS=${TYPEDEF_POINTS_TO_VECTORS} \
-DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
-DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V41=OFF \
-DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install
make -j$(nproc) install &
while ps -p $! > /dev/null
do
sleep 60
now=$(date +%s)
printf "%d seconds have elapsed\n" $(( (now - start) ))
done
case $WRAPPER in
"cython")
cd $GITHUB_WORKSPACE/build/cython
$PYTHON setup.py install --user --prefix=
cd $GITHUB_WORKSPACE/build/cython/gtsam/tests
$PYTHON -m unittest discover
;;
"pybind")
cd $GITHUB_WORKSPACE/python
$PYTHON setup.py install --user --prefix=
cd $GITHUB_WORKSPACE/wrap/python/gtsam_py/tests
$PYTHON -m unittest discover
;;
*)
echo "THIS SHOULD NEVER HAPPEN!"
exit 125
;;
esac

34
.travis.sh → .github/scripts/unix.sh vendored Executable file → Normal file
View File

@ -1,20 +1,25 @@
#!/bin/bash #!/bin/bash
##########################################################
# Build and test GTSAM for *nix based systems.
# Specifically Linux and macOS.
##########################################################
# install TBB with _debug.so files # install TBB with _debug.so files
function install_tbb() function install_tbb()
{ {
TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download
TBB_VERSION=4.4.2 TBB_VERSION=4.4.5
TBB_DIR=tbb44_20151115oss TBB_DIR=tbb44_20160526oss
TBB_SAVEPATH="/tmp/tbb.tgz" TBB_SAVEPATH="/tmp/tbb.tgz"
if [ "$TRAVIS_OS_NAME" == "linux" ]; then if [ "$(uname)" == "Linux" ]; then
OS_SHORT="lin" OS_SHORT="lin"
TBB_LIB_DIR="intel64/gcc4.4" TBB_LIB_DIR="intel64/gcc4.4"
SUDO="sudo" SUDO="sudo"
elif [ "$TRAVIS_OS_NAME" == "osx" ]; then elif [ "$(uname)" == "Darwin" ]; then
OS_SHORT="lin" OS_SHORT="osx"
TBB_LIB_DIR="" TBB_LIB_DIR=""
SUDO="" SUDO=""
@ -25,7 +30,7 @@ function install_tbb()
TBBROOT=/tmp/$TBB_DIR TBBROOT=/tmp/$TBB_DIR
# Copy the needed files to the correct places. # Copy the needed files to the correct places.
# This works correctly for travis builds, instead of setting path variables. # This works correctly for CI builds, instead of setting path variables.
# This is what Homebrew does to install TBB on Macs # This is what Homebrew does to install TBB on Macs
$SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/ $SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/
$SUDO cp -R $TBBROOT/include/ /usr/local/include/ $SUDO cp -R $TBBROOT/include/ /usr/local/include/
@ -38,15 +43,15 @@ function configure()
set -e # Make sure any error makes the script to return an error code set -e # Make sure any error makes the script to return an error code
set -x # echo set -x # echo
SOURCE_DIR=`pwd` SOURCE_DIR=$GITHUB_WORKSPACE
BUILD_DIR=build BUILD_DIR=$GITHUB_WORKSPACE/build
#env #env
git clean -fd || true git submodule update --init --recursive
rm -fr $BUILD_DIR || true rm -fr $BUILD_DIR || true
mkdir $BUILD_DIR && cd $BUILD_DIR mkdir $BUILD_DIR && cd $BUILD_DIR
install_tbb [ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
if [ ! -z "$GCC_VERSION" ]; then if [ ! -z "$GCC_VERSION" ]; then
export CC=gcc-$GCC_VERSION export CC=gcc-$GCC_VERSION
@ -63,7 +68,10 @@ function configure()
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DCMAKE_VERBOSE_MAKEFILE=OFF -DCMAKE_VERBOSE_MAKEFILE=ON \
-DBOOST_ROOT=$BOOST_ROOT \
-DBoost_NO_SYSTEM_PATHS=ON \
-DBoost_ARCHITECTURE=-x64
} }
@ -71,7 +79,7 @@ function configure()
function finish () function finish ()
{ {
# Print ccache stats # Print ccache stats
ccache -s [ -x "$(command -v ccache)" ] && ccache -s
cd $SOURCE_DIR cd $SOURCE_DIR
} }
@ -111,4 +119,4 @@ case $1 in
-t) -t)
test test
;; ;;
esac esac

78
.github/workflows/build-linux.yml vendored Normal file
View File

@ -0,0 +1,78 @@
name: Linux CI
on: [push, pull_request]
jobs:
build:
name: ${{ matrix.name }} ${{ matrix.build_type }}
runs-on: ${{ matrix.os }}
env:
CTEST_OUTPUT_ON_FAILURE: ON
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
strategy:
fail-fast: false
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-18.04-gcc-5,
ubuntu-18.04-gcc-9,
ubuntu-18.04-clang-9,
]
build_type: [Debug, Release]
build_unstable: [ON]
include:
- name: ubuntu-18.04-gcc-5
os: ubuntu-18.04
compiler: gcc
version: "5"
- name: ubuntu-18.04-gcc-9
os: ubuntu-18.04
compiler: gcc
version: "9"
- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
compiler: clang
version: "9"
steps:
- name: Checkout
uses: actions/checkout@master
- 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 add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
fi
sudo apt-get -y update
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)"
echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)"
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
else
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
echo "::set-env name=CC::clang-${{ matrix.version }}"
echo "::set-env name=CXX::clang++-${{ matrix.version }}"
fi
- name: Check Boost version
if: runner.os == 'Linux'
run: |
echo "BOOST_ROOT = $BOOST_ROOT"
- name: Build (Linux)
if: runner.os == 'Linux'
run: |
bash .github/scripts/unix.sh -t

51
.github/workflows/build-macos.yml vendored Normal file
View File

@ -0,0 +1,51 @@
name: macOS CI
on: [pull_request]
jobs:
build:
name: ${{ matrix.name }} ${{ matrix.build_type }}
runs-on: ${{ matrix.os }}
env:
CTEST_OUTPUT_ON_FAILURE: ON
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
strategy:
fail-fast: false
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: [
macOS-10.15-xcode-11.3.1,
]
build_type: [Debug, Release]
build_unstable: [ON]
include:
- name: macOS-10.15-xcode-11.3.1
os: macOS-10.15
compiler: xcode
version: "11.3.1"
steps:
- name: Checkout
uses: actions/checkout@master
- name: Install (macOS)
if: runner.os == 'macOS'
run: |
brew install cmake ninja boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
else
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
echo "::set-env name=CC::clang"
echo "::set-env name=CXX::clang++"
fi
- name: Build (macOS)
if: runner.os == 'macOS'
run: |
bash .github/scripts/unix.sh -t

95
.github/workflows/build-python.yml vendored Normal file
View File

@ -0,0 +1,95 @@
name: Python CI
on: [pull_request]
jobs:
build:
name: ${{ matrix.name }} ${{ matrix.build_type }} Python ${{ matrix.python_version }}
runs-on: ${{ matrix.os }}
env:
CTEST_OUTPUT_ON_FAILURE: ON
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
PYTHON_VERSION: ${{ matrix.python_version }}
WRAPPER: ${{ matrix.wrapper }}
strategy:
fail-fast: false
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-18.04-gcc-5,
ubuntu-18.04-gcc-9,
ubuntu-18.04-clang-9,
macOS-10.15-xcode-11.3.1,
]
build_type: [Debug, Release]
python_version: [3]
wrapper: [cython]
include:
- name: ubuntu-18.04-gcc-5
os: ubuntu-18.04
compiler: gcc
version: "5"
- name: ubuntu-18.04-gcc-9
os: ubuntu-18.04
compiler: gcc
version: "9"
- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
compiler: clang
version: "9"
- name: macOS-10.15-xcode-11.3.1
os: macOS-10.15
compiler: xcode
version: "11.3.1"
steps:
- name: Checkout
uses: actions/checkout@master
- 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 add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
fi
sudo apt-get -y update
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
else
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
echo "::set-env name=CC::clang-${{ matrix.version }}"
echo "::set-env name=CXX::clang++-${{ matrix.version }}"
fi
- name: Install (macOS)
if: runner.os == 'macOS'
run: |
brew install cmake ninja boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
else
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
echo "::set-env name=CC::clang"
echo "::set-env name=CXX::clang++"
fi
- name: Build (Linux)
if: runner.os == 'Linux'
run: |
bash .github/scripts/python.sh
- name: Build (macOS)
if: runner.os == 'macOS'
run: |
bash .github/scripts/python.sh

75
.github/workflows/build-windows.yml vendored Normal file
View File

@ -0,0 +1,75 @@
name: Windows CI
on: [pull_request]
jobs:
build:
name: ${{ matrix.name }} ${{ matrix.build_type }}
runs-on: ${{ matrix.os }}
env:
CTEST_OUTPUT_ON_FAILURE: ON
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
strategy:
fail-fast: false
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: [
windows-2016-cl,
windows-2019-cl,
]
build_type: [Debug, Release]
build_unstable: [ON]
include:
- name: windows-2016-cl
os: windows-2016
compiler: cl
- name: windows-2019-cl
os: windows-2019
compiler: cl
steps:
- name: Checkout
uses: actions/checkout@master
- name: Install (Windows)
if: runner.os == 'Windows'
shell: powershell
run: |
Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh')
scoop install ninja --global
if ("${{ matrix.compiler }}".StartsWith("clang")) {
scoop install llvm --global
}
if ("${{ matrix.compiler }}" -eq "gcc") {
# Chocolatey GCC is broken on the windows-2019 image.
# See: https://github.com/DaanDeMeyer/doctest/runs/231595515
# See: https://github.community/t5/GitHub-Actions/Something-is-wrong-with-the-chocolatey-installed-version-of-gcc/td-p/32413
scoop install gcc --global
echo "::set-env name=CC::gcc"
echo "::set-env name=CXX::g++"
} elseif ("${{ matrix.compiler }}" -eq "clang") {
echo "::set-env name=CC::clang"
echo "::set-env name=CXX::clang++"
} else {
echo "::set-env name=CC::${{ matrix.compiler }}"
echo "::set-env name=CXX::${{ matrix.compiler }}"
}
# Scoop modifies the PATH so we make the modified PATH global.
echo "::set-env name=PATH::$env:PATH"
- name: Build (Windows)
if: runner.os == 'Windows'
run: |
cmake -E remove_directory build
echo "BOOST_ROOT_1_72_0: ${env:BOOST_ROOT_1_72_0}"
cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT_1_72_0}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT_1_72_0}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT_1_72_0}\lib"
cmake --build build --config ${{ matrix.build_type }} --target gtsam
cmake --build build --config ${{ matrix.build_type }} --target gtsam_unstable
cmake --build build --config ${{ matrix.build_type }} --target wrap
cmake --build build --config ${{ matrix.build_type }} --target check.base
cmake --build build --config ${{ matrix.build_type }} --target check.base_unstable
cmake --build build --config ${{ matrix.build_type }} --target check.linear

5
.github/workflows/trigger-python.yml vendored
View File

@ -1,6 +1,9 @@
# This triggers Cython builds on `gtsam-manylinux-build` # This triggers Cython builds on `gtsam-manylinux-build`
name: Trigger Python Builds name: Trigger Python Builds
on: push on:
push:
branches:
- develop
jobs: jobs:
triggerCython: triggerCython:
runs-on: ubuntu-latest runs-on: ubuntu-latest

43
.travis.python.sh
View File

@ -1,43 +0,0 @@
#!/bin/bash
set -x -e
if [ -z ${PYTHON_VERSION+x} ]; then
echo "Please provide the Python version to build against!"
exit 127
fi
PYTHON="python${PYTHON_VERSION}"
if [[ $(uname) == "Darwin" ]]; then
brew install wget
else
# Install a system package required by our library
sudo apt-get install wget libicu-dev python3-pip python3-setuptools
fi
CURRDIR=$(pwd)
sudo $PYTHON -m pip install -r ./cython/requirements.txt
mkdir $CURRDIR/build
cd $CURRDIR/build
cmake $CURRDIR -DCMAKE_BUILD_TYPE=Release \
-DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_UNSTABLE=ON \
-DGTSAM_USE_QUATERNIONS=OFF \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DGTSAM_INSTALL_CYTHON_TOOLBOX=ON \
-DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V41=OFF \
-DCMAKE_INSTALL_PREFIX=$CURRDIR/../gtsam_install
make -j$(nproc) install
cd cython
sudo $PYTHON setup.py install
cd $CURRDIR/cython/gtsam/tests
$PYTHON -m unittest discover

140
.travis.yml
View File

@ -1,140 +0,0 @@
language: cpp
cache: ccache
dist: xenial
addons:
apt:
sources:
- ubuntu-toolchain-r-test
- sourceline: 'deb http://apt.llvm.org/xenial/ llvm-toolchain-xenial-9 main'
key_url: 'https://apt.llvm.org/llvm-snapshot.gpg.key'
packages:
- g++-9
- clang-9
- build-essential pkg-config
- cmake
- python3-dev libpython-dev
- python3-numpy
- libboost-all-dev
# before_install:
# - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update; fi
install:
- if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache ; fi
- if [ "$TRAVIS_OS_NAME" == "osx" ]; then export PATH="/usr/local/opt/ccache/libexec:$PATH" ; fi
# We first do the compile stage specified below, then the matrix expansion specified after.
stages:
- compile
- test
- special
env:
global:
- MAKEFLAGS="-j3"
- CCACHE_SLOPPINESS=pch_defines,time_macros
# Compile stage without building examples/tests to populate the caches.
jobs:
# -------- STAGE 1: COMPILE -----------
include:
# on Mac, GCC
- stage: compile
os: osx
compiler: gcc
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -b
- stage: compile
os: osx
compiler: gcc
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -b
# on Mac, CLANG
- stage: compile
os: osx
compiler: clang
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -b
- stage: compile
os: osx
compiler: clang
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -b
# on Linux, GCC
- stage: compile
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -b
- stage: compile
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -b
# on Linux, CLANG
- stage: compile
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -b
- stage: compile
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -b
# on Linux, with deprecated ON to make sure that path still compiles/tests
- stage: special
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V41=ON
script: bash .travis.sh -b
# on Linux, with GTSAM_WITH_TBB on to make sure GTSAM still compiles/tests
# - stage: special
# os: linux
# compiler: gcc
# env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=ON
# script: bash .travis.sh -t
# -------- STAGE 2: TESTS -----------
# on Mac, GCC
- stage: test
os: osx
compiler: clang
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -t
- stage: test
os: osx
compiler: clang
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -t
- stage: test
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -t
- stage: test
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -t
- stage: test
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -t
# on Linux, with quaternions ON to make sure that path still compiles/tests
- stage: special
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release GTSAM_BUILD_UNSTABLE=OFF GTSAM_USE_QUATERNIONS=ON
script: bash .travis.sh -t
- stage: special
os: linux
compiler: gcc
env: PYTHON_VERSION=3
script: bash .travis.python.sh
- stage: special
os: osx
compiler: clang
env: PYTHON_VERSION=3
script: bash .travis.python.sh

104
CMakeLists.txt
View File

@ -509,104 +509,104 @@ set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>=
# Print configuration variables # Print configuration variables
message(STATUS "===============================================================") message(STATUS "===============================================================")
message(STATUS "================ Configuration Options ======================") message(STATUS "================ Configuration Options ======================")
message(STATUS " CMAKE_CXX_COMPILER_ID type : ${CMAKE_CXX_COMPILER_ID}") print_config("CMAKE_CXX_COMPILER_ID type" "${CMAKE_CXX_COMPILER_ID}")
message(STATUS " CMAKE_CXX_COMPILER_VERSION : ${CMAKE_CXX_COMPILER_VERSION}") print_config("CMAKE_CXX_COMPILER_VERSION" "${CMAKE_CXX_COMPILER_VERSION}")
message(STATUS " CMake version : ${CMAKE_VERSION}") print_config("CMake version" "${CMAKE_VERSION}")
message(STATUS " CMake generator : ${CMAKE_GENERATOR}") print_config("CMake generator" "${CMAKE_GENERATOR}")
message(STATUS " CMake build tool : ${CMAKE_BUILD_TOOL}") print_config("CMake build tool" "${CMAKE_BUILD_TOOL}")
message(STATUS "Build flags ") message(STATUS "Build flags ")
print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ") print_enabled_config(${GTSAM_BUILD_TESTS} "Build Tests")
print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all' ") print_enabled_config(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all'")
print_config_flag(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts with 'make all'") print_enabled_config(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts with 'make all'")
if (DOXYGEN_FOUND) if (DOXYGEN_FOUND)
print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ") print_enabled_config(${GTSAM_BUILD_DOCS} "Build Docs")
endif() endif()
print_config_flag(${BUILD_SHARED_LIBS} "Build shared GTSAM libraries ") print_enabled_config(${BUILD_SHARED_LIBS} "Build shared GTSAM libraries")
print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name ") print_enabled_config(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name")
if(GTSAM_UNSTABLE_AVAILABLE) if(GTSAM_UNSTABLE_AVAILABLE)
print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ") print_enabled_config(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable")
endif() endif()
if(NOT MSVC AND NOT XCODE_VERSION) if(NOT MSVC AND NOT XCODE_VERSION)
print_config_flag(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ") print_enabled_config(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ")
message(STATUS " Build type : ${CMAKE_BUILD_TYPE}") print_config("Build type" "${CMAKE_BUILD_TYPE}")
message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}") print_config("C compilation flags" "${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}") print_config("C++ compilation flags" "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
endif() endif()
print_build_options_for_target(gtsam) print_build_options_for_target(gtsam)
message(STATUS " Use System Eigen : ${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})") print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
if(GTSAM_USE_TBB) if(GTSAM_USE_TBB)
message(STATUS " Use Intel TBB : Yes") print_config("Use Intel TBB" "Yes")
elseif(TBB_FOUND) elseif(TBB_FOUND)
message(STATUS " Use Intel TBB : TBB found but GTSAM_WITH_TBB is disabled") print_config("Use Intel TBB" "TBB found but GTSAM_WITH_TBB is disabled")
else() else()
message(STATUS " Use Intel TBB : TBB not found") print_config("Use Intel TBB" "TBB not found")
endif() endif()
if(GTSAM_USE_EIGEN_MKL) if(GTSAM_USE_EIGEN_MKL)
message(STATUS " Eigen will use MKL : Yes") print_config("Eigen will use MKL" "Yes")
elseif(MKL_FOUND) elseif(MKL_FOUND)
message(STATUS " Eigen will use MKL : MKL found but GTSAM_WITH_EIGEN_MKL is disabled") print_config("Eigen will use MKL" "MKL found but GTSAM_WITH_EIGEN_MKL is disabled")
else() else()
message(STATUS " Eigen will use MKL : MKL not found") print_config("Eigen will use MKL" "MKL not found")
endif() endif()
if(GTSAM_USE_EIGEN_MKL_OPENMP) if(GTSAM_USE_EIGEN_MKL_OPENMP)
message(STATUS " Eigen will use MKL and OpenMP : Yes") print_config("Eigen will use MKL and OpenMP" "Yes")
elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL) elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled") print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled")
elseif(OPENMP_FOUND AND NOT MKL_FOUND) elseif(OPENMP_FOUND AND NOT MKL_FOUND)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but MKL not found") print_config("Eigen will use MKL and OpenMP" "OpenMP found but MKL not found")
elseif(OPENMP_FOUND) elseif(OPENMP_FOUND)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled") print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled")
else() else()
message(STATUS " Eigen will use MKL and OpenMP : OpenMP not found") print_config("Eigen will use MKL and OpenMP" "OpenMP not found")
endif() endif()
message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}") print_config("Default allocator" "${GTSAM_DEFAULT_ALLOCATOR}")
if(GTSAM_THROW_CHEIRALITY_EXCEPTION) if(GTSAM_THROW_CHEIRALITY_EXCEPTION)
message(STATUS " Cheirality exceptions enabled : YES") print_config("Cheirality exceptions enabled" "YES")
else() else()
message(STATUS " Cheirality exceptions enabled : NO") print_config("Cheirality exceptions enabled" "NO")
endif() endif()
if(NOT MSVC AND NOT XCODE_VERSION) if(NOT MSVC AND NOT XCODE_VERSION)
if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE) if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
message(STATUS " Build with ccache : Yes") print_config("Build with ccache" "Yes")
elseif(CCACHE_FOUND) elseif(CCACHE_FOUND)
message(STATUS " Build with ccache : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled") print_config("Build with ccache" "ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
else() else()
message(STATUS " Build with ccache : No") print_config("Build with ccache" "No")
endif() endif()
endif() endif()
message(STATUS "Packaging flags ") message(STATUS "Packaging flags")
message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}") print_config("CPack Source Generator" "${CPACK_SOURCE_GENERATOR}")
message(STATUS " CPack Generator : ${CPACK_GENERATOR}") print_config("CPack Generator" "${CPACK_GENERATOR}")
message(STATUS "GTSAM flags ") message(STATUS "GTSAM flags ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") print_enabled_config(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V41} "Allow features deprecated in GTSAM 4.1") print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V41} "Allow features deprecated in GTSAM 4.1")
print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ") print_enabled_config(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")
print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") print_enabled_config(${GTSAM_BUILD_WRAP} "Build Wrap ")
message(STATUS "MATLAB toolbox flags ") message(STATUS "MATLAB toolbox flags")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ") print_enabled_config(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ")
if (${GTSAM_INSTALL_MATLAB_TOOLBOX}) if (${GTSAM_INSTALL_MATLAB_TOOLBOX})
message(STATUS " MATLAB root : ${MATLAB_ROOT}") print_config("MATLAB root" "${MATLAB_ROOT}")
message(STATUS " MEX binary : ${MEX_COMMAND}") print_config("MEX binary" "${MEX_COMMAND}")
endif() endif()
message(STATUS "Cython toolbox flags ") message(STATUS "Cython toolbox flags ")
print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ") print_enabled_config(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ")
if(GTSAM_INSTALL_CYTHON_TOOLBOX) if(GTSAM_INSTALL_CYTHON_TOOLBOX)
message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}") print_config("Python version" "${GTSAM_PYTHON_VERSION}")
endif() endif()
message(STATUS "===============================================================") message(STATUS "===============================================================")

33
appveyor.yml
View File

@ -1,33 +0,0 @@
# version format
version: 4.0.3-{branch}-build{build}
os: Visual Studio 2019
clone_folder: c:\projects\gtsam
platform: x64
configuration: Release
environment:
CTEST_OUTPUT_ON_FAILURE: 1
BOOST_ROOT: C:/Libraries/boost_1_71_0
build_script:
- cd c:\projects\gtsam\build
# As of Dec 2019, not all unit tests build cleanly for MSVC, so we'll just
# check that parts of GTSAM build correctly:
#- cmake --build .
- cmake --build . --config Release --target gtsam
- cmake --build . --config Release --target gtsam_unstable
- cmake --build . --config Release --target wrap
#- cmake --build . --target check
- cmake --build . --config Release --target check.base
- cmake --build . --config Release --target check.base_unstable
- cmake --build . --config Release --target check.linear
before_build:
- cd c:\projects\gtsam
- mkdir build
- cd build
# Disable examples to avoid AppVeyor timeout
- cmake -G "Visual Studio 16 2019" .. -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF

32
cmake/GtsamPrinting.cmake
View File

@ -1,14 +1,3 @@
# print configuration variables
# Usage:
#print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ")
function(print_config_flag flag msg)
if (flag)
message(STATUS " ${msg}: Enabled")
else ()
message(STATUS " ${msg}: Disabled")
endif ()
endfunction()
# Based on https://github.com/jimbraun/XCDF/blob/master/cmake/CMakePadString.cmake # Based on https://github.com/jimbraun/XCDF/blob/master/cmake/CMakePadString.cmake
function(string_pad RESULT_NAME DESIRED_LENGTH VALUE) function(string_pad RESULT_NAME DESIRED_LENGTH VALUE)
string(LENGTH "${VALUE}" VALUE_LENGTH) string(LENGTH "${VALUE}" VALUE_LENGTH)
@ -26,6 +15,27 @@ endfunction()
set(GTSAM_PRINT_SUMMARY_PADDING_LENGTH 50 CACHE STRING "Padding of cmake summary report lines after configuring.") set(GTSAM_PRINT_SUMMARY_PADDING_LENGTH 50 CACHE STRING "Padding of cmake summary report lines after configuring.")
mark_as_advanced(GTSAM_PRINT_SUMMARY_PADDING_LENGTH) mark_as_advanced(GTSAM_PRINT_SUMMARY_PADDING_LENGTH)
# print configuration variables with automatic padding
# Usage:
# print_config(${GTSAM_BUILD_TESTS} "Build Tests")
function(print_config config msg)
string_pad(padded_config ${GTSAM_PRINT_SUMMARY_PADDING_LENGTH} " ${config}")
message(STATUS "${padded_config}: ${msg}")
endfunction()
# print configuration variable with enabled/disabled value
# Usage:
# print_enabled_config(${GTSAM_BUILD_TESTS} "Build Tests ")
function(print_enabled_config config msg)
string_pad(padded_msg ${GTSAM_PRINT_SUMMARY_PADDING_LENGTH} " ${msg}")
if (config)
message(STATUS "${padded_msg}: Enabled")
else ()
message(STATUS "${padded_msg}: Disabled")
endif ()
endfunction()
# Print " var: ${var}" padding with spaces as needed # Print " var: ${var}" padding with spaces as needed
function(print_padded variable_name) function(print_padded variable_name)
string_pad(padded_prop ${GTSAM_PRINT_SUMMARY_PADDING_LENGTH} " ${variable_name}") string_pad(padded_prop ${GTSAM_PRINT_SUMMARY_PADDING_LENGTH} " ${variable_name}")

1
cython/gtsam/utils/plot.py
View File

@ -155,7 +155,6 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None,
axes.set_xlabel(axis_labels[0]) axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1]) axes.set_ylabel(axis_labels[1])
axes.set_zlabel(axis_labels[2])
return fig return fig

14
cython/gtsam/utils/visual_data_generator.py
View File

@ -30,8 +30,8 @@ class GroundTruth:
def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
self.K = K self.K = K
self.cameras = [Pose3()] * nrCameras self.cameras = [gtsam.Pose3()] * nrCameras
self.points = [Point3()] * nrPoints self.points = [gtsam.Point3(0, 0, 0)] * nrPoints
def print_(self, s=""): def print_(self, s=""):
print(s) print(s)
@ -99,11 +99,11 @@ def generate_data(options):
r = 40 r = 40
for i in range(options.nrCameras): for i in range(options.nrCameras):
theta = i * 2 * np.pi / options.nrCameras theta = i * 2 * np.pi / options.nrCameras
t = Point3(r * np.cos(theta), r * np.sin(theta), height) t = gtsam.Point3(r * np.cos(theta), r * np.sin(theta), height)
truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t, truth.cameras[i] = gtsam.SimpleCamera.Lookat(t,
Point3(), gtsam.Point3(0, 0, 0),
Point3(0, 0, 1), gtsam.Point3(0, 0, 1),
truth.K) truth.K)
# Create measurements # Create measurements
for j in range(nrPoints): for j in range(nrPoints):
# All landmarks seen in every frame # All landmarks seen in every frame

32
examples/Data/example_with_vertices.g2o
View File

@ -1,16 +1,16 @@
VERTEX_SE3:QUAT 8646911284551352320 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162 VERTEX_SE3:QUAT 0 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162
VERTEX_SE3:QUAT 8646911284551352321 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508 VERTEX_SE3:QUAT 1 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508
VERTEX_SE3:QUAT 8646911284551352322 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10 VERTEX_SE3:QUAT 2 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10
VERTEX_SE3:QUAT 8646911284551352323 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508 VERTEX_SE3:QUAT 3 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508
VERTEX_SE3:QUAT 8646911284551352324 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162 VERTEX_SE3:QUAT 4 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162
VERTEX_SE3:QUAT 8646911284551352325 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567 VERTEX_SE3:QUAT 5 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567
VERTEX_SE3:QUAT 8646911284551352326 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412 VERTEX_SE3:QUAT 6 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412
VERTEX_SE3:QUAT 8646911284551352327 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567 VERTEX_SE3:QUAT 7 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567
VERTEX_TRACKXYZ 7782220156096217088 10 10 10 VERTEX_TRACKXYZ 0 10 10 10
VERTEX_TRACKXYZ 7782220156096217089 -10 10 10 VERTEX_TRACKXYZ 1 -10 10 10
VERTEX_TRACKXYZ 7782220156096217090 -10 -10 10 VERTEX_TRACKXYZ 2 -10 -10 10
VERTEX_TRACKXYZ 7782220156096217091 10 -10 10 VERTEX_TRACKXYZ 3 10 -10 10
VERTEX_TRACKXYZ 7782220156096217092 10 10 -10 VERTEX_TRACKXYZ 4 10 10 -10
VERTEX_TRACKXYZ 7782220156096217093 -10 10 -10 VERTEX_TRACKXYZ 5 -10 10 -10
VERTEX_TRACKXYZ 7782220156096217094 -10 -10 -10 VERTEX_TRACKXYZ 6 -10 -10 -10
VERTEX_TRACKXYZ 7782220156096217095 10 -10 -10 VERTEX_TRACKXYZ 7 10 -10 -10

8
gtsam/base/GenericValue.h
View File

@ -30,6 +30,14 @@
#include <iostream> #include <iostream>
#include <typeinfo> // operator typeid #include <typeinfo> // operator typeid
#ifdef _WIN32
#define GENERICVALUE_VISIBILITY
#else
// This will trigger a LNKxxxx on MSVC, so disable for MSVC build
// Please refer to https://github.com/borglab/gtsam/blob/develop/Using-GTSAM-EXPORT.md
#define GENERICVALUE_VISIBILITY GTSAM_EXPORT
#endif
namespace gtsam { namespace gtsam {
/** /**

2
gtsam/nonlinear/Values.h
View File

@ -397,7 +397,7 @@ namespace gtsam {
template<class ValueType> template<class ValueType>
size_t count() const { size_t count() const {
size_t i = 0; size_t i = 0;
for (const auto& key_value : *this) { for (const auto key_value : *this) {
if (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value)) if (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value))
++i; ++i;
} }

19
gtsam/sam/BearingRangeFactor.h
View File

@ -42,14 +42,21 @@ class BearingRangeFactor
public: public:
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
/// default constructor /// Default constructor
BearingRangeFactor() {} BearingRangeFactor() {}
/// primary constructor /// Construct from BearingRange instance
BearingRangeFactor(Key key1, Key key2, const B& measuredBearing, BearingRangeFactor(Key key1, Key key2, const T &bearingRange,
const R& measuredRange, const SharedNoiseModel& model) const SharedNoiseModel &model)
: Base({key1, key2}, model, T(measuredBearing, measuredRange)) { : Base({{key1, key2}}, model, T(bearingRange)) {
this->initialize(expression({key1, key2})); this->initialize(expression({{key1, key2}}));
}
/// Construct from separate bearing and range
BearingRangeFactor(Key key1, Key key2, const B &measuredBearing,
const R &measuredRange, const SharedNoiseModel &model)
: Base({{key1, key2}}, model, T(measuredBearing, measuredRange)) {
this->initialize(expression({{key1, key2}}));
} }
virtual ~BearingRangeFactor() {} virtual ~BearingRangeFactor() {}

93
gtsam/sfm/BinaryMeasurement.h
View File

@ -15,78 +15,69 @@
* @file BinaryMeasurement.h * @file BinaryMeasurement.h
* @author Akshay Krishnan * @author Akshay Krishnan
* @date July 2020 * @date July 2020
* @brief Binary measurement represents a measurement between two keys in a graph. * @brief Binary measurement represents a measurement between two keys in a
* A binary measurement is similar to a BetweenFactor, except that it does not contain * graph. A binary measurement is similar to a BetweenFactor, except that it
* an error function. It is a measurement (along with a noise model) from one key to * does not contain an error function. It is a measurement (along with a noise
* another. Note that the direction is important. A measurement from key1 to key2 is not * model) from one key to another. Note that the direction is important. A
* the same as the same measurement from key2 to key1. * measurement from key1 to key2 is not the same as the same measurement from
* key2 to key1.
*/ */
#include <ostream>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h> #include <gtsam/inference/Factor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <iostream>
#include <vector>
namespace gtsam { namespace gtsam {
template<class VALUE> template <class T> class BinaryMeasurement : public Factor {
class BinaryMeasurement { // Check that T type is testable
// Check that VALUE type is testable BOOST_CONCEPT_ASSERT((IsTestable<T>));
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
public:
typedef VALUE T;
public:
// shorthand for a smart pointer to a measurement // shorthand for a smart pointer to a measurement
typedef typename boost::shared_ptr<BinaryMeasurement> shared_ptr; using shared_ptr = typename boost::shared_ptr<BinaryMeasurement>;
private: private:
Key key1_, key2_; /** Keys */ T measured_; ///< The measurement
SharedNoiseModel noiseModel_; ///< Noise model
VALUE measured_; /** The measurement */ public:
BinaryMeasurement(Key key1, Key key2, const T &measured,
const SharedNoiseModel &model = nullptr)
: Factor(std::vector<Key>({key1, key2})), measured_(measured),
noiseModel_(model) {}
SharedNoiseModel noiseModel_; /** Noise model */ /// @name Standard Interface
/// @{
public:
/** Constructor */
BinaryMeasurement(Key key1, Key key2, const VALUE &measured,
const SharedNoiseModel &model = nullptr) :
key1_(key1), key2_(key2), measured_(measured), noiseModel_(model) {
}
Key key1() const { return key1_; }
Key key2() const { return key2_; }
Key key1() const { return keys_[0]; }
Key key2() const { return keys_[1]; }
const T &measured() const { return measured_; }
const SharedNoiseModel &noiseModel() const { return noiseModel_; } const SharedNoiseModel &noiseModel() const { return noiseModel_; }
/** implement functions needed for Testable */ /// @}
/// @name Testable
/// @{
/** print */ void print(const std::string &s,
void print(const std::string &s, const KeyFormatter &keyFormatter = DefaultKeyFormatter) const { const KeyFormatter &keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BinaryMeasurement(" std::cout << s << "BinaryMeasurement(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n"; << keyFormatter(this->key2()) << ")\n";
traits<T>::Print(measured_, " measured: "); traits<T>::Print(measured_, " measured: ");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
/** equals */
bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const { bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const {
const BinaryMeasurement<VALUE> *e = dynamic_cast<const BinaryMeasurement<VALUE> *> (&expected); const BinaryMeasurement<T> *e =
return e != nullptr && key1_ == e->key1_ && dynamic_cast<const BinaryMeasurement<T> *>(&expected);
key2_ == e->key2_ return e != nullptr && Factor::equals(*e) &&
&& traits<VALUE>::Equals(this->measured_, e->measured_, tol) && traits<T>::Equals(this->measured_, e->measured_, tol) &&
noiseModel_->equals(*expected.noiseModel()); noiseModel_->equals(*expected.noiseModel());
} }
/// @}
/** return the measured value */ };
VALUE measured() const { } // namespace gtsam
return measured_;
}
}; // \class BetweenMeasurement
}

8
gtsam/slam/BetweenFactor.h
View File

@ -21,6 +21,14 @@
#include <gtsam/base/Lie.h> #include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#ifdef _WIN32
#define BETWEENFACTOR_VISIBILITY
#else
// This will trigger a LNKxxxx on MSVC, so disable for MSVC build
// Please refer to https://github.com/borglab/gtsam/blob/develop/Using-GTSAM-EXPORT.md
#define BETWEENFACTOR_VISIBILITY GTSAM_EXPORT
#endif
namespace gtsam { namespace gtsam {
/** /**

1116
gtsam/slam/dataset.cpp
View File

File diff suppressed because it is too large Load Diff

105
gtsam/slam/dataset.h
View File

@ -20,13 +20,12 @@
#pragma once #pragma once
#include <gtsam/sfm/BinaryMeasurement.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
@ -74,20 +73,42 @@ enum KernelFunctionType {
KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
}; };
/// Return type for auxiliary functions
typedef std::pair<Key, Pose2> IndexedPose;
typedef std::pair<Key, Point2> IndexedLandmark;
typedef std::pair<std::pair<Key, Key>, Pose2> IndexedEdge;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** /**
* Parse TORO/G2O vertex "id x y yaw" * Parse variables in a line-based text format (like g2o) into a map.
* @param is input stream * Instantiated in .cpp Pose2, Point2, Pose3, and Point3.
* @param tag string parsed from input stream, will only parse if vertex type * Note the map keys are integer indices, *not* gtsam::Keys. This is is
* different below where landmarks will use L(index) symbols.
*/ */
GTSAM_EXPORT boost::optional<IndexedPose> parseVertex(std::istream& is, template <typename T>
const std::string& tag); GTSAM_EXPORT std::map<size_t, T> parseVariables(const std::string &filename,
#endif size_t maxIndex = 0);
/**
* Parse binary measurements in a line-based text format (like g2o) into a
* vector. Instantiated in .cpp for Pose2, Rot2, Pose3, and Rot3. The rotation
* versions parse poses and extract only the rotation part, using the marginal
* covariance as noise model.
*/
template <typename T>
GTSAM_EXPORT std::vector<BinaryMeasurement<T>>
parseMeasurements(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model = nullptr,
size_t maxIndex = 0);
/**
* Parse BetweenFactors in a line-based text format (like g2o) into a vector of
* shared pointers. Instantiated in .cpp T equal to Pose2 and Pose3.
*/
template <typename T>
GTSAM_EXPORT std::vector<typename BetweenFactor<T>::shared_ptr>
parseFactors(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model = nullptr,
size_t maxIndex = 0);
/// Return type for auxiliary functions
typedef std::pair<size_t, Pose2> IndexedPose;
typedef std::pair<size_t, Point2> IndexedLandmark;
typedef std::pair<std::pair<size_t, size_t>, Pose2> IndexedEdge;
/** /**
* Parse TORO/G2O vertex "id x y yaw" * Parse TORO/G2O vertex "id x y yaw"
@ -102,7 +123,6 @@ GTSAM_EXPORT boost::optional<IndexedPose> parseVertexPose(std::istream& is,
* @param is input stream * @param is input stream
* @param tag string parsed from input stream, will only parse if vertex type * @param tag string parsed from input stream, will only parse if vertex type
*/ */
GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream& is, GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream& is,
const std::string& tag); const std::string& tag);
@ -114,18 +134,21 @@ GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream&
GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is, GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is,
const std::string& tag); const std::string& tag);
/// Return type for load functions /// Return type for load functions, which return a graph and initial values. For
typedef std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> GraphAndValues; /// landmarks, the gtsam::Symbol L(index) is used to insert into the Values.
/// Bearing-range measurements also refer to landmarks with L(index).
using GraphAndValues =
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr>;
/** /**
* Load TORO 2D Graph * Load TORO 2D Graph
* @param dataset/model pair as constructed by [dataset] * @param dataset/model pair as constructed by [dataset]
* @param maxID if non-zero cut out vertices >= maxID * @param maxIndex if non-zero cut out vertices >= maxIndex
* @param addNoise add noise to the edges * @param addNoise add noise to the edges
* @param smart try to reduce complexity of covariance to cheapest model * @param smart try to reduce complexity of covariance to cheapest model
*/ */
GTSAM_EXPORT GraphAndValues load2D( GTSAM_EXPORT GraphAndValues load2D(
std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0, std::pair<std::string, SharedNoiseModel> dataset, size_t maxIndex = 0,
bool addNoise = false, bool addNoise = false,
bool smart = true, // bool smart = true, //
NoiseFormat noiseFormat = NoiseFormatAUTO, NoiseFormat noiseFormat = NoiseFormatAUTO,
@ -135,7 +158,7 @@ GTSAM_EXPORT GraphAndValues load2D(
* Load TORO/G2O style graph files * Load TORO/G2O style graph files
* @param filename * @param filename
* @param model optional noise model to use instead of one specified by file * @param model optional noise model to use instead of one specified by file
* @param maxID if non-zero cut out vertices >= maxID * @param maxIndex if non-zero cut out vertices >= maxIndex
* @param addNoise add noise to the edges * @param addNoise add noise to the edges
* @param smart try to reduce complexity of covariance to cheapest model * @param smart try to reduce complexity of covariance to cheapest model
* @param noiseFormat how noise parameters are stored * @param noiseFormat how noise parameters are stored
@ -143,13 +166,13 @@ GTSAM_EXPORT GraphAndValues load2D(
* @return graph and initial values * @return graph and initial values
*/ */
GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
SharedNoiseModel model = SharedNoiseModel(), Key maxID = 0, bool addNoise = SharedNoiseModel model = SharedNoiseModel(), size_t maxIndex = 0, bool addNoise =
false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, // false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, //
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel /// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel
GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename, GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
noiseModel::Base::shared_ptr& model, int maxID = 0); noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
/** save 2d graph */ /** save 2d graph */
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
@ -173,21 +196,15 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D
* @param filename The name of the g2o file to write * @param filename The name of the g2o file to write
* @param graph NonlinearFactor graph storing the measurements * @param graph NonlinearFactor graph storing the measurements
* @param estimate Values * @param estimate Values
*
* Note:behavior change in PR #471: to be consistent with load2D and load3D, we
* write the *indices* to file and not the full Keys. This change really only
* affects landmarks, which get read as indices but stored in values with the
* symbol L(index).
*/ */
GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
const Values& estimate, const std::string& filename); const Values& estimate, const std::string& filename);
/// Parse edges in 3D TORO graph file into a set of BetweenFactors.
using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename,
const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr);
/// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s.
GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
/// Parse landmarks in 3D g2o graph file into a map of Point3s.
GTSAM_EXPORT std::map<Key, Point3> parse3DLandmarks(const string& filename);
/// Load TORO 3D Graph /// Load TORO 3D Graph
GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
@ -324,4 +341,24 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
*/ */
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
// To be deprecated when pybind wrapper is merged:
using BetweenFactorPose3s = std::vector<BetweenFactor<Pose3>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose3s
parse3DFactors(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model = nullptr,
size_t maxIndex = 0);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
inline boost::optional<IndexedPose> parseVertex(std::istream &is,
const std::string &tag) {
return parseVertexPose(is, tag);
}
GTSAM_EXPORT std::map<size_t, Pose3> parse3DPoses(const std::string &filename,
size_t maxIndex = 0);
GTSAM_EXPORT std::map<size_t, Point3>
parse3DLandmarks(const std::string &filename, size_t maxIndex = 0);
#endif
} // namespace gtsam } // namespace gtsam

92
gtsam/slam/tests/testDataset.cpp
View File

@ -82,38 +82,73 @@ TEST( dataSet, parseEdge)
const auto actual = parseEdge(is, tag); const auto actual = parseEdge(is, tag);
EXPECT(actual); EXPECT(actual);
if (actual) { if (actual) {
pair<Key, Key> expected(0, 1); pair<size_t, size_t> expected(0, 1);
EXPECT(expected == actual->first); EXPECT(expected == actual->first);
EXPECT(assert_equal(Pose2(2, 3, 4), actual->second)); EXPECT(assert_equal(Pose2(2, 3, 4), actual->second));
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( dataSet, load2D) TEST(dataSet, load2D) {
{
///< The structure where we will save the SfM data ///< The structure where we will save the SfM data
const string filename = findExampleDataFile("w100.graph"); const string filename = findExampleDataFile("w100.graph");
NonlinearFactorGraph::shared_ptr graph; NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial; Values::shared_ptr initial;
boost::tie(graph, initial) = load2D(filename); boost::tie(graph, initial) = load2D(filename);
EXPECT_LONGS_EQUAL(300,graph->size()); EXPECT_LONGS_EQUAL(300, graph->size());
EXPECT_LONGS_EQUAL(100,initial->size()); EXPECT_LONGS_EQUAL(100, initial->size());
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3); auto model = noiseModel::Unit::Create(3);
BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879,0.0417574,-0.00818381), model); BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879, 0.0417574, -0.00818381),
BetweenFactor<Pose2>::shared_ptr actual = boost::dynamic_pointer_cast< model);
BetweenFactor<Pose2> >(graph->at(0)); BetweenFactor<Pose2>::shared_ptr actual =
boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(0));
EXPECT(assert_equal(expected, *actual)); EXPECT(assert_equal(expected, *actual));
// Check binary measurements, Pose2
size_t maxIndex = 5;
auto measurements = parseMeasurements<Pose2>(filename, nullptr, maxIndex);
EXPECT_LONGS_EQUAL(5, measurements.size());
// Check binary measurements, Rot2
auto measurements2 = parseMeasurements<Rot2>(filename);
EXPECT_LONGS_EQUAL(300, measurements2.size());
// // Check factor parsing
const auto actualFactors = parseFactors<Pose2>(filename);
for (size_t i : {0, 1, 2, 3, 4, 5}) {
EXPECT(assert_equal(
*boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(i)),
*actualFactors[i], 1e-5));
}
// Check pose parsing
const auto actualPoses = parseVariables<Pose2>(filename);
for (size_t j : {0, 1, 2, 3, 4}) {
EXPECT(assert_equal(initial->at<Pose2>(j), actualPoses.at(j), 1e-5));
}
// Check landmark parsing
const auto actualLandmarks = parseVariables<Point2>(filename);
EXPECT_LONGS_EQUAL(0, actualLandmarks.size());
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( dataSet, load2DVictoriaPark) TEST(dataSet, load2DVictoriaPark) {
{
const string filename = findExampleDataFile("victoria_park.txt"); const string filename = findExampleDataFile("victoria_park.txt");
NonlinearFactorGraph::shared_ptr graph; NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial; Values::shared_ptr initial;
// Load all
boost::tie(graph, initial) = load2D(filename); boost::tie(graph, initial) = load2D(filename);
EXPECT_LONGS_EQUAL(10608,graph->size()); EXPECT_LONGS_EQUAL(10608, graph->size());
EXPECT_LONGS_EQUAL(7120,initial->size()); EXPECT_LONGS_EQUAL(7120, initial->size());
// Restrict keys
size_t maxIndex = 5;
boost::tie(graph, initial) = load2D(filename, nullptr, maxIndex);
EXPECT_LONGS_EQUAL(5, graph->size());
EXPECT_LONGS_EQUAL(6, initial->size()); // file has 0 as well
EXPECT_LONGS_EQUAL(L(5), graph->at(4)->keys()[1]);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -184,7 +219,7 @@ TEST(dataSet, readG2o3D) {
} }
// Check factor parsing // Check factor parsing
const auto actualFactors = parse3DFactors(g2oFile); const auto actualFactors = parseFactors<Pose3>(g2oFile);
for (size_t i : {0, 1, 2, 3, 4, 5}) { for (size_t i : {0, 1, 2, 3, 4, 5}) {
EXPECT(assert_equal( EXPECT(assert_equal(
*boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]), *boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
@ -192,13 +227,13 @@ TEST(dataSet, readG2o3D) {
} }
// Check pose parsing // Check pose parsing
const auto actualPoses = parse3DPoses(g2oFile); const auto actualPoses = parseVariables<Pose3>(g2oFile);
for (size_t j : {0, 1, 2, 3, 4}) { for (size_t j : {0, 1, 2, 3, 4}) {
EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
} }
// Check landmark parsing // Check landmark parsing
const auto actualLandmarks = parse3DLandmarks(g2oFile); const auto actualLandmarks = parseVariables<Point3>(g2oFile);
for (size_t j : {0, 1, 2, 3, 4}) { for (size_t j : {0, 1, 2, 3, 4}) {
EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
} }
@ -259,7 +294,7 @@ TEST(dataSet, readG2oCheckDeterminants) {
const string g2oFile = findExampleDataFile("toyExample.g2o"); const string g2oFile = findExampleDataFile("toyExample.g2o");
// Check determinants in factors // Check determinants in factors
auto factors = parse3DFactors(g2oFile); auto factors = parseFactors<Pose3>(g2oFile);
EXPECT_LONGS_EQUAL(6, factors.size()); EXPECT_LONGS_EQUAL(6, factors.size());
for (const auto& factor : factors) { for (const auto& factor : factors) {
const Rot3 R = factor->measured().rotation(); const Rot3 R = factor->measured().rotation();
@ -267,13 +302,13 @@ TEST(dataSet, readG2oCheckDeterminants) {
} }
// Check determinants in initial values // Check determinants in initial values
const map<Key, Pose3> poses = parse3DPoses(g2oFile); const map<size_t, Pose3> poses = parseVariables<Pose3>(g2oFile);
EXPECT_LONGS_EQUAL(5, poses.size()); EXPECT_LONGS_EQUAL(5, poses.size());
for (const auto& key_value : poses) { for (const auto& key_value : poses) {
const Rot3 R = key_value.second.rotation(); const Rot3 R = key_value.second.rotation();
EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9); EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9);
} }
const map<Key, Point3> landmarks = parse3DLandmarks(g2oFile); const map<size_t, Point3> landmarks = parseVariables<Point3>(g2oFile);
EXPECT_LONGS_EQUAL(0, landmarks.size()); EXPECT_LONGS_EQUAL(0, landmarks.size());
} }
@ -282,10 +317,13 @@ TEST(dataSet, readG2oLandmarks) {
const string g2oFile = findExampleDataFile("example_with_vertices.g2o"); const string g2oFile = findExampleDataFile("example_with_vertices.g2o");
// Check number of poses and landmarks. Should be 8 each. // Check number of poses and landmarks. Should be 8 each.
const map<Key, Pose3> poses = parse3DPoses(g2oFile); const map<size_t, Pose3> poses = parseVariables<Pose3>(g2oFile);
EXPECT_LONGS_EQUAL(8, poses.size()); EXPECT_LONGS_EQUAL(8, poses.size());
const map<Key, Point3> landmarks = parse3DLandmarks(g2oFile); const map<size_t, Point3> landmarks = parseVariables<Point3>(g2oFile);
EXPECT_LONGS_EQUAL(8, landmarks.size()); EXPECT_LONGS_EQUAL(8, landmarks.size());
auto graphAndValues = load3D(g2oFile);
EXPECT(graphAndValues.second->exists(L(0)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -538,14 +576,12 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
Values value; Values value;
for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera
Key poseKey = symbol('x',i);
Pose3 pose = poseChange.compose(readData.cameras[i].pose()); Pose3 pose = poseChange.compose(readData.cameras[i].pose());
value.insert(poseKey, pose); value.insert(X(i), pose);
} }
for(size_t j=0; j < readData.number_tracks(); j++){ // for each point for(size_t j=0; j < readData.number_tracks(); j++){ // for each point
Key pointKey = P(j);
Point3 point = poseChange.transformFrom( readData.tracks[j].p ); Point3 point = poseChange.transformFrom( readData.tracks[j].p );
value.insert(pointKey, point); value.insert(P(j), point);
} }
// Write values and readData to a file // Write values and readData to a file
@ -570,13 +606,11 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
EXPECT(assert_equal(expected,actual,12)); EXPECT(assert_equal(expected,actual,12));
Pose3 expectedPose = camera0.pose(); Pose3 expectedPose = camera0.pose();
Key poseKey = symbol('x',0); Pose3 actualPose = value.at<Pose3>(X(0));
Pose3 actualPose = value.at<Pose3>(poseKey);
EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); EXPECT(assert_equal(expectedPose,actualPose, 1e-7));
Point3 expectedPoint = track0.p; Point3 expectedPoint = track0.p;
Key pointKey = P(0); Point3 actualPoint = value.at<Point3>(P(0));
Point3 actualPoint = value.at<Point3>(pointKey);
EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6));
} }

2
tests/testNonlinearISAM.cpp
View File

@ -308,7 +308,7 @@ TEST(testNonlinearISAM, loop_closures ) {
// check if edge // check if edge
const auto betweenPose = parseEdge(is, tag); const auto betweenPose = parseEdge(is, tag);
if (betweenPose) { if (betweenPose) {
Key id1, id2; size_t id1, id2;
tie(id1, id2) = betweenPose->first; tie(id1, id2) = betweenPose->first;
graph.emplace_shared<BetweenFactor<Pose2> >(Symbol('x', id2), graph.emplace_shared<BetweenFactor<Pose2> >(Symbol('x', id2),
Symbol('x', id1), betweenPose->second, model); Symbol('x', id1), betweenPose->second, model);

14
timing/timeFrobeniusFactor.cpp
View File

@ -57,8 +57,8 @@ int main(int argc, char* argv[]) {
} }
// Read G2O file // Read G2O file
const auto factors = parse3DFactors(g2oFile); const auto measurements = parseMeasurements<Rot3>(g2oFile);
const auto poses = parse3DPoses(g2oFile); const auto poses = parseVariables<Pose3>(g2oFile);
// Build graph // Build graph
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
@ -66,12 +66,12 @@ int main(int argc, char* argv[]) {
auto priorModel = noiseModel::Isotropic::Sigma(6, 10000); auto priorModel = noiseModel::Isotropic::Sigma(6, 10000);
graph.add(PriorFactor<SOn>(0, SOn::identity(4), priorModel)); graph.add(PriorFactor<SOn>(0, SOn::identity(4), priorModel));
auto G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(4)); auto G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(4));
for (const auto& factor : factors) { for (const auto &m : measurements) {
const auto& keys = factor->keys(); const auto &keys = m.keys();
const auto& Tij = factor->measured(); const Rot3 &Rij = m.measured();
const auto& model = factor->noiseModel(); const auto &model = m.noiseModel();
graph.emplace_shared<FrobeniusWormholeFactor>( graph.emplace_shared<FrobeniusWormholeFactor>(
keys[0], keys[1], Rot3(Tij.rotation().matrix()), 4, model, G); keys[0], keys[1], Rij, 4, model, G);
} }
std::mt19937 rng(42); std::mt19937 rng(42);