Added AggregateReadings class and local functors.h header. Implemented the derivative of ExpmapDerivative correction.
Frank Dellaert
parent
9a26f8508e
commit
242a387ef1
|
|
@ -0,0 +1,209 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 AggregateImuReadings.cpp
|
||||
* @brief Integrates IMU readings on the NavState tangent space
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/AggregateImuReadings.h>
|
||||
#include <gtsam/navigation/functors.h>
|
||||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
|
||||
#include <boost/assign/std/list.hpp>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
using namespace boost::assign;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using symbol_shorthand::T; // for theta
|
||||
using symbol_shorthand::P; // for position
|
||||
using symbol_shorthand::V; // for velocity
|
||||
|
||||
static const Symbol kBiasKey('B', 0);
|
||||
|
||||
SharedDiagonal AggregateImuReadings::discreteAccelerometerNoiseModel(
|
||||
double dt) const {
|
||||
return noiseModel::Diagonal::Sigmas(accelerometerNoiseModel_->sigmas() /
|
||||
std::sqrt(dt));
|
||||
}
|
||||
|
||||
SharedDiagonal AggregateImuReadings::discreteGyroscopeNoiseModel(
|
||||
double dt) const {
|
||||
return noiseModel::Diagonal::Sigmas(gyroscopeNoiseModel_->sigmas() /
|
||||
std::sqrt(dt));
|
||||
}
|
||||
|
||||
NonlinearFactorGraph AggregateImuReadings::createGraph(
|
||||
const Vector3_& theta_, const Vector3_& pos_, const Vector3_& vel_,
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) const {
|
||||
NonlinearFactorGraph graph;
|
||||
Expression<Bias> bias_(kBiasKey);
|
||||
Vector3_ theta_plus_(T(k_ + 1)), pos_plus_(P(k_ + 1)), vel_plus_(V(k_ + 1));
|
||||
|
||||
Vector3_ omega_(PredictAngularVelocity(dt), theta_, theta_plus_);
|
||||
Vector3_ measuredOmega_(boost::bind(&Bias::correctGyroscope, _1, _2, _3, _4),
|
||||
bias_, omega_);
|
||||
auto gyroModel = discreteGyroscopeNoiseModel(dt);
|
||||
graph.addExpressionFactor(gyroModel, measuredOmega, measuredOmega_);
|
||||
|
||||
Vector3_ averageVelocity_(averageVelocity, vel_, vel_plus_);
|
||||
Vector3_ defect_(PositionDefect(dt), pos_, pos_plus_, averageVelocity_);
|
||||
static const auto constrModel = noiseModel::Constrained::All(3);
|
||||
static const Vector3 kZero(Vector3::Zero());
|
||||
graph.addExpressionFactor(constrModel, kZero, defect_);
|
||||
|
||||
Vector3_ acc_(PredictAcceleration(dt), vel_, vel_plus_, theta_);
|
||||
Vector3_ measuredAcc_(
|
||||
boost::bind(&Bias::correctAccelerometer, _1, _2, _3, _4), bias_, acc_);
|
||||
auto accModel = discreteAccelerometerNoiseModel(dt);
|
||||
graph.addExpressionFactor(accModel, measuredAcc, measuredAcc_);
|
||||
|
||||
return graph;
|
||||
}
|
||||
|
||||
AggregateImuReadings::SharedBayesNet AggregateImuReadings::initPosterior(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
|
||||
static const Vector3 kZero(Vector3::Zero());
|
||||
static const Vector3_ zero_(kZero);
|
||||
|
||||
// We create a factor graph and then compute P(zeta|bias)
|
||||
auto graph = createGraph(zero_, zero_, zero_, measuredAcc, measuredOmega, dt);
|
||||
|
||||
// These values are exact the first time
|
||||
values.insert<Vector3>(T(k_ + 1), measuredOmega * dt);
|
||||
values.insert<Vector3>(P(k_ + 1), measuredAcc * (0.5 * dt * dt));
|
||||
values.insert<Vector3>(V(k_ + 1), measuredAcc * dt);
|
||||
values.insert<Bias>(kBiasKey, estimatedBias_);
|
||||
auto linear_graph = graph.linearize(values);
|
||||
|
||||
// eliminate all but biases
|
||||
// NOTE(frank): After this, posterior_k_ contains P(zeta(1)|bias)
|
||||
Ordering keys = list_of(T(k_ + 1))(P(k_ + 1))(V(k_ + 1));
|
||||
return linear_graph->eliminatePartialSequential(keys, EliminateQR).first;
|
||||
}
|
||||
|
||||
AggregateImuReadings::SharedBayesNet AggregateImuReadings::integrateCorrected(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
|
||||
static const Vector3 kZero(Vector3::Zero());
|
||||
static const auto constrModel = noiseModel::Constrained::All(3);
|
||||
|
||||
// We create a factor graph and then compute P(zeta|bias)
|
||||
auto graph = createGraph(Vector3_(T(k_)), Vector3_(P(k_)), Vector3_(V(k_)),
|
||||
measuredAcc, measuredOmega, dt);
|
||||
|
||||
// Get current estimates
|
||||
const Vector3 theta = values.at<Vector3>(T(k_));
|
||||
const Vector3 pos = values.at<Vector3>(P(k_));
|
||||
const Vector3 vel = values.at<Vector3>(V(k_));
|
||||
|
||||
// Calculate exact solution: means we do not have to update values
|
||||
// TODO(frank): Expmap and ExpmapDerivative are called again :-(
|
||||
const Vector3 correctedAcc = measuredAcc - estimatedBias_.accelerometer();
|
||||
const Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope();
|
||||
Matrix3 H;
|
||||
const Rot3 R = Rot3::Expmap(theta, H);
|
||||
const Vector3 theta_plus = theta + H.inverse() * correctedOmega * dt;
|
||||
const Vector3 vel_plus = vel + R.rotate(correctedAcc) * dt;
|
||||
const Vector3 vel_avg = 0.5 * (vel + vel_plus);
|
||||
const Vector3 pos_plus = pos + vel_avg * dt;
|
||||
|
||||
// Add those values to estimate and linearize around them
|
||||
values.insert<Vector3>(T(k_ + 1), theta_plus);
|
||||
values.insert<Vector3>(P(k_ + 1), pos_plus);
|
||||
values.insert<Vector3>(V(k_ + 1), vel_plus);
|
||||
auto linear_graph = graph.linearize(values);
|
||||
|
||||
// add previous posterior
|
||||
for (const auto& conditional : *posterior_k_)
|
||||
linear_graph->add(boost::static_pointer_cast<GaussianFactor>(conditional));
|
||||
|
||||
// eliminate all but biases
|
||||
// TODO(frank): does not seem to eliminate in order I want. What gives?
|
||||
Ordering keys = list_of(T(k_))(P(k_))(V(k_))(T(k_ + 1))(P(k_ + 1))(V(k_ + 1));
|
||||
SharedBayesNet bayesNet =
|
||||
linear_graph->eliminatePartialSequential(keys, EliminateQR).first;
|
||||
|
||||
// The Bayes net now contains P(zeta(k)|zeta(k+1),bias) P(zeta(k+1)|bias)
|
||||
// We marginalize zeta(k) by removing the conditionals on zeta(k)
|
||||
// TODO(frank): could use erase(begin, begin+3) if order above was correct
|
||||
SharedBayesNet marginal = boost::make_shared<GaussianBayesNet>();
|
||||
for (const auto& conditional : *bayesNet) {
|
||||
Symbol symbol(conditional->front());
|
||||
if (symbol.index() > k_) marginal->push_back(conditional);
|
||||
}
|
||||
|
||||
return marginal;
|
||||
}
|
||||
|
||||
void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega,
|
||||
double dt) {
|
||||
typedef map<Key, Matrix> Terms;
|
||||
|
||||
// Handle first time differently
|
||||
if (k_ == 0)
|
||||
posterior_k_ = initPosterior(measuredAcc, measuredOmega, dt);
|
||||
else
|
||||
posterior_k_ = integrateCorrected(measuredAcc, measuredOmega, dt);
|
||||
|
||||
// increment counter and time
|
||||
k_ += 1;
|
||||
deltaTij_ += dt;
|
||||
}
|
||||
|
||||
NavState AggregateImuReadings::predict(const NavState& state_i,
|
||||
const Bias& bias_i,
|
||||
OptionalJacobian<9, 9> H1,
|
||||
OptionalJacobian<9, 6> H2) const {
|
||||
// TODO(frank): handle bias
|
||||
|
||||
// Get current estimates
|
||||
Vector3 theta = values.at<Vector3>(T(k_));
|
||||
Vector3 pos = values.at<Vector3>(P(k_));
|
||||
Vector3 vel = values.at<Vector3>(V(k_));
|
||||
|
||||
// Correct for initial velocity and gravity
|
||||
Rot3 Ri = state_i.attitude();
|
||||
Matrix3 Rit = Ri.transpose();
|
||||
Vector3 gt = deltaTij_ * p_->n_gravity;
|
||||
pos += Rit * (state_i.velocity() * deltaTij_ + 0.5 * deltaTij_ * gt);
|
||||
vel += Rit * gt;
|
||||
|
||||
// Convert local coordinates to manifold near state_i
|
||||
Vector9 zeta;
|
||||
zeta << theta, pos, vel;
|
||||
return state_i.retract(zeta);
|
||||
}
|
||||
|
||||
SharedGaussian AggregateImuReadings::noiseModel() const {
|
||||
Matrix RS;
|
||||
Vector d;
|
||||
boost::tie(RS, d) = posterior_k_->matrix();
|
||||
|
||||
// R'*R = A'*A = inv(Cov)
|
||||
// TODO(frank): think of a faster way - implement in noiseModel
|
||||
return noiseModel::Gaussian::SqrtInformation(RS.block<9, 9>(0, 0), false);
|
||||
}
|
||||
|
||||
Matrix9 AggregateImuReadings::preintMeasCov() const {
|
||||
return noiseModel()->covariance();
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,120 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 AggregateImuReadings.h
|
||||
* @brief Integrates IMU readings on the NavState tangent space
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class NonlinearFactorGraph;
|
||||
template <typename T>
|
||||
class Expression;
|
||||
typedef Expression<Vector3> Vector3_;
|
||||
|
||||
// Convert covariance to diagonal noise model, if possible, otherwise throw
|
||||
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
|
||||
bool smart = true;
|
||||
auto model = noiseModel::Gaussian::Covariance(covariance, smart);
|
||||
auto diagonal = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
|
||||
if (!diagonal)
|
||||
throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
|
||||
return diagonal;
|
||||
}
|
||||
|
||||
class GaussianBayesNet;
|
||||
|
||||
/**
|
||||
* Class that integrates state estimate on the manifold.
|
||||
* We integrate zeta = [theta, position, velocity]
|
||||
* See ImuFactor.lyx for the relevant math.
|
||||
*/
|
||||
class AggregateImuReadings {
|
||||
public:
|
||||
typedef imuBias::ConstantBias Bias;
|
||||
typedef ImuFactor::PreintegratedMeasurements::Params Params;
|
||||
typedef boost::shared_ptr<GaussianBayesNet> SharedBayesNet;
|
||||
|
||||
private:
|
||||
const boost::shared_ptr<Params> p_;
|
||||
const SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_;
|
||||
const Bias estimatedBias_;
|
||||
|
||||
size_t k_; ///< index/count of measurements integrated
|
||||
double deltaTij_; ///< sum of time increments
|
||||
|
||||
/// posterior on current iterate, stored as a Bayes net
|
||||
/// P(delta_zeta|estimatedBias_delta):
|
||||
SharedBayesNet posterior_k_;
|
||||
|
||||
/// Current estimate of zeta_k
|
||||
Values values;
|
||||
|
||||
public:
|
||||
AggregateImuReadings(const boost::shared_ptr<Params>& p,
|
||||
const Bias& estimatedBias = Bias())
|
||||
: p_(p),
|
||||
accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)),
|
||||
gyroscopeNoiseModel_(Diagonal(p->gyroscopeCovariance)),
|
||||
estimatedBias_(estimatedBias),
|
||||
k_(0),
|
||||
deltaTij_(0.0) {}
|
||||
|
||||
// We obtain discrete-time noise models by dividing the continuous-time
|
||||
// covariances by dt:
|
||||
|
||||
SharedDiagonal discreteAccelerometerNoiseModel(double dt) const;
|
||||
SharedDiagonal discreteGyroscopeNoiseModel(double dt) const;
|
||||
|
||||
/**
|
||||
* Add a single IMU measurement to the preintegration.
|
||||
* @param measuredAcc Measured acceleration (in body frame)
|
||||
* @param measuredOmega Measured angular velocity (in body frame)
|
||||
* @param dt Time interval between this and the last IMU measurement
|
||||
*/
|
||||
void integrateMeasurement(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, double dt);
|
||||
|
||||
/// Predict state at time j
|
||||
NavState predict(const NavState& state_i, const Bias& estimatedBias_i,
|
||||
OptionalJacobian<9, 9> H1 = boost::none,
|
||||
OptionalJacobian<9, 6> H2 = boost::none) const;
|
||||
|
||||
/// Return Gaussian noise model on prediction
|
||||
SharedGaussian noiseModel() const;
|
||||
|
||||
/// @deprecated: Explicitly calculate covariance
|
||||
Matrix9 preintMeasCov() const;
|
||||
|
||||
private:
|
||||
NonlinearFactorGraph createGraph(const Vector3_& theta_,
|
||||
const Vector3_& pose_, const Vector3_& vel_,
|
||||
const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega,
|
||||
double dt) const;
|
||||
|
||||
// initialize posterior with first (corrected) IMU measurement
|
||||
SharedBayesNet initPosterior(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, double dt);
|
||||
|
||||
// integrate
|
||||
SharedBayesNet integrateCorrected(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, double dt);
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -78,19 +78,19 @@ public:
|
|||
|
||||
/** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */
|
||||
Vector3 correctAccelerometer(const Vector3& measurement,
|
||||
OptionalJacobian<3, 6> H = boost::none) const {
|
||||
if (H) {
|
||||
(*H) << -I_3x3, Z_3x3;
|
||||
}
|
||||
OptionalJacobian<3, 6> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none) const {
|
||||
if (H1) (*H1) << -I_3x3, Z_3x3;
|
||||
if (H2) (*H2) << I_3x3;
|
||||
return measurement - biasAcc_;
|
||||
}
|
||||
|
||||
/** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
|
||||
Vector3 correctGyroscope(const Vector3& measurement,
|
||||
OptionalJacobian<3, 6> H = boost::none) const {
|
||||
if (H) {
|
||||
(*H) << Z_3x3, -I_3x3;
|
||||
}
|
||||
OptionalJacobian<3, 6> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none) const {
|
||||
if (H1) (*H1) << Z_3x3, -I_3x3;
|
||||
if (H2) (*H2) << I_3x3;
|
||||
return measurement - biasGyro_;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -16,12 +16,6 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/navigation/ScenarioRunner.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
|
||||
#include <boost/assign/std/list.hpp>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -29,194 +23,13 @@ using namespace boost::assign;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
using symbol_shorthand::T; // for theta
|
||||
using symbol_shorthand::P; // for position
|
||||
using symbol_shorthand::V; // for velocity
|
||||
|
||||
static const Symbol kBiasKey('B', 0);
|
||||
static const Matrix36 acc_H_bias = (Matrix36() << I_3x3, Z_3x3).finished();
|
||||
static const Matrix36 omega_H_bias = (Matrix36() << Z_3x3, I_3x3).finished();
|
||||
|
||||
Vector9 PreintegratedMeasurements2::currentEstimate() const {
|
||||
VectorValues biasValues;
|
||||
biasValues.insert(kBiasKey, estimatedBias_.vector());
|
||||
VectorValues zetaValues = posterior_k_->optimize(biasValues);
|
||||
Vector9 zeta;
|
||||
zeta << zetaValues.at(T(k_)), zetaValues.at(P(k_)), zetaValues.at(V(k_));
|
||||
return zeta;
|
||||
}
|
||||
|
||||
Vector3 PreintegratedMeasurements2::currentTheta() const {
|
||||
// TODO(frank): make faster version theta = inv(R)*d
|
||||
VectorValues biasValues;
|
||||
biasValues.insert(kBiasKey, estimatedBias_.vector());
|
||||
VectorValues zetaValues = posterior_k_->optimize(biasValues);
|
||||
return zetaValues.at(T(k_));
|
||||
}
|
||||
|
||||
SharedDiagonal PreintegratedMeasurements2::discreteAccelerometerNoiseModel(
|
||||
double dt) const {
|
||||
return noiseModel::Diagonal::Sigmas(accelerometerNoiseModel_->sigmas() /
|
||||
std::sqrt(dt));
|
||||
}
|
||||
|
||||
SharedDiagonal PreintegratedMeasurements2::discreteGyroscopeNoiseModel(
|
||||
double dt) const {
|
||||
return noiseModel::Diagonal::Sigmas(gyroscopeNoiseModel_->sigmas() /
|
||||
std::sqrt(dt));
|
||||
}
|
||||
|
||||
PreintegratedMeasurements2::SharedBayesNet
|
||||
PreintegratedMeasurements2::initPosterior(const Vector3& correctedAcc,
|
||||
const Vector3& correctedOmega,
|
||||
double dt) const {
|
||||
typedef map<Key, Matrix> Terms;
|
||||
|
||||
// We create a factor graph and then compute P(zeta|bias)
|
||||
GaussianFactorGraph graph;
|
||||
|
||||
// theta(1) = (correctedOmega - bias_delta) * dt
|
||||
// => theta(1)/dt + bias_delta = correctedOmega
|
||||
auto I_dt = I_3x3 / dt;
|
||||
graph.add<Terms>({{T(k_ + 1), I_dt}, {kBiasKey, omega_H_bias}},
|
||||
correctedOmega, discreteGyroscopeNoiseModel(dt));
|
||||
|
||||
// pose(1) = (correctedAcc - bias_delta) * dt22
|
||||
// => pose(1) / dt22 + bias_delta = correctedAcc
|
||||
auto accModel = discreteAccelerometerNoiseModel(dt);
|
||||
graph.add<Terms>({{P(k_ + 1), I_dt * (2.0 / dt)}, {kBiasKey, acc_H_bias}},
|
||||
correctedAcc, accModel);
|
||||
|
||||
// vel(1) = (correctedAcc - bias_delta) * dt
|
||||
// => vel(1)/dt + bias_delta = correctedAcc
|
||||
graph.add<Terms>({{V(k_ + 1), I_dt}, {kBiasKey, acc_H_bias}}, correctedAcc,
|
||||
accModel);
|
||||
|
||||
// eliminate all but biases
|
||||
// NOTE(frank): After this, posterior_k_ contains P(zeta(1)|bias)
|
||||
Ordering keys = list_of(T(k_ + 1))(P(k_ + 1))(V(k_ + 1));
|
||||
return graph.eliminatePartialSequential(keys, EliminateQR).first;
|
||||
}
|
||||
|
||||
PreintegratedMeasurements2::SharedBayesNet
|
||||
PreintegratedMeasurements2::integrateCorrected(const Vector3& correctedAcc,
|
||||
const Vector3& correctedOmega,
|
||||
double dt) const {
|
||||
typedef map<Key, Matrix> Terms;
|
||||
|
||||
GaussianFactorGraph graph;
|
||||
|
||||
// estimate current theta from posterior
|
||||
Vector3 theta_k = currentTheta();
|
||||
Rot3 Rk = Rot3::Expmap(theta_k);
|
||||
Matrix3 Rkt = Rk.transpose();
|
||||
|
||||
// add previous posterior
|
||||
for (const auto& conditional : *posterior_k_)
|
||||
graph.add(boost::static_pointer_cast<GaussianFactor>(conditional));
|
||||
|
||||
// theta(k+1) = theta(k) + inverse(H)*(correctedOmega - bias_delta) dt
|
||||
// => H*theta(k+1)/dt - H*theta(k)/dt + bias_delta = (measuredOmega - bias)
|
||||
Matrix3 H = Rot3::ExpmapDerivative(theta_k) / dt;
|
||||
graph.add<Terms>({{T(k_ + 1), H}, {T(k_), -H}, {kBiasKey, omega_H_bias}},
|
||||
correctedOmega, discreteGyroscopeNoiseModel(dt));
|
||||
|
||||
double dt22 = 0.5 * dt * dt;
|
||||
// pos(k+1) = pos(k) + vel(k) dt + Rk*(correctedAcc - bias_delta) dt22
|
||||
// => Rkt*pos(k+1)/dt22 - Rkt*pos(k)/dt22 - Rkt*vel(k) dt/dt22 + bias_delta
|
||||
// = correctedAcc
|
||||
auto accModel = discreteAccelerometerNoiseModel(dt);
|
||||
graph.add<Terms>({{P(k_ + 1), Rkt / dt22},
|
||||
{P(k_), -Rkt / dt22},
|
||||
{V(k_), -Rkt * (2.0 / dt)},
|
||||
{kBiasKey, acc_H_bias}},
|
||||
correctedAcc, accModel);
|
||||
|
||||
// vel(k+1) = vel(k) + Rk*(correctedAcc - bias_delta) dt
|
||||
// => Rkt*vel(k+1)/dt - Rkt*vel(k)/dt + bias_delta = correctedAcc
|
||||
graph.add<Terms>(
|
||||
{{V(k_ + 1), Rkt / dt}, {V(k_), -Rkt / dt}, {kBiasKey, acc_H_bias}},
|
||||
correctedAcc, accModel);
|
||||
|
||||
// eliminate all but biases
|
||||
// TODO(frank): does not seem to eliminate in order I want. What gives?
|
||||
Ordering keys = list_of(T(k_))(P(k_))(V(k_))(T(k_ + 1))(P(k_ + 1))(V(k_ + 1));
|
||||
SharedBayesNet bayesNet =
|
||||
graph.eliminatePartialSequential(keys, EliminateQR).first;
|
||||
|
||||
// The Bayes net now contains P(zeta(k)|zeta(k+1),bias) P(zeta(k+1)|bias)
|
||||
// We marginalize zeta(k) by removing the conditionals on zeta(k)
|
||||
// TODO(frank): could use erase(begin, begin+3) if order above was correct
|
||||
SharedBayesNet marginal = boost::make_shared<GaussianBayesNet>();
|
||||
for (const auto& conditional : *bayesNet) {
|
||||
Symbol symbol(conditional->front());
|
||||
if (symbol.index() > k_) marginal->push_back(conditional);
|
||||
}
|
||||
|
||||
return marginal;
|
||||
}
|
||||
|
||||
void PreintegratedMeasurements2::integrateMeasurement(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
|
||||
typedef map<Key, Matrix> Terms;
|
||||
|
||||
// Correct measurements by subtracting bias
|
||||
Vector3 correctedAcc = measuredAcc - estimatedBias_.accelerometer();
|
||||
Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope();
|
||||
|
||||
// Handle first time differently
|
||||
if (k_ == 0)
|
||||
posterior_k_ = initPosterior(correctedAcc, correctedOmega, dt);
|
||||
else
|
||||
posterior_k_ = integrateCorrected(correctedAcc, correctedOmega, dt);
|
||||
|
||||
// increment counter and time
|
||||
k_ += 1;
|
||||
deltaTij_ += dt;
|
||||
}
|
||||
|
||||
NavState PreintegratedMeasurements2::predict(
|
||||
const NavState& state_i, const imuBias::ConstantBias& bias_i,
|
||||
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const {
|
||||
// Get mean of current posterior on zeta
|
||||
// TODO(frank): handle bias
|
||||
Vector9 zeta = currentEstimate();
|
||||
|
||||
// Correct for initial velocity and gravity
|
||||
Rot3 Ri = state_i.attitude();
|
||||
Matrix3 Rit = Ri.transpose();
|
||||
Vector3 gt = deltaTij_ * p_->n_gravity;
|
||||
zeta.segment<3>(3) +=
|
||||
Rit * (state_i.velocity() * deltaTij_ + 0.5 * deltaTij_ * gt);
|
||||
zeta.segment<3>(6) += Rit * gt;
|
||||
|
||||
// Convert local coordinates to manifold near state_i
|
||||
return state_i.retract(zeta);
|
||||
}
|
||||
|
||||
SharedGaussian PreintegratedMeasurements2::noiseModel() const {
|
||||
Matrix RS;
|
||||
Vector d;
|
||||
boost::tie(RS, d) = posterior_k_->matrix();
|
||||
|
||||
// R'*R = A'*A = inv(Cov)
|
||||
// TODO(frank): think of a faster way - implement in noiseModel
|
||||
return noiseModel::Gaussian::SqrtInformation(RS.block<9, 9>(0, 0), false);
|
||||
}
|
||||
|
||||
Matrix9 PreintegratedMeasurements2::preintMeasCov() const {
|
||||
return noiseModel()->covariance();
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
static double intNoiseVar = 0.0000001;
|
||||
static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
|
||||
|
||||
PreintegratedMeasurements2 ScenarioRunner::integrate(
|
||||
double T, const imuBias::ConstantBias& estimatedBias,
|
||||
AggregateImuReadings ScenarioRunner::integrate(double T,
|
||||
const Bias& estimatedBias,
|
||||
bool corrupted) const {
|
||||
PreintegratedMeasurements2 pim(p_, estimatedBias);
|
||||
AggregateImuReadings pim(p_, estimatedBias);
|
||||
|
||||
const double dt = imuSampleTime();
|
||||
const size_t nrSteps = T / dt;
|
||||
|
|
@ -231,15 +44,14 @@ PreintegratedMeasurements2 ScenarioRunner::integrate(
|
|||
return pim;
|
||||
}
|
||||
|
||||
NavState ScenarioRunner::predict(
|
||||
const PreintegratedMeasurements2& pim,
|
||||
const imuBias::ConstantBias& estimatedBias) const {
|
||||
NavState ScenarioRunner::predict(const AggregateImuReadings& pim,
|
||||
const Bias& estimatedBias) const {
|
||||
const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0));
|
||||
return pim.predict(state_i, estimatedBias);
|
||||
}
|
||||
|
||||
Matrix9 ScenarioRunner::estimateCovariance(
|
||||
double T, size_t N, const imuBias::ConstantBias& estimatedBias) const {
|
||||
Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N,
|
||||
const Bias& estimatedBias) const {
|
||||
// Get predict prediction from ground truth measurements
|
||||
NavState prediction = predict(integrate(T));
|
||||
|
||||
|
|
|
|||
|
|
@ -16,126 +16,38 @@
|
|||
*/
|
||||
|
||||
#pragma once
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/navigation/AggregateImuReadings.h>
|
||||
#include <gtsam/navigation/Scenario.h>
|
||||
#include <gtsam/linear/Sampler.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Convert covariance to diagonal noise model, if possible, otherwise throw
|
||||
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
|
||||
bool smart = true;
|
||||
auto model = noiseModel::Gaussian::Covariance(covariance, smart);
|
||||
auto diagonal = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
|
||||
if (!diagonal)
|
||||
throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
|
||||
return diagonal;
|
||||
}
|
||||
|
||||
class GaussianBayesNet;
|
||||
|
||||
/**
|
||||
* Class that integrates state estimate on the manifold.
|
||||
* We integrate zeta = [theta, position, velocity]
|
||||
* See ImuFactor.lyx for the relevant math.
|
||||
*/
|
||||
class PreintegratedMeasurements2 {
|
||||
public:
|
||||
typedef ImuFactor::PreintegratedMeasurements::Params Params;
|
||||
typedef boost::shared_ptr<GaussianBayesNet> SharedBayesNet;
|
||||
|
||||
private:
|
||||
const boost::shared_ptr<Params> p_;
|
||||
const SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_;
|
||||
const imuBias::ConstantBias estimatedBias_;
|
||||
|
||||
size_t k_; ///< index/count of measurements integrated
|
||||
double deltaTij_; ///< sum of time increments
|
||||
|
||||
/// posterior on current iterate, stored as a Bayes net P(zeta|bias_delta):
|
||||
SharedBayesNet posterior_k_;
|
||||
|
||||
public:
|
||||
PreintegratedMeasurements2(
|
||||
const boost::shared_ptr<Params>& p,
|
||||
const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias())
|
||||
: p_(p),
|
||||
accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)),
|
||||
gyroscopeNoiseModel_(Diagonal(p->gyroscopeCovariance)),
|
||||
estimatedBias_(estimatedBias),
|
||||
k_(0),
|
||||
deltaTij_(0.0) {}
|
||||
|
||||
SharedDiagonal discreteAccelerometerNoiseModel(double dt) const;
|
||||
SharedDiagonal discreteGyroscopeNoiseModel(double dt) const;
|
||||
|
||||
/**
|
||||
* Add a single IMU measurement to the preintegration.
|
||||
* @param measuredAcc Measured acceleration (in body frame)
|
||||
* @param measuredOmega Measured angular velocity (in body frame)
|
||||
* @param dt Time interval between this and the last IMU measurement
|
||||
*/
|
||||
void integrateMeasurement(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, double dt);
|
||||
|
||||
/// Predict state at time j
|
||||
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
|
||||
OptionalJacobian<9, 9> H1 = boost::none,
|
||||
OptionalJacobian<9, 6> H2 = boost::none) const;
|
||||
|
||||
/// Return Gaussian noise model on prediction
|
||||
SharedGaussian noiseModel() const;
|
||||
|
||||
/// @deprecated: Explicitly calculate covariance
|
||||
Matrix9 preintMeasCov() const;
|
||||
|
||||
private:
|
||||
// estimate zeta given estimated biases
|
||||
// calculates conditional mean of P(zeta|bias_delta)
|
||||
Vector9 currentEstimate() const;
|
||||
|
||||
// estimate theta given estimated biases
|
||||
Vector3 currentTheta() const;
|
||||
|
||||
// We obtain discrete-time noise models by dividing the continuous-time
|
||||
// covariances by dt:
|
||||
|
||||
// initialize posterior with first (corrected) IMU measurement
|
||||
SharedBayesNet initPosterior(const Vector3& correctedAcc,
|
||||
const Vector3& correctedOmega, double dt) const;
|
||||
|
||||
// integrate
|
||||
SharedBayesNet integrateCorrected(const Vector3& correctedAcc,
|
||||
const Vector3& correctedOmega,
|
||||
double dt) const;
|
||||
};
|
||||
|
||||
/*
|
||||
* Simple class to test navigation scenarios.
|
||||
* Takes a trajectory scenario as input, and can generate IMU measurements
|
||||
*/
|
||||
class ScenarioRunner {
|
||||
public:
|
||||
typedef boost::shared_ptr<PreintegratedMeasurements2::Params> SharedParams;
|
||||
typedef imuBias::ConstantBias Bias;
|
||||
typedef boost::shared_ptr<AggregateImuReadings::Params> SharedParams;
|
||||
|
||||
private:
|
||||
const Scenario* scenario_;
|
||||
const SharedParams p_;
|
||||
const double imuSampleTime_, sqrt_dt_;
|
||||
const imuBias::ConstantBias bias_;
|
||||
const Bias estimatedBias_;
|
||||
|
||||
// Create two samplers for acceleration and omega noise
|
||||
mutable Sampler gyroSampler_, accSampler_;
|
||||
|
||||
public:
|
||||
ScenarioRunner(const Scenario* scenario, const SharedParams& p,
|
||||
double imuSampleTime = 1.0 / 100.0,
|
||||
const imuBias::ConstantBias& bias = imuBias::ConstantBias())
|
||||
double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias())
|
||||
: scenario_(scenario),
|
||||
p_(p),
|
||||
imuSampleTime_(imuSampleTime),
|
||||
sqrt_dt_(std::sqrt(imuSampleTime)),
|
||||
bias_(bias),
|
||||
estimatedBias_(bias),
|
||||
// NOTE(duy): random seeds that work well:
|
||||
gyroSampler_(Diagonal(p->gyroscopeCovariance), 10),
|
||||
accSampler_(Diagonal(p->accelerometerCovariance), 29284) {}
|
||||
|
|
@ -155,31 +67,27 @@ class ScenarioRunner {
|
|||
|
||||
// versions corrupted by bias and noise
|
||||
Vector3 measured_omega_b(double t) const {
|
||||
return actual_omega_b(t) + bias_.gyroscope() +
|
||||
return actual_omega_b(t) + estimatedBias_.gyroscope() +
|
||||
gyroSampler_.sample() / sqrt_dt_;
|
||||
}
|
||||
Vector3 measured_specific_force_b(double t) const {
|
||||
return actual_specific_force_b(t) + bias_.accelerometer() +
|
||||
return actual_specific_force_b(t) + estimatedBias_.accelerometer() +
|
||||
accSampler_.sample() / sqrt_dt_;
|
||||
}
|
||||
|
||||
const double& imuSampleTime() const { return imuSampleTime_; }
|
||||
|
||||
/// Integrate measurements for T seconds into a PIM
|
||||
PreintegratedMeasurements2 integrate(
|
||||
double T,
|
||||
const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(),
|
||||
AggregateImuReadings integrate(double T, const Bias& estimatedBias = Bias(),
|
||||
bool corrupted = false) const;
|
||||
|
||||
/// Predict predict given a PIM
|
||||
NavState predict(const PreintegratedMeasurements2& pim,
|
||||
const imuBias::ConstantBias& estimatedBias =
|
||||
imuBias::ConstantBias()) const;
|
||||
NavState predict(const AggregateImuReadings& pim,
|
||||
const Bias& estimatedBias = Bias()) const;
|
||||
|
||||
/// Compute a Monte Carlo estimate of the predict covariance using N samples
|
||||
Matrix9 estimateCovariance(double T, size_t N = 1000,
|
||||
const imuBias::ConstantBias& estimatedBias =
|
||||
imuBias::ConstantBias()) const;
|
||||
const Bias& estimatedBias = Bias()) const;
|
||||
|
||||
/// Estimate covariance of sampled noise for sanity-check
|
||||
Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,154 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 functors.h
|
||||
* @brief Functors for use in Navigation factors
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/OptionalJacobian.h>
|
||||
#include <cmath>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Implement Rot3::ExpmapDerivative(omega) * theta, with derivatives
|
||||
static Vector3 correctWithExpmapDerivative(
|
||||
const Vector3& omega, const Vector3& theta,
|
||||
OptionalJacobian<3, 3> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none) {
|
||||
using std::sin;
|
||||
const double angle2 = omega.dot(omega); // rotation angle, squared
|
||||
if (angle2 <= std::numeric_limits<double>::epsilon()) {
|
||||
if (H1) *H1 = 0.5 * skewSymmetric(theta);
|
||||
if (H2) *H2 = I_3x3;
|
||||
return theta;
|
||||
}
|
||||
|
||||
const double angle = std::sqrt(angle2); // rotation angle
|
||||
const double s1 = sin(angle) / angle;
|
||||
const double s2 = sin(angle / 2.0);
|
||||
const double a = 2.0 * s2 * s2 / angle2;
|
||||
const double b = (1.0 - s1) / angle2;
|
||||
|
||||
const Vector3 omega_x_theta = omega.cross(theta);
|
||||
const Vector3 yt = a * omega_x_theta;
|
||||
|
||||
const Matrix3 W = skewSymmetric(omega);
|
||||
const Vector3 omega_x_omega_x_theta = omega.cross(omega_x_theta);
|
||||
const Vector3 yyt = b * omega_x_omega_x_theta;
|
||||
|
||||
if (H1) {
|
||||
Matrix13 omega_t = omega.transpose();
|
||||
const Matrix3 T = skewSymmetric(theta);
|
||||
const double Da = (s1 - 2.0 * a) / angle2;
|
||||
const double Db = (3.0 * s1 - cos(angle) - 2.0) / angle2 / angle2;
|
||||
*H1 = (-Da * omega_x_theta + Db * omega_x_omega_x_theta) * omega_t + a * T -
|
||||
b * skewSymmetric(omega_x_theta) - b * W * T;
|
||||
}
|
||||
if (H2) *H2 = I_3x3 - a* W + b* W* W;
|
||||
|
||||
return theta - yt + yyt;
|
||||
}
|
||||
|
||||
// theta(k+1) = theta(k) + inverse(H)*omega dt
|
||||
// omega = (H/dt_)*(theta(k+1) - H*theta(k))
|
||||
// TODO(frank): make linear expression
|
||||
class PredictAngularVelocity {
|
||||
private:
|
||||
double dt_;
|
||||
|
||||
public:
|
||||
typedef Vector3 result_type;
|
||||
|
||||
PredictAngularVelocity(double dt) : dt_(dt) {}
|
||||
|
||||
Vector3 operator()(const Vector3& theta, const Vector3& theta_plus,
|
||||
OptionalJacobian<3, 3> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none) const {
|
||||
// TODO(frank): take into account derivative of ExpmapDerivative
|
||||
const Vector3 predicted = (theta_plus - theta) / dt_;
|
||||
Matrix3 D_c_t, D_c_p;
|
||||
const Vector3 corrected =
|
||||
correctWithExpmapDerivative(theta, predicted, D_c_t, D_c_p);
|
||||
if (H1) *H1 = D_c_t - D_c_p / dt_;
|
||||
if (H2) *H2 = D_c_p / dt_;
|
||||
return corrected;
|
||||
}
|
||||
};
|
||||
|
||||
// TODO(frank): make linear expression
|
||||
static Vector3 averageVelocity(const Vector3& vel, const Vector3& vel_plus,
|
||||
OptionalJacobian<3, 3> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none) {
|
||||
// TODO(frank): take into account derivative of ExpmapDerivative
|
||||
if (H1) *H1 = 0.5 * I_3x3;
|
||||
if (H2) *H2 = 0.5 * I_3x3;
|
||||
return 0.5 * (vel + vel_plus);
|
||||
}
|
||||
|
||||
// pos(k+1) = pos(k) + average_velocity * dt
|
||||
// TODO(frank): make linear expression
|
||||
class PositionDefect {
|
||||
private:
|
||||
double dt_;
|
||||
|
||||
public:
|
||||
typedef Vector3 result_type;
|
||||
|
||||
PositionDefect(double dt) : dt_(dt) {}
|
||||
|
||||
Vector3 operator()(const Vector3& pos, const Vector3& pos_plus,
|
||||
const Vector3& average_velocity,
|
||||
OptionalJacobian<3, 3> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none,
|
||||
OptionalJacobian<3, 3> H3 = boost::none) const {
|
||||
// TODO(frank): take into account derivative of ExpmapDerivative
|
||||
if (H1) *H1 = I_3x3;
|
||||
if (H2) *H2 = -I_3x3;
|
||||
if (H3) *H3 = I_3x3* dt_;
|
||||
return (pos + average_velocity * dt_) - pos_plus;
|
||||
}
|
||||
};
|
||||
|
||||
// vel(k+1) = vel(k) + Rk * acc * dt
|
||||
// acc = Rkt * [vel(k+1) - vel(k)]/dt
|
||||
// TODO(frank): take in Rot3
|
||||
class PredictAcceleration {
|
||||
private:
|
||||
double dt_;
|
||||
|
||||
public:
|
||||
typedef Vector3 result_type;
|
||||
|
||||
PredictAcceleration(double dt) : dt_(dt) {}
|
||||
|
||||
Vector3 operator()(const Vector3& vel, const Vector3& vel_plus,
|
||||
const Vector3& theta,
|
||||
OptionalJacobian<3, 3> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none,
|
||||
OptionalJacobian<3, 3> H3 = boost::none) const {
|
||||
Matrix3 D_R_theta;
|
||||
// TODO(frank): D_R_theta is ExpmapDerivative (computed again)
|
||||
Rot3 nRb = Rot3::Expmap(theta, D_R_theta);
|
||||
Vector3 n_acc = (vel_plus - vel) / dt_;
|
||||
Matrix3 D_b_R, D_b_n;
|
||||
Vector3 b_acc = nRb.unrotate(n_acc, D_b_R, D_b_n);
|
||||
if (H1) *H1 = -D_b_n / dt_;
|
||||
if (H2) *H2 = D_b_n / dt_;
|
||||
if (H3) *H3 = D_b_R* D_R_theta;
|
||||
return b_acc;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,165 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testInertialNavFactor.cpp
|
||||
* @brief Unit test for the InertialNavFactor
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/functors.h>
|
||||
#include <gtsam/navigation/AggregateImuReadings.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
static const double kDt = 0.1;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(AggregateImuReadings, CorrectWithExpmapDerivative1) {
|
||||
Matrix aH1, aH2;
|
||||
boost::function<Vector3(const Vector3&, const Vector3&)> f = boost::bind(
|
||||
correctWithExpmapDerivative, _1, _2, boost::none, boost::none);
|
||||
for (Vector3 omega : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) {
|
||||
for (Vector3 theta :
|
||||
{Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) {
|
||||
Vector3 expected = Rot3::ExpmapDerivative(omega) * theta;
|
||||
EXPECT(assert_equal(expected, correctWithExpmapDerivative(omega, theta)));
|
||||
EXPECT(assert_equal(expected,
|
||||
correctWithExpmapDerivative(omega, theta, aH1, aH2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(AggregateImuReadings, CorrectWithExpmapDerivative2) {
|
||||
Matrix aH1, aH2;
|
||||
boost::function<Vector3(const Vector3&, const Vector3&)> f = boost::bind(
|
||||
correctWithExpmapDerivative, _1, _2, boost::none, boost::none);
|
||||
const Vector3 omega(0, 0, 0);
|
||||
for (Vector3 theta : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) {
|
||||
Vector3 expected = Rot3::ExpmapDerivative(omega) * theta;
|
||||
EXPECT(assert_equal(expected, correctWithExpmapDerivative(omega, theta)));
|
||||
EXPECT(assert_equal(expected,
|
||||
correctWithExpmapDerivative(omega, theta, aH1, aH2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(AggregateImuReadings, CorrectWithExpmapDerivative3) {
|
||||
Matrix aH1, aH2;
|
||||
boost::function<Vector3(const Vector3&, const Vector3&)> f = boost::bind(
|
||||
correctWithExpmapDerivative, _1, _2, boost::none, boost::none);
|
||||
const Vector3 omega(0.1, 0.2, 0.3), theta(0.4, 0.3, 0.2);
|
||||
Vector3 expected = Rot3::ExpmapDerivative(omega) * theta;
|
||||
EXPECT(assert_equal(expected, correctWithExpmapDerivative(omega, theta)));
|
||||
EXPECT(assert_equal(expected,
|
||||
correctWithExpmapDerivative(omega, theta, aH1, aH2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(AggregateImuReadings, PredictAngularVelocity1) {
|
||||
Matrix aH1, aH2;
|
||||
PredictAngularVelocity functor(kDt);
|
||||
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
boost::bind(functor, _1, _2, boost::none, boost::none);
|
||||
const Vector3 theta(0, 0, 0), theta_plus(0.4, 0.3, 0.2);
|
||||
EXPECT(assert_equal(Vector3(4, 3, 2), functor(theta, theta_plus, aH1, aH2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, theta, theta_plus), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, theta, theta_plus), aH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(AggregateImuReadings, PredictAngularVelocity2) {
|
||||
Matrix aH1, aH2;
|
||||
PredictAngularVelocity functor(kDt);
|
||||
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
boost::bind(functor, _1, _2, boost::none, boost::none);
|
||||
const Vector3 theta(0.1, 0.2, 0.3), theta_plus(0.4, 0.3, 0.2);
|
||||
EXPECT(
|
||||
assert_equal(Vector3(Rot3::ExpmapDerivative(theta) * Vector3(3, 1, -1)),
|
||||
functor(theta, theta_plus, aH1, aH2), 1e-5));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, theta, theta_plus), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, theta, theta_plus), aH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(AggregateImuReadings, AverageVelocity) {
|
||||
Matrix aH1, aH2;
|
||||
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
boost::bind(averageVelocity, _1, _2, boost::none, boost::none);
|
||||
const Vector3 v(1, 2, 3), v_plus(3, 2, 1);
|
||||
EXPECT(assert_equal(Vector3(2, 2, 2), averageVelocity(v, v_plus, aH1, aH2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, v, v_plus), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, v, v_plus), aH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(AggregateImuReadings, PositionDefect) {
|
||||
Matrix aH1, aH2, aH3;
|
||||
PositionDefect functor(kDt);
|
||||
boost::function<Vector3(const Vector3&, const Vector3&, const Vector3&)> f =
|
||||
boost::bind(functor, _1, _2, _3, boost::none, boost::none, boost::none);
|
||||
const Vector3 pos(1, 2, 3), pos_plus(2, 4, 6);
|
||||
const Vector3 avg(10, 20, 30);
|
||||
EXPECT(assert_equal(Vector3(0, 0, 0),
|
||||
functor(pos, pos_plus, avg, aH1, aH2, aH3)));
|
||||
EXPECT(assert_equal(numericalDerivative31(f, pos, pos_plus, avg), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative32(f, pos, pos_plus, avg), aH2));
|
||||
EXPECT(assert_equal(numericalDerivative33(f, pos, pos_plus, avg), aH3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(AggregateImuReadings, PredictAcceleration1) {
|
||||
Matrix aH1, aH2, aH3;
|
||||
PredictAcceleration functor(kDt);
|
||||
boost::function<Vector3(const Vector3&, const Vector3&, const Vector3&)> f =
|
||||
boost::bind(functor, _1, _2, _3, boost::none, boost::none, boost::none);
|
||||
const Vector3 vel(1, 2, 3), vel_plus(2, 4, 6);
|
||||
const Vector3 theta(0, 0, 0);
|
||||
EXPECT(assert_equal(Vector3(10, 20, 30),
|
||||
functor(vel, vel_plus, theta, aH1, aH2, aH3)));
|
||||
EXPECT(assert_equal(numericalDerivative31(f, vel, vel_plus, theta), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative32(f, vel, vel_plus, theta), aH2));
|
||||
EXPECT(assert_equal(numericalDerivative33(f, vel, vel_plus, theta), aH3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(AggregateImuReadings, PredictAcceleration2) {
|
||||
Matrix aH1, aH2, aH3;
|
||||
PredictAcceleration functor(kDt);
|
||||
boost::function<Vector3(const Vector3&, const Vector3&, const Vector3&)> f =
|
||||
boost::bind(functor, _1, _2, _3, boost::none, boost::none, boost::none);
|
||||
const Vector3 vel(1, 2, 3), vel_plus(2, 4, 6);
|
||||
const Vector3 theta(0.1, 0.2, 0.3);
|
||||
EXPECT(assert_equal(Vector3(10, 20, 30),
|
||||
functor(vel, vel_plus, theta, aH1, aH2, aH3)));
|
||||
EXPECT(assert_equal(numericalDerivative31(f, vel, vel_plus, theta), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative32(f, vel, vel_plus, theta), aH2));
|
||||
EXPECT(assert_equal(numericalDerivative33(f, vel, vel_plus, theta), aH3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -16,113 +16,135 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
typedef imuBias::ConstantBias Bias;
|
||||
|
||||
Vector biasAcc1(Vector3(0.2, -0.1, 0));
|
||||
Vector biasGyro1(Vector3(0.1, -0.3, -0.2));
|
||||
imuBias::ConstantBias bias1(biasAcc1, biasGyro1);
|
||||
Bias bias1(biasAcc1, biasGyro1);
|
||||
|
||||
Vector biasAcc2(Vector3(0.1, 0.2, 0.04));
|
||||
Vector biasGyro2(Vector3(-0.002, 0.005, 0.03));
|
||||
imuBias::ConstantBias bias2(biasAcc2, biasGyro2);
|
||||
Bias bias2(biasAcc2, biasGyro2);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuBias, Constructor) {
|
||||
TEST(ImuBias, Constructor) {
|
||||
// Default Constructor
|
||||
imuBias::ConstantBias bias1;
|
||||
Bias bias1;
|
||||
|
||||
// Acc + Gyro Constructor
|
||||
imuBias::ConstantBias bias2(biasAcc2, biasGyro2);
|
||||
Bias bias2(biasAcc2, biasGyro2);
|
||||
|
||||
// Copy Constructor
|
||||
imuBias::ConstantBias bias3(bias2);
|
||||
Bias bias3(bias2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuBias, inverse) {
|
||||
imuBias::ConstantBias biasActual = bias1.inverse();
|
||||
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1,
|
||||
-biasGyro1);
|
||||
TEST(ImuBias, inverse) {
|
||||
Bias biasActual = bias1.inverse();
|
||||
Bias biasExpected = Bias(-biasAcc1, -biasGyro1);
|
||||
EXPECT(assert_equal(biasExpected, biasActual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuBias, compose) {
|
||||
imuBias::ConstantBias biasActual = bias2.compose(bias1);
|
||||
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
|
||||
biasAcc1 + biasAcc2, biasGyro1 + biasGyro2);
|
||||
TEST(ImuBias, compose) {
|
||||
Bias biasActual = bias2.compose(bias1);
|
||||
Bias biasExpected = Bias(biasAcc1 + biasAcc2, biasGyro1 + biasGyro2);
|
||||
EXPECT(assert_equal(biasExpected, biasActual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuBias, between) {
|
||||
TEST(ImuBias, between) {
|
||||
// p.between(q) == q - p
|
||||
imuBias::ConstantBias biasActual = bias2.between(bias1);
|
||||
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
|
||||
biasAcc1 - biasAcc2, biasGyro1 - biasGyro2);
|
||||
Bias biasActual = bias2.between(bias1);
|
||||
Bias biasExpected = Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2);
|
||||
EXPECT(assert_equal(biasExpected, biasActual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuBias, localCoordinates) {
|
||||
TEST(ImuBias, localCoordinates) {
|
||||
Vector deltaActual = Vector(bias2.localCoordinates(bias1));
|
||||
Vector deltaExpected = (imuBias::ConstantBias(biasAcc1 - biasAcc2,
|
||||
biasGyro1 - biasGyro2)).vector();
|
||||
Vector deltaExpected =
|
||||
(Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2)).vector();
|
||||
EXPECT(assert_equal(deltaExpected, deltaActual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuBias, retract) {
|
||||
TEST(ImuBias, retract) {
|
||||
Vector6 delta;
|
||||
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
|
||||
imuBias::ConstantBias biasActual = bias2.retract(delta);
|
||||
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
|
||||
biasAcc2 + delta.block<3, 1>(0, 0), biasGyro2 + delta.block<3, 1>(3, 0));
|
||||
Bias biasActual = bias2.retract(delta);
|
||||
Bias biasExpected = Bias(biasAcc2 + delta.block<3, 1>(0, 0),
|
||||
biasGyro2 + delta.block<3, 1>(3, 0));
|
||||
EXPECT(assert_equal(biasExpected, biasActual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuBias, Logmap) {
|
||||
TEST(ImuBias, Logmap) {
|
||||
Vector deltaActual = bias2.Logmap(bias1);
|
||||
Vector deltaExpected = bias1.vector();
|
||||
EXPECT(assert_equal(deltaExpected, deltaActual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuBias, Expmap) {
|
||||
TEST(ImuBias, Expmap) {
|
||||
Vector6 delta;
|
||||
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
|
||||
imuBias::ConstantBias biasActual = bias2.Expmap(delta);
|
||||
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta);
|
||||
Bias biasActual = bias2.Expmap(delta);
|
||||
Bias biasExpected = Bias(delta);
|
||||
EXPECT(assert_equal(biasExpected, biasActual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuBias, operatorSub) {
|
||||
imuBias::ConstantBias biasActual = -bias1;
|
||||
imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1);
|
||||
TEST(ImuBias, operatorSub) {
|
||||
Bias biasActual = -bias1;
|
||||
Bias biasExpected(-biasAcc1, -biasGyro1);
|
||||
EXPECT(assert_equal(biasExpected, biasActual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuBias, operatorAdd) {
|
||||
imuBias::ConstantBias biasActual = bias2 + bias1;
|
||||
imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1,
|
||||
biasGyro2 + biasGyro1);
|
||||
TEST(ImuBias, operatorAdd) {
|
||||
Bias biasActual = bias2 + bias1;
|
||||
Bias biasExpected(biasAcc2 + biasAcc1, biasGyro2 + biasGyro1);
|
||||
EXPECT(assert_equal(biasExpected, biasActual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuBias, operatorSubB) {
|
||||
imuBias::ConstantBias biasActual = bias2 - bias1;
|
||||
imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1,
|
||||
biasGyro2 - biasGyro1);
|
||||
TEST(ImuBias, operatorSubB) {
|
||||
Bias biasActual = bias2 - bias1;
|
||||
Bias biasExpected(biasAcc2 - biasAcc1, biasGyro2 - biasGyro1);
|
||||
EXPECT(assert_equal(biasExpected, biasActual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuBias, Correct1) {
|
||||
Matrix aH1, aH2;
|
||||
const Vector3 measurement(1, 2, 3);
|
||||
boost::function<Vector3(const Bias&, const Vector3&)> f = boost::bind(
|
||||
&Bias::correctAccelerometer, _1, _2, boost::none, boost::none);
|
||||
bias1.correctAccelerometer(measurement, aH1, aH2);
|
||||
EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuBias, Correct2) {
|
||||
Matrix aH1, aH2;
|
||||
const Vector3 measurement(1, 2, 3);
|
||||
boost::function<Vector3(const Bias&, const Vector3&)> f =
|
||||
boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none);
|
||||
bias1.correctGyroscope(measurement, aH1, aH2);
|
||||
EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@ static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
|
|||
static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);
|
||||
|
||||
// Create default parameters with Z-down and above noise parameters
|
||||
static boost::shared_ptr<PreintegratedMeasurements2::Params> defaultParams() {
|
||||
static boost::shared_ptr<AggregateImuReadings::Params> defaultParams() {
|
||||
auto p = PreintegratedImuMeasurements::Params::MakeSharedD(10);
|
||||
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
|
||||
|
|
|
|||
Loading…
Reference in New Issue