Merge branch 'develop' into fix/doxygen
Varun Agrawal
commit
1f6816d974
|
|
@ -43,8 +43,10 @@ if [ -z ${PYTHON_VERSION+x} ]; then
|
|||
exit 127
|
||||
fi
|
||||
|
||||
PYTHON="python${PYTHON_VERSION}"
|
||||
export PYTHON="python${PYTHON_VERSION}"
|
||||
|
||||
function install_dependencies()
|
||||
{
|
||||
if [[ $(uname) == "Darwin" ]]; then
|
||||
brew install wget
|
||||
else
|
||||
|
|
@ -52,18 +54,20 @@ else
|
|||
sudo apt-get install -y wget libicu-dev python3-pip python3-setuptools
|
||||
fi
|
||||
|
||||
PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
|
||||
export PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
|
||||
|
||||
[ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
|
||||
|
||||
$PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
|
||||
}
|
||||
|
||||
BUILD_PYBIND="ON"
|
||||
|
||||
sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
|
||||
|
||||
function build()
|
||||
{
|
||||
mkdir $GITHUB_WORKSPACE/build
|
||||
cd $GITHUB_WORKSPACE/build
|
||||
|
||||
BUILD_PYBIND="ON"
|
||||
|
||||
cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
|
||||
-DGTSAM_BUILD_TESTS=OFF \
|
||||
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
|
||||
|
|
@ -84,5 +88,23 @@ make -j2 install
|
|||
|
||||
cd $GITHUB_WORKSPACE/build/python
|
||||
$PYTHON -m pip install --user .
|
||||
}
|
||||
|
||||
function test()
|
||||
{
|
||||
cd $GITHUB_WORKSPACE/python/gtsam/tests
|
||||
$PYTHON -m unittest discover -v
|
||||
}
|
||||
|
||||
# select between build or test
|
||||
case $1 in
|
||||
-d)
|
||||
install_dependencies
|
||||
;;
|
||||
-b)
|
||||
build
|
||||
;;
|
||||
-t)
|
||||
test
|
||||
;;
|
||||
esac
|
||||
|
|
|
|||
|
|
@ -20,26 +20,26 @@ 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-18.04-gcc-5,
|
||||
ubuntu-18.04-gcc-9,
|
||||
ubuntu-18.04-clang-9,
|
||||
ubuntu-20.04-gcc-7,
|
||||
ubuntu-20.04-gcc-9,
|
||||
ubuntu-20.04-clang-9,
|
||||
]
|
||||
|
||||
build_type: [Debug, Release]
|
||||
build_unstable: [ON]
|
||||
include:
|
||||
- name: ubuntu-18.04-gcc-5
|
||||
os: ubuntu-18.04
|
||||
- name: ubuntu-20.04-gcc-7
|
||||
os: ubuntu-20.04
|
||||
compiler: gcc
|
||||
version: "5"
|
||||
version: "7"
|
||||
|
||||
- name: ubuntu-18.04-gcc-9
|
||||
os: ubuntu-18.04
|
||||
- name: ubuntu-20.04-gcc-9
|
||||
os: ubuntu-20.04
|
||||
compiler: gcc
|
||||
version: "9"
|
||||
|
||||
- name: ubuntu-18.04-clang-9
|
||||
os: ubuntu-18.04
|
||||
- name: ubuntu-20.04-clang-9
|
||||
os: ubuntu-20.04
|
||||
compiler: clang
|
||||
version: "9"
|
||||
|
||||
|
|
@ -60,9 +60,9 @@ jobs:
|
|||
gpg -a --export $LLVM_KEY | sudo apt-key add -
|
||||
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
|
||||
fi
|
||||
sudo apt-get -y update
|
||||
|
||||
sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
|
||||
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
|
||||
|
|
|
|||
|
|
@ -19,34 +19,34 @@ 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-18.04-gcc-5,
|
||||
ubuntu-18.04-gcc-9,
|
||||
ubuntu-18.04-clang-9,
|
||||
ubuntu-20.04-gcc-7,
|
||||
ubuntu-20.04-gcc-9,
|
||||
ubuntu-20.04-clang-9,
|
||||
macOS-11-xcode-13.4.1,
|
||||
ubuntu-18.04-gcc-5-tbb,
|
||||
ubuntu-20.04-gcc-7-tbb,
|
||||
]
|
||||
|
||||
build_type: [Debug, Release]
|
||||
python_version: [3]
|
||||
include:
|
||||
- name: ubuntu-18.04-gcc-5
|
||||
os: ubuntu-18.04
|
||||
- name: ubuntu-20.04-gcc-7
|
||||
os: ubuntu-20.04
|
||||
compiler: gcc
|
||||
version: "5"
|
||||
version: "7"
|
||||
|
||||
- name: ubuntu-18.04-gcc-9
|
||||
os: ubuntu-18.04
|
||||
- name: ubuntu-20.04-gcc-9
|
||||
os: ubuntu-20.04
|
||||
compiler: gcc
|
||||
version: "9"
|
||||
|
||||
- name: ubuntu-18.04-clang-9
|
||||
os: ubuntu-18.04
|
||||
- name: ubuntu-20.04-clang-9
|
||||
os: ubuntu-20.04
|
||||
compiler: clang
|
||||
version: "9"
|
||||
|
||||
# NOTE temporarily added this as it is a required check.
|
||||
- name: ubuntu-18.04-clang-9
|
||||
os: ubuntu-18.04
|
||||
- name: ubuntu-20.04-clang-9
|
||||
os: ubuntu-20.04
|
||||
compiler: clang
|
||||
version: "9"
|
||||
build_type: Debug
|
||||
|
|
@ -57,10 +57,10 @@ jobs:
|
|||
compiler: xcode
|
||||
version: "13.4.1"
|
||||
|
||||
- name: ubuntu-18.04-gcc-5-tbb
|
||||
os: ubuntu-18.04
|
||||
- name: ubuntu-20.04-gcc-7-tbb
|
||||
os: ubuntu-20.04
|
||||
compiler: gcc
|
||||
version: "5"
|
||||
version: "7"
|
||||
flag: tbb
|
||||
|
||||
steps:
|
||||
|
|
@ -79,9 +79,9 @@ jobs:
|
|||
gpg -a --export $LLVM_KEY | sudo apt-key add -
|
||||
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
|
||||
fi
|
||||
sudo apt-get -y update
|
||||
|
||||
sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
|
||||
sudo apt-get -y update
|
||||
sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libboost-all-dev
|
||||
|
||||
if [ "${{ matrix.compiler }}" = "gcc" ]; then
|
||||
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
|
||||
|
|
@ -117,11 +117,12 @@ jobs:
|
|||
uses: pierotofy/set-swap-space@master
|
||||
with:
|
||||
swap-size-gb: 6
|
||||
- name: Build (Linux)
|
||||
if: runner.os == 'Linux'
|
||||
- name: Install Dependencies
|
||||
run: |
|
||||
bash .github/scripts/python.sh
|
||||
- name: Build (macOS)
|
||||
if: runner.os == 'macOS'
|
||||
bash .github/scripts/python.sh -d
|
||||
- name: Build
|
||||
run: |
|
||||
bash .github/scripts/python.sh
|
||||
bash .github/scripts/python.sh -b
|
||||
- name: Test
|
||||
run: |
|
||||
bash .github/scripts/python.sh -t
|
||||
|
|
|
|||
|
|
@ -32,31 +32,31 @@ jobs:
|
|||
|
||||
include:
|
||||
- name: ubuntu-gcc-deprecated
|
||||
os: ubuntu-18.04
|
||||
os: ubuntu-20.04
|
||||
compiler: gcc
|
||||
version: "9"
|
||||
flag: deprecated
|
||||
|
||||
- name: ubuntu-gcc-quaternions
|
||||
os: ubuntu-18.04
|
||||
os: ubuntu-20.04
|
||||
compiler: gcc
|
||||
version: "9"
|
||||
flag: quaternions
|
||||
|
||||
- name: ubuntu-gcc-tbb
|
||||
os: ubuntu-18.04
|
||||
os: ubuntu-20.04
|
||||
compiler: gcc
|
||||
version: "9"
|
||||
flag: tbb
|
||||
|
||||
- name: ubuntu-cayleymap
|
||||
os: ubuntu-18.04
|
||||
os: ubuntu-20.04
|
||||
compiler: gcc
|
||||
version: "9"
|
||||
flag: cayley
|
||||
|
||||
- name: ubuntu-system-libs
|
||||
os: ubuntu-18.04
|
||||
os: ubuntu-20.04
|
||||
compiler: gcc
|
||||
version: "9"
|
||||
flag: system-libs
|
||||
|
|
@ -74,9 +74,9 @@ jobs:
|
|||
gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add -
|
||||
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
|
||||
fi
|
||||
sudo apt-get -y update
|
||||
|
||||
sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
|
||||
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
|
||||
|
|
|
|||
25
README.md
25
README.md
|
|
@ -64,6 +64,29 @@ GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionali
|
|||
|
||||
We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md) wrappers for GTSAM. Please refer to the linked documents for more details.
|
||||
|
||||
## Citation
|
||||
|
||||
If you are using GTSAM for academic work, please use the following citation:
|
||||
|
||||
```
|
||||
@software{gtsam,
|
||||
author = {Frank Dellaert and Richard Roberts and Varun Agrawal and Alex Cunningham and Chris Beall and Duy-Nguyen Ta and Fan Jiang and lucacarlone and nikai and Jose Luis Blanco-Claraco and Stephen Williams and ydjian and John Lambert and Andy Melim and Zhaoyang Lv and Akshay Krishnan and Jing Dong and Gerry Chen and Krunal Chande and balderdash-devil and DiffDecisionTrees and Sungtae An and mpaluri and Ellon Paiva Mendes and Mike Bosse and Akash Patel and Ayush Baid and Paul Furgale and matthewbroadwaynavenio and roderick-koehle},
|
||||
title = {borglab/gtsam},
|
||||
month = may,
|
||||
year = 2022,
|
||||
publisher = {Zenodo},
|
||||
version = {4.2a7},
|
||||
doi = {10.5281/zenodo.5794541},
|
||||
url = {https://doi.org/10.5281/zenodo.5794541}
|
||||
}
|
||||
```
|
||||
|
||||
You can also get the latest citation available from Zenodo below:
|
||||
|
||||
[](https://doi.org/10.5281/zenodo.5794541)
|
||||
|
||||
Specific formats are available in the bottom-right corner of the Zenodo page.
|
||||
|
||||
## The Preintegrated IMU Factor
|
||||
|
||||
GTSAM includes a state of the art IMU handling scheme based on
|
||||
|
|
@ -73,7 +96,7 @@ GTSAM includes a state of the art IMU handling scheme based on
|
|||
Our implementation improves on this using integration on the manifold, as detailed in
|
||||
|
||||
- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, _"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors"_, Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483)
|
||||
- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf)
|
||||
- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, _"IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation"_, Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf)
|
||||
|
||||
If you are using the factor in academic work, please cite the publications above.
|
||||
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -190,7 +190,7 @@ if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
|
|||
endif()
|
||||
|
||||
if (NOT MSVC)
|
||||
option(GTSAM_BUILD_WITH_MARCH_NATIVE "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" ON)
|
||||
option(GTSAM_BUILD_WITH_MARCH_NATIVE "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" OFF)
|
||||
if(GTSAM_BUILD_WITH_MARCH_NATIVE AND (APPLE AND NOT CMAKE_SYSTEM_PROCESSOR STREQUAL "arm64"))
|
||||
# Add as public flag so all dependant projects also use it, as required
|
||||
# by Eigen to avid crashes due to SIMD vectorization:
|
||||
|
|
|
|||
|
|
@ -1,7 +1,10 @@
|
|||
###############################################################################
|
||||
# Option for using system Eigen or GTSAM-bundled Eigen
|
||||
|
||||
option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
|
||||
# Default: Use system's Eigen if found automatically:
|
||||
find_package(Eigen3 QUIET)
|
||||
set(USE_SYSTEM_EIGEN_INITIAL_VALUE ${Eigen3_FOUND})
|
||||
option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" ${USE_SYSTEM_EIGEN_INITIAL_VALUE})
|
||||
unset(USE_SYSTEM_EIGEN_INITIAL_VALUE)
|
||||
|
||||
if(NOT GTSAM_USE_SYSTEM_EIGEN)
|
||||
# This option only makes sense if using the embedded copy of Eigen, it is
|
||||
|
|
@ -11,7 +14,7 @@ endif()
|
|||
|
||||
# Switch for using system Eigen or GTSAM-bundled Eigen
|
||||
if(GTSAM_USE_SYSTEM_EIGEN)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(Eigen3 REQUIRED) # need to find again as REQUIRED
|
||||
|
||||
# Use generic Eigen include paths e.g. <Eigen/Core>
|
||||
set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@ print_build_options_for_target(gtsam)
|
|||
|
||||
print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
|
||||
print_config("Use System Metis" "${GTSAM_USE_SYSTEM_METIS}")
|
||||
print_config("Using Boost version" "${Boost_VERSION}")
|
||||
|
||||
if(GTSAM_USE_TBB)
|
||||
print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})")
|
||||
|
|
|
|||
|
|
@ -1,7 +1,9 @@
|
|||
#LyX 2.0 created this file. For more info see http://www.lyx.org/
|
||||
\lyxformat 413
|
||||
#LyX 2.3 created this file. For more info see http://www.lyx.org/
|
||||
\lyxformat 544
|
||||
\begin_document
|
||||
\begin_header
|
||||
\save_transient_properties true
|
||||
\origin unavailable
|
||||
\textclass article
|
||||
\use_default_options true
|
||||
\maintain_unincluded_children false
|
||||
|
|
@ -9,16 +11,18 @@
|
|||
\language_package default
|
||||
\inputencoding auto
|
||||
\fontencoding global
|
||||
\font_roman default
|
||||
\font_sans default
|
||||
\font_typewriter default
|
||||
\font_roman "default" "default"
|
||||
\font_sans "default" "default"
|
||||
\font_typewriter "default" "default"
|
||||
\font_math "auto" "auto"
|
||||
\font_default_family default
|
||||
\use_non_tex_fonts false
|
||||
\font_sc false
|
||||
\font_osf false
|
||||
\font_sf_scale 100
|
||||
\font_tt_scale 100
|
||||
|
||||
\font_sf_scale 100 100
|
||||
\font_tt_scale 100 100
|
||||
\use_microtype false
|
||||
\use_dash_ligatures true
|
||||
\graphics default
|
||||
\default_output_format default
|
||||
\output_sync 0
|
||||
|
|
@ -29,16 +33,26 @@
|
|||
\use_hyperref false
|
||||
\papersize default
|
||||
\use_geometry true
|
||||
\use_amsmath 1
|
||||
\use_esint 1
|
||||
\use_mhchem 1
|
||||
\use_mathdots 1
|
||||
\use_package amsmath 1
|
||||
\use_package amssymb 1
|
||||
\use_package cancel 1
|
||||
\use_package esint 1
|
||||
\use_package mathdots 1
|
||||
\use_package mathtools 1
|
||||
\use_package mhchem 1
|
||||
\use_package stackrel 1
|
||||
\use_package stmaryrd 1
|
||||
\use_package undertilde 1
|
||||
\cite_engine basic
|
||||
\cite_engine_type default
|
||||
\biblio_style plain
|
||||
\use_bibtopic false
|
||||
\use_indices false
|
||||
\paperorientation portrait
|
||||
\suppress_date false
|
||||
\justification true
|
||||
\use_refstyle 1
|
||||
\use_minted 0
|
||||
\index Index
|
||||
\shortcut idx
|
||||
\color #008000
|
||||
|
|
@ -51,7 +65,10 @@
|
|||
\tocdepth 3
|
||||
\paragraph_separation indent
|
||||
\paragraph_indentation default
|
||||
\quotes_language english
|
||||
\is_math_indent 0
|
||||
\math_numbering_side default
|
||||
\quotes_style english
|
||||
\dynamic_quotes 0
|
||||
\papercolumns 1
|
||||
\papersides 1
|
||||
\paperpagestyle default
|
||||
|
|
@ -65,11 +82,11 @@
|
|||
\begin_body
|
||||
|
||||
\begin_layout Title
|
||||
The new IMU Factor
|
||||
The New IMU Factor
|
||||
\end_layout
|
||||
|
||||
\begin_layout Author
|
||||
Frank Dellaert
|
||||
Frank Dellaert & Varun Agrawal
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
|
|
@ -91,6 +108,282 @@ filename "macros.lyx"
|
|||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset FormulaMacro
|
||||
\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}}
|
||||
{\mathfrak{\mathbb{R}^{9}}}
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset FormulaMacro
|
||||
\newcommand{\Rninethree}{\mathfrak{\mathbb{R}^{9\times3}}}
|
||||
{\mathfrak{\mathbb{R}^{9\times3}}}
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset FormulaMacro
|
||||
\newcommand{\Rninesix}{\mathfrak{\mathbb{R}^{9\times6}}}
|
||||
{\mathfrak{\mathbb{R}^{9\times6}}}
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset FormulaMacro
|
||||
\newcommand{\Rninenine}{\mathfrak{\mathbb{R}^{9\times9}}}
|
||||
{\mathfrak{\mathbb{R}^{9\times9}}}
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
IMU Factor
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The IMU factor has 2 variants:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
ImuFactor is a 5-way factor between the previous pose and velocity, the
|
||||
current pose and velocity, and the current IMU bias.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
ImuFactor2 is a 3-way factor between the previous NavState, the current
|
||||
NavState and the current IMU bias.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
Both variants take a PreintegratedMeasurements object which encodes all
|
||||
the IMU measurements between the previous timestep and the current timestep.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
There are also 2 variants of this class:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
Manifold Preintegration: This version keeps track of the incremental NavState
|
||||
|
||||
\begin_inset Formula $\Delta X_{ij}$
|
||||
\end_inset
|
||||
|
||||
with respect to the previous NavState, on the NavState manifold itself.
|
||||
It also keeps track of the
|
||||
\begin_inset Formula $\Rninesix$
|
||||
\end_inset
|
||||
|
||||
Jacobian of
|
||||
\begin_inset Formula $\Delta X_{ij}$
|
||||
\end_inset
|
||||
|
||||
w.r.t.
|
||||
the bias.
|
||||
This corresponds to Forster et.
|
||||
al.
|
||||
\begin_inset CommandInset citation
|
||||
LatexCommand cite
|
||||
key "Forster15rss"
|
||||
literal "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
Tangent Preintegration: This version keeps track of the incremental NavState
|
||||
in the NavState tangent space instead.
|
||||
This is a
|
||||
\begin_inset Formula $\Rnine$
|
||||
\end_inset
|
||||
|
||||
vector
|
||||
\emph on
|
||||
preintegrated_
|
||||
\emph default
|
||||
.
|
||||
It also keeps track of the
|
||||
\begin_inset Formula $\Rninesix$
|
||||
\end_inset
|
||||
|
||||
jacobian of the
|
||||
\emph on
|
||||
preintegrated_
|
||||
\emph default
|
||||
w.r.t.
|
||||
the bias.
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The main function of a factor is to calculate an error.
|
||||
This is done exactly the same in both variants:
|
||||
\begin_inset Formula
|
||||
\begin{equation}
|
||||
e(X_{i},X_{j})=X_{j}\ominus\widehat{X_{j}}\label{eq:imu-factor-error}
|
||||
\end{equation}
|
||||
|
||||
\end_inset
|
||||
|
||||
where the predicted NavState
|
||||
\begin_inset Formula $\widehat{X_{j}}$
|
||||
\end_inset
|
||||
|
||||
at time
|
||||
\begin_inset Formula $t_{j}$
|
||||
\end_inset
|
||||
|
||||
is a function of the NavState
|
||||
\begin_inset Formula $X_{i}$
|
||||
\end_inset
|
||||
|
||||
at time
|
||||
\begin_inset Formula $t_{i}$
|
||||
\end_inset
|
||||
|
||||
and the preintegrated measurements
|
||||
\begin_inset Formula $PIM$
|
||||
\end_inset
|
||||
|
||||
:
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\widehat{X_{j}}=f(X_{i},PIM)
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
The noise model associated with this factor is assumed to be zero-mean Gaussian
|
||||
with a
|
||||
\begin_inset Formula $9\times9$
|
||||
\end_inset
|
||||
|
||||
covariance matrix
|
||||
\begin_inset Formula $\Sigma_{ij}$
|
||||
\end_inset
|
||||
|
||||
, which is defined in the tangent space
|
||||
\begin_inset Formula $T_{X_{j}}\mathcal{N}$
|
||||
\end_inset
|
||||
|
||||
of the NavState manifold at the NavState
|
||||
\begin_inset Formula $X_{j}$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
This (discrete-time) covariance matrix is computed in the preintegrated
|
||||
measurement class, of which there are two variants as discussed above.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Combined IMU Factor
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The IMU factor above requires that bias drift over time be modeled as a
|
||||
separate stochastic process (using a BetweenFactor for example), a crucial
|
||||
aspect given that the preintegrated measurements depend on these bias values
|
||||
and are thus correlated.
|
||||
For this reason, we provide another type of IMU factor which we term the
|
||||
Combined IMU Factor.
|
||||
This factor similarly has 2 variants:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
CombinedImuFactor is a 6-way factor between the previous pose, velocity
|
||||
and IMU bias and the current pose, velocity and IMU bias.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
CombinedImuFactor2 is a 4-way factor between the previous NavState and IMU
|
||||
bias and the current NavState and IMU bias.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
Since the Combined IMU Factor has a larger state variable due to the inclusion
|
||||
of IMU biases, the noise model associated with this factor is assumed to
|
||||
be a zero mean Gaussian with a
|
||||
\begin_inset Formula $15\times15$
|
||||
\end_inset
|
||||
|
||||
covariance matrix
|
||||
\begin_inset Formula $\Sigma$
|
||||
\end_inset
|
||||
|
||||
, similarly defined on the tangent space of the NavState manifold.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Covariance Matrices
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
For IMU preintegration, it is important to propagate the uncertainty accurately
|
||||
as well.
|
||||
As such, we detail the various covariance matrices used in the preintegration
|
||||
step.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
Gyroscope Covariance
|
||||
\begin_inset Formula $Q_{\omega}$
|
||||
\end_inset
|
||||
|
||||
: Measurement uncertainty of the gyroscope.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
Gyroscope Bias Covariance
|
||||
\begin_inset Formula $Q_{\Delta b^{\omega}}$
|
||||
\end_inset
|
||||
|
||||
: The covariance associated with the gyroscope bias random walk.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
Accelerometer Covariance
|
||||
\begin_inset Formula $Q_{acc}$
|
||||
\end_inset
|
||||
|
||||
: Measurement uncertainty of the accelerometer.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
Accelerometer Bias Covariance
|
||||
\begin_inset Formula $Q_{\Delta b^{acc}}$
|
||||
\end_inset
|
||||
|
||||
: The covariance associated with the accelerometer bias random walk.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
Integration Covariance
|
||||
\begin_inset Formula $Q_{int}$
|
||||
\end_inset
|
||||
|
||||
: This is the uncertainty due to modeling errors in the integration from
|
||||
acceleration to velocity and position.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
Initial Bias Estimate Covariance
|
||||
\begin_inset Formula $Q_{init}$
|
||||
\end_inset
|
||||
|
||||
: This is the uncertainty associated with the estimation of the bias (since
|
||||
we jointly estimate the bias as well).
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Navigation States
|
||||
\end_layout
|
||||
|
|
@ -285,7 +578,15 @@ acceleration
|
|||
\end_inset
|
||||
|
||||
in the body frame.
|
||||
We know (from Murray84book) that the derivative of
|
||||
We know (from
|
||||
\begin_inset CommandInset citation
|
||||
LatexCommand cite
|
||||
key "Murray94book"
|
||||
literal "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
) that the derivative of
|
||||
\begin_inset Formula $R$
|
||||
\end_inset
|
||||
|
||||
|
|
@ -592,6 +893,7 @@ Lie Group Methods
|
|||
\begin_inset CommandInset citation
|
||||
LatexCommand cite
|
||||
key "Iserles00an"
|
||||
literal "true"
|
||||
|
||||
\end_inset
|
||||
|
||||
|
|
@ -707,15 +1009,6 @@ In other words, the vector field
|
|||
Retractions
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset FormulaMacro
|
||||
\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}}
|
||||
{\mathfrak{\mathbb{R}^{9}}}
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
Note that the use of the exponential map in local coordinate mappings is
|
||||
not obligatory, even in the context of Lie groups.
|
||||
|
|
@ -1000,7 +1293,15 @@ In the IMU factor, we need to predict the NavState
|
|||
|
||||
needs to be known in order to compensate properly for the initial velocity
|
||||
and rotated gravity vector.
|
||||
Hence, the idea of Lupton was to split up
|
||||
Hence, the idea of Lupton
|
||||
\begin_inset CommandInset citation
|
||||
LatexCommand cite
|
||||
key "Lupton12tro"
|
||||
literal "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
was to split up
|
||||
\begin_inset Formula $v(t)$
|
||||
\end_inset
|
||||
|
||||
|
|
@ -1057,7 +1358,10 @@ p_{g}(t) & = & R_{i}^{T}\frac{gt^{2}}{2}
|
|||
|
||||
\end_inset
|
||||
|
||||
The recipe for the IMU factor is then, in summary.
|
||||
The recipe for the IMU factor is then, in summary:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
Solve the ordinary differential equations
|
||||
\begin_inset Formula
|
||||
\begin{eqnarray*}
|
||||
|
|
@ -1077,6 +1381,9 @@ starting from zero, up to time
|
|||
\end_inset
|
||||
|
||||
at all times.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
Form the local coordinate vector as
|
||||
\begin_inset Formula
|
||||
\[
|
||||
|
|
@ -1085,6 +1392,10 @@ starting from zero, up to time
|
|||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
Predict the NavState
|
||||
\begin_inset Formula $X_{j}$
|
||||
\end_inset
|
||||
|
|
@ -1179,10 +1490,58 @@ where we defined the rotation matrix
|
|||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Noise Propagation
|
||||
Noise Modeling
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
Given the above solutions to the differential equations, we add noise modeling
|
||||
to account for the various sources of error in the system
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\begin{eqnarray}
|
||||
\theta_{k+1} & = & \theta_{k}+H(\theta_{k})^{-1}\,(\omega_{k}^{b}+\epsilon_{k}^{\omega}-b_{k}^{\omega}-\epsilon_{init}^{\omega})\Delta_{t}\nonumber \\
|
||||
p_{k+1} & = & p_{k}+v_{k}\Delta_{t}+R_{k}(a_{k}^{b}+\epsilon_{k}^{a}-b_{k}^{a}-\epsilon_{init}^{a})\frac{\Delta_{t}^{2}}{2}+\epsilon_{k}^{int}\label{eq:preintegration}\\
|
||||
v_{k+1} & = & v_{k}+R_{k}(a_{k}^{b}+\epsilon_{k}^{a}-b_{k}^{a}-\epsilon_{init}^{a})\Delta_{t}\nonumber \\
|
||||
b_{k+1}^{a} & = & b_{k}^{a}+\epsilon_{k}^{b^{a}}\nonumber \\
|
||||
b_{k+1}^{\omega} & = & b_{k}^{\omega}+\epsilon_{k}^{b^{\omega}}\nonumber
|
||||
\end{eqnarray}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
which we can write compactly as,
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\begin{eqnarray}
|
||||
\theta_{k+1} & = & f_{\theta}(\theta_{k},b_{k}^{w},\epsilon_{k}^{\omega},\epsilon_{init}^{b^{\omega}})\label{eq:compact-preintegration}\\
|
||||
p_{k+1} & = & f_{p}(p_{k},v_{k},\theta_{k},b_{k}^{a},\epsilon_{k}^{a},\epsilon_{init}^{a},\epsilon_{k}^{int})\nonumber \\
|
||||
v_{k+1} & = & f_{v}(v_{k,}\theta_{k,}b_{k}^{a},\epsilon_{k}^{a},\epsilon_{init}^{a})\nonumber \\
|
||||
b_{k+1}^{a} & = & f_{b^{a}}(b_{k}^{a},\epsilon_{k}^{b^{a}})\nonumber \\
|
||||
b_{k+1}^{\omega} & = & f_{b^{\omega}}(b_{k}^{\omega},\epsilon_{k}^{b^{\omega}})\nonumber
|
||||
\end{eqnarray}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Noise Propagation in IMU Factor
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
We wish to compute the ImuFactor covariance matrix
|
||||
\begin_inset Formula $\Sigma_{ij}$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
Even when we assume uncorrelated noise on
|
||||
\begin_inset Formula $\omega^{b}$
|
||||
\end_inset
|
||||
|
|
@ -1201,11 +1560,12 @@ Even when we assume uncorrelated noise on
|
|||
\end_inset
|
||||
|
||||
appear in multiple places.
|
||||
To model the noise propagation, let us define
|
||||
To model the noise propagation, let us define the preintegrated navigation
|
||||
state
|
||||
\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k}]$
|
||||
\end_inset
|
||||
|
||||
and rewrite Eqns.
|
||||
, as a 9D vector on tangent space at and rewrite Eqns.
|
||||
(
|
||||
\begin_inset CommandInset ref
|
||||
LatexCommand ref
|
||||
|
|
@ -1239,7 +1599,7 @@ Then the noise on
|
|||
propagates as
|
||||
\begin_inset Formula
|
||||
\begin{equation}
|
||||
\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}+C_{k}\Sigma_{\eta}^{gd}C_{k}\label{eq:prop}
|
||||
\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}^{T}+C_{k}\Sigma_{\eta}^{gd}C_{k}^{T}\label{eq:prop}
|
||||
\end{equation}
|
||||
|
||||
\end_inset
|
||||
|
|
@ -1280,6 +1640,42 @@ where
|
|||
\begin_inset Formula $\omega^{b}$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
Note that
|
||||
\begin_inset Formula $\Sigma_{k},$
|
||||
\end_inset
|
||||
|
||||
|
||||
\begin_inset Formula $\Sigma_{\eta}^{ad}$
|
||||
\end_inset
|
||||
|
||||
, and
|
||||
\begin_inset Formula $\Sigma_{\eta}^{gd}$
|
||||
\end_inset
|
||||
|
||||
are discrete time covariances with
|
||||
\begin_inset Formula $\Sigma_{\eta}^{ad}$
|
||||
\end_inset
|
||||
|
||||
, and
|
||||
\begin_inset Formula $\Sigma_{\eta}^{gd}$
|
||||
\end_inset
|
||||
|
||||
divided by
|
||||
\begin_inset Formula $\Delta_{t}$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
Please see the section on Covariance Discretization
|
||||
\begin_inset CommandInset ref
|
||||
LatexCommand vpageref
|
||||
reference "subsec:Covariance-Discretization"
|
||||
plural "false"
|
||||
caps "false"
|
||||
noprefix "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
.
|
||||
\end_layout
|
||||
|
||||
|
|
@ -1295,7 +1691,7 @@ We start with the noise propagation on
|
|||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
|
||||
\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
|
@ -1307,7 +1703,7 @@ It can be shown that for small
|
|||
we have
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}-\frac{\Delta t}{2}\Skew{\omega_{k}^{b}}
|
||||
\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
|
@ -1357,9 +1753,9 @@ Putting all this together, we finally obtain
|
|||
\begin_inset Formula
|
||||
\[
|
||||
A_{k}\approx\left[\begin{array}{ccc}
|
||||
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}\\
|
||||
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{3\times3}\\
|
||||
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t}\\
|
||||
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & & I_{3\times3}
|
||||
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3}
|
||||
\end{array}\right]
|
||||
\]
|
||||
|
||||
|
|
@ -1384,9 +1780,393 @@ H(\theta_{k})^{-1}\Delta_{t}\\
|
|||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Noise Propagation in Combined IMU Factor
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
We can similarly account for bias drift over time, as is commonly seen in
|
||||
commercial grade IMUs.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
We expand the state vector as
|
||||
\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k},b_{k}^{a},b_{k}^{\omega}]$
|
||||
\end_inset
|
||||
|
||||
to include the bias terms and define the augmented noise vector
|
||||
\begin_inset Formula $\epsilon=[\epsilon_{k}^{\omega},\epsilon_{k}^{a},\epsilon_{k}^{b^{a}},\epsilon_{k}^{b^{\omega}},\epsilon_{k}^{int},\epsilon_{init}^{b^{a}},\epsilon_{init}^{b^{\omega}}]$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
This gives the noise propagation equation as
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\begin{equation}
|
||||
\Sigma_{k+1}=F_{k}\Sigma_{k}F_{k}^{T}+G_{k}Q_{k}G_{k}^{T}\label{eq:prop-combined}
|
||||
\end{equation}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
where
|
||||
\begin_inset Formula $F_{k}$
|
||||
\end_inset
|
||||
|
||||
is the
|
||||
\begin_inset Formula $15\times15$
|
||||
\end_inset
|
||||
|
||||
derivative of
|
||||
\begin_inset Formula $f$
|
||||
\end_inset
|
||||
|
||||
wrpt this new
|
||||
\begin_inset Formula $\zeta$
|
||||
\end_inset
|
||||
|
||||
, and
|
||||
\begin_inset Formula $G_{k}$
|
||||
\end_inset
|
||||
|
||||
is the
|
||||
\begin_inset Formula $15\times21$
|
||||
\end_inset
|
||||
|
||||
matrix for first order uncertainty propagation.
|
||||
|
||||
\begin_inset Formula $Q_{k}$
|
||||
\end_inset
|
||||
|
||||
defines the uncertainty of
|
||||
\begin_inset Formula $\eta$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
The top-left
|
||||
\begin_inset Formula $9\times9$
|
||||
\end_inset
|
||||
|
||||
of
|
||||
\begin_inset Formula $F_{k}$
|
||||
\end_inset
|
||||
|
||||
is the same as
|
||||
\begin_inset Formula $A_{k}$
|
||||
\end_inset
|
||||
|
||||
, thus we only have the jacobians wrpt the biases left to account for.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
Conveniently, the jacobians of the pose and velocity wrpt the biases are
|
||||
already computed in the
|
||||
\emph on
|
||||
ImuFactor
|
||||
\emph default
|
||||
derivation as matrices
|
||||
\begin_inset Formula $B_{k}$
|
||||
\end_inset
|
||||
|
||||
and
|
||||
\begin_inset Formula $C_{k}$
|
||||
\end_inset
|
||||
|
||||
, while they are identity matrices wrpt the biases themselves.
|
||||
Thus, we can easily plug-in the values from the previous section to give
|
||||
us the final result
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
F_{k}\approx\left[\begin{array}{ccccc}
|
||||
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & H(\theta_{k})^{-1}\Delta_{t}\\
|
||||
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t} & R_{k}\frac{\Delta_{t}}{2}^{2} & 0_{3\times3}\\
|
||||
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3} & R_{k}\Delta_{t} & 0_{3\times3}\\
|
||||
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3} & 0_{3\times3}\\
|
||||
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3}
|
||||
\end{array}\right]
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
Similarly for
|
||||
\begin_inset Formula $Q_{k},$
|
||||
\end_inset
|
||||
|
||||
we get
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
Q_{k}=\left[\begin{array}{ccccccc}
|
||||
\Sigma^{\omega}\\
|
||||
& \Sigma^{a}\\
|
||||
& & \Sigma^{b^{a}}\\
|
||||
& & & \Sigma^{b^{\omega}}\\
|
||||
& & & & \Sigma^{int}\\
|
||||
& & & & & \Sigma^{init_{11}} & \Sigma^{init_{12}}\\
|
||||
& & & & & \Sigma^{init_{21}} & \Sigma^{init_{22}}
|
||||
\end{array}\right]
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
and for
|
||||
\begin_inset Formula $G_{k}$
|
||||
\end_inset
|
||||
|
||||
we get
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
G_{k}=\left[\begin{array}{ccccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}} & \deriv{\theta}{\epsilon^{a}} & \deriv{\theta}{\epsilon^{b^{a}}} & \deriv{\theta}{\epsilon^{b^{\omega}}} & \deriv{\theta}{\epsilon^{int}} & \deriv{\theta}{\epsilon_{init}^{b^{a}}} & \deriv{\theta}{\epsilon_{init}^{b^{\omega}}}\\
|
||||
\deriv p{\epsilon^{\omega}} & \deriv p{\epsilon^{a}} & \deriv p{\epsilon^{b^{a}}} & \deriv p{\epsilon^{b^{\omega}}} & \deriv p{\epsilon^{int}} & \deriv p{\epsilon_{init}^{b^{a}}} & \deriv p{\epsilon_{init}^{b^{\omega}}}\\
|
||||
\deriv v{\epsilon^{\omega}} & \deriv v{\epsilon^{a}} & \deriv v{\epsilon^{b^{a}}} & \deriv v{\epsilon^{b^{\omega}}} & \deriv v{\epsilon^{int}} & \deriv v{\epsilon_{init}^{b^{a}}} & \deriv v{\epsilon_{init}^{b^{\omega}}}\\
|
||||
\deriv{b^{a}}{\epsilon^{\omega}} & \deriv{b^{a}}{\epsilon^{a}} & \deriv{b^{a}}{\epsilon^{b^{a}}} & \deriv{b^{a}}{\epsilon^{b^{\omega}}} & \deriv{b^{a}}{\epsilon^{int}} & \deriv{b^{a}}{\epsilon_{init}^{b^{a}}} & \deriv{b^{a}}{\epsilon_{init}^{b^{\omega}}}\\
|
||||
\deriv{b^{\omega}}{\epsilon^{\omega}} & \deriv{b^{\omega}}{\epsilon^{a}} & \deriv{b^{\omega}}{\epsilon^{b^{a}}} & \deriv{b^{\omega}}{\epsilon^{b^{\omega}}} & \deriv{b^{\omega}}{\epsilon^{int}} & \deriv{b^{\omega}}{\epsilon_{init}^{b^{a}}} & \deriv{b^{\omega}}{\epsilon_{init}^{b^{\omega}}}
|
||||
\end{array}\right]=\left[\begin{array}{ccccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\\
|
||||
0 & \deriv p{\epsilon^{a}} & 0 & 0 & \deriv p{\epsilon^{int}} & \deriv p{\eta_{init}^{b^{a}}} & 0\\
|
||||
0 & \deriv v{\epsilon^{a}} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}} & 0\\
|
||||
0 & 0 & I_{3\times3} & 0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & I_{3\times3} & 0 & 0 & 0
|
||||
\end{array}\right]
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
We can perform the block-wise computation of
|
||||
\begin_inset Formula $G_{k}Q_{k}G_{k}^{T}$
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\\
|
||||
0 & \deriv p{\epsilon^{a}} & 0 & 0 & \deriv p{\epsilon^{int}} & \deriv p{\eta_{init}^{b^{a}}} & 0\\
|
||||
0 & \deriv v{\epsilon^{a}} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}} & 0\\
|
||||
0 & 0 & I_{3\times3} & 0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & I_{3\times3} & 0 & 0 & 0
|
||||
\end{array}\right]\left[\begin{array}{ccccccc}
|
||||
\Sigma^{\omega}\\
|
||||
& \Sigma^{a}\\
|
||||
& & \Sigma^{b^{a}}\\
|
||||
& & & \Sigma^{b^{\omega}}\\
|
||||
& & & & \Sigma^{int}\\
|
||||
& & & & & \Sigma^{init_{11}} & \Sigma^{init_{12}}\\
|
||||
& & & & & \Sigma^{init_{21}} & \Sigma^{init_{22}}
|
||||
\end{array}\right]G_{k}^{T}
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega} & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\\
|
||||
0 & \deriv p{\epsilon^{a}}\Sigma^{a} & 0 & 0 & \deriv p{\epsilon^{int}}\Sigma^{int} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
|
||||
0 & \deriv v{\epsilon^{a}}\Sigma^{a} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
|
||||
0 & 0 & \Sigma^{b^{a}} & 0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & \Sigma^{b^{\omega}} & 0 & 0 & 0
|
||||
\end{array}\right]G_{k}^{T}
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\begin{multline*}
|
||||
G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega} & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\\
|
||||
0 & \deriv p{\epsilon^{a}}\Sigma^{a} & 0 & 0 & \deriv p{\epsilon^{int}}\Sigma^{int} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
|
||||
0 & \deriv v{\epsilon^{a}}\Sigma^{a} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
|
||||
0 & 0 & \Sigma^{b^{a}} & 0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & \Sigma^{b^{\omega}} & 0 & 0 & 0
|
||||
\end{array}\right]\\
|
||||
\left[\begin{array}{ccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}}^{T} & 0 & 0 & 0 & 0\\
|
||||
0 & \deriv p{\epsilon^{a}}^{T} & \deriv v{\epsilon^{a}}^{T} & 0 & 0\\
|
||||
0 & 0 & 0 & I_{3\times3} & 0\\
|
||||
0 & 0 & 0 & 0 & I_{3\times3}\\
|
||||
0 & \deriv p{\epsilon^{int}}^{T} & 0 & 0 & 0\\
|
||||
0 & \deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
||||
\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & 0 & 0 & 0
|
||||
\end{array}\right]
|
||||
\end{multline*}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\begin{multline*}
|
||||
=\\
|
||||
\left[\begin{array}{ccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}}^{T}+\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
||||
\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}^{T}\\
|
||||
& +\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
||||
\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
||||
0 & 0 & 0 & \Sigma^{b^{a}} & 0\\
|
||||
0 & 0 & 0 & 0 & \Sigma^{b^{\omega}}
|
||||
\end{array}\right]
|
||||
\end{multline*}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
which we can break into 3 matrices for clarity, representing the main diagonal
|
||||
and off-diagonal elements
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\begin{multline*}
|
||||
=\\
|
||||
\left[\begin{array}{ccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}}^{T} & 0 & 0 & 0 & 0\\
|
||||
0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T} & 0 & 0 & 0\\
|
||||
0 & 0 & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T} & 0 & 0\\
|
||||
0 & 0 & 0 & \Sigma^{b^{a}} & 0\\
|
||||
0 & 0 & 0 & 0 & \Sigma^{b^{\omega}}
|
||||
\end{array}\right]+\\
|
||||
\left[\begin{array}{ccccc}
|
||||
\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & 0 & 0 & 0\\
|
||||
0 & \deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & 0 & 0 & 0\\
|
||||
0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
||||
0 & 0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0 & 0
|
||||
\end{array}\right]+\\
|
||||
\left[\begin{array}{ccccc}
|
||||
0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
||||
\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
||||
\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0 & 0
|
||||
\end{array}\right]
|
||||
\end{multline*}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Covariance Discretization
|
||||
\begin_inset CommandInset label
|
||||
LatexCommand label
|
||||
name "subsec:Covariance-Discretization"
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
So far, all the covariances are assumed to be continuous since the state
|
||||
and measurement models are considered to be continuous-time stochastic
|
||||
processes.
|
||||
However, we sample measurements in a discrete-time fashion, necessitating
|
||||
the need to convert the covariances to their discrete time equivalents.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The IMU is modeled as a first order Gauss-Markov process, with a measurement
|
||||
noise and a process noise.
|
||||
Following
|
||||
\begin_inset CommandInset citation
|
||||
LatexCommand cite
|
||||
after "Alg. 1 Page 57"
|
||||
key "Nikolic16thesis"
|
||||
literal "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
and
|
||||
\begin_inset CommandInset citation
|
||||
LatexCommand cite
|
||||
after "Eqns 129-130"
|
||||
key "Trawny05report_IndirectKF"
|
||||
literal "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
, the measurement noises
|
||||
\begin_inset Formula $[\epsilon^{a},\epsilon^{\omega},\epsilon_{init}]$
|
||||
\end_inset
|
||||
|
||||
are simply scaled by
|
||||
\begin_inset Formula $\frac{1}{\Delta t}$
|
||||
\end_inset
|
||||
|
||||
, and the process noises
|
||||
\begin_inset Formula $[\epsilon^{int},\epsilon^{b^{a}},\epsilon^{b^{\omega}}]$
|
||||
\end_inset
|
||||
|
||||
are scaled by
|
||||
\begin_inset Formula $\Delta t$
|
||||
\end_inset
|
||||
|
||||
where
|
||||
\begin_inset Formula $\Delta t$
|
||||
\end_inset
|
||||
|
||||
is the time interval between 2 consecutive samples.
|
||||
For a thorough explanation of the discretization process, please refer
|
||||
to
|
||||
\begin_inset CommandInset citation
|
||||
LatexCommand cite
|
||||
after "Section 8.1"
|
||||
key "Simon06book"
|
||||
literal "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset CommandInset bibtex
|
||||
LatexCommand bibtex
|
||||
btprint "btPrintCited"
|
||||
bibfiles "refs"
|
||||
options "plain"
|
||||
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
76
doc/refs.bib
76
doc/refs.bib
|
|
@ -1,26 +1,72 @@
|
|||
%% This BibTeX bibliography file was created using BibDesk.
|
||||
%% https://bibdesk.sourceforge.io/
|
||||
|
||||
%% Created for Varun Agrawal at 2021-09-27 17:39:09 -0400
|
||||
|
||||
|
||||
%% Saved with string encoding Unicode (UTF-8)
|
||||
|
||||
|
||||
|
||||
@article{Lupton12tro,
|
||||
author = {Lupton, Todd and Sukkarieh, Salah},
|
||||
date-added = {2021-09-27 17:38:56 -0400},
|
||||
date-modified = {2021-09-27 17:39:09 -0400},
|
||||
doi = {10.1109/TRO.2011.2170332},
|
||||
journal = {IEEE Transactions on Robotics},
|
||||
number = {1},
|
||||
pages = {61-76},
|
||||
title = {Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions},
|
||||
volume = {28},
|
||||
year = {2012},
|
||||
Bdsk-Url-1 = {https://doi.org/10.1109/TRO.2011.2170332}}
|
||||
|
||||
@inproceedings{Forster15rss,
|
||||
author = {Christian Forster and Luca Carlone and Frank Dellaert and Davide Scaramuzza},
|
||||
booktitle = {Robotics: Science and Systems},
|
||||
date-added = {2021-09-26 20:44:41 -0400},
|
||||
date-modified = {2021-09-26 20:45:03 -0400},
|
||||
title = {IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation},
|
||||
year = {2015}}
|
||||
|
||||
@article{Iserles00an,
|
||||
title = {Lie-group methods},
|
||||
author = {Iserles, Arieh and Munthe-Kaas, Hans Z and
|
||||
N{\o}rsett, Syvert P and Zanna, Antonella},
|
||||
author = {Iserles, Arieh and Munthe-Kaas, Hans Z and N{\o}rsett, Syvert P and Zanna, Antonella},
|
||||
journal = {Acta Numerica 2000},
|
||||
volume = {9},
|
||||
pages = {215--365},
|
||||
year = {2000},
|
||||
publisher = {Cambridge Univ Press}
|
||||
}
|
||||
publisher = {Cambridge Univ Press},
|
||||
title = {Lie-group methods},
|
||||
volume = {9},
|
||||
year = {2000}}
|
||||
|
||||
@book{Murray94book,
|
||||
author = {Murray, Richard M and Li, Zexiang and Sastry, S Shankar and Sastry, S Shankara},
|
||||
publisher = {CRC press},
|
||||
title = {A mathematical introduction to robotic manipulation},
|
||||
author = {Murray, Richard M and Li, Zexiang and Sastry, S
|
||||
Shankar and Sastry, S Shankara},
|
||||
year = {1994},
|
||||
publisher = {CRC press}
|
||||
}
|
||||
year = {1994}}
|
||||
|
||||
@book{Spivak65book,
|
||||
title = {Calculus on manifolds},
|
||||
author = {Spivak, Michael},
|
||||
publisher = {WA Benjamin New York},
|
||||
title = {Calculus on manifolds},
|
||||
volume = {1},
|
||||
year = {1965},
|
||||
publisher = {WA Benjamin New York}
|
||||
year = {1965}}
|
||||
|
||||
@phdthesis{Nikolic16thesis,
|
||||
title={Characterisation, calibration, and design of visual-inertial sensor systems for robot navigation},
|
||||
author={Nikolic, Janosch},
|
||||
year={2016},
|
||||
school={ETH Zurich}
|
||||
}
|
||||
|
||||
@book{Simon06book,
|
||||
title={Optimal state estimation: Kalman, H infinity, and nonlinear approaches},
|
||||
author={Simon, Dan},
|
||||
year={2006},
|
||||
publisher={John Wiley \& Sons}
|
||||
}
|
||||
|
||||
@inproceedings{Trawny05report_IndirectKF,
|
||||
title={Indirect Kalman Filter for 3 D Attitude Estimation},
|
||||
author={Nikolas Trawny and Stergios I. Roumeliotis},
|
||||
year={2005}
|
||||
}
|
||||
|
|
@ -60,13 +60,14 @@ namespace po = boost::program_options;
|
|||
|
||||
po::variables_map parseOptions(int argc, char* argv[]) {
|
||||
po::options_description desc;
|
||||
desc.add_options()("help,h", "produce help message")(
|
||||
"data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
|
||||
"path to the CSV file with the IMU data")(
|
||||
"output_filename",
|
||||
desc.add_options()("help,h", "produce help message") // help message
|
||||
("data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
|
||||
"path to the CSV file with the IMU data") // path to the data file
|
||||
("output_filename",
|
||||
po::value<string>()->default_value("imuFactorExampleResults.csv"),
|
||||
"path to the result file to use")("use_isam", po::bool_switch(),
|
||||
"use ISAM as the optimizer");
|
||||
"path to the result file to use") // filename to save results to
|
||||
("use_isam", po::bool_switch(),
|
||||
"use ISAM as the optimizer"); // flag for ISAM optimizer
|
||||
|
||||
po::variables_map vm;
|
||||
po::store(po::parse_command_line(argc, argv, desc), vm);
|
||||
|
|
@ -106,7 +107,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
|
|||
I_3x3 * 1e-8; // error committed in integrating position from velocities
|
||||
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
|
||||
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
|
||||
Matrix66 bias_acc_omega_int =
|
||||
Matrix66 bias_acc_omega_init =
|
||||
I_6x6 * 1e-5; // error in the bias used for preintegration
|
||||
|
||||
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
|
||||
|
|
@ -122,7 +123,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
|
|||
// PreintegrationCombinedMeasurements params:
|
||||
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
|
||||
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
|
||||
p->biasAccOmegaInt = bias_acc_omega_int;
|
||||
p->biasAccOmegaInt = bias_acc_omega_init;
|
||||
|
||||
return p;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -94,7 +94,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
|
|||
I_3x3 * 1e-8; // error committed in integrating position from velocities
|
||||
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
|
||||
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
|
||||
Matrix66 bias_acc_omega_int =
|
||||
Matrix66 bias_acc_omega_init =
|
||||
I_6x6 * 1e-5; // error in the bias used for preintegration
|
||||
|
||||
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
|
||||
|
|
@ -110,7 +110,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
|
|||
// PreintegrationCombinedMeasurements params:
|
||||
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
|
||||
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
|
||||
p->biasAccOmegaInt = bias_acc_omega_int;
|
||||
p->biasAccOmegaInt = bias_acc_omega_init;
|
||||
|
||||
return p;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -33,7 +33,6 @@ The following examples illustrate some concepts from Georgia Tech's research pap
|
|||
## 2D Pose SLAM
|
||||
|
||||
* **LocalizationExample.cpp**: modeling robot motion
|
||||
* **LocalizationExample2.cpp**: example with GPS like measurements
|
||||
* **Pose2SLAMExample**: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
|
||||
* **Pose2SLAMExample_advanced**: same, but uses an Optimizer object
|
||||
* **Pose2SLAMwSPCG**: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
|
||||
|
|
|
|||
|
|
@ -95,7 +95,7 @@ template<class Class>
|
|||
struct MultiplicativeGroupTraits {
|
||||
typedef group_tag structure_category;
|
||||
typedef multiplicative_group_tag group_flavor;
|
||||
static Class Identity() { return Class::identity(); }
|
||||
static Class Identity() { return Class::Identity(); }
|
||||
static Class Compose(const Class &g, const Class & h) { return g * h;}
|
||||
static Class Between(const Class &g, const Class & h) { return g.inverse() * h;}
|
||||
static Class Inverse(const Class &g) { return g.inverse();}
|
||||
|
|
@ -111,7 +111,7 @@ template<class Class>
|
|||
struct AdditiveGroupTraits {
|
||||
typedef group_tag structure_category;
|
||||
typedef additive_group_tag group_flavor;
|
||||
static Class Identity() { return Class::identity(); }
|
||||
static Class Identity() { return Class::Identity(); }
|
||||
static Class Compose(const Class &g, const Class & h) { return g + h;}
|
||||
static Class Between(const Class &g, const Class & h) { return h - g;}
|
||||
static Class Inverse(const Class &g) { return -g;}
|
||||
|
|
@ -147,7 +147,7 @@ public:
|
|||
DirectProduct(const G& g, const H& h):std::pair<G,H>(g,h) {}
|
||||
|
||||
// identity
|
||||
static DirectProduct identity() { return DirectProduct(); }
|
||||
static DirectProduct Identity() { return DirectProduct(); }
|
||||
|
||||
DirectProduct operator*(const DirectProduct& other) const {
|
||||
return DirectProduct(traits<G>::Compose(this->first, other.first),
|
||||
|
|
@ -181,7 +181,7 @@ public:
|
|||
DirectSum(const G& g, const H& h):std::pair<G,H>(g,h) {}
|
||||
|
||||
// identity
|
||||
static DirectSum identity() { return DirectSum(); }
|
||||
static DirectSum Identity() { return DirectSum(); }
|
||||
|
||||
DirectSum operator+(const DirectSum& other) const {
|
||||
return DirectSum(g()+other.g(), h()+other.h());
|
||||
|
|
|
|||
|
|
@ -177,7 +177,7 @@ struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
|
|||
/// @name Group
|
||||
/// @{
|
||||
typedef multiplicative_group_tag group_flavor;
|
||||
static Class Identity() { return Class::identity();}
|
||||
static Class Identity() { return Class::Identity();}
|
||||
/// @}
|
||||
|
||||
/// @name Manifold
|
||||
|
|
|
|||
|
|
@ -48,7 +48,7 @@ public:
|
|||
/// @name Group
|
||||
/// @{
|
||||
typedef multiplicative_group_tag group_flavor;
|
||||
static ProductLieGroup identity() {return ProductLieGroup();}
|
||||
static ProductLieGroup Identity() {return ProductLieGroup();}
|
||||
|
||||
ProductLieGroup operator*(const ProductLieGroup& other) const {
|
||||
return ProductLieGroup(traits<G>::Compose(this->first,other.first),
|
||||
|
|
|
|||
|
|
@ -60,6 +60,7 @@ GTSAM_MAKE_VECTOR_DEFS(9)
|
|||
GTSAM_MAKE_VECTOR_DEFS(10)
|
||||
GTSAM_MAKE_VECTOR_DEFS(11)
|
||||
GTSAM_MAKE_VECTOR_DEFS(12)
|
||||
GTSAM_MAKE_VECTOR_DEFS(15)
|
||||
|
||||
typedef Eigen::VectorBlock<Vector> SubVector;
|
||||
typedef Eigen::VectorBlock<const Vector> ConstSubVector;
|
||||
|
|
|
|||
|
|
@ -169,7 +169,7 @@ struct HasVectorSpacePrereqs {
|
|||
Vector v;
|
||||
|
||||
BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) {
|
||||
p = Class::identity(); // identity
|
||||
p = Class::Identity(); // identity
|
||||
q = p + p; // addition
|
||||
q = p - p; // subtraction
|
||||
v = p.vector(); // conversion to vector
|
||||
|
|
@ -192,7 +192,7 @@ struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
|
|||
/// @name Group
|
||||
/// @{
|
||||
typedef additive_group_tag group_flavor;
|
||||
static Class Identity() { return Class::identity();}
|
||||
static Class Identity() { return Class::Identity();}
|
||||
/// @}
|
||||
|
||||
/// @name Manifold
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ class Symmetric: private Eigen::PermutationMatrix<N> {
|
|||
Eigen::PermutationMatrix<N>(P) {
|
||||
}
|
||||
public:
|
||||
static Symmetric identity() { return Symmetric(); }
|
||||
static Symmetric Identity() { return Symmetric(); }
|
||||
Symmetric() {
|
||||
Eigen::PermutationMatrix<N>::setIdentity();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -189,9 +189,9 @@ class ParameterMatrix {
|
|||
* NOTE: The size at compile time is unknown so this identity is zero
|
||||
* length and thus not valid.
|
||||
*/
|
||||
inline static ParameterMatrix identity() {
|
||||
inline static ParameterMatrix Identity() {
|
||||
// throw std::runtime_error(
|
||||
// "ParameterMatrix::identity(): Don't use this function");
|
||||
// "ParameterMatrix::Identity(): Don't use this function");
|
||||
return ParameterMatrix(0);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -137,7 +137,7 @@ class FitBasis {
|
|||
static gtsam::GaussianFactorGraph::shared_ptr LinearGraph(
|
||||
const std::map<double, double>& sequence,
|
||||
const gtsam::noiseModel::Base* model, size_t N);
|
||||
This::Parameters parameters() const;
|
||||
gtsam::This::Parameters parameters() const;
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -133,7 +133,7 @@ TEST(ParameterMatrix, VectorSpace) {
|
|||
// vector
|
||||
EXPECT(assert_equal(Vector::Ones(M * N), params.vector()));
|
||||
// identity
|
||||
EXPECT(assert_equal(ParameterMatrix<M>::identity(),
|
||||
EXPECT(assert_equal(ParameterMatrix<M>::Identity(),
|
||||
ParameterMatrix<M>(Matrix::Zero(M, 0))));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ public:
|
|||
/// Default constructor yields identity
|
||||
Cyclic():i_(0) {
|
||||
}
|
||||
static Cyclic identity() { return Cyclic();}
|
||||
static Cyclic Identity() { return Cyclic();}
|
||||
|
||||
/// Cast to size_t
|
||||
operator size_t() const {
|
||||
|
|
|
|||
|
|
@ -213,7 +213,7 @@ public:
|
|||
}
|
||||
|
||||
/// for Canonical
|
||||
static PinholeCamera identity() {
|
||||
static PinholeCamera Identity() {
|
||||
return PinholeCamera(); // assumes that the default constructor is valid
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -412,7 +412,7 @@ public:
|
|||
}
|
||||
|
||||
/// for Canonical
|
||||
static PinholePose identity() {
|
||||
static PinholePose Identity() {
|
||||
return PinholePose(); // assumes that the default constructor is valid
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -119,7 +119,7 @@ public:
|
|||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
inline static Pose2 identity() { return Pose2(); }
|
||||
inline static Pose2 Identity() { return Pose2(); }
|
||||
|
||||
/// inverse
|
||||
GTSAM_EXPORT Pose2 inverse() const;
|
||||
|
|
|
|||
|
|
@ -103,7 +103,7 @@ public:
|
|||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
static Pose3 identity() {
|
||||
static Pose3 Identity() {
|
||||
return Pose3();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -107,8 +107,8 @@ namespace gtsam {
|
|||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/** identity */
|
||||
inline static Rot2 identity() { return Rot2(); }
|
||||
/** Identity */
|
||||
inline static Rot2 Identity() { return Rot2(); }
|
||||
|
||||
/** The inverse rotation - negative angle */
|
||||
Rot2 inverse() const { return Rot2(c_, -s_);}
|
||||
|
|
|
|||
|
|
@ -297,7 +297,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
|
|||
/// @{
|
||||
|
||||
/// identity rotation for group operation
|
||||
inline static Rot3 identity() {
|
||||
inline static Rot3 Identity() {
|
||||
return Rot3();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -179,13 +179,13 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
|
|||
|
||||
/// SO<N> identity for N >= 2
|
||||
template <int N_ = N, typename = IsFixed<N_>>
|
||||
static SO identity() {
|
||||
static SO Identity() {
|
||||
return SO();
|
||||
}
|
||||
|
||||
/// SO<N> identity for N == Eigen::Dynamic
|
||||
template <int N_ = N, typename = IsDynamic<N_>>
|
||||
static SO identity(size_t n = 0) {
|
||||
static SO Identity(size_t n = 0) {
|
||||
return SO(n);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -134,7 +134,7 @@ void Similarity2::print(const std::string& s) const {
|
|||
<< std::endl;
|
||||
}
|
||||
|
||||
Similarity2 Similarity2::identity() { return Similarity2(); }
|
||||
Similarity2 Similarity2::Identity() { return Similarity2(); }
|
||||
|
||||
Similarity2 Similarity2::operator*(const Similarity2& S) const {
|
||||
return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
|
||||
|
|
|
|||
|
|
@ -83,7 +83,7 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
|
|||
/// @{
|
||||
|
||||
/// Return an identity transform
|
||||
static Similarity2 identity();
|
||||
static Similarity2 Identity();
|
||||
|
||||
/// Composition
|
||||
Similarity2 operator*(const Similarity2& S) const;
|
||||
|
|
|
|||
|
|
@ -122,7 +122,7 @@ void Similarity3::print(const std::string& s) const {
|
|||
std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl;
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::identity() {
|
||||
Similarity3 Similarity3::Identity() {
|
||||
return Similarity3();
|
||||
}
|
||||
Similarity3 Similarity3::operator*(const Similarity3& S) const {
|
||||
|
|
|
|||
|
|
@ -84,7 +84,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
|
|||
/// @{
|
||||
|
||||
/// Return an identity transform
|
||||
static Similarity3 identity();
|
||||
static Similarity3 Identity();
|
||||
|
||||
/// Composition
|
||||
Similarity3 operator*(const Similarity3& S) const;
|
||||
|
|
|
|||
|
|
@ -87,7 +87,7 @@ class GTSAM_EXPORT SphericalCamera {
|
|||
|
||||
/// Default constructor
|
||||
SphericalCamera()
|
||||
: pose_(Pose3::identity()), emptyCal_(boost::make_shared<EmptyCal>()) {}
|
||||
: pose_(Pose3()), emptyCal_(boost::make_shared<EmptyCal>()) {}
|
||||
|
||||
/// Constructor with pose
|
||||
explicit SphericalCamera(const Pose3& pose)
|
||||
|
|
@ -197,9 +197,9 @@ class GTSAM_EXPORT SphericalCamera {
|
|||
}
|
||||
|
||||
/// for Canonical
|
||||
static SphericalCamera identity() {
|
||||
static SphericalCamera Identity() {
|
||||
return SphericalCamera(
|
||||
Pose3::identity()); // assumes that the default constructor is valid
|
||||
Pose3::Identity()); // assumes that the default constructor is valid
|
||||
}
|
||||
|
||||
/// for Linear Triangulation
|
||||
|
|
|
|||
|
|
@ -71,7 +71,7 @@ public:
|
|||
/// @{
|
||||
|
||||
/// identity
|
||||
inline static StereoPoint2 identity() {
|
||||
inline static StereoPoint2 Identity() {
|
||||
return StereoPoint2();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@ class Point2 {
|
|||
bool equals(const gtsam::Point2& point, double tol) const;
|
||||
|
||||
// Group
|
||||
static gtsam::Point2 identity();
|
||||
static gtsam::Point2 Identity();
|
||||
|
||||
// Standard Interface
|
||||
double x() const;
|
||||
|
|
@ -73,7 +73,7 @@ class StereoPoint2 {
|
|||
bool equals(const gtsam::StereoPoint2& point, double tol) const;
|
||||
|
||||
// Group
|
||||
static gtsam::StereoPoint2 identity();
|
||||
static gtsam::StereoPoint2 Identity();
|
||||
gtsam::StereoPoint2 inverse() const;
|
||||
gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const;
|
||||
gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const;
|
||||
|
|
@ -115,7 +115,7 @@ class Point3 {
|
|||
bool equals(const gtsam::Point3& p, double tol) const;
|
||||
|
||||
// Group
|
||||
static gtsam::Point3 identity();
|
||||
static gtsam::Point3 Identity();
|
||||
|
||||
// Standard Interface
|
||||
Vector vector() const;
|
||||
|
|
@ -149,7 +149,7 @@ class Rot2 {
|
|||
bool equals(const gtsam::Rot2& rot, double tol) const;
|
||||
|
||||
// Group
|
||||
static gtsam::Rot2 identity();
|
||||
static gtsam::Rot2 Identity();
|
||||
gtsam::Rot2 inverse();
|
||||
gtsam::Rot2 compose(const gtsam::Rot2& p2) const;
|
||||
gtsam::Rot2 between(const gtsam::Rot2& p2) const;
|
||||
|
|
@ -198,7 +198,7 @@ class SO3 {
|
|||
bool equals(const gtsam::SO3& other, double tol) const;
|
||||
|
||||
// Group
|
||||
static gtsam::SO3 identity();
|
||||
static gtsam::SO3 Identity();
|
||||
gtsam::SO3 inverse() const;
|
||||
gtsam::SO3 between(const gtsam::SO3& R) const;
|
||||
gtsam::SO3 compose(const gtsam::SO3& R) const;
|
||||
|
|
@ -228,7 +228,7 @@ class SO4 {
|
|||
bool equals(const gtsam::SO4& other, double tol) const;
|
||||
|
||||
// Group
|
||||
static gtsam::SO4 identity();
|
||||
static gtsam::SO4 Identity();
|
||||
gtsam::SO4 inverse() const;
|
||||
gtsam::SO4 between(const gtsam::SO4& Q) const;
|
||||
gtsam::SO4 compose(const gtsam::SO4& Q) const;
|
||||
|
|
@ -258,7 +258,7 @@ class SOn {
|
|||
bool equals(const gtsam::SOn& other, double tol) const;
|
||||
|
||||
// Group
|
||||
static gtsam::SOn identity();
|
||||
static gtsam::SOn Identity();
|
||||
gtsam::SOn inverse() const;
|
||||
gtsam::SOn between(const gtsam::SOn& Q) const;
|
||||
gtsam::SOn compose(const gtsam::SOn& Q) const;
|
||||
|
|
@ -322,7 +322,7 @@ class Rot3 {
|
|||
bool equals(const gtsam::Rot3& rot, double tol) const;
|
||||
|
||||
// Group
|
||||
static gtsam::Rot3 identity();
|
||||
static gtsam::Rot3 Identity();
|
||||
gtsam::Rot3 inverse() const;
|
||||
gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
|
||||
gtsam::Rot3 between(const gtsam::Rot3& p2) const;
|
||||
|
|
@ -380,7 +380,7 @@ class Pose2 {
|
|||
bool equals(const gtsam::Pose2& pose, double tol) const;
|
||||
|
||||
// Group
|
||||
static gtsam::Pose2 identity();
|
||||
static gtsam::Pose2 Identity();
|
||||
gtsam::Pose2 inverse() const;
|
||||
gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
|
||||
gtsam::Pose2 between(const gtsam::Pose2& p2) const;
|
||||
|
|
@ -444,7 +444,7 @@ class Pose3 {
|
|||
bool equals(const gtsam::Pose3& pose, double tol) const;
|
||||
|
||||
// Group
|
||||
static gtsam::Pose3 identity();
|
||||
static gtsam::Pose3 Identity();
|
||||
gtsam::Pose3 inverse() const;
|
||||
gtsam::Pose3 inverse(Eigen::Ref<Eigen::MatrixXd> H) const;
|
||||
gtsam::Pose3 compose(const gtsam::Pose3& pose) const;
|
||||
|
|
@ -1126,10 +1126,10 @@ class TriangulationResult {
|
|||
Status status;
|
||||
TriangulationResult(const gtsam::Point3& p);
|
||||
const gtsam::Point3& get() const;
|
||||
static TriangulationResult Degenerate();
|
||||
static TriangulationResult Outlier();
|
||||
static TriangulationResult FarPoint();
|
||||
static TriangulationResult BehindCamera();
|
||||
static gtsam::TriangulationResult Degenerate();
|
||||
static gtsam::TriangulationResult Outlier();
|
||||
static gtsam::TriangulationResult FarPoint();
|
||||
static gtsam::TriangulationResult BehindCamera();
|
||||
bool valid() const;
|
||||
bool degenerate() const;
|
||||
bool outlier() const;
|
||||
|
|
|
|||
|
|
@ -902,7 +902,7 @@ TEST(Pose2 , TransformCovariance3) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, Print) {
|
||||
Pose2 pose(Rot2::identity(), Point2(1, 2));
|
||||
Pose2 pose(Rot2::Identity(), Point2(1, 2));
|
||||
|
||||
// Generate the expected output
|
||||
string s = "Planar Pose";
|
||||
|
|
|
|||
|
|
@ -1133,7 +1133,7 @@ Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { retu
|
|||
|
||||
TEST(Pose3, interpolateJacobians) {
|
||||
{
|
||||
Pose3 X = Pose3::identity();
|
||||
Pose3 X = Pose3::Identity();
|
||||
Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
|
||||
double t = 0.5;
|
||||
Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation
|
||||
|
|
@ -1147,10 +1147,10 @@ TEST(Pose3, interpolateJacobians) {
|
|||
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
|
||||
}
|
||||
{
|
||||
Pose3 X = Pose3::identity();
|
||||
Pose3 Y(Rot3::identity(), Point3(1, 0, 0));
|
||||
Pose3 X = Pose3::Identity();
|
||||
Pose3 Y(Rot3::Identity(), Point3(1, 0, 0));
|
||||
double t = 0.3;
|
||||
Pose3 expectedPoseInterp(Rot3::identity(), Point3(0.3, 0, 0));
|
||||
Pose3 expectedPoseInterp(Rot3::Identity(), Point3(0.3, 0, 0));
|
||||
Matrix actualJacobianX, actualJacobianY;
|
||||
EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
|
||||
|
||||
|
|
@ -1161,7 +1161,7 @@ TEST(Pose3, interpolateJacobians) {
|
|||
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
|
||||
}
|
||||
{
|
||||
Pose3 X = Pose3::identity();
|
||||
Pose3 X = Pose3::Identity();
|
||||
Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0));
|
||||
double t = 0.5;
|
||||
Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
|
||||
|
|
@ -1204,7 +1204,7 @@ TEST(Pose3, Create) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, Print) {
|
||||
Pose3 pose(Rot3::identity(), Point3(1, 2, 3));
|
||||
Pose3 pose(Rot3::Identity(), Point3(1, 2, 3));
|
||||
|
||||
// Generate the expected output
|
||||
std::string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 1 2 3\n";
|
||||
|
|
|
|||
|
|
@ -203,11 +203,11 @@ TEST(Similarity3, ExpLogMap) {
|
|||
|
||||
Vector7 zeros;
|
||||
zeros << 0, 0, 0, 0, 0, 0, 0;
|
||||
Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity());
|
||||
Vector7 logIdentity = Similarity3::Logmap(Similarity3::Identity());
|
||||
EXPECT(assert_equal(zeros, logIdentity));
|
||||
|
||||
Similarity3 expZero = Similarity3::Expmap(zeros);
|
||||
Similarity3 ident = Similarity3::identity();
|
||||
Similarity3 ident = Similarity3::Identity();
|
||||
EXPECT(assert_equal(expZero, ident));
|
||||
|
||||
// Compare to matrix exponential, using expm in Lie.h
|
||||
|
|
@ -391,7 +391,7 @@ TEST(Similarity3, Optimization) {
|
|||
NonlinearFactorGraph graph;
|
||||
graph.addPrior(key, prior, model);
|
||||
|
||||
// Create initial estimate with identity transform
|
||||
// Create initial estimate with Identity transform
|
||||
Values initial;
|
||||
initial.insert<Similarity3>(key, Similarity3());
|
||||
|
||||
|
|
|
|||
|
|
@ -66,27 +66,6 @@ class KeySet {
|
|||
void serialize() const;
|
||||
};
|
||||
|
||||
// Actually a vector<Key>
|
||||
class KeyVector {
|
||||
KeyVector();
|
||||
KeyVector(const gtsam::KeyVector& other);
|
||||
|
||||
// Note: no print function
|
||||
|
||||
// common STL methods
|
||||
size_t size() const;
|
||||
bool empty() const;
|
||||
void clear();
|
||||
|
||||
// structure specific methods
|
||||
size_t at(size_t i) const;
|
||||
size_t front() const;
|
||||
size_t back() const;
|
||||
void push_back(size_t key) const;
|
||||
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
// Actually a FastMap<Key,int>
|
||||
class KeyGroupMap {
|
||||
KeyGroupMap();
|
||||
|
|
|
|||
|
|
@ -93,9 +93,14 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
|
|||
//------------------------------------------------------------------------------
|
||||
void PreintegratedCombinedMeasurements::integrateMeasurement(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
|
||||
if (dt <= 0) {
|
||||
throw std::runtime_error(
|
||||
"PreintegratedCombinedMeasurements::integrateMeasurement: dt <=0");
|
||||
}
|
||||
|
||||
// Update preintegrated measurements.
|
||||
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
Matrix93 B, C;
|
||||
Matrix9 A; // Jacobian wrt preintegrated measurements without bias (df/dx)
|
||||
Matrix93 B, C; // Jacobian of state wrpt accel bias and omega bias respectively.
|
||||
PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
|
||||
|
||||
// Update preintegrated measurements covariance: as in [2] we consider a first
|
||||
|
|
@ -105,47 +110,78 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
|
|||
// and preintegrated measurements
|
||||
|
||||
// Single Jacobians to propagate covariance
|
||||
// TODO(frank): should we not also account for bias on position?
|
||||
Matrix3 theta_H_biasOmega = -C.topRows<3>();
|
||||
Matrix3 vel_H_biasAcc = -B.bottomRows<3>();
|
||||
Matrix3 theta_H_biasOmega = C.topRows<3>();
|
||||
Matrix3 pos_H_biasAcc = B.middleRows<3>(3);
|
||||
Matrix3 vel_H_biasAcc = B.bottomRows<3>();
|
||||
|
||||
Matrix3 theta_H_biasOmegaInit = -theta_H_biasOmega;
|
||||
Matrix3 pos_H_biasAccInit = -pos_H_biasAcc;
|
||||
Matrix3 vel_H_biasAccInit = -vel_H_biasAcc;
|
||||
|
||||
// overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
Eigen::Matrix<double, 15, 15> F;
|
||||
F.setZero();
|
||||
F.block<9, 9>(0, 0) = A;
|
||||
F.block<3, 3>(0, 12) = theta_H_biasOmega;
|
||||
F.block<3, 3>(3, 9) = pos_H_biasAcc;
|
||||
F.block<3, 3>(6, 9) = vel_H_biasAcc;
|
||||
F.block<6, 6>(9, 9) = I_6x6;
|
||||
|
||||
// Update the uncertainty on the state (matrix F in [4]).
|
||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose();
|
||||
|
||||
// propagate uncertainty
|
||||
// TODO(frank): use noiseModel routine so we can have arbitrary noise models.
|
||||
const Matrix3& aCov = p().accelerometerCovariance;
|
||||
const Matrix3& wCov = p().gyroscopeCovariance;
|
||||
const Matrix3& iCov = p().integrationCovariance;
|
||||
const Matrix6& bInitCov = p().biasAccOmegaInt;
|
||||
|
||||
// first order uncertainty propagation
|
||||
// Optimized matrix multiplication (1/dt) * G * measurementCovariance *
|
||||
// G.transpose()
|
||||
// Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose()
|
||||
Eigen::Matrix<double, 15, 15> G_measCov_Gt;
|
||||
G_measCov_Gt.setZero(15, 15);
|
||||
|
||||
const Matrix3& bInitCov11 = bInitCov.block<3, 3>(0, 0) / dt;
|
||||
const Matrix3& bInitCov12 = bInitCov.block<3, 3>(0, 3) / dt;
|
||||
const Matrix3& bInitCov21 = bInitCov.block<3, 3>(3, 0) / dt;
|
||||
const Matrix3& bInitCov22 = bInitCov.block<3, 3>(3, 3) / dt;
|
||||
|
||||
// BLOCK DIAGONAL TERMS
|
||||
D_t_t(&G_measCov_Gt) = dt * iCov;
|
||||
D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
|
||||
* (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0))
|
||||
* (vel_H_biasAcc.transpose());
|
||||
D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
|
||||
* (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
|
||||
* (theta_H_biasOmega.transpose());
|
||||
D_R_R(&G_measCov_Gt) =
|
||||
(theta_H_biasOmega * (wCov / dt) * theta_H_biasOmega.transpose()) //
|
||||
+
|
||||
(theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose());
|
||||
|
||||
D_t_t(&G_measCov_Gt) =
|
||||
(pos_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) //
|
||||
+ (pos_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose()) //
|
||||
+ (dt * iCov);
|
||||
|
||||
D_v_v(&G_measCov_Gt) =
|
||||
(vel_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) //
|
||||
+ (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
|
||||
|
||||
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
|
||||
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
|
||||
|
||||
// OFF BLOCK DIAGONAL TERMS
|
||||
Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
|
||||
* theta_H_biasOmega.transpose();
|
||||
D_v_R(&G_measCov_Gt) = temp;
|
||||
D_R_v(&G_measCov_Gt) = temp.transpose();
|
||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
|
||||
D_R_t(&G_measCov_Gt) =
|
||||
theta_H_biasOmegaInit * bInitCov21 * pos_H_biasAccInit.transpose();
|
||||
D_R_v(&G_measCov_Gt) =
|
||||
theta_H_biasOmegaInit * bInitCov21 * vel_H_biasAccInit.transpose();
|
||||
D_t_R(&G_measCov_Gt) =
|
||||
pos_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
|
||||
D_t_v(&G_measCov_Gt) =
|
||||
(pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) +
|
||||
(pos_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
|
||||
D_v_R(&G_measCov_Gt) =
|
||||
vel_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
|
||||
D_v_t(&G_measCov_Gt) =
|
||||
(vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) +
|
||||
(vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose());
|
||||
|
||||
preintMeasCov_.noalias() += G_measCov_Gt;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
@ -253,6 +289,5 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
|
|||
os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
|
||||
return os;
|
||||
}
|
||||
}
|
||||
/// namespace gtsam
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -51,6 +51,7 @@ typedef ManifoldPreintegration PreintegrationType;
|
|||
* TRO, 28(1):61-76, 2012.
|
||||
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
|
||||
* Computation of the Jacobian Matrices", Tech. Report, 2013.
|
||||
* Available in this repo as "PreintegratedIMUJacobians.pdf".
|
||||
* [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
|
||||
* Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
|
||||
* Robotics: Science and Systems (RSS), 2015.
|
||||
|
|
@ -61,7 +62,7 @@ typedef ManifoldPreintegration PreintegrationType;
|
|||
struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
|
||||
Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk
|
||||
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
|
||||
Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration
|
||||
Matrix6 biasAccOmegaInt; ///< covariance of bias used as initial estimate.
|
||||
|
||||
/// Default constructor makes uninitialized params struct.
|
||||
/// Used for serialization.
|
||||
|
|
@ -92,11 +93,11 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
|
|||
|
||||
void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
|
||||
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
|
||||
void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
|
||||
void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; }
|
||||
|
||||
const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
|
||||
const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
|
||||
const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
|
||||
const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
|
||||
|
||||
private:
|
||||
|
||||
|
|
|
|||
|
|
@ -109,7 +109,7 @@ public:
|
|||
/// @{
|
||||
|
||||
/** identity for group operation */
|
||||
static ConstantBias identity() {
|
||||
static ConstantBias Identity() {
|
||||
return ConstantBias();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -59,7 +59,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
|
|||
|
||||
// Update preintegrated measurements (also get Jacobian)
|
||||
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
Matrix93 B, C;
|
||||
Matrix93 B, C; // Jacobian of state wrpt accel bias and omega bias respectively.
|
||||
PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
|
||||
|
||||
// first order covariance propagation:
|
||||
|
|
@ -73,11 +73,13 @@ void PreintegratedImuMeasurements::integrateMeasurement(
|
|||
const Matrix3& iCov = p().integrationCovariance;
|
||||
|
||||
// (1/dt) allows to pass from continuous time noise to discrete time noise
|
||||
// Update the uncertainty on the state (matrix A in [4]).
|
||||
preintMeasCov_ = A * preintMeasCov_ * A.transpose();
|
||||
// These 2 updates account for uncertainty on the IMU measurement (matrix B in [4]).
|
||||
preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose();
|
||||
preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();
|
||||
|
||||
// NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3
|
||||
// NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 (9x3 matrix)
|
||||
preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -53,6 +53,7 @@ typedef ManifoldPreintegration PreintegrationType;
|
|||
* TRO, 28(1):61-76, 2012.
|
||||
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
|
||||
* Computation of the Jacobian Matrices", Tech. Report, 2013.
|
||||
* Available in this repo as "PreintegratedIMUJacobians.pdf".
|
||||
* [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
|
||||
* Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
|
||||
* Robotics: Science and Systems (RSS), 2015.
|
||||
|
|
|
|||
|
|
@ -15,8 +15,8 @@
|
|||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/ScenarioRunner.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/navigation/ScenarioRunner.h>
|
||||
|
||||
#include <boost/assign.hpp>
|
||||
#include <cmath>
|
||||
|
|
@ -105,4 +105,62 @@ Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
|
|||
return Q / (N - 1);
|
||||
}
|
||||
|
||||
PreintegratedCombinedMeasurements CombinedScenarioRunner::integrate(
|
||||
double T, const Bias& estimatedBias, bool corrupted) const {
|
||||
gttic_(integrate);
|
||||
PreintegratedCombinedMeasurements pim(p_, estimatedBias);
|
||||
|
||||
const double dt = imuSampleTime();
|
||||
const size_t nrSteps = T / dt;
|
||||
double t = 0;
|
||||
for (size_t k = 0; k < nrSteps; k++, t += dt) {
|
||||
Vector3 measuredOmega =
|
||||
corrupted ? measuredAngularVelocity(t) : actualAngularVelocity(t);
|
||||
Vector3 measuredAcc =
|
||||
corrupted ? measuredSpecificForce(t) : actualSpecificForce(t);
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
|
||||
}
|
||||
|
||||
return pim;
|
||||
}
|
||||
|
||||
NavState CombinedScenarioRunner::predict(
|
||||
const PreintegratedCombinedMeasurements& pim,
|
||||
const Bias& estimatedBias) const {
|
||||
const NavState state_i(scenario().pose(0), scenario().velocity_n(0));
|
||||
return pim.predict(state_i, estimatedBias);
|
||||
}
|
||||
|
||||
Eigen::Matrix<double, 15, 15> CombinedScenarioRunner::estimateCovariance(
|
||||
double T, size_t N, const Bias& estimatedBias) const {
|
||||
gttic_(estimateCovariance);
|
||||
|
||||
// Get predict prediction from ground truth measurements
|
||||
NavState prediction = predict(integrate(T));
|
||||
|
||||
// Sample !
|
||||
Matrix samples(15, N);
|
||||
Vector15 sum = Vector15::Zero();
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
auto pim = integrate(T, estimatedBias, true);
|
||||
NavState sampled = predict(pim);
|
||||
Vector15 xi = Vector15::Zero();
|
||||
xi << sampled.localCoordinates(prediction),
|
||||
(estimatedBias_.vector() - estimatedBias.vector());
|
||||
samples.col(i) = xi;
|
||||
sum += xi;
|
||||
}
|
||||
|
||||
// Compute MC covariance
|
||||
Vector15 sampleMean = sum / N;
|
||||
Eigen::Matrix<double, 15, 15> Q;
|
||||
Q.setZero();
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
Vector15 xi = samples.col(i) - sampleMean;
|
||||
Q += xi * xi.transpose();
|
||||
}
|
||||
|
||||
return Q / (N - 1);
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -16,9 +16,10 @@
|
|||
*/
|
||||
|
||||
#pragma once
|
||||
#include <gtsam/linear/Sampler.h>
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/navigation/Scenario.h>
|
||||
#include <gtsam/linear/Sampler.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -66,10 +67,10 @@ class GTSAM_EXPORT ScenarioRunner {
|
|||
// also, uses g=10 for easy debugging
|
||||
const Vector3& gravity_n() const { return p_->n_gravity; }
|
||||
|
||||
const Scenario& scenario() const { return scenario_; }
|
||||
|
||||
// A gyro simply measures angular velocity in body frame
|
||||
Vector3 actualAngularVelocity(double t) const {
|
||||
return scenario_.omega_b(t);
|
||||
}
|
||||
Vector3 actualAngularVelocity(double t) const { return scenario_.omega_b(t); }
|
||||
|
||||
// An accelerometer measures acceleration in body, but not gravity
|
||||
Vector3 actualSpecificForce(double t) const {
|
||||
|
|
@ -106,4 +107,39 @@ class GTSAM_EXPORT ScenarioRunner {
|
|||
Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
|
||||
};
|
||||
|
||||
/*
|
||||
* Simple class to test navigation scenarios with CombinedImuMeasurements.
|
||||
* Takes a trajectory scenario as input, and can generate IMU measurements
|
||||
*/
|
||||
class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
|
||||
public:
|
||||
typedef boost::shared_ptr<PreintegrationCombinedParams> SharedParams;
|
||||
|
||||
private:
|
||||
const SharedParams p_;
|
||||
const Bias estimatedBias_;
|
||||
|
||||
public:
|
||||
CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
|
||||
double imuSampleTime = 1.0 / 100.0,
|
||||
const Bias& bias = Bias())
|
||||
: ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
|
||||
imuSampleTime, bias),
|
||||
p_(p),
|
||||
estimatedBias_(bias) {}
|
||||
|
||||
/// Integrate measurements for T seconds into a PIM
|
||||
PreintegratedCombinedMeasurements integrate(
|
||||
double T, const Bias& estimatedBias = Bias(),
|
||||
bool corrupted = false) const;
|
||||
|
||||
/// Predict predict given a PIM
|
||||
NavState predict(const PreintegratedCombinedMeasurements& pim,
|
||||
const Bias& estimatedBias = Bias()) const;
|
||||
|
||||
/// Compute a Monte Carlo estimate of the predict covariance using N samples
|
||||
Eigen::Matrix<double, 15, 15> estimateCovariance(
|
||||
double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const;
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -17,7 +17,7 @@ class ConstantBias {
|
|||
bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const;
|
||||
|
||||
// Group
|
||||
static gtsam::imuBias::ConstantBias identity();
|
||||
static gtsam::imuBias::ConstantBias Identity();
|
||||
|
||||
// Operator Overloads
|
||||
gtsam::imuBias::ConstantBias operator-() const;
|
||||
|
|
@ -165,11 +165,11 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
|
|||
|
||||
void setBiasAccCovariance(Matrix cov);
|
||||
void setBiasOmegaCovariance(Matrix cov);
|
||||
void setBiasAccOmegaInt(Matrix cov);
|
||||
void setBiasAccOmegaInit(Matrix cov);
|
||||
|
||||
Matrix getBiasAccCovariance() const ;
|
||||
Matrix getBiasOmegaCovariance() const ;
|
||||
Matrix getBiasAccOmegaInt() const;
|
||||
Matrix getBiasAccOmegaInit() const;
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -16,18 +16,19 @@
|
|||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
* @author Stephen Williams
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/navigation/ScenarioRunner.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
#include <list>
|
||||
|
||||
|
|
@ -40,9 +41,12 @@ static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
|
|||
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
|
||||
p->integrationCovariance = 0.0001 * I_3x3;
|
||||
p->biasAccCovariance = Z_3x3;
|
||||
p->biasOmegaCovariance = Z_3x3;
|
||||
p->biasAccOmegaInt = Z_6x6;
|
||||
return p;
|
||||
}
|
||||
}
|
||||
} // namespace testing
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(CombinedImuFactor, PreintegratedMeasurements ) {
|
||||
|
|
@ -71,6 +75,7 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
|
|||
DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(CombinedImuFactor, ErrorWithBiases ) {
|
||||
Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
|
||||
|
|
@ -203,6 +208,114 @@ TEST(CombinedImuFactor, PredictRotation) {
|
|||
EXPECT(assert_equal(expectedPose, actual.pose(), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Testing covariance to check if all the jacobians are accounted for.
|
||||
TEST(CombinedImuFactor, CheckCovariance) {
|
||||
auto params = PreintegrationCombinedParams::MakeSharedU(9.81);
|
||||
|
||||
params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
|
||||
params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
|
||||
params->setIntegrationCovariance(pow(0.0, 2) * I_3x3);
|
||||
params->setOmegaCoriolis(Vector3::Zero());
|
||||
|
||||
imuBias::ConstantBias currentBias;
|
||||
|
||||
PreintegratedCombinedMeasurements actual(params, currentBias);
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredAcc(0.1577, -0.8251, 9.6111);
|
||||
Vector3 measuredOmega(-0.0210, 0.0311, 0.0145);
|
||||
double deltaT = 0.01;
|
||||
|
||||
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
Eigen::Matrix<double, 15, 15> expected;
|
||||
expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01;
|
||||
|
||||
// regression
|
||||
EXPECT(assert_equal(expected, actual.preintMeasCov()));
|
||||
}
|
||||
|
||||
// Test that the covariance values for the ImuFactor and the CombinedImuFactor
|
||||
// (top-left 9x9) are the same
|
||||
TEST(CombinedImuFactor, SameCovariance) {
|
||||
// IMU measurements and time delta
|
||||
Vector3 accMeas(0.1577, -0.8251, 9.6111);
|
||||
Vector3 omegaMeas(-0.0210, 0.0311, 0.0145);
|
||||
double deltaT = 0.01;
|
||||
|
||||
// Assume zero bias
|
||||
imuBias::ConstantBias currentBias;
|
||||
|
||||
// Define params for ImuFactor
|
||||
auto params = PreintegrationParams::MakeSharedU();
|
||||
params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
|
||||
params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
|
||||
params->setIntegrationCovariance(pow(0, 2) * I_3x3);
|
||||
params->setOmegaCoriolis(Vector3::Zero());
|
||||
|
||||
// The IMU preintegration object for ImuFactor
|
||||
PreintegratedImuMeasurements pim(params, currentBias);
|
||||
pim.integrateMeasurement(accMeas, omegaMeas, deltaT);
|
||||
|
||||
// Define params for CombinedImuFactor
|
||||
auto combined_params = PreintegrationCombinedParams::MakeSharedU();
|
||||
combined_params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
|
||||
combined_params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
|
||||
// Set bias integration covariance explicitly to zero
|
||||
combined_params->setIntegrationCovariance(Z_3x3);
|
||||
combined_params->setOmegaCoriolis(Z_3x1);
|
||||
// Set bias initial covariance explicitly to zero
|
||||
combined_params->setBiasAccOmegaInit(Z_6x6);
|
||||
|
||||
// The IMU preintegration object for CombinedImuFactor
|
||||
PreintegratedCombinedMeasurements cpim(combined_params, currentBias);
|
||||
cpim.integrateMeasurement(accMeas, omegaMeas, deltaT);
|
||||
|
||||
// Assert if the noise covariance
|
||||
EXPECT(assert_equal(pim.preintMeasCov(),
|
||||
cpim.preintMeasCov().block(0, 0, 9, 9)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(CombinedImuFactor, Accelerating) {
|
||||
const double a = 0.2, v = 50;
|
||||
|
||||
// Set up body pointing towards y axis, and start at 10,20,0 with velocity
|
||||
// going in X The body itself has Z axis pointing down
|
||||
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
const Point3 initial_position(10, 20, 0);
|
||||
const Vector3 initial_velocity(v, 0, 0);
|
||||
|
||||
const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
|
||||
Vector3(a, 0, 0));
|
||||
|
||||
const double T = 3.0; // seconds
|
||||
|
||||
CombinedScenarioRunner runner(scenario, testing::Params(), T / 10);
|
||||
|
||||
PreintegratedCombinedMeasurements pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
||||
auto estimatedCov = runner.estimateCovariance(T, 100);
|
||||
Eigen::Matrix<double, 15, 15> expected = pim.preintMeasCov();
|
||||
EXPECT(assert_equal(estimatedCov, expected, 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -19,8 +19,6 @@
|
|||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using JacobianVector = std::vector<Matrix>;
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@ static double Chi2inv(const double alpha, const size_t dofs) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class GncParameters>
|
||||
class GTSAM_EXPORT GncOptimizer {
|
||||
class GncOptimizer {
|
||||
public:
|
||||
/// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
|
||||
typedef typename GncParameters::OptimizerType BaseOptimizer;
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ enum GncLossType {
|
|||
};
|
||||
|
||||
template<class BaseOptimizerParameters>
|
||||
class GTSAM_EXPORT GncParams {
|
||||
class GncParams {
|
||||
public:
|
||||
/// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
|
||||
typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;
|
||||
|
|
@ -72,8 +72,14 @@ class GTSAM_EXPORT GncParams {
|
|||
double relativeCostTol = 1e-5; ///< If relative cost change is below this threshold, stop iterating
|
||||
double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
|
||||
Verbosity verbosity = SILENT; ///< Verbosity level
|
||||
std::vector<size_t> knownInliers = std::vector<size_t>(); ///< Slots in the factor graph corresponding to measurements that we know are inliers
|
||||
std::vector<size_t> knownOutliers = std::vector<size_t>(); ///< Slots in the factor graph corresponding to measurements that we know are outliers
|
||||
|
||||
//TODO(Varun) replace IndexVector with vector<size_t> once pybind11/stl.h is globally enabled.
|
||||
/// Use IndexVector for inliers and outliers since it is fast + wrapping
|
||||
using IndexVector = FastVector<uint64_t>;
|
||||
///< Slots in the factor graph corresponding to measurements that we know are inliers
|
||||
IndexVector knownInliers = IndexVector();
|
||||
///< Slots in the factor graph corresponding to measurements that we know are outliers
|
||||
IndexVector knownOutliers = IndexVector();
|
||||
|
||||
/// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
|
||||
void setLossType(const GncLossType type) {
|
||||
|
|
@ -114,7 +120,7 @@ class GTSAM_EXPORT GncParams {
|
|||
* This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and
|
||||
* only apply GNC to prune outliers from the loop closures.
|
||||
* */
|
||||
void setKnownInliers(const std::vector<size_t>& knownIn) {
|
||||
void setKnownInliers(const IndexVector& knownIn) {
|
||||
for (size_t i = 0; i < knownIn.size(); i++){
|
||||
knownInliers.push_back(knownIn[i]);
|
||||
}
|
||||
|
|
@ -125,7 +131,7 @@ class GTSAM_EXPORT GncParams {
|
|||
* corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg,
|
||||
* and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15].
|
||||
* */
|
||||
void setKnownOutliers(const std::vector<size_t>& knownOut) {
|
||||
void setKnownOutliers(const IndexVector& knownOut) {
|
||||
for (size_t i = 0; i < knownOut.size(); i++){
|
||||
knownOutliers.push_back(knownOut[i]);
|
||||
}
|
||||
|
|
@ -163,7 +169,7 @@ class GTSAM_EXPORT GncParams {
|
|||
std::cout << "knownInliers: " << knownInliers[i] << "\n";
|
||||
for (size_t i = 0; i < knownOutliers.size(); i++)
|
||||
std::cout << "knownOutliers: " << knownOutliers[i] << "\n";
|
||||
baseOptimizerParams.print(str);
|
||||
baseOptimizerParams.print("Base optimizer params: ");
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -167,9 +167,10 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
|
|||
return GaussianFactor::shared_ptr(
|
||||
new JacobianFactor(terms, b,
|
||||
boost::static_pointer_cast<Constrained>(noiseModel_)->unit()));
|
||||
else
|
||||
else {
|
||||
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,38 @@
|
|||
//*************************************************************************
|
||||
// Custom Factor wrapping
|
||||
//*************************************************************************
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
#include <gtsam/nonlinear/CustomFactor.h>
|
||||
virtual class CustomFactor : gtsam::NoiseModelFactor {
|
||||
/*
|
||||
* Note CustomFactor will not be wrapped for MATLAB, as there is no supporting
|
||||
* machinery there. This is achieved by adding `gtsam::CustomFactor` to the
|
||||
* ignore list in `matlab/CMakeLists.txt`.
|
||||
*/
|
||||
CustomFactor();
|
||||
/*
|
||||
* Example:
|
||||
* ```
|
||||
* def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
|
||||
* <calculated error>
|
||||
* if not H is None:
|
||||
* <calculate the Jacobian>
|
||||
* H[0] = J1 # 2-d numpy array for a Jacobian block
|
||||
* H[1] = J2
|
||||
* ...
|
||||
* return error # 1-d numpy array
|
||||
*
|
||||
* cf = CustomFactor(noise_model, keys, error_func)
|
||||
* ```
|
||||
*/
|
||||
CustomFactor(const gtsam::SharedNoiseModel& noiseModel,
|
||||
const gtsam::KeyVector& keys,
|
||||
const gtsam::CustomErrorFunction& errorFunction);
|
||||
|
||||
void print(string s = "",
|
||||
gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -99,11 +99,11 @@ class NonlinearFactorGraph {
|
|||
string dot(
|
||||
const gtsam::Values& values,
|
||||
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
|
||||
const GraphvizFormatting& writer = GraphvizFormatting());
|
||||
const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting());
|
||||
void saveGraph(
|
||||
const string& s, const gtsam::Values& values,
|
||||
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
|
||||
const GraphvizFormatting& writer = GraphvizFormatting()) const;
|
||||
const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting()) const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
|
|
@ -135,37 +135,6 @@ virtual class NoiseModelFactor : gtsam::NonlinearFactor {
|
|||
Vector whitenedError(const gtsam::Values& x) const;
|
||||
};
|
||||
|
||||
#include <gtsam/nonlinear/CustomFactor.h>
|
||||
virtual class CustomFactor : gtsam::NoiseModelFactor {
|
||||
/*
|
||||
* Note CustomFactor will not be wrapped for MATLAB, as there is no supporting
|
||||
* machinery there. This is achieved by adding `gtsam::CustomFactor` to the
|
||||
* ignore list in `matlab/CMakeLists.txt`.
|
||||
*/
|
||||
CustomFactor();
|
||||
/*
|
||||
* Example:
|
||||
* ```
|
||||
* def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
|
||||
* <calculated error>
|
||||
* if not H is None:
|
||||
* <calculate the Jacobian>
|
||||
* H[0] = J1 # 2-d numpy array for a Jacobian block
|
||||
* H[1] = J2
|
||||
* ...
|
||||
* return error # 1-d numpy array
|
||||
*
|
||||
* cf = CustomFactor(noise_model, keys, error_func)
|
||||
* ```
|
||||
*/
|
||||
CustomFactor(const gtsam::SharedNoiseModel& noiseModel,
|
||||
const gtsam::KeyVector& keys,
|
||||
const gtsam::CustomErrorFunction& errorFunction);
|
||||
|
||||
void print(string s = "",
|
||||
gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
|
||||
};
|
||||
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
class Values {
|
||||
Values();
|
||||
|
|
@ -544,12 +513,34 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams {
|
|||
};
|
||||
|
||||
#include <gtsam/nonlinear/GncParams.h>
|
||||
enum GncLossType {
|
||||
GM /*Geman McClure*/,
|
||||
TLS /*Truncated least squares*/
|
||||
};
|
||||
|
||||
template<PARAMS>
|
||||
virtual class GncParams {
|
||||
GncParams(const PARAMS& baseOptimizerParams);
|
||||
GncParams();
|
||||
void setVerbosityGNC(const This::Verbosity value);
|
||||
void print(const string& str) const;
|
||||
BaseOptimizerParameters baseOptimizerParams;
|
||||
gtsam::GncLossType lossType;
|
||||
size_t maxIterations;
|
||||
double muStep;
|
||||
double relativeCostTol;
|
||||
double weightsTol;
|
||||
Verbosity verbosity;
|
||||
gtsam::KeyVector knownInliers;
|
||||
gtsam::KeyVector knownOutliers;
|
||||
|
||||
void setLossType(const gtsam::GncLossType type);
|
||||
void setMaxIterations(const size_t maxIter);
|
||||
void setMuStep(const double step);
|
||||
void setRelativeCostTol(double value);
|
||||
void setWeightsTol(double value);
|
||||
void setVerbosityGNC(const gtsam::This::Verbosity value);
|
||||
void setKnownInliers(const gtsam::KeyVector& knownIn);
|
||||
void setKnownOutliers(const gtsam::KeyVector& knownOut);
|
||||
void print(const string& str = "GncParams: ") const;
|
||||
|
||||
enum Verbosity {
|
||||
SILENT,
|
||||
|
|
@ -597,6 +588,11 @@ virtual class GncOptimizer {
|
|||
GncOptimizer(const gtsam::NonlinearFactorGraph& graph,
|
||||
const gtsam::Values& initialValues,
|
||||
const PARAMS& params);
|
||||
void setInlierCostThresholds(const double inth);
|
||||
const Vector& getInlierCostThresholds();
|
||||
void setInlierCostThresholdsAtProbability(const double alpha);
|
||||
void setWeights(const Vector w);
|
||||
const Vector& getWeights();
|
||||
gtsam::Values optimize();
|
||||
};
|
||||
|
||||
|
|
@ -705,7 +701,7 @@ class ISAM2Result {
|
|||
/** Getters and Setters for all properties */
|
||||
size_t getVariablesRelinearized() const;
|
||||
size_t getVariablesReeliminated() const;
|
||||
FactorIndices getNewFactorsIndices() const;
|
||||
gtsam::FactorIndices getNewFactorsIndices() const;
|
||||
size_t getCliques() const;
|
||||
double getErrorBefore() const;
|
||||
double getErrorAfter() const;
|
||||
|
|
@ -873,7 +869,7 @@ template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
|
|||
gtsam::PinholeCamera<gtsam::Cal3Unified>,
|
||||
gtsam::imuBias::ConstantBias}>
|
||||
virtual class NonlinearEquality2 : gtsam::NoiseModelFactor {
|
||||
NonlinearEquality2(Key key1, Key key2, double mu = 1e4);
|
||||
NonlinearEquality2(gtsam::Key key1, gtsam::Key key2, double mu = 1e4);
|
||||
gtsam::Vector evaluateError(const T& x1, const T& x2);
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -122,7 +122,7 @@ class Class : public Point3 {
|
|||
enum {dimension = 3};
|
||||
using Point3::Point3;
|
||||
const Vector3& vector() const { return *this; }
|
||||
inline static Class identity() { return Class(0,0,0); }
|
||||
inline static Class Identity() { return Class(0,0,0); }
|
||||
double norm(OptionalJacobian<1,3> H = boost::none) const {
|
||||
return norm3(*this, H);
|
||||
}
|
||||
|
|
@ -285,7 +285,7 @@ TEST(Expression, compose2) {
|
|||
// Test compose with one arguments referring to constant rotation.
|
||||
TEST(Expression, compose3) {
|
||||
// Create expression
|
||||
Rot3_ R1(Rot3::identity()), R2(3);
|
||||
Rot3_ R1(Rot3::Identity()), R2(3);
|
||||
Rot3_ R3 = R1 * R2;
|
||||
|
||||
// Check keys
|
||||
|
|
|
|||
|
|
@ -217,7 +217,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) {
|
|||
|
||||
// Setup prior factors
|
||||
// Note: If x0 is too far away from the origin (e.g. x=100) this test can fail.
|
||||
Pose3 x0(Rot3::identity(), Vector3(10, -1, 1));
|
||||
Pose3 x0(Rot3::Identity(), Vector3(10, -1, 1));
|
||||
auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
|
||||
graph.addPrior<Pose3>(X(0), x0, x0_noise);
|
||||
|
||||
|
|
@ -241,7 +241,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) {
|
|||
// Initial values
|
||||
// Just offset the initial pose by 1m. This is what we are trying to optimize.
|
||||
Values initialEstimate;
|
||||
Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1,0,0)));
|
||||
Pose3 x0_initial = x0.compose(Pose3(Rot3::Identity(), Vector3(1,0,0)));
|
||||
initialEstimate.insert(P(1), p1);
|
||||
initialEstimate.insert(P(2), p2);
|
||||
initialEstimate.insert(X(0), x0_initial);
|
||||
|
|
|
|||
|
|
@ -69,7 +69,7 @@ SmartProjectionParams params(
|
|||
TEST(SmartProjectionRigFactor, Constructor) {
|
||||
using namespace vanillaRig;
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK));
|
||||
SmartRigFactor::shared_ptr factor1(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
}
|
||||
|
|
@ -89,7 +89,7 @@ TEST(SmartProjectionRigFactor, Constructor2) {
|
|||
TEST(SmartProjectionRigFactor, Constructor3) {
|
||||
using namespace vanillaRig;
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK));
|
||||
SmartRigFactor::shared_ptr factor1(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
factor1->add(measurement1, x1, cameraId1);
|
||||
|
|
@ -99,7 +99,7 @@ TEST(SmartProjectionRigFactor, Constructor3) {
|
|||
TEST(SmartProjectionRigFactor, Constructor4) {
|
||||
using namespace vanillaRig;
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK));
|
||||
SmartProjectionParams params2(
|
||||
gtsam::HESSIAN,
|
||||
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
|
||||
|
|
@ -112,7 +112,7 @@ TEST(SmartProjectionRigFactor, Constructor4) {
|
|||
TEST(SmartProjectionRigFactor, Equals) {
|
||||
using namespace vanillaRig;
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK));
|
||||
|
||||
SmartRigFactor::shared_ptr factor1(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
|
|
@ -140,7 +140,7 @@ TEST(SmartProjectionRigFactor, noiseless) {
|
|||
Point2 level_uv_right = level_camera_right.project(landmark1);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK));
|
||||
|
||||
SmartRigFactor factor(model, cameraRig, params);
|
||||
factor.add(level_uv, x1); // both taken from the same camera
|
||||
|
|
@ -197,7 +197,7 @@ TEST(SmartProjectionRigFactor, noisy) {
|
|||
using namespace vanillaRig;
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK));
|
||||
|
||||
// Project two landmarks into two cameras
|
||||
Point2 pixelError(0.2, 0.2);
|
||||
|
|
@ -323,7 +323,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
|
|||
Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));
|
||||
Pose3 body_T_sensor2 =
|
||||
Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1));
|
||||
Pose3 body_T_sensor3 = Pose3::identity();
|
||||
Pose3 body_T_sensor3 = Pose3();
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(body_T_sensor1, sharedK));
|
||||
|
|
@ -407,7 +407,7 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) {
|
|||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK2));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK2));
|
||||
|
||||
// Project three landmarks into three cameras
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||
|
|
@ -495,7 +495,7 @@ TEST(SmartProjectionRigFactor, Factors) {
|
|||
FastVector<size_t> cameraIds{0, 0};
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK));
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>(
|
||||
model, cameraRig, params);
|
||||
|
|
@ -578,7 +578,7 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) {
|
|||
|
||||
// create smart factor
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK));
|
||||
FastVector<size_t> cameraIds{0, 0, 0};
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
|
|
@ -646,7 +646,7 @@ TEST(SmartProjectionRigFactor, landmarkDistance) {
|
|||
params.setEnableEPI(false);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK));
|
||||
FastVector<size_t> cameraIds{0, 0, 0};
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
|
|
@ -717,7 +717,7 @@ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) {
|
|||
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK));
|
||||
FastVector<size_t> cameraIds{0, 0, 0};
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
|
|
@ -783,7 +783,7 @@ TEST(SmartProjectionRigFactor, CheckHessian) {
|
|||
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK));
|
||||
FastVector<size_t> cameraIds{0, 0, 0};
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
|
|
@ -859,7 +859,7 @@ TEST(SmartProjectionRigFactor, Hessian) {
|
|||
measurements_cam1.push_back(cam2_uv1);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK2));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK2));
|
||||
FastVector<size_t> cameraIds{0, 0};
|
||||
|
||||
SmartProjectionParams params(
|
||||
|
|
@ -889,7 +889,7 @@ TEST(SmartProjectionRigFactor, Hessian) {
|
|||
TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
|
||||
using namespace bundlerPose;
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedBundlerK));
|
||||
|
||||
SmartProjectionParams params;
|
||||
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
||||
|
|
@ -917,7 +917,7 @@ TEST(SmartProjectionRigFactor, Cal3Bundler) {
|
|||
KeyVector views{x1, x2, x3};
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedBundlerK));
|
||||
FastVector<size_t> cameraIds{0, 0, 0};
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
|
|
@ -988,7 +988,7 @@ TEST(SmartProjectionRigFactor,
|
|||
KeyVector keys{x1, x2, x3, x1};
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK));
|
||||
FastVector<size_t> cameraIds{0, 0, 0, 0};
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
|
|
@ -1116,7 +1116,7 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) {
|
|||
// create inputs
|
||||
KeyVector keys{x1, x2, x3};
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3(), sharedK));
|
||||
FastVector<size_t> cameraIds{0, 0, 0};
|
||||
FastVector<size_t> cameraIdsRedundant{0, 0, 0, 0};
|
||||
|
||||
|
|
@ -1195,11 +1195,11 @@ TEST(SmartProjectionRigFactor, timing) {
|
|||
// Default cameras for simple derivatives
|
||||
static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
|
||||
|
||||
Rot3 R = Rot3::identity();
|
||||
Rot3 R = Rot3::Identity();
|
||||
Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
|
||||
Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
|
||||
Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
|
||||
Pose3 body_P_sensorId = Pose3::identity();
|
||||
Pose3 body_P_sensorId = Pose3();
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));
|
||||
|
|
@ -1267,7 +1267,7 @@ TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
|
|||
keys.push_back(x3);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), emptyK));
|
||||
cameraRig->push_back(Camera(Pose3(), emptyK));
|
||||
|
||||
SmartProjectionParams params(
|
||||
gtsam::HESSIAN,
|
||||
|
|
@ -1330,10 +1330,10 @@ TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
|
|||
/* *************************************************************************/
|
||||
TEST(SmartProjectionFactorP, timing_sphericalCamera) {
|
||||
// create common data
|
||||
Rot3 R = Rot3::identity();
|
||||
Rot3 R = Rot3::Identity();
|
||||
Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
|
||||
Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
|
||||
Pose3 body_P_sensorId = Pose3::identity();
|
||||
Pose3 body_P_sensorId = Pose3();
|
||||
Point3 landmark1(0, 0, 10);
|
||||
|
||||
// create spherical data
|
||||
|
|
@ -1423,7 +1423,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
|
|||
|
||||
boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
|
||||
new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
|
||||
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), sharedK));
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
|
|
@ -1461,7 +1461,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
|
|||
|
||||
boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
|
||||
new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
|
||||
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
|
||||
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), canonicalK));
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
|
|
@ -1498,7 +1498,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
|
|||
|
||||
boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
|
||||
new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
|
||||
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
|
||||
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), canonicalK));
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
|
|
@ -1537,7 +1537,7 @@ TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) {
|
|||
|
||||
boost::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
|
||||
new CameraSet<SphericalCamera>()); // single camera in the rig
|
||||
cameraRig->push_back(SphericalCamera(Pose3::identity(), emptyK));
|
||||
cameraRig->push_back(SphericalCamera(Pose3(), emptyK));
|
||||
|
||||
// TRIANGULATION TEST WITH DEFAULT RANK TOL
|
||||
{ // rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a
|
||||
|
|
|
|||
|
|
@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2;
|
|||
const double dt = 1.0;
|
||||
|
||||
PoseRTV origin,
|
||||
pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)),
|
||||
pose1(Point3(0.5, 0.0, 0.0), Rot3(), Velocity3(1.0, 0.0, 0.0)),
|
||||
pose1a(Point3(0.5, 0.0, 0.0)),
|
||||
pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0));
|
||||
pose2(Point3(1.5, 0.0, 0.0), Rot3(), Velocity3(1.0, 0.0, 0.0));
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testVelocityConstraint, trapezoidal ) {
|
||||
|
|
|
|||
|
|
@ -53,7 +53,7 @@ int main(int argc, char** argv){
|
|||
normal_distribution<double> normalInliers(0.0, 0.05);
|
||||
|
||||
Values initial;
|
||||
initial.insert(0, Pose3::identity()); // identity pose as initialization
|
||||
initial.insert(0, Pose3()); // identity pose as initialization
|
||||
|
||||
// create ground truth pose
|
||||
Vector6 poseGtVector;
|
||||
|
|
|
|||
|
|
@ -129,8 +129,8 @@ int main(int argc, char* argv[]) {
|
|||
// Pose prior - at identity
|
||||
auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
|
||||
graph.addPrior(X(1), Pose3::identity(), priorPoseNoise);
|
||||
initialEstimate.insert(X(0), Pose3::identity());
|
||||
graph.addPrior(X(1), Pose3::Identity(), priorPoseNoise);
|
||||
initialEstimate.insert(X(0), Pose3::Identity());
|
||||
|
||||
// Bias prior
|
||||
graph.addPrior(B(1), imu.priorImuBias,
|
||||
|
|
@ -157,7 +157,7 @@ int main(int argc, char* argv[]) {
|
|||
if (frame != lastFrame || in.eof()) {
|
||||
cout << "Running iSAM for frame: " << lastFrame << "\n";
|
||||
|
||||
initialEstimate.insert(X(lastFrame), Pose3::identity());
|
||||
initialEstimate.insert(X(lastFrame), Pose3::Identity());
|
||||
initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0));
|
||||
initialEstimate.insert(B(lastFrame), imu.prevBias);
|
||||
|
||||
|
|
|
|||
|
|
@ -95,7 +95,7 @@ public:
|
|||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
static Pose3Upright identity() { return Pose3Upright(); }
|
||||
static Pose3Upright Identity() { return Pose3Upright(); }
|
||||
|
||||
/// inverse transformation with derivatives
|
||||
Pose3Upright inverse(boost::optional<Matrix&> H1=boost::none) const;
|
||||
|
|
|
|||
|
|
@ -130,7 +130,7 @@ class Pose3Upright {
|
|||
gtsam::Pose3Upright retract(Vector v) const;
|
||||
Vector localCoordinates(const gtsam::Pose3Upright& p2) const;
|
||||
|
||||
static gtsam::Pose3Upright identity();
|
||||
static gtsam::Pose3Upright Identity();
|
||||
gtsam::Pose3Upright inverse() const;
|
||||
gtsam::Pose3Upright compose(const gtsam::Pose3Upright& p2) const;
|
||||
gtsam::Pose3Upright between(const gtsam::Pose3Upright& p2) const;
|
||||
|
|
|
|||
|
|
@ -1,2 +1,2 @@
|
|||
set(ignore_test "testNestedDissection.cpp")
|
||||
gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis-gtsam")
|
||||
gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis-gtsam-if")
|
||||
|
|
|
|||
|
|
@ -44,8 +44,8 @@ TEST(LocalOrientedPlane3Factor, lm_translation_error) {
|
|||
|
||||
// Init pose and prior. Pose Prior is needed since a single plane measurement
|
||||
// does not fully constrain the pose
|
||||
Pose3 init_pose = Pose3::identity();
|
||||
Pose3 anchor_pose = Pose3::identity();
|
||||
Pose3 init_pose = Pose3::Identity();
|
||||
Pose3 anchor_pose = Pose3::Identity();
|
||||
graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001));
|
||||
graph.addPrior(X(1), anchor_pose, noiseModel::Isotropic::Sigma(6, 0.001));
|
||||
|
||||
|
|
@ -89,7 +89,7 @@ TEST (LocalOrientedPlane3Factor, lm_rotation_error) {
|
|||
|
||||
// Init pose and prior. Pose Prior is needed since a single plane measurement
|
||||
// does not fully constrain the pose
|
||||
Pose3 init_pose = Pose3::identity();
|
||||
Pose3 init_pose = Pose3::Identity();
|
||||
graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001));
|
||||
|
||||
// Add two landmark measurements, differing in angle
|
||||
|
|
@ -180,8 +180,8 @@ TEST(LocalOrientedPlane3Factor, Issue561Simplified) {
|
|||
NonlinearFactorGraph graph;
|
||||
|
||||
// Setup prior factors
|
||||
Pose3 x0(Rot3::identity(), Vector3(100, 30, 10)); // the "sensor pose"
|
||||
Pose3 x1(Rot3::identity(), Vector3(90, 40, 5) ); // the "anchor pose"
|
||||
Pose3 x0(Rot3::Identity(), Vector3(100, 30, 10)); // the "sensor pose"
|
||||
Pose3 x1(Rot3::Identity(), Vector3(90, 40, 5) ); // the "anchor pose"
|
||||
|
||||
auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
|
||||
auto x1_noise = noiseModel::Isotropic::Sigma(6, 0.01);
|
||||
|
|
@ -213,7 +213,7 @@ TEST(LocalOrientedPlane3Factor, Issue561Simplified) {
|
|||
// Initial values
|
||||
// Just offset the initial pose by 1m. This is what we are trying to optimize.
|
||||
Values initialEstimate;
|
||||
Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1, 0, 0)));
|
||||
Pose3 x0_initial = x0.compose(Pose3(Rot3::Identity(), Vector3(1, 0, 0)));
|
||||
initialEstimate.insert(P(1), p1_in_x1);
|
||||
initialEstimate.insert(P(2), p2_in_x1);
|
||||
initialEstimate.insert(X(0), x0_initial);
|
||||
|
|
|
|||
|
|
@ -173,7 +173,7 @@ TEST(PartialPriorFactor, Constructors3) {
|
|||
/* ************************************************************************* */
|
||||
TEST(PartialPriorFactor, JacobianAtIdentity3) {
|
||||
Key poseKey(1);
|
||||
Pose3 measurement = Pose3::identity();
|
||||
Pose3 measurement = Pose3::Identity();
|
||||
SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25);
|
||||
|
||||
TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model);
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@ using namespace gtsam::noiseModel;
|
|||
/* ************************************************************************* */
|
||||
// Verify zero error when there is no noise
|
||||
TEST(PoseToPointFactor, errorNoiseless_2D) {
|
||||
Pose2 pose = Pose2::identity();
|
||||
Pose2 pose = Pose2::Identity();
|
||||
Point2 point(1.0, 2.0);
|
||||
Point2 noise(0.0, 0.0);
|
||||
Point2 measured = point + noise;
|
||||
|
|
@ -33,7 +33,7 @@ TEST(PoseToPointFactor, errorNoiseless_2D) {
|
|||
/* ************************************************************************* */
|
||||
// Verify expected error in test scenario
|
||||
TEST(PoseToPointFactor, errorNoise_2D) {
|
||||
Pose2 pose = Pose2::identity();
|
||||
Pose2 pose = Pose2::Identity();
|
||||
Point2 point(1.0, 2.0);
|
||||
Point2 noise(-1.0, 0.5);
|
||||
Point2 measured = point + noise;
|
||||
|
|
@ -86,7 +86,7 @@ TEST(PoseToPointFactor, jacobian_2D) {
|
|||
/* ************************************************************************* */
|
||||
// Verify zero error when there is no noise
|
||||
TEST(PoseToPointFactor, errorNoiseless_3D) {
|
||||
Pose3 pose = Pose3::identity();
|
||||
Pose3 pose = Pose3::Identity();
|
||||
Point3 point(1.0, 2.0, 3.0);
|
||||
Point3 noise(0.0, 0.0, 0.0);
|
||||
Point3 measured = point + noise;
|
||||
|
|
@ -103,7 +103,7 @@ TEST(PoseToPointFactor, errorNoiseless_3D) {
|
|||
/* ************************************************************************* */
|
||||
// Verify expected error in test scenario
|
||||
TEST(PoseToPointFactor, errorNoise_3D) {
|
||||
Pose3 pose = Pose3::identity();
|
||||
Pose3 pose = Pose3::Identity();
|
||||
Point3 point(1.0, 2.0, 3.0);
|
||||
Point3 noise(-1.0, 0.5, 0.3);
|
||||
Point3 measured = point + noise;
|
||||
|
|
|
|||
|
|
@ -88,7 +88,7 @@ typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
|
|||
TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
|
||||
using namespace vanillaPoseRS;
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
|
||||
SmartFactorRS::shared_ptr factor1(
|
||||
new SmartFactorRS(model, cameraRig, params));
|
||||
}
|
||||
|
|
@ -97,7 +97,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
|
|||
TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
|
||||
using namespace vanillaPoseRS;
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactorRS factor1(model, cameraRig, params);
|
||||
}
|
||||
|
|
@ -106,7 +106,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
|
|||
TEST(SmartProjectionPoseFactorRollingShutter, add) {
|
||||
using namespace vanillaPoseRS;
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
|
||||
SmartFactorRS::shared_ptr factor1(
|
||||
new SmartFactorRS(model, cameraRig, params));
|
||||
factor1->add(measurement1, x1, x2, interp_factor);
|
||||
|
|
@ -230,7 +230,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
|
|||
// Project two landmarks into two cameras
|
||||
Point2 level_uv = cam1.project(landmark1);
|
||||
Point2 level_uv_right = cam2.project(landmark1);
|
||||
Pose3 body_P_sensorId = Pose3::identity();
|
||||
Pose3 body_P_sensorId = Pose3::Identity();
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(body_P_sensorId, sharedK));
|
||||
|
|
@ -405,7 +405,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
|
|||
interp_factors.push_back(interp_factor3);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
|
||||
|
||||
SmartFactorRS::shared_ptr smartFactor1(
|
||||
new SmartFactorRS(model, cameraRig, params));
|
||||
|
|
@ -480,7 +480,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) {
|
|||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(body_P_sensor, sharedK));
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
|
||||
|
||||
SmartFactorRS::shared_ptr smartFactor1(
|
||||
new SmartFactorRS(model, cameraRig, params));
|
||||
|
|
@ -633,11 +633,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
|
|||
// Default cameras for simple derivatives
|
||||
static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
|
||||
|
||||
Rot3 R = Rot3::identity();
|
||||
Rot3 R = Rot3::Identity();
|
||||
Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
|
||||
Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
|
||||
Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
|
||||
Pose3 body_P_sensorId = Pose3::identity();
|
||||
Pose3 body_P_sensorId = Pose3::Identity();
|
||||
|
||||
// one landmarks 1m in front of camera
|
||||
Point3 landmark1(0, 0, 10);
|
||||
|
|
@ -747,7 +747,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
|
|||
params.setEnableEPI(true);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
|
||||
|
||||
SmartFactorRS smartFactor1(model, cameraRig, params);
|
||||
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
|
||||
|
|
@ -816,7 +816,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
|
|||
params.setEnableEPI(false);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
|
||||
|
||||
SmartFactorRS smartFactor1(model, cameraRig, params);
|
||||
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
|
||||
|
|
@ -894,7 +894,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
|
|||
params.setEnableEPI(false);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
|
||||
|
||||
SmartFactorRS::shared_ptr smartFactor1(
|
||||
new SmartFactorRS(model, cameraRig, params));
|
||||
|
|
@ -961,7 +961,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
|
|||
interp_factors.push_back(interp_factor3);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
|
||||
|
||||
SmartFactorRS::shared_ptr smartFactor1(
|
||||
new SmartFactorRS(model, cameraRig, params));
|
||||
|
|
@ -1102,7 +1102,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
|
|||
interp_factors.push_back(interp_factor1);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
|
||||
|
||||
SmartFactorRS::shared_ptr smartFactor1(
|
||||
new SmartFactorRS(model, cameraRig, params));
|
||||
|
|
@ -1261,7 +1261,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
|
|||
interp_factors.at(0)); // we readd the first interp factor
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
|
||||
|
||||
SmartFactorRS::shared_ptr smartFactor1(
|
||||
new SmartFactorRS(model, cameraRig, params));
|
||||
|
|
@ -1331,11 +1331,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
|
|||
gtsam::HESSIAN,
|
||||
gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
|
||||
|
||||
Rot3 R = Rot3::identity();
|
||||
Rot3 R = Rot3::Identity();
|
||||
Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
|
||||
Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
|
||||
Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
|
||||
Pose3 body_P_sensorId = Pose3::identity();
|
||||
Pose3 body_P_sensorId = Pose3::Identity();
|
||||
|
||||
// one landmarks 1m in front of camera
|
||||
Point3 landmark1(0, 0, 10);
|
||||
|
|
@ -1431,7 +1431,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
|
|||
params.setRankTolerance(0.1);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), emptyK));
|
||||
cameraRig->push_back(Camera(Pose3::Identity(), emptyK));
|
||||
|
||||
SmartFactorRS_spherical::shared_ptr smartFactor1(
|
||||
new SmartFactorRS_spherical(model, cameraRig, params));
|
||||
|
|
|
|||
|
|
@ -198,10 +198,10 @@ TEST(testISAM2SmartFactor, Stereo_Batch) {
|
|||
|
||||
// prior, for the first keyframe:
|
||||
if (kf_id == 0) {
|
||||
batch_graph.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise);
|
||||
batch_graph.addPrior(X(kf_id), Pose3::Identity(), priorPoseNoise);
|
||||
}
|
||||
|
||||
batch_values.insert(X(kf_id), Pose3::identity());
|
||||
batch_values.insert(X(kf_id), Pose3::Identity());
|
||||
}
|
||||
|
||||
LevenbergMarquardtParams parameters;
|
||||
|
|
@ -267,7 +267,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) {
|
|||
// Storage of smart factors:
|
||||
std::map<lm_id_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
|
||||
|
||||
Pose3 lastKeyframePose = Pose3::identity();
|
||||
Pose3 lastKeyframePose = Pose3::Identity();
|
||||
|
||||
// Run one timestep at once:
|
||||
for (const auto &entries : dataset) {
|
||||
|
|
@ -307,7 +307,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) {
|
|||
|
||||
// prior, for the first keyframe:
|
||||
if (kf_id == 0) {
|
||||
newFactors.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise);
|
||||
newFactors.addPrior(X(kf_id), Pose3::Identity(), priorPoseNoise);
|
||||
}
|
||||
|
||||
// 2) Run iSAM2:
|
||||
|
|
|
|||
|
|
@ -181,8 +181,8 @@ TEST_UNSAFE( SmartStereoProjectionFactorPP, noiseless_error_identityExtrinsics )
|
|||
Values values;
|
||||
values.insert(x1, w_Pose_cam1);
|
||||
values.insert(x2, w_Pose_cam2);
|
||||
values.insert(body_P_cam1_key, Pose3::identity());
|
||||
values.insert(body_P_cam2_key, Pose3::identity());
|
||||
values.insert(body_P_cam1_key, Pose3::Identity());
|
||||
values.insert(body_P_cam2_key, Pose3::Identity());
|
||||
|
||||
SmartStereoProjectionFactorPP factor1(model);
|
||||
factor1.add(cam1_uv, x1, body_P_cam1_key, K2);
|
||||
|
|
@ -426,7 +426,7 @@ TEST( SmartStereoProjectionFactorPP, 3poses_optimization_multipleExtrinsics ) {
|
|||
// Values
|
||||
Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
|
||||
Pose3 body_Pose_cam2 = Pose3(Rot3::Ypr(-M_PI / 4, 0.1, 1.0),Point3(1, 1, 1));
|
||||
Pose3 body_Pose_cam3 = Pose3::identity();
|
||||
Pose3 body_Pose_cam3 = Pose3::Identity();
|
||||
Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse());
|
||||
Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam2.inverse());
|
||||
Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam3.inverse());
|
||||
|
|
@ -1147,7 +1147,7 @@ TEST( SmartStereoProjectionFactorPP, landmarkDistance ) {
|
|||
graph.push_back(smartFactor3);
|
||||
graph.addPrior(x1, pose1, noisePrior);
|
||||
graph.addPrior(x2, pose2, noisePrior);
|
||||
graph.addPrior(body_P_cam_key, Pose3::identity(), noisePrior);
|
||||
graph.addPrior(body_P_cam_key, Pose3::Identity(), noisePrior);
|
||||
|
||||
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
|
||||
|
|
@ -1156,7 +1156,7 @@ TEST( SmartStereoProjectionFactorPP, landmarkDistance ) {
|
|||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
values.insert(x3, pose3 * noise_pose);
|
||||
values.insert(body_P_cam_key, Pose3::identity());
|
||||
values.insert(body_P_cam_key, Pose3::Identity());
|
||||
|
||||
// All smart factors are disabled and pose should remain where it is
|
||||
Values result;
|
||||
|
|
@ -1245,7 +1245,7 @@ TEST( SmartStereoProjectionFactorPP, dynamicOutlierRejection ) {
|
|||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
values.insert(x3, pose3);
|
||||
values.insert(body_P_cam_key, Pose3::identity());
|
||||
values.insert(body_P_cam_key, Pose3::Identity());
|
||||
|
||||
EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9);
|
||||
EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9);
|
||||
|
|
@ -1267,7 +1267,7 @@ TEST( SmartStereoProjectionFactorPP, dynamicOutlierRejection ) {
|
|||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(Pose3::identity(), result.at<Pose3>(body_P_cam_key)));
|
||||
EXPECT(assert_equal(Pose3::Identity(), result.at<Pose3>(body_P_cam_key)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -73,6 +73,7 @@ set(interface_files
|
|||
${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i
|
||||
${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i
|
||||
${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
|
||||
${GTSAM_SOURCE_DIR}/gtsam/nonlinear/custom.i
|
||||
${GTSAM_SOURCE_DIR}/gtsam/symbolic/symbolic.i
|
||||
${GTSAM_SOURCE_DIR}/gtsam/sam/sam.i
|
||||
${GTSAM_SOURCE_DIR}/gtsam/slam/slam.i
|
||||
|
|
@ -98,7 +99,6 @@ endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
|
|||
|
||||
# Record the root dir for gtsam - needed during external builds, e.g., ROS
|
||||
set(GTSAM_SOURCE_ROOT_DIR ${GTSAM_SOURCE_DIR})
|
||||
message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]")
|
||||
|
||||
# Tests message(STATUS "Installing Matlab Toolbox")
|
||||
install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig")
|
||||
|
|
|
|||
|
|
@ -61,6 +61,7 @@ set(interface_headers
|
|||
${PROJECT_SOURCE_DIR}/gtsam/geometry/geometry.i
|
||||
${PROJECT_SOURCE_DIR}/gtsam/linear/linear.i
|
||||
${PROJECT_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
|
||||
${PROJECT_SOURCE_DIR}/gtsam/nonlinear/custom.i
|
||||
${PROJECT_SOURCE_DIR}/gtsam/symbolic/symbolic.i
|
||||
${PROJECT_SOURCE_DIR}/gtsam/sam/sam.i
|
||||
${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i
|
||||
|
|
|
|||
|
|
@ -0,0 +1,12 @@
|
|||
/* Please refer to:
|
||||
* https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
|
||||
* These are required to save one copy operation on Python calls.
|
||||
*
|
||||
* NOTES
|
||||
* =================
|
||||
*
|
||||
* `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
|
||||
* automatic STL binding, such that the raw objects can be accessed in Python.
|
||||
* Without this they will be automatically converted to a Python object, and all
|
||||
* mutations on Python side will not be reflected on C++.
|
||||
*/
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
/* Please refer to:
|
||||
* https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
|
||||
* These are required to save one copy operation on Python calls.
|
||||
*
|
||||
* NOTES
|
||||
* =================
|
||||
*
|
||||
* `py::bind_vector` and similar machinery gives the std container a Python-like
|
||||
* interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
|
||||
* with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
|
||||
* and saves one copy operation.
|
||||
*/
|
||||
|
|
@ -15,10 +15,12 @@ from __future__ import print_function
|
|||
import unittest
|
||||
|
||||
import gtsam
|
||||
from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters,
|
||||
GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLMOptimizer,
|
||||
LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph,
|
||||
Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values)
|
||||
from gtsam import (
|
||||
DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, GaussNewtonOptimizer,
|
||||
GaussNewtonParams, GncLMParams, GncLossType, GncLMOptimizer, LevenbergMarquardtOptimizer,
|
||||
LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2,
|
||||
PriorFactorPoint2, Values
|
||||
)
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
KEY1 = 1
|
||||
|
|
@ -27,7 +29,6 @@ KEY2 = 2
|
|||
|
||||
class TestScenario(GtsamTestCase):
|
||||
"""Do trivial test with three optimizer variants."""
|
||||
|
||||
def setUp(self):
|
||||
"""Set up the optimization problem and ordering"""
|
||||
# create graph
|
||||
|
|
@ -83,16 +84,83 @@ class TestScenario(GtsamTestCase):
|
|||
actual = GncLMOptimizer(self.fg, self.initial_values, gncParams).optimize()
|
||||
self.assertAlmostEqual(0, self.fg.error(actual))
|
||||
|
||||
def test_gnc_params(self):
|
||||
base_params = LevenbergMarquardtParams()
|
||||
# Test base params
|
||||
for base_max_iters in (50, 100):
|
||||
base_params.setMaxIterations(base_max_iters)
|
||||
params = GncLMParams(base_params)
|
||||
self.assertEqual(params.baseOptimizerParams.getMaxIterations(), base_max_iters)
|
||||
|
||||
# Test printing
|
||||
params_str = str(params)
|
||||
for s in (
|
||||
"lossType",
|
||||
"maxIterations",
|
||||
"muStep",
|
||||
"relativeCostTol",
|
||||
"weightsTol",
|
||||
"verbosity",
|
||||
):
|
||||
self.assertTrue(s in params_str)
|
||||
|
||||
# Test each parameter
|
||||
for loss_type in (GncLossType.TLS, GncLossType.GM):
|
||||
params.setLossType(loss_type) # Default is TLS
|
||||
self.assertEqual(params.lossType, loss_type)
|
||||
for max_iter in (1, 10, 100):
|
||||
params.setMaxIterations(max_iter)
|
||||
self.assertEqual(params.maxIterations, max_iter)
|
||||
for mu_step in (1.1, 1.2, 1.5):
|
||||
params.setMuStep(mu_step)
|
||||
self.assertEqual(params.muStep, mu_step)
|
||||
for rel_cost_tol in (1e-5, 1e-6, 1e-7):
|
||||
params.setRelativeCostTol(rel_cost_tol)
|
||||
self.assertEqual(params.relativeCostTol, rel_cost_tol)
|
||||
for weights_tol in (1e-4, 1e-3, 1e-2):
|
||||
params.setWeightsTol(weights_tol)
|
||||
self.assertEqual(params.weightsTol, weights_tol)
|
||||
for i in (0, 1, 2):
|
||||
verb = GncLMParams.Verbosity(i)
|
||||
params.setVerbosityGNC(verb)
|
||||
self.assertEqual(params.verbosity, verb)
|
||||
for inl in ([], [10], [0, 100]):
|
||||
params.setKnownInliers(inl)
|
||||
self.assertEqual(params.knownInliers, inl)
|
||||
params.knownInliers = []
|
||||
for out in ([], [1], [0, 10]):
|
||||
params.setKnownInliers(out)
|
||||
self.assertEqual(params.knownInliers, out)
|
||||
params.knownInliers = []
|
||||
|
||||
# Test optimizer params
|
||||
optimizer = GncLMOptimizer(self.fg, self.initial_values, params)
|
||||
for ict_factor in (0.9, 1.1):
|
||||
new_ict = ict_factor * optimizer.getInlierCostThresholds()
|
||||
optimizer.setInlierCostThresholds(new_ict)
|
||||
self.assertAlmostEqual(optimizer.getInlierCostThresholds(), new_ict)
|
||||
for w_factor in (0.8, 0.9):
|
||||
new_weights = w_factor * optimizer.getWeights()
|
||||
optimizer.setWeights(new_weights)
|
||||
self.assertAlmostEqual(optimizer.getWeights(), new_weights)
|
||||
optimizer.setInlierCostThresholdsAtProbability(0.9)
|
||||
w1 = optimizer.getInlierCostThresholds()
|
||||
optimizer.setInlierCostThresholdsAtProbability(0.8)
|
||||
w2 = optimizer.getInlierCostThresholds()
|
||||
self.assertLess(w2, w1)
|
||||
|
||||
def test_iteration_hook(self):
|
||||
# set up iteration hook to track some testable values
|
||||
iteration_count = 0
|
||||
final_error = 0
|
||||
final_values = None
|
||||
|
||||
def iteration_hook(iter, error_before, error_after):
|
||||
nonlocal iteration_count, final_error, final_values
|
||||
iteration_count = iter
|
||||
final_error = error_after
|
||||
final_values = optimizer.values()
|
||||
|
||||
# optimize
|
||||
params = LevenbergMarquardtParams.CeresDefaults()
|
||||
params.setOrdering(self.ordering)
|
||||
|
|
@ -104,5 +172,6 @@ class TestScenario(GtsamTestCase):
|
|||
self.assertEqual(self.fg.error(actual), final_error)
|
||||
self.assertEqual(optimizer.iterations(), iteration_count)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
|
|||
|
|
@ -382,7 +382,7 @@ TEST(ExpressionFactor, compose2) {
|
|||
TEST(ExpressionFactor, compose3) {
|
||||
|
||||
// Create expression
|
||||
Rot3_ R1(Rot3::identity()), R2(3);
|
||||
Rot3_ R1(Rot3::Identity()), R2(3);
|
||||
Rot3_ R3 = R1 * R2;
|
||||
|
||||
// Create factor
|
||||
|
|
|
|||
|
|
@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
|
|||
Values initial;
|
||||
initial.insert(X(1), p0);
|
||||
|
||||
std::vector<size_t> knownInliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownInliers;
|
||||
knownInliers.push_back(0);
|
||||
knownInliers.push_back(1);
|
||||
knownInliers.push_back(2);
|
||||
|
|
@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) {
|
|||
Values initial;
|
||||
initial.insert(X(1), p0);
|
||||
|
||||
std::vector<size_t> knownInliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownInliers;
|
||||
knownInliers.push_back(0);
|
||||
knownInliers.push_back(1);
|
||||
knownInliers.push_back(2);
|
||||
|
|
@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) {
|
|||
Values initial;
|
||||
initial.insert(X(1), p0);
|
||||
|
||||
std::vector<size_t> knownInliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownInliers;
|
||||
knownInliers.push_back(0);
|
||||
knownInliers.push_back(1);
|
||||
knownInliers.push_back(2);
|
||||
|
|
@ -763,7 +763,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) {
|
|||
// GNC
|
||||
// Note: in difficult instances, we set the odometry measurements to be
|
||||
// inliers, but this problem is simple enought to succeed even without that
|
||||
// assumption std::vector<size_t> knownInliers;
|
||||
// assumption GncParams<GaussNewtonParams>::IndexVector knownInliers;
|
||||
GncParams<GaussNewtonParams> gncParams;
|
||||
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(*graph, *initial,
|
||||
gncParams);
|
||||
|
|
@ -784,12 +784,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
|
|||
// nonconvexity with known inliers and known outliers (check early stopping
|
||||
// when all measurements are known to be inliers or outliers)
|
||||
{
|
||||
std::vector<size_t> knownInliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownInliers;
|
||||
knownInliers.push_back(0);
|
||||
knownInliers.push_back(1);
|
||||
knownInliers.push_back(2);
|
||||
|
||||
std::vector<size_t> knownOutliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownOutliers;
|
||||
knownOutliers.push_back(3);
|
||||
|
||||
GncParams<GaussNewtonParams> gncParams;
|
||||
|
|
@ -813,11 +813,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
|
|||
|
||||
// nonconvexity with known inliers and known outliers
|
||||
{
|
||||
std::vector<size_t> knownInliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownInliers;
|
||||
knownInliers.push_back(2);
|
||||
knownInliers.push_back(0);
|
||||
|
||||
std::vector<size_t> knownOutliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownOutliers;
|
||||
knownOutliers.push_back(3);
|
||||
|
||||
GncParams<GaussNewtonParams> gncParams;
|
||||
|
|
@ -841,7 +841,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
|
|||
|
||||
// only known outliers
|
||||
{
|
||||
std::vector<size_t> knownOutliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownOutliers;
|
||||
knownOutliers.push_back(3);
|
||||
|
||||
GncParams<GaussNewtonParams> gncParams;
|
||||
|
|
@ -916,11 +916,11 @@ TEST(GncOptimizer, setWeights) {
|
|||
// initialize weights and also set known inliers/outliers
|
||||
{
|
||||
GncParams<GaussNewtonParams> gncParams;
|
||||
std::vector<size_t> knownInliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownInliers;
|
||||
knownInliers.push_back(2);
|
||||
knownInliers.push_back(0);
|
||||
|
||||
std::vector<size_t> knownOutliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownOutliers;
|
||||
knownOutliers.push_back(3);
|
||||
gncParams.setKnownInliers(knownInliers);
|
||||
gncParams.setKnownOutliers(knownOutliers);
|
||||
|
|
|
|||
|
|
@ -139,7 +139,7 @@ TEST(Manifold, DefaultChart) {
|
|||
|
||||
Vector v3(3);
|
||||
v3 << 1, 1, 1;
|
||||
Rot3 I = Rot3::identity();
|
||||
Rot3 I = Rot3::Identity();
|
||||
Rot3 R = I.retract(v3);
|
||||
//DefaultChart<Rot3> chart5;
|
||||
EXPECT(assert_equal(v3, traits<Rot3>::Local(I, R)));
|
||||
|
|
|
|||
|
|
@ -62,9 +62,9 @@ int main(int argc, char* argv[]) {
|
|||
|
||||
// Build graph
|
||||
NonlinearFactorGraph graph;
|
||||
// graph.add(NonlinearEquality<SOn>(0, SOn::identity(4)));
|
||||
// graph.add(NonlinearEquality<SOn>(0, SOn::Identity(4)));
|
||||
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));
|
||||
for (const auto &m : measurements) {
|
||||
const auto &keys = m.keys();
|
||||
|
|
@ -92,7 +92,7 @@ int main(int argc, char* argv[]) {
|
|||
for (size_t i = 0; i < 100; i++) {
|
||||
gttic_(optimize);
|
||||
Values initial;
|
||||
initial.insert(0, SOn::identity(4));
|
||||
initial.insert(0, SOn::Identity(4));
|
||||
for (size_t j = 1; j < poses.size(); j++) {
|
||||
initial.insert(j, SOn::Random(rng, 4));
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue