Removed imu dynamics example slam namespace
Alex Cunningham
parent
dda625b2e0
commit
46b2971e45
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -7,23 +7,37 @@
|
|||
|
||||
#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 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<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) {
|
||||
// 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<PoseRTV> imu(accel, gyro, 0.01, x1, x2, model6);
|
||||
FullIMUFactor<PoseRTV> full_imu(accel, gyro, 0.01, x1, x2, model9);
|
||||
NonlinearEquality<gtsam::PoseRTV> poseHardPrior(x1, x1_v);
|
||||
BetweenFactor<gtsam::PoseRTV> odom(x1, x2, x1_v, model9);
|
||||
RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> range(x1, x2, 1.0, model1);
|
||||
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);
|
||||
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<gtsam::PoseRTV>(x1, pose1));
|
||||
graph.add(IMUFactor<PoseRTV>(imu12, dt, x1, x2, model));
|
||||
graph.add(IMUFactor<PoseRTV>(imu23, dt, x2, x3, model));
|
||||
graph.add(IMUFactor<PoseRTV>(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<gtsam::PoseRTV>(x1, pose1));
|
||||
graph.add(FullIMUFactor<PoseRTV>(imu12, dt, x1, x2, model));
|
||||
graph.add(FullIMUFactor<PoseRTV>(imu23, dt, x2, x3, model));
|
||||
graph.add(FullIMUFactor<PoseRTV>(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<gtsam::PoseRTV>(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<nrPoses; ++i) {
|
||||
Key xA = i-1, xB = i;
|
||||
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);
|
||||
init_traj.insert(xB, PoseRTV());
|
||||
}
|
||||
|
|
|
|||
|
|
@ -4,10 +4,10 @@
|
|||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam_unstable/dynamics/imuSystem.h>
|
||||
|
||||
#include <gtsam_unstable/dynamics/VelocityConstraint.h>
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace imu;
|
||||
|
||||
const double tol=1e-5;
|
||||
|
||||
|
|
|
|||
|
|
@ -102,7 +102,7 @@ virtual class NonlinearEquality : gtsam::NonlinearFactor {
|
|||
NonlinearEquality(size_t j, const T& feasible, double error_gain);
|
||||
};
|
||||
|
||||
|
||||
#include <gtsam_unstable/dynamics/IMUFactor.h>
|
||||
template<POSE = {gtsam::PoseRTV}>
|
||||
virtual class IMUFactor : gtsam::NonlinearFactor {
|
||||
/** Standard constructor */
|
||||
|
|
@ -120,7 +120,7 @@ virtual class IMUFactor : gtsam::NonlinearFactor {
|
|||
size_t key2() const;
|
||||
};
|
||||
|
||||
|
||||
#include <gtsam_unstable/dynamics/FullIMUFactor.h>
|
||||
template<POSE = {gtsam::PoseRTV}>
|
||||
virtual class FullIMUFactor : gtsam::NonlinearFactor {
|
||||
/** Standard constructor */
|
||||
|
|
@ -167,38 +167,3 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
|
|||
|
||||
|
||||
}///\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
|
||||
|
|
|
|||
Loading…
Reference in New Issue