Merge branch 'develop' into model-selection-integration
Varun Agrawal
commit
2a080bb2a6
|
|
@ -19,3 +19,4 @@ CMakeLists.txt.user*
|
|||
xcode/
|
||||
/Dockerfile
|
||||
/python/gtsam/notebooks/.ipynb_checkpoints/ellipses-checkpoint.ipynb
|
||||
.cache/
|
||||
|
|
|
|||
10
README.md
10
README.md
|
|
@ -15,11 +15,11 @@ matrices.
|
|||
|
||||
The current support matrix is:
|
||||
|
||||
| Platform | Compiler | Build Status |
|
||||
|:------------:|:---------:|:-------------:|
|
||||
| Ubuntu 18.04 | gcc/clang |  |
|
||||
| macOS | clang |  |
|
||||
| Windows | MSVC |  |
|
||||
| Platform | Compiler | Build Status |
|
||||
|:------------------:|:---------:|:--------------------------------------------------------------------------------:|
|
||||
| Ubuntu 20.04/22.04 | gcc/clang |  |
|
||||
| macOS | clang |  |
|
||||
| Windows | MSVC |  |
|
||||
|
||||
|
||||
On top of the C++ library, GTSAM includes [wrappers for MATLAB & Python](#wrappers).
|
||||
|
|
|
|||
|
|
@ -0,0 +1,45 @@
|
|||
# base image off ubuntu image
|
||||
ARG UBUNTU_TAG=22.04
|
||||
FROM docker.io/ubuntu:${UBUNTU_TAG}
|
||||
|
||||
RUN apt-get update && apt-get install -y --no-install-recommends \
|
||||
# dependencies
|
||||
libboost-all-dev \
|
||||
# optional dependencies
|
||||
libtbb-dev \
|
||||
python3-dev \
|
||||
python3-pip \
|
||||
python3-pyparsing \
|
||||
python3-numpy \
|
||||
# build dependencies
|
||||
build-essential \
|
||||
cmake \
|
||||
# download dependencies
|
||||
git \
|
||||
ca-certificates && \
|
||||
rm -rf /var/lib/apt/lists/*
|
||||
|
||||
# build flags
|
||||
ARG GTSAM_GIT_TAG=4.2.0
|
||||
ARG GTSAM_WITH_TBB=ON
|
||||
ARG GTSAM_BUILD_PYTHON=ON
|
||||
ARG CORES=4
|
||||
|
||||
# build and install gtsam
|
||||
RUN mkdir -p /src/github/borglab && cd /src/github/borglab && \
|
||||
git clone https://github.com/borglab/gtsam --depth 1 --branch ${GTSAM_GIT_TAG} && \
|
||||
cd gtsam && \
|
||||
mkdir build && \
|
||||
cd build && \
|
||||
cmake \
|
||||
-DCMAKE_BUILD_TYPE=Release \
|
||||
-DGTSAM_BUILD_TESTS=OFF \
|
||||
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB} \
|
||||
-DGTSAM_BUILD_PYTHON=${GTSAM_BUILD_PYTHON} \
|
||||
.. && \
|
||||
make -j${CORES} install && \
|
||||
if [ "${GTSAM_BUILD_PYTHON}" = "ON" ] ; then \
|
||||
make python-install; \
|
||||
fi
|
||||
|
||||
CMD ["/bin/bash"]
|
||||
|
|
@ -0,0 +1,127 @@
|
|||
# GTSAM Containers
|
||||
|
||||
- container files to build images
|
||||
- script to push images to a registry
|
||||
- instructions to pull images and run containers
|
||||
|
||||
## Dependencies
|
||||
|
||||
- a container engine such as [`Docker Engine`](https://docs.docker.com/engine/install/)
|
||||
|
||||
## Pull from Docker Hub
|
||||
|
||||
Various GTSAM image configurations are available at [`docker.io/borglab/gtsam`](https://hub.docker.com/r/borglab/gtsam). Determine which [tag](https://hub.docker.com/r/borglab/gtsam/tags) you want and pull the image.
|
||||
|
||||
Example for pulling an image with GTSAM compiled with TBB and Python support on top of a base Ubuntu 22.04 image.
|
||||
|
||||
```bash
|
||||
docker pull docker.io/borglab/gtsam:4.2.0-tbb-ON-python-ON_22.04
|
||||
```
|
||||
|
||||
[`docker.io/borglab/gtsam-vnc`](https://hub.docker.com/r/borglab/gtsam-vnc) is also provided as an image with GTSAM that will run a VNC server to connect to.
|
||||
|
||||
## Using the images
|
||||
|
||||
### Just GTSAM
|
||||
|
||||
To start the image, execute
|
||||
|
||||
```bash
|
||||
docker run -it borglab/gtsam:4.2.0-tbb-ON-python-OFF_22.04
|
||||
```
|
||||
|
||||
after you will find yourself in a bash shell.
|
||||
|
||||
### GTSAM with Python wrapper
|
||||
|
||||
To use GTSAM via the python wrapper, similarly execute
|
||||
|
||||
```bash
|
||||
docker run -it borglab/gtsam:4.2.0-tbb-ON-python-ON_22.04
|
||||
```
|
||||
|
||||
and then launch `python3`:
|
||||
|
||||
```bash
|
||||
python3
|
||||
>>> import gtsam
|
||||
>>> gtsam.Pose2(1,2,3)
|
||||
(1, 2, 3)
|
||||
```
|
||||
|
||||
### GTSAM with Python wrapper and VNC
|
||||
|
||||
First, start the image, which will run a VNC server on port 5900:
|
||||
|
||||
```bash
|
||||
docker run -p 5900:5900 borglab/gtsam-vnc:4.2.0-tbb-ON-python-ON_22.04
|
||||
```
|
||||
|
||||
Then open a remote VNC X client, for example:
|
||||
|
||||
#### Linux
|
||||
|
||||
```bash
|
||||
sudo apt-get install tigervnc-viewer
|
||||
xtigervncviewer :5900
|
||||
```
|
||||
|
||||
#### Mac
|
||||
|
||||
The Finder's "Connect to Server..." with `vnc://127.0.0.1` does not work, for some reason. Using the free [VNC Viewer](https://www.realvnc.com/en/connect/download/viewer/), enter `0.0.0.0:5900` as the server.
|
||||
|
||||
## Build images locally
|
||||
|
||||
### Build Dependencies
|
||||
|
||||
- a [Compose Spec](https://compose-spec.io/) implementation such as [docker-compose](https://docs.docker.com/compose/install/)
|
||||
|
||||
### `gtsam` image
|
||||
|
||||
#### `.env` file
|
||||
|
||||
- `GTSAM_GIT_TAG`: [git tag from the gtsam repo](https://github.com/borglab/gtsam/tags)
|
||||
- `UBUNTU_TAG`: image tag provided by [ubuntu](https://hub.docker.com/_/ubuntu/tags) to base the image off of
|
||||
- `GTSAM_WITH_TBB`: to build GTSAM with TBB, set to `ON`
|
||||
- `GTSAM_BUILD_PYTHON`: to build python bindings, set to `ON`
|
||||
- `CORES`: number of cores to compile with
|
||||
|
||||
#### Build `gtsam` image
|
||||
|
||||
```bash
|
||||
docker compose build
|
||||
```
|
||||
|
||||
### `gtsam-vnc` image
|
||||
|
||||
#### `gtsam-vnc/.env` file
|
||||
|
||||
- `GTSAM_TAG`: image tag provided by [gtsam](https://hub.docker.com/r/borglab/gtsam/tags)
|
||||
|
||||
#### Build `gtsam-vnc` image
|
||||
|
||||
```bash
|
||||
docker compose --file gtsam-vnc/compose.yaml build
|
||||
```
|
||||
|
||||
## Push to Docker Hub
|
||||
|
||||
Make sure you are logged in via: `docker login docker.io`.
|
||||
|
||||
### `gtsam` images
|
||||
|
||||
Specify the variables described in the `.env` file in the `hub_push.sh` script.
|
||||
To push images to Docker Hub, run as follows:
|
||||
|
||||
```bash
|
||||
./hub_push.sh
|
||||
```
|
||||
|
||||
### `gtsam-vnc` images
|
||||
|
||||
Specify the variables described in the `gtsam-vnc/.env` file in the `gtsam-vnc/hub_push.sh` script.
|
||||
To push images to Docker Hub, run as follows:
|
||||
|
||||
```bash
|
||||
./gtsam-vnc/hub_push.sh
|
||||
```
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
services:
|
||||
gtsam:
|
||||
build:
|
||||
args:
|
||||
UBUNTU_TAG: ${UBUNTU_TAG}
|
||||
GTSAM_GIT_TAG: ${GTSAM_GIT_TAG}
|
||||
GTSAM_WITH_TBB: ${GTSAM_WITH_TBB}
|
||||
GTSAM_BUILD_PYTHON: ${GTSAM_BUILD_PYTHON}
|
||||
CORES: ${CORES}
|
||||
context: .
|
||||
dockerfile: Containerfile
|
||||
env_file:
|
||||
- .env
|
||||
image: gtsam:${GTSAM_GIT_TAG}-tbb-${GTSAM_WITH_TBB}-python-${GTSAM_BUILD_PYTHON}_${UBUNTU_TAG}
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
# This image connects to the host X-server via VNC to provide a Graphical User Interface for interaction.
|
||||
|
||||
# base image off gtsam image
|
||||
ARG GTSAM_TAG=4.2.0-tbb-ON-python-ON_22.04
|
||||
FROM docker.io/borglab/gtsam:${GTSAM_TAG}
|
||||
|
||||
RUN apt-get update && apt-get install -y --no-install-recommends \
|
||||
# Things needed to get a python GUI
|
||||
python3-tk \
|
||||
python3-matplotlib \
|
||||
# Install a VNC X-server, Frame buffer, and windows manager
|
||||
x11vnc \
|
||||
xvfb \
|
||||
fluxbox \
|
||||
# Finally, install wmctrl needed for bootstrap script
|
||||
wmctrl \
|
||||
rm -rf /var/lib/apt/lists/*
|
||||
|
||||
COPY bootstrap.sh /
|
||||
CMD ["/bootstrap.sh"]
|
||||
0
docker/ubuntu-gtsam-python-vnc/bootstrap.sh → containers/gtsam-vnc/bootstrap.sh
Executable file → Normal file
0
docker/ubuntu-gtsam-python-vnc/bootstrap.sh → containers/gtsam-vnc/bootstrap.sh
Executable file → Normal file
|
|
@ -0,0 +1,10 @@
|
|||
services:
|
||||
gtsam_vnc:
|
||||
build:
|
||||
args:
|
||||
GTSAM_TAG: ${GTSAM_TAG}
|
||||
context: .
|
||||
dockerfile: Containerfile
|
||||
env_file:
|
||||
- .env
|
||||
image: gtsam-vnc:${GTSAM_TAG}
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
#!/usr/bin/env bash
|
||||
|
||||
# A script to push images to Docker Hub
|
||||
|
||||
declare -a gtsam_tags=("4.2.0-tbb-ON-python-ON_22.04")
|
||||
|
||||
for gtsam_tag in "${gtsam_tags[@]}"; do
|
||||
|
||||
touch gtsam-vnc/.env
|
||||
echo "GTSAM_TAG=${gtsam_tag}" > gtsam-vnc/.env
|
||||
|
||||
docker compose --file gtsam-vnc/compose.yaml build
|
||||
|
||||
docker tag gtsam-vnc:"${gtsam_tag}" \
|
||||
docker.io/borglab/gtsam-vnc:"${gtsam_tag}"
|
||||
|
||||
docker push docker.io/borglab/gtsam-vnc:"${gtsam_tag}"
|
||||
|
||||
done
|
||||
|
|
@ -0,0 +1,32 @@
|
|||
#!/usr/bin/env bash
|
||||
|
||||
# A script to push images to Docker Hub
|
||||
|
||||
declare -a ubuntu_tags=("22.04")
|
||||
declare -a gtsam_git_tags=("4.2.0")
|
||||
declare -a gtsam_with_tbb_options=("OFF" "ON")
|
||||
declare -a gtsam_build_python_options=("OFF" "ON")
|
||||
|
||||
for ubuntu_tag in "${ubuntu_tags[@]}"; do
|
||||
for gtsam_git_tag in "${gtsam_git_tags[@]}"; do
|
||||
for gtsam_with_tbb in "${gtsam_with_tbb_options[@]}"; do
|
||||
for gtsam_build_python in "${gtsam_build_python_options[@]}"; do
|
||||
|
||||
touch .env
|
||||
echo "UBUNTU_TAG=${ubuntu_tag}" > .env
|
||||
echo "GTSAM_GIT_TAG=${gtsam_git_tag}" >> .env
|
||||
echo "GTSAM_WITH_TBB=${gtsam_with_tbb}" >> .env
|
||||
echo "GTSAM_BUILD_PYTHON=${gtsam_build_python}" >> .env
|
||||
echo "CORES=4" >> .env
|
||||
|
||||
docker compose build
|
||||
|
||||
docker tag gtsam:"${gtsam_git_tag}-tbb-${gtsam_with_tbb}-python-${gtsam_build_python}_${ubuntu_tag}" \
|
||||
docker.io/borglab/gtsam:"${gtsam_git_tag}-tbb-${gtsam_with_tbb}-python-${gtsam_build_python}_${ubuntu_tag}"
|
||||
|
||||
docker push docker.io/borglab/gtsam:"${gtsam_git_tag}-tbb-${gtsam_with_tbb}-python-${gtsam_build_python}_${ubuntu_tag}"
|
||||
|
||||
done
|
||||
done
|
||||
done
|
||||
done
|
||||
|
|
@ -1,63 +0,0 @@
|
|||
# Instructions
|
||||
|
||||
# Images on Docker Hub
|
||||
|
||||
There are 4 images available on https://hub.docker.com/orgs/borglab/repositories:
|
||||
|
||||
- `borglab/ubuntu-boost-tbb`: 18.06 Linux (nicknamed `bionic`) base image, with Boost and TBB installed.
|
||||
- `borglab/ubuntu-gtsam`: GTSAM Release version installed in `/usr/local`.
|
||||
- `borglab/ubuntu-gtsam-python`: installed GTSAM with python wrapper.
|
||||
- `borglab/ubuntu-gtsam-python-vnc`: image with GTSAM+python wrapper that will run a VNC server to connect to.
|
||||
|
||||
# Using the images
|
||||
|
||||
## Just GTSAM
|
||||
|
||||
To start the Docker image, execute
|
||||
```bash
|
||||
docker run -it borglab/ubuntu-gtsam:bionic
|
||||
```
|
||||
after you will find yourself in a bash shell, in the directory `/usr/src/gtsam/build`.
|
||||
## GTSAM with Python wrapper
|
||||
|
||||
To use GTSAM via the python wrapper, similarly execute
|
||||
```bash
|
||||
docker run -it borglab/ubuntu-gtsam-python:bionic
|
||||
```
|
||||
and then launch `python3`:
|
||||
```bash
|
||||
python3
|
||||
>>> import gtsam
|
||||
>>> gtsam.Pose2(1,2,3)
|
||||
(1, 2, 3)
|
||||
```
|
||||
|
||||
## GTSAM with Python wrapper and VNC
|
||||
|
||||
First, start the docker image, which will run a VNC server on port 5900:
|
||||
```bash
|
||||
docker run -p 5900:5900 borglab/ubuntu-gtsam-python-vnc:bionic
|
||||
```
|
||||
|
||||
Then open a remote VNC X client, for example:
|
||||
|
||||
### Linux
|
||||
```bash
|
||||
sudo apt-get install tigervnc-viewer
|
||||
xtigervncviewer :5900
|
||||
```
|
||||
### Mac
|
||||
The Finder's "Connect to Server..." with `vnc://127.0.0.1` does not work, for some reason. Using the free [VNC Viewer](https://www.realvnc.com/en/connect/download/viewer/), enter `0.0.0.0:5900` as the server.
|
||||
|
||||
# Re-building the images locally
|
||||
|
||||
To build all docker images, in order:
|
||||
|
||||
```bash
|
||||
(cd ubuntu-boost-tbb && ./build.sh)
|
||||
(cd ubuntu-gtsam && ./build.sh)
|
||||
(cd ubuntu-gtsam-python && ./build.sh)
|
||||
(cd ubuntu-gtsam-python-vnc && ./build.sh)
|
||||
```
|
||||
|
||||
Note: building GTSAM can take a lot of memory because of the heavy templating. It is advisable to give Docker enough resources, e.g., 8GB, to avoid OOM errors while compiling.
|
||||
|
|
@ -1,19 +0,0 @@
|
|||
# Basic Ubuntu 18.04 image with Boost and TBB installed. To be used for building further downstream packages.
|
||||
|
||||
# Get the base Ubuntu image from Docker Hub
|
||||
FROM ubuntu:bionic
|
||||
|
||||
# Disable GUI prompts
|
||||
ENV DEBIAN_FRONTEND noninteractive
|
||||
|
||||
# Update apps on the base image
|
||||
RUN apt-get -y update && apt-get -y install
|
||||
|
||||
# Install C++
|
||||
RUN apt-get -y install build-essential apt-utils
|
||||
|
||||
# Install boost and cmake
|
||||
RUN apt-get -y install libboost-all-dev cmake
|
||||
|
||||
# Install TBB
|
||||
RUN apt-get -y install libtbb-dev
|
||||
|
|
@ -1,3 +0,0 @@
|
|||
# Build command for Docker image
|
||||
# TODO(dellaert): use docker compose and/or cmake
|
||||
docker build --no-cache -t borglab/ubuntu-boost-tbb:bionic .
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
# This GTSAM image connects to the host X-server via VNC to provide a Graphical User Interface for interaction.
|
||||
|
||||
# Get the base Ubuntu/GTSAM image from Docker Hub
|
||||
FROM borglab/ubuntu-gtsam-python:bionic
|
||||
|
||||
# Things needed to get a python GUI
|
||||
ENV DEBIAN_FRONTEND noninteractive
|
||||
RUN apt install -y python-tk
|
||||
RUN python3 -m pip install matplotlib
|
||||
|
||||
# Install a VNC X-server, Frame buffer, and windows manager
|
||||
RUN apt install -y x11vnc xvfb fluxbox
|
||||
|
||||
# Finally, install wmctrl needed for bootstrap script
|
||||
RUN apt install -y wmctrl
|
||||
|
||||
# Copy bootstrap script and make sure it runs
|
||||
COPY bootstrap.sh /
|
||||
|
||||
CMD '/bootstrap.sh'
|
||||
|
|
@ -1,4 +0,0 @@
|
|||
# Build command for Docker image
|
||||
# TODO(dellaert): use docker compose and/or cmake
|
||||
# Needs to be run in docker/ubuntu-gtsam-python-vnc directory
|
||||
docker build -t borglab/ubuntu-gtsam-python-vnc:bionic .
|
||||
|
|
@ -1,5 +0,0 @@
|
|||
# After running this script, connect VNC client to 0.0.0.0:5900
|
||||
docker run -it \
|
||||
--workdir="/usr/src/gtsam" \
|
||||
-p 5900:5900 \
|
||||
borglab/ubuntu-gtsam-python-vnc:bionic
|
||||
|
|
@ -1,30 +0,0 @@
|
|||
# GTSAM Ubuntu image with Python wrapper support.
|
||||
|
||||
# Get the base Ubuntu/GTSAM image from Docker Hub
|
||||
FROM borglab/ubuntu-gtsam:bionic
|
||||
|
||||
# Install pip
|
||||
RUN apt-get install -y python3-pip python3-dev
|
||||
|
||||
# Run cmake again, now with python toolbox on
|
||||
WORKDIR /usr/src/gtsam/build
|
||||
RUN cmake \
|
||||
-DCMAKE_BUILD_TYPE=Release \
|
||||
-DGTSAM_WITH_EIGEN_MKL=OFF \
|
||||
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
|
||||
-DGTSAM_BUILD_TIMING_ALWAYS=OFF \
|
||||
-DGTSAM_BUILD_TESTS=OFF \
|
||||
-DGTSAM_BUILD_PYTHON=ON \
|
||||
-DGTSAM_PYTHON_VERSION=3\
|
||||
..
|
||||
|
||||
# Build again, as ubuntu-gtsam image cleaned
|
||||
RUN make -j4 install
|
||||
RUN make python-install
|
||||
RUN make clean
|
||||
|
||||
# Needed to run python wrapper:
|
||||
RUN echo 'export PYTHONPATH=/usr/local/python/:$PYTHONPATH' >> /root/.bashrc
|
||||
|
||||
# Run bash
|
||||
CMD ["bash"]
|
||||
|
|
@ -1,3 +0,0 @@
|
|||
# Build command for Docker image
|
||||
# TODO(dellaert): use docker compose and/or cmake
|
||||
docker build --no-cache -t borglab/ubuntu-gtsam-python:bionic .
|
||||
|
|
@ -1,35 +0,0 @@
|
|||
# Ubuntu image with GTSAM installed. Configured with Boost and TBB support.
|
||||
|
||||
# Get the base Ubuntu image from Docker Hub
|
||||
FROM borglab/ubuntu-boost-tbb:bionic
|
||||
|
||||
# Install git
|
||||
RUN apt-get update && \
|
||||
apt-get install -y git
|
||||
|
||||
# Install compiler
|
||||
RUN apt-get install -y build-essential
|
||||
|
||||
# Clone GTSAM (develop branch)
|
||||
WORKDIR /usr/src/
|
||||
RUN git clone --single-branch --branch develop https://github.com/borglab/gtsam.git
|
||||
|
||||
# Change to build directory. Will be created automatically.
|
||||
WORKDIR /usr/src/gtsam/build
|
||||
# Run cmake
|
||||
RUN cmake \
|
||||
-DCMAKE_BUILD_TYPE=Release \
|
||||
-DGTSAM_WITH_EIGEN_MKL=OFF \
|
||||
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
|
||||
-DGTSAM_BUILD_TIMING_ALWAYS=OFF \
|
||||
-DGTSAM_BUILD_TESTS=OFF \
|
||||
..
|
||||
|
||||
# Build
|
||||
RUN make -j4 install && make clean
|
||||
|
||||
# Needed to link with GTSAM
|
||||
RUN echo 'export LD_LIBRARY_PATH=/usr/local/lib:LD_LIBRARY_PATH' >> /root/.bashrc
|
||||
|
||||
# Run bash
|
||||
CMD ["bash"]
|
||||
|
|
@ -1,3 +0,0 @@
|
|||
# Build command for Docker image
|
||||
# TODO(dellaert): use docker compose and/or cmake
|
||||
docker build --no-cache -t borglab/ubuntu-gtsam:bionic .
|
||||
|
|
@ -18,6 +18,8 @@
|
|||
|
||||
#include <gtsam/discrete/DiscreteBayesNet.h>
|
||||
#include <gtsam/discrete/DiscreteConditional.h>
|
||||
#include <gtsam/discrete/DiscreteFactorGraph.h>
|
||||
#include <gtsam/discrete/DiscreteLookupDAG.h>
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -56,7 +58,8 @@ DiscreteValues DiscreteBayesNet::sample() const {
|
|||
|
||||
DiscreteValues DiscreteBayesNet::sample(DiscreteValues result) const {
|
||||
// sample each node in turn in topological sort order (parents first)
|
||||
for (auto it = std::make_reverse_iterator(end()); it != std::make_reverse_iterator(begin()); ++it) {
|
||||
for (auto it = std::make_reverse_iterator(end());
|
||||
it != std::make_reverse_iterator(begin()); ++it) {
|
||||
(*it)->sampleInPlace(&result);
|
||||
}
|
||||
return result;
|
||||
|
|
|
|||
|
|
@ -235,16 +235,19 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
|
|||
}
|
||||
|
||||
/* ************************************************************************** */
|
||||
size_t DiscreteConditional::argmax() const {
|
||||
size_t DiscreteConditional::argmax(const DiscreteValues& parentsValues) const {
|
||||
ADT pFS = choose(parentsValues, true); // P(F|S=parentsValues)
|
||||
|
||||
// Initialize
|
||||
size_t maxValue = 0;
|
||||
double maxP = 0;
|
||||
DiscreteValues values = parentsValues;
|
||||
|
||||
assert(nrFrontals() == 1);
|
||||
assert(nrParents() == 0);
|
||||
DiscreteValues frontals;
|
||||
Key j = firstFrontalKey();
|
||||
for (size_t value = 0; value < cardinality(j); value++) {
|
||||
frontals[j] = value;
|
||||
double pValueS = (*this)(frontals);
|
||||
values[j] = value;
|
||||
double pValueS = (*this)(values);
|
||||
// Update MPE solution if better
|
||||
if (pValueS > maxP) {
|
||||
maxP = pValueS;
|
||||
|
|
@ -459,7 +462,7 @@ string DiscreteConditional::html(const KeyFormatter& keyFormatter,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double DiscreteConditional::evaluate(const HybridValues& x) const{
|
||||
double DiscreteConditional::evaluate(const HybridValues& x) const {
|
||||
return this->evaluate(x.discrete());
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -18,9 +18,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/Conditional-inst.h>
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam/discrete/Signature.h>
|
||||
#include <gtsam/inference/Conditional-inst.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
|
@ -39,7 +39,7 @@ class GTSAM_EXPORT DiscreteConditional
|
|||
public Conditional<DecisionTreeFactor, DiscreteConditional> {
|
||||
public:
|
||||
// typedefs needed to play nice with gtsam
|
||||
typedef DiscreteConditional This; ///< Typedef to this class
|
||||
typedef DiscreteConditional This; ///< Typedef to this class
|
||||
typedef std::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
|
||||
typedef DecisionTreeFactor BaseFactor; ///< Typedef to our factor base class
|
||||
typedef Conditional<BaseFactor, This>
|
||||
|
|
@ -159,9 +159,7 @@ class GTSAM_EXPORT DiscreteConditional
|
|||
/// @{
|
||||
|
||||
/// Log-probability is just -error(x).
|
||||
double logProbability(const DiscreteValues& x) const {
|
||||
return -error(x);
|
||||
}
|
||||
double logProbability(const DiscreteValues& x) const { return -error(x); }
|
||||
|
||||
/// print index signature only
|
||||
void printSignature(
|
||||
|
|
@ -214,10 +212,11 @@ class GTSAM_EXPORT DiscreteConditional
|
|||
size_t sample() const;
|
||||
|
||||
/**
|
||||
* @brief Return assignment that maximizes distribution.
|
||||
* @return Optimal assignment (1 frontal variable).
|
||||
* @brief Return assignment for single frontal variable that maximizes value.
|
||||
* @param parentsValues Known assignments for the parents.
|
||||
* @return maximizing assignment for the frontal variable.
|
||||
*/
|
||||
size_t argmax() const;
|
||||
size_t argmax(const DiscreteValues& parentsValues = DiscreteValues()) const;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
|
|
@ -244,7 +243,6 @@ class GTSAM_EXPORT DiscreteConditional
|
|||
std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
|
||||
const Names& names = {}) const override;
|
||||
|
||||
|
||||
/// @}
|
||||
/// @name HybridValues methods.
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -119,7 +119,8 @@ DiscreteLookupDAG DiscreteLookupDAG::FromBayesNet(
|
|||
|
||||
DiscreteValues DiscreteLookupDAG::argmax(DiscreteValues result) const {
|
||||
// Argmax each node in turn in topological sort order (parents first).
|
||||
for (auto it = std::make_reverse_iterator(end()); it != std::make_reverse_iterator(begin()); ++it) {
|
||||
for (auto it = std::make_reverse_iterator(end());
|
||||
it != std::make_reverse_iterator(begin()); ++it) {
|
||||
// dereference to get the sharedFactor to the lookup table
|
||||
(*it)->argmaxInPlace(&result);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -14,6 +14,9 @@ class DiscreteKeys {
|
|||
bool empty() const;
|
||||
gtsam::DiscreteKey at(size_t n) const;
|
||||
void push_back(const gtsam::DiscreteKey& point_pair);
|
||||
void print(const std::string& s = "",
|
||||
const gtsam::KeyFormatter& keyFormatter =
|
||||
gtsam::DefaultKeyFormatter) const;
|
||||
};
|
||||
|
||||
// DiscreteValues is added in specializations/discrete.h as a std::map
|
||||
|
|
@ -104,6 +107,9 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor {
|
|||
DiscreteConditional(const gtsam::DecisionTreeFactor& joint,
|
||||
const gtsam::DecisionTreeFactor& marginal,
|
||||
const gtsam::Ordering& orderedKeys);
|
||||
DiscreteConditional(const gtsam::DiscreteKey& key,
|
||||
const gtsam::DiscreteKeys& parents,
|
||||
const std::vector<double>& table);
|
||||
|
||||
// Standard interface
|
||||
double logNormalizationConstant() const;
|
||||
|
|
@ -131,6 +137,7 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor {
|
|||
size_t sample(size_t value) const;
|
||||
size_t sample() const;
|
||||
void sampleInPlace(gtsam::DiscreteValues @parentsValues) const;
|
||||
size_t argmax(const gtsam::DiscreteValues& parents) const;
|
||||
|
||||
// Markdown and HTML
|
||||
string markdown(const gtsam::KeyFormatter& keyFormatter =
|
||||
|
|
@ -159,7 +166,6 @@ virtual class DiscreteDistribution : gtsam::DiscreteConditional {
|
|||
gtsam::DefaultKeyFormatter) const;
|
||||
double operator()(size_t value) const;
|
||||
std::vector<double> pmf() const;
|
||||
size_t argmax() const;
|
||||
};
|
||||
|
||||
#include <gtsam/discrete/DiscreteBayesNet.h>
|
||||
|
|
|
|||
|
|
@ -16,14 +16,13 @@
|
|||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <gtsam/discrete/DiscreteBayesNet.h>
|
||||
#include <gtsam/discrete/DiscreteFactorGraph.h>
|
||||
#include <gtsam/discrete/DiscreteMarginals.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
|
@ -43,8 +42,7 @@ TEST(DiscreteBayesNet, bayesNet) {
|
|||
DiscreteKey Parent(0, 2), Child(1, 2);
|
||||
|
||||
auto prior = std::make_shared<DiscreteConditional>(Parent % "6/4");
|
||||
CHECK(assert_equal(ADT({Parent}, "0.6 0.4"),
|
||||
(ADT)*prior));
|
||||
CHECK(assert_equal(ADT({Parent}, "0.6 0.4"), (ADT)*prior));
|
||||
bayesNet.push_back(prior);
|
||||
|
||||
auto conditional =
|
||||
|
|
|
|||
|
|
@ -289,6 +289,35 @@ TEST(DiscreteConditional, choose) {
|
|||
EXPECT(assert_equal(expected3, *actual3, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Check argmax on P(C|D) and P(D), plus tie-breaking for P(B)
|
||||
TEST(DiscreteConditional, Argmax) {
|
||||
DiscreteKey B(2, 2), C(2, 2), D(4, 2);
|
||||
DiscreteConditional B_prior(D, "1/1");
|
||||
DiscreteConditional D_prior(D, "1/3");
|
||||
DiscreteConditional C_given_D((C | D) = "1/4 1/1");
|
||||
|
||||
// Case 1: Tie breaking
|
||||
size_t actual1 = B_prior.argmax();
|
||||
// In the case of ties, the first value is chosen.
|
||||
EXPECT_LONGS_EQUAL(0, actual1);
|
||||
// Case 2: No parents
|
||||
size_t actual2 = D_prior.argmax();
|
||||
// Selects 1 since it has 0.75 probability
|
||||
EXPECT_LONGS_EQUAL(1, actual2);
|
||||
|
||||
// Case 3: Given parent values
|
||||
DiscreteValues given;
|
||||
given[D.first] = 1;
|
||||
size_t actual3 = C_given_D.argmax(given);
|
||||
// Should be 0 since D=1 gives 0.5/0.5
|
||||
EXPECT_LONGS_EQUAL(0, actual3);
|
||||
|
||||
given[D.first] = 0;
|
||||
size_t actual4 = C_given_D.argmax(given);
|
||||
EXPECT_LONGS_EQUAL(1, actual4);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Check markdown representation looks as expected, no parents.
|
||||
TEST(DiscreteConditional, markdown_prior) {
|
||||
|
|
|
|||
|
|
@ -49,13 +49,14 @@ void PreintegratedAhrsMeasurements::resetIntegration() {
|
|||
//------------------------------------------------------------------------------
|
||||
void PreintegratedAhrsMeasurements::integrateMeasurement(
|
||||
const Vector3& measuredOmega, double deltaT) {
|
||||
Matrix3 Fr;
|
||||
PreintegratedRotation::integrateGyroMeasurement(measuredOmega, biasHat_,
|
||||
deltaT, &Fr);
|
||||
|
||||
Matrix3 D_incrR_integratedOmega, Fr;
|
||||
PreintegratedRotation::integrateMeasurement(measuredOmega,
|
||||
biasHat_, deltaT, &D_incrR_integratedOmega, &Fr);
|
||||
|
||||
// first order uncertainty propagation
|
||||
// the deltaT allows to pass from continuous time noise to discrete time noise
|
||||
// First order uncertainty propagation
|
||||
// The deltaT allows to pass from continuous time noise to discrete time
|
||||
// noise. Comparing with the IMUFactor.cpp implementation, the latter is an
|
||||
// approximation for C * (wCov / dt) * C.transpose(), with C \approx I * dt.
|
||||
preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + p().gyroscopeCovariance * deltaT;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -68,39 +68,40 @@ bool PreintegratedRotation::equals(const PreintegratedRotation& other,
|
|||
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
|
||||
}
|
||||
|
||||
Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
|
||||
const Vector3& biasHat, double deltaT,
|
||||
OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
|
||||
|
||||
namespace internal {
|
||||
Rot3 IncrementalRotation::operator()(
|
||||
const Vector3& bias, OptionalJacobian<3, 3> H_bias) const {
|
||||
// First we compensate the measurements for the bias
|
||||
Vector3 correctedOmega = measuredOmega - biasHat;
|
||||
Vector3 correctedOmega = measuredOmega - bias;
|
||||
|
||||
// Then compensate for sensor-body displacement: we express the quantities
|
||||
// (originally in the IMU frame) into the body frame
|
||||
if (p_->body_P_sensor) {
|
||||
Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix();
|
||||
// (originally in the IMU frame) into the body frame. If Jacobian is
|
||||
// requested, the rotation matrix is obtained as `rotate` Jacobian.
|
||||
Matrix3 body_R_sensor;
|
||||
if (body_P_sensor) {
|
||||
// rotation rate vector in the body frame
|
||||
correctedOmega = body_R_sensor * correctedOmega;
|
||||
correctedOmega = body_P_sensor->rotation().rotate(
|
||||
correctedOmega, {}, H_bias ? &body_R_sensor : nullptr);
|
||||
}
|
||||
|
||||
// rotation vector describing rotation increment computed from the
|
||||
// current rotation rate measurement
|
||||
const Vector3 integratedOmega = correctedOmega * deltaT;
|
||||
return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
|
||||
}
|
||||
|
||||
void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
|
||||
const Vector3& biasHat, double deltaT,
|
||||
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
|
||||
OptionalJacobian<3, 3> F) {
|
||||
Matrix3 D_incrR_integratedOmega;
|
||||
const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
|
||||
D_incrR_integratedOmega);
|
||||
|
||||
// If asked, pass first derivative as well
|
||||
if (optional_D_incrR_integratedOmega) {
|
||||
*optional_D_incrR_integratedOmega << D_incrR_integratedOmega;
|
||||
Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias); // expensive !!
|
||||
if (H_bias) {
|
||||
*H_bias *= -deltaT; // Correct so accurately reflects bias derivative
|
||||
if (body_P_sensor) *H_bias *= body_R_sensor;
|
||||
}
|
||||
return incrR;
|
||||
}
|
||||
} // namespace internal
|
||||
|
||||
void PreintegratedRotation::integrateGyroMeasurement(
|
||||
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
|
||||
OptionalJacobian<3, 3> F) {
|
||||
Matrix3 H_bias;
|
||||
internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
|
||||
const Rot3 incrR = f(biasHat, H_bias);
|
||||
|
||||
// Update deltaTij and rotation
|
||||
deltaTij_ += deltaT;
|
||||
|
|
@ -108,10 +109,26 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
|
|||
|
||||
// Update Jacobian
|
||||
const Matrix3 incrRt = incrR.transpose();
|
||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
|
||||
- D_incrR_integratedOmega * deltaT;
|
||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + H_bias;
|
||||
}
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
|
||||
void PreintegratedRotation::integrateMeasurement(
|
||||
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
|
||||
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
|
||||
OptionalJacobian<3, 3> F) {
|
||||
integrateGyroMeasurement(measuredOmega, biasHat, deltaT, F);
|
||||
|
||||
// If asked, pass obsolete Jacobians as well
|
||||
if (optional_D_incrR_integratedOmega) {
|
||||
Matrix3 H_bias;
|
||||
internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
|
||||
const Rot3 incrR = f(biasHat, H_bias);
|
||||
*optional_D_incrR_integratedOmega << H_bias / -deltaT;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
|
||||
OptionalJacobian<3, 3> H) const {
|
||||
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr;
|
||||
|
|
|
|||
|
|
@ -21,12 +21,37 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/std_optional_serialization.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include "gtsam/dllexport.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace internal {
|
||||
/**
|
||||
* @brief Function object for incremental rotation.
|
||||
* @param measuredOmega The measured angular velocity (as given by the sensor)
|
||||
* @param deltaT The time interval over which the rotation is integrated.
|
||||
* @param body_P_sensor Optional transform between body and IMU.
|
||||
*/
|
||||
struct GTSAM_EXPORT IncrementalRotation {
|
||||
const Vector3& measuredOmega;
|
||||
const double deltaT;
|
||||
const std::optional<Pose3>& body_P_sensor;
|
||||
|
||||
/**
|
||||
* @brief Integrate angular velocity, but corrected by bias.
|
||||
* @param bias The bias estimate
|
||||
* @param H_bias Jacobian of the rotation w.r.t. bias.
|
||||
* @return The incremental rotation
|
||||
*/
|
||||
Rot3 operator()(const Vector3& bias,
|
||||
OptionalJacobian<3, 3> H_bias = {}) const;
|
||||
};
|
||||
|
||||
} // namespace internal
|
||||
|
||||
/// Parameters for pre-integration:
|
||||
/// Usage: Create just a single Params and pass a shared pointer to the constructor
|
||||
struct GTSAM_EXPORT PreintegratedRotationParams {
|
||||
|
|
@ -65,7 +90,6 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
|
|||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
namespace bs = ::boost::serialization;
|
||||
ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
|
||||
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
|
||||
|
||||
|
|
@ -136,18 +160,10 @@ class GTSAM_EXPORT PreintegratedRotation {
|
|||
|
||||
/// @name Access instance variables
|
||||
/// @{
|
||||
const std::shared_ptr<Params>& params() const {
|
||||
return p_;
|
||||
}
|
||||
const double& deltaTij() const {
|
||||
return deltaTij_;
|
||||
}
|
||||
const Rot3& deltaRij() const {
|
||||
return deltaRij_;
|
||||
}
|
||||
const Matrix3& delRdelBiasOmega() const {
|
||||
return delRdelBiasOmega_;
|
||||
}
|
||||
const std::shared_ptr<Params>& params() const { return p_; }
|
||||
const double& deltaTij() const { return deltaTij_; }
|
||||
const Rot3& deltaRij() const { return deltaRij_; }
|
||||
const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; }
|
||||
/// @}
|
||||
|
||||
/// @name Testable
|
||||
|
|
@ -159,19 +175,24 @@ class GTSAM_EXPORT PreintegratedRotation {
|
|||
/// @name Main functionality
|
||||
/// @{
|
||||
|
||||
/// Take the gyro measurement, correct it using the (constant) bias estimate
|
||||
/// and possibly the sensor pose, and then integrate it forward in time to yield
|
||||
/// an incremental rotation.
|
||||
Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
|
||||
OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
|
||||
/**
|
||||
* @brief Calculate an incremental rotation given the gyro measurement and a
|
||||
* time interval, and update both deltaTij_ and deltaRij_.
|
||||
* @param measuredOmega The measured angular velocity (as given by the sensor)
|
||||
* @param bias The biasHat estimate
|
||||
* @param deltaT The time interval
|
||||
* @param F optional Jacobian of internal compose, used in AhrsFactor.
|
||||
*/
|
||||
void integrateGyroMeasurement(const Vector3& measuredOmega,
|
||||
const Vector3& biasHat, double deltaT,
|
||||
OptionalJacobian<3, 3> F = {});
|
||||
|
||||
/// Calculate an incremental rotation given the gyro measurement and a time interval,
|
||||
/// and update both deltaTij_ and deltaRij_.
|
||||
void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
|
||||
OptionalJacobian<3, 3> D_incrR_integratedOmega = {},
|
||||
OptionalJacobian<3, 3> F = {});
|
||||
|
||||
/// Return a bias corrected version of the integrated rotation, with optional Jacobian
|
||||
/**
|
||||
* @brief Return a bias corrected version of the integrated rotation.
|
||||
* @param biasOmegaIncr An increment with respect to biasHat used above.
|
||||
* @param H optional Jacobian of the correction w.r.t. the bias increment.
|
||||
* @note The *key* functionality of this class used in optimizing the bias.
|
||||
*/
|
||||
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
|
||||
OptionalJacobian<3, 3> H = {}) const;
|
||||
|
||||
|
|
@ -180,6 +201,31 @@ class GTSAM_EXPORT PreintegratedRotation {
|
|||
|
||||
/// @}
|
||||
|
||||
/// @name Deprecated API
|
||||
/// @{
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
|
||||
/// @deprecated: use IncrementalRotation functor with sane Jacobian
|
||||
inline Rot3 GTSAM_DEPRECATED incrementalRotation(
|
||||
const Vector3& measuredOmega, const Vector3& bias, double deltaT,
|
||||
OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
|
||||
internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
|
||||
Rot3 incrR = f(bias, D_incrR_integratedOmega);
|
||||
// Backwards compatible "weird" Jacobian, no longer used.
|
||||
if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT;
|
||||
return incrR;
|
||||
}
|
||||
|
||||
/// @deprecated: use integrateGyroMeasurement from now on
|
||||
/// @note this returned hard-to-understand Jacobian D_incrR_integratedOmega.
|
||||
void GTSAM_DEPRECATED integrateMeasurement(
|
||||
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
|
||||
OptionalJacobian<3, 3> D_incrR_integratedOmega, OptionalJacobian<3, 3> F);
|
||||
|
||||
#endif
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
/** Serialization function */
|
||||
|
|
|
|||
|
|
@ -18,47 +18,44 @@
|
|||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/AHRSFactor.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/navigation/AHRSFactor.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/factorTesting.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <list>
|
||||
#include <memory>
|
||||
#include "gtsam/nonlinear/LevenbergMarquardtParams.h"
|
||||
|
||||
using namespace std::placeholders;
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Convenience for named keys
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::V;
|
||||
using symbol_shorthand::B;
|
||||
using symbol_shorthand::R;
|
||||
|
||||
Vector3 kZeroOmegaCoriolis(0,0,0);
|
||||
Vector3 kZeroOmegaCoriolis(0, 0, 0);
|
||||
|
||||
// Define covariance matrices
|
||||
double accNoiseVar = 0.01;
|
||||
const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;
|
||||
double gyroNoiseVar = 0.01;
|
||||
const Matrix3 kMeasuredOmegaCovariance = gyroNoiseVar * I_3x3;
|
||||
|
||||
//******************************************************************************
|
||||
namespace {
|
||||
Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i,
|
||||
const Rot3 rot_j, const Vector3& bias) {
|
||||
return factor.evaluateError(rot_i, rot_j, bias);
|
||||
}
|
||||
|
||||
Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i,
|
||||
const Rot3 rot_j, const Vector3& bias) {
|
||||
return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3));
|
||||
}
|
||||
|
||||
PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements(
|
||||
const Vector3& bias, const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3::Zero()) {
|
||||
PreintegratedAhrsMeasurements result(bias, I_3x3);
|
||||
PreintegratedAhrsMeasurements integrateMeasurements(
|
||||
const Vector3& biasHat, const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs) {
|
||||
PreintegratedAhrsMeasurements result(biasHat, I_3x3);
|
||||
|
||||
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
|
||||
list<double>::const_iterator itDeltaT = deltaTs.begin();
|
||||
|
|
@ -68,79 +65,59 @@ PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements(
|
|||
|
||||
return result;
|
||||
}
|
||||
|
||||
Rot3 evaluatePreintegratedMeasurementsRotation(
|
||||
const Vector3& bias, const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3::Zero()) {
|
||||
return Rot3(
|
||||
evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
|
||||
initialRotationRate).deltaRij());
|
||||
}
|
||||
|
||||
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
|
||||
const double deltaT) {
|
||||
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
|
||||
}
|
||||
|
||||
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
|
||||
return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
|
||||
}
|
||||
}
|
||||
} // namespace
|
||||
|
||||
//******************************************************************************
|
||||
TEST( AHRSFactor, PreintegratedAhrsMeasurements ) {
|
||||
TEST(AHRSFactor, PreintegratedAhrsMeasurements) {
|
||||
// Linearization point
|
||||
Vector3 bias(0,0,0); ///< Current estimate of angular rate bias
|
||||
Vector3 biasHat(0, 0, 0); ///< Current estimate of angular rate bias
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
|
||||
double deltaT = 0.5;
|
||||
|
||||
// Expected preintegrated values
|
||||
Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0);
|
||||
double expectedDeltaT1(0.5);
|
||||
Rot3 expectedDeltaR1 = Rot3::Roll(0.5 * M_PI / 100.0);
|
||||
|
||||
// Actual preintegrated values
|
||||
PreintegratedAhrsMeasurements actual1(bias, Z_3x3);
|
||||
PreintegratedAhrsMeasurements actual1(biasHat, kMeasuredOmegaCovariance);
|
||||
actual1.integrateMeasurement(measuredOmega, deltaT);
|
||||
|
||||
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
|
||||
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
|
||||
DOUBLES_EQUAL(deltaT, actual1.deltaTij(), 1e-6);
|
||||
|
||||
// Check the covariance
|
||||
Matrix3 expectedMeasCov = kMeasuredOmegaCovariance * deltaT;
|
||||
EXPECT(assert_equal(expectedMeasCov, actual1.preintMeasCov(), 1e-6));
|
||||
|
||||
// Integrate again
|
||||
Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
|
||||
double expectedDeltaT2(1);
|
||||
Rot3 expectedDeltaR2 = Rot3::Roll(2.0 * 0.5 * M_PI / 100.0);
|
||||
|
||||
// Actual preintegrated values
|
||||
PreintegratedAhrsMeasurements actual2 = actual1;
|
||||
actual2.integrateMeasurement(measuredOmega, deltaT);
|
||||
|
||||
EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
|
||||
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
|
||||
DOUBLES_EQUAL(deltaT * 2, actual2.deltaTij(), 1e-6);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) {
|
||||
Matrix3 gyroscopeCovariance = Matrix3::Ones()*0.4;
|
||||
TEST(AHRSFactor, PreintegratedAhrsMeasurementsConstructor) {
|
||||
Matrix3 gyroscopeCovariance = I_3x3 * 0.4;
|
||||
Vector3 omegaCoriolis(0.1, 0.5, 0.9);
|
||||
PreintegratedRotationParams params(gyroscopeCovariance, omegaCoriolis);
|
||||
Vector3 bias(1.0,2.0,3.0); ///< Current estimate of angular rate bias
|
||||
Vector3 bias(1.0, 2.0, 3.0); ///< Current estimate of angular rate bias
|
||||
Rot3 deltaRij(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
|
||||
double deltaTij = 0.02;
|
||||
Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5;
|
||||
Matrix3 preintMeasCov = Matrix3::Ones()*0.2;
|
||||
Matrix3 delRdelBiasOmega = I_3x3 * 0.5;
|
||||
Matrix3 preintMeasCov = I_3x3 * 0.2;
|
||||
PreintegratedAhrsMeasurements actualPim(
|
||||
std::make_shared<PreintegratedRotationParams>(params),
|
||||
bias,
|
||||
deltaTij,
|
||||
deltaRij,
|
||||
delRdelBiasOmega,
|
||||
preintMeasCov);
|
||||
std::make_shared<PreintegratedRotationParams>(params), bias, deltaTij,
|
||||
deltaRij, delRdelBiasOmega, preintMeasCov);
|
||||
EXPECT(assert_equal(gyroscopeCovariance,
|
||||
actualPim.p().getGyroscopeCovariance(), 1e-6));
|
||||
EXPECT(assert_equal(omegaCoriolis,
|
||||
*(actualPim.p().getOmegaCoriolis()), 1e-6));
|
||||
actualPim.p().getGyroscopeCovariance(), 1e-6));
|
||||
EXPECT(
|
||||
assert_equal(omegaCoriolis, *(actualPim.p().getOmegaCoriolis()), 1e-6));
|
||||
EXPECT(assert_equal(bias, actualPim.biasHat(), 1e-6));
|
||||
DOUBLES_EQUAL(deltaTij, actualPim.deltaTij(), 1e-6);
|
||||
EXPECT(assert_equal(deltaRij, Rot3(actualPim.deltaRij()), 1e-6));
|
||||
|
|
@ -151,198 +128,148 @@ TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) {
|
|||
/* ************************************************************************* */
|
||||
TEST(AHRSFactor, Error) {
|
||||
// Linearization point
|
||||
Vector3 bias(0.,0.,0.); // Bias
|
||||
Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
|
||||
Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0));
|
||||
Vector3 bias(0., 0., 0.); // Bias
|
||||
Rot3 Ri(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
|
||||
Rot3 Rj(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0));
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega;
|
||||
measuredOmega << M_PI / 100, 0, 0;
|
||||
Vector3 measuredOmega(M_PI / 100, 0, 0);
|
||||
double deltaT = 1.0;
|
||||
PreintegratedAhrsMeasurements pim(bias, Z_3x3);
|
||||
PreintegratedAhrsMeasurements pim(bias, kMeasuredOmegaCovariance);
|
||||
pim.integrateMeasurement(measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, {});
|
||||
AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis, {});
|
||||
|
||||
Vector3 errorActual = factor.evaluateError(x1, x2, bias);
|
||||
|
||||
// Expected error
|
||||
Vector3 errorExpected(3);
|
||||
errorExpected << 0, 0, 0;
|
||||
// Check value
|
||||
Vector3 errorActual = factor.evaluateError(Ri, Rj, bias);
|
||||
Vector3 errorExpected(0, 0, 0);
|
||||
EXPECT(assert_equal(Vector(errorExpected), Vector(errorActual), 1e-6));
|
||||
|
||||
// Expected Jacobians
|
||||
Matrix H1e = numericalDerivative11<Vector3, Rot3>(
|
||||
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
|
||||
Matrix H2e = numericalDerivative11<Vector3, Rot3>(
|
||||
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
|
||||
Matrix H3e = numericalDerivative11<Vector3, Vector3>(
|
||||
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
|
||||
|
||||
// Check rotation Jacobians
|
||||
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
|
||||
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
|
||||
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
|
||||
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
|
||||
|
||||
// Actual Jacobians
|
||||
Matrix H1a, H2a, H3a;
|
||||
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
|
||||
|
||||
// rotations
|
||||
EXPECT(assert_equal(RH1e, H1a, 1e-5));
|
||||
// 1e-5 needs to be added only when using quaternions for rotations
|
||||
|
||||
EXPECT(assert_equal(H2e, H2a, 1e-5));
|
||||
|
||||
// rotations
|
||||
EXPECT(assert_equal(RH2e, H2a, 1e-5));
|
||||
// 1e-5 needs to be added only when using quaternions for rotations
|
||||
|
||||
EXPECT(assert_equal(H3e, H3a, 1e-5));
|
||||
// 1e-5 needs to be added only when using quaternions for rotations
|
||||
// Check Derivatives
|
||||
Values values;
|
||||
values.insert(R(1), Ri);
|
||||
values.insert(R(2), Rj);
|
||||
values.insert(B(1), bias);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(AHRSFactor, ErrorWithBiases) {
|
||||
// Linearization point
|
||||
|
||||
Vector3 bias(0, 0, 0.3);
|
||||
Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
|
||||
Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
|
||||
Rot3 Ri(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
|
||||
Rot3 Rj(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega;
|
||||
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
|
||||
Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3);
|
||||
double deltaT = 1.0;
|
||||
|
||||
PreintegratedAhrsMeasurements pim(Vector3(0,0,0),
|
||||
Z_3x3);
|
||||
PreintegratedAhrsMeasurements pim(Vector3(0, 0, 0), kMeasuredOmegaCovariance);
|
||||
pim.integrateMeasurement(measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
|
||||
AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
|
||||
|
||||
Vector errorActual = factor.evaluateError(x1, x2, bias);
|
||||
|
||||
// Expected error
|
||||
Vector errorExpected(3);
|
||||
errorExpected << 0, 0, 0;
|
||||
// Check value
|
||||
Vector3 errorExpected(0, 0, 0);
|
||||
Vector3 errorActual = factor.evaluateError(Ri, Rj, bias);
|
||||
EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
|
||||
|
||||
// Expected Jacobians
|
||||
Matrix H1e = numericalDerivative11<Vector, Rot3>(
|
||||
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
|
||||
Matrix H2e = numericalDerivative11<Vector, Rot3>(
|
||||
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
|
||||
Matrix H3e = numericalDerivative11<Vector, Vector3>(
|
||||
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
|
||||
|
||||
// Check rotation Jacobians
|
||||
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
|
||||
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
|
||||
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
|
||||
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
|
||||
Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
|
||||
std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias);
|
||||
|
||||
// Actual Jacobians
|
||||
Matrix H1a, H2a, H3a;
|
||||
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
|
||||
|
||||
EXPECT(assert_equal(H1e, H1a));
|
||||
EXPECT(assert_equal(H2e, H2a));
|
||||
EXPECT(assert_equal(H3e, H3a));
|
||||
// Check Derivatives
|
||||
Values values;
|
||||
values.insert(R(1), Ri);
|
||||
values.insert(R(2), Rj);
|
||||
values.insert(B(1), bias);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST( AHRSFactor, PartialDerivativeExpmap ) {
|
||||
TEST(AHRSFactor, PartialDerivativeExpmap) {
|
||||
// Linearization point
|
||||
Vector3 biasOmega(0,0,0);
|
||||
Vector3 biasOmega(0, 0, 0);
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega;
|
||||
measuredOmega << 0.1, 0, 0;
|
||||
Vector3 measuredOmega(0.1, 0, 0);
|
||||
double deltaT = 0.5;
|
||||
|
||||
auto f = [&](const Vector3& biasOmega) {
|
||||
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
|
||||
};
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
|
||||
std::bind(&evaluateRotation, measuredOmega, std::placeholders::_1, deltaT), biasOmega);
|
||||
Matrix expectedH = numericalDerivative11<Rot3, Vector3>(f, biasOmega);
|
||||
|
||||
const Matrix3 Jr = Rot3::ExpmapDerivative(
|
||||
(measuredOmega - biasOmega) * deltaT);
|
||||
const Matrix3 Jr =
|
||||
Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
|
||||
|
||||
Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
|
||||
Matrix3 actualH = -Jr * deltaT; // the delta bias appears with the minus sign
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3));
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-3));
|
||||
// 1e-3 needs to be added only when using quaternions for rotations
|
||||
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST( AHRSFactor, PartialDerivativeLogmap ) {
|
||||
TEST(AHRSFactor, PartialDerivativeLogmap) {
|
||||
// Linearization point
|
||||
Vector3 thetahat;
|
||||
thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias
|
||||
Vector3 thetaHat(0.1, 0.1, 0); ///< Current estimate of rotation rate bias
|
||||
|
||||
// Measurements
|
||||
Vector3 deltatheta;
|
||||
deltatheta << 0, 0, 0;
|
||||
auto f = [thetaHat](const Vector3 deltaTheta) {
|
||||
return Rot3::Logmap(
|
||||
Rot3::Expmap(thetaHat).compose(Rot3::Expmap(deltaTheta)));
|
||||
};
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelFdeltheta = numericalDerivative11<Vector3, Vector3>(
|
||||
std::bind(&evaluateLogRotation, thetahat, std::placeholders::_1), deltatheta);
|
||||
Vector3 deltaTheta(0, 0, 0);
|
||||
Matrix expectedH = numericalDerivative11<Vector3, Vector3>(f, deltaTheta);
|
||||
|
||||
const Vector3 x = thetahat; // parametrization of so(3)
|
||||
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
|
||||
double normx = x.norm();
|
||||
const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X
|
||||
+ (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X
|
||||
* X;
|
||||
const Vector3 x = thetaHat; // parametrization of so(3)
|
||||
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
|
||||
double norm = x.norm();
|
||||
const Matrix3 actualH =
|
||||
I_3x3 + 0.5 * X +
|
||||
(1 / (norm * norm) - (1 + cos(norm)) / (2 * norm * sin(norm))) * X * X;
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
|
||||
|
||||
EXPECT(assert_equal(expectedH, actualH));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST( AHRSFactor, fistOrderExponential ) {
|
||||
TEST(AHRSFactor, fistOrderExponential) {
|
||||
// Linearization point
|
||||
Vector3 biasOmega(0,0,0);
|
||||
Vector3 biasOmega(0, 0, 0);
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega;
|
||||
measuredOmega << 0.1, 0, 0;
|
||||
Vector3 measuredOmega(0.1, 0, 0);
|
||||
double deltaT = 1.0;
|
||||
|
||||
// change w.r.t. linearization point
|
||||
double alpha = 0.0;
|
||||
Vector3 deltabiasOmega;
|
||||
deltabiasOmega << alpha, alpha, alpha;
|
||||
Vector3 deltaBiasOmega(alpha, alpha, alpha);
|
||||
|
||||
const Matrix3 Jr = Rot3::ExpmapDerivative(
|
||||
(measuredOmega - biasOmega) * deltaT);
|
||||
const Matrix3 Jr =
|
||||
Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
|
||||
|
||||
Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
|
||||
Matrix3 delRdelBiasOmega =
|
||||
-Jr * deltaT; // the delta bias appears with the minus sign
|
||||
|
||||
const Matrix expectedRot = Rot3::Expmap(
|
||||
(measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
|
||||
const Matrix expectedRot =
|
||||
Rot3::Expmap((measuredOmega - biasOmega - deltaBiasOmega) * deltaT)
|
||||
.matrix();
|
||||
|
||||
const Matrix3 hatRot =
|
||||
Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
|
||||
const Matrix3 actualRot = hatRot
|
||||
* Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
|
||||
const Matrix3 actualRot =
|
||||
hatRot * Rot3::Expmap(delRdelBiasOmega * deltaBiasOmega).matrix();
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedRot, actualRot));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
|
||||
TEST(AHRSFactor, FirstOrderPreIntegratedMeasurements) {
|
||||
// Linearization point
|
||||
Vector3 bias = Vector3::Zero(); ///< Current estimate of rotation rate bias
|
||||
Vector3 bias = Vector3::Zero(); ///< Current estimate of rotation rate bias
|
||||
|
||||
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
|
||||
|
||||
|
|
@ -361,98 +288,76 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
|
|||
|
||||
// Actual preintegrated values
|
||||
PreintegratedAhrsMeasurements preintegrated =
|
||||
evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
|
||||
Vector3(M_PI / 100.0, 0.0, 0.0));
|
||||
integrateMeasurements(bias, measuredOmegas, deltaTs);
|
||||
|
||||
auto f = [&](const Vector3& bias) {
|
||||
return integrateMeasurements(bias, measuredOmegas, deltaTs).deltaRij();
|
||||
};
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelRdelBias =
|
||||
numericalDerivative11<Rot3, Vector3>(
|
||||
std::bind(&evaluatePreintegratedMeasurementsRotation, std::placeholders::_1,
|
||||
measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
|
||||
Matrix expectedDelRdelBias = numericalDerivative11<Rot3, Vector3>(f, bias);
|
||||
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(
|
||||
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3));
|
||||
EXPECT(assert_equal(expectedDelRdelBiasOmega,
|
||||
preintegrated.delRdelBiasOmega(), 1e-3));
|
||||
// 1e-3 needs to be added only when using quaternions for rotations
|
||||
}
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
|
||||
//******************************************************************************
|
||||
TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
|
||||
|
||||
TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
|
||||
Vector3 bias(0, 0, 0.3);
|
||||
Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
|
||||
Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
|
||||
Rot3 Ri(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
|
||||
Rot3 Rj(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
|
||||
|
||||
// Measurements
|
||||
Vector3 omegaCoriolis;
|
||||
omegaCoriolis << 0, 0.1, 0.1;
|
||||
Vector3 measuredOmega;
|
||||
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
|
||||
Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3);
|
||||
double deltaT = 1.0;
|
||||
|
||||
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
|
||||
Point3(1, 0, 0));
|
||||
|
||||
PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance);
|
||||
auto p = std::make_shared<PreintegratedAhrsMeasurements::Params>();
|
||||
p->gyroscopeCovariance = kMeasuredOmegaCovariance;
|
||||
p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(1, 2, 3)), Point3(1, 0, 0));
|
||||
PreintegratedAhrsMeasurements pim(p, Vector3::Zero());
|
||||
|
||||
pim.integrateMeasurement(measuredOmega, deltaT);
|
||||
|
||||
// Check preintegrated covariance
|
||||
EXPECT(assert_equal(kMeasuredAccCovariance, pim.preintMeasCov()));
|
||||
EXPECT(assert_equal(kMeasuredOmegaCovariance, pim.preintMeasCov()));
|
||||
|
||||
// Create factor
|
||||
AHRSFactor factor(X(1), X(2), B(1), pim, omegaCoriolis);
|
||||
AHRSFactor factor(R(1), R(2), B(1), pim, omegaCoriolis);
|
||||
|
||||
// Expected Jacobians
|
||||
Matrix H1e = numericalDerivative11<Vector, Rot3>(
|
||||
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
|
||||
Matrix H2e = numericalDerivative11<Vector, Rot3>(
|
||||
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
|
||||
Matrix H3e = numericalDerivative11<Vector, Vector3>(
|
||||
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
|
||||
|
||||
// Check rotation Jacobians
|
||||
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
|
||||
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
|
||||
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
|
||||
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
|
||||
Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
|
||||
std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias);
|
||||
|
||||
// Actual Jacobians
|
||||
Matrix H1a, H2a, H3a;
|
||||
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
|
||||
|
||||
EXPECT(assert_equal(H1e, H1a));
|
||||
EXPECT(assert_equal(H2e, H2a));
|
||||
EXPECT(assert_equal(H3e, H3a));
|
||||
// Check Derivatives
|
||||
Values values;
|
||||
values.insert(R(1), Ri);
|
||||
values.insert(R(2), Rj);
|
||||
values.insert(B(1), bias);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST (AHRSFactor, predictTest) {
|
||||
Vector3 bias(0,0,0);
|
||||
TEST(AHRSFactor, predictTest) {
|
||||
Vector3 bias(0, 0, 0);
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega;
|
||||
measuredOmega << 0, 0, M_PI / 10.0;
|
||||
Vector3 measuredOmega(0, 0, M_PI / 10.0);
|
||||
double deltaT = 0.2;
|
||||
PreintegratedAhrsMeasurements pim(bias, kMeasuredAccCovariance);
|
||||
PreintegratedAhrsMeasurements pim(bias, kMeasuredOmegaCovariance);
|
||||
for (int i = 0; i < 1000; ++i) {
|
||||
pim.integrateMeasurement(measuredOmega, deltaT);
|
||||
}
|
||||
// Check preintegrated covariance
|
||||
Matrix expectedMeasCov(3,3);
|
||||
expectedMeasCov = 200*kMeasuredAccCovariance;
|
||||
Matrix expectedMeasCov(3, 3);
|
||||
expectedMeasCov = 200 * kMeasuredOmegaCovariance;
|
||||
EXPECT(assert_equal(expectedMeasCov, pim.preintMeasCov()));
|
||||
|
||||
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
|
||||
AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
|
||||
|
||||
// Predict
|
||||
Rot3 x;
|
||||
Rot3 expectedRot = Rot3::Ypr(20*M_PI, 0, 0);
|
||||
Rot3 expectedRot = Rot3::Ypr(20 * M_PI, 0, 0);
|
||||
Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis);
|
||||
EXPECT(assert_equal(expectedRot, actualRot, 1e-6));
|
||||
|
||||
|
|
@ -462,29 +367,26 @@ TEST (AHRSFactor, predictTest) {
|
|||
|
||||
// Actual Jacobians
|
||||
Matrix H;
|
||||
(void) pim.predict(bias,H);
|
||||
(void)pim.predict(bias, H);
|
||||
EXPECT(assert_equal(expectedH, H, 1e-8));
|
||||
}
|
||||
//******************************************************************************
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
|
||||
TEST (AHRSFactor, graphTest) {
|
||||
TEST(AHRSFactor, graphTest) {
|
||||
// linearization point
|
||||
Rot3 x1(Rot3::RzRyRx(0, 0, 0));
|
||||
Rot3 x2(Rot3::RzRyRx(0, M_PI / 4, 0));
|
||||
Vector3 bias(0,0,0);
|
||||
Rot3 Ri(Rot3::RzRyRx(0, 0, 0));
|
||||
Rot3 Rj(Rot3::RzRyRx(0, M_PI / 4, 0));
|
||||
Vector3 bias(0, 0, 0);
|
||||
|
||||
// PreIntegrator
|
||||
Vector3 biasHat(0, 0, 0);
|
||||
PreintegratedAhrsMeasurements pim(biasHat, kMeasuredAccCovariance);
|
||||
PreintegratedAhrsMeasurements pim(biasHat, kMeasuredOmegaCovariance);
|
||||
|
||||
// Pre-integrate measurements
|
||||
Vector3 measuredOmega(0, M_PI / 20, 0);
|
||||
double deltaT = 1;
|
||||
|
||||
// Create Factor
|
||||
noiseModel::Base::shared_ptr model = //
|
||||
noiseModel::Base::shared_ptr model = //
|
||||
noiseModel::Gaussian::Covariance(pim.preintMeasCov());
|
||||
NonlinearFactorGraph graph;
|
||||
Values values;
|
||||
|
|
@ -492,16 +394,88 @@ TEST (AHRSFactor, graphTest) {
|
|||
pim.integrateMeasurement(measuredOmega, deltaT);
|
||||
}
|
||||
|
||||
// pim.print("Pre integrated measurementes");
|
||||
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
|
||||
values.insert(X(1), x1);
|
||||
values.insert(X(2), x2);
|
||||
// pim.print("Pre integrated measurements");
|
||||
AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
|
||||
values.insert(R(1), Ri);
|
||||
values.insert(R(2), Rj);
|
||||
values.insert(B(1), bias);
|
||||
graph.push_back(factor);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values);
|
||||
Values result = optimizer.optimize();
|
||||
Rot3 expectedRot(Rot3::RzRyRx(0, M_PI / 4, 0));
|
||||
EXPECT(assert_equal(expectedRot, result.at<Rot3>(X(2))));
|
||||
EXPECT(assert_equal(expectedRot, result.at<Rot3>(R(2))));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(AHRSFactor, bodyPSensorWithBias) {
|
||||
using noiseModel::Diagonal;
|
||||
|
||||
int numRotations = 10;
|
||||
const Vector3 noiseBetweenBiasSigma(3.0e-6, 3.0e-6, 3.0e-6);
|
||||
SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma);
|
||||
|
||||
// Measurements in the sensor frame:
|
||||
const double omega = 0.1;
|
||||
const Vector3 realOmega(omega, 0, 0);
|
||||
const Vector3 realBias(1, 2, 3); // large !
|
||||
const Vector3 measuredOmega = realOmega + realBias;
|
||||
|
||||
auto p = std::make_shared<PreintegratedAhrsMeasurements::Params>();
|
||||
p->body_P_sensor = Pose3(Rot3::Yaw(M_PI_2), Point3(0, 0, 0));
|
||||
p->gyroscopeCovariance = 1e-8 * I_3x3;
|
||||
double deltaT = 0.005;
|
||||
|
||||
// Specify noise values on priors
|
||||
const Vector3 priorNoisePoseSigmas(0.001, 0.001, 0.001);
|
||||
const Vector3 priorNoiseBiasSigmas(0.5e-1, 0.5e-1, 0.5e-1);
|
||||
SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas);
|
||||
SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
|
||||
|
||||
// Create a factor graph with priors on initial pose, velocity and bias
|
||||
NonlinearFactorGraph graph;
|
||||
Values values;
|
||||
|
||||
graph.addPrior(R(0), Rot3(), priorNoisePose);
|
||||
values.insert(R(0), Rot3());
|
||||
|
||||
// The key to this test is that we specify the bias, in the sensor frame, as
|
||||
// known a priori. We also create factors below that encode our assumption
|
||||
// that this bias is constant over time In theory, after optimization, we
|
||||
// should recover that same bias estimate
|
||||
graph.addPrior(B(0), realBias, priorNoiseBias);
|
||||
values.insert(B(0), realBias);
|
||||
|
||||
// Now add IMU factors and bias noise models
|
||||
const Vector3 zeroBias(0, 0, 0);
|
||||
for (int i = 1; i < numRotations; i++) {
|
||||
PreintegratedAhrsMeasurements pim(p, realBias);
|
||||
for (int j = 0; j < 200; ++j)
|
||||
pim.integrateMeasurement(measuredOmega, deltaT);
|
||||
|
||||
// Create factors
|
||||
graph.emplace_shared<AHRSFactor>(R(i - 1), R(i), B(i - 1), pim);
|
||||
graph.emplace_shared<BetweenFactor<Vector3> >(B(i - 1), B(i), zeroBias,
|
||||
biasNoiseModel);
|
||||
|
||||
values.insert(R(i), Rot3());
|
||||
values.insert(B(i), realBias);
|
||||
}
|
||||
|
||||
// Finally, optimize, and get bias at last time step
|
||||
LevenbergMarquardtParams params;
|
||||
// params.setVerbosityLM("SUMMARY");
|
||||
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
|
||||
const Vector3 biasActual = result.at<Vector3>(B(numRotations - 1));
|
||||
|
||||
// Bias should be a self-fulfilling prophesy:
|
||||
EXPECT(assert_equal(realBias, biasActual, 1e-3));
|
||||
|
||||
// Check that the successive rotations are all `omega` apart:
|
||||
for (int i = 0; i < numRotations; i++) {
|
||||
Rot3 expectedRot = Rot3::Pitch(omega * i);
|
||||
Rot3 actualRot = result.at<Rot3>(R(i));
|
||||
EXPECT(assert_equal(expectedRot, actualRot, 1e-3));
|
||||
}
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
|||
|
|
@ -410,33 +410,33 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) {
|
|||
const Matrix3 Jr =
|
||||
Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
|
||||
|
||||
Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
|
||||
Matrix3 actualDelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-9));
|
||||
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualDelRdelBiasOmega, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, PartialDerivativeLogmap) {
|
||||
// Linearization point
|
||||
Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias
|
||||
Vector3 thetaHat(0.1, 0.1, 0); // Current estimate of rotation rate bias
|
||||
|
||||
// Measurements
|
||||
Vector3 deltatheta(0, 0, 0);
|
||||
Vector3 deltaTheta(0, 0, 0);
|
||||
|
||||
auto evaluateLogRotation = [=](const Vector3 deltatheta) {
|
||||
auto evaluateLogRotation = [=](const Vector3 delta) {
|
||||
return Rot3::Logmap(
|
||||
Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
|
||||
Rot3::Expmap(thetaHat).compose(Rot3::Expmap(delta)));
|
||||
};
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelFdeltheta =
|
||||
numericalDerivative11<Vector, Vector3>(evaluateLogRotation, deltatheta);
|
||||
Matrix expectedDelFdelTheta =
|
||||
numericalDerivative11<Vector, Vector3>(evaluateLogRotation, deltaTheta);
|
||||
|
||||
Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat);
|
||||
Matrix3 actualDelFdelTheta = Rot3::LogmapDerivative(thetaHat);
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
|
||||
EXPECT(assert_equal(expectedDelFdelTheta, actualDelFdelTheta));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -450,8 +450,8 @@ TEST(ImuFactor, fistOrderExponential) {
|
|||
|
||||
// change w.r.t. linearization point
|
||||
double alpha = 0.0;
|
||||
Vector3 deltabiasOmega;
|
||||
deltabiasOmega << alpha, alpha, alpha;
|
||||
Vector3 deltaBiasOmega;
|
||||
deltaBiasOmega << alpha, alpha, alpha;
|
||||
|
||||
const Matrix3 Jr = Rot3::ExpmapDerivative(
|
||||
(measuredOmega - biasOmega) * deltaT);
|
||||
|
|
@ -459,13 +459,12 @@ TEST(ImuFactor, fistOrderExponential) {
|
|||
Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
|
||||
|
||||
const Matrix expectedRot = Rot3::Expmap(
|
||||
(measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
|
||||
(measuredOmega - biasOmega - deltaBiasOmega) * deltaT).matrix();
|
||||
|
||||
const Matrix3 hatRot =
|
||||
Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
|
||||
const Matrix3 actualRot = hatRot
|
||||
* Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
|
||||
// hatRot * (I_3x3 + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
|
||||
* Rot3::Expmap(delRdelBiasOmega * deltaBiasOmega).matrix();
|
||||
|
||||
// This is a first order expansion so the equality is only an approximation
|
||||
EXPECT(assert_equal(expectedRot, actualRot));
|
||||
|
|
@ -728,7 +727,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
|
|||
using noiseModel::Diagonal;
|
||||
typedef Bias Bias;
|
||||
|
||||
int numFactors = 10;
|
||||
int numPoses = 10;
|
||||
Vector6 noiseBetweenBiasSigma;
|
||||
noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6,
|
||||
3.0e-6, 3.0e-6);
|
||||
|
|
@ -761,7 +760,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
|
|||
SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
|
||||
Vector3 zeroVel(0, 0, 0);
|
||||
|
||||
// Create a factor graph with priors on initial pose, vlocity and bias
|
||||
// Create a factor graph with priors on initial pose, velocity and bias
|
||||
NonlinearFactorGraph graph;
|
||||
Values values;
|
||||
|
||||
|
|
@ -780,9 +779,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
|
|||
|
||||
// Now add IMU factors and bias noise models
|
||||
Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0));
|
||||
for (int i = 1; i < numFactors; i++) {
|
||||
PreintegratedImuMeasurements pim = PreintegratedImuMeasurements(p,
|
||||
priorBias);
|
||||
for (int i = 1; i < numPoses; i++) {
|
||||
PreintegratedImuMeasurements pim(p, priorBias);
|
||||
for (int j = 0; j < 200; ++j)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
|
|
@ -796,8 +794,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
|
|||
}
|
||||
|
||||
// Finally, optimize, and get bias at last time step
|
||||
Values results = LevenbergMarquardtOptimizer(graph, values).optimize();
|
||||
Bias biasActual = results.at<Bias>(B(numFactors - 1));
|
||||
Values result = LevenbergMarquardtOptimizer(graph, values).optimize();
|
||||
Bias biasActual = result.at<Bias>(B(numPoses - 1));
|
||||
|
||||
// And compare it with expected value (our prior)
|
||||
Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0));
|
||||
|
|
@ -851,11 +849,11 @@ struct ImuFactorMergeTest {
|
|||
actual_pim02.preintegrated(), tol));
|
||||
EXPECT(assert_equal(pim02_expected, actual_pim02, tol));
|
||||
|
||||
ImuFactor::shared_ptr factor01 =
|
||||
auto factor01 =
|
||||
std::make_shared<ImuFactor>(X(0), V(0), X(1), V(1), B(0), pim01);
|
||||
ImuFactor::shared_ptr factor12 =
|
||||
auto factor12 =
|
||||
std::make_shared<ImuFactor>(X(1), V(1), X(2), V(2), B(0), pim12);
|
||||
ImuFactor::shared_ptr factor02_expected = std::make_shared<ImuFactor>(
|
||||
auto factor02_expected = std::make_shared<ImuFactor>(
|
||||
X(0), V(0), X(2), V(2), B(0), pim02_expected);
|
||||
|
||||
ImuFactor::shared_ptr factor02_merged = ImuFactor::Merge(factor01, factor12);
|
||||
|
|
|
|||
|
|
@ -109,6 +109,59 @@ TEST(ManifoldPreintegration, computeError) {
|
|||
EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) {
|
||||
// Create a PreintegratedRotation object
|
||||
auto p = std::make_shared<PreintegratedRotationParams>();
|
||||
PreintegratedRotation pim(p);
|
||||
|
||||
// Integrate a single measurement
|
||||
const double omega = 0.1;
|
||||
const Vector3 trueOmega(omega, 0, 0);
|
||||
const Vector3 bias(1, 2, 3);
|
||||
const Vector3 measuredOmega = trueOmega + bias;
|
||||
const double deltaT = 0.5;
|
||||
|
||||
// Check the accumulated rotation.
|
||||
Rot3 expected = Rot3::Roll(omega * deltaT);
|
||||
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT);
|
||||
EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
|
||||
|
||||
// Now do the same for a ManifoldPreintegration object
|
||||
imuBias::ConstantBias biasHat {Z_3x1, bias};
|
||||
ManifoldPreintegration manifoldPim(testing::Params(), biasHat);
|
||||
manifoldPim.integrateMeasurement(Z_3x1, measuredOmega, deltaT);
|
||||
EXPECT(assert_equal(expected, manifoldPim.deltaRij(), 1e-9));
|
||||
|
||||
// Check their internal Jacobians are the same:
|
||||
EXPECT(assert_equal(pim.delRdelBiasOmega(), manifoldPim.delRdelBiasOmega()));
|
||||
|
||||
// Check PreintegratedRotation::biascorrectedDeltaRij.
|
||||
Matrix3 H;
|
||||
const double delta = 0.05;
|
||||
const Vector3 biasOmegaIncr(delta, 0, 0);
|
||||
Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
|
||||
EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected));
|
||||
const Rot3 expected2 = Rot3::Roll((omega - delta) * deltaT);
|
||||
EXPECT(assert_equal(expected2, corrected, 1e-9));
|
||||
|
||||
// Check ManifoldPreintegration::biasCorrectedDelta.
|
||||
imuBias::ConstantBias bias_i {Z_3x1, bias + biasOmegaIncr};
|
||||
Matrix96 H2;
|
||||
Vector9 biasCorrected = manifoldPim.biasCorrectedDelta(bias_i, H2);
|
||||
Vector9 expected3;
|
||||
expected3 << Rot3::Logmap(expected2), Z_3x1, Z_3x1;
|
||||
EXPECT(assert_equal(expected3, biasCorrected, 1e-9));
|
||||
|
||||
// Check that this one is sane:
|
||||
auto g = [&](const Vector3& increment) {
|
||||
return manifoldPim.biasCorrectedDelta({Z_3x1, bias + increment}, {});
|
||||
};
|
||||
EXPECT(assert_equal<Matrix>(numericalDerivative11<Vector9, Vector3>(g, Z_3x1),
|
||||
H2.rightCols<3>(),
|
||||
1e-4)); // NOTE(frank): reduced tolerance
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,138 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testPreintegratedRotation.cpp
|
||||
* @brief Unit test for PreintegratedRotation
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/navigation/PreintegratedRotation.h>
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "gtsam/base/Matrix.h"
|
||||
#include "gtsam/base/Vector.h"
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
//******************************************************************************
|
||||
// Example where gyro measures small rotation about x-axis, with bias.
|
||||
namespace biased_x_rotation {
|
||||
const double omega = 0.1;
|
||||
const Vector3 trueOmega(omega, 0, 0);
|
||||
const Vector3 bias(1, 2, 3);
|
||||
const Vector3 measuredOmega = trueOmega + bias;
|
||||
const double deltaT = 0.5;
|
||||
} // namespace biased_x_rotation
|
||||
|
||||
// Create params where x and y axes are exchanged.
|
||||
static std::shared_ptr<PreintegratedRotationParams> paramsWithTransform() {
|
||||
auto p = std::make_shared<PreintegratedRotationParams>();
|
||||
p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}});
|
||||
return p;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(PreintegratedRotation, integrateGyroMeasurement) {
|
||||
// Example where IMU is identical to body frame, then omega is roll
|
||||
using namespace biased_x_rotation;
|
||||
auto p = std::make_shared<PreintegratedRotationParams>();
|
||||
PreintegratedRotation pim(p);
|
||||
|
||||
// Check the value.
|
||||
Matrix3 H_bias;
|
||||
internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()};
|
||||
const Rot3 incrR = f(bias, H_bias);
|
||||
Rot3 expected = Rot3::Roll(omega * deltaT);
|
||||
EXPECT(assert_equal(expected, incrR, 1e-9));
|
||||
|
||||
// Check the derivative:
|
||||
EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias));
|
||||
|
||||
// Check value of deltaRij() after integration.
|
||||
Matrix3 F;
|
||||
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F);
|
||||
EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
|
||||
|
||||
// Check that system matrix F is the first derivative of compose:
|
||||
EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F));
|
||||
|
||||
// Make sure delRdelBiasOmega is H_bias after integration.
|
||||
EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
|
||||
|
||||
// Check if we make a correction to the bias, the value and Jacobian are
|
||||
// correct. Note that the bias is subtracted from the measurement, and the
|
||||
// integration time is taken into account, so we expect -deltaT*delta change.
|
||||
Matrix3 H;
|
||||
const double delta = 0.05;
|
||||
const Vector3 biasOmegaIncr(delta, 0, 0);
|
||||
Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
|
||||
EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected));
|
||||
EXPECT(assert_equal(Rot3::Roll((omega - delta) * deltaT), corrected, 1e-9));
|
||||
|
||||
// TODO(frank): again the derivative is not the *sane* one!
|
||||
// auto g = [&](const Vector3& increment) {
|
||||
// return pim.biascorrectedDeltaRij(increment, {});
|
||||
// };
|
||||
// EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(g, Z_3x1), H));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) {
|
||||
// Example where IMU is rotated, so measured omega indicates pitch.
|
||||
using namespace biased_x_rotation;
|
||||
auto p = paramsWithTransform();
|
||||
PreintegratedRotation pim(p);
|
||||
|
||||
// Check the value.
|
||||
Matrix3 H_bias;
|
||||
internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()};
|
||||
Rot3 expected = Rot3::Pitch(omega * deltaT);
|
||||
EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9));
|
||||
|
||||
// Check the derivative:
|
||||
EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias));
|
||||
|
||||
// Check value of deltaRij() after integration.
|
||||
Matrix3 F;
|
||||
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F);
|
||||
EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
|
||||
|
||||
// Check that system matrix F is the first derivative of compose:
|
||||
EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F));
|
||||
|
||||
// Make sure delRdelBiasOmega is H_bias after integration.
|
||||
EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
|
||||
|
||||
// Check the bias correction in same way, but will now yield pitch change.
|
||||
Matrix3 H;
|
||||
const double delta = 0.05;
|
||||
const Vector3 biasOmegaIncr(delta, 0, 0);
|
||||
Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
|
||||
EQUALITY(Vector3(0, -deltaT * delta, 0), expected.logmap(corrected));
|
||||
EXPECT(assert_equal(Rot3::Pitch((omega - delta) * deltaT), corrected, 1e-9));
|
||||
|
||||
// TODO(frank): again the derivative is not the *sane* one!
|
||||
// auto g = [&](const Vector3& increment) {
|
||||
// return pim.biascorrectedDeltaRij(increment, {});
|
||||
// };
|
||||
// EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(g, Z_3x1), H));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
//******************************************************************************
|
||||
Loading…
Reference in New Issue