From 18234f68fd0a694e0d14c07499fc910bbd06ffff Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Oct 2018 16:25:58 -0400 Subject: [PATCH] Cleanup, c++11 --- examples/ImuFactorExample2.cpp | 96 ++++++++++++++++++---------------- 1 file changed, 51 insertions(+), 45 deletions(-) diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 059c72d83..1d6b32685 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -1,11 +1,13 @@ -#include -#include +#include +#include #include #include +#include #include -#include -#include +#include + +#include #define GTSAM4 @@ -13,36 +15,32 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -std::vector createPoses() { - +vector createPoses() { // Create the set of ground-truth poses - std::vector poses; + vector 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); + Point3 up(0, 0, 1); + Point3 target(0, 0, 0); + for (; i < 80; ++i, theta += 2 * M_PI / 8) { + Point3 position(radius * cos(theta), radius * sin(theta), 0.0); + SimpleCamera camera = 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 poses = createPoses(); // Create a factor graph - NonlinearFactorGraph newgraph; - NonlinearFactorGraph totalgraph; + NonlinearFactorGraph newgraph, totalgraph; - // Create ISAM2 solver - ISAM2 isam, isam_full; + // Create (incremental) ISAM2 solver + ISAM2 isam; // Create the initial estimate to the solution // Intentionally initialize the variables off from the ground truth @@ -50,41 +48,46 @@ int main(int argc, char* argv[]) { // 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( + auto noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); newgraph.push_back(PriorFactor(0, poses[0], noise)); totalgraph.push_back(PriorFactor(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 biasprior(biasidx,imuBias::ConstantBias(),biasnoise); + auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1)); + PriorFactor 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()); + auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); + + Vector gravity(3), zero(3); + gravity << 0, 0, -9.8; + zero << 0, 0, 0; #ifdef GTSAM4 - PriorFactor velprior(100,(Vector(3) << 0, 0, 0).finished(), velnoise); + PriorFactor velprior(100, zero, velnoise); #else - PriorFactor velprior(100,(Vector(3) << 0, 0, 0).finished(), velnoise); + PriorFactor velprior(100, zero, 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()); + initialEstimate.insert(100, zero); + totalEstimate.insert(100, zero); #else - initialEstimate.insert(100, LieVector((Vector(3) << 0, 0, 0).finished())); - totalEstimate.insert(100, LieVector((Vector(3) << 0, 0, 0).finished())); + initialEstimate.insert(100, LieVector(zero)); + totalEstimate.insert(100, LieVector(zero)); #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; + Matrix3 accCov = I * 0.1; + Matrix3 gyroCov = I * 0.1; + Matrix3 intCov = I * 0.1; bool secOrder = false; #ifdef GTSAM4 // IMU preintegrator @@ -95,7 +98,8 @@ int main(int argc, char* argv[]) { accum.params()->setUse2ndOrderCoriolis(secOrder); accum.params()->setOmegaCoriolis(Vector3(0, 0, 0)); #else - ImuFactor::PreintegratedMeasurements accum(imuBias::ConstantBias(), accCov, gyroCov, intCov, secOrder); + ImuFactor::PreintegratedMeasurements accum(imuBias::ConstantBias(), accCov, + gyroCov, intCov, secOrder); #endif // Simulate poses and imu measurements, adding them to the factor graph @@ -119,42 +123,45 @@ int main(int argc, char* argv[]) { // Add Bias variables periodically if (i % 5 == 0) { biasidx++; - Symbol b1 = biasidx-1; + 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); + auto cov = noiseModel::Diagonal::Variances(covvec); Vector6 zerovec; zerovec << 0, 0, 0, 0, 0, 0; - BetweenFactor::shared_ptr f(new BetweenFactor(b1, b2, imuBias::ConstantBias(), cov)); + auto f = boost::make_shared >( + 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); + accum.integrateMeasurement(gravity, zero, 0.5); #ifdef GTSAM4 - ImuFactor imufac(i-1, 100+i-1,i,100+i, biasidx, accum); + 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()); + ImuFactor imufac(i - 1, 100 + i - 1, i, 100 + i, biasidx, accum, gravity, + zero); #endif newgraph.add(imufac); totalgraph.add(imufac); + + // insert new velocity #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 + initialEstimate.insert(100 + i, gravity); + totalEstimate.insert(100 + i, gravity); #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 + initialEstimate.insert(100 + i, LieVector(gravity)); + totalEstimate.insert(100 + i, LieVector(gravity)); #endif accum.resetIntegration(); } // Batch solution - isam_full = ISAM2(); + ISAM2 isam_full; isam_full.update(totalgraph, totalEstimate); result = isam_full.calculateEstimate(); @@ -168,4 +175,3 @@ int main(int argc, char* argv[]) { return 0; } /* ************************************************************************* */ -