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 
118 
120  parameters.relinearizeThreshold = 0.1;
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 }
SmartStereoProjectionPoseFactor.h
Smart stereo factor on poses, assuming camera calibration is fixed.
gtsam::ISAM2Params
Definition: ISAM2Params.h:136
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
cy
const double cy
Definition: testSmartStereoFactor_iSAM2.cpp:50
IMUHelper::IMUHelper
IMUHelper()
Definition: ISAM2_SmartFactorStereo_IMU.cpp:28
gtsam::ISAM2
Definition: ISAM2.h:45
gtsam.examples.DogLegOptimizerExample.type
type
Definition: DogLegOptimizerExample.py:111
IMUHelper::priorImuBias
imuBias::ConstantBias priorImuBias
Definition: ISAM2_SmartFactorStereo_IMU.cpp:88
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
fx
const double fx
Definition: testSmartStereoFactor_iSAM2.cpp:47
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
HESSIAN
#define HESSIAN
Definition: timeSchurFactors.cpp:24
IMUHelper
Definition: ISAM2_SmartFactorStereo_IMU.cpp:27
IMUHelper::prevState
NavState prevState
Definition: ISAM2_SmartFactorStereo_IMU.cpp:91
dt
const double dt
Definition: testVelocityConstraint.cpp:15
gtsam::NavState
Definition: NavState.h:34
IMUHelper::propState
NavState propState
Definition: ISAM2_SmartFactorStereo_IMU.cpp:92
X
#define X
Definition: icosphere.cpp:20
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
IMUHelper::biasNoiseModel
noiseModel::Robust::shared_ptr biasNoiseModel
Definition: ISAM2_SmartFactorStereo_IMU.cpp:90
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::NonlinearISAM::update
void update(const NonlinearFactorGraph &newFactors, const Values &initialValues)
Definition: NonlinearISAM.cpp:35
gtsam::PreintegratedCombinedMeasurements::integrateMeasurement
void integrateMeasurement(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override
Definition: CombinedImuFactor.cpp:106
ss
static std::stringstream ss
Definition: testBTree.cpp:31
gtsam::ZERO_ON_DEGENERACY
@ ZERO_ON_DEGENERACY
Definition: SmartFactorParams.h:36
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Definition: FactorGraph.h:367
gtsam::SmartStereoProjectionPoseFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartStereoProjectionPoseFactor.h:62
gtsam::SmartStereoProjectionPoseFactor
Definition: SmartStereoProjectionPoseFactor.h:46
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
isam
NonlinearISAM isam(relinearizeInterval)
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::PreintegrationBase::predict
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.
Definition: PreintegrationBase.cpp:115
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
landmark
static Point3 landmark(0, 0, 5)
gtsam::Cal3_S2Stereo::shared_ptr
std::shared_ptr< Cal3_S2Stereo > shared_ptr
Definition: Cal3_S2Stereo.h:38
ceres::pow
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
y
Scalar * y
Definition: level1_cplx_impl.h:124
IMUHelper::velocityNoiseModel
noiseModel::Robust::shared_ptr velocityNoiseModel
Definition: ISAM2_SmartFactorStereo_IMU.cpp:89
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
gtsam::CombinedImuFactor
Definition: CombinedImuFactor.h:205
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam
traits
Definition: SFMdata.h:40
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
K
#define K
Definition: igam.h:8
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::Values
Definition: Values.h:65
gtsam::StereoPoint2
Definition: StereoPoint2.h:34
std
Definition: BFloat16.h:88
IMUHelper::preintegrated
PreintegratedCombinedMeasurements * preintegrated
Definition: ISAM2_SmartFactorStereo_IMU.cpp:94
gtsam::Values::clear
void clear()
Definition: Values.h:347
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::noiseModel::Robust::shared_ptr
std::shared_ptr< Robust > shared_ptr
Definition: NoiseModel.h:680
baseline
const double baseline
Definition: testSmartStereoFactor_iSAM2.cpp:51
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
CombinedImuFactor.h
gtsam::PreintegrationBase::resetIntegrationAndSetBias
void resetIntegrationAndSetBias(const Bias &biasHat)
Definition: PreintegrationBase.cpp:53
cx
const double cx
Definition: testSmartStereoFactor_iSAM2.cpp:49
IMUHelper::prevBias
imuBias::ConstantBias prevBias
Definition: ISAM2_SmartFactorStereo_IMU.cpp:93
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:171
gtsam::PreintegratedCombinedMeasurements
Definition: CombinedImuFactor.h:66
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
main
int main(int argc, char *argv[])
Definition: ISAM2_SmartFactorStereo_IMU.cpp:97
fy
const double fy
Definition: testSmartStereoFactor_iSAM2.cpp:48


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