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;
127  accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t);
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  }
144  GTSAM_PRINT(result);
145  return 0;
146 }
147 /* ************************************************************************* */
Simple class to test navigation scenarios.
void integrateMeasurement(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override
Definition: ImuFactor.cpp:53
const double kGravity
void clear()
Definition: Values.h:298
Eigen::Vector3d Vector3
Definition: Vector.h:43
#define M_PI
Definition: main.h:106
Vector2 b2(4, -5)
Definition: BFloat16.h:88
virtual ISAM2Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const FactorIndices &removeFactorIndices=FactorIndices(), const std::optional< FastMap< Key, int > > &constrainedKeys={}, const std::optional< FastList< Key > > &noRelinKeys={}, const std::optional< FastList< Key > > &extraReelimKeys={}, bool force_relinearize=false)
Definition: ISAM2.cpp:400
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition: Scenario.h:70
Rot3 rotation(double t) const
Definition: Scenario.h:39
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:214
static const SmartProjectionParams params
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Base class for all pinhole cameras.
Eigen::VectorXd Vector
Definition: Vector.h:38
Pose3 pose(double t) const override
pose at time t
Definition: Scenario.h:67
Values result
const Pose3 & pose() const
return pose, constant version
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
NonlinearISAM isam(relinearizeInterval)
static const Vector3 measuredAcc
ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0))
traits
Definition: chartTesting.h:28
static const Vector3 measuredOmega(w, 0, 0)
#define GTSAM_PRINT(x)
Definition: Testable.h:43
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Vector3 Point3
Definition: Point3.h:38
static const CalibratedCamera camera(kDefaultPose)
#define X
Definition: icosphere.cpp:20
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
void resetIntegration() override
Re-initialize PreintegratedIMUMeasurements.
Definition: ImuFactor.cpp:47
int main(int argc, char *argv[])
Matrix3 transpose() const
Definition: Rot3M.cpp:143
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Vector3 acceleration_b(double t) const
Definition: Scenario.h:47
Values calculateEstimate() const
Definition: ISAM2.cpp:766
Point2 t(10, 10)
The most common 5DOF 3D->2D calibration.
Vector2 b1(2, -1)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:21