Added 'examples' folder to gtsam_unstable
Stephen Williams
parent
95e97c2dfc
commit
c902908115
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
@ -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 <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
|
||||
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
|
||||
|
||||
// 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 <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
// 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 <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
// 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 <gtsam/nonlinear/Values.h>
|
||||
|
||||
// We will use simple integer Keys to uniquely identify each robot pose.
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
|
||||
// We will use Pose2 variables (x, y, theta) to represent the robot positions
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
||||
#include <iomanip>
|
||||
|
||||
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<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(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<Pose2>(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<Pose2>(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<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;
|
||||
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;
|
||||
}
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue