diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index ebaac166a..10bd3de11 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -31,24 +31,6 @@ def vector3(x, y, z): return np.array([x, y, z], dtype=np.float) -def create_poses(angular_velocity=np.pi, - delta_t=0.01, radius=30.0): - """Create the set of ground-truth poses.""" - poses = [] - theta = 0.0 - up = gtsam.Point3(0, 0, 1) - target = gtsam.Point3(0, 0, 0) - for i in range(80): - position = gtsam.Point3(radius * math.cos(theta), - radius * math.sin(theta), 0.0) - camera = gtsam.SimpleCamera.Lookat( - position, target, up, gtsam.Cal3_S2()) - poses.append(camera.pose()) - theta += delta_t * angular_velocity - - return poses - - def ISAM2_plot(values, fignum=0): """Plot poses.""" fig = plt.figure(fignum) @@ -77,8 +59,9 @@ def ISAM2_plot(values, fignum=0): # IMU preintegration parameters # Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis g = 9.81 -I = np.eye(3) +n_gravity = vector3(0, 0, -g) PARAMS = gtsam.PreintegrationParams.MakeSharedU(g) +I = np.eye(3) PARAMS.setAccelerometerCovariance(I * 0.1) PARAMS.setGyroscopeCovariance(I * 0.1) PARAMS.setIntegrationCovariance(I * 0.1) @@ -86,16 +69,29 @@ PARAMS.setUse2ndOrderCoriolis(False) PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1) +DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), + gtsam.Point3(0.05, -0.10, 0.20)) def IMU_example(): + """Run iSAM 2 example with IMU factor.""" + + # Start with a camera on x-axis looking at origin + radius = 30 + up = gtsam.Point3(0, 0, 1) + target = gtsam.Point3(0, 0, 0) + position = gtsam.Point3(radius, 0, 0) + camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2()) + pose_0 = camera.pose() # Create the set of ground-truth landmarks and poses angular_velocity = math.radians(180) # rad/sec delta_t = 1.0/18 # makes for 10 degrees per step - radius = 30 - acceleration = radius * angular_velocity**2 - poses = create_poses(angular_velocity, delta_t, radius) + + angular_velocity_vector = vector3(0, -angular_velocity, 0) + linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) + scenario = gtsam.ConstantTwistScenario( + angular_velocity_vector, linear_velocity_vector, pose_0) # Create a factor graph newgraph = gtsam.NonlinearFactorGraph() @@ -111,7 +107,7 @@ def IMU_example(): # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) - newgraph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], noise)) + newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise)) # Add imu priors biasKey = gtsam.symbol(ord('b'), 0) @@ -123,22 +119,23 @@ def IMU_example(): velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) # Calculate with correct initial velocity - velocity = vector3(0, angular_velocity * radius, 0) - velprior = gtsam.PriorFactorVector(V(0), velocity, velnoise) + n_velocity = vector3(0, angular_velocity * radius, 0) + velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise) newgraph.push_back(velprior) - initialEstimate.insert(V(0), velocity) + initialEstimate.insert(V(0), n_velocity) accum = gtsam.PreintegratedImuMeasurements(PARAMS) # Simulate poses and imu measurements, adding them to the factor graph - for i, pose_i in enumerate(poses): - delta = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), - gtsam.Point3(0.05, -0.10, 0.20)) + for i in range(80): + t = i * delta_t # simulation time if i == 0: # First time add two poses - initialEstimate.insert(X(0), poses[0].compose(delta)) - initialEstimate.insert(X(1), poses[1].compose(delta)) + pose_1 = scenario.pose(delta_t) + initialEstimate.insert(X(0), pose_0.compose(DELTA)) + initialEstimate.insert(X(1), pose_1.compose(DELTA)) elif i >= 2: # Add more poses as necessary - initialEstimate.insert(X(i), pose_i.compose(delta)) + pose_i = scenario.pose(t) + initialEstimate.insert(X(i), pose_i.compose(DELTA)) if i > 0: # Add Bias variables periodically @@ -150,11 +147,10 @@ def IMU_example(): initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) # Predict acceleration and gyro measurements in (actual) body frame - nRb = pose_i.rotation().matrix() + nRb = scenario.rotation(t).matrix() bRn = np.transpose(nRb) - measuredAcc = - np.dot(bRn, vector3(0, 0, -g)) + \ - vector3(0, 0, acceleration) # in body - measuredOmega = np.dot(bRn, vector3(0, 0, angular_velocity)) + measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity) + measuredOmega = scenario.omega_b(t) accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) # Add Imu Factor @@ -162,8 +158,8 @@ def IMU_example(): X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) newgraph.add(imufac) - # insert new velocity - initialEstimate.insert(V(i), velocity) + # insert new velocity, which is wrong + initialEstimate.insert(V(i), n_velocity) accum.resetIntegration() # Incremental solution diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 97002eb2a..1137ca214 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -3,42 +3,51 @@ #include #include #include +#include #include #include #include #include -#define GTSAM4 - using namespace std; using namespace gtsam; -/* ************************************************************************* */ -vector createPoses() { - // Create the set of ground-truth poses - vector poses; - double radius = 30.0, theta = 0.0; - Point3 up(0, 0, 1), target(0, 0, 0); - for (size_t i = 0; 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; -} +// Shorthand for velocity and pose variables +using symbol_shorthand::V; +using symbol_shorthand::X; + +const double kGravity = 9.81; /* ************************************************************************* */ int main(int argc, char* argv[]) { - // Shorthand for velocity and pose variables - using symbol_shorthand::V; - using symbol_shorthand::X; + auto params = PreintegrationParams::MakeSharedU(kGravity); + params->setAccelerometerCovariance(I_3x3 * 0.1); + params->setGyroscopeCovariance(I_3x3 * 0.1); + params->setIntegrationCovariance(I_3x3 * 0.1); + params->setUse2ndOrderCoriolis(false); + params->setOmegaCoriolis(Vector3(0, 0, 0)); - // Create the set of ground-truth landmarks and poses - vector poses = createPoses(); + Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + + // Start with a camera on x-axis looking at origin + double radius = 30; + const Point3 up(0, 0, 1), target(0, 0, 0); + const Point3 position(radius, 0, 0); + const SimpleCamera camera = SimpleCamera::Lookat(position, target, up); + const auto pose_0 = camera.pose(); + + // Now, create a constant-twist scenario that makes the camera orbit the + // origin + double angular_velocity = M_PI, // rad/sec + delta_t = 1.0 / 18; // makes for 10 degrees per step + Vector3 angular_velocity_vector(0, -angular_velocity, 0); + Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0); + auto scenario = ConstantTwistScenario(angular_velocity_vector, + linear_velocity_vector, pose_0); // Create a factor graph - NonlinearFactorGraph newgraph, totalgraph; + NonlinearFactorGraph newgraph; // Create (incremental) ISAM2 solver ISAM2 isam; @@ -51,8 +60,7 @@ int main(int argc, char* argv[]) { // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw auto noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - newgraph.push_back(PriorFactor(X(0), poses[0], noise)); - totalgraph.push_back(PriorFactor(X(0), poses[0], noise)); + newgraph.push_back(PriorFactor(X(0), pose_0, noise)); // Add imu priors Key biasKey = Symbol('b', 0); @@ -60,65 +68,29 @@ int main(int argc, char* argv[]) { PriorFactor biasprior(biasKey, imuBias::ConstantBias(), biasnoise); newgraph.push_back(biasprior); - totalgraph.push_back(biasprior); initialEstimate.insert(biasKey, imuBias::ConstantBias()); - totalEstimate.insert(biasKey, imuBias::ConstantBias()); auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); - Vector gravity(3), zero(3), velocity(3); - gravity << 0, 0, -9.8; - zero << 0, 0, 0; - velocity << 0, 0, 0; -#ifdef GTSAM4 - PriorFactor velprior(V(0), zero, velnoise); -#else - PriorFactor velprior(V(0), zero, velnoise); -#endif + Vector n_velocity(3); + n_velocity << 0, angular_velocity * radius, 0; + PriorFactor velprior(V(0), n_velocity, velnoise); newgraph.push_back(velprior); - totalgraph.push_back(velprior); -#ifdef GTSAM4 - initialEstimate.insert(V(0), zero); - totalEstimate.insert(V(0), zero); -#else - initialEstimate.insert(V(0), LieVector(zero)); - totalEstimate.insert(V(0), LieVector(zero)); -#endif + initialEstimate.insert(V(0), n_velocity); - 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 + PreintegratedImuMeasurements accum(params); // 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 + for (size_t i = 0; i < 36; ++i) { + double t = i * delta_t; if (i == 0) { // First time add two poses - initialEstimate.insert(X(0), poses[0].compose(delta)); - initialEstimate.insert(X(1), poses[1].compose(delta)); - totalEstimate.insert(X(0), poses[0].compose(delta)); - totalEstimate.insert(X(1), poses[1].compose(delta)); + auto pose_1 = scenario.pose(delta_t); + initialEstimate.insert(X(0), pose_0.compose(delta)); + initialEstimate.insert(X(1), pose_1.compose(delta)); } else if (i >= 2) { // Add more poses as necessary - initialEstimate.insert(X(i), poses[i].compose(delta)); - totalEstimate.insert(X(i), poses[i].compose(delta)); + auto pose_i = scenario.pose(t); + initialEstimate.insert(X(i), pose_i.compose(delta)); } if (i > 0) { @@ -135,44 +107,30 @@ int main(int argc, char* argv[]) { auto f = boost::make_shared >( b1, b2, imuBias::ConstantBias(), cov); newgraph.add(f); - totalgraph.add(f); initialEstimate.insert(biasKey, imuBias::ConstantBias()); - totalEstimate.insert(biasKey, imuBias::ConstantBias()); } - // Add Imu Factor - accum.integrateMeasurement(gravity, zero, 0.5); -#ifdef GTSAM4 - ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum); -#else - ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum, gravity, - zero); -#endif - newgraph.add(imufac); - totalgraph.add(imufac); + // Predict acceleration and gyro measurements in (actual) body frame + auto measuredAcc = scenario.acceleration_b(t) - + scenario.rotation(t).transpose() * params->n_gravity; + auto measuredOmega = scenario.omega_b(t); + accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t); - // insert new velocity -#ifdef GTSAM4 - initialEstimate.insert(V(i), velocity); - totalEstimate.insert(V(i), velocity); -#else - initialEstimate.insert(V(i), LieVector(velocity)); - totalEstimate.insert(V(i), LieVector(velocity)); -#endif + // Add Imu Factor + ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum); + newgraph.add(imufac); + + // insert new velocity, which is wrong + initialEstimate.insert(V(i), n_velocity); accum.resetIntegration(); } - // Batch solution - ISAM2 isam_full; - isam_full.update(totalgraph, totalEstimate); - result = isam_full.calculateEstimate(); - // Incremental solution isam.update(newgraph, initialEstimate); result = isam.calculateEstimate(); newgraph = NonlinearFactorGraph(); initialEstimate.clear(); } - + GTSAM_PRINT(result); return 0; } /* ************************************************************************* */