Fix merge conflicts
Ankur Roy Chowdhury
commit
8830896f01
|
|
@ -1,18 +0,0 @@
|
|||
### Script to install Boost
|
||||
BOOST_FOLDER=boost_${BOOST_VERSION//./_}
|
||||
|
||||
# Download Boost
|
||||
wget https://boostorg.jfrog.io/artifactory/main/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz
|
||||
|
||||
# Unzip
|
||||
tar -zxf ${BOOST_FOLDER}.tar.gz
|
||||
|
||||
# Bootstrap
|
||||
cd ${BOOST_FOLDER}/
|
||||
./bootstrap.sh --with-libraries=serialization,filesystem,thread,system,atomic,date_time,timer,chrono,program_options,regex
|
||||
|
||||
# Build and install
|
||||
sudo ./b2 -j$(nproc) install
|
||||
|
||||
# Rebuild ld cache
|
||||
sudo ldconfig
|
||||
|
|
@ -58,8 +58,10 @@ function configure()
|
|||
fi
|
||||
|
||||
# GTSAM_BUILD_WITH_MARCH_NATIVE=OFF: to avoid crashes in builder VMs
|
||||
# CMAKE_CXX_FLAGS="-w": Suppress warnings to avoid IO latency in CI logs
|
||||
cmake $SOURCE_DIR \
|
||||
-DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \
|
||||
-DCMAKE_CXX_FLAGS="-w" \
|
||||
-DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
|
||||
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
|
||||
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
|
||||
|
|
@ -70,11 +72,9 @@ function configure()
|
|||
-DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \
|
||||
-DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \
|
||||
-DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \
|
||||
-DGTSAM_FORCE_SHARED_LIB=${GTSAM_FORCE_SHARED_LIB:-OFF} \
|
||||
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
|
||||
-DGTSAM_SINGLE_TEST_EXE=OFF \
|
||||
-DBOOST_ROOT=$BOOST_ROOT \
|
||||
-DBoost_NO_SYSTEM_PATHS=ON \
|
||||
-DBoost_ARCHITECTURE=-x64
|
||||
-DGTSAM_SINGLE_TEST_EXE=OFF
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -97,7 +97,7 @@ function build ()
|
|||
|
||||
if [ "$(uname)" == "Linux" ]; then
|
||||
if (($(nproc) > 2)); then
|
||||
make -j$(nproc)
|
||||
make -j4
|
||||
else
|
||||
make -j2
|
||||
fi
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
name: Linux CI
|
||||
|
||||
on: [push, pull_request]
|
||||
on: [pull_request]
|
||||
|
||||
jobs:
|
||||
build:
|
||||
|
|
@ -12,7 +12,6 @@ jobs:
|
|||
CTEST_PARALLEL_LEVEL: 2
|
||||
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
|
||||
GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
|
||||
BOOST_VERSION: 1.67.0
|
||||
|
||||
strategy:
|
||||
fail-fast: true
|
||||
|
|
@ -82,7 +81,7 @@ jobs:
|
|||
|
||||
- name: Install Boost
|
||||
run: |
|
||||
bash .github/scripts/boost.sh
|
||||
sudo apt-get -y install libboost-all-dev
|
||||
|
||||
- name: Build and Test
|
||||
run: bash .github/scripts/unix.sh -t
|
||||
|
|
|
|||
|
|
@ -53,6 +53,7 @@ jobs:
|
|||
steps:
|
||||
- name: Checkout
|
||||
uses: actions/checkout@v3
|
||||
|
||||
- name: Install (Linux)
|
||||
if: runner.os == 'Linux'
|
||||
run: |
|
||||
|
|
@ -79,6 +80,7 @@ jobs:
|
|||
echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
|
||||
echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
|
||||
fi
|
||||
|
||||
- name: Install (macOS)
|
||||
if: runner.os == 'macOS'
|
||||
run: |
|
||||
|
|
@ -88,22 +90,27 @@ jobs:
|
|||
sudo xcode-select -switch /Applications/Xcode.app
|
||||
echo "CC=clang" >> $GITHUB_ENV
|
||||
echo "CXX=clang++" >> $GITHUB_ENV
|
||||
|
||||
- name: Set GTSAM_WITH_TBB Flag
|
||||
if: matrix.flag == 'tbb'
|
||||
run: |
|
||||
echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV
|
||||
echo "GTSAM Uses TBB"
|
||||
|
||||
- name: Set Swap Space
|
||||
if: runner.os == 'Linux'
|
||||
uses: pierotofy/set-swap-space@master
|
||||
with:
|
||||
swap-size-gb: 6
|
||||
|
||||
- name: Install Dependencies
|
||||
run: |
|
||||
bash .github/scripts/python.sh -d
|
||||
|
||||
- name: Build
|
||||
run: |
|
||||
bash .github/scripts/python.sh -b
|
||||
|
||||
- name: Test
|
||||
run: |
|
||||
bash .github/scripts/python.sh -t
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@ jobs:
|
|||
CTEST_PARALLEL_LEVEL: 2
|
||||
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
|
||||
GTSAM_BUILD_UNSTABLE: ON
|
||||
BOOST_VERSION: 1.67.0
|
||||
GTSAM_FORCE_SHARED_LIB: ON # Make shared library to save memory on CI
|
||||
|
||||
strategy:
|
||||
fail-fast: false
|
||||
|
|
@ -100,14 +100,20 @@ jobs:
|
|||
|
||||
sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev
|
||||
|
||||
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
|
||||
echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
|
||||
echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
|
||||
if [ "${{ matrix.compiler }}" = "gcc" ]; then
|
||||
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
|
||||
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
|
||||
echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
|
||||
else
|
||||
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
|
||||
echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
|
||||
echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
|
||||
fi
|
||||
|
||||
- name: Install Boost
|
||||
if: runner.os == 'Linux'
|
||||
run: |
|
||||
bash .github/scripts/boost.sh
|
||||
sudo apt-get -y install libboost-all-dev
|
||||
|
||||
- name: Install (macOS)
|
||||
if: runner.os == 'macOS'
|
||||
|
|
@ -164,6 +170,13 @@ jobs:
|
|||
echo "GTSAM_BUILD_UNSTABLE=OFF" >> $GITHUB_ENV
|
||||
echo "GTSAM 'unstable' will not be built."
|
||||
|
||||
- name: Set Swap Space
|
||||
if: runner.os == 'Linux'
|
||||
uses: pierotofy/set-swap-space@master
|
||||
with:
|
||||
swap-size-gb: 12
|
||||
|
||||
|
||||
- name: Build & Test
|
||||
run: |
|
||||
bash .github/scripts/unix.sh -t
|
||||
|
|
|
|||
|
|
@ -5,6 +5,7 @@
|
|||
\textclass article
|
||||
\begin_preamble
|
||||
\usepackage{color}
|
||||
\usepackage{listings}
|
||||
|
||||
\definecolor{mygreen}{rgb}{0,0.6,0}
|
||||
\definecolor{mygray}{rgb}{0.5,0.5,0.5}
|
||||
|
|
|
|||
|
|
@ -143,6 +143,9 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
|
|||
*/
|
||||
double error(const HybridValues &values) const override;
|
||||
|
||||
/// Getter for GaussianFactor decision tree
|
||||
const Factors &factors() const { return factors_; }
|
||||
|
||||
/// Add MixtureFactor to a Sum, syntactic sugar.
|
||||
friend GaussianFactorGraphTree &operator+=(
|
||||
GaussianFactorGraphTree &sum, const GaussianMixtureFactor &factor) {
|
||||
|
|
|
|||
|
|
@ -190,8 +190,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
|
|||
/* ************************************************************************ */
|
||||
// If any GaussianFactorGraph in the decision tree contains a nullptr, convert
|
||||
// that leaf to an empty GaussianFactorGraph. Needed since the DecisionTree will
|
||||
// otherwise create a GFG with a single (null) factor.
|
||||
// TODO(dellaert): still a mystery to me why this is needed.
|
||||
// otherwise create a GFG with a single (null) factor, which doesn't register as null.
|
||||
GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) {
|
||||
auto emptyGaussian = [](const GaussianFactorGraph &graph) {
|
||||
bool hasNull =
|
||||
|
|
@ -275,9 +274,9 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
|
|||
};
|
||||
|
||||
DecisionTree<Key, double> probabilities(eliminationResults, probability);
|
||||
return {std::make_shared<HybridConditional>(gaussianMixture),
|
||||
std::make_shared<DecisionTreeFactor>(discreteSeparator,
|
||||
probabilities)};
|
||||
return {
|
||||
std::make_shared<HybridConditional>(gaussianMixture),
|
||||
std::make_shared<DecisionTreeFactor>(discreteSeparator, probabilities)};
|
||||
} else {
|
||||
// Otherwise, we create a resulting GaussianMixtureFactor on the separator,
|
||||
// taking care to correct for conditional constant.
|
||||
|
|
|
|||
|
|
@ -46,7 +46,7 @@ Ordering HybridSmoother::getOrdering(
|
|||
std::copy(allDiscrete.begin(), allDiscrete.end(),
|
||||
std::back_inserter(newKeysDiscreteLast));
|
||||
|
||||
const VariableIndex index(newFactors);
|
||||
const VariableIndex index(factors);
|
||||
|
||||
// Get an ordering where the new keys are eliminated last
|
||||
Ordering ordering = Ordering::ColamdConstrainedLast(
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@
|
|||
|
||||
#include <gtsam/discrete/DiscreteBayesNet.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearISAM.h>
|
||||
|
|
@ -35,14 +36,15 @@
|
|||
// Include for test suite
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "Switching.h"
|
||||
|
||||
#include <bitset>
|
||||
|
||||
#include "Switching.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::Z;
|
||||
|
||||
Ordering getOrdering(HybridGaussianFactorGraph& factors,
|
||||
const HybridGaussianFactorGraph& newFactors) {
|
||||
|
|
@ -91,8 +93,7 @@ TEST(HybridEstimation, Full) {
|
|||
hybridOrdering.push_back(M(k));
|
||||
}
|
||||
|
||||
HybridBayesNet::shared_ptr bayesNet =
|
||||
graph.eliminateSequential();
|
||||
HybridBayesNet::shared_ptr bayesNet = graph.eliminateSequential();
|
||||
|
||||
EXPECT_LONGS_EQUAL(2 * K - 1, bayesNet->size());
|
||||
|
||||
|
|
@ -338,7 +339,6 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
|
|||
Switching switching(K, between_sigma, measurement_sigma, measurements,
|
||||
"1/1 1/1");
|
||||
auto graph = switching.linearizedFactorGraph;
|
||||
Ordering ordering = getOrdering(graph, HybridGaussianFactorGraph());
|
||||
|
||||
// Get the tree of unnormalized probabilities for each mode sequence.
|
||||
AlgebraicDecisionTree<Key> expected_probPrimeTree = getProbPrimeTree(graph);
|
||||
|
|
@ -503,6 +503,179 @@ TEST(HybridEstimation, CorrectnessViaSampling) {
|
|||
}
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
/**
|
||||
* Helper function to add the constant term corresponding to
|
||||
* the difference in noise models.
|
||||
*/
|
||||
std::shared_ptr<GaussianMixtureFactor> mixedVarianceFactor(
|
||||
const MixtureFactor& mf, const Values& initial, const Key& mode,
|
||||
double noise_tight, double noise_loose, size_t d, size_t tight_index) {
|
||||
GaussianMixtureFactor::shared_ptr gmf = mf.linearize(initial);
|
||||
|
||||
constexpr double log2pi = 1.8378770664093454835606594728112;
|
||||
// logConstant will be of the tighter model
|
||||
double logNormalizationConstant = log(1.0 / noise_tight);
|
||||
double logConstant = -0.5 * d * log2pi + logNormalizationConstant;
|
||||
|
||||
auto func = [&](const Assignment<Key>& assignment,
|
||||
const GaussianFactor::shared_ptr& gf) {
|
||||
if (assignment.at(mode) != tight_index) {
|
||||
double factor_log_constant = -0.5 * d * log2pi + log(1.0 / noise_loose);
|
||||
|
||||
GaussianFactorGraph _gfg;
|
||||
_gfg.push_back(gf);
|
||||
Vector c(d);
|
||||
for (size_t i = 0; i < d; i++) {
|
||||
c(i) = std::sqrt(2.0 * (logConstant - factor_log_constant));
|
||||
}
|
||||
|
||||
_gfg.emplace_shared<JacobianFactor>(c);
|
||||
return std::make_shared<JacobianFactor>(_gfg);
|
||||
} else {
|
||||
return dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
}
|
||||
};
|
||||
auto updated_components = gmf->factors().apply(func);
|
||||
auto updated_gmf = std::make_shared<GaussianMixtureFactor>(
|
||||
gmf->continuousKeys(), gmf->discreteKeys(), updated_components);
|
||||
|
||||
return updated_gmf;
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
TEST(HybridEstimation, ModeSelection) {
|
||||
HybridNonlinearFactorGraph graph;
|
||||
Values initial;
|
||||
|
||||
auto measurement_model = noiseModel::Isotropic::Sigma(1, 0.1);
|
||||
auto motion_model = noiseModel::Isotropic::Sigma(1, 1.0);
|
||||
|
||||
graph.emplace_shared<PriorFactor<double>>(X(0), 0.0, measurement_model);
|
||||
graph.emplace_shared<PriorFactor<double>>(X(1), 0.0, measurement_model);
|
||||
|
||||
DiscreteKeys modes;
|
||||
modes.emplace_back(M(0), 2);
|
||||
|
||||
// The size of the noise model
|
||||
size_t d = 1;
|
||||
double noise_tight = 0.5, noise_loose = 5.0;
|
||||
|
||||
auto model0 = std::make_shared<MotionModel>(
|
||||
X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_loose)),
|
||||
model1 = std::make_shared<MotionModel>(
|
||||
X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
|
||||
|
||||
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
|
||||
|
||||
KeyVector keys = {X(0), X(1)};
|
||||
MixtureFactor mf(keys, modes, components);
|
||||
|
||||
initial.insert(X(0), 0.0);
|
||||
initial.insert(X(1), 0.0);
|
||||
|
||||
auto gmf =
|
||||
mixedVarianceFactor(mf, initial, M(0), noise_tight, noise_loose, d, 1);
|
||||
graph.add(gmf);
|
||||
|
||||
auto gfg = graph.linearize(initial);
|
||||
|
||||
HybridBayesNet::shared_ptr bayesNet = gfg->eliminateSequential();
|
||||
|
||||
HybridValues delta = bayesNet->optimize();
|
||||
EXPECT_LONGS_EQUAL(1, delta.discrete().at(M(0)));
|
||||
|
||||
/**************************************************************/
|
||||
HybridBayesNet bn;
|
||||
const DiscreteKey mode{M(0), 2};
|
||||
|
||||
bn.push_back(
|
||||
GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(0), Z_1x1, 0.1));
|
||||
bn.push_back(
|
||||
GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(1), Z_1x1, 0.1));
|
||||
bn.emplace_back(new GaussianMixture(
|
||||
{Z(0)}, {X(0), X(1)}, {mode},
|
||||
{GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1),
|
||||
Z_1x1, noise_loose),
|
||||
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1),
|
||||
Z_1x1, noise_tight)}));
|
||||
|
||||
VectorValues vv;
|
||||
vv.insert(Z(0), Z_1x1);
|
||||
|
||||
auto fg = bn.toFactorGraph(vv);
|
||||
auto expected_posterior = fg.eliminateSequential();
|
||||
|
||||
EXPECT(assert_equal(*expected_posterior, *bayesNet, 1e-6));
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
TEST(HybridEstimation, ModeSelection2) {
|
||||
using symbol_shorthand::Z;
|
||||
|
||||
// The size of the noise model
|
||||
size_t d = 3;
|
||||
double noise_tight = 0.5, noise_loose = 5.0;
|
||||
|
||||
HybridBayesNet bn;
|
||||
const DiscreteKey mode{M(0), 2};
|
||||
|
||||
bn.push_back(
|
||||
GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(0), Z_3x1, 0.1));
|
||||
bn.push_back(
|
||||
GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(1), Z_3x1, 0.1));
|
||||
bn.emplace_back(new GaussianMixture(
|
||||
{Z(0)}, {X(0), X(1)}, {mode},
|
||||
{GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1),
|
||||
Z_3x1, noise_loose),
|
||||
GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1),
|
||||
Z_3x1, noise_tight)}));
|
||||
|
||||
VectorValues vv;
|
||||
vv.insert(Z(0), Z_3x1);
|
||||
|
||||
auto fg = bn.toFactorGraph(vv);
|
||||
|
||||
auto expected_posterior = fg.eliminateSequential();
|
||||
|
||||
// =====================================
|
||||
|
||||
HybridNonlinearFactorGraph graph;
|
||||
Values initial;
|
||||
|
||||
auto measurement_model = noiseModel::Isotropic::Sigma(d, 0.1);
|
||||
auto motion_model = noiseModel::Isotropic::Sigma(d, 1.0);
|
||||
|
||||
graph.emplace_shared<PriorFactor<Vector3>>(X(0), Z_3x1, measurement_model);
|
||||
graph.emplace_shared<PriorFactor<Vector3>>(X(1), Z_3x1, measurement_model);
|
||||
|
||||
DiscreteKeys modes;
|
||||
modes.emplace_back(M(0), 2);
|
||||
|
||||
auto model0 = std::make_shared<BetweenFactor<Vector3>>(
|
||||
X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_loose)),
|
||||
model1 = std::make_shared<BetweenFactor<Vector3>>(
|
||||
X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
|
||||
|
||||
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
|
||||
|
||||
KeyVector keys = {X(0), X(1)};
|
||||
MixtureFactor mf(keys, modes, components);
|
||||
|
||||
initial.insert<Vector3>(X(0), Z_3x1);
|
||||
initial.insert<Vector3>(X(1), Z_3x1);
|
||||
|
||||
auto gmf =
|
||||
mixedVarianceFactor(mf, initial, M(0), noise_tight, noise_loose, d, 1);
|
||||
graph.add(gmf);
|
||||
|
||||
auto gfg = graph.linearize(initial);
|
||||
|
||||
HybridBayesNet::shared_ptr bayesNet = gfg->eliminateSequential();
|
||||
|
||||
EXPECT(assert_equal(*expected_posterior, *bayesNet, 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -131,7 +131,7 @@ namespace gtsam {
|
|||
* term, and f the constant term.
|
||||
* JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f]
|
||||
* HessianFactor error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx - x'*g + 0.5*f \f]
|
||||
* So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have
|
||||
* So, with \f$ A = [A1 A2] \f$ and \f$ G=A'*M*A = [A1';A2']*M*[A1 A2] \f$ we have
|
||||
\code
|
||||
n1*n1 G11 = A1'*M*A1
|
||||
n1*n2 G12 = A1'*M*A2
|
||||
|
|
|
|||
|
|
@ -74,10 +74,20 @@ bool PreintegratedCombinedMeasurements::equals(
|
|||
|
||||
//------------------------------------------------------------------------------
|
||||
void PreintegratedCombinedMeasurements::resetIntegration() {
|
||||
// Base class method to reset the preintegrated measurements
|
||||
PreintegrationType::resetIntegration();
|
||||
preintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void PreintegratedCombinedMeasurements::resetIntegration(
|
||||
const gtsam::Matrix6& Q_init) {
|
||||
// Base class method to reset the preintegrated measurements
|
||||
PreintegrationType::resetIntegration();
|
||||
p().biasAccOmegaInt = Q_init;
|
||||
preintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// sugar for derivative blocks
|
||||
#define D_R_R(H) (H)->block<3,3>(0,0)
|
||||
|
|
@ -112,21 +122,21 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
|
|||
// and preintegrated measurements
|
||||
|
||||
// Single Jacobians to propagate covariance
|
||||
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_omega = C.topRows<3>();
|
||||
Matrix3 pos_H_acc = B.middleRows<3>(3);
|
||||
Matrix3 vel_H_acc = B.bottomRows<3>();
|
||||
|
||||
Matrix3 theta_H_biasOmegaInit = -theta_H_biasOmega;
|
||||
Matrix3 pos_H_biasAccInit = -pos_H_biasAcc;
|
||||
Matrix3 vel_H_biasAccInit = -vel_H_biasAcc;
|
||||
Matrix3 theta_H_biasOmegaInit = -theta_H_omega;
|
||||
Matrix3 pos_H_biasAccInit = -pos_H_acc;
|
||||
Matrix3 vel_H_biasAccInit = -vel_H_acc;
|
||||
|
||||
// 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<3, 3>(0, 12) = theta_H_omega;
|
||||
F.block<3, 3>(3, 9) = pos_H_acc;
|
||||
F.block<3, 3>(6, 9) = vel_H_acc;
|
||||
F.block<6, 6>(9, 9) = I_6x6;
|
||||
|
||||
// Update the uncertainty on the state (matrix F in [4]).
|
||||
|
|
@ -151,17 +161,17 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
|
|||
|
||||
// BLOCK DIAGONAL TERMS
|
||||
D_R_R(&G_measCov_Gt) =
|
||||
(theta_H_biasOmega * (wCov / dt) * theta_H_biasOmega.transpose()) //
|
||||
(theta_H_omega * (wCov / dt) * theta_H_omega.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_acc * (aCov / dt) * pos_H_acc.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_acc * (aCov / dt) * vel_H_acc.transpose()) //
|
||||
+ (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
|
||||
|
||||
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
|
||||
|
|
@ -175,12 +185,12 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
|
|||
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_acc * (aCov / dt) * vel_H_acc.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_acc * (aCov / dt) * pos_H_acc.transpose()) +
|
||||
(vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose());
|
||||
|
||||
preintMeasCov_.noalias() += G_measCov_Gt;
|
||||
|
|
|
|||
|
|
@ -23,10 +23,7 @@
|
|||
#pragma once
|
||||
|
||||
/* GTSAM includes */
|
||||
#include <gtsam/navigation/ManifoldPreintegration.h>
|
||||
#include <gtsam/navigation/TangentPreintegration.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/navigation/PreintegrationCombinedParams.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -56,67 +53,6 @@ typedef ManifoldPreintegration PreintegrationType;
|
|||
* Robotics: Science and Systems (RSS), 2015.
|
||||
*/
|
||||
|
||||
/// Parameters for pre-integration using PreintegratedCombinedMeasurements:
|
||||
/// Usage: Create just a single Params and pass a shared pointer to the constructor
|
||||
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 as initial estimate.
|
||||
|
||||
/// Default constructor makes uninitialized params struct.
|
||||
/// Used for serialization.
|
||||
PreintegrationCombinedParams()
|
||||
: biasAccCovariance(I_3x3),
|
||||
biasOmegaCovariance(I_3x3),
|
||||
biasAccOmegaInt(I_6x6) {}
|
||||
|
||||
/// See two named constructors below for good values of n_gravity in body frame
|
||||
PreintegrationCombinedParams(const Vector3& _n_gravity) :
|
||||
PreintegrationParams(_n_gravity), biasAccCovariance(I_3x3),
|
||||
biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
|
||||
|
||||
}
|
||||
|
||||
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
|
||||
static std::shared_ptr<PreintegrationCombinedParams> MakeSharedD(double g = 9.81) {
|
||||
return std::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, g)));
|
||||
}
|
||||
|
||||
// Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
|
||||
static std::shared_ptr<PreintegrationCombinedParams> MakeSharedU(double g = 9.81) {
|
||||
return std::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
|
||||
}
|
||||
|
||||
void print(const std::string& s="") const override;
|
||||
bool equals(const PreintegratedRotationParams& other, double tol) const override;
|
||||
|
||||
void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
|
||||
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
|
||||
void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; }
|
||||
|
||||
const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
|
||||
const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
|
||||
const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
|
||||
|
||||
private:
|
||||
|
||||
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
namespace bs = ::boost::serialization;
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
|
||||
ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
|
||||
ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
|
||||
ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
|
||||
}
|
||||
#endif
|
||||
|
||||
public:
|
||||
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
/**
|
||||
* PreintegratedCombinedMeasurements integrates the IMU measurements
|
||||
* (rotation rates and accelerations) and the corresponding covariance matrix.
|
||||
|
|
@ -127,17 +63,17 @@ public:
|
|||
*
|
||||
* @ingroup navigation
|
||||
*/
|
||||
class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType {
|
||||
|
||||
public:
|
||||
class GTSAM_EXPORT PreintegratedCombinedMeasurements
|
||||
: public PreintegrationType {
|
||||
public:
|
||||
typedef PreintegrationCombinedParams Params;
|
||||
|
||||
protected:
|
||||
/* Covariance matrix of the preintegrated measurements
|
||||
* COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc BiasOmega]
|
||||
* (first-order propagation from *measurementCovariance*).
|
||||
* PreintegratedCombinedMeasurements also include the biases and keep the correlation
|
||||
* between the preintegrated measurements and the biases
|
||||
* COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc
|
||||
* BiasOmega] (first-order propagation from *measurementCovariance*).
|
||||
* PreintegratedCombinedMeasurements also include the biases and keep the
|
||||
* correlation between the preintegrated measurements and the biases
|
||||
*/
|
||||
Eigen::Matrix<double, 15, 15> preintMeasCov_;
|
||||
|
||||
|
|
@ -148,31 +84,31 @@ public:
|
|||
/// @{
|
||||
|
||||
/// Default constructor only for serialization and wrappers
|
||||
PreintegratedCombinedMeasurements() {
|
||||
preintMeasCov_.setZero();
|
||||
}
|
||||
PreintegratedCombinedMeasurements() { preintMeasCov_.setZero(); }
|
||||
|
||||
/**
|
||||
* Default constructor, initializes the class with no measurements
|
||||
* @param p Parameters, typically fixed in a single application
|
||||
* @param p Parameters, typically fixed in a single application
|
||||
* @param biasHat Current estimate of acceleration and rotation rate biases
|
||||
* @param preintMeasCov Covariance matrix used in noise model.
|
||||
*/
|
||||
PreintegratedCombinedMeasurements(
|
||||
const std::shared_ptr<Params>& p,
|
||||
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
|
||||
: PreintegrationType(p, biasHat) {
|
||||
preintMeasCov_.setZero();
|
||||
}
|
||||
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias(),
|
||||
const Eigen::Matrix<double, 15, 15>& preintMeasCov =
|
||||
Eigen::Matrix<double, 15, 15>::Zero())
|
||||
: PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) {}
|
||||
|
||||
/**
|
||||
* Construct preintegrated directly from members: base class and preintMeasCov
|
||||
* @param base PreintegrationType instance
|
||||
* @param preintMeasCov Covariance matrix used in noise model.
|
||||
*/
|
||||
PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix<double, 15, 15>& preintMeasCov)
|
||||
: PreintegrationType(base),
|
||||
preintMeasCov_(preintMeasCov) {
|
||||
}
|
||||
* Construct preintegrated directly from members: base class and
|
||||
* preintMeasCov
|
||||
* @param base PreintegrationType instance
|
||||
* @param preintMeasCov Covariance matrix used in noise model.
|
||||
*/
|
||||
PreintegratedCombinedMeasurements(
|
||||
const PreintegrationType& base,
|
||||
const Eigen::Matrix<double, 15, 15>& preintMeasCov)
|
||||
: PreintegrationType(base), preintMeasCov_(preintMeasCov) {}
|
||||
|
||||
/// Virtual destructor
|
||||
~PreintegratedCombinedMeasurements() override {}
|
||||
|
|
@ -185,6 +121,14 @@ public:
|
|||
/// Re-initialize PreintegratedCombinedMeasurements
|
||||
void resetIntegration() override;
|
||||
|
||||
/**
|
||||
* @brief Re-initialize PreintegratedCombinedMeasurements with initial bias
|
||||
* covariance estimate.
|
||||
*
|
||||
* @param Q_init The initial bias covariance estimates as a 6x6 matrix.
|
||||
*/
|
||||
void resetIntegration(const gtsam::Matrix6& Q_init);
|
||||
|
||||
/// const reference to params, shadows definition in base class
|
||||
Params& p() const { return *std::static_pointer_cast<Params>(this->p_); }
|
||||
/// @}
|
||||
|
|
@ -198,13 +142,13 @@ public:
|
|||
/// @name Testable
|
||||
/// @{
|
||||
/// print
|
||||
void print(const std::string& s = "Preintegrated Measurements:") const override;
|
||||
void print(
|
||||
const std::string& s = "Preintegrated Measurements:") const override;
|
||||
/// equals
|
||||
bool equals(const PreintegratedCombinedMeasurements& expected,
|
||||
double tol = 1e-9) const;
|
||||
/// @}
|
||||
|
||||
|
||||
/// @name Main functionality
|
||||
/// @{
|
||||
|
||||
|
|
@ -219,7 +163,8 @@ public:
|
|||
* @param dt Time interval between two consecutive IMU measurements
|
||||
*/
|
||||
void integrateMeasurement(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, const double dt) override;
|
||||
const Vector3& measuredOmega,
|
||||
const double dt) override;
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
@ -235,7 +180,7 @@ public:
|
|||
}
|
||||
#endif
|
||||
|
||||
public:
|
||||
public:
|
||||
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
|
|
@ -244,34 +189,32 @@ public:
|
|||
* velocity of the vehicle, as well as bias at previous time step), and current
|
||||
* state (pose, velocity, bias at current time step). Following the pre-
|
||||
* integration scheme proposed in [2], the CombinedImuFactor includes many IMU
|
||||
* measurements, which are "summarized" using the PreintegratedCombinedMeasurements
|
||||
* class. There are 3 main differences wrpt the ImuFactor class:
|
||||
* 1) The factor is 6-ways, meaning that it also involves both biases (previous
|
||||
* and current time step).Therefore, the factor internally imposes the biases
|
||||
* to be slowly varying; in particular, the matrices "biasAccCovariance" and
|
||||
* "biasOmegaCovariance" described the random walk that models bias evolution.
|
||||
* 2) The preintegration covariance takes into account the noise in the bias
|
||||
* estimate used for integration.
|
||||
* 3) The covariance matrix of the PreintegratedCombinedMeasurements preserves
|
||||
* the correlation between the bias uncertainty and the preintegrated
|
||||
* measurements uncertainty.
|
||||
* measurements, which are "summarized" using the
|
||||
* PreintegratedCombinedMeasurements class. There are 3 main differences wrpt
|
||||
* the ImuFactor class: 1) The factor is 6-ways, meaning that it also involves
|
||||
* both biases (previous and current time step).Therefore, the factor internally
|
||||
* imposes the biases to be slowly varying; in particular, the matrices
|
||||
* "biasAccCovariance" and "biasOmegaCovariance" described the random walk that
|
||||
* models bias evolution. 2) The preintegration covariance takes into account
|
||||
* the noise in the bias estimate used for integration. 3) The covariance matrix
|
||||
* of the PreintegratedCombinedMeasurements preserves the correlation between
|
||||
* the bias uncertainty and the preintegrated measurements uncertainty.
|
||||
*
|
||||
* @ingroup navigation
|
||||
*/
|
||||
class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3,
|
||||
Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
|
||||
public:
|
||||
|
||||
private:
|
||||
|
||||
class GTSAM_EXPORT CombinedImuFactor
|
||||
: public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
|
||||
imuBias::ConstantBias, imuBias::ConstantBias> {
|
||||
public:
|
||||
private:
|
||||
typedef CombinedImuFactor This;
|
||||
typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
|
||||
imuBias::ConstantBias, imuBias::ConstantBias> Base;
|
||||
imuBias::ConstantBias, imuBias::ConstantBias>
|
||||
Base;
|
||||
|
||||
PreintegratedCombinedMeasurements _PIM_;
|
||||
|
||||
public:
|
||||
|
||||
public:
|
||||
// Provide access to Matrix& version of evaluateError:
|
||||
using Base::evaluateError;
|
||||
|
||||
|
|
@ -315,7 +258,8 @@ public:
|
|||
DefaultKeyFormatter) const override;
|
||||
|
||||
/// equals
|
||||
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
|
||||
bool equals(const NonlinearFactor& expected,
|
||||
double tol = 1e-9) const override;
|
||||
/// @}
|
||||
|
||||
/** Access the preintegrated measurements. */
|
||||
|
|
@ -328,11 +272,13 @@ public:
|
|||
|
||||
/// vector of errors
|
||||
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
|
||||
OptionalMatrixType H1, OptionalMatrixType H2,
|
||||
OptionalMatrixType H3, OptionalMatrixType H4,
|
||||
OptionalMatrixType H5, OptionalMatrixType H6) const override;
|
||||
const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias_i,
|
||||
const imuBias::ConstantBias& bias_j,
|
||||
OptionalMatrixType H1, OptionalMatrixType H2,
|
||||
OptionalMatrixType H3, OptionalMatrixType H4,
|
||||
OptionalMatrixType H5,
|
||||
OptionalMatrixType H6) const override;
|
||||
|
||||
private:
|
||||
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
|
|
@ -347,7 +293,7 @@ public:
|
|||
}
|
||||
#endif
|
||||
|
||||
public:
|
||||
public:
|
||||
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
// class CombinedImuFactor
|
||||
|
|
|
|||
|
|
@ -0,0 +1,106 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 PreintegrationCombinedParams.h
|
||||
* @author Luca Carlone
|
||||
* @author Stephen Williams
|
||||
* @author Richard Roberts
|
||||
* @author Vadim Indelman
|
||||
* @author David Jensen
|
||||
* @author Frank Dellaert
|
||||
* @author Varun Agrawal
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
/* GTSAM includes */
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/serialization.h>
|
||||
#include <gtsam/navigation/ManifoldPreintegration.h>
|
||||
#include <gtsam/navigation/PreintegrationCombinedParams.h>
|
||||
#include <gtsam/navigation/TangentPreintegration.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// Parameters for pre-integration using PreintegratedCombinedMeasurements:
|
||||
/// Usage: Create just a single Params and pass a shared pointer to the
|
||||
/// constructor
|
||||
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 as initial estimate.
|
||||
|
||||
/// Default constructor makes uninitialized params struct.
|
||||
/// Used for serialization.
|
||||
PreintegrationCombinedParams()
|
||||
: biasAccCovariance(I_3x3),
|
||||
biasOmegaCovariance(I_3x3),
|
||||
biasAccOmegaInt(I_6x6) {}
|
||||
|
||||
/// See two named constructors below for good values of n_gravity in body
|
||||
/// frame
|
||||
PreintegrationCombinedParams(const Vector3& n_gravity)
|
||||
: PreintegrationParams(n_gravity),
|
||||
biasAccCovariance(I_3x3),
|
||||
biasOmegaCovariance(I_3x3),
|
||||
biasAccOmegaInt(I_6x6) {}
|
||||
|
||||
// Default Params for a Z-down navigation frame, such as NED: gravity points
|
||||
// along positive Z-axis
|
||||
static std::shared_ptr<PreintegrationCombinedParams> MakeSharedD(
|
||||
double g = 9.81) {
|
||||
return std::shared_ptr<PreintegrationCombinedParams>(
|
||||
new PreintegrationCombinedParams(Vector3(0, 0, g)));
|
||||
}
|
||||
|
||||
// Default Params for a Z-up navigation frame, such as ENU: gravity points
|
||||
// along negative Z-axis
|
||||
static std::shared_ptr<PreintegrationCombinedParams> MakeSharedU(
|
||||
double g = 9.81) {
|
||||
return std::shared_ptr<PreintegrationCombinedParams>(
|
||||
new PreintegrationCombinedParams(Vector3(0, 0, -g)));
|
||||
}
|
||||
|
||||
void print(const std::string& s = "") const override;
|
||||
bool equals(const PreintegratedRotationParams& other,
|
||||
double tol) const override;
|
||||
|
||||
void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance = cov; }
|
||||
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance = cov; }
|
||||
void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt = cov; }
|
||||
|
||||
const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
|
||||
const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
|
||||
const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
|
||||
|
||||
private:
|
||||
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
namespace bs = ::boost::serialization;
|
||||
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
|
||||
ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
|
||||
ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
|
||||
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
|
||||
}
|
||||
#endif
|
||||
|
||||
public:
|
||||
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -106,7 +106,7 @@ Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
|
|||
PreintegratedCombinedMeasurements CombinedScenarioRunner::integrate(
|
||||
double T, const Bias& estimatedBias, bool corrupted) const {
|
||||
gttic_(integrate);
|
||||
PreintegratedCombinedMeasurements pim(p_, estimatedBias);
|
||||
PreintegratedCombinedMeasurements pim(p_, estimatedBias, preintMeasCov_);
|
||||
|
||||
const double dt = imuSampleTime();
|
||||
const size_t nrSteps = T / dt;
|
||||
|
|
|
|||
|
|
@ -118,15 +118,19 @@ class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
|
|||
private:
|
||||
const SharedParams p_;
|
||||
const Bias estimatedBias_;
|
||||
const Eigen::Matrix<double, 15, 15> preintMeasCov_;
|
||||
|
||||
public:
|
||||
CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
|
||||
double imuSampleTime = 1.0 / 100.0,
|
||||
const Bias& bias = Bias())
|
||||
const Bias& bias = Bias(),
|
||||
const Eigen::Matrix<double, 15, 15>& preintMeasCov =
|
||||
Eigen::Matrix<double, 15, 15>::Zero())
|
||||
: ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
|
||||
imuSampleTime, bias),
|
||||
p_(p),
|
||||
estimatedBias_(bias) {}
|
||||
estimatedBias_(bias),
|
||||
preintMeasCov_(preintMeasCov) {}
|
||||
|
||||
/// Integrate measurements for T seconds into a PIM
|
||||
PreintegratedCombinedMeasurements integrate(
|
||||
|
|
|
|||
|
|
@ -36,14 +36,17 @@
|
|||
|
||||
namespace testing {
|
||||
// Create default parameters with Z-down and above noise parameters
|
||||
static std::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
|
||||
static std::shared_ptr<PreintegratedCombinedMeasurements::Params> Params(
|
||||
const Matrix3& biasAccCovariance = Matrix3::Zero(),
|
||||
const Matrix3& biasOmegaCovariance = Matrix3::Zero(),
|
||||
const Matrix6& biasAccOmegaInt = Matrix6::Zero()) {
|
||||
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity);
|
||||
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;
|
||||
p->biasAccCovariance = biasAccCovariance;
|
||||
p->biasOmegaCovariance = biasOmegaCovariance;
|
||||
p->biasAccOmegaInt = biasAccOmegaInt;
|
||||
return p;
|
||||
}
|
||||
} // namespace testing
|
||||
|
|
@ -250,6 +253,7 @@ TEST(CombinedImuFactor, CheckCovariance) {
|
|||
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) {
|
||||
|
|
@ -316,6 +320,43 @@ TEST(CombinedImuFactor, Accelerating) {
|
|||
EXPECT(assert_equal(estimatedCov, expected, 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(CombinedImuFactor, ResetIntegration) {
|
||||
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
|
||||
|
||||
auto preinMeasCov = 0.001 * Eigen::Matrix<double, 15, 15>::Identity();
|
||||
CombinedScenarioRunner runner(
|
||||
scenario,
|
||||
testing::Params(Matrix3::Zero(), Matrix3::Zero(),
|
||||
0.1 * Matrix6::Identity()),
|
||||
T / 10, imuBias::ConstantBias(), preinMeasCov);
|
||||
|
||||
PreintegratedCombinedMeasurements pim = runner.integrate(T);
|
||||
// Make copy for testing different conditions
|
||||
PreintegratedCombinedMeasurements pim2 = pim;
|
||||
|
||||
// Test default method
|
||||
pim.resetIntegration();
|
||||
Matrix6 expected = 0.1 * I_6x6;
|
||||
EXPECT(assert_equal(expected, pim.p().biasAccOmegaInt, 1e-9));
|
||||
|
||||
// Test method where Q_init is provided
|
||||
Matrix6 expected_Q_init = I_6x6 * 0.001;
|
||||
pim2.resetIntegration(expected_Q_init);
|
||||
EXPECT(assert_equal(expected_Q_init, pim.p().biasAccOmegaInt, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,256 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
A script validating and demonstrating inference with the CombinedImuFactor.
|
||||
|
||||
Author: Varun Agrawal
|
||||
"""
|
||||
|
||||
# pylint: disable=no-name-in-module,unused-import,arguments-differ,import-error,wrong-import-order
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import argparse
|
||||
import math
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from gtsam.symbol_shorthand import B, V, X
|
||||
from gtsam.utils.plot import plot_pose3
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
from PreintegrationExample import POSES_FIG, PreintegrationExample
|
||||
|
||||
import gtsam
|
||||
|
||||
GRAVITY = 9.81
|
||||
|
||||
np.set_printoptions(precision=3, suppress=True)
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
"""Parse command line arguments."""
|
||||
parser = argparse.ArgumentParser("CombinedImuFactorExample.py")
|
||||
parser.add_argument("--twist_scenario",
|
||||
default="sick_twist",
|
||||
choices=("zero_twist", "forward_twist", "loop_twist",
|
||||
"sick_twist"))
|
||||
parser.add_argument("--time",
|
||||
"-T",
|
||||
default=12,
|
||||
type=int,
|
||||
help="Total navigation time in seconds")
|
||||
parser.add_argument("--compute_covariances",
|
||||
default=False,
|
||||
action='store_true')
|
||||
parser.add_argument("--verbose", default=False, action='store_true')
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
class CombinedImuFactorExample(PreintegrationExample):
|
||||
"""Class to run example of the Imu Factor."""
|
||||
def __init__(self, twist_scenario: str = "sick_twist"):
|
||||
self.velocity = np.array([2, 0, 0])
|
||||
self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
|
||||
self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
|
||||
self.biasNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.001)
|
||||
|
||||
# Choose one of these twists to change scenario:
|
||||
twist_scenarios = dict(
|
||||
zero_twist=(np.zeros(3), np.zeros(3)),
|
||||
forward_twist=(np.zeros(3), self.velocity),
|
||||
loop_twist=(np.array([0, -math.radians(30), 0]), self.velocity),
|
||||
sick_twist=(np.array([math.radians(30), -math.radians(30),
|
||||
0]), self.velocity))
|
||||
|
||||
accBias = np.array([-0.3, 0.1, 0.2])
|
||||
gyroBias = np.array([0.1, 0.3, -0.1])
|
||||
bias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
|
||||
|
||||
params = gtsam.PreintegrationCombinedParams.MakeSharedU(GRAVITY)
|
||||
|
||||
# Some arbitrary noise sigmas
|
||||
gyro_sigma = 1e-3
|
||||
accel_sigma = 1e-3
|
||||
I_3x3 = np.eye(3)
|
||||
params.setGyroscopeCovariance(gyro_sigma**2 * I_3x3)
|
||||
params.setAccelerometerCovariance(accel_sigma**2 * I_3x3)
|
||||
params.setIntegrationCovariance(1e-7**2 * I_3x3)
|
||||
|
||||
dt = 1e-2
|
||||
super(CombinedImuFactorExample,
|
||||
self).__init__(twist_scenarios[twist_scenario], bias, params, dt)
|
||||
|
||||
def addPrior(self, i: int, graph: gtsam.NonlinearFactorGraph):
|
||||
"""Add a prior on the navigation state at time `i`."""
|
||||
state = self.scenario.navState(i)
|
||||
graph.push_back(
|
||||
gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
|
||||
graph.push_back(
|
||||
gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise))
|
||||
graph.push_back(
|
||||
gtsam.PriorFactorConstantBias(B(i), self.actualBias,
|
||||
self.biasNoise))
|
||||
|
||||
def optimize(self, graph: gtsam.NonlinearFactorGraph,
|
||||
initial: gtsam.Values):
|
||||
"""Optimize using Levenberg-Marquardt optimization."""
|
||||
params = gtsam.LevenbergMarquardtParams()
|
||||
params.setVerbosityLM("SUMMARY")
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
|
||||
result = optimizer.optimize()
|
||||
return result
|
||||
|
||||
def plot(self,
|
||||
values: gtsam.Values,
|
||||
title: str = "Estimated Trajectory",
|
||||
fignum: int = POSES_FIG + 1,
|
||||
show: bool = False):
|
||||
"""
|
||||
Plot poses in values.
|
||||
|
||||
Args:
|
||||
values: The values object with the poses to plot.
|
||||
title: The title of the plot.
|
||||
fignum: The matplotlib figure number.
|
||||
POSES_FIG is a value from the PreintegrationExample
|
||||
which we simply increment to generate a new figure.
|
||||
show: Flag indicating whether to display the figure.
|
||||
"""
|
||||
i = 0
|
||||
while values.exists(X(i)):
|
||||
pose_i = values.atPose3(X(i))
|
||||
plot_pose3(fignum, pose_i, 1)
|
||||
i += 1
|
||||
plt.title(title)
|
||||
|
||||
gtsam.utils.plot.set_axes_equal(fignum)
|
||||
|
||||
i = 0
|
||||
while values.exists(B(i)):
|
||||
print("Bias Value {0}".format(i), values.atConstantBias(B(i)))
|
||||
i += 1
|
||||
|
||||
plt.ioff()
|
||||
|
||||
if show:
|
||||
plt.show()
|
||||
|
||||
def run(self,
|
||||
T: int = 12,
|
||||
compute_covariances: bool = False,
|
||||
verbose: bool = True):
|
||||
"""
|
||||
Main runner.
|
||||
|
||||
Args:
|
||||
T: Total trajectory time.
|
||||
compute_covariances: Flag indicating whether to compute marginal covariances.
|
||||
verbose: Flag indicating if printing should be verbose.
|
||||
"""
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# initialize data structure for pre-integrated IMU measurements
|
||||
pim = gtsam.PreintegratedCombinedMeasurements(self.params,
|
||||
self.actualBias)
|
||||
|
||||
num_poses = T # assumes 1 factor per second
|
||||
initial = gtsam.Values()
|
||||
|
||||
# simulate the loop
|
||||
i = 0 # state index
|
||||
initial_state_i = self.scenario.navState(0)
|
||||
initial.insert(X(i), initial_state_i.pose())
|
||||
initial.insert(V(i), initial_state_i.velocity())
|
||||
initial.insert(B(i), self.actualBias)
|
||||
|
||||
# add prior on beginning
|
||||
self.addPrior(0, graph)
|
||||
|
||||
for k, t in enumerate(np.arange(0, T, self.dt)):
|
||||
# get measurements and add them to PIM
|
||||
measuredOmega = self.runner.measuredAngularVelocity(t)
|
||||
measuredAcc = self.runner.measuredSpecificForce(t)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
|
||||
|
||||
# Plot IMU many times
|
||||
if k % 10 == 0:
|
||||
self.plotImu(t, measuredOmega, measuredAcc)
|
||||
|
||||
if (k + 1) % int(1 / self.dt) == 0:
|
||||
# Plot every second
|
||||
self.plotGroundTruthPose(t, scale=1)
|
||||
plt.title("Ground Truth Trajectory")
|
||||
|
||||
# create IMU factor every second
|
||||
factor = gtsam.CombinedImuFactor(X(i), V(i), X(i + 1),
|
||||
V(i + 1), B(i), B(i + 1), pim)
|
||||
graph.push_back(factor)
|
||||
|
||||
if verbose:
|
||||
print(factor)
|
||||
print("Predicted state at {0}:\n{1}".format(
|
||||
t + self.dt,
|
||||
pim.predict(initial_state_i, self.actualBias)))
|
||||
|
||||
pim.resetIntegration()
|
||||
|
||||
rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1)
|
||||
translationNoise = gtsam.Point3(*np.random.randn(3) * 1)
|
||||
poseNoise = gtsam.Pose3(rotationNoise, translationNoise)
|
||||
|
||||
actual_state_i = self.scenario.navState(t + self.dt)
|
||||
print("Actual state at {0}:\n{1}".format(
|
||||
t + self.dt, actual_state_i))
|
||||
|
||||
# Set initial state to current
|
||||
initial_state_i = actual_state_i
|
||||
|
||||
noisy_state_i = gtsam.NavState(
|
||||
actual_state_i.pose().compose(poseNoise),
|
||||
actual_state_i.velocity() + np.random.randn(3) * 0.1)
|
||||
noisy_bias_i = self.actualBias + gtsam.imuBias.ConstantBias(
|
||||
np.random.randn(3) * 0.1,
|
||||
np.random.randn(3) * 0.1)
|
||||
|
||||
initial.insert(X(i + 1), noisy_state_i.pose())
|
||||
initial.insert(V(i + 1), noisy_state_i.velocity())
|
||||
initial.insert(B(i + 1), noisy_bias_i)
|
||||
i += 1
|
||||
|
||||
# add priors on end
|
||||
self.addPrior(num_poses - 1, graph)
|
||||
|
||||
initial.print("Initial values:")
|
||||
|
||||
result = self.optimize(graph, initial)
|
||||
|
||||
result.print("Optimized values:")
|
||||
print("------------------")
|
||||
print("Initial Error =", graph.error(initial))
|
||||
print("Final Error =", graph.error(result))
|
||||
print("------------------")
|
||||
|
||||
if compute_covariances:
|
||||
# Calculate and print marginal covariances
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
print("Covariance on bias:\n",
|
||||
marginals.marginalCovariance(BIAS_KEY))
|
||||
for i in range(num_poses):
|
||||
print("Covariance on pose {}:\n{}\n".format(
|
||||
i, marginals.marginalCovariance(X(i))))
|
||||
print("Covariance on vel {}:\n{}\n".format(
|
||||
i, marginals.marginalCovariance(V(i))))
|
||||
|
||||
self.plot(result, show=True)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
args = parse_args()
|
||||
|
||||
CombinedImuFactorExample(args.twist_scenario).run(args.time,
|
||||
args.compute_covariances,
|
||||
args.verbose)
|
||||
|
|
@ -17,15 +17,15 @@ from __future__ import print_function
|
|||
import argparse
|
||||
import math
|
||||
|
||||
import gtsam
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from gtsam.symbol_shorthand import B, V, X
|
||||
from gtsam.utils.plot import plot_pose3
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
from PreintegrationExample import POSES_FIG, PreintegrationExample
|
||||
|
||||
import gtsam
|
||||
|
||||
BIAS_KEY = B(0)
|
||||
GRAVITY = 9.81
|
||||
|
||||
|
|
@ -185,7 +185,9 @@ class ImuFactorExample(PreintegrationExample):
|
|||
|
||||
if verbose:
|
||||
print(factor)
|
||||
print(pim.predict(initial_state_i, self.actualBias))
|
||||
print("Predicted state at {0}:\n{1}".format(
|
||||
t + self.dt,
|
||||
pim.predict(initial_state_i, self.actualBias)))
|
||||
|
||||
pim.resetIntegration()
|
||||
|
||||
|
|
@ -197,6 +199,9 @@ class ImuFactorExample(PreintegrationExample):
|
|||
print("Actual state at {0}:\n{1}".format(
|
||||
t + self.dt, actual_state_i))
|
||||
|
||||
# Set initial state to current
|
||||
initial_state_i = actual_state_i
|
||||
|
||||
noisy_state_i = gtsam.NavState(
|
||||
actual_state_i.pose().compose(poseNoise),
|
||||
actual_state_i.velocity() + np.random.randn(3) * 0.1)
|
||||
|
|
|
|||
Loading…
Reference in New Issue