1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
|
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ImuFactorExample2
* @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor with ISAM2.
* @author Robert Truax
*/
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/Scenario.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/BetweenFactor.h>
#include <vector>
using namespace std;
using namespace gtsam;
// Shorthand for velocity and pose variables
using symbol_shorthand::V;
using symbol_shorthand::X;
const double kGravity = 9.81;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
auto params = PreintegrationParams::MakeSharedU(kGravity);
params->setAccelerometerCovariance(I_3x3 * 0.1);
params->setGyroscopeCovariance(I_3x3 * 0.1);
params->setIntegrationCovariance(I_3x3 * 0.1);
params->setUse2ndOrderCoriolis(false);
params->setOmegaCoriolis(Vector3(0, 0, 0));
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
// Start with a camera on x-axis looking at origin
double radius = 30;
const Point3 up(0, 0, 1), target(0, 0, 0);
const Point3 position(radius, 0, 0);
const auto camera = PinholeCamera<Cal3_S2>::Lookat(position, target, up);
const auto pose_0 = camera.pose();
// Now, create a constant-twist scenario that makes the camera orbit the
// origin
double angular_velocity = M_PI, // rad/sec
delta_t = 1.0 / 18; // makes for 10 degrees per step
Vector3 angular_velocity_vector(0, -angular_velocity, 0);
Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0);
auto scenario = ConstantTwistScenario(angular_velocity_vector,
linear_velocity_vector, pose_0);
// Create a factor graph
NonlinearFactorGraph newgraph;
// Create (incremental) ISAM2 solver
ISAM2 isam;
// Create the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth
Values initialEstimate, totalEstimate, result;
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z.
auto noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
newgraph.addPrior(X(0), pose_0, noise);
// Add imu priors
Key biasKey = Symbol('b', 0);
auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1));
newgraph.addPrior(biasKey, imuBias::ConstantBias(), biasnoise);
initialEstimate.insert(biasKey, imuBias::ConstantBias());
auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
Vector n_velocity(3);
n_velocity << 0, angular_velocity * radius, 0;
newgraph.addPrior(V(0), n_velocity, velnoise);
initialEstimate.insert(V(0), n_velocity);
// IMU preintegrator
PreintegratedImuMeasurements accum(params);
// Simulate poses and imu measurements, adding them to the factor graph
for (size_t i = 0; i < 36; ++i) {
double t = i * delta_t;
if (i == 0) { // First time add two poses
auto pose_1 = scenario.pose(delta_t);
initialEstimate.insert(X(0), pose_0.compose(delta));
initialEstimate.insert(X(1), pose_1.compose(delta));
} else if (i >= 2) { // Add more poses as necessary
auto pose_i = scenario.pose(t);
initialEstimate.insert(X(i), pose_i.compose(delta));
}
if (i > 0) {
// Add Bias variables periodically
if (i % 5 == 0) {
biasKey++;
Symbol b1 = biasKey - 1;
Symbol b2 = biasKey;
Vector6 covvec;
covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
auto cov = noiseModel::Diagonal::Variances(covvec);
auto f = boost::make_shared<BetweenFactor<imuBias::ConstantBias> >(
b1, b2, imuBias::ConstantBias(), cov);
newgraph.add(f);
initialEstimate.insert(biasKey, imuBias::ConstantBias());
}
// Predict acceleration and gyro measurements in (actual) body frame
Vector3 measuredAcc = scenario.acceleration_b(t) -
scenario.rotation(t).transpose() * params->n_gravity;
Vector3 measuredOmega = scenario.omega_b(t);
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t);
// Add Imu Factor
ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum);
newgraph.add(imufac);
// insert new velocity, which is wrong
initialEstimate.insert(V(i), n_velocity);
accum.resetIntegration();
}
// Incremental solution
isam.update(newgraph, initialEstimate);
result = isam.calculateEstimate();
newgraph = NonlinearFactorGraph();
initialEstimate.clear();
}
GTSAM_PRINT(result);
return 0;
}
/* ************************************************************************* */
|
