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

Removed use of EasyFactorGraph

release/4.3a0
Alex Cunningham 2012-08-04 19:48:52 +00:00
parent e9de9f3242
commit 83a853f6f1
1 changed files with 7 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -12,7 +12,8 @@
#include <gtsam/slam/RangeFactor.h> #include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/PartialPriorFactor.h> #include <gtsam/slam/PartialPriorFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/EasyFactorGraph.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam_unstable/dynamics/IMUFactor.h> #include <gtsam_unstable/dynamics/IMUFactor.h>
#include <gtsam_unstable/dynamics/FullIMUFactor.h> #include <gtsam_unstable/dynamics/FullIMUFactor.h>
@ -27,13 +28,6 @@ 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
@ -74,7 +68,7 @@ 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
EasyFactorGraph graph; NonlinearFactorGraph graph;
graph.add(NonlinearEquality<gtsam::PoseRTV>(x1, pose1)); graph.add(NonlinearEquality<gtsam::PoseRTV>(x1, pose1));
graph.add(IMUFactor<PoseRTV>(imu12, dt, x1, x2, model)); graph.add(IMUFactor<PoseRTV>(imu12, dt, x1, x2, model));
graph.add(IMUFactor<PoseRTV>(imu23, dt, x2, x3, model)); graph.add(IMUFactor<PoseRTV>(imu23, dt, x2, x3, model));
@ -100,7 +94,7 @@ TEST( testIMUSystem, optimize_chain ) {
values.insert(x3, PoseRTV()); values.insert(x3, PoseRTV());
values.insert(x4, PoseRTV()); values.insert(x4, PoseRTV());
Values actual = graph.optimize(values); Values actual = LevenbergMarquardtOptimizer(graph, values).optimize();
EXPECT(assert_equal(true_values, actual, tol)); EXPECT(assert_equal(true_values, actual, tol));
} }
@ -121,7 +115,7 @@ 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
EasyFactorGraph graph; NonlinearFactorGraph graph;
graph.add(NonlinearEquality<gtsam::PoseRTV>(x1, pose1)); graph.add(NonlinearEquality<gtsam::PoseRTV>(x1, pose1));
graph.add(FullIMUFactor<PoseRTV>(imu12, dt, x1, x2, model)); graph.add(FullIMUFactor<PoseRTV>(imu12, dt, x1, x2, model));
graph.add(FullIMUFactor<PoseRTV>(imu23, dt, x2, x3, model)); graph.add(FullIMUFactor<PoseRTV>(imu23, dt, x2, x3, model));
@ -146,7 +140,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
Values actual = graph.optimize(values); Values actual = LevenbergMarquardtOptimizer(graph, values).optimize();
// EXPECT(assert_equal(true_values, actual, tol)); // FAIL // EXPECT(assert_equal(true_values, actual, tol)); // FAIL
} }
@ -162,7 +156,7 @@ TEST( testIMUSystem, linear_trajectory) {
SharedDiagonal model = noiseModel::Unit::Create(9); SharedDiagonal model = noiseModel::Unit::Create(9);
Values true_traj, init_traj; Values true_traj, init_traj;
EasyFactorGraph graph; NonlinearFactorGraph graph;
graph.add(NonlinearEquality<gtsam::PoseRTV>(x0, start)); graph.add(NonlinearEquality<gtsam::PoseRTV>(x0, start));
true_traj.insert(x0, start); true_traj.insert(x0, start);