Example due to Robert Truax in Issue #280
Frank Dellaert
parent
5494bee054
commit
c428e30784
|
|
@ -0,0 +1,171 @@
|
|||
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
|
||||
#define GTSAM4
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::vector<gtsam::Pose3> createPoses() {
|
||||
|
||||
// Create the set of ground-truth poses
|
||||
std::vector<gtsam::Pose3> poses;
|
||||
double radius = 30.0;
|
||||
int i = 0;
|
||||
double theta = 0.0;
|
||||
gtsam::Point3 up(0,0,1);
|
||||
gtsam::Point3 target(0,0,0);
|
||||
for(; i < 80; ++i, theta += 2*M_PI/8) {
|
||||
gtsam::Point3 position = gtsam::Point3(radius*cos(theta), radius*sin(theta), 0.0);
|
||||
gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(position, target, up);
|
||||
poses.push_back(camera.pose());
|
||||
}
|
||||
return poses;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(int argc, char* argv[]) {
|
||||
|
||||
// Create the set of ground-truth landmarks and poses
|
||||
vector<Pose3> poses = createPoses();
|
||||
|
||||
// Create a factor graph
|
||||
NonlinearFactorGraph newgraph;
|
||||
NonlinearFactorGraph totalgraph;
|
||||
|
||||
// Create ISAM2 solver
|
||||
ISAM2 isam, isam_full;
|
||||
|
||||
// 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.
|
||||
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
|
||||
newgraph.push_back(PriorFactor<Pose3>(0, poses[0], noise));
|
||||
totalgraph.push_back(PriorFactor<Pose3>(0, poses[0], noise));
|
||||
|
||||
// Add imu priors
|
||||
int biasidx = 1000;
|
||||
noiseModel::Diagonal::shared_ptr biasnoise = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished());
|
||||
PriorFactor<imuBias::ConstantBias> biasprior(biasidx,imuBias::ConstantBias(),biasnoise);
|
||||
newgraph.push_back(biasprior);
|
||||
totalgraph.push_back(biasprior);
|
||||
initialEstimate.insert(biasidx, imuBias::ConstantBias());
|
||||
totalEstimate.insert(biasidx, imuBias::ConstantBias());
|
||||
noiseModel::Diagonal::shared_ptr velnoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.1).finished());
|
||||
#ifdef GTSAM4
|
||||
PriorFactor<Vector> velprior(100,(Vector(3) << 0, 0, 0).finished(), velnoise);
|
||||
#else
|
||||
PriorFactor<LieVector> velprior(100,(Vector(3) << 0, 0, 0).finished(), velnoise);
|
||||
#endif
|
||||
newgraph.push_back(velprior);
|
||||
totalgraph.push_back(velprior);
|
||||
|
||||
#ifdef GTSAM4
|
||||
initialEstimate.insert(100, (Vector(3) << 0, 0, 0).finished());
|
||||
totalEstimate.insert(100, (Vector(3) << 0, 0, 0).finished());
|
||||
#else
|
||||
initialEstimate.insert(100, LieVector((Vector(3) << 0, 0, 0).finished()));
|
||||
totalEstimate.insert(100, LieVector((Vector(3) << 0, 0, 0).finished()));
|
||||
#endif
|
||||
|
||||
Matrix3 I;
|
||||
I << 1, 0, 0, 0, 1, 0, 0, 0, 1;
|
||||
Matrix3 accCov = I*0.1;
|
||||
Matrix3 gyroCov = I*0.1;
|
||||
Matrix3 intCov = I*0.1;
|
||||
bool secOrder = false;
|
||||
#ifdef GTSAM4
|
||||
// IMU preintegrator
|
||||
PreintegratedImuMeasurements accum(PreintegrationParams::MakeSharedD());
|
||||
accum.params()->setAccelerometerCovariance(accCov);
|
||||
accum.params()->setGyroscopeCovariance(gyroCov);
|
||||
accum.params()->setIntegrationCovariance(intCov);
|
||||
accum.params()->setUse2ndOrderCoriolis(secOrder);
|
||||
accum.params()->setOmegaCoriolis(Vector3(0, 0, 0));
|
||||
#else
|
||||
ImuFactor::PreintegratedMeasurements accum(imuBias::ConstantBias(), accCov, gyroCov, intCov, secOrder);
|
||||
#endif
|
||||
|
||||
// Simulate poses and imu measurements, adding them to the factor graph
|
||||
for (size_t i = 0; i < poses.size(); ++i) {
|
||||
#ifdef GTSAM4
|
||||
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
|
||||
#else
|
||||
Pose3 delta(Rot3::ypr(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
|
||||
#endif
|
||||
if (i == 0) { // First time add two poses
|
||||
initialEstimate.insert(0, poses[0].compose(delta));
|
||||
initialEstimate.insert(1, poses[1].compose(delta));
|
||||
totalEstimate.insert(0, poses[0].compose(delta));
|
||||
totalEstimate.insert(1, poses[1].compose(delta));
|
||||
} else if (i >= 2) { // Add more poses as necessary
|
||||
initialEstimate.insert(i, poses[i].compose(delta));
|
||||
totalEstimate.insert(i, poses[i].compose(delta));
|
||||
}
|
||||
|
||||
if (i > 0) {
|
||||
// Add Bias variables periodically
|
||||
if (i % 5 == 0) {
|
||||
biasidx++;
|
||||
Symbol b1 = biasidx-1;
|
||||
Symbol b2 = biasidx;
|
||||
imuBias::ConstantBias basebias = imuBias::ConstantBias();
|
||||
Vector6 covvec;
|
||||
covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
|
||||
noiseModel::Diagonal::shared_ptr cov = noiseModel::Diagonal::Variances(covvec);
|
||||
Vector6 zerovec;
|
||||
zerovec << 0, 0, 0, 0, 0, 0;
|
||||
BetweenFactor<imuBias::ConstantBias>::shared_ptr f(new BetweenFactor<imuBias::ConstantBias>(b1, b2, imuBias::ConstantBias(), cov));
|
||||
newgraph.add(f);
|
||||
totalgraph.add(f);
|
||||
initialEstimate.insert(biasidx, imuBias::ConstantBias());
|
||||
totalEstimate.insert(biasidx, imuBias::ConstantBias());
|
||||
}
|
||||
// Add Imu Factor
|
||||
accum.integrateMeasurement((Vector(3) << 0.0, 0.0, -9.8).finished(), (Vector(3) << 0, 0, 0).finished(), 0.5);
|
||||
#ifdef GTSAM4
|
||||
ImuFactor imufac(i-1, 100+i-1,i,100+i, biasidx, accum);
|
||||
#else
|
||||
ImuFactor imufac(i-1, 100+i-1,i,100+i, biasidx, accum, (Vector(3) << 0.0, 0.0, -9.8).finished(), (Vector(3) << 0.0, 0.0, 0.0).finished());
|
||||
#endif
|
||||
|
||||
newgraph.add(imufac);
|
||||
totalgraph.add(imufac);
|
||||
#ifdef GTSAM4
|
||||
initialEstimate.insert(100+i, (Vector(3) << 0.0, 0.0, -9.8).finished()); // insert new velocity
|
||||
totalEstimate.insert(100+i, (Vector(3) << 0.0, 0.0, -9.8).finished()); // insert new velocity
|
||||
#else
|
||||
initialEstimate.insert(100+i, LieVector((Vector(3) << 0.0, 0.0, -9.8).finished())); // insert new velocity
|
||||
totalEstimate.insert(100+i, LieVector((Vector(3) << 0.0, 0.0, -9.8).finished())); // insert new velocity
|
||||
#endif
|
||||
accum.resetIntegration();
|
||||
}
|
||||
|
||||
// Batch solution
|
||||
isam_full = ISAM2();
|
||||
isam_full.update(totalgraph, totalEstimate);
|
||||
result = isam_full.calculateEstimate();
|
||||
|
||||
// Incremental solution
|
||||
isam.update(newgraph, initialEstimate);
|
||||
result = isam.calculateEstimate();
|
||||
newgraph = NonlinearFactorGraph();
|
||||
initialEstimate.clear();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue