22 using namespace gtsam;
30 auto gaussian = noiseModel::Diagonal::Sigmas(
31 (
Vector(6) << Vector3::Constant(5.0
e-2), Vector3::Constant(5.0
e-3))
33 auto huber = noiseModel::Robust::Create(
34 noiseModel::mEstimator::Huber::Create(1.345), gaussian);
36 biasNoiseModel = huber;
40 auto gaussian = noiseModel::Isotropic::Sigma(3, 0.01);
41 auto huber = noiseModel::Robust::Create(
42 noiseModel::mEstimator::Huber::Create(1.345), gaussian);
44 velocityNoiseModel = huber;
48 auto p = std::make_shared<PreintegratedCombinedMeasurements::Params>(
51 p->accelerometerCovariance =
52 I_3x3 *
pow(0.0565, 2.0);
53 p->integrationCovariance =
55 p->gyroscopeCovariance =
56 I_3x3 *
pow(4.0
e-5, 2.0);
57 p->biasAccCovariance = I_3x3 *
pow(0.00002, 2.0);
58 p->biasOmegaCovariance =
59 I_3x3 *
pow(0.001, 2.0);
60 p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1
e-5;
63 Rot3 iRb(0.036129, -0.998727, 0.035207,
64 0.045417, -0.033553, -0.998404,
65 0.998315, 0.037670, 0.044147);
68 Point3 iTb(0.03, -0.025, -0.06);
71 p->body_P_sensor =
Pose3(iRb, iTb);
76 Vector3 acc_bias(0.0, -0.0942015, 0.0);
77 Vector3 gyro_bias(-0.00527483, -0.00757152, -0.00469968);
82 propState = prevState;
83 prevBias = priorImuBias;
97 int main(
int argc,
char* argv[]) {
99 cout <<
"./ISAM2_SmartFactorStereo_IMU [data.txt]\n";
103 ifstream in(argv[1]);
106 cerr <<
"error opening: " << argv[1] <<
"\n";
124 std::map<size_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
130 auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
131 (
Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
133 initialEstimate.
insert(
X(0), Pose3::Identity());
150 in.getline(line,
sizeof(line));
151 stringstream
ss(line);
157 if (frame != lastFrame || in.eof()) {
158 cout <<
"Running iSAM for frame: " << lastFrame <<
"\n";
160 initialEstimate.
insert(
X(lastFrame), Pose3::Identity());
165 X(lastFrame),
V(lastFrame),
B(lastFrame - 1),
172 Values currentEstimate =
isam.calculateEstimate();
181 initialEstimate.
clear();
191 double dt = 1 / 800.0;
205 }
else if (
type ==
's') {
214 if (smartFactors.count(
landmark) == 0) {
215 auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0);
226 throw runtime_error(
"unexpected data type: " +
string(1,
type));