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

Removed imu dynamics example slam namespace

release/4.3a0
Alex Cunningham 2012-07-22 18:49:07 +00:00
parent dda625b2e0
commit 46b2971e45
5 changed files with 48 additions and 210 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
gtsam_unstable/dynamics/imuSystem.cpp
View File

@ -1,63 +0,0 @@
/**
* @file ilm3DSystem.cpp
* @brief Implementations of ilm 3D domain
* @author Alex Cunningham
*/
#include <gtsam_unstable/dynamics/imuSystem.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
namespace imu {
using namespace gtsam;
/* ************************************************************************* */
void Graph::addPrior(Key key, const PoseRTV& pose, const SharedNoiseModel& noiseModel) {
add(Prior(key, pose, noiseModel));
}
/* ************************************************************************* */
void Graph::addConstraint(Key key, const PoseRTV& pose) {
add(Constraint(key, pose));
}
/* ************************************************************************* */
void Graph::addHeightPrior(Key key, double z, const SharedNoiseModel& noiseModel) {
add(DHeightPrior(key, z, noiseModel));
}
/* ************************************************************************* */
void Graph::addFullIMUMeasurement(Key key1, Key key2,
const Vector& accel, const Vector& gyro, double dt, const SharedNoiseModel& noiseModel) {
add(FullIMUMeasurement(accel, gyro, dt, key1, key2, noiseModel));
}
/* ************************************************************************* */
void Graph::addIMUMeasurement(Key key1, Key key2,
const Vector& accel, const Vector& gyro, double dt, const SharedNoiseModel& noiseModel) {
add(IMUMeasurement(accel, gyro, dt, key1, key2, noiseModel));
}
/* ************************************************************************* */
void Graph::addVelocityConstraint(Key key1, Key key2, double dt, const SharedNoiseModel& noiseModel) {
add(VelocityConstraint(key1, key2, dt, noiseModel));
}
/* ************************************************************************* */
void Graph::addBetween(Key key1, Key key2, const PoseRTV& z, const SharedNoiseModel& noiseModel) {
add(Between(key1, key2, z, noiseModel));
}
/* ************************************************************************* */
void Graph::addRange(Key key1, Key key2, double z, const SharedNoiseModel& noiseModel) {
add(Range(key1, key2, z, noiseModel));
}
/* ************************************************************************* */
Values Graph::optimize(const Values& init) const {
return LevenbergMarquardtOptimizer(*this, init).optimize();
}
/* ************************************************************************* */
} // \namespace imu

78
gtsam_unstable/dynamics/imuSystem.h
View File

@ -1,78 +0,0 @@
/**
* @file imuSystem.h
* @brief A 3D Dynamic system domain as a demonstration of IMU factors
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/PartialPriorFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam_unstable/dynamics/PoseRTV.h>
#include <gtsam_unstable/dynamics/IMUFactor.h>
#include <gtsam_unstable/dynamics/FullIMUFactor.h>
#include <gtsam_unstable/dynamics/VelocityConstraint.h>
#include <gtsam_unstable/dynamics/DynamicsPriors.h>
/**
* This domain focuses on a single class of variables: PoseRTV, which
* models a dynamic pose operating with IMU measurements and assorted priors.
*
* There are also partial priors that constraint certain components of the
* poses, as well as between and range factors to model other between-pose
* information.
*/
namespace imu {
struct Values : public gtsam::Values {
typedef gtsam::Values Base;
Values() {}
Values(const Values& values) : Base(values) {}
Values(const Base& values) : Base(values) {}
void insertPose(gtsam::Key key, const gtsam::PoseRTV& pose) { insert(key, pose); }
gtsam::PoseRTV pose(gtsam::Key key) const { return at<gtsam::PoseRTV>(key); }
};
// factors
typedef gtsam::IMUFactor<gtsam::PoseRTV> IMUMeasurement; // IMU between measurements
typedef gtsam::FullIMUFactor<gtsam::PoseRTV> FullIMUMeasurement; // Full-state IMU between measurements
typedef gtsam::BetweenFactor<gtsam::PoseRTV> Between; // full odometry (including velocity)
typedef gtsam::NonlinearEquality<gtsam::PoseRTV> Constraint;
typedef gtsam::PriorFactor<gtsam::PoseRTV> Prior;
typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> Range;
// graph components
struct Graph : public gtsam::NonlinearFactorGraph {
typedef gtsam::NonlinearFactorGraph Base;
Graph() {}
Graph(const Base& graph) : Base(graph) {}
Graph(const Graph& graph) : Base(graph) {}
// prior factors
void addPrior(size_t key, const gtsam::PoseRTV& pose, const gtsam::SharedNoiseModel& noiseModel);
void addConstraint(size_t key, const gtsam::PoseRTV& pose);
void addHeightPrior(size_t key, double z, const gtsam::SharedNoiseModel& noiseModel);
// inertial factors
void addFullIMUMeasurement(size_t key1, size_t key2, const gtsam::Vector& accel, const gtsam::Vector& gyro, double dt, const gtsam::SharedNoiseModel& noiseModel);
void addIMUMeasurement(size_t key1, size_t key2, const gtsam::Vector& accel, const gtsam::Vector& gyro, double dt, const gtsam::SharedNoiseModel& noiseModel);
void addVelocityConstraint(size_t key1, size_t key2, double dt, const gtsam::SharedNoiseModel& noiseModel);
// other measurements
void addBetween(size_t key1, size_t key2, const gtsam::PoseRTV& z, const gtsam::SharedNoiseModel& noiseModel);
void addRange(size_t key1, size_t key2, double z, const gtsam::SharedNoiseModel& noiseModel);
// optimization
Values optimize(const Values& init) const;
};
} // \namespace imu

74
gtsam_unstable/dynamics/tests/testIMUSystem.cpp
View File

@ -7,23 +7,37 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/dynamics/imuSystem.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/PartialPriorFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/EasyFactorGraph.h>
#include <gtsam_unstable/dynamics/IMUFactor.h>
#include <gtsam_unstable/dynamics/FullIMUFactor.h>
#include <gtsam_unstable/dynamics/VelocityConstraint.h>
#include <gtsam_unstable/dynamics/DynamicsPriors.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace imu;
const double tol=1e-5; const double tol=1e-5;
static const Key x0 = 0, x1 = 1, x2 = 2, x3 = 3, x4 = 4; static const Key x0 = 0, x1 = 1, x2 = 2, x3 = 3, x4 = 4;
static const Vector g = delta(3, 2, -9.81); static const Vector g = delta(3, 2, -9.81);
//typedef gtsam::IMUFactor<gtsam::PoseRTV> IMUFactor<PoseRTV>; // IMU between measurements
//typedef gtsam::FullIMUFactor<gtsam::PoseRTV> IMUFactor<PoseRTV>; // Full-state IMU between measurements
//typedef gtsam::BetweenFactor<gtsam::PoseRTV> Between; // full odometry (including velocity)
//typedef gtsam::NonlinearEquality<gtsam::PoseRTV> Constraint;
//typedef gtsam::PriorFactor<gtsam::PoseRTV> Prior;
//typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> Range;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(testIMUSystem, instantiations) { TEST(testIMUSystem, instantiations) {
// just checking for compilation // just checking for compilation
PoseRTV x1_v; PoseRTV x1_v;
imu::Values local_values;
Graph graph;
gtsam::SharedNoiseModel model1 = gtsam::noiseModel::Unit::Create(1); gtsam::SharedNoiseModel model1 = gtsam::noiseModel::Unit::Create(1);
gtsam::SharedNoiseModel model3 = gtsam::noiseModel::Unit::Create(3); gtsam::SharedNoiseModel model3 = gtsam::noiseModel::Unit::Create(3);
@ -32,13 +46,13 @@ TEST(testIMUSystem, instantiations) {
Vector accel = ones(3), gyro = ones(3); Vector accel = ones(3), gyro = ones(3);
IMUMeasurement imu(accel, gyro, 0.01, x1, x2, model6); IMUFactor<PoseRTV> imu(accel, gyro, 0.01, x1, x2, model6);
FullIMUMeasurement full_imu(accel, gyro, 0.01, x1, x2, model9); FullIMUFactor<PoseRTV> full_imu(accel, gyro, 0.01, x1, x2, model9);
Constraint poseHardPrior(x1, x1_v); NonlinearEquality<gtsam::PoseRTV> poseHardPrior(x1, x1_v);
Between odom(x1, x2, x1_v, model9); BetweenFactor<gtsam::PoseRTV> odom(x1, x2, x1_v, model9);
Range range(x1, x2, 1.0, model1); RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> range(x1, x2, 1.0, model1);
VelocityConstraint constraint(x1, x2, 0.1, 10000); VelocityConstraint constraint(x1, x2, 0.1, 10000);
Prior posePrior(x1, x1_v, model9); PriorFactor<gtsam::PoseRTV> posePrior(x1, x1_v, model9);
DHeightPrior heightPrior(x1, 0.1, model1); DHeightPrior heightPrior(x1, 0.1, model1);
VelocityPrior velPrior(x1, ones(3), model3); VelocityPrior velPrior(x1, ones(3), model3);
} }
@ -60,17 +74,17 @@ TEST( testIMUSystem, optimize_chain ) {
imu34 = pose3.imuPrediction(pose4, dt); imu34 = pose3.imuPrediction(pose4, dt);
// assemble simple graph with IMU measurements and velocity constraints // assemble simple graph with IMU measurements and velocity constraints
Graph graph; EasyFactorGraph graph;
graph.add(Constraint(x1, pose1)); graph.add(NonlinearEquality<gtsam::PoseRTV>(x1, pose1));
graph.add(IMUMeasurement(imu12, dt, x1, x2, model)); graph.add(IMUFactor<PoseRTV>(imu12, dt, x1, x2, model));
graph.add(IMUMeasurement(imu23, dt, x2, x3, model)); graph.add(IMUFactor<PoseRTV>(imu23, dt, x2, x3, model));
graph.add(IMUMeasurement(imu34, dt, x3, x4, model)); graph.add(IMUFactor<PoseRTV>(imu34, dt, x3, x4, model));
graph.add(VelocityConstraint(x1, x2, dt)); graph.add(VelocityConstraint(x1, x2, dt));
graph.add(VelocityConstraint(x2, x3, dt)); graph.add(VelocityConstraint(x2, x3, dt));
graph.add(VelocityConstraint(x3, x4, dt)); graph.add(VelocityConstraint(x3, x4, dt));
// ground truth values // ground truth values
imu::Values true_values; Values true_values;
true_values.insert(x1, pose1); true_values.insert(x1, pose1);
true_values.insert(x2, pose2); true_values.insert(x2, pose2);
true_values.insert(x3, pose3); true_values.insert(x3, pose3);
@ -80,13 +94,13 @@ TEST( testIMUSystem, optimize_chain ) {
EXPECT_DOUBLES_EQUAL(0, graph.error(true_values), 1e-5); EXPECT_DOUBLES_EQUAL(0, graph.error(true_values), 1e-5);
// initialize with zero values and optimize // initialize with zero values and optimize
imu::Values values; Values values;
values.insert(x1, PoseRTV()); values.insert(x1, PoseRTV());
values.insert(x2, PoseRTV()); values.insert(x2, PoseRTV());
values.insert(x3, PoseRTV()); values.insert(x3, PoseRTV());
values.insert(x4, PoseRTV()); values.insert(x4, PoseRTV());
imu::Values actual = graph.optimize(values); Values actual = graph.optimize(values);
EXPECT(assert_equal(true_values, actual, tol)); EXPECT(assert_equal(true_values, actual, tol));
} }
@ -107,14 +121,14 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
imu34 = pose3.imuPrediction(pose4, dt); imu34 = pose3.imuPrediction(pose4, dt);
// assemble simple graph with IMU measurements and velocity constraints // assemble simple graph with IMU measurements and velocity constraints
Graph graph; EasyFactorGraph graph;
graph.add(Constraint(x1, pose1)); graph.add(NonlinearEquality<gtsam::PoseRTV>(x1, pose1));
graph.add(FullIMUMeasurement(imu12, dt, x1, x2, model)); graph.add(FullIMUFactor<PoseRTV>(imu12, dt, x1, x2, model));
graph.add(FullIMUMeasurement(imu23, dt, x2, x3, model)); graph.add(FullIMUFactor<PoseRTV>(imu23, dt, x2, x3, model));
graph.add(FullIMUMeasurement(imu34, dt, x3, x4, model)); graph.add(FullIMUFactor<PoseRTV>(imu34, dt, x3, x4, model));
// ground truth values // ground truth values
imu::Values true_values; Values true_values;
true_values.insert(x1, pose1); true_values.insert(x1, pose1);
true_values.insert(x2, pose2); true_values.insert(x2, pose2);
true_values.insert(x3, pose3); true_values.insert(x3, pose3);
@ -124,7 +138,7 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
EXPECT_DOUBLES_EQUAL(0, graph.error(true_values), 1e-5); EXPECT_DOUBLES_EQUAL(0, graph.error(true_values), 1e-5);
// initialize with zero values and optimize // initialize with zero values and optimize
imu::Values values; Values values;
values.insert(x1, PoseRTV()); values.insert(x1, PoseRTV());
values.insert(x2, PoseRTV()); values.insert(x2, PoseRTV());
values.insert(x3, PoseRTV()); values.insert(x3, PoseRTV());
@ -132,7 +146,7 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
cout << "Initial Error: " << graph.error(values) << endl; // Initial error is 0.5 - need better prediction model cout << "Initial Error: " << graph.error(values) << endl; // Initial error is 0.5 - need better prediction model
imu::Values actual = graph.optimize(values); Values actual = graph.optimize(values);
// EXPECT(assert_equal(true_values, actual, tol)); // FAIL // EXPECT(assert_equal(true_values, actual, tol)); // FAIL
} }
@ -147,10 +161,10 @@ TEST( testIMUSystem, linear_trajectory) {
Vector gyro = delta(3, 0, 0.1); // constant rotation Vector gyro = delta(3, 0, 0.1); // constant rotation
SharedDiagonal model = noiseModel::Unit::Create(9); SharedDiagonal model = noiseModel::Unit::Create(9);
imu::Values true_traj, init_traj; Values true_traj, init_traj;
Graph graph; EasyFactorGraph graph;
graph.add(Constraint(x0, start)); graph.add(NonlinearEquality<gtsam::PoseRTV>(x0, start));
true_traj.insert(x0, start); true_traj.insert(x0, start);
init_traj.insert(x0, start); init_traj.insert(x0, start);
@ -159,7 +173,7 @@ TEST( testIMUSystem, linear_trajectory) {
for (size_t i=1; i<nrPoses; ++i) { for (size_t i=1; i<nrPoses; ++i) {
Key xA = i-1, xB = i; Key xA = i-1, xB = i;
cur_pose = cur_pose.generalDynamics(accel, gyro, dt); cur_pose = cur_pose.generalDynamics(accel, gyro, dt);
graph.add(FullIMUMeasurement(accel - g, gyro, dt, xA, xB, model)); graph.add(FullIMUFactor<PoseRTV>(accel - g, gyro, dt, xA, xB, model));
true_traj.insert(xB, cur_pose); true_traj.insert(xB, cur_pose);
init_traj.insert(xB, PoseRTV()); init_traj.insert(xB, PoseRTV());
} }

4
gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
View File

@ -4,10 +4,10 @@
*/ */
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/dynamics/imuSystem.h>
#include <gtsam_unstable/dynamics/VelocityConstraint.h>
using namespace gtsam; using namespace gtsam;
using namespace imu;
const double tol=1e-5; const double tol=1e-5;

39
gtsam_unstable/gtsam_unstable.h
View File

@ -102,7 +102,7 @@ virtual class NonlinearEquality : gtsam::NonlinearFactor {
NonlinearEquality(size_t j, const T& feasible, double error_gain); NonlinearEquality(size_t j, const T& feasible, double error_gain);
}; };
#include <gtsam_unstable/dynamics/IMUFactor.h>
template<POSE = {gtsam::PoseRTV}> template<POSE = {gtsam::PoseRTV}>
virtual class IMUFactor : gtsam::NonlinearFactor { virtual class IMUFactor : gtsam::NonlinearFactor {
/** Standard constructor */ /** Standard constructor */
@ -120,7 +120,7 @@ virtual class IMUFactor : gtsam::NonlinearFactor {
size_t key2() const; size_t key2() const;
}; };
#include <gtsam_unstable/dynamics/FullIMUFactor.h>
template<POSE = {gtsam::PoseRTV}> template<POSE = {gtsam::PoseRTV}>
virtual class FullIMUFactor : gtsam::NonlinearFactor { virtual class FullIMUFactor : gtsam::NonlinearFactor {
/** Standard constructor */ /** Standard constructor */
@ -167,38 +167,3 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
}///\namespace gtsam }///\namespace gtsam
namespace imu {
#include <gtsam_unstable/dynamics/imuSystem.h>
class Values {
Values();
void print(string s) const;
void insertPose(size_t key, const gtsam::PoseRTV& pose);
gtsam::PoseRTV pose(size_t key) const;
};
class Graph {
Graph();
void print(string s) const;
// prior factors
void addPrior(size_t key, const gtsam::PoseRTV& pose, const gtsam::noiseModel::Base* noiseModel);
void addConstraint(size_t key, const gtsam::PoseRTV& pose);
void addHeightPrior(size_t key, double z, const gtsam::noiseModel::Base* noiseModel);
// inertial factors
void addFullIMUMeasurement(size_t key1, size_t key2, const Vector& accel, const Vector& gyro, double dt, const gtsam::noiseModel::Base* noiseModel);
void addIMUMeasurement(size_t key1, size_t key2, const Vector& accel, const Vector& gyro, double dt, const gtsam::noiseModel::Base* noiseModel);
void addVelocityConstraint(size_t key1, size_t key2, double dt, const gtsam::noiseModel::Base* noiseModel);
// other measurements
void addBetween(size_t key1, size_t key2, const gtsam::PoseRTV& z, const gtsam::noiseModel::Base* noiseModel);
void addRange(size_t key1, size_t key2, double z, const gtsam::noiseModel::Base* noiseModel);
// optimization
imu::Values optimize(const imu::Values& init) const;
};
}///\namespace imu