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

Developed linear factor graph algorithm

release/4.3a0
Frank Dellaert 2015-12-28 09:40:05 -08:00
parent 874d492318
commit 68b6d31494
3 changed files with 234 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
gtsam/navigation/ScenarioRunner.cpp
View File

@ -16,15 +16,73 @@
*/
#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;
using namespace boost::assign;
namespace gtsam {
////////////////////////////////////////////////////////////////////////////////////////////
using symbol_shorthand::T;
using symbol_shorthand::P;
using symbol_shorthand::V;
static const Symbol kBiasKey('B', 0);
void PreintegratedMeasurements2::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {}
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
typedef map<Key, Matrix> Terms;
static const Matrix36 omega_D_bias = (Matrix36() << Z_3x3, I_3x3).finished();
// Correct measurements by subtractig bias
Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope();
GaussianFactorGraph graph;
boost::shared_ptr<GaussianBayesNet> bayesNet;
GaussianFactorGraph::shared_ptr shouldBeEmpty;
// Handle first time differently
if (k_ == 0) {
// theta(1) = measuredOmega - (bias + bias_delta)
graph.add<Terms>({{T(k_ + 1), I_3x3}, {kBiasKey, omega_D_bias}},
correctedOmega, gyroscopeNoiseModel_);
GTSAM_PRINT(graph);
// eliminate all but biases
Ordering keys = list_of(T(k_ + 1));
boost::tie(bayesNet, shouldBeEmpty) =
graph.eliminatePartialSequential(keys, EliminateQR);
} else {
// add previous posterior
graph.add(posterior_k_);
// theta(k+1) = theta(k) + inverse(H)*(measuredOmega - (bias + bias_delta))
// => H*theta(k+1) - H*theta(k) + bias_delta = measuredOmega - bias
Matrix3 H = Rot3::ExpmapDerivative(theta_);
graph.add<Terms>({{T(k_ + 1), H}, {T(k_), -H}, {kBiasKey, omega_D_bias}},
correctedOmega, gyroscopeNoiseModel_);
GTSAM_PRINT(graph);
// eliminate all but biases
Ordering keys = list_of(T(k_))(T(k_ + 1));
boost::tie(bayesNet, shouldBeEmpty) =
graph.eliminatePartialSequential(keys, EliminateQR);
}
GTSAM_PRINT(*bayesNet);
GTSAM_PRINT(*shouldBeEmpty);
// The bayesNet now contains P(zeta(k)|zeta(k+1),bias) P(zeta(k+1)|bias)
// We marginalize zeta(k) by dropping the first factor
posterior_k_ = bayesNet->back();
k_ += 1;
}
NavState PreintegratedMeasurements2::predict(
const NavState& state_i, const imuBias::ConstantBias& bias_i,

57
gtsam/navigation/ScenarioRunner.h
View File

@ -22,10 +22,29 @@
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 that integrates on the manifold
* Class that integrates state estimate on the manifold.
* We integrate zeta = [theta, position, velocity]
* See ImuFactor.lyx for the relevant math.
*/
class PreintegratedMeasurements2 {
private:
SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_;
const imuBias::ConstantBias estimatedBias_;
size_t k_; ///< index/count of measurements integrated
Vector3 theta_; ///< current estimate for theta
GaussianFactor::shared_ptr posterior_k_; ///< posterior on current iterate
public:
typedef ImuFactor::PreintegratedMeasurements::Params Params;
@ -33,7 +52,11 @@ class PreintegratedMeasurements2 {
PreintegratedMeasurements2(
const boost::shared_ptr<Params>& p,
const gtsam::imuBias::ConstantBias& estimatedBias) {}
const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias())
: accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)),
gyroscopeNoiseModel_(Diagonal(p->gyroscopeCovariance)),
estimatedBias_(estimatedBias),
k_(0) {}
/**
* Add a single IMU measurement to the preintegration.
@ -57,6 +80,17 @@ class PreintegratedMeasurements2 {
class ScenarioRunner {
public:
typedef boost::shared_ptr<PreintegratedMeasurements2::Params> SharedParams;
private:
const Scenario* scenario_;
const SharedParams p_;
const double imuSampleTime_;
const imuBias::ConstantBias bias_;
// 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())
@ -117,25 +151,6 @@ class ScenarioRunner {
Matrix6 estimatePoseCovariance(double T, size_t N = 1000,
const imuBias::ConstantBias& estimatedBias =
imuBias::ConstantBias()) const;
private:
// 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;
}
const Scenario* scenario_;
const SharedParams p_;
const double imuSampleTime_;
const imuBias::ConstantBias bias_;
// Create two samplers for acceleration and omega noise
mutable Sampler gyroSampler_, accSampler_;
};
} // namespace gtsam

249
gtsam/navigation/tests/testScenarioRunner.cpp
View File

@ -40,128 +40,155 @@ static boost::shared_ptr<PreintegratedMeasurements2::Params> defaultParams() {
}
/* ************************************************************************* */
namespace forward {
const double v = 2; // m/s
ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
}
/* ************************************************************************* */
TEST(ScenarioRunner, Forward) {
using namespace forward;
TEST(ScenarioRunner, Spin) {
// angular velocity 6 kDegree/sec
const double w = 6 * kDegree;
const Vector3 W(0, 0, w), V(0, 0, 0);
const ExpmapScenario scenario(W, V);
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
}
/* ************************************************************************* */
TEST(ScenarioRunner, ForwardWithBias) {
using namespace forward;
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
auto pim = runner.integrate(T, kNonZeroBias);
Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
}
/* ************************************************************************* */
TEST(ScenarioRunner, Circle) {
// Forward velocity 2m/s, angular velocity 6 kDegree/sec
const double v = 2, w = 6 * kDegree;
ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
}
/* ************************************************************************* */
TEST(ScenarioRunner, Loop) {
// Forward velocity 2m/s
// Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
const double v = 2, w = 6 * kDegree;
ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
}
/* ************************************************************************* */
namespace initial {
// 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 P0(10, 20, 0);
const Vector3 V0(50, 0, 0);
}
/* ************************************************************************* */
namespace accelerating {
using namespace initial;
const double a = 0.2; // m/s^2
const Vector3 A(0, a, 0);
const AcceleratingScenario scenario(nRb, P0, V0, A);
const double T = 3; // seconds
}
/* ************************************************************************* */
TEST(ScenarioRunner, Accelerating) {
using namespace accelerating;
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
}
/* ************************************************************************* */
// TODO(frank):Fails !
// TEST(ScenarioRunner, AcceleratingWithBias) {
// using namespace accelerating;
// ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma,
// kNonZeroBias);
///* *************************************************************************
///*/
// namespace forward {
// const double v = 2; // m/s
// ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
//}
///* *************************************************************************
///*/
// TEST(ScenarioRunner, Forward) {
// using namespace forward;
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
// const double T = 0.1; // seconds
//
// auto pim = runner.integrate(T,
// kNonZeroBias);
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000,
// kNonZeroBias);
// auto pim = runner.integrate(T);
// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
//
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
//}
//
///* *************************************************************************
///*/
// TEST(ScenarioRunner, ForwardWithBias) {
// using namespace forward;
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
// const double T = 0.1; // seconds
//
// auto pim = runner.integrate(T, kNonZeroBias);
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
//}
//
///* *************************************************************************
///*/
// TEST(ScenarioRunner, Circle) {
// // Forward velocity 2m/s, angular velocity 6 kDegree/sec
// const double v = 2, w = 6 * kDegree;
// ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
//
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
// const double T = 0.1; // seconds
//
// auto pim = runner.integrate(T);
// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
//
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
//}
//
///* *************************************************************************
///*/
// TEST(ScenarioRunner, Loop) {
// // Forward velocity 2m/s
// // Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
// const double v = 2, w = 6 * kDegree;
// ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
//
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
// const double T = 0.1; // seconds
//
// auto pim = runner.integrate(T);
// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
//
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
//}
//
///* *************************************************************************
///*/
// namespace initial {
//// 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 P0(10, 20, 0);
// const Vector3 V0(50, 0, 0);
//}
//
///* *************************************************************************
///*/
// namespace accelerating {
// using namespace initial;
// const double a = 0.2; // m/s^2
// const Vector3 A(0, a, 0);
// const AcceleratingScenario scenario(nRb, P0, V0, A);
//
// const double T = 3; // seconds
//}
//
///* *************************************************************************
///*/
// TEST(ScenarioRunner, Accelerating) {
// using namespace accelerating;
// ScenarioRunner runner(&scenario, defaultParams(), T / 10);
//
// auto pim = runner.integrate(T);
// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
//
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
//}
//
///* *************************************************************************
///*/
//// TODO(frank):Fails !
//// TEST(ScenarioRunner, AcceleratingWithBias) {
//// using namespace accelerating;
//// ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma,
//// kNonZeroBias);
////
//// auto pim = runner.integrate(T,
//// kNonZeroBias);
//// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000,
//// kNonZeroBias);
//// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
////}
//
///* *************************************************************************
///*/
// TEST(ScenarioRunner, AcceleratingAndRotating) {
// using namespace initial;
// const double a = 0.2; // m/s^2
// const Vector3 A(0, a, 0), W(0, 0.1, 0);
// const AcceleratingScenario scenario(nRb, P0, V0, A, W);
//
// const double T = 3; // seconds
// ScenarioRunner runner(&scenario, defaultParams(), T / 10);
//
// auto pim = runner.integrate(T);
// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
//
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
//}
/* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingAndRotating) {
using namespace initial;
const double a = 0.2; // m/s^2
const Vector3 A(0, a, 0), W(0, 0.1, 0);
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
const double T = 3; // seconds
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
}
/* ************************************************************************* */
int main() {