From 46b2971e459d78f89ef2652cdac3735d761a24f4 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sun, 22 Jul 2012 18:49:07 +0000 Subject: [PATCH] Removed imu dynamics example slam namespace --- gtsam_unstable/dynamics/imuSystem.cpp | 63 --------------- gtsam_unstable/dynamics/imuSystem.h | 78 ------------------- .../dynamics/tests/testIMUSystem.cpp | 74 +++++++++++------- .../dynamics/tests/testVelocityConstraint.cpp | 4 +- gtsam_unstable/gtsam_unstable.h | 39 +--------- 5 files changed, 48 insertions(+), 210 deletions(-) delete mode 100644 gtsam_unstable/dynamics/imuSystem.cpp delete mode 100644 gtsam_unstable/dynamics/imuSystem.h diff --git a/gtsam_unstable/dynamics/imuSystem.cpp b/gtsam_unstable/dynamics/imuSystem.cpp deleted file mode 100644 index 376868583..000000000 --- a/gtsam_unstable/dynamics/imuSystem.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/** - * @file ilm3DSystem.cpp - * @brief Implementations of ilm 3D domain - * @author Alex Cunningham - */ - -#include -#include - -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 - diff --git a/gtsam_unstable/dynamics/imuSystem.h b/gtsam_unstable/dynamics/imuSystem.h deleted file mode 100644 index ef5437fdf..000000000 --- a/gtsam_unstable/dynamics/imuSystem.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -/** - * 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(key); } -}; - -// factors -typedef gtsam::IMUFactor IMUMeasurement; // IMU between measurements -typedef gtsam::FullIMUFactor FullIMUMeasurement; // Full-state IMU between measurements -typedef gtsam::BetweenFactor Between; // full odometry (including velocity) -typedef gtsam::NonlinearEquality Constraint; -typedef gtsam::PriorFactor Prior; -typedef gtsam::RangeFactor 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 - diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 783c420d4..2f6b8b201 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -7,23 +7,37 @@ #include -#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include using namespace std; using namespace gtsam; -using namespace imu; const double tol=1e-5; static const Key x0 = 0, x1 = 1, x2 = 2, x3 = 3, x4 = 4; static const Vector g = delta(3, 2, -9.81); +//typedef gtsam::IMUFactor IMUFactor; // IMU between measurements +//typedef gtsam::FullIMUFactor IMUFactor; // Full-state IMU between measurements +//typedef gtsam::BetweenFactor Between; // full odometry (including velocity) +//typedef gtsam::NonlinearEquality Constraint; +//typedef gtsam::PriorFactor Prior; +//typedef gtsam::RangeFactor Range; + /* ************************************************************************* */ TEST(testIMUSystem, instantiations) { // just checking for compilation PoseRTV x1_v; - imu::Values local_values; - Graph graph; gtsam::SharedNoiseModel model1 = gtsam::noiseModel::Unit::Create(1); gtsam::SharedNoiseModel model3 = gtsam::noiseModel::Unit::Create(3); @@ -32,13 +46,13 @@ TEST(testIMUSystem, instantiations) { Vector accel = ones(3), gyro = ones(3); - IMUMeasurement imu(accel, gyro, 0.01, x1, x2, model6); - FullIMUMeasurement full_imu(accel, gyro, 0.01, x1, x2, model9); - Constraint poseHardPrior(x1, x1_v); - Between odom(x1, x2, x1_v, model9); - Range range(x1, x2, 1.0, model1); + IMUFactor imu(accel, gyro, 0.01, x1, x2, model6); + FullIMUFactor full_imu(accel, gyro, 0.01, x1, x2, model9); + NonlinearEquality poseHardPrior(x1, x1_v); + BetweenFactor odom(x1, x2, x1_v, model9); + RangeFactor range(x1, x2, 1.0, model1); VelocityConstraint constraint(x1, x2, 0.1, 10000); - Prior posePrior(x1, x1_v, model9); + PriorFactor posePrior(x1, x1_v, model9); DHeightPrior heightPrior(x1, 0.1, model1); VelocityPrior velPrior(x1, ones(3), model3); } @@ -60,17 +74,17 @@ TEST( testIMUSystem, optimize_chain ) { imu34 = pose3.imuPrediction(pose4, dt); // assemble simple graph with IMU measurements and velocity constraints - Graph graph; - graph.add(Constraint(x1, pose1)); - graph.add(IMUMeasurement(imu12, dt, x1, x2, model)); - graph.add(IMUMeasurement(imu23, dt, x2, x3, model)); - graph.add(IMUMeasurement(imu34, dt, x3, x4, model)); + EasyFactorGraph graph; + graph.add(NonlinearEquality(x1, pose1)); + graph.add(IMUFactor(imu12, dt, x1, x2, model)); + graph.add(IMUFactor(imu23, dt, x2, x3, model)); + graph.add(IMUFactor(imu34, dt, x3, x4, model)); graph.add(VelocityConstraint(x1, x2, dt)); graph.add(VelocityConstraint(x2, x3, dt)); graph.add(VelocityConstraint(x3, x4, dt)); // ground truth values - imu::Values true_values; + Values true_values; true_values.insert(x1, pose1); true_values.insert(x2, pose2); true_values.insert(x3, pose3); @@ -80,13 +94,13 @@ TEST( testIMUSystem, optimize_chain ) { EXPECT_DOUBLES_EQUAL(0, graph.error(true_values), 1e-5); // initialize with zero values and optimize - imu::Values values; + Values values; values.insert(x1, PoseRTV()); values.insert(x2, PoseRTV()); values.insert(x3, PoseRTV()); values.insert(x4, PoseRTV()); - imu::Values actual = graph.optimize(values); + Values actual = graph.optimize(values); EXPECT(assert_equal(true_values, actual, tol)); } @@ -107,14 +121,14 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { imu34 = pose3.imuPrediction(pose4, dt); // assemble simple graph with IMU measurements and velocity constraints - Graph graph; - graph.add(Constraint(x1, pose1)); - graph.add(FullIMUMeasurement(imu12, dt, x1, x2, model)); - graph.add(FullIMUMeasurement(imu23, dt, x2, x3, model)); - graph.add(FullIMUMeasurement(imu34, dt, x3, x4, model)); + EasyFactorGraph graph; + graph.add(NonlinearEquality(x1, pose1)); + graph.add(FullIMUFactor(imu12, dt, x1, x2, model)); + graph.add(FullIMUFactor(imu23, dt, x2, x3, model)); + graph.add(FullIMUFactor(imu34, dt, x3, x4, model)); // ground truth values - imu::Values true_values; + Values true_values; true_values.insert(x1, pose1); true_values.insert(x2, pose2); true_values.insert(x3, pose3); @@ -124,7 +138,7 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { EXPECT_DOUBLES_EQUAL(0, graph.error(true_values), 1e-5); // initialize with zero values and optimize - imu::Values values; + Values values; values.insert(x1, PoseRTV()); values.insert(x2, 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 - imu::Values actual = graph.optimize(values); + Values actual = graph.optimize(values); // 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 SharedDiagonal model = noiseModel::Unit::Create(9); - imu::Values true_traj, init_traj; - Graph graph; + Values true_traj, init_traj; + EasyFactorGraph graph; - graph.add(Constraint(x0, start)); + graph.add(NonlinearEquality(x0, start)); true_traj.insert(x0, start); init_traj.insert(x0, start); @@ -159,7 +173,7 @@ TEST( testIMUSystem, linear_trajectory) { for (size_t i=1; i(accel - g, gyro, dt, xA, xB, model)); true_traj.insert(xB, cur_pose); init_traj.insert(xB, PoseRTV()); } diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index 61588841d..ea4a3493d 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp @@ -4,10 +4,10 @@ */ #include -#include + +#include using namespace gtsam; -using namespace imu; const double tol=1e-5; diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index c77c71e22..a14ad34f9 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -102,7 +102,7 @@ virtual class NonlinearEquality : gtsam::NonlinearFactor { NonlinearEquality(size_t j, const T& feasible, double error_gain); }; - +#include template virtual class IMUFactor : gtsam::NonlinearFactor { /** Standard constructor */ @@ -120,7 +120,7 @@ virtual class IMUFactor : gtsam::NonlinearFactor { size_t key2() const; }; - +#include template virtual class FullIMUFactor : gtsam::NonlinearFactor { /** Standard constructor */ @@ -167,38 +167,3 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor { }///\namespace gtsam - -namespace imu { - -#include -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