ISAM2_SmartFactorStereo_IMU.cpp
Go to the documentation of this file.
1 
12 #include <gtsam/nonlinear/ISAM2.h>
14 
15 #include <fstream>
16 #include <iostream>
17 #include <sstream>
18 #include <string>
19 #include <vector>
20 
21 using namespace std;
22 using namespace gtsam;
26 
27 struct IMUHelper {
29  {
30  auto gaussian = noiseModel::Diagonal::Sigmas(
31  (Vector(6) << Vector3::Constant(5.0e-2), Vector3::Constant(5.0e-3))
32  .finished());
33  auto huber = noiseModel::Robust::Create(
34  noiseModel::mEstimator::Huber::Create(1.345), gaussian);
35 
36  biasNoiseModel = huber;
37  }
38 
39  {
40  auto gaussian = noiseModel::Isotropic::Sigma(3, 0.01);
41  auto huber = noiseModel::Robust::Create(
42  noiseModel::mEstimator::Huber::Create(1.345), gaussian);
43 
44  velocityNoiseModel = huber;
45  }
46 
47  // expect IMU to be rotated in image space co-ords
48  auto p = std::make_shared<PreintegratedCombinedMeasurements::Params>(
49  Vector3(0.0, 9.8, 0.0));
50 
51  p->accelerometerCovariance =
52  I_3x3 * pow(0.0565, 2.0); // acc white noise in continuous
53  p->integrationCovariance =
54  I_3x3 * 1e-9; // integration uncertainty continuous
55  p->gyroscopeCovariance =
56  I_3x3 * pow(4.0e-5, 2.0); // gyro white noise in continuous
57  p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0); // acc bias in continuous
58  p->biasOmegaCovariance =
59  I_3x3 * pow(0.001, 2.0); // gyro bias in continuous
60  p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5;
61 
62  // body to IMU rotation
63  Rot3 iRb(0.036129, -0.998727, 0.035207,
64  0.045417, -0.033553, -0.998404,
65  0.998315, 0.037670, 0.044147);
66 
67  // body to IMU translation (meters)
68  Point3 iTb(0.03, -0.025, -0.06);
69 
70  // body in this example is the left camera
71  p->body_P_sensor = Pose3(iRb, iTb);
72 
73  Rot3 prior_rotation = Rot3(I_3x3);
74  Pose3 prior_pose(prior_rotation, Point3(0, 0, 0));
75 
76  Vector3 acc_bias(0.0, -0.0942015, 0.0); // in camera frame
77  Vector3 gyro_bias(-0.00527483, -0.00757152, -0.00469968);
78 
79  priorImuBias = imuBias::ConstantBias(acc_bias, gyro_bias);
80 
81  prevState = NavState(prior_pose, Vector3(0, 0, 0));
82  propState = prevState;
83  prevBias = priorImuBias;
84 
85  preintegrated = new PreintegratedCombinedMeasurements(p, priorImuBias);
86  }
87 
88  imuBias::ConstantBias priorImuBias; // assume zero initial bias
95 };
96 
97 int main(int argc, char* argv[]) {
98  if (argc != 2) {
99  cout << "./ISAM2_SmartFactorStereo_IMU [data.txt]\n";
100  return 0;
101  }
102 
103  ifstream in(argv[1]);
104 
105  if (!in) {
106  cerr << "error opening: " << argv[1] << "\n";
107  return 1;
108  }
109 
110  // Camera parameters
111  double fx = 822.37;
112  double fy = 822.37;
113  double cx = 538.73;
114  double cy = 579.10;
115  double baseline = 0.372; // meters
116 
117  Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline));
118 
120  parameters.relinearizeThreshold = 0.1;
121  ISAM2 isam(parameters);
122 
123  // Create a factor graph
124  std::map<size_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
126  Values initialEstimate;
127  IMUHelper imu;
128 
129  // Pose prior - at identity
130  auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
131  (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
132  graph.addPrior(X(1), Pose3::Identity(), priorPoseNoise);
133  initialEstimate.insert(X(0), Pose3::Identity());
134 
135  // Bias prior
136  graph.addPrior(B(1), imu.priorImuBias,
137  imu.biasNoiseModel);
138  initialEstimate.insert(B(0), imu.priorImuBias);
139 
140  // Velocity prior - assume stationary
141  graph.addPrior(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel);
142  initialEstimate.insert(V(0), Vector3(0, 0, 0));
143 
144  int lastFrame = 1;
145  int frame;
146 
147  while (true) {
148  char line[1024];
149 
150  in.getline(line, sizeof(line));
151  stringstream ss(line);
152  char type;
153 
154  ss >> type;
155  ss >> frame;
156 
157  if (frame != lastFrame || in.eof()) {
158  cout << "Running iSAM for frame: " << lastFrame << "\n";
159 
160  initialEstimate.insert(X(lastFrame), Pose3::Identity());
161  initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0));
162  initialEstimate.insert(B(lastFrame), imu.prevBias);
163 
164  CombinedImuFactor imuFactor(X(lastFrame - 1), V(lastFrame - 1),
165  X(lastFrame), V(lastFrame), B(lastFrame - 1),
166  B(lastFrame), *imu.preintegrated);
167 
168  graph.add(imuFactor);
169 
170  isam.update(graph, initialEstimate);
171 
172  Values currentEstimate = isam.calculateEstimate();
173 
174  imu.propState = imu.preintegrated->predict(imu.prevState, imu.prevBias);
175  imu.prevState = NavState(currentEstimate.at<Pose3>(X(lastFrame)),
176  currentEstimate.at<Vector3>(V(lastFrame)));
177  imu.prevBias = currentEstimate.at<imuBias::ConstantBias>(B(lastFrame));
179 
180  graph.resize(0);
181  initialEstimate.clear();
182 
183  if (in.eof()) {
184  break;
185  }
186  }
187 
188  if (type == 'i') { // Process IMU measurement
189  double ax, ay, az;
190  double gx, gy, gz;
191  double dt = 1 / 800.0; // IMU at ~800Hz
192 
193  ss >> ax;
194  ss >> ay;
195  ss >> az;
196 
197  ss >> gx;
198  ss >> gy;
199  ss >> gz;
200 
201  Vector3 acc(ax, ay, az);
202  Vector3 gyr(gx, gy, gz);
203 
204  imu.preintegrated->integrateMeasurement(acc, gyr, dt);
205  } else if (type == 's') { // Process stereo measurement
206  int landmark;
207  double xl, xr, y;
208 
209  ss >> landmark;
210  ss >> xl;
211  ss >> xr;
212  ss >> y;
213 
214  if (smartFactors.count(landmark) == 0) {
215  auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0);
216 
218 
220  new SmartStereoProjectionPoseFactor(gaussian, params));
221  graph.push_back(smartFactors[landmark]);
222  }
223 
224  smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K);
225  } else {
226  throw runtime_error("unexpected data type: " + string(1, type));
227  }
228 
229  lastFrame = frame;
230  }
231 
232  return 0;
233 }
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
void clear()
Definition: Values.h:298
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Scalar * y
Eigen::Vector3d Vector3
Definition: Vector.h:43
const ValueType at(Key j) const
Definition: Values-inl.h:204
imuBias::ConstantBias prevBias
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
const double baseline
void integrateMeasurement(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override
const double cx
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
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
NonlinearFactorGraph graph
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Smart stereo factor on poses, assuming camera calibration is fixed.
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:214
static const SmartProjectionParams params
static Point3 landmark(0, 0, 5)
const double dt
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
noiseModel::Robust::shared_ptr biasNoiseModel
const double fy
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
void resetIntegrationAndSetBias(const Bias &biasHat)
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
NonlinearISAM isam(relinearizeInterval)
static ConjugateGradientParameters parameters
std::shared_ptr< Cal3_S2Stereo > shared_ptr
Definition: Cal3_S2Stereo.h:38
static std::stringstream ss
Definition: testBTree.cpp:31
RelinearizationThreshold relinearizeThreshold
Definition: ISAM2Params.h:169
traits
Definition: chartTesting.h:28
#define HESSIAN
std::shared_ptr< Robust > shared_ptr
Definition: NoiseModel.h:659
virtual void resize(size_t size)
Definition: FactorGraph.h:389
NavState predict(const NavState &state_i, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 6 > H2={}) const
Predict state at time j.
const double cy
float * p
PreintegratedCombinedMeasurements * preintegrated
void insert(Key j, const Value &val)
Definition: Values.cpp:155
imuBias::ConstantBias priorImuBias
Vector3 Point3
Definition: Point3.h:38
const double fx
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
#define X
Definition: icosphere.cpp:20
int main(int argc, char *argv[])
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
noiseModel::Robust::shared_ptr velocityNoiseModel
Values calculateEstimate() const
Definition: ISAM2.cpp:766


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