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

Added symbol keys

release/4.3a0
Frank Dellaert 2018-10-13 17:48:36 -04:00
parent 18234f68fd
commit eb447d28a1
1 changed files with 38 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
examples/ImuFactorExample2.cpp
View File

@ -18,12 +18,9 @@ using namespace gtsam;
vector<Pose3> createPoses() { vector<Pose3> createPoses() {
// Create the set of ground-truth poses // Create the set of ground-truth poses
vector<Pose3> poses; vector<Pose3> poses;
double radius = 30.0; double radius = 30.0, theta = 0.0;
int i = 0; Point3 up(0, 0, 1), target(0, 0, 0);
double theta = 0.0; for (size_t i = 0; i < 80; ++i, theta += 2 * M_PI / 8) {
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); Point3 position(radius * cos(theta), radius * sin(theta), 0.0);
SimpleCamera camera = SimpleCamera::Lookat(position, target, up); SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
poses.push_back(camera.pose()); poses.push_back(camera.pose());
@ -33,6 +30,10 @@ vector<Pose3> createPoses() {
/* ************************************************************************* */ /* ************************************************************************* */
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
// Shorthand for velocity and pose variables
using symbol_shorthand::V;
using symbol_shorthand::X;
// 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();
@ -50,37 +51,38 @@ int main(int argc, char* argv[]) {
// 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
auto 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>(X(0), poses[0], noise));
totalgraph.push_back(PriorFactor<Pose3>(0, poses[0], noise)); totalgraph.push_back(PriorFactor<Pose3>(X(0), poses[0], noise));
// Add imu priors // Add imu priors
int biasidx = 1000; Key biasKey = Symbol('b', 0);
auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1)); auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1));
PriorFactor<imuBias::ConstantBias> biasprior(biasidx, imuBias::ConstantBias(), PriorFactor<imuBias::ConstantBias> biasprior(biasKey, imuBias::ConstantBias(),
biasnoise); biasnoise);
newgraph.push_back(biasprior); newgraph.push_back(biasprior);
totalgraph.push_back(biasprior); totalgraph.push_back(biasprior);
initialEstimate.insert(biasidx, imuBias::ConstantBias()); initialEstimate.insert(biasKey, imuBias::ConstantBias());
totalEstimate.insert(biasidx, imuBias::ConstantBias()); totalEstimate.insert(biasKey, imuBias::ConstantBias());
auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
Vector gravity(3), zero(3); Vector gravity(3), zero(3), velocity(3);
gravity << 0, 0, -9.8; gravity << 0, 0, -9.8;
zero << 0, 0, 0; zero << 0, 0, 0;
velocity << 0, 0, 0;
#ifdef GTSAM4 #ifdef GTSAM4
PriorFactor<Vector> velprior(100, zero, velnoise); PriorFactor<Vector> velprior(V(0), zero, velnoise);
#else #else
PriorFactor<LieVector> velprior(100, zero, velnoise); PriorFactor<LieVector> velprior(V(0), 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, zero); initialEstimate.insert(V(0), zero);
totalEstimate.insert(100, zero); totalEstimate.insert(V(0), zero);
#else #else
initialEstimate.insert(100, LieVector(zero)); initialEstimate.insert(V(0), LieVector(zero));
totalEstimate.insert(100, LieVector(zero)); totalEstimate.insert(V(0), LieVector(zero));
#endif #endif
Matrix3 I; Matrix3 I;
@ -110,21 +112,21 @@ int main(int argc, char* argv[]) {
Pose3 delta(Rot3::ypr(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); Pose3 delta(Rot3::ypr(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
#endif #endif
if (i == 0) { // First time add two poses if (i == 0) { // First time add two poses
initialEstimate.insert(0, poses[0].compose(delta)); initialEstimate.insert(X(0), poses[0].compose(delta));
initialEstimate.insert(1, poses[1].compose(delta)); initialEstimate.insert(X(1), poses[1].compose(delta));
totalEstimate.insert(0, poses[0].compose(delta)); totalEstimate.insert(X(0), poses[0].compose(delta));
totalEstimate.insert(1, poses[1].compose(delta)); totalEstimate.insert(X(1), poses[1].compose(delta));
} else if (i >= 2) { // Add more poses as necessary } else if (i >= 2) { // Add more poses as necessary
initialEstimate.insert(i, poses[i].compose(delta)); initialEstimate.insert(X(i), poses[i].compose(delta));
totalEstimate.insert(i, poses[i].compose(delta)); totalEstimate.insert(X(i), poses[i].compose(delta));
} }
if (i > 0) { if (i > 0) {
// Add Bias variables periodically // Add Bias variables periodically
if (i % 5 == 0) { if (i % 5 == 0) {
biasidx++; biasKey++;
Symbol b1 = biasidx - 1; Symbol b1 = biasKey - 1;
Symbol b2 = biasidx; Symbol b2 = biasKey;
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;
auto cov = noiseModel::Diagonal::Variances(covvec); auto cov = noiseModel::Diagonal::Variances(covvec);
@ -134,28 +136,27 @@ int main(int argc, char* argv[]) {
b1, b2, imuBias::ConstantBias(), cov); b1, b2, imuBias::ConstantBias(), cov);
newgraph.add(f); newgraph.add(f);
totalgraph.add(f); totalgraph.add(f);
initialEstimate.insert(biasidx, imuBias::ConstantBias()); initialEstimate.insert(biasKey, imuBias::ConstantBias());
totalEstimate.insert(biasidx, imuBias::ConstantBias()); totalEstimate.insert(biasKey, imuBias::ConstantBias());
} }
// Add Imu Factor // Add Imu Factor
accum.integrateMeasurement(gravity, zero, 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(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum);
#else #else
ImuFactor imufac(i - 1, 100 + i - 1, i, 100 + i, biasidx, accum, gravity, ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum, gravity,
zero); zero);
#endif #endif
newgraph.add(imufac); newgraph.add(imufac);
totalgraph.add(imufac); totalgraph.add(imufac);
// insert new velocity // insert new velocity
#ifdef GTSAM4 #ifdef GTSAM4
initialEstimate.insert(100 + i, gravity); initialEstimate.insert(V(i), velocity);
totalEstimate.insert(100 + i, gravity); totalEstimate.insert(V(i), velocity);
#else #else
initialEstimate.insert(100 + i, LieVector(gravity)); initialEstimate.insert(V(i), LieVector(velocity));
totalEstimate.insert(100 + i, LieVector(gravity)); totalEstimate.insert(V(i), LieVector(velocity));
#endif #endif
accum.resetIntegration(); accum.resetIntegration();
} }