diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index fb6768c1b..954b6b4c1 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -105,3 +105,9 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX) wrap_library(gtsam_unstable "${mexFlags}" "./" gtsam) endif(GTSAM_INSTALL_MATLAB_TOOLBOX) + + +# Build examples +if (GTSAM_BUILD_EXAMPLES) + add_subdirectory(examples) +endif(GTSAM_BUILD_EXAMPLES) diff --git a/gtsam_unstable/examples/CMakeLists.txt b/gtsam_unstable/examples/CMakeLists.txt new file mode 100644 index 000000000..7af84a784 --- /dev/null +++ b/gtsam_unstable/examples/CMakeLists.txt @@ -0,0 +1,32 @@ +if(NOT MSVC) + add_custom_target(unstable_examples) +endif() + +# Build example executables +FILE(GLOB example_srcs "*.cpp") +foreach(example_src ${example_srcs} ) + get_filename_component(example_base ${example_src} NAME_WE) + set( example_bin ${example_base} ) + message(STATUS "Adding Example ${example_bin}") + if(NOT MSVC) + add_dependencies(examples ${example_bin}) + endif() + add_executable(${example_bin} ${example_src}) + + # Disable building during make all/install + if (GTSAM_DISABLE_EXAMPLES_ON_INSTALL) + set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON) + endif() + + target_link_libraries(${example_bin} ${gtsam-default} ${gtsam_unstable-default}) + if(NOT MSVC) + add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN}) + endif() + + # Set up Visual Studio folder + if(MSVC) + set_property(TARGET ${example_bin} PROPERTY FOLDER "Examples") + endif() + +endforeach(example_src) + diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp new file mode 100644 index 000000000..9b48227d9 --- /dev/null +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -0,0 +1,139 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file FixedLagSmootherExample.cpp + * @brief Demonstration of the fixed-lag smoothers using a planar robot example and multiple odometry-like sensors + * @author Stephen Williams + */ + +/** + * A simple 2D pose slam example with multiple odometry-like measurements + * - The robot initially faces along the X axis (horizontal, to the right in 2D) + * - The robot moves forward at 2m/s + * - We have measurements between each pose from multiple odometry sensors + */ + +// This example demonstrates the use of the Fixed-Lag Smoothers in GTSAM unstable +#include +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Between factors for the relative motion described by odometry measurements. +// Also, we will initialize the robot at the origin using a Prior factor. +#include +#include + +// When the factors are created, we will add them to a Factor Graph. As the factors we are using +// are nonlinear factors, we will need a Nonlinear Factor Graph. +#include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + +// We will use simple integer Keys to uniquely identify each robot pose. +#include + +// We will use Pose2 variables (x, y, theta) to represent the robot positions +#include + +#include + +using namespace std; +using namespace gtsam; + + +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 + IncrementalFixedLagSmoother smootherISAM2(lag); + + // 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(Vector_(3, 0.3, 0.3, 0.1)); + Key priorKey = 0; + newFactors.add(PriorFactor(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(2.32, -0.18, 0.02); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + newFactors.add(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); + + Pose2 odometryMeasurement2 = Pose2(1.95, 0.07, 0.01); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 0.05)); + newFactors.add(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); + + // Update the smoothers with the new factors + smootherBatch.update(newFactors, newValues, newTimestamps); + smootherISAM2.update(newFactors, newValues, newTimestamps); + + // Print the optimized current pose + cout << setprecision(5) << "Timestamp = " << time << endl; + smootherBatch.calculateEstimate(currentKey).print("Batch Estimate:"); + smootherISAM2.calculateEstimate(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; + BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, smootherBatch.getTimestamps()) { + cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl; + } + cout << " iSAM2 Smoother Keys: " << endl; + BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, smootherISAM2.getTimestamps()) { + cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl; + } + + return 0; +} diff --git a/gtsam_unstable/examples/README b/gtsam_unstable/examples/README new file mode 100644 index 000000000..503dc8670 --- /dev/null +++ b/gtsam_unstable/examples/README @@ -0,0 +1,4 @@ +This directory contains a number of examples that illustrate the use of unstable components in GTSAM: + +FixedLagSmootherExample: a 2D Pose SLAM example fusing measurements from multiple odometry-type sensors +ConcurrentFilteringAndSmoothingExample: a 2D Pose SLAM example demonstrating the Concurrent Filtering and Smoothing architecture