30 using namespace gtsam;
39 int main(
int argc,
char* argv[]) {
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);
51 const Point3 up(0, 0, 1), target(0, 0, 0);
58 double angular_velocity =
M_PI,
60 Vector3 angular_velocity_vector(0, -angular_velocity, 0);
61 Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0);
63 linear_velocity_vector, pose_0);
77 auto noise = noiseModel::Diagonal::Sigmas(
78 (
Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
83 auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1));
86 auto velnoise = noiseModel::Diagonal::Sigmas(
Vector3(0.1, 0.1, 0.1));
89 n_velocity << 0, angular_velocity * radius, 0;
90 newgraph.
addPrior(
V(0), n_velocity, velnoise);
92 initialEstimate.
insert(
V(0), n_velocity);
98 for (
size_t i = 0;
i < 36; ++
i) {
99 double t =
i * delta_t;
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> >(
131 newgraph.
add(imufac);
134 initialEstimate.
insert(
V(
i), n_velocity);
142 initialEstimate.
clear();