12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Cleanup, c++11

release/4.3a0
Frank Dellaert 2018-10-13 16:25:58 -04:00
parent c428e30784
commit 18234f68fd
1 changed files with 51 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
examples/ImuFactorExample2.cpp
View File

@ -1,11 +1,13 @@
#include <gtsam/nonlinear/ISAM2.h> #include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/ImuFactor.h> #include <gtsam/navigation/ImuFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <vector>
#define GTSAM4 #define GTSAM4
@ -13,36 +15,32 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
std::vector<gtsam::Pose3> createPoses() { vector<Pose3> createPoses() {
// Create the set of ground-truth poses // Create the set of ground-truth poses
std::vector<gtsam::Pose3> poses; vector<Pose3> poses;
double radius = 30.0; double radius = 30.0;
int i = 0; int i = 0;
double theta = 0.0; double theta = 0.0;
gtsam::Point3 up(0,0,1); Point3 up(0, 0, 1);
gtsam::Point3 target(0,0,0); Point3 target(0, 0, 0);
for (; i < 80; ++i, theta += 2 * M_PI / 8) { for (; i < 80; ++i, theta += 2 * M_PI / 8) {
gtsam::Point3 position = gtsam::Point3(radius*cos(theta), radius*sin(theta), 0.0); Point3 position(radius * cos(theta), radius * sin(theta), 0.0);
gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(position, target, up); SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
poses.push_back(camera.pose()); poses.push_back(camera.pose());
} }
return poses; return poses;
} }
/* ************************************************************************* */
/* ************************************************************************* */ /* ************************************************************************* */
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
// Create the set of ground-truth landmarks and poses // Create the set of ground-truth landmarks and poses
vector<Pose3> poses = createPoses(); vector<Pose3> poses = createPoses();
// Create a factor graph // Create a factor graph
NonlinearFactorGraph newgraph; NonlinearFactorGraph newgraph, totalgraph;
NonlinearFactorGraph totalgraph;
// Create ISAM2 solver // Create (incremental) ISAM2 solver
ISAM2 isam, isam_full; ISAM2 isam;
// Create the initial estimate to the solution // Create the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth // Intentionally initialize the variables off from the ground truth
@ -50,34 +48,39 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x0. This indirectly specifies where the origin is. // 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 // 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()); (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
newgraph.push_back(PriorFactor<Pose3>(0, poses[0], noise)); newgraph.push_back(PriorFactor<Pose3>(0, poses[0], noise));
totalgraph.push_back(PriorFactor<Pose3>(0, poses[0], noise)); totalgraph.push_back(PriorFactor<Pose3>(0, poses[0], noise));
// Add imu priors // Add imu priors
int biasidx = 1000; 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()); auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1));
PriorFactor<imuBias::ConstantBias> biasprior(biasidx,imuBias::ConstantBias(),biasnoise); PriorFactor<imuBias::ConstantBias> biasprior(biasidx, imuBias::ConstantBias(),
biasnoise);
newgraph.push_back(biasprior); newgraph.push_back(biasprior);
totalgraph.push_back(biasprior); totalgraph.push_back(biasprior);
initialEstimate.insert(biasidx, imuBias::ConstantBias()); initialEstimate.insert(biasidx, imuBias::ConstantBias());
totalEstimate.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 #ifdef GTSAM4
PriorFactor<Vector> velprior(100,(Vector(3) << 0, 0, 0).finished(), velnoise); PriorFactor<Vector> velprior(100, zero, velnoise);
#else #else
PriorFactor<LieVector> velprior(100,(Vector(3) << 0, 0, 0).finished(), velnoise); PriorFactor<LieVector> velprior(100, zero, velnoise);
#endif #endif
newgraph.push_back(velprior); newgraph.push_back(velprior);
totalgraph.push_back(velprior); totalgraph.push_back(velprior);
#ifdef GTSAM4 #ifdef GTSAM4
initialEstimate.insert(100, (Vector(3) << 0, 0, 0).finished()); initialEstimate.insert(100, zero);
totalEstimate.insert(100, (Vector(3) << 0, 0, 0).finished()); totalEstimate.insert(100, zero);
#else #else
initialEstimate.insert(100, LieVector((Vector(3) << 0, 0, 0).finished())); initialEstimate.insert(100, LieVector(zero));
totalEstimate.insert(100, LieVector((Vector(3) << 0, 0, 0).finished())); totalEstimate.insert(100, LieVector(zero));
#endif #endif
Matrix3 I; Matrix3 I;
@ -95,7 +98,8 @@ int main(int argc, char* argv[]) {
accum.params()->setUse2ndOrderCoriolis(secOrder); accum.params()->setUse2ndOrderCoriolis(secOrder);
accum.params()->setOmegaCoriolis(Vector3(0, 0, 0)); accum.params()->setOmegaCoriolis(Vector3(0, 0, 0));
#else #else
ImuFactor::PreintegratedMeasurements accum(imuBias::ConstantBias(), accCov, gyroCov, intCov, secOrder); ImuFactor::PreintegratedMeasurements accum(imuBias::ConstantBias(), accCov,
gyroCov, intCov, secOrder);
#endif #endif
// Simulate poses and imu measurements, adding them to the factor graph // Simulate poses and imu measurements, adding them to the factor graph
@ -121,40 +125,43 @@ int main(int argc, char* argv[]) {
biasidx++; biasidx++;
Symbol b1 = biasidx - 1; Symbol b1 = biasidx - 1;
Symbol b2 = biasidx; Symbol b2 = biasidx;
imuBias::ConstantBias basebias = imuBias::ConstantBias();
Vector6 covvec; Vector6 covvec;
covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1; 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; Vector6 zerovec;
zerovec << 0, 0, 0, 0, 0, 0; zerovec << 0, 0, 0, 0, 0, 0;
BetweenFactor<imuBias::ConstantBias>::shared_ptr f(new BetweenFactor<imuBias::ConstantBias>(b1, b2, imuBias::ConstantBias(), cov)); auto f = boost::make_shared<BetweenFactor<imuBias::ConstantBias> >(
b1, b2, imuBias::ConstantBias(), cov);
newgraph.add(f); newgraph.add(f);
totalgraph.add(f); totalgraph.add(f);
initialEstimate.insert(biasidx, imuBias::ConstantBias()); initialEstimate.insert(biasidx, imuBias::ConstantBias());
totalEstimate.insert(biasidx, imuBias::ConstantBias()); totalEstimate.insert(biasidx, imuBias::ConstantBias());
} }
// Add Imu Factor // 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 #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 #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 #endif
newgraph.add(imufac); newgraph.add(imufac);
totalgraph.add(imufac); totalgraph.add(imufac);
// insert new velocity
#ifdef GTSAM4 #ifdef GTSAM4
initialEstimate.insert(100+i, (Vector(3) << 0.0, 0.0, -9.8).finished()); // insert new velocity initialEstimate.insert(100 + i, gravity);
totalEstimate.insert(100+i, (Vector(3) << 0.0, 0.0, -9.8).finished()); // insert new velocity totalEstimate.insert(100 + i, gravity);
#else #else
initialEstimate.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((Vector(3) << 0.0, 0.0, -9.8).finished())); // insert new velocity totalEstimate.insert(100 + i, LieVector(gravity));
#endif #endif
accum.resetIntegration(); accum.resetIntegration();
} }
// Batch solution // Batch solution
isam_full = ISAM2(); ISAM2 isam_full;
isam_full.update(totalgraph, totalEstimate); isam_full.update(totalgraph, totalEstimate);
result = isam_full.calculateEstimate(); result = isam_full.calculateEstimate();
@ -168,4 +175,3 @@ int main(int argc, char* argv[]) {
return 0; return 0;
} }
/* ************************************************************************* */ /* ************************************************************************* */