ImuFactorsExample2.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #include <gtsam/geometry/Cal3_S2.h>
20 #include <gtsam/inference/Symbol.h>
24 #include <gtsam/nonlinear/ISAM2.h>
26 
27 #include <vector>
28 
29 using namespace std;
30 using namespace gtsam;
31 
32 // Shorthand for velocity and pose variables
35 
36 const double kGravity = 9.81;
37 
38 /* ************************************************************************* */
39 int main(int argc, char* argv[]) {
40  auto params = PreintegrationParams::MakeSharedU(kGravity);
41  params->setAccelerometerCovariance(I_3x3 * 0.1);
42  params->setGyroscopeCovariance(I_3x3 * 0.1);
43  params->setIntegrationCovariance(I_3x3 * 0.1);
44  params->setUse2ndOrderCoriolis(false);
45  params->setOmegaCoriolis(Vector3(0, 0, 0));
46 
47  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
48 
49  // Start with a camera on x-axis looking at origin
50  double radius = 30;
51  const Point3 up(0, 0, 1), target(0, 0, 0);
52  const Point3 position(radius, 0, 0);
53  const auto camera = PinholeCamera<Cal3_S2>::Lookat(position, target, up);
54  const auto pose_0 = camera.pose();
55 
56  // Now, create a constant-twist scenario that makes the camera orbit the
57  // origin
58  double angular_velocity = M_PI, // rad/sec
59  delta_t = 1.0 / 18; // makes for 10 degrees per step
60  Vector3 angular_velocity_vector(0, -angular_velocity, 0);
61  Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0);
62  auto scenario = ConstantTwistScenario(angular_velocity_vector,
63  linear_velocity_vector, pose_0);
64 
65  // Create a factor graph
66  NonlinearFactorGraph newgraph;
67 
68  // Create (incremental) ISAM2 solver
69  ISAM2 isam;
70 
71  // Create the initial estimate to the solution
72  // Intentionally initialize the variables off from the ground truth
73  Values initialEstimate, totalEstimate, result;
74 
75  // Add a prior on pose x0. This indirectly specifies where the origin is.
76  // 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z.
77  auto noise = noiseModel::Diagonal::Sigmas(
78  (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
79  newgraph.addPrior(X(0), pose_0, noise);
80 
81  // Add imu priors
82  Key biasKey = Symbol('b', 0);
83  auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1));
84  newgraph.addPrior(biasKey, imuBias::ConstantBias(), biasnoise);
85  initialEstimate.insert(biasKey, imuBias::ConstantBias());
86  auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
87 
88  Vector n_velocity(3);
89  n_velocity << 0, angular_velocity * radius, 0;
90  newgraph.addPrior(V(0), n_velocity, velnoise);
91 
92  initialEstimate.insert(V(0), n_velocity);
93 
94  // IMU preintegrator
96 
97  // Simulate poses and imu measurements, adding them to the factor graph
98  for (size_t i = 0; i < 36; ++i) {
99  double t = i * delta_t;
100  if (i == 0) { // First time add two poses
101  auto pose_1 = scenario.pose(delta_t);
102  initialEstimate.insert(X(0), pose_0.compose(delta));
103  initialEstimate.insert(X(1), pose_1.compose(delta));
104  } else if (i >= 2) { // Add more poses as necessary
105  auto pose_i = scenario.pose(t);
106  initialEstimate.insert(X(i), pose_i.compose(delta));
107  }
108 
109  if (i > 0) {
110  // Add Bias variables periodically
111  if (i % 5 == 0) {
112  biasKey++;
113  Symbol b1 = biasKey - 1;
114  Symbol b2 = biasKey;
115  Vector6 covvec;
116  covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
117  auto cov = noiseModel::Diagonal::Variances(covvec);
118  auto f = std::make_shared<BetweenFactor<imuBias::ConstantBias> >(
119  b1, b2, imuBias::ConstantBias(), cov);
120  newgraph.add(f);
121  initialEstimate.insert(biasKey, imuBias::ConstantBias());
122  }
123  // Predict acceleration and gyro measurements in (actual) body frame
125  scenario.rotation(t).transpose() * params->n_gravity;
128 
129  // Add Imu Factor
130  ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum);
131  newgraph.add(imufac);
132 
133  // insert new velocity, which is wrong
134  initialEstimate.insert(V(i), n_velocity);
135  accum.resetIntegration();
136  }
137 
138  // Incremental solution
139  isam.update(newgraph, initialEstimate);
140  result = isam.calculateEstimate();
141  newgraph = NonlinearFactorGraph();
142  initialEstimate.clear();
143  }
145  return 0;
146 }
147 /* ************************************************************************* */
simple_graph::b1
Vector2 b1(2, -1)
gtsam::ISAM2
Definition: ISAM2.h:45
Scenario.h
Simple class to test navigation scenarios.
gtsam::PreintegratedImuMeasurements::integrateMeasurement
void integrateMeasurement(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override
Definition: ImuFactor.cpp:53
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::ConstantTwistScenario::omega_b
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition: Scenario.h:70
simple_graph::b2
Vector2 b2(4, -5)
gtsam::PreintegratedImuMeasurements::resetIntegration
void resetIntegration() override
Re-initialize PreintegratedIMUMeasurements.
Definition: ImuFactor.cpp:47
gtsam::ConstantTwistScenario
Definition: Scenario.h:60
X
#define X
Definition: icosphere.cpp:20
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::ConstantTwistScenario::pose
Pose3 pose(double t) const override
pose at time t
Definition: Scenario.h:67
gtsam::Scenario::rotation
Rot3 rotation(double t) const
Definition: Scenario.h:39
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::NonlinearISAM::update
void update(const NonlinearFactorGraph &newFactors, const Values &initialValues)
Definition: NonlinearISAM.cpp:35
biased_x_rotation::measuredOmega
const Vector3 measuredOmega
Definition: testPreintegratedRotation.cpp:35
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
BetweenFactor.h
gtsam::Pose3
Definition: Pose3.h:37
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
isam
NonlinearISAM isam(relinearizeInterval)
gtsam::PreintegratedImuMeasurements
Definition: ImuFactor.h:68
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
PinholeCamera.h
Base class for all pinhole cameras.
kGravity
const double kGravity
Definition: ImuFactorsExample2.cpp:36
forward::scenario
ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0))
Symbol.h
gtsam::ImuFactor
Definition: ImuFactor.h:169
GTSAM_PRINT
#define GTSAM_PRINT(x)
Definition: Testable.h:43
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
ImuBias.h
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam
traits
Definition: SFMdata.h:40
main
int main(int argc, char *argv[])
Definition: ImuFactorsExample2.cpp:39
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
gtsam::Values::clear
void clear()
Definition: Values.h:347
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition: CalibratedCamera.h:154
gtsam::internal::position
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
Definition: navigation/expressions.h:25
gtsam::Rot3::transpose
Matrix3 transpose() const
Definition: Rot3M.cpp:143
gtsam::Scenario::acceleration_b
Vector3 acceleration_b(double t) const
Definition: Scenario.h:47
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
common::measuredAcc
static const Vector3 measuredAcc
Definition: testImuFactor.cpp:181
camera
static const CalibratedCamera camera(kDefaultPose)
M_PI
#define M_PI
Definition: mconf.h:117
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:171
align_3::t
Point2 t(10, 10)
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
ImuFactor.h
gtsam::Symbol
Definition: inference/Symbol.h:37


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:27