/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr noise = //
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks
vector<Point3> points = createPoints();
// Create the set of ground-truth poses
vector<Pose3> poses = createPoses();
// Create a NonlinearISAM object which will relinearize and reorder the variables
// every "relinearizeInterval" updates
int relinearizeInterval = 3;
NonlinearISAM isam(relinearizeInterval);
// Create a Factor Graph and Values to hold the new data
NonlinearFactorGraph graph;
Values initialEstimate;
// Loop over the different poses, adding the observations to iSAM incrementally
for (size_t i = 0; i < poses.size(); ++i) {
// Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) {
// Create ground truth measurement
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
// Add measurement
graph.add(
GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, noise,
Symbol('x', i), Symbol('l', j), K));
}
// Intentionally initialize the variables off from the ground truth
Pose3 noise(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
Pose3 initial_xi = poses[i].compose(noise);
// Add an initial guess for the current pose
initialEstimate.insert(Symbol('x', i), initial_xi);
// If this is the first iteration, add a prior on the first pose to set the coordinate frame
// and a prior on the first landmark to set the scale
// Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
// adding it to iSAM.
if (i == 0) {
// Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise =
noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise));
// Add initial guesses to all observed landmarks
Point3 noise(-0.25, 0.20, 0.15);
for (size_t j = 0; j < points.size(); ++j) {
// Intentionally initialize the variables off from the ground truth
Point3 initial_lj = points[j].compose(noise);
initialEstimate.insert(Symbol('l', j), initial_lj);
}
} else {
// Update iSAM with the new factors
isam.update(graph, initialEstimate);
Values currentEstimate = isam.estimate();
cout << "****************************************************" << endl;
cout << "Frame " << i << ": " << endl;
currentEstimate.print("Current estimate: ");
// Clear the factor graph and values for the next iteration
graph.resize(0);
initialEstimate.clear();
}
}
return 0;
}
int main(int argc, char** argv) {
// Define the smoother lag (in seconds)
double lag = 2.0;
// Create a Concurrent Filter and Smoother
ConcurrentBatchFilter concurrentFilter;
ConcurrentBatchSmoother concurrentSmoother;
// And a fixed lag smoother with a short lag
BatchFixedLagSmoother fixedlagSmoother(lag);
// And a fixed lag smoother with very long lag (i.e. a full batch smoother)
BatchFixedLagSmoother batchSmoother(1000.0);
// Create containers to store the factors and linearization points that
// will be sent to the smoothers
NonlinearFactorGraph newFactors;
Values newValues;
FixedLagSmoother::KeyTimestampMap newTimestamps;
// Create a prior on the first pose, placing it at the origin
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
Key priorKey = 0;
newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds;
// Now, loop through several time steps, creating factors from different "sensors"
// and adding them to the fixed-lag smoothers
double deltaT = 0.25;
for(double time = 0.0+deltaT; time <= 5.0; time += deltaT) {
// Define the keys related to this timestamp
Key previousKey(1000 * (time-deltaT));
Key currentKey(1000 * (time));
// Assign the current key to the current timestamp
newTimestamps[currentKey] = time;
// Add a guess for this pose to the new values
// Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
// {This is not a particularly good way to guess, but this is just an example}
Pose2 currentPose(time * 2.0, 0.0, 0.0);
newValues.insert(currentKey, currentPose);
// Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Unlike the fixed-lag versions, the concurrent filter implementation
// requires the user to supply the specify which keys to move to the smoother
FastList<Key> oldKeys;
if(time >= lag+deltaT) {
oldKeys.push_back(1000 * (time-lag-deltaT));
}
// Update the various inference engines
concurrentFilter.update(newFactors, newValues, oldKeys);
fixedlagSmoother.update(newFactors, newValues, newTimestamps);
batchSmoother.update(newFactors, newValues, newTimestamps);
// Manually synchronize the Concurrent Filter and Smoother every 1.0 s
if(fmod(time, 1.0) < 0.01) {
// Synchronize the Filter and Smoother
concurrentSmoother.update();
synchronize(concurrentFilter, concurrentSmoother);
}
// Print the optimized current pose
cout << setprecision(5) << "Timestamp = " << time << endl;
concurrentFilter.calculateEstimate<Pose2>(currentKey).print("Concurrent Estimate: ");
fixedlagSmoother.calculateEstimate<Pose2>(currentKey).print("Fixed Lag Estimate: ");
batchSmoother.calculateEstimate<Pose2>(currentKey).print("Batch Estimate: ");
cout << endl;
// Clear contains for the next iteration
newTimestamps.clear();
newValues.clear();
newFactors.resize(0);
}
cout << "******************************************************************" << endl;
cout << "All three versions should be identical." << endl;
cout << "Adding a loop closure factor to the Batch version only." << endl;
cout << "******************************************************************" << endl;
cout << endl;
// At the moment, all three versions produce the same output.
// Now lets create a "loop closure" factor between the first pose and the current pose
Key loopKey1(1000 * (0.0));
Key loopKey2(1000 * (5.0));
Pose2 loopMeasurement = Pose2(9.5, 1.00, 0.00);
noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.25));
NonlinearFactor::shared_ptr loopFactor(new BetweenFactor<Pose2>(loopKey1, loopKey2, loopMeasurement, loopNoise));
// This measurement cannot be added directly to the concurrent filter because it connects a filter state to a smoother state
// This measurement can never be added to the fixed-lag smoother, as one of the poses has been permanently marginalized out
// This measurement can be incorporated into the full batch version though
newFactors.push_back(loopFactor);
batchSmoother.update(newFactors, Values(), FixedLagSmoother::KeyTimestampMap());
newFactors.resize(0);
// Continue adding odometry factors until the loop closure may be incorporated into the concurrent smoother
for(double time = 5.0+deltaT; time <= 8.0; time += deltaT) {
// Define the keys related to this timestamp
Key previousKey(1000 * (time-deltaT));
Key currentKey(1000 * (time));
// Assign the current key to the current timestamp
newTimestamps[currentKey] = time;
// Add a guess for this pose to the new values
// Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
// {This is not a particularly good way to guess, but this is just an example}
Pose2 currentPose(time * 2.0, 0.0, 0.0);
newValues.insert(currentKey, currentPose);
// Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Unlike the fixed-lag versions, the concurrent filter implementation
// requires the user to supply the specify which keys to marginalize
FastList<Key> oldKeys;
if(time >= lag+deltaT) {
oldKeys.push_back(1000 * (time-lag-deltaT));
}
// Update the various inference engines
concurrentFilter.update(newFactors, newValues, oldKeys);
fixedlagSmoother.update(newFactors, newValues, newTimestamps);
batchSmoother.update(newFactors, newValues, newTimestamps);
// Manually synchronize the Concurrent Filter and Smoother every 1.0 s
if(fmod(time, 1.0) < 0.01) {
// Synchronize the Filter and Smoother
concurrentSmoother.update();
synchronize(concurrentFilter, concurrentSmoother);
}
// Print the optimized current pose
cout << setprecision(5) << "Timestamp = " << time << endl;
concurrentFilter.calculateEstimate<Pose2>(currentKey).print("Concurrent Estimate: ");
fixedlagSmoother.calculateEstimate<Pose2>(currentKey).print("Fixed Lag Estimate: ");
batchSmoother.calculateEstimate<Pose2>(currentKey).print("Batch Estimate: ");
cout << endl;
// Clear contains for the next iteration
newTimestamps.clear();
newValues.clear();
newFactors.resize(0);
}
cout << "******************************************************************" << endl;
cout << "The Concurrent system and the Fixed-Lag Smoother should be " << endl;
cout << "the same, but the Batch version has a loop closure." << endl;
cout << "Adding the loop closure factor to the Concurrent version." << endl;
cout << "This will not update the Concurrent Filter until the next " << endl;
cout << "synchronization, but the Concurrent solution should be identical " << endl;
cout << "to the Batch solution afterwards." << endl;
cout << "******************************************************************" << endl;
cout << endl;
// The state at 5.0s should have been transferred to the concurrent smoother at this point. Add the loop closure.
newFactors.push_back(loopFactor);
concurrentSmoother.update(newFactors, Values());
newFactors.resize(0);
// Now run for a few more seconds so the concurrent smoother and filter have to to re-sync
// Continue adding odometry factors until the loop closure may be incorporated into the concurrent smoother
for(double time = 8.0+deltaT; time <= 15.0; time += deltaT) {
// Define the keys related to this timestamp
Key previousKey(1000 * (time-deltaT));
Key currentKey(1000 * (time));
// Assign the current key to the current timestamp
newTimestamps[currentKey] = time;
// Add a guess for this pose to the new values
// Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
// {This is not a particularly good way to guess, but this is just an example}
Pose2 currentPose(time * 2.0, 0.0, 0.0);
newValues.insert(currentKey, currentPose);
// Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Unlike the fixed-lag versions, the concurrent filter implementation
// requires the user to supply the specify which keys to marginalize
FastList<Key> oldKeys;
if(time >= lag+deltaT) {
oldKeys.push_back(1000 * (time-lag-deltaT));
}
// Update the various inference engines
concurrentFilter.update(newFactors, newValues, oldKeys);
fixedlagSmoother.update(newFactors, newValues, newTimestamps);
batchSmoother.update(newFactors, newValues, newTimestamps);
// Manually synchronize the Concurrent Filter and Smoother every 1.0 s
if(fmod(time, 1.0) < 0.01) {
// Synchronize the Filter and Smoother
concurrentSmoother.update();
synchronize(concurrentFilter, concurrentSmoother);
cout << "******************************************************************" << endl;
cout << "Syncing Concurrent Filter and Smoother." << endl;
cout << "******************************************************************" << endl;
cout << endl;
}
// Print the optimized current pose
cout << setprecision(5) << "Timestamp = " << time << endl;
concurrentFilter.calculateEstimate<Pose2>(currentKey).print("Concurrent Estimate: ");
fixedlagSmoother.calculateEstimate<Pose2>(currentKey).print("Fixed Lag Estimate: ");
batchSmoother.calculateEstimate<Pose2>(currentKey).print("Batch Estimate: ");
cout << endl;
// Clear contains for the next iteration
newTimestamps.clear();
newValues.clear();
newFactors.resize(0);
}
// And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
cout << "After 15.0 seconds, each version contains to the following keys:" << endl;
cout << " Concurrent Filter Keys: " << endl;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, concurrentFilter.getLinearizationPoint()) {
cout << setprecision(5) << " Key: " << key_value.key << endl;
}
cout << " Concurrent Smoother Keys: " << endl;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, concurrentSmoother.getLinearizationPoint()) {
cout << setprecision(5) << " Key: " << key_value.key << endl;
}
cout << " Fixed-Lag Smoother Keys: " << endl;
BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, fixedlagSmoother.timestamps()) {
cout << setprecision(5) << " Key: " << key_timestamp.first << endl;
}
cout << " Batch Smoother Keys: " << endl;
BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, batchSmoother.timestamps()) {
cout << setprecision(5) << " Key: " << key_timestamp.first << endl;
}
return 0;
}
void StateEstimator::optimizationLoop()
{
ISAM2Params parameters;
// parameters.relinearizeThreshold = 0.0; // Set the relin threshold to zero such that the batch estimate is recovered
// parameters.relinearizeSkip = 1; // Relinearize every time
gtsam::IncrementalFixedLagSmoother graph(optLag_, parameters);
double startTime;
sensor_msgs::ImuConstPtr lastImu;
double lastImuT;
int imuKey = 1;
int gpsKey = 1;
// first we will initialize the graph with appropriate priors
NonlinearFactorGraph newFactors;
Values newVariables;
FixedLagSmoother::KeyTimestampMap newTimestamps;
sensor_msgs::NavSatFixConstPtr fix = gpsQ_.pop();
startTime = ROS_TIME(fix);
enu_.Reset(fix->latitude, fix->longitude, fix->altitude);
sensor_msgs::ImuConstPtr imu = imuQ_.pop();
lastImu = imu;
lastImuT = ROS_TIME(imu) - 1 / imuFreq_;
Rot3 initialOrientation =
Rot3::Quaternion(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
// we set out initial position to the origin and assume we are stationary
Pose3 x0(initialOrientation, Point3(0, 0, 0));
PriorFactor<Pose3> priorPose(X(0), x0,
noiseModel::Diagonal::Sigmas((Vector(6) << priorOSigma_, priorOSigma_, priorOSigma_,
priorPSigma_, priorPSigma_, priorPSigma_)
.finished()));
newFactors.add(priorPose);
Vector3 v0 = Vector3(0, 0, 0);
PriorFactor<Vector3> priorVel(
V(0), v0, noiseModel::Diagonal::Sigmas((Vector(3) << priorVSigma_, priorVSigma_, priorVSigma_).finished()));
newFactors.add(priorVel);
imuBias::ConstantBias b0((Vector(6) << 0, 0, 0, 0, 0, 0).finished());
PriorFactor<imuBias::ConstantBias> priorBias(
B(0), b0,
noiseModel::Diagonal::Sigmas(
(Vector(6) << priorABias_, priorABias_, priorABias_, priorGBias_, priorGBias_, priorGBias_).finished()));
newFactors.add(priorBias);
noiseModel::Diagonal::shared_ptr imuToGpsFactorNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << gpsTSigma_, gpsTSigma_, gpsTSigma_, gpsTSigma_, gpsTSigma_, gpsTSigma_).finished());
newFactors.add(BetweenFactor<Pose3>(X(0), G(0), imuToGps_, imuToGpsFactorNoise));
newVariables.insert(X(0), x0);
newVariables.insert(V(0), v0);
newVariables.insert(B(0), b0);
newVariables.insert(G(0), x0.compose(imuToGps_));
newTimestamps[X(0)] = 0;
newTimestamps[G(0)] = 0;
newTimestamps[V(0)] = 0;
newTimestamps[B(0)] = 0;
graph.update(newFactors, newVariables); //, newTimestamps);
Pose3 prevPose = prevPose_ = x0;
Vector3 prevVel = prevVel_ = v0;
imuBias::ConstantBias prevBias = prevBias_ = b0;
// remove old imu messages
while (!imuQ_.empty() && ROS_TIME(imuQ_.front()) < ROS_TIME(fix))
{
lastImuT = ROS_TIME(lastImu);
lastImu = imuQ_.pop();
}
// setting up the IMU integration
boost::shared_ptr<gtsam::PreintegrationParams> preintegrationParams =
PreintegrationParams::MakeSharedU(gravityMagnitude_);
preintegrationParams->accelerometerCovariance = accelSigma_ * I_3x3;
preintegrationParams->gyroscopeCovariance = gyroSigma_ * I_3x3;
preintegrationParams->integrationCovariance = imuIntSigma_ * I_3x3;
PreintegratedImuMeasurements imuIntegrator(preintegrationParams, prevBias);
Vector noiseModelBetweenBias =
(Vector(6) << accelBSigma_, accelBSigma_, accelBSigma_, gyroBSigma_, gyroBSigma_, gyroBSigma_).finished();
SharedDiagonal gpsNoise = noiseModel::Diagonal::Sigmas(Vector3(gpsSigma_, gpsSigma_, 3 * gpsSigma_));
newFactors.resize(0);
newVariables.clear();
newTimestamps.clear();
// now we loop and let use the queues to grab messages
while (ros::ok())
{
bool optimize = false;
// integrate imu messages
while (!imuQ_.empty() && ROS_TIME(imuQ_.back()) > (startTime + 0.1 * imuKey) && !optimize)
{
double curTime = startTime + 0.1 * imuKey;
// we reset the integrator, then integrate
imuIntegrator.resetIntegrationAndSetBias(prevBias);
while (ROS_TIME(lastImu) < curTime)
{
double dt = ROS_TIME(lastImu) - lastImuT;
imuIntegrator.integrateMeasurement(
Vector3(lastImu->linear_acceleration.x, lastImu->linear_acceleration.y, lastImu->linear_acceleration.z),
Vector3(lastImu->angular_velocity.x, lastImu->angular_velocity.y, lastImu->angular_velocity.z), dt);
lastImuT = ROS_TIME(lastImu);
lastImu = imuQ_.pop();
}
// now put this into the graph
ImuFactor imuf(X(imuKey - 1), V(imuKey - 1), X(imuKey), V(imuKey), B(imuKey - 1), imuIntegrator);
newFactors.add(imuf);
newFactors.add(BetweenFactor<imuBias::ConstantBias>(
B(imuKey - 1), B(imuKey), imuBias::ConstantBias(),
noiseModel::Diagonal::Sigmas(sqrt(imuIntegrator.deltaTij()) * noiseModelBetweenBias)));
Rot3 orientation = Rot3::Quaternion(lastImu->orientation.w, lastImu->orientation.x, lastImu->orientation.y,
lastImu->orientation.z);
// std::cout << "adding orientation: " << orientation.xyz() << std::endl;
newFactors.add(UnaryRotationFactor(X(imuKey), orientation.yaw(),
noiseModel::Diagonal::Sigmas((Vector(1) << yawSigma_).finished())));
NavState cur(prevPose, prevVel);
NavState next = imuIntegrator.predict(cur, prevBias);
prevPose = next.pose();
prevVel = next.v();
newVariables.insert(X(imuKey), prevPose);
newVariables.insert(G(imuKey), prevPose.compose(imuToGps_));
newVariables.insert(V(imuKey), prevVel);
newVariables.insert(B(imuKey), prevBias);
Pose3 temp = prevPose.compose(imuToGps_);
std::cout << "imu(" << imuKey << "): " << temp.x() << " " << temp.y() << " " << temp.z() << std::endl;
// for marginalizing out past the time window
newTimestamps[X(imuKey)] = 0.1 * imuKey;
newTimestamps[G(imuKey)] = 0.1 * imuKey;
newTimestamps[V(imuKey)] = 0.1 * imuKey;
newTimestamps[B(imuKey)] = 0.1 * imuKey;
++imuKey;
optimize = true;
}
while (!gpsQ_.empty() && gpsKey < imuKey && optimize && ROS_TIME(gpsQ_.back()) > (startTime + gpsKey * 0.1))
{
fix = gpsQ_.pop();
// we don't want all gps messages, just ones that are very close to the factors (10 hz)
if (std::abs(ROS_TIME(fix) - (startTime + gpsKey * 0.1)) > 1e-2)
continue;
double E, N, U;
enu_.Forward(fix->latitude, fix->longitude, fix->altitude, E, N, U);
// we should maybe do a check on the GPS to make sure it's valid
newFactors.add(GPSFactor(G(gpsKey), Point3(E, N, U), gpsNoise));
newFactors.add(BetweenFactor<Pose3>(X(gpsKey), G(gpsKey), imuToGps_, imuToGpsFactorNoise));
std::cout << "gps(" << gpsKey << "): " << E << " " << N << " " << U << std::endl;
++gpsKey;
}
if (!optimize)
continue;
try
{
graph.update(newFactors, newVariables); //, newTimestamps);
prevPose = graph.calculateEstimate<Pose3>(X(imuKey - 1));
prevVel = graph.calculateEstimate<Vector3>(V(imuKey - 1));
prevBias = graph.calculateEstimate<imuBias::ConstantBias>(B(imuKey - 1));
// pass this to the other thread
{
std::lock_guard<std::mutex> lock(mutex_);
prevPose_ = prevPose;
prevVel_ = prevVel;
prevBias_ = prevBias;
currentTime_ = (imuKey - 1) * 0.1 + startTime;
doneFirstOpt_ = true;
}
}
catch (IndeterminantLinearSystemException ex)
{
// optimization blew up, not much to do just warn user
ROS_ERROR("Indeterminant linear system error");
}
newFactors.resize(0);
newVariables.clear();
newTimestamps.clear();
}
}
int main(int argc, char** argv) {
// Define the smoother lag (in seconds)
double lag = 2.0;
// Create a fixed lag smoother
// The Batch version uses Levenberg-Marquardt to perform the nonlinear optimization
BatchFixedLagSmoother smootherBatch(lag);
// The Incremental version uses iSAM2 to perform the nonlinear optimization
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.0; // Set the relin threshold to zero such that the batch estimate is recovered
parameters.relinearizeSkip = 1; // Relinearize every time
IncrementalFixedLagSmoother smootherISAM2(lag, parameters);
// Create containers to store the factors and linearization points that
// will be sent to the smoothers
NonlinearFactorGraph newFactors;
Values newValues;
FixedLagSmoother::KeyTimestampMap newTimestamps;
// Create a prior on the first pose, placing it at the origin
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
Key priorKey = 0;
newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds;
// Now, loop through several time steps, creating factors from different "sensors"
// and adding them to the fixed-lag smoothers
double deltaT = 0.25;
for(double time = deltaT; time <= 3.0; time += deltaT) {
// Define the keys related to this timestamp
Key previousKey(1000 * (time-deltaT));
Key currentKey(1000 * (time));
// Assign the current key to the current timestamp
newTimestamps[currentKey] = time;
// Add a guess for this pose to the new values
// Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
// {This is not a particularly good way to guess, but this is just an example}
Pose2 currentPose(time * 2.0, 0.0, 0.0);
newValues.insert(currentKey, currentPose);
// Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Update the smoothers with the new factors
smootherBatch.update(newFactors, newValues, newTimestamps);
smootherISAM2.update(newFactors, newValues, newTimestamps);
for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations
smootherISAM2.update();
}
// Print the optimized current pose
cout << setprecision(5) << "Timestamp = " << time << endl;
smootherBatch.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:");
smootherISAM2.calculateEstimate<Pose2>(currentKey).print("iSAM2 Estimate:");
cout << endl;
// Clear contains for the next iteration
newTimestamps.clear();
newValues.clear();
newFactors.resize(0);
}
// And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
cout << "After 3.0 seconds, " << endl;
cout << " Batch Smoother Keys: " << endl;
for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherBatch.timestamps()) {
cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl;
}
cout << " iSAM2 Smoother Keys: " << endl;
for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherISAM2.timestamps()) {
cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl;
}
return 0;
}