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

Added two example scripts to gtsam from the tutorial, with a single planar example that either contains all typedefs and manually creates the structure, and another that uses planarSLAM.h. Also added a BearingRange helper function to planarSLAM

release/4.3a0
Alex Cunningham 2010-08-26 21:21:15 +00:00
parent 23a30f8475
commit d17aef492c
7 changed files with 233 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.cproject
View File

@ -1097,6 +1097,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SLAMSelfContained.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>SLAMSelfContained.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="PlanarSLAMExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>PlanarSLAMExample.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>

4
examples/Makefile.am
View File

@ -11,7 +11,9 @@ sources =
check_PROGRAMS =
# Examples
noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation variable
noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation variable
noinst_PROGRAMS += SLAMSelfContained # Solves SLAM example from tutorial with all typedefs in the script
noinst_PROGRAMS += PlanarSLAMExample # Solves SLAM example from tutorial by using planarSLAM
#----------------------------------------------------------------------------------------------------
# rules to build local programs

87
examples/PlanarSLAMExample.cpp Normal file
View File

@ -0,0 +1,87 @@
/**
* @file PlanarSLAMExample.cpp
* @brief Simple robotics example from tutorial Figure 1.1 (left) by using the
* pre-built planar SLAM domain
* @author Alex Cunningham
*/
#include <cmath>
#include <iostream>
// This is should probably be pulled in elsewhere
#include <gtsam/inference/Ordering.h>
// pull in the planar SLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/planarSLAM.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::planarSLAM;
/**
* In this version of the system we make the following assumptions:
* - All values are axis aligned
* - Robot poses are facing along the X axis (horizontal, to the right in images)
* - We have bearing and range information for measurements
* - We have full odometry for measurements
* - The robot and landmarks are on a grid, moving 2 meters each step
* - Landmarks are 2 meters away from the robot trajectory
*/
int main(int argc, char** argv) {
// create keys for variables
PoseKey x1(1), x2(2), x3(3);
PointKey l1(1), l2(2);
// create graph container and add factors to it
Graph graph;
/* add prior */
// gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
graph.addPrior(x1, prior_measurement, prior_model); // add directly to graph
/* add odometry */
// general noisemodel for odometry
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model);
/* add measurements */
// general noisemodel for measurements
SharedDiagonal meas_model = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2));
// create the measurement values - indices are (pose id, landmark id)
Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90);
double range11 = sqrt(4+4),
range21 = 2.0,
range32 = 2.0;
// create bearing/range factors and add them
graph.addBearingRange(x1, l1, bearing11, range11, meas_model);
graph.addBearingRange(x2, l1, bearing21, range21, meas_model);
graph.addBearingRange(x3, l2, bearing32, range32, meas_model);
graph.print("Full Graph");
// initialize to noisy points
Config initialEstimate;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insert(l1, Point2(1.8, 2.1));
initialEstimate.insert(l2, Point2(4.1, 1.8));
initialEstimate.print("Initial Estimate");
// optimize using Levenburg-Marquadt optimization with an ordering from colamd
Optimizer::shared_config result = Optimizer::optimizeLM(graph, initialEstimate);
result->print("Final Result");
return 0;
}

118
examples/SLAMSelfContained.cpp Normal file
View File

@ -0,0 +1,118 @@
/**
* @file SLAMSelfContained.cpp
* @brief Simple robotics example from tutorial Figure 1.1 (left), with all typedefs
* internal to this script.
* @author Alex Cunningham
*/
#include <cmath>
#include <iostream>
// for all nonlinear keys
#include <gtsam/inference/Key.h>
// for points and poses
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
// for modeling measurement uncertainty - all models included here
#include <gtsam/linear/NoiseModel.h>
// add in headers for specific factors
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
// implementations for structures - needed if self-contained, and these should be included last
#include <gtsam/nonlinear/TupleConfig-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
// Main typedefs
typedef gtsam::TypedSymbol<gtsam::Pose2, 'x'> PoseKey; // Key for poses, with type included
typedef gtsam::TypedSymbol<gtsam::Point2,'l'> PointKey; // Key for points, with type included
typedef gtsam::LieConfig<PoseKey> PoseConfig; // config type for poses
typedef gtsam::LieConfig<PointKey> PointConfig; // config type for points
typedef gtsam::TupleConfig2<PoseConfig, PointConfig> Config; // main config with two variable classes
typedef gtsam::NonlinearFactorGraph<Config> Graph; // graph structure
typedef gtsam::NonlinearOptimizer<Graph,Config> Optimizer; // optimization engine for this domain
using namespace std;
using namespace gtsam;
/**
* In this version of the system we make the following assumptions:
* - All values are axis aligned
* - Robot poses are facing along the X axis (horizontal, to the right in images)
* - We have bearing and range information for measurements
* - We have full odometry for measurements
* - The robot and landmarks are on a grid, moving 2 meters each step
* - Landmarks are 2 meters away from the robot trajectory
*/
int main(int argc, char** argv) {
// create keys for variables
PoseKey x1(1), x2(2), x3(3);
PointKey l1(1), l2(2);
// create graph container and add factors to it
Graph graph;
/* add prior */
// gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
PriorFactor<Config, PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
graph.add(posePrior); // add the factor to the graph
/* add odometry */
// general noisemodel for odometry
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
// create between factors to represent odometry
BetweenFactor<Config,PoseKey> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<Config,PoseKey> odom23(x2, x3, odom_measurement, odom_model);
graph.add(odom12); // add both to graph
graph.add(odom23);
/* add measurements */
// general noisemodel for measurements
SharedDiagonal meas_model = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2));
// create the measurement values - indices are (pose id, landmark id)
Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90);
double range11 = sqrt(4+4),
range21 = 2.0,
range32 = 2.0;
// create bearing/range factors
BearingRangeFactor<Config, PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
BearingRangeFactor<Config, PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
BearingRangeFactor<Config, PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model);
// add the factors
graph.add(meas11);
graph.add(meas21);
graph.add(meas32);
graph.print("Full Graph");
// initialize to noisy points
Config initialEstimate;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insert(l1, Point2(1.8, 2.1));
initialEstimate.insert(l2, Point2(4.1, 1.8));
initialEstimate.print("Initial Estimate");
// optimize using Levenburg-Marquadt optimization with an ordering from colamd
Optimizer::shared_config result = Optimizer::optimizeLM(graph, initialEstimate);
result->print("Final Result");
return 0;
}

2
examples/SimpleRotation.cpp
View File

@ -24,7 +24,7 @@
* TODO: make factors independent of Config
* TODO: get rid of excessive shared pointer stuff: mostly gone
* TODO: make toplevel documentation
* TODO: investigate whether we can just use ints as keys
* TODO: investigate whether we can just use ints as keys: will occur for linear, not nonlinear
*/
using namespace std;

6
slam/planarSLAM.cpp
View File

@ -52,6 +52,12 @@ namespace gtsam {
push_back(factor);
}
void Graph::addBearingRange(const PoseKey& i, const PointKey& j, const Rot2& z1,
double z2, const SharedGaussian& model) {
sharedFactor factor(new BearingRange(i, j, z1, z2, model));
push_back(factor);
}
} // planarSLAM
} // gtsam

2
slam/planarSLAM.h
View File

@ -47,6 +47,8 @@ namespace gtsam {
const SharedGaussian& model);
void addRange(const PoseKey& i, const PointKey& j, double z,
const SharedGaussian& model);
void addBearingRange(const PoseKey& i, const PointKey& j,
const Rot2& z1, double z2, const SharedGaussian& model);
};
// Optimizer