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

Merge branch 'trunk'

release/4.3a0
Richard Roberts 2013-06-17 21:15:57 +00:00
parent af90ece44c d85c773f7c
commit 8f9d5ec2e2
64 changed files with 2169 additions and 1055 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
.cproject
View File

@ -1,7 +1,5 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?>
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
@ -453,6 +451,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVelocityConstraint3.run" path="build/gtsam_unstable/dynamics" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVelocityConstraint3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -613,54 +619,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPlanarSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPlanarSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose2SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose3SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimulated2D.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimulated2DOriented.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVisualSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVisualSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testProjectionFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -669,14 +627,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSerializationSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSerializationSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -685,6 +635,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSerialization.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSerialization.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -917,6 +875,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBayesTreeOperations.run" path="build/gtsam_unstable/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBayesTreeOperations.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1149,6 +1115,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSerializationSLAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSerializationSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1613,6 +1587,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3Q.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRot3Q.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

2
CMakeLists.txt
View File

@ -100,7 +100,7 @@ endif()
# Find boost
# If using Boost shared libs, set up auto linking for shared libs
if(NOT Boost_USE_STATIC_LIBS)
if(MSVC AND NOT Boost_USE_STATIC_LIBS)
add_definitions(-DBOOST_ALL_DYN_LINK)
endif()

82
gtsam.h
View File

@ -62,7 +62,7 @@
* of using the copy constructor (which is used for non-virtual objects).
* - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree
* virtual boost::shared_ptr<CLASS_NAME> clone() const;
* Templates
* Class Templates
* - Basic templates are supported either with an explicit list of types to instantiate,
* e.g. template<T = {gtsam::Pose2, gtsam::Rot2, gtsam::Point3}> class Class1 { ... };
* or with typedefs, e.g.
@ -81,7 +81,12 @@
/**
* Status:
* - TODO: default values for arguments
* - WORKAROUND: make multiple versions of the same function for different configurations of default arguments
* - TODO: Handle gtsam::Rot3M conversions to quaternions
* - TODO: Parse return of const ref arguments
* - TODO: Parse std::string variants and convert directly to special string
* - TODO: Add enum support
* - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load
*/
namespace std {
@ -98,7 +103,7 @@ namespace std {
bool empty() const;
void reserve(size_t n);
//Element acces
//Element access
T* at(size_t n);
T* front();
T* back();
@ -397,7 +402,7 @@ virtual class Rot3 : gtsam::Value {
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options
//gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options
gtsam::Rot3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot3& p) const;
@ -811,6 +816,7 @@ virtual class BayesTree {
//Standard Interface
//size_t findParentClique(const gtsam::IndexVector& parents) const;
size_t size();
size_t nrNodes() const;
void saveGraph(string s) const;
CLIQUE* root() const;
void clear();
@ -894,6 +900,7 @@ class SymbolicFactorGraph {
void print(string s) const;
bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const;
size_t size() const;
bool exists(size_t i) const;
// Standard interface
// FIXME: Must wrap FastSet<Index> for this to work
@ -978,7 +985,7 @@ virtual class Base {
virtual class Gaussian : gtsam::noiseModel::Base {
static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
//Matrix R() const; // FIXME: cannot parse!!!
Matrix R() const;
bool equals(gtsam::noiseModel::Base& expected, double tol);
void print(string s) const;
};
@ -987,7 +994,7 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
static gtsam::noiseModel::Diagonal* Variances(Vector variances);
static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
// Matrix R() const; // FIXME: cannot parse!!!
Matrix R() const;
void print(string s) const;
};
@ -1281,6 +1288,7 @@ class GaussianFactorGraph {
bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const;
size_t size() const;
gtsam::GaussianFactor* at(size_t idx) const;
bool exists(size_t idx) const;
// Inference
pair<gtsam::GaussianConditional*, gtsam::GaussianFactorGraph> eliminateFrontals(size_t nFrontals) const;
@ -1450,11 +1458,6 @@ size_t symbol(char chr, size_t index);
char symbolChr(size_t key);
size_t symbolIndex(size_t key);
// Key utilities
gtsam::KeySet keyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB);
gtsam::KeySet keyDifference(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB);
bool hasKeyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB);
// Default keyformatter
void printKeySet(const gtsam::KeySet& keys);
void printKeySet(const gtsam::KeySet& keys, string s);
@ -1512,7 +1515,8 @@ class NonlinearFactorGraph {
bool empty() const;
void remove(size_t i);
size_t nrFactors() const;
gtsam::NonlinearFactor* at(size_t i) const;
gtsam::NonlinearFactor* at(size_t idx) const;
bool exists(size_t idx) const;
void push_back(const gtsam::NonlinearFactorGraph& factors);
// NonlinearFactorGraph
@ -1959,6 +1963,8 @@ class NonlinearISAM {
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera}>
virtual class PriorFactor : gtsam::NonlinearFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
gtsam::noiseModel::Base* get_noiseModel() const;
};
@ -1966,6 +1972,8 @@ virtual class PriorFactor : gtsam::NonlinearFactor {
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
virtual class BetweenFactor : gtsam::NonlinearFactor {
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
T measured() const;
gtsam::noiseModel::Base* get_noiseModel() const;
};
@ -1983,6 +1991,7 @@ virtual class NonlinearEquality : gtsam::NonlinearFactor {
template<POSE, POINT>
virtual class RangeFactor : gtsam::NonlinearFactor {
RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
gtsam::noiseModel::Base* get_noiseModel() const;
};
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
@ -1999,6 +2008,7 @@ typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactor
template<POSE, POINT, ROTATION>
virtual class BearingFactor : gtsam::NonlinearFactor {
BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel);
gtsam::noiseModel::Base* get_noiseModel() const;
};
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
@ -2008,6 +2018,7 @@ typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFa
template<POSE, POINT, ROTATION>
virtual class BearingRangeFactor : gtsam::NonlinearFactor {
BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
gtsam::noiseModel::Base* get_noiseModel() const;
};
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingRangeFactor2D;
@ -2031,6 +2042,7 @@ virtual class GenericProjectionFactor : gtsam::NonlinearFactor {
CALIBRATION* calibration() const;
bool verboseCheirality() const;
bool throwCheirality() const;
gtsam::noiseModel::Base* get_noiseModel() const;
};
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> GenericProjectionFactorCal3_S2;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
@ -2059,6 +2071,7 @@ virtual class GenericStereoFactor : gtsam::NonlinearFactor {
size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K);
gtsam::StereoPoint2 measured() const;
gtsam::Cal3_S2Stereo* calibration() const;
gtsam::noiseModel::Base* get_noiseModel() const;
};
typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3> GenericStereoFactor3D;
@ -2066,6 +2079,7 @@ typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3> GenericStereoFac
template<POSE>
virtual class PoseTranslationPrior : gtsam::NonlinearFactor {
PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
gtsam::noiseModel::Base* get_noiseModel() const;
};
typedef gtsam::PoseTranslationPrior<gtsam::Pose2> PoseTranslationPrior2D;
@ -2075,6 +2089,7 @@ typedef gtsam::PoseTranslationPrior<gtsam::Pose3> PoseTranslationPrior3D;
template<POSE>
virtual class PoseRotationPrior : gtsam::NonlinearFactor {
PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
gtsam::noiseModel::Base* get_noiseModel() const;
};
typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;
@ -2092,6 +2107,51 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
gtsam::noiseModel::Base* model);
//*************************************************************************
// Serialization
//*************************************************************************
#include <gtsam/slam/serialization.h>
// Serialize/Deserialize a NonlinearFactorGraph
string serializeGraph(const gtsam::NonlinearFactorGraph& graph);
gtsam::NonlinearFactorGraph* deserializeGraph(string serialized_graph);
string serializeGraphXML(const gtsam::NonlinearFactorGraph& graph);
string serializeGraphXML(const gtsam::NonlinearFactorGraph& graph, string name);
gtsam::NonlinearFactorGraph* deserializeGraphXML(string serialized_graph);
gtsam::NonlinearFactorGraph* deserializeGraphXML(string serialized_graph, string name);
// Serialize/Deserialize a Values
string serializeValues(const gtsam::Values& values);
gtsam::Values* deserializeValues(string serialized_values);
string serializeValuesXML(const gtsam::Values& values);
string serializeValuesXML(const gtsam::Values& values, string name);
gtsam::Values* deserializeValuesXML(string serialized_values);
gtsam::Values* deserializeValuesXML(string serialized_values, string name);
// Serialize
bool serializeGraphToFile(const gtsam::NonlinearFactorGraph& graph, string fname);
bool serializeGraphToXMLFile(const gtsam::NonlinearFactorGraph& graph, string fname);
bool serializeGraphToXMLFile(const gtsam::NonlinearFactorGraph& graph, string fname, string name);
bool serializeValuesToFile(const gtsam::Values& values, string fname);
bool serializeValuesToXMLFile(const gtsam::Values& values, string fname);
bool serializeValuesToXMLFile(const gtsam::Values& values, string fname, string name);
// Deserialize
gtsam::NonlinearFactorGraph* deserializeGraphFromFile(string fname);
gtsam::NonlinearFactorGraph* deserializeGraphFromXMLFile(string fname);
gtsam::NonlinearFactorGraph* deserializeGraphFromXMLFile(string fname, string name);
gtsam::Values* deserializeValuesFromFile(string fname);
gtsam::Values* deserializeValuesFromXMLFile(string fname);
gtsam::Values* deserializeValuesFromXMLFile(string fname, string name);
//*************************************************************************
// Utilities
//*************************************************************************

26
gtsam/base/numericalDerivative.h
View File

@ -32,8 +32,6 @@
#include <gtsam/base/LieVector.h>
#include <gtsam/base/Matrix.h>
namespace gtsam {
/*
@ -106,8 +104,8 @@ namespace gtsam {
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
d(j) += delta;
H.col(j) << (hxplus-hxmin)*factor;
}
return H;
}
@ -160,8 +158,8 @@ namespace gtsam {
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
d(j) += delta;
H.col(j) << (hxplus-hxmin)*factor;
}
return H;
}
@ -224,8 +222,8 @@ namespace gtsam {
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1,x2.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1,x2.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
d(j) += delta;
H.col(j) << (hxplus-hxmin)*factor;
}
return H;
}
@ -290,8 +288,8 @@ namespace gtsam {
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2,x3));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2,x3));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
d(j) += delta;
H.col(j) << (hxplus-hxmin)*factor;
}
return H;
}
@ -355,8 +353,8 @@ namespace gtsam {
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1, x2.retract(d),x3));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1, x2.retract(d),x3));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
d(j) += delta;
H.col(j) << (hxplus-hxmin)*factor;
}
return H;
}
@ -420,8 +418,8 @@ namespace gtsam {
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1, x2, x3.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1, x2, x3.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
d(j) += delta;
H.col(j) << (hxplus-hxmin)*factor;
}
return H;
}

159
gtsam/base/serialization.h Normal file
View File

@ -0,0 +1,159 @@
/* ----------------------------------------------------------------------------
* 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 serialization.h
* @brief Convenience functions for serializing data structures via boost.serialization
* @author Alex Cunningham
* @author Richard Roberts
* @date Feb 7, 2012
*/
#pragma once
#include <sstream>
#include <fstream>
#include <string>
// includes for standard serialization types
#include <boost/serialization/export.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/list.hpp>
#include <boost/serialization/deque.hpp>
#include <boost/serialization/weak_ptr.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
namespace gtsam {
// Serialization directly to strings in compressed format
template<class T>
std::string serialize(const T& input) {
std::ostringstream out_archive_stream;
boost::archive::text_oarchive out_archive(out_archive_stream);
out_archive << input;
return out_archive_stream.str();
}
template<class T>
void deserialize(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
boost::archive::text_iarchive in_archive(in_archive_stream);
in_archive >> output;
}
template<class T>
bool serializeToFile(const T& input, const std::string& filename) {
std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open())
return false;
boost::archive::text_oarchive out_archive(out_archive_stream);
out_archive << input;
out_archive_stream.close();
return true;
}
template<class T>
bool deserializeFromFile(const std::string& filename, T& output) {
std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open())
return false;
boost::archive::text_iarchive in_archive(in_archive_stream);
in_archive >> output;
in_archive_stream.close();
return true;
}
// Serialization to XML format with named structures
template<class T>
std::string serializeXML(const T& input, const std::string& name="data") {
std::ostringstream out_archive_stream;
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
return out_archive_stream.str();
}
template<class T>
void deserializeXML(const std::string& serialized, T& output, const std::string& name="data") {
std::istringstream in_archive_stream(serialized);
boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
}
template<class T>
bool serializeToXMLFile(const T& input, const std::string& filename, const std::string& name="data") {
std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open())
return false;
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);;
out_archive_stream.close();
return true;
}
template<class T>
bool deserializeFromXMLFile(const std::string& filename, T& output, const std::string& name="data") {
std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open())
return false;
boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
in_archive_stream.close();
return true;
}
// Serialization to binary format with named structures
template<class T>
std::string serializeBinary(const T& input, const std::string& name="data") {
std::ostringstream out_archive_stream;
boost::archive::binary_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
return out_archive_stream.str();
}
template<class T>
void deserializeBinary(const std::string& serialized, T& output, const std::string& name="data") {
std::istringstream in_archive_stream(serialized);
boost::archive::binary_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
}
template<class T>
bool deserializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") {
std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open())
return false;
boost::archive::binary_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);;
out_archive_stream.close();
return true;
}
template<class T>
bool deserializeFromBinaryFile(const std::string& filename, T& output, const std::string& name="data") {
std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open())
return false;
boost::archive::binary_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
in_archive_stream.close();
return true;
}
} // \namespace gtsam

74
gtsam/base/serializationTestHelpers.h
View File

@ -22,46 +22,14 @@
#include <sstream>
#include <string>
// includes for standard serialization types
#include <boost/serialization/export.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/list.hpp>
#include <boost/serialization/deque.hpp>
#include <boost/serialization/weak_ptr.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <gtsam/base/serialization.h>
// whether to print the serialized text to stdout
const bool verbose = false;
namespace gtsam { namespace serializationTestHelpers {
namespace gtsam {
namespace serializationTestHelpers {
/* ************************************************************************* */
// Serialization testing code.
/* ************************************************************************* */
template<class T>
std::string serialize(const T& input) {
std::ostringstream out_archive_stream;
boost::archive::text_oarchive out_archive(out_archive_stream);
out_archive << input;
return out_archive_stream.str();
}
template<class T>
void deserialize(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
boost::archive::text_iarchive in_archive(in_archive_stream);
in_archive >> output;
}
// Templated round-trip serialization
template<class T>
@ -97,22 +65,6 @@ bool equalsDereferenced(const T& input) {
return input->equals(*output);
}
/* ************************************************************************* */
template<class T>
std::string serializeXML(const T& input) {
std::ostringstream out_archive_stream;
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp("data", input);
return out_archive_stream.str();
}
template<class T>
void deserializeXML(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp("data", output);
}
// Templated round-trip serialization using XML
template<class T>
void roundtripXML(const T& input, T& output) {
@ -148,22 +100,6 @@ bool equalsDereferencedXML(const T& input = T()) {
return input->equals(*output);
}
/* ************************************************************************* */
template<class T>
std::string serializeBinary(const T& input) {
std::ostringstream out_archive_stream;
boost::archive::binary_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp("data", input);
return out_archive_stream.str();
}
template<class T>
void deserializeBinary(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
boost::archive::binary_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp("data", output);
}
// Templated round-trip serialization using XML
template<class T>
void roundtripBinary(const T& input, T& output) {
@ -199,4 +135,6 @@ bool equalsDereferencedBinary(const T& input = T()) {
return input->equals(*output);
}
} }
} // \namespace serializationTestHelpers
} // \namespace gtsam

2
gtsam/geometry/Cal3DS2.cpp
View File

@ -83,7 +83,7 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
const int maxIterations = 10;
int iteration;
for ( iteration = 0 ; iteration < maxIterations ; ++iteration ) {
if ( uncalibrate(pn).dist(pi) <= tol ) break;
if ( uncalibrate(pn).distance(pi) <= tol ) break;
const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y ;
const double rr = xx + yy ;
const double g = (1+k1_*rr+k2_*rr*rr) ;

8
gtsam/geometry/Point2.cpp
View File

@ -27,7 +27,7 @@ INSTANTIATE_LIE(Point2);
/* ************************************************************************* */
void Point2::print(const string& s) const {
cout << s << "(" << x_ << ", " << y_ << ")" << endl;
cout << s << *this << endl;
}
/* ************************************************************************* */
@ -40,4 +40,10 @@ double Point2::norm() const {
return sqrt(x_*x_ + y_*y_);
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point2& p) {
os << '(' << p.x() << ", " << p.y() << ')';
return os;
}
} // namespace gtsam

8
gtsam/geometry/Point2.h
View File

@ -151,6 +151,11 @@ public:
Point2 unit() const { return *this/norm(); }
/** distance between two points */
inline double distance(const Point2& p2) const {
return (p2 - *this).norm();
}
/** @deprecated The following function has been deprecated, use distance above */
inline double dist(const Point2& p2) const {
return (p2 - *this).norm();
}
@ -184,6 +189,9 @@ public:
inline void operator *= (double s) {x_*=s;y_*=s;}
/// @}
/// Streaming
friend std::ostream &operator<<(std::ostream &os, const Point2& p);
private:
/// @name Advanced Interface

21
gtsam/geometry/Point3.cpp
View File

@ -14,10 +14,11 @@
* @brief 3D Point
*/
#include <cmath>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/Lie-inl.h>
using namespace std;
namespace gtsam {
/** Explicit instantiation of base class to export members */
@ -30,8 +31,8 @@ bool Point3::equals(const Point3 & q, double tol) const {
/* ************************************************************************* */
void Point3::print(const std::string& s) const {
std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl;
void Point3::print(const string& s) const {
cout << s << *this << endl;
}
/* ************************************************************************* */
@ -49,14 +50,17 @@ Point3 Point3::operator+(const Point3& q) const {
Point3 Point3::operator- (const Point3& q) const {
return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ );
}
/* ************************************************************************* */
Point3 Point3::operator*(double s) const {
return Point3(x_ * s, y_ * s, z_ * s);
}
/* ************************************************************************* */
Point3 Point3::operator/(double s) const {
return Point3(x_ / s, y_ / s, z_ / s);
}
/* ************************************************************************* */
Point3 Point3::add(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@ -64,6 +68,7 @@ Point3 Point3::add(const Point3 &q,
if (H2) *H2 = eye(3,3);
return *this + q;
}
/* ************************************************************************* */
Point3 Point3::sub(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@ -71,20 +76,30 @@ Point3 Point3::sub(const Point3 &q,
if (H2) *H2 = -eye(3,3);
return *this - q;
}
/* ************************************************************************* */
Point3 Point3::cross(const Point3 &q) const {
return Point3( y_*q.z_ - z_*q.y_,
z_*q.x_ - x_*q.z_,
x_*q.y_ - y_*q.x_ );
}
/* ************************************************************************* */
double Point3::dot(const Point3 &q) const {
return ( x_*q.x_ + y_*q.y_ + z_*q.z_ );
}
/* ************************************************************************* */
double Point3::norm() const {
return sqrt( x_*x_ + y_*y_ + z_*z_ );
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point3& p) {
os << '(' << p.x() << ", " << p.y() << ", " << p.z() << ')';
return os;
}
/* ************************************************************************* */
} // namespace gtsam

19
gtsam/geometry/Point3.h
View File

@ -21,13 +21,14 @@
#pragma once
#include <cmath>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h>
#include <boost/serialization/nvp.hpp>
#include <cmath>
namespace gtsam {
/**
@ -153,8 +154,13 @@ namespace gtsam {
Point3 operator / (double s) const;
/** distance between two points */
double dist(const Point3& p2) const {
return std::sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0));
inline double distance(const Point3& p2) const {
return (p2 - *this).norm();
}
/** @deprecated The following function has been deprecated, use distance above */
inline double dist(const Point3& p2) const {
return (p2 - *this).norm();
}
/** Distance of the point from the origin */
@ -197,6 +203,9 @@ namespace gtsam {
/// @}
/// Output stream operator
friend std::ostream &operator<<(std::ostream &os, const Point3& p);
private:
/// @name Advanced Interface

7
gtsam/geometry/Pose3.cpp
View File

@ -348,4 +348,11 @@ namespace gtsam {
Point3 t = Point3(cq) - R * Point3(cp);
return Pose3(R, t);
}
/* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
os << pose.rotation() << "\n" << pose.translation() << endl;
return os;
}
} // namespace gtsam

3
gtsam/geometry/Pose3.h
View File

@ -290,6 +290,9 @@ namespace gtsam {
*/
static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(0, 2); }
/// Output stream operator
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
private:
/** Serialization function */
friend class boost::serialization::access;

8
gtsam/geometry/Rot2.h
View File

@ -116,11 +116,11 @@ namespace gtsam {
}
/** Compose - make a new rotation by adding angles */
inline Rot2 compose(const Rot2& R1, boost::optional<Matrix&> H1 =
inline Rot2 compose(const Rot2& R, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = eye(1);
if (H2) *H2 = eye(1);
return *this * R1;
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
/** Compose - make a new rotation by adding angles */
@ -129,11 +129,11 @@ namespace gtsam {
}
/** Between using the default implementation */
inline Rot2 between(const Rot2& p2, boost::optional<Matrix&> H1 =
inline Rot2 between(const Rot2& R, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = -eye(1);
if (H2) *H2 = eye(1);
return between_default(*this, p2);
return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
}
/// @}

3
gtsam/geometry/Rot3.h
View File

@ -355,6 +355,9 @@ namespace gtsam {
*/
Vector quaternion() const;
/// Output stream operator
friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
private:
/** Serialization function */
friend class boost::serialization::access;

9
gtsam/geometry/Rot3M.cpp
View File

@ -380,6 +380,15 @@ pair<Matrix3, Vector3> RQ(const Matrix3& A) {
return make_pair(R, xyz);
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Rot3& R) {
os << "\n";
os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
return os;
}
/* ************************************************************************* */
} // namespace gtsam

9
gtsam/geometry/Rot3Q.cpp
View File

@ -237,6 +237,15 @@ namespace gtsam {
return make_pair(R, xyz);
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Rot3& R) {
os << "\n";
os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
return os;
}
} // namespace gtsam
#endif

17
gtsam/geometry/tests/testPoint2.cpp
View File

@ -75,19 +75,28 @@ TEST( Point2, norm)
Point2 p0(cos(5.0), sin(5.0));
DOUBLES_EQUAL(1,p0.norm(),1e-6);
Point2 p1(4, 5), p2(1, 1);
DOUBLES_EQUAL( 5,p1.dist(p2),1e-6);
DOUBLES_EQUAL( 5,p1.distance(p2),1e-6);
DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6);
}
/* ************************************************************************* */
TEST( Point2, unit)
{
Point2 p0(10.0, 0.0), p1(0.0,-10.0), p2(10.0, 10.0);
EXPECT(assert_equal(Point2(1.0, 0.0), p0.unit(), 1e-6));
EXPECT(assert_equal(Point2(0.0,-1.0), p1.unit(), 1e-6));
Point2 p0(10, 0), p1(0,-10), p2(10, 10);
EXPECT(assert_equal(Point2(1, 0), p0.unit(), 1e-6));
EXPECT(assert_equal(Point2(0,-1), p1.unit(), 1e-6));
EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6));
}
/* ************************************************************************* */
TEST( Point2, stream)
{
Point2 p(1,2);
std::ostringstream os;
os << p;
EXPECT(os.str() == "(1, 2)");
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

9
gtsam/geometry/tests/testPoint3.cpp
View File

@ -71,6 +71,15 @@ TEST( Point3, dot)
CHECK(ones.dot(Point3(1,1,0)) == 2);
}
/* ************************************************************************* */
TEST( Point3, stream)
{
Point3 p(1,2, -3);
std::ostringstream os;
os << p;
EXPECT(os.str() == "(1, 2, -3)");
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

9
gtsam/geometry/tests/testPose3.cpp
View File

@ -741,6 +741,15 @@ TEST( Pose3, adjointTranspose) {
EXPECT(assert_equal(numericalH,actualH,1e-5));
}
/* ************************************************************************* */
TEST( Pose3, stream)
{
Pose3 T;
std::ostringstream os;
os << T;
EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n\n(0, 0, 0)\n");
}
/* ************************************************************************* */
int main(){ TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

9
gtsam/geometry/tests/testRot3M.cpp
View File

@ -562,6 +562,15 @@ TEST( Rot3, Cayley ) {
EXPECT(assert_equal(A, Cayley(Q)));
}
/* ************************************************************************* */
TEST( Rot3, stream)
{
Rot3 R;
std::ostringstream os;
os << R;
EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n");
}
#endif
/* ************************************************************************* */

9
gtsam/geometry/tests/testRot3Q.cpp
View File

@ -475,6 +475,15 @@ TEST(Rot3, quaternion) {
EXPECT(assert_equal(expected2, actual2));
}
/* ************************************************************************* */
TEST( Rot3, stream)
{
Rot3 R;
std::ostringstream os;
os << R;
EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n");
}
#endif
/* ************************************************************************* */

5
gtsam/inference/BayesTree-inl.h
View File

@ -799,5 +799,6 @@ namespace gtsam {
}
}
}
/// namespace gtsam
/* ************************************************************************* */
} // \namespace gtsam

3
gtsam/inference/BayesTree.h
View File

@ -162,6 +162,9 @@ namespace gtsam {
return 0;
}
/** number of nodes */
inline size_t nrNodes() const { return nodes_.size(); }
/** Check if there are any cliques in the tree */
inline bool empty() const {
return nodes_.empty();

3
gtsam/inference/FactorGraph.h
View File

@ -165,6 +165,9 @@ class VariableIndex;
const sharedFactor operator[](size_t i) const { return at(i); }
sharedFactor& operator[](size_t i) { return at(i); }
/** Checks whether a valid factor exists at the given index */
inline bool exists(size_t i) const { return i<factors_.size() && factors_[i]; }
/** STL begin, so we can use BOOST_FOREACH */
const_iterator begin() const { return factors_.begin();}

6
gtsam/inference/ISAM-inl.h
View File

@ -36,8 +36,14 @@ namespace gtsam {
const FG& newFactors, Cliques& orphans, typename FG::Eliminate function) {
// Remove the contaminated part of the Bayes tree
// Throw exception if disconnected
BayesNet<CONDITIONAL> bn;
if (!this->empty()) {
this->removeTop(newFactors.keys(), bn, orphans);
if (bn.empty())
throw std::runtime_error(
"ISAM::update_internal(): no variables in common between existing Bayes tree and incoming factors!");
}
FG factors(bn);
// add the factors themselves

33
gtsam/inference/Key.cpp
View File

@ -52,39 +52,6 @@ void printKeySet(const gtsam::KeySet& keys, const std::string& s, const KeyForma
std::cout << std::endl;
}
}
/* ************************************************************************* */
gtsam::KeySet keyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB) {
gtsam::KeySet intersection;
if (keysA.empty() || keysB.empty())
return intersection;
BOOST_FOREACH(const gtsam::Key& key, keysA)
if (keysB.count(key))
intersection.insert(key);
return intersection;
}
/* ************************************************************************* */
bool hasKeyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB) {
if (keysA.empty() || keysB.empty())
return false;
BOOST_FOREACH(const gtsam::Key& key, keysA)
if (keysB.count(key))
return true;
return false;
}
/* ************************************************************************* */
gtsam::KeySet keyDifference(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB) {
if (keysA.empty() || keysB.empty())
return keysA;
gtsam::KeySet difference;
BOOST_FOREACH(const gtsam::Key& key, keysA)
if (!keysB.count(key))
difference.insert(key);
return difference;
}
/* ************************************************************************* */
} // \namespace gtsam

9
gtsam/inference/Key.h
View File

@ -47,14 +47,5 @@ namespace gtsam {
/// Utility function to print sets of keys with optional prefix
GTSAM_EXPORT void printKeySet(const KeySet& keys, const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/// Computes the intersection between two sets
GTSAM_EXPORT gtsam::KeySet keyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB);
/// Checks if an intersection exists - faster checking size of above
GTSAM_EXPORT bool hasKeyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB);
/// Computes a difference between sets, so result is those that are in A, but not B
GTSAM_EXPORT gtsam::KeySet keyDifference(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB);
}

1
gtsam/inference/tests/testPermutation.cpp
View File

@ -125,6 +125,7 @@ TEST(Reduction, CreateFromPartialPermutation) {
expected.insert(make_pair(6,4));
internal::Reduction actual = internal::Reduction::CreateFromPartialPermutation(selector, p);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */

2
gtsam/nonlinear/ISAM2.cpp
View File

@ -125,7 +125,7 @@ ISAM2& ISAM2::operator=(const ISAM2& rhs) {
linearFactors_ = GaussianFactorGraph();
linearFactors_.reserve(rhs.linearFactors_.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& linearFactor, rhs.linearFactors_) {
linearFactors_.push_back(linearFactor->clone()); }
linearFactors_.push_back(linearFactor ? linearFactor->clone() : GaussianFactor::shared_ptr()); }
ordering_ = rhs.ordering_;
params_ = rhs.params_;

19
gtsam/nonlinear/summarization.cpp
View File

@ -18,18 +18,13 @@ std::pair<GaussianFactorGraph,Ordering>
summarize(const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys, SummarizationMode mode) {
const size_t nrEliminatedKeys = values.size() - saved_keys.size();
// // compute a new ordering with non-saved keys first
// Ordering ordering;
// KeySet eliminatedKeys;
// BOOST_FOREACH(const Key& key, values.keys()) {
// if (!saved_keys.count(key)) {
// eliminatedKeys.insert(key);
// ordering += key;
// }
// }
//
// BOOST_FOREACH(const Key& key, saved_keys)
// ordering += key;
// If we aren't eliminating anything, linearize and return
if (!nrEliminatedKeys || saved_keys.empty()) {
Ordering ordering = *values.orderingArbitrary();
GaussianFactorGraph linear_graph = *graph.linearize(values, ordering);
return make_pair(linear_graph, ordering);
}
// Compute a constrained ordering with variables grouped to end
std::map<gtsam::Key, int> ordering_constraints;

3
gtsam/nonlinear/summarization.h
View File

@ -29,6 +29,9 @@ typedef enum {
* Summarization function to remove a subset of variables from a system with the
* sequential solver. This does not require that the system be fully constrained.
*
* Requirement: set of keys in the graph should match the set of keys in the
* values structure.
*
* @param graph A full nonlinear graph
* @param values The chosen linearization point
* @param saved_keys is the set of keys for variables that should remain

313
gtsam/slam/serialization.cpp Normal file
View File

@ -0,0 +1,313 @@
/**
* @file serialization.cpp
*
* @date Jun 12, 2013
* @author Alex Cunningham
*/
#include <gtsam/slam/serialization.h>
#include <gtsam/base/serialization.h>
//#include <gtsam/slam/AntiFactor.h>
#include <gtsam/slam/BearingFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/slam/BetweenFactor.h>
//#include <gtsam/slam/BoundingConstraint.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/LieMatrix.h>
//#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3DS2.h>
//#include <gtsam/geometry/Cal3_S2Stereo.h>
using namespace gtsam;
// Creating as many permutations of factors as possible
typedef PriorFactor<LieVector> PriorFactorLieVector;
typedef PriorFactor<LieMatrix> PriorFactorLieMatrix;
typedef PriorFactor<Point2> PriorFactorPoint2;
typedef PriorFactor<StereoPoint2> PriorFactorStereoPoint2;
typedef PriorFactor<Point3> PriorFactorPoint3;
typedef PriorFactor<Rot2> PriorFactorRot2;
typedef PriorFactor<Rot3> PriorFactorRot3;
typedef PriorFactor<Pose2> PriorFactorPose2;
typedef PriorFactor<Pose3> PriorFactorPose3;
typedef PriorFactor<Cal3_S2> PriorFactorCal3_S2;
typedef PriorFactor<Cal3DS2> PriorFactorCal3DS2;
typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera;
typedef PriorFactor<SimpleCamera> PriorFactorSimpleCamera;
typedef PriorFactor<StereoCamera> PriorFactorStereoCamera;
typedef BetweenFactor<LieVector> BetweenFactorLieVector;
typedef BetweenFactor<LieMatrix> BetweenFactorLieMatrix;
typedef BetweenFactor<Point2> BetweenFactorPoint2;
typedef BetweenFactor<Point3> BetweenFactorPoint3;
typedef BetweenFactor<Rot2> BetweenFactorRot2;
typedef BetweenFactor<Rot3> BetweenFactorRot3;
typedef BetweenFactor<Pose2> BetweenFactorPose2;
typedef BetweenFactor<Pose3> BetweenFactorPose3;
typedef NonlinearEquality<LieVector> NonlinearEqualityLieVector;
typedef NonlinearEquality<LieMatrix> NonlinearEqualityLieMatrix;
typedef NonlinearEquality<Point2> NonlinearEqualityPoint2;
typedef NonlinearEquality<StereoPoint2> NonlinearEqualityStereoPoint2;
typedef NonlinearEquality<Point3> NonlinearEqualityPoint3;
typedef NonlinearEquality<Rot2> NonlinearEqualityRot2;
typedef NonlinearEquality<Rot3> NonlinearEqualityRot3;
typedef NonlinearEquality<Pose2> NonlinearEqualityPose2;
typedef NonlinearEquality<Pose3> NonlinearEqualityPose3;
typedef NonlinearEquality<Cal3_S2> NonlinearEqualityCal3_S2;
typedef NonlinearEquality<Cal3DS2> NonlinearEqualityCal3DS2;
typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera;
typedef NonlinearEquality<SimpleCamera> NonlinearEqualitySimpleCamera;
typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera;
typedef RangeFactor<Pose2, Point2> RangeFactorPosePoint2;
typedef RangeFactor<Pose3, Point3> RangeFactorPosePoint3;
typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint;
typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraPoint;
typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera;
typedef BearingFactor<Pose2, Point2, Rot2> BearingFactor2D;
typedef BearingFactor<Pose3, Point3, Rot3> BearingFactor3D;
typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorCal3_S2;
typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3> GenericStereoFactor3D;
/* Create GUIDs for Noisemodels */
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* Create GUIDs for geometry */
/* ************************************************************************* */
BOOST_CLASS_EXPORT(gtsam::LieVector);
BOOST_CLASS_EXPORT(gtsam::LieMatrix);
BOOST_CLASS_EXPORT(gtsam::Point2);
BOOST_CLASS_EXPORT(gtsam::StereoPoint2);
BOOST_CLASS_EXPORT(gtsam::Point3);
BOOST_CLASS_EXPORT(gtsam::Rot2);
BOOST_CLASS_EXPORT(gtsam::Rot3);
BOOST_CLASS_EXPORT(gtsam::Pose2);
BOOST_CLASS_EXPORT(gtsam::Pose3);
BOOST_CLASS_EXPORT(gtsam::Cal3_S2);
BOOST_CLASS_EXPORT(gtsam::Cal3DS2);
BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo);
BOOST_CLASS_EXPORT(gtsam::CalibratedCamera);
BOOST_CLASS_EXPORT(gtsam::SimpleCamera);
BOOST_CLASS_EXPORT(gtsam::StereoCamera);
/* Create GUIDs for factors */
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector");
BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix");
BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2");
BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2");
BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3");
BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2");
BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3");
BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2");
BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3");
BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2");
BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera");
BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera");
BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera");
BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector");
BOOST_CLASS_EXPORT_GUID(BetweenFactorLieMatrix, "gtsam::BetweenFactorLieMatrix");
BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2");
BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3");
BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2");
BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3");
BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2");
BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieVector, "gtsam::NonlinearEqualityLieVector");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieMatrix, "gtsam::NonlinearEqualityLieMatrix");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera");
BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2");
BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3");
BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2");
BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3");
BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint");
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint");
BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera");
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera");
BOOST_CLASS_EXPORT_GUID(BearingFactor2D, "gtsam::BearingFactor2D");
BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D");
BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2");
BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2");
//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2");
BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D");
/* ************************************************************************* */
// Actual implementations of functions
/* ************************************************************************* */
std::string gtsam::serializeGraph(const NonlinearFactorGraph& graph) {
return serialize<NonlinearFactorGraph>(graph);
}
/* ************************************************************************* */
NonlinearFactorGraph::shared_ptr gtsam::deserializeGraph(const std::string& serialized_graph) {
NonlinearFactorGraph::shared_ptr result(new NonlinearFactorGraph());
deserialize<NonlinearFactorGraph>(serialized_graph, *result);
return result;
}
/* ************************************************************************* */
std::string gtsam::serializeGraphXML(const NonlinearFactorGraph& graph, const std::string& name) {
return serializeXML<NonlinearFactorGraph>(graph, name);
}
/* ************************************************************************* */
NonlinearFactorGraph::shared_ptr gtsam::deserializeGraphXML(const std::string& serialized_graph,
const std::string& name) {
NonlinearFactorGraph::shared_ptr result(new NonlinearFactorGraph());
deserializeXML<NonlinearFactorGraph>(serialized_graph, *result, name);
return result;
}
/* ************************************************************************* */
std::string gtsam::serializeValues(const Values& values) {
return serialize<Values>(values);
}
/* ************************************************************************* */
Values::shared_ptr gtsam::deserializeValues(const std::string& serialized_values) {
Values::shared_ptr result(new Values());
deserialize<Values>(serialized_values, *result);
return result;
}
/* ************************************************************************* */
std::string gtsam::serializeValuesXML(const Values& values, const std::string& name) {
return serializeXML<Values>(values, name);
}
/* ************************************************************************* */
Values::shared_ptr gtsam::deserializeValuesXML(const std::string& serialized_values,
const std::string& name) {
Values::shared_ptr result(new Values());
deserializeXML<Values>(serialized_values, *result, name);
return result;
}
/* ************************************************************************* */
bool gtsam::serializeGraphToFile(const NonlinearFactorGraph& graph, const std::string& fname) {
return serializeToFile<NonlinearFactorGraph>(graph, fname);
}
/* ************************************************************************* */
bool gtsam::serializeGraphToXMLFile(const NonlinearFactorGraph& graph,
const std::string& fname, const std::string& name) {
return serializeToXMLFile<NonlinearFactorGraph>(graph, fname, name);
}
/* ************************************************************************* */
bool gtsam::serializeValuesToFile(const Values& values, const std::string& fname) {
return serializeToFile<Values>(values, fname);
}
/* ************************************************************************* */
bool gtsam::serializeValuesToXMLFile(const Values& values,
const std::string& fname, const std::string& name) {
return serializeToXMLFile<Values>(values, fname, name);
}
/* ************************************************************************* */
NonlinearFactorGraph::shared_ptr gtsam::deserializeGraphFromFile(const std::string& fname) {
NonlinearFactorGraph::shared_ptr result(new NonlinearFactorGraph());
if (!deserializeFromFile<NonlinearFactorGraph>(fname, *result))
throw std::invalid_argument("Failed to open file" + fname);
return result;
}
/* ************************************************************************* */
NonlinearFactorGraph::shared_ptr gtsam::deserializeGraphFromXMLFile(const std::string& fname,
const std::string& name) {
NonlinearFactorGraph::shared_ptr result(new NonlinearFactorGraph());
if (!deserializeFromXMLFile<NonlinearFactorGraph>(fname, *result, name))
throw std::invalid_argument("Failed to open file" + fname);
return result;
}
/* ************************************************************************* */
Values::shared_ptr gtsam::deserializeValuesFromFile(const std::string& fname) {
Values::shared_ptr result(new Values());
if (!deserializeFromFile<Values>(fname, *result))
throw std::invalid_argument("Failed to open file" + fname);
return result;
}
/* ************************************************************************* */
Values::shared_ptr gtsam::deserializeValuesFromXMLFile(const std::string& fname,
const std::string& name) {
Values::shared_ptr result(new Values());
if (!deserializeFromXMLFile<Values>(fname, *result, name))
throw std::invalid_argument("Failed to open file" + fname);
return result;
}
/* ************************************************************************* */

62
gtsam/slam/serialization.h Normal file
View File

@ -0,0 +1,62 @@
/**
* @file serialization.h
*
* @brief Global functions for performing serialization, designed for use with matlab
*
* @date Jun 12, 2013
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
namespace gtsam {
// Serialize/Deserialize a NonlinearFactorGraph
std::string serializeGraph(const NonlinearFactorGraph& graph);
NonlinearFactorGraph::shared_ptr deserializeGraph(const std::string& serialized_graph);
std::string serializeGraphXML(const NonlinearFactorGraph& graph,
const std::string& name = "graph");
NonlinearFactorGraph::shared_ptr deserializeGraphXML(const std::string& serialized_graph,
const std::string& name = "graph");
// Serialize/Deserialize a Values
std::string serializeValues(const Values& values);
Values::shared_ptr deserializeValues(const std::string& serialized_values);
std::string serializeValuesXML(const Values& values, const std::string& name = "values");
Values::shared_ptr deserializeValuesXML(const std::string& serialized_values,
const std::string& name = "values");
// Serialize to/from files
// serialize functions return true if successful
// Filename arguments include path
// Serialize
bool serializeGraphToFile(const NonlinearFactorGraph& graph, const std::string& fname);
bool serializeGraphToXMLFile(const NonlinearFactorGraph& graph,
const std::string& fname, const std::string& name = "graph");
bool serializeValuesToFile(const Values& values, const std::string& fname);
bool serializeValuesToXMLFile(const Values& values,
const std::string& fname, const std::string& name = "values");
// Deserialize
NonlinearFactorGraph::shared_ptr deserializeGraphFromFile(const std::string& fname);
NonlinearFactorGraph::shared_ptr deserializeGraphFromXMLFile(const std::string& fname,
const std::string& name = "graph");
Values::shared_ptr deserializeValuesFromFile(const std::string& fname);
Values::shared_ptr deserializeValuesFromXMLFile(const std::string& fname,
const std::string& name = "values");
} // \namespace gtsam

122
gtsam/slam/tests/testSerialization.cpp Normal file
View File

@ -0,0 +1,122 @@
/**
* @file testSerialization.cpp
*
* @brief Tests for serialization global functions using boost.serialization
*
* @date Jun 12, 2013
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/serialization.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <stdlib.h>
#include <fstream>
#include <sstream>
#include <boost/assign/std/vector.hpp>
#include <boost/filesystem.hpp>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
namespace fs = boost::filesystem;
#ifdef TOPSRCDIR
static string topdir = TOPSRCDIR;
#else
static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not defined, we error
#endif
Values exampleValues() {
Values result;
result.insert(234, gtsam::Rot2::fromAngle(0.1));
result.insert(123, gtsam::Point2(1.0, 2.0));
result.insert(254, gtsam::Pose2(1.0, 2.0, 0.3));
result.insert(678, gtsam::Rot3::Rx(0.1));
result.insert(498, gtsam::Point3(1.0, 2.0, 3.0));
result.insert(345, gtsam::Pose3(Rot3::Rx(0.1), Point3(1.0, 2.0, 3.0)));
return result;
}
NonlinearFactorGraph exampleGraph() {
NonlinearFactorGraph graph;
graph.add(PriorFactor<Pose2>(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3))));
graph.add(BetweenFactor<Pose2>(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3))));
graph.add(BearingRangeFactor<Pose2,Point2>(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(ones(2))));
return graph;
}
/* ************************************************************************* */
TEST( testSerialization, text_graph_serialization ) {
NonlinearFactorGraph graph = exampleGraph();
string serialized = serializeGraph(graph);
NonlinearFactorGraph actGraph = *deserializeGraph(serialized);
EXPECT(assert_equal(graph, actGraph));
}
/* ************************************************************************* */
TEST( testSerialization, xml_graph_serialization ) {
NonlinearFactorGraph graph = exampleGraph();
string serialized = serializeGraphXML(graph, "graph1");
NonlinearFactorGraph actGraph = *deserializeGraphXML(serialized, "graph1");
EXPECT(assert_equal(graph, actGraph));
}
/* ************************************************************************* */
TEST( testSerialization, text_values_serialization ) {
Values values = exampleValues();
string serialized = serializeValues(values);
Values actValues = *deserializeValues(serialized);
EXPECT(assert_equal(values, actValues));
}
/* ************************************************************************* */
TEST( testSerialization, xml_values_serialization ) {
Values values = exampleValues();
string serialized = serializeValuesXML(values, "values1");
Values actValues = *deserializeValuesXML(serialized, "values1");
EXPECT(assert_equal(values, actValues, 1e-5));
}
/* ************************************************************************* */
TEST( testSerialization, serialization_file ) {
// Create files in folder in build folder
fs::remove_all("actual");
fs::create_directory("actual");
string path = "actual/";
NonlinearFactorGraph graph = exampleGraph();
Values values = exampleValues();
// Serialize objects using each configuration
EXPECT(serializeGraphToFile(graph, path + "graph.dat"));
EXPECT(serializeGraphToXMLFile(graph, path + "graph.xml", "graph1"));
EXPECT(serializeValuesToFile(values, path + "values.dat"));
EXPECT(serializeValuesToXMLFile(values, path + "values.xml", "values1"));
// Deserialize
NonlinearFactorGraph actGraph = *deserializeGraphFromFile(path + "graph.dat");
NonlinearFactorGraph actGraphXML = *deserializeGraphFromXMLFile(path + "graph.xml", "graph1");
Values actValues = *deserializeValuesFromFile(path + "values.dat");
Values actValuesXML = *deserializeValuesFromXMLFile(path + "values.xml", "values1");
// Verify
EXPECT(assert_equal(graph, actGraph));
EXPECT(assert_equal(graph, actGraphXML));
EXPECT(assert_equal(values, actValues));
EXPECT(assert_equal(values, actValuesXML));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

2
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -232,7 +232,7 @@ double PoseRTV::range(const PoseRTV& other,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5);
if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5);
return t().dist(other.t());
return t().distance(other.t());
}
/* ************************************************************************* */

2
gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp
View File

@ -118,7 +118,7 @@ int main(int argc, char** argv) {
newFactors.add(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
// 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));

6
gtsam_unstable/geometry/SimPolygon2D.cpp
View File

@ -152,7 +152,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
Pose2 xC = xB.retract(delta(3, 0, dBC));
// use triangle equality to verify non-degenerate triangle
double dAC = xA.t().dist(xC.t());
double dAC = xA.t().distance(xC.t());
// form a triangle and test if it meets requirements
SimPolygon2D test_tri = SimPolygon2D::createTriangle(xA.t(), xB.t(), xC.t());
@ -165,7 +165,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
insideBox(side_len, test_tri.landmark(0)) &&
insideBox(side_len, test_tri.landmark(1)) &&
insideBox(side_len, test_tri.landmark(2)) &&
test_tri.landmark(1).dist(test_tri.landmark(2)) > min_side_len &&
test_tri.landmark(1).distance(test_tri.landmark(2)) > min_side_len &&
!nearExisting(lms, test_tri.landmark(0), min_vertex_dist) &&
!nearExisting(lms, test_tri.landmark(1), min_vertex_dist) &&
!nearExisting(lms, test_tri.landmark(2), min_vertex_dist) &&
@ -321,7 +321,7 @@ bool SimPolygon2D::insideBox(double s, const Point2& p) {
bool SimPolygon2D::nearExisting(const std::vector<Point2>& S,
const Point2& p, double threshold) {
BOOST_FOREACH(const Point2& Sp, S)
if (Sp.dist(p) < threshold)
if (Sp.distance(p) < threshold)
return true;
return false;
}

4
gtsam_unstable/geometry/SimWall2D.cpp
View File

@ -139,7 +139,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
Point2 cur_intersection;
if (wall.intersects(traj,cur_intersection)) {
collision = true;
if (cur_pose.t().dist(cur_intersection) < cur_pose.t().dist(intersection)) {
if (cur_pose.t().distance(cur_intersection) < cur_pose.t().distance(intersection)) {
intersection = cur_intersection;
closest_wall = wall;
}
@ -155,7 +155,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
norm = norm / norm.norm();
// Simple check to make sure norm is on side closest robot
if (cur_pose.t().dist(intersection + norm) > cur_pose.t().dist(intersection - norm))
if (cur_pose.t().distance(intersection + norm) > cur_pose.t().distance(intersection - norm))
norm = norm.inverse();
// using the reflection

2
gtsam_unstable/geometry/SimWall2D.h
View File

@ -43,7 +43,7 @@ namespace gtsam {
SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); }
/** geometry */
double length() const { return a_.dist(b_); }
double length() const { return a_.distance(b_); }
Point2 midpoint() const;
/**

22
gtsam_unstable/linear/bayesTreeOperations.cpp
View File

@ -48,7 +48,7 @@ GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor) {
JacobianFactor::const_iterator rowIt, colIt;
const size_t totalRows = jf->rows();
size_t rowsRemaining = totalRows;
for (rowIt = jf->begin(); rowIt != jf->end(); ++rowIt) {
for (rowIt = jf->begin(); rowIt != jf->end() && rowsRemaining > 0; ++rowIt) {
// get dim of current variable
size_t varDim = jf->getDim(rowIt);
size_t startRow = totalRows - rowsRemaining;
@ -127,26 +127,6 @@ findPathCliques(const GaussianBayesTree::sharedClique& initial) {
return result;
}
/* ************************************************************************* */
GaussianFactorGraph liquefy(const GaussianBayesTree::sharedClique& root, bool splitConditionals) {
GaussianFactorGraph result;
if (root && root->conditional()) {
GaussianConditional::shared_ptr conditional = root->conditional();
if (!splitConditionals)
result.push_back(conditional->toFactor());
else
result.push_back(splitFactor(conditional->toFactor()));
}
BOOST_FOREACH(const GaussianBayesTree::sharedClique& child, root->children())
result.push_back(liquefy(child, splitConditionals));
return result;
}
/* ************************************************************************* */
GaussianFactorGraph liquefy(const GaussianBayesTree& bayesTree, bool splitConditionals) {
return liquefy(bayesTree.root(), splitConditionals);
}
/* ************************************************************************* */
} // \namespace gtsam

30
gtsam_unstable/linear/bayesTreeOperations.h
View File

@ -67,13 +67,35 @@ GTSAM_UNSTABLE_EXPORT std::deque<GaussianBayesTree::sharedClique>
findPathCliques(const GaussianBayesTree::sharedClique& initial);
/**
* Liquefies a GaussianBayesTree into a GaussianFactorGraph recursively, given either a
* root clique or a full bayes tree.
* Liquefies a BayesTree into a GaussianFactorGraph recursively, given a
* root clique
*
* @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors
*/
GTSAM_UNSTABLE_EXPORT GaussianFactorGraph liquefy(const GaussianBayesTree::sharedClique& root, bool splitConditionals = false);
GTSAM_UNSTABLE_EXPORT GaussianFactorGraph liquefy(const GaussianBayesTree& bayesTree, bool splitConditionals = false);
template <class BAYESTREE>
GaussianFactorGraph liquefy(const typename BAYESTREE::sharedClique& root, bool splitConditionals = false) {
GaussianFactorGraph result;
if (root && root->conditional()) {
GaussianConditional::shared_ptr conditional = root->conditional();
if (!splitConditionals)
result.push_back(conditional->toFactor());
else
result.push_back(splitFactor(conditional->toFactor()));
}
BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, root->children())
result.push_back(liquefy<BAYESTREE>(child, splitConditionals));
return result;
}
/**
* Liquefies a BayesTree into a GaussianFactorGraph recursively, from a full bayes tree.
*
* @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors
*/
template <class BAYESTREE>
GaussianFactorGraph liquefy(const BAYESTREE& bayesTree, bool splitConditionals = false) {
return liquefy<BAYESTREE>(bayesTree.root(), splitConditionals);
}
} // \namespace gtsam

253
gtsam_unstable/linear/tests/testBayesTreeOperations.cpp
View File

@ -10,6 +10,9 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/linear/bayesTreeOperations.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <tests/smallExample.h>
using namespace gtsam;
@ -17,8 +20,15 @@ SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(ones(2));
SharedDiagonal model4 = noiseModel::Diagonal::Sigmas(ones(4));
SharedDiagonal model6 = noiseModel::Diagonal::Sigmas(ones(6));
using namespace std;
using symbol_shorthand::X;
using symbol_shorthand::L;
static const double tol = 1e-4;
/* ************************************************************************* */
TEST( testLinearTools, splitFactor ) {
TEST( testBayesTreeOperations, splitFactor1 ) {
// Build upper-triangular system
JacobianFactor initFactor(
@ -58,7 +68,7 @@ TEST( testLinearTools, splitFactor ) {
}
/* ************************************************************************* */
TEST_UNSAFE( testLinearTools, splitFactor2 ) {
TEST( testBayesTreeOperations, splitFactor2 ) {
// Build upper-triangular system
JacobianFactor initFactor(
@ -120,6 +130,245 @@ TEST_UNSAFE( testLinearTools, splitFactor2 ) {
EXPECT(assert_equal(expSplit, actSplit));
}
/* ************************************************************************* */
TEST( testBayesTreeOperations, splitFactor3 ) {
// Build upper-triangular system
JacobianFactor initFactor(
0,Matrix_(4, 2,
1.0, 2.0,
0.0, 3.0,
0.0, 0.0,
0.0, 0.0),
1,Matrix_(4, 2,
1.0, 2.0,
9.0, 3.0,
6.0, 8.0,
0.0, 7.0),
2,Matrix_(4, 2,
1.1, 2.2,
9.1, 3.2,
6.1, 8.2,
0.1, 7.2),
Vector_(4, 0.1, 0.2, 0.3, 0.4),
model4);
GaussianFactorGraph actSplit = splitFactor(initFactor.clone());
GaussianFactorGraph expSplit;
expSplit.add(
0,Matrix_(2, 2,
1.0, 2.0,
0.0, 3.0),
1,Matrix_(2, 2,
1.0, 2.0,
9.0, 3.0),
2,Matrix_(2, 2,
1.1, 2.2,
9.1, 3.2),
Vector_(2, 0.1, 0.2),
model2);
expSplit.add(
1,Matrix_(2, 2,
6.0, 8.0,
0.0, 7.0),
2,Matrix_(2, 2,
6.1, 8.2,
0.1, 7.2),
Vector_(2, 0.3, 0.4),
model2);
EXPECT(assert_equal(expSplit, actSplit));
}
/* ************************************************************************* */
// Some numbers that should be consistent among all smoother tests
//static double sigmax1 = 0.786153, /*sigmax2 = 1.0/1.47292,*/ sigmax3 = 0.671512, sigmax4 =
// 0.669534 /*, sigmax5 = sigmax3, sigmax6 = sigmax2*/, sigmax7 = sigmax1;
/* ************************************************************************* */
TEST( testBayesTreeOperations, liquefy ) {
using namespace example;
// Create smoother with 7 nodes
Ordering ordering;
ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraph smoother = createSmoother(7, ordering).first;
// Create the Bayes tree
GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
// bayesTree.print("Full tree");
SharedDiagonal unit6 = noiseModel::Diagonal::Sigmas(Vector_(ones(6)));
SharedDiagonal unit4 = noiseModel::Diagonal::Sigmas(Vector_(ones(4)));
SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector_(ones(2)));
// Liquefy the tree back into a graph
{
GaussianFactorGraph actGraph = liquefy(bayesTree, false); // Doesn't split conditionals
GaussianFactorGraph expGraph;
Matrix A12 = Matrix_(6, 2,
1.73205081, 0.0,
0.0, 1.73205081,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0);
Matrix A15 = Matrix_(6, 2,
-0.577350269, 0.0,
0.0, -0.577350269,
1.47196014, 0.0,
0.0, 1.47196014,
0.0, 0.0,
0.0, 0.0);
Matrix A16 = Matrix_(6, 2,
-0.577350269, 0.0,
0.0, -0.577350269,
-0.226455407, 0.0,
0.0, -0.226455407,
1.49357599, 0.0,
0.0, 1.49357599);
expGraph.add(2, A12, 5, A15, 6, A16, zeros(6,1), unit6);
Matrix A21 = Matrix_(4, 2,
1.73205081, 0.0,
0.0, 1.73205081,
0.0, 0.0,
0.0, 0.0);
Matrix A24 = Matrix_(4, 2,
-0.577350269, 0.0,
0.0, -0.577350269,
1.47196014, 0.0,
0.0, 1.47196014);
Matrix A26 = Matrix_(4, 2,
-0.577350269, 0.0,
0.0, -0.577350269,
-0.226455407, 0.0,
0.0, -0.226455407);
expGraph.add(1, A21, 4, A24, 6, A26, zeros(4,1), unit4);
Matrix A30 = Matrix_(2, 2,
1.41421356, 0.0,
0.0, 1.41421356);
Matrix A34 = Matrix_(2, 2,
-0.707106781, 0.0,
0.0, -0.707106781);
expGraph.add(0, A30, 4, A34, zeros(2,1), unit2);
Matrix A43 = Matrix_(2, 2,
1.41421356, 0.0,
0.0, 1.41421356);
Matrix A45 = Matrix_(2, 2,
-0.707106781, 0.0,
0.0, -0.707106781);
expGraph.add(3, A43, 5, A45, zeros(2,1), unit2);
EXPECT(assert_equal(expGraph, actGraph, tol));
}
// Liquefy the tree back into a graph, splitting factors
{
GaussianFactorGraph actGraph = liquefy(bayesTree, true);
GaussianFactorGraph expGraph;
// Conditional 1
{
Matrix A12 = Matrix_(2, 2,
1.73205081, 0.0,
0.0, 1.73205081);
Matrix A15 = Matrix_(2, 2,
-0.577350269, 0.0,
0.0, -0.577350269);
Matrix A16 = Matrix_(2, 2,
-0.577350269, 0.0,
0.0, -0.577350269);
expGraph.add(2, A12, 5, A15, 6, A16, zeros(2,1), unit2);
}
{
Matrix A15 = Matrix_(2, 2,
1.47196014, 0.0,
0.0, 1.47196014);
Matrix A16 = Matrix_(2, 2,
-0.226455407, 0.0,
0.0, -0.226455407);
expGraph.add(5, A15, 6, A16, zeros(2,1), unit2);
}
{
Matrix A16 = Matrix_(2, 2,
1.49357599, 0.0,
0.0, 1.49357599);
expGraph.add(6, A16, zeros(2,1), unit2);
}
// Conditional 2
{
Matrix A21 = Matrix_(2, 2,
1.73205081, 0.0,
0.0, 1.73205081);
Matrix A24 = Matrix_(2, 2,
-0.577350269, 0.0,
0.0, -0.577350269);
Matrix A26 = Matrix_(2, 2,
-0.577350269, 0.0,
0.0, -0.577350269);
expGraph.add(1, A21, 4, A24, 6, A26, zeros(2,1), unit2);
}
{
Matrix A24 = Matrix_(2, 2,
1.47196014, 0.0,
0.0, 1.47196014);
Matrix A26 = Matrix_(2, 2,
-0.226455407, 0.0,
0.0, -0.226455407);
expGraph.add(4, A24, 6, A26, zeros(2,1), unit2);
}
// Conditional 3
Matrix A30 = Matrix_(2, 2,
1.41421356, 0.0,
0.0, 1.41421356);
Matrix A34 = Matrix_(2, 2,
-0.707106781, 0.0,
0.0, -0.707106781);
expGraph.add(0, A30, 4, A34, zeros(2,1), unit2);
// Conditional 4
Matrix A43 = Matrix_(2, 2,
1.41421356, 0.0,
0.0, 1.41421356);
Matrix A45 = Matrix_(2, 2,
-0.707106781, 0.0,
0.0, -0.707106781);
expGraph.add(3, A43, 5, A45, zeros(2,1), unit2);
EXPECT(assert_equal(expGraph, actGraph, tol));
}
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

23
gtsam_unstable/slam/AHRS.cpp
View File

@ -24,11 +24,12 @@ Matrix I3 = eye(3);
Matrix Z3 = zeros(3, 3);
/* ************************************************************************* */
AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e) :
AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
bool flat) :
KF_(9) {
// Initial state
mech0_ = Mechanization_bRn2::initialize(stationaryU, stationaryF, g_e);
mech0_ = Mechanization_bRn2::initialize(stationaryU, stationaryF, g_e, flat);
size_t T = stationaryU.cols();
@ -145,6 +146,18 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::integrate(
return make_pair(newMech, newState);
}
/* ************************************************************************* */
bool AHRS::isAidingAvailable(const Mechanization_bRn2& mech,
const gtsam::Vector& f, const gtsam::Vector& u, double ge) {
// Subtract the biases
Vector f_ = f - mech.x_a();
Vector u_ = u - mech.x_g();
double mu_f = f_.norm() - ge;
double mu_u = u_.norm();
return (fabs(mu_f)<0.5 && mu_u < 5.0 / 180.0 * 3.1415926);
}
/* ************************************************************************* */
std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
@ -169,10 +182,14 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
// F(:,k) = mech.x_a + dx_a - bRn*n_g;
// F(:,k) = mech.x_a + dx_a - bRn*(I+P)*n_g;
// F(:,k) = mech.x_a + dx_a - b_g - bRn*(rho x n_g); // P = [rho]_x
// b_g - (mech.x_a - F(:,k)) = dx_a + bRn*(n_g x rho);
// Hence, the measurement z = b_g - (mech.x_a - F(:,k)) is predicted
// from the filter state (dx_a, rho) as dx_a + bRn*(n_g x rho)
// z = b_g - (mech.x_a - F(:,k)) = dx_a + bRn*(n_g x rho)
z = bRn * n_g_ - measured_b_g;
// Now the Jacobian H
Matrix b_g = bRn * n_g_cross_;
H = collect(3, &b_g, &Z3, &I3);
// And the measurement noise, TODO: should be created once where sigmas_v_a is given
R = diag(emul(sigmas_v_a_, sigmas_v_a_));
}

7
gtsam_unstable/slam/AHRS.h
View File

@ -14,7 +14,7 @@
namespace gtsam {
Matrix cov(const Matrix& m);
GTSAM_UNSTABLE_EXPORT Matrix cov(const Matrix& m);
class GTSAM_UNSTABLE_EXPORT AHRS {
@ -43,7 +43,7 @@ public:
* @param stationaryF initial interval of accelerator measurements, 3xn matrix
* @param g_e if given, initializes gravity with correct value g_e
*/
AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e);
AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, bool flat=false);
std::pair<Mechanization_bRn2, KalmanFilter::State> initialize(double g_e);
@ -51,6 +51,9 @@ public:
const Mechanization_bRn2& mech, KalmanFilter::State state,
const Vector& u, double dt);
bool isAidingAvailable(const Mechanization_bRn2& mech,
const Vector& f, const Vector& u, double ge);
std::pair<Mechanization_bRn2, KalmanFilter::State> aid(
const Mechanization_bRn2& mech, KalmanFilter::State state,
const Vector& f, bool Farrell=0);

32
gtsam_unstable/slam/Mechanization_bRn2.cpp
View File

@ -12,35 +12,43 @@ namespace gtsam {
/* ************************************************************************* */
Mechanization_bRn2 Mechanization_bRn2::initializeVector(const std::list<Vector>& U,
const std::list<Vector>& F, const double g_e) {
const std::list<Vector>& F, const double g_e, bool flat) {
Matrix Umat = Matrix_(3,U.size(), concatVectors(U));
Matrix Fmat = Matrix_(3,F.size(), concatVectors(F));
return initialize(Umat, Fmat, g_e);
return initialize(Umat, Fmat, g_e, flat);
}
/* ************************************************************************* */
Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U,
const Matrix& F, const double g_e) {
const Matrix& F, const double g_e, bool flat) {
// estimate gyro bias by averaging gyroscope readings (10.62)
Vector x_g = U.rowwise().mean();
Vector meanF = -F.rowwise().mean();
Vector meanF = F.rowwise().mean();
Vector b_g, x_a;
// estimate gravity
Vector b_g;
if(g_e == 0) {
// estimate gravity in body frame from accelerometer (10.13)
b_g = meanF;
// estimate accelerometer bias as zero (10.65)
x_a = zero(3);
if (flat)
// acceleration measured is along the z-axis.
b_g = Vector_(3, 0.0, 0.0, norm_2(meanF));
else
// acceleration measured is the opposite of gravity (10.13)
b_g = -meanF;
} else {
if (flat)
// gravity is downward along the z-axis since we are flat on the ground
b_g = Vector_(3,0.0,0.0,g_e);
else
// normalize b_g and attribute remainder to biases
b_g = g_e * meanF/meanF.norm();
x_a = -(meanF - b_g);
b_g = - g_e * meanF/meanF.norm();
}
// estimate accelerometer bias
Vector x_a = meanF + b_g;
// initialize roll, pitch (eqns. 10.14, 10.15)
double g1=b_g(0);
double g2=b_g(1);

7
gtsam_unstable/slam/Mechanization_bRn2.h
View File

@ -47,12 +47,15 @@ public:
* Initialize the first Mechanization state
* @param U a list of gyro measurement vectors
* @param F a list of accelerometer measurement vectors
* @param g_e gravity magnitude
* @param flat flag saying whether this is a flat trim init
*/
static Mechanization_bRn2 initializeVector(const std::list<Vector>& U,
const std::list<Vector>& F, const double g_e = 0);
const std::list<Vector>& F, const double g_e = 0, bool flat=false);
/// Matrix version of initialize
static Mechanization_bRn2 initialize(const Matrix& U,
const Matrix& F, const double g_e = 0);
const Matrix& F, const double g_e = 0, bool flat=false);
/**
* Correct AHRS full state given error state dx

5
matlab/+gtsam/Contents.m
View File

@ -29,6 +29,11 @@
% GaussianBayesTreeClique - class GaussianBayesTreeClique, see Doxygen page for details
% GaussianBayesTree - class GaussianBayesTree, see Doxygen page for details
% GaussianISAM - class GaussianISAM, see Doxygen page for details
% noiseModel.Gaussian - class noiseModel.Gaussian, see Doxygen page for details
% noiseModel.Diagonal - class noiseModel.Diagonal, see Doxygen page for details
% noiseModel.Constrained - class noiseModel.Constrained, see Doxygen page for details
% noiseModel.Isotropic - class noiseModel.Isotropic, see Doxygen page for details
% noiseModel.Unit - class noiseModel.Unit, see Doxygen page for details
%
%% Linear Inference
% GaussianSequentialSolver - class GaussianSequentialSolver, see Doxygen page for details

65
matlab/gtsam_tests/testGraphValuesSerialization.m Normal file
View File

@ -0,0 +1,65 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%
% @brief Simple robotics example using the pre-built planar SLAM domain
% @author Alex Cunningham
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Create keys for variables
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
j1 = symbol('l',1); j2 = symbol('l',2);
%% Create graph and factors
graph = NonlinearFactorGraph;
% Prior factor - FAIL: unregistered class
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
% Between Factors - FAIL: unregistered class
odometry = Pose2(2.0, 0.0, 0.0);
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
% BearingRange Factors - FAIL: unregistered class
degrees = pi/180;
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
%% Create Values
values = Values;
values.insert(i1, Pose2(0.5, 0.0, 0.2));
values.insert(i2, Pose2(2.3, 0.1,-0.2));
values.insert(i3, Pose2(4.1, 0.1, 0.1));
values.insert(j1, Point2(1.8, 2.1));
values.insert(j2, Point2(4.1, 1.8));
%% Check that serialization works properly
serialized_values = serializeValues(values); % returns a string
deserializedValues = deserializeValues(serialized_values); % returns a new values
CHECK('values.equals(deserializedValues)',values.equals(deserializedValues,1e-9));
CHECK('serializeValuesToFile(values, values.dat)', serializeValuesToFile(values, 'values.dat'));
deserializedValuesFile = deserializeValuesFromFile('values.dat'); % returns a new values
CHECK('values.equals(deserializedValuesFile)',values.equals(deserializedValuesFile,1e-9));
% % FAIL: unregistered class - derived class not registered or exported
% serialized_graph = serializeGraph(graph); % returns a string
% deserializedGraph = deserializeGraph(serialized_graph); % returns a new graph
% CHECK('graph.equals(deserializedGraph)',graph.equals(deserializedGraph,1e-9));
%
% CHECK('serializeGraphToFile(graph, graph.dat)', serializeGraphToFile(graph, 'graph.dat'));
% deserializedGraphFile = deserializeGraphFromFile('graph.dat'); % returns a new graph
% CHECK('graph.equals(deserializedGraphFile)',graph.equals(deserializedGraphFile,1e-9));

2
matlab/gtsam_tests/testPlanarSLAMExample.m
View File

@ -67,5 +67,3 @@ CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
point_1 = result.at(symbol('l',1));
marginals.marginalCovariance(symbol('l',1));
CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4));

1
matlab/gtsam_tests/testPose2SLAMExample.m
View File

@ -60,4 +60,3 @@ P = marginals.marginalCovariance(1);
pose_1 = result.at(1);
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

3
matlab/gtsam_tests/test_gtsam.m
View File

@ -30,5 +30,8 @@ testStereoVOExample
display 'Starting: testVisualISAMExample'
testVisualISAMExample
display 'Starting: testGraphValuesSerialization'
testVisualISAMExample
% end of tests
display 'Tests complete!'

132
matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m Normal file
View File

@ -0,0 +1,132 @@
% ----------------------------------------------------------------------------
%
% 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 ConcurrentFilteringAndSmoothingExample.m
% @brief Demonstration of the concurrent filtering and smoothing architecture 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
clear all;
clear all;
import gtsam.*;
% Define the smoother lag (in seconds)
lag = 2.0;
% Create a Concurrent Filter and Smoother
concurrentFilter = ConcurrentBatchFilter;
concurrentSmoother = ConcurrentBatchSmoother;
%% Create containers to store the factors and linearization points that
% will be sent to the smoothers
newFactors = NonlinearFactorGraph;
newValues = Values;
%% Create a prior on the first pose, placing it at the origin
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3 ; 0.3 ; 0.1]);
priorKey = uint64(0);
newFactors.add(PriorFactorPose2(priorKey, priorMean, priorNoise));
newValues.insert(priorKey, priorMean); % Initialize the first pose at the mean of the prior
%% Insert the prior factor into the filter
concurrentFilter.update(newFactors, newValues);
%% Now, loop through several time steps, creating factors from different "sensors"
% and adding them to the fixed-lag smoothers
deltaT = 0.25;
for time = deltaT : deltaT : 10.0
%% Initialize factor and values for this loop iteration
newFactors = NonlinearFactorGraph;
newValues = Values;
%% Define the keys related to this timestamp
previousKey = uint64(1000 * (time-deltaT));
currentKey = uint64(1000 * (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}
currentPose = Pose2(time * 2.0, 0.0, 0.0);
newValues.insert(currentKey, currentPose);
%% Add odometry factors from two different sources with different error stats
odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
odometryNoise1 = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]);
newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
odometryNoise2 = noiseModel.Isotropic.Sigma(3, 0.05);
newFactors.add(BetweenFactorPose2(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
oldKeys = KeyList;
if time >= lag+deltaT
oldKeys.push_back(uint64(1000 * (time-lag-deltaT)));
end
%% Update the various inference engines
concurrentFilter.update(newFactors, newValues, oldKeys);
%% Add a loop closure to the smoother at a particular time
if time == 8.0
% Now lets create a "loop closure" factor between some poses
loopKey1 = uint64(1000 * (0.0));
loopKey2 = uint64(1000 * (5.0));
loopMeasurement = Pose2(9.5, 1.00, 0.00);
loopNoise = noiseModel.Diagonal.Sigmas([0.5; 0.5; 0.25]);
loopFactor = BetweenFactorPose2(loopKey1, loopKey2, loopMeasurement, loopNoise);
% The state at 5.0s should have been transferred to the concurrent smoother at this point. Add the loop closure.
smootherFactors = NonlinearFactorGraph;
smootherFactors.add(loopFactor);
concurrentSmoother.update(smootherFactors, Values());
end
%% Manually synchronize the Concurrent Filter and Smoother every 1.0 s
if mod(time, 1.0) < 0.01
% Synchronize the Filter and Smoother
concurrentSmoother.update();
synchronize(concurrentFilter, concurrentSmoother);
end
%% Print the filter optimized poses
fprintf(1, 'Timestamp = %5.3f\n', time);
filterResult = concurrentFilter.calculateEstimate;
filterResult.at(currentKey).print('Concurrent Estimate: ');
%% Plot Covariance Ellipses
cla;
hold on
filterMarginals = Marginals(concurrentFilter.getFactors, filterResult);
plot2DTrajectory(filterResult, 'k*-', filterMarginals);
smootherGraph = concurrentSmoother.getFactors;
if smootherGraph.size > 0
smootherResult = concurrentSmoother.calculateEstimate;
smootherMarginals = Marginals(smootherGraph, smootherResult);
plot2DTrajectory(smootherResult, 'r*-', smootherMarginals);
end
axis equal
axis tight
view(2)
pause(0.01)
end

14
tests/CMakeLists.txt
View File

@ -1,15 +1,5 @@
# Build a library of example domains, just for tests
file(GLOB test_lib_srcs "*.cpp")
file(GLOB test_srcs "test*.cpp")
file(GLOB time_srcs "time*.cpp")
list(REMOVE_ITEM test_lib_srcs ${test_srcs})
list(REMOVE_ITEM test_lib_srcs ${time_srcs})
add_library(test_lib STATIC ${test_lib_srcs})
target_link_libraries(test_lib ${gtsam-default})
# Assemble local libraries
set(tests_local_libs
test_lib
slam
nonlinear
linear
@ -45,7 +35,7 @@ if (GTSAM_BUILD_TESTS)
# Build grouped tests
gtsam_add_grouped_scripts("tests" # Use subdirectory as group label
"test*.cpp" check "Test" # Standard for all tests
"${tests_local_libs}" "${gtsam-default};CppUnitLite;test_lib" "${tests_exclude}" # Pass in linking and exclusion lists
"${tests_local_libs}" "${gtsam-default};CppUnitLite" "${tests_exclude}" # Pass in linking and exclusion lists
${is_test}) # Set all as tests
endif (GTSAM_BUILD_TESTS)
@ -58,6 +48,6 @@ if (GTSAM_BUILD_TIMING)
# Build grouped benchmarks
gtsam_add_grouped_scripts("tests" # Use subdirectory as group label
"time*.cpp" timing "Timing Benchmark" # Standard for all timing scripts
"${tests_local_libs}" "${gtsam-default};CppUnitLite;test_lib" "${tests_exclude}" # Pass in linking and exclusion lists
"${tests_local_libs}" "${gtsam-default};CppUnitLite" "${tests_exclude}" # Pass in linking and exclusion lists
${is_test})
endif (GTSAM_BUILD_TIMING)

49
tests/simulated2D.cpp
View File

@ -1,49 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 simulated2D.cpp
* @brief measurement functions and derivatives for simulated 2D robot
* @author Frank Dellaert
*/
#include <tests/simulated2D.h>
namespace simulated2D {
static Matrix I = gtsam::eye(2);
/* ************************************************************************* */
Point2 prior(const Point2& x, boost::optional<Matrix&> H) {
if (H) *H = I;
return x;
}
/* ************************************************************************* */
Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
if (H1) *H1 = -I;
if (H2) *H2 = I;
return x2 - x1;
}
/* ************************************************************************* */
Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
if (H1) *H1 = -I;
if (H2) *H2 = I;
return l - x;
}
/* ************************************************************************* */
} // namespace simulated2D

17
tests/simulated2D.h
View File

@ -90,7 +90,10 @@ namespace simulated2D {
}
/// Prior on a single pose, optionally returns derivative
Point2 prior(const Point2& x, boost::optional<Matrix&> H = boost::none);
Point2 prior(const Point2& x, boost::optional<Matrix&> H = boost::none) {
if (H) *H = gtsam::eye(2);
return x;
}
/// odometry between two poses
inline Point2 odo(const Point2& x1, const Point2& x2) {
@ -99,7 +102,11 @@ namespace simulated2D {
/// odometry between two poses, optionally returns derivative
Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
boost::none, boost::optional<Matrix&> H2 = boost::none) {
if (H1) *H1 = -gtsam::eye(2);
if (H2) *H2 = gtsam::eye(2);
return x2 - x1;
}
/// measurement between landmark and pose
inline Point2 mea(const Point2& x, const Point2& l) {
@ -108,7 +115,11 @@ namespace simulated2D {
/// measurement between landmark and pose, optionally returns derivative
Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
boost::none, boost::optional<Matrix&> H2 = boost::none) {
if (H1) *H1 = -gtsam::eye(2);
if (H2) *H2 = gtsam::eye(2);
return l - x;
}
/**
* Unary factor encoding a soft prior on a vector

39
tests/simulated2DOriented.cpp
View File

@ -1,39 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 simulated2DOriented
* @brief measurement functions and derivatives for simulated 2D robot
* @author Frank Dellaert
*/
#include <tests/simulated2DOriented.h>
namespace simulated2DOriented {
static Matrix I = gtsam::eye(3);
/* ************************************************************************* */
Pose2 prior(const Pose2& x, boost::optional<Matrix&> H) {
if (H) *H = I;
return x;
}
/* ************************************************************************* */
Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
return x1.between(x2, H1, H2);
}
/* ************************************************************************* */
} // namespace simulated2DOriented

9
tests/simulated2DOriented.h
View File

@ -62,7 +62,10 @@ namespace simulated2DOriented {
}
/// Prior on a single pose, optional derivative version
Pose2 prior(const Pose2& x, boost::optional<Matrix&> H = boost::none);
Pose2 prior(const Pose2& x, boost::optional<Matrix&> H = boost::none) {
if (H) *H = gtsam::eye(3);
return x;
}
/// odometry between two poses
inline Pose2 odo(const Pose2& x1, const Pose2& x2) {
@ -71,7 +74,9 @@ namespace simulated2DOriented {
/// odometry between two poses, optional derivative version
Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
boost::none, boost::optional<Matrix&> H2 = boost::none) {
return x1.between(x2, H1, H2);
}
/// Unary factor encoding a soft prior on a vector
template<class VALUE = Pose2>

43
tests/simulated3D.cpp
View File

@ -1,43 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 Simulated3D.cpp
* @brief measurement functions and derivatives for simulated 3D robot
* @author Alex Cunningham
**/
#include <tests/simulated3D.h>
namespace gtsam {
namespace simulated3D {
Point3 prior (const Point3& x, boost::optional<Matrix&> H) {
if (H) *H = eye(3);
return x;
}
Point3 odo(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = -1 * eye(3);
if (H2) *H2 = eye(3);
return x2 - x1;
}
Point3 mea(const Point3& x, const Point3& l,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = -1 * eye(3);
if (H2) *H2 = eye(3);
return l - x;
}
}} // namespace gtsam::simulated3D

17
tests/simulated3D.h
View File

@ -38,21 +38,32 @@ namespace simulated3D {
/**
* Prior on a single pose
*/
Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none);
Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none) {
if (H) *H = eye(3);
return x;
}
/**
* odometry between two poses
*/
Point3 odo(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none);
boost::optional<Matrix&> H2 = boost::none) {
if (H1) *H1 = -1 * eye(3);
if (H2) *H2 = eye(3);
return x2 - x1;
}
/**
* measurement between landmark and pose
*/
Point3 mea(const Point3& x, const Point3& l,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none);
boost::optional<Matrix&> H2 = boost::none) {
if (H1) *H1 = -1 * eye(3);
if (H2) *H2 = eye(3);
return l - x;
}
/**
* A prior factor on a single linear robot pose

502
tests/smallExample.cpp
View File

@ -1,502 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 smallExample.cpp
* @brief Create small example with two poses and one landmark
* @brief smallExample
* @author Carlos Nieto
* @author Frank dellaert
*/
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/base/Matrix.h>
#include <tests/smallExample.h>
#include <boost/optional.hpp>
#include <boost/shared_ptr.hpp>
#include <iostream>
#include <string>
using namespace std;
namespace gtsam {
namespace example {
using namespace gtsam::noiseModel;
typedef boost::shared_ptr<NonlinearFactor> shared;
static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
static SharedDiagonal constraintModel = noiseModel::Constrained::All(2);
static const Index _l1_=0, _x1_=1, _x2_=2;
static const Index _x_=0, _y_=1, _z_=2;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
// Create
boost::shared_ptr<Graph> nlfg(
new Graph);
// prior on x1
Point2 mu;
shared f1(new simulated2D::Prior(mu, sigma0_1, X(1)));
nlfg->push_back(f1);
// odometry between x1 and x2
Point2 z2(1.5, 0);
shared f2(new simulated2D::Odometry(z2, sigma0_1, X(1), X(2)));
nlfg->push_back(f2);
// measurement between x1 and l1
Point2 z3(0, -1);
shared f3(new simulated2D::Measurement(z3, sigma0_2, X(1), L(1)));
nlfg->push_back(f3);
// measurement between x2 and l1
Point2 z4(-1.5, -1.);
shared f4(new simulated2D::Measurement(z4, sigma0_2, X(2), L(1)));
nlfg->push_back(f4);
return nlfg;
}
/* ************************************************************************* */
Graph createNonlinearFactorGraph() {
return *sharedNonlinearFactorGraph();
}
/* ************************************************************************* */
Values createValues() {
Values c;
c.insert(X(1), Point2(0.0, 0.0));
c.insert(X(2), Point2(1.5, 0.0));
c.insert(L(1), Point2(0.0, -1.0));
return c;
}
/* ************************************************************************* */
VectorValues createVectorValues() {
VectorValues c(vector<size_t>(3, 2));
c[_l1_] = Vector_(2, 0.0, -1.0);
c[_x1_] = Vector_(2, 0.0, 0.0);
c[_x2_] = Vector_(2, 1.5, 0.0);
return c;
}
/* ************************************************************************* */
boost::shared_ptr<const Values> sharedNoisyValues() {
boost::shared_ptr<Values> c(new Values);
c->insert(X(1), Point2(0.1, 0.1));
c->insert(X(2), Point2(1.4, 0.2));
c->insert(L(1), Point2(0.1, -1.1));
return c;
}
/* ************************************************************************* */
Values createNoisyValues() {
return *sharedNoisyValues();
}
/* ************************************************************************* */
VectorValues createCorrectDelta(const Ordering& ordering) {
VectorValues c(vector<size_t>(3,2));
c[ordering[L(1)]] = Vector_(2, -0.1, 0.1);
c[ordering[X(1)]] = Vector_(2, -0.1, -0.1);
c[ordering[X(2)]] = Vector_(2, 0.1, -0.2);
return c;
}
/* ************************************************************************* */
VectorValues createZeroDelta(const Ordering& ordering) {
VectorValues c(vector<size_t>(3,2));
c[ordering[L(1)]] = zero(2);
c[ordering[X(1)]] = zero(2);
c[ordering[X(2)]] = zero(2);
return c;
}
/* ************************************************************************* */
GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
// Create empty graph
GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2));
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2));
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2));
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2));
return fg;
}
/* ************************************************************************* */
/** create small Chordal Bayes Net x <- y
* x y d
* 1 1 9
* 1 5
*/
GaussianBayesNet createSmallGaussianBayesNet() {
Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
Matrix R22 = Matrix_(1, 1, 1.0);
Vector d1(1), d2(1);
d1(0) = 9;
d2(0) = 5;
Vector tau(1);
tau(0) = 1.0;
// define nodes and specify in reverse topological sort (i.e. parents last)
GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12, tau));
GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22, tau));
GaussianBayesNet cbn;
cbn.push_back(Px_y);
cbn.push_back(Py);
return cbn;
}
/* ************************************************************************* */
// Some nonlinear functions to optimize
/* ************************************************************************* */
namespace smallOptimize {
Point2 h(const Point2& v) {
return Point2(cos(v.x()), sin(v.y()));
}
Matrix H(const Point2& v) {
return Matrix_(2, 2,
-sin(v.x()), 0.0,
0.0, cos(v.y()));
}
struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> {
Point2 z_;
UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) :
gtsam::NoiseModelFactor1<Point2>(model, key), z_(z) {
}
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const {
if (A) *A = H(x);
return (h(x) - z_).vector();
}
};
}
/* ************************************************************************* */
boost::shared_ptr<const Graph> sharedReallyNonlinearFactorGraph() {
boost::shared_ptr<Graph> fg(new Graph);
Vector z = Vector_(2, 1.0, 0.0);
double sigma = 0.1;
boost::shared_ptr<smallOptimize::UnaryFactor> factor(
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
fg->push_back(factor);
return fg;
}
Graph createReallyNonlinearFactorGraph() {
return *sharedReallyNonlinearFactorGraph();
}
/* ************************************************************************* */
pair<Graph, Values> createNonlinearSmoother(int T) {
// Create
Graph nlfg;
Values poses;
// prior on x1
Point2 x1(1.0, 0.0);
shared prior(new simulated2D::Prior(x1, sigma1_0, X(1)));
nlfg.push_back(prior);
poses.insert(X(1), x1);
for (int t = 2; t <= T; t++) {
// odometry between x_t and x_{t-1}
Point2 odo(1.0, 0.0);
shared odometry(new simulated2D::Odometry(odo, sigma1_0, X(t - 1), X(t)));
nlfg.push_back(odometry);
// measurement on x_t is like perfect GPS
Point2 xt(t, 0);
shared measurement(new simulated2D::Prior(xt, sigma1_0, X(t)));
nlfg.push_back(measurement);
// initial estimate
poses.insert(X(t), xt);
}
return make_pair(nlfg, poses);
}
/* ************************************************************************* */
pair<FactorGraph<GaussianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering) {
Graph nlfg;
Values poses;
boost::tie(nlfg, poses) = createNonlinearSmoother(T);
if(!ordering) ordering = *poses.orderingArbitrary();
return make_pair(*nlfg.linearize(poses, *ordering), *ordering);
}
/* ************************************************************************* */
GaussianFactorGraph createSimpleConstraintGraph() {
// create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1
Matrix Ax = eye(2);
Vector b1(2);
b1(0) = 1.0;
b1(1) = -1.0;
JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));
// create binary constraint factor
// between _x_ and _y_, that is going to be the only factor on _y_
// |1 0||x_1| + |-1 0||y_1| = |0|
// |0 1||x_2| | 0 -1||y_2| |0|
Matrix Ax1 = eye(2);
Matrix Ay1 = eye(2) * -1;
Vector b2 = Vector_(2, 0.0, 0.0);
JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
constraintModel));
// construct the graph
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
return fg;
}
/* ************************************************************************* */
VectorValues createSimpleConstraintValues() {
VectorValues config(vector<size_t>(2,2));
Vector v = Vector_(2, 1.0, -1.0);
config[_x_] = v;
config[_y_] = v;
return config;
}
/* ************************************************************************* */
GaussianFactorGraph createSingleConstraintGraph() {
// create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1
Matrix Ax = eye(2);
Vector b1(2);
b1(0) = 1.0;
b1(1) = -1.0;
//GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1));
JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));
// create binary constraint factor
// between _x_ and _y_, that is going to be the only factor on _y_
// |1 2||x_1| + |10 0||y_1| = |1|
// |2 1||x_2| |0 10||y_2| |2|
Matrix Ax1(2, 2);
Ax1(0, 0) = 1.0;
Ax1(0, 1) = 2.0;
Ax1(1, 0) = 2.0;
Ax1(1, 1) = 1.0;
Matrix Ay1 = eye(2) * 10;
Vector b2 = Vector_(2, 1.0, 2.0);
JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
constraintModel));
// construct the graph
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
return fg;
}
/* ************************************************************************* */
VectorValues createSingleConstraintValues() {
VectorValues config(vector<size_t>(2,2));
config[_x_] = Vector_(2, 1.0, -1.0);
config[_y_] = Vector_(2, 0.2, 0.1);
return config;
}
/* ************************************************************************* */
GaussianFactorGraph createMultiConstraintGraph() {
// unary factor 1
Matrix A = eye(2);
Vector b = Vector_(2, -2.0, 2.0);
JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1));
// constraint 1
Matrix A11(2, 2);
A11(0, 0) = 1.0;
A11(0, 1) = 2.0;
A11(1, 0) = 2.0;
A11(1, 1) = 1.0;
Matrix A12(2, 2);
A12(0, 0) = 10.0;
A12(0, 1) = 0.0;
A12(1, 0) = 0.0;
A12(1, 1) = 10.0;
Vector b1(2);
b1(0) = 1.0;
b1(1) = 2.0;
JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1,
constraintModel));
// constraint 2
Matrix A21(2, 2);
A21(0, 0) = 3.0;
A21(0, 1) = 4.0;
A21(1, 0) = -1.0;
A21(1, 1) = -2.0;
Matrix A22(2, 2);
A22(0, 0) = 1.0;
A22(0, 1) = 1.0;
A22(1, 0) = 1.0;
A22(1, 1) = 2.0;
Vector b2(2);
b2(0) = 3.0;
b2(1) = 4.0;
JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2,
constraintModel));
// construct the graph
GaussianFactorGraph fg;
fg.push_back(lf1);
fg.push_back(lc1);
fg.push_back(lc2);
return fg;
}
/* ************************************************************************* */
VectorValues createMultiConstraintValues() {
VectorValues config(vector<size_t>(3,2));
config[_x_] = Vector_(2, -2.0, 2.0);
config[_y_] = Vector_(2, -0.1, 0.4);
config[_z_] = Vector_(2, -4.0, 5.0);
return config;
}
/* ************************************************************************* */
// Create key for simulated planar graph
Symbol key(int x, int y) {
return X(1000*x+y);
}
/* ************************************************************************* */
boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
// create empty graph
NonlinearFactorGraph nlfg;
// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), Isotropic::Sigma(2,1e-3), key(1,1)));
nlfg.push_back(constraint);
// Create horizontal constraints, 1...N*(N-1)
Point2 z1(1.0, 0.0); // move right
for (size_t x = 1; x < N; x++)
for (size_t y = 1; y <= N; y++) {
shared f(new simulated2D::Odometry(z1, Isotropic::Sigma(2,0.01), key(x, y), key(x + 1, y)));
nlfg.push_back(f);
}
// Create vertical constraints, N*(N-1)+1..2*N*(N-1)
Point2 z2(0.0, 1.0); // move up
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y < N; y++) {
shared f(new simulated2D::Odometry(z2, Isotropic::Sigma(2,0.01), key(x, y), key(x, y + 1)));
nlfg.push_back(f);
}
// Create linearization and ground xtrue config
Values zeros;
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++)
zeros.insert(key(x, y), Point2());
Ordering ordering(planarOrdering(N));
VectorValues xtrue(zeros.dims(ordering));
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++)
xtrue[ordering[key(x, y)]] = Point2(x,y).vector();
// linearize around zero
boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros, ordering);
return boost::make_tuple(*gfg, xtrue);
}
/* ************************************************************************* */
Ordering planarOrdering(size_t N) {
Ordering ordering;
for (size_t y = N; y >= 1; y--)
for (size_t x = N; x >= 1; x--)
ordering.push_back(key(x, y));
return ordering;
}
/* ************************************************************************* */
pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N,
const GaussianFactorGraph& original) {
GaussianFactorGraph T, C;
// Add the x11 constraint to the tree
T.push_back(original[0]);
// Add all horizontal constraints to the tree
size_t i = 1;
for (size_t x = 1; x < N; x++)
for (size_t y = 1; y <= N; y++, i++)
T.push_back(original[i]);
// Add first vertical column of constraints to T, others to C
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y < N; y++, i++)
if (x == 1)
T.push_back(original[i]);
else
C.push_back(original[i]);
return make_pair(T, C);
}
/* ************************************************************************* */
} // example
} // namespace gtsam

500
tests/smallExample.h
View File

@ -22,6 +22,7 @@
#pragma once
#include <tests/simulated2D.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <boost/tuple/tuple.hpp>
@ -151,3 +152,502 @@ namespace gtsam {
} // example
} // gtsam
// Implementations
namespace gtsam {
namespace example {
// using namespace gtsam::noiseModel;
namespace impl {
typedef boost::shared_ptr<NonlinearFactor> shared_nlf;
static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
static SharedDiagonal constraintModel = noiseModel::Constrained::All(2);
static const Index _l1_=0, _x1_=1, _x2_=2;
static const Index _x_=0, _y_=1, _z_=2;
} // \namespace impl
/* ************************************************************************* */
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
// Create
boost::shared_ptr<Graph> nlfg(
new Graph);
// prior on x1
Point2 mu;
shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1)));
nlfg->push_back(f1);
// odometry between x1 and x2
Point2 z2(1.5, 0);
shared_nlf f2(new simulated2D::Odometry(z2, sigma0_1, X(1), X(2)));
nlfg->push_back(f2);
// measurement between x1 and l1
Point2 z3(0, -1);
shared_nlf f3(new simulated2D::Measurement(z3, sigma0_2, X(1), L(1)));
nlfg->push_back(f3);
// measurement between x2 and l1
Point2 z4(-1.5, -1.);
shared_nlf f4(new simulated2D::Measurement(z4, sigma0_2, X(2), L(1)));
nlfg->push_back(f4);
return nlfg;
}
/* ************************************************************************* */
Graph createNonlinearFactorGraph() {
return *sharedNonlinearFactorGraph();
}
/* ************************************************************************* */
Values createValues() {
using symbol_shorthand::X;
using symbol_shorthand::L;
Values c;
c.insert(X(1), Point2(0.0, 0.0));
c.insert(X(2), Point2(1.5, 0.0));
c.insert(L(1), Point2(0.0, -1.0));
return c;
}
/* ************************************************************************* */
VectorValues createVectorValues() {
using namespace impl;
VectorValues c(std::vector<size_t>(3, 2));
c[_l1_] = Vector_(2, 0.0, -1.0);
c[_x1_] = Vector_(2, 0.0, 0.0);
c[_x2_] = Vector_(2, 1.5, 0.0);
return c;
}
/* ************************************************************************* */
boost::shared_ptr<const Values> sharedNoisyValues() {
using symbol_shorthand::X;
using symbol_shorthand::L;
boost::shared_ptr<Values> c(new Values);
c->insert(X(1), Point2(0.1, 0.1));
c->insert(X(2), Point2(1.4, 0.2));
c->insert(L(1), Point2(0.1, -1.1));
return c;
}
/* ************************************************************************* */
Values createNoisyValues() {
return *sharedNoisyValues();
}
/* ************************************************************************* */
VectorValues createCorrectDelta(const Ordering& ordering) {
using symbol_shorthand::X;
using symbol_shorthand::L;
VectorValues c(std::vector<size_t>(3,2));
c[ordering[L(1)]] = Vector_(2, -0.1, 0.1);
c[ordering[X(1)]] = Vector_(2, -0.1, -0.1);
c[ordering[X(2)]] = Vector_(2, 0.1, -0.2);
return c;
}
/* ************************************************************************* */
VectorValues createZeroDelta(const Ordering& ordering) {
using symbol_shorthand::X;
using symbol_shorthand::L;
VectorValues c(std::vector<size_t>(3,2));
c[ordering[L(1)]] = zero(2);
c[ordering[X(1)]] = zero(2);
c[ordering[X(2)]] = zero(2);
return c;
}
/* ************************************************************************* */
GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
using symbol_shorthand::X;
using symbol_shorthand::L;
// Create empty graph
GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2));
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2));
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2));
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2));
return fg;
}
/* ************************************************************************* */
/** create small Chordal Bayes Net x <- y
* x y d
* 1 1 9
* 1 5
*/
GaussianBayesNet createSmallGaussianBayesNet() {
using namespace impl;
Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
Matrix R22 = Matrix_(1, 1, 1.0);
Vector d1(1), d2(1);
d1(0) = 9;
d2(0) = 5;
Vector tau(1);
tau(0) = 1.0;
// define nodes and specify in reverse topological sort (i.e. parents last)
GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12, tau));
GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22, tau));
GaussianBayesNet cbn;
cbn.push_back(Px_y);
cbn.push_back(Py);
return cbn;
}
/* ************************************************************************* */
// Some nonlinear functions to optimize
/* ************************************************************************* */
namespace smallOptimize {
Point2 h(const Point2& v) {
return Point2(cos(v.x()), sin(v.y()));
}
Matrix H(const Point2& v) {
return Matrix_(2, 2,
-sin(v.x()), 0.0,
0.0, cos(v.y()));
}
struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> {
Point2 z_;
UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) :
gtsam::NoiseModelFactor1<Point2>(model, key), z_(z) {
}
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const {
if (A) *A = H(x);
return (h(x) - z_).vector();
}
};
}
/* ************************************************************************* */
boost::shared_ptr<const Graph> sharedReallyNonlinearFactorGraph() {
using symbol_shorthand::X;
using symbol_shorthand::L;
boost::shared_ptr<Graph> fg(new Graph);
Vector z = Vector_(2, 1.0, 0.0);
double sigma = 0.1;
boost::shared_ptr<smallOptimize::UnaryFactor> factor(
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
fg->push_back(factor);
return fg;
}
Graph createReallyNonlinearFactorGraph() {
return *sharedReallyNonlinearFactorGraph();
}
/* ************************************************************************* */
std::pair<Graph, Values> createNonlinearSmoother(int T) {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
// Create
Graph nlfg;
Values poses;
// prior on x1
Point2 x1(1.0, 0.0);
shared_nlf prior(new simulated2D::Prior(x1, sigma1_0, X(1)));
nlfg.push_back(prior);
poses.insert(X(1), x1);
for (int t = 2; t <= T; t++) {
// odometry between x_t and x_{t-1}
Point2 odo(1.0, 0.0);
shared_nlf odometry(new simulated2D::Odometry(odo, sigma1_0, X(t - 1), X(t)));
nlfg.push_back(odometry);
// measurement on x_t is like perfect GPS
Point2 xt(t, 0);
shared_nlf measurement(new simulated2D::Prior(xt, sigma1_0, X(t)));
nlfg.push_back(measurement);
// initial estimate
poses.insert(X(t), xt);
}
return std::make_pair(nlfg, poses);
}
/* ************************************************************************* */
std::pair<FactorGraph<GaussianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering) {
Graph nlfg;
Values poses;
boost::tie(nlfg, poses) = createNonlinearSmoother(T);
if(!ordering) ordering = *poses.orderingArbitrary();
return std::make_pair(*nlfg.linearize(poses, *ordering), *ordering);
}
/* ************************************************************************* */
GaussianFactorGraph createSimpleConstraintGraph() {
using namespace impl;
// create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1
Matrix Ax = eye(2);
Vector b1(2);
b1(0) = 1.0;
b1(1) = -1.0;
JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));
// create binary constraint factor
// between _x_ and _y_, that is going to be the only factor on _y_
// |1 0||x_1| + |-1 0||y_1| = |0|
// |0 1||x_2| | 0 -1||y_2| |0|
Matrix Ax1 = eye(2);
Matrix Ay1 = eye(2) * -1;
Vector b2 = Vector_(2, 0.0, 0.0);
JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
constraintModel));
// construct the graph
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
return fg;
}
/* ************************************************************************* */
VectorValues createSimpleConstraintValues() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
VectorValues config(std::vector<size_t>(2,2));
Vector v = Vector_(2, 1.0, -1.0);
config[_x_] = v;
config[_y_] = v;
return config;
}
/* ************************************************************************* */
GaussianFactorGraph createSingleConstraintGraph() {
using namespace impl;
// create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1
Matrix Ax = eye(2);
Vector b1(2);
b1(0) = 1.0;
b1(1) = -1.0;
//GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1));
JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));
// create binary constraint factor
// between _x_ and _y_, that is going to be the only factor on _y_
// |1 2||x_1| + |10 0||y_1| = |1|
// |2 1||x_2| |0 10||y_2| |2|
Matrix Ax1(2, 2);
Ax1(0, 0) = 1.0;
Ax1(0, 1) = 2.0;
Ax1(1, 0) = 2.0;
Ax1(1, 1) = 1.0;
Matrix Ay1 = eye(2) * 10;
Vector b2 = Vector_(2, 1.0, 2.0);
JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
constraintModel));
// construct the graph
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
return fg;
}
/* ************************************************************************* */
VectorValues createSingleConstraintValues() {
using namespace impl;
VectorValues config(std::vector<size_t>(2,2));
config[_x_] = Vector_(2, 1.0, -1.0);
config[_y_] = Vector_(2, 0.2, 0.1);
return config;
}
/* ************************************************************************* */
GaussianFactorGraph createMultiConstraintGraph() {
using namespace impl;
// unary factor 1
Matrix A = eye(2);
Vector b = Vector_(2, -2.0, 2.0);
JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1));
// constraint 1
Matrix A11(2, 2);
A11(0, 0) = 1.0;
A11(0, 1) = 2.0;
A11(1, 0) = 2.0;
A11(1, 1) = 1.0;
Matrix A12(2, 2);
A12(0, 0) = 10.0;
A12(0, 1) = 0.0;
A12(1, 0) = 0.0;
A12(1, 1) = 10.0;
Vector b1(2);
b1(0) = 1.0;
b1(1) = 2.0;
JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1,
constraintModel));
// constraint 2
Matrix A21(2, 2);
A21(0, 0) = 3.0;
A21(0, 1) = 4.0;
A21(1, 0) = -1.0;
A21(1, 1) = -2.0;
Matrix A22(2, 2);
A22(0, 0) = 1.0;
A22(0, 1) = 1.0;
A22(1, 0) = 1.0;
A22(1, 1) = 2.0;
Vector b2(2);
b2(0) = 3.0;
b2(1) = 4.0;
JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2,
constraintModel));
// construct the graph
GaussianFactorGraph fg;
fg.push_back(lf1);
fg.push_back(lc1);
fg.push_back(lc2);
return fg;
}
/* ************************************************************************* */
VectorValues createMultiConstraintValues() {
using namespace impl;
VectorValues config(std::vector<size_t>(3,2));
config[_x_] = Vector_(2, -2.0, 2.0);
config[_y_] = Vector_(2, -0.1, 0.4);
config[_z_] = Vector_(2, -4.0, 5.0);
return config;
}
/* ************************************************************************* */
// Create key for simulated planar graph
namespace impl {
Symbol key(int x, int y) {
using symbol_shorthand::X;
return X(1000*x+y);
}
} // \namespace impl
/* ************************************************************************* */
boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
using namespace impl;
// create empty graph
NonlinearFactorGraph nlfg;
// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
shared_nlf constraint(new simulated2D::Prior(Point2(1.0, 1.0), noiseModel::Isotropic::Sigma(2,1e-3), key(1,1)));
nlfg.push_back(constraint);
// Create horizontal constraints, 1...N*(N-1)
Point2 z1(1.0, 0.0); // move right
for (size_t x = 1; x < N; x++)
for (size_t y = 1; y <= N; y++) {
shared_nlf f(new simulated2D::Odometry(z1, noiseModel::Isotropic::Sigma(2,0.01), key(x, y), key(x + 1, y)));
nlfg.push_back(f);
}
// Create vertical constraints, N*(N-1)+1..2*N*(N-1)
Point2 z2(0.0, 1.0); // move up
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y < N; y++) {
shared_nlf f(new simulated2D::Odometry(z2, noiseModel::Isotropic::Sigma(2,0.01), key(x, y), key(x, y + 1)));
nlfg.push_back(f);
}
// Create linearization and ground xtrue config
Values zeros;
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++)
zeros.insert(key(x, y), Point2());
Ordering ordering(planarOrdering(N));
VectorValues xtrue(zeros.dims(ordering));
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++)
xtrue[ordering[key(x, y)]] = Point2(x,y).vector();
// linearize around zero
boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros, ordering);
return boost::make_tuple(*gfg, xtrue);
}
/* ************************************************************************* */
Ordering planarOrdering(size_t N) {
Ordering ordering;
for (size_t y = N; y >= 1; y--)
for (size_t x = N; x >= 1; x--)
ordering.push_back(impl::key(x, y));
return ordering;
}
/* ************************************************************************* */
std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N,
const GaussianFactorGraph& original) {
GaussianFactorGraph T, C;
// Add the x11 constraint to the tree
T.push_back(original[0]);
// Add all horizontal constraints to the tree
size_t i = 1;
for (size_t x = 1; x < N; x++)
for (size_t y = 1; y <= N; y++, i++)
T.push_back(original[i]);
// Add first vertical column of constraints to T, others to C
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y < N; y++, i++)
if (x == 1)
T.push_back(original[i]);
else
C.push_back(original[i]);
return std::make_pair(T, C);
}
/* ************************************************************************* */
} // \namespace example
} // \namespace gtsam

25
tests/testSerializationSLAM.cpp
View File

@ -133,6 +133,13 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
@ -155,7 +162,6 @@ BOOST_CLASS_EXPORT(gtsam::CalibratedCamera);
BOOST_CLASS_EXPORT(gtsam::SimpleCamera);
BOOST_CLASS_EXPORT(gtsam::StereoCamera);
/* Create GUIDs for factors */
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
@ -225,7 +231,7 @@ BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D");
/* ************************************************************************* */
TEST (Serialization, smallExample_linear) {
TEST (testSerializationSLAM, smallExample_linear) {
using namespace example;
Ordering ordering; ordering += X(1),X(2),L(1);
@ -245,7 +251,7 @@ TEST (Serialization, smallExample_linear) {
}
/* ************************************************************************* */
TEST (Serialization, gaussianISAM) {
TEST (testSerializationSLAM, gaussianISAM) {
using namespace example;
Ordering ordering;
GaussianFactorGraph smoother;
@ -265,7 +271,7 @@ BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry"
BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement")
/* ************************************************************************* */
TEST (Serialization, smallExample_nonlinear) {
TEST (testSerializationSLAM, smallExample_nonlinear) {
using namespace example;
NonlinearFactorGraph nfg = createNonlinearFactorGraph();
Values c1 = createValues();
@ -279,7 +285,7 @@ TEST (Serialization, smallExample_nonlinear) {
}
/* ************************************************************************* */
TEST (Serialization, factors) {
TEST (testSerializationSLAM, factors) {
LieVector lieVector(4, 1.0, 2.0, 3.0, 4.0);
LieMatrix lieMatrix(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0);
@ -331,7 +337,13 @@ TEST (Serialization, factors) {
SharedNoiseModel model9 = noiseModel::Isotropic::Sigma(9, 0.3);
SharedNoiseModel model11 = noiseModel::Isotropic::Sigma(11, 0.3);
SharedNoiseModel robust1 = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(10.0, noiseModel::mEstimator::Huber::Scalar),
noiseModel::Unit::Create(2));
EXPECT(equalsDereferenced(robust1));
EXPECT(equalsDereferencedXML(robust1));
EXPECT(equalsDereferencedBinary(robust1));
PriorFactorLieVector priorFactorLieVector(a01, lieVector, model4);
PriorFactorLieMatrix priorFactorLieMatrix(a02, lieMatrix, model6);
@ -660,11 +672,8 @@ TEST (Serialization, factors) {
EXPECT(equalsBinary<GeneralSFMFactor2Cal3_S2>(generalSFMFactor2Cal3_S2));
EXPECT(equalsBinary<GenericStereoFactor3D>(genericStereoFactor3D));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

27
tests/testSummarization.cpp
View File

@ -34,17 +34,16 @@ typedef gtsam::PriorFactor<gtsam::Pose2> PosePrior;
typedef gtsam::BetweenFactor<gtsam::Pose2> PoseBetween;
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2> PosePointBearingRange;
gtsam::noiseModel::Base::shared_ptr model2 = noiseModel::Unit::Create(2);
gtsam::noiseModel::Base::shared_ptr model3 = noiseModel::Unit::Create(3);
/* ************************************************************************* */
TEST( testSummarization, example_from_ddf1 ) {
Key xA0 = LabeledSymbol('x', 'A', 0),
xA1 = LabeledSymbol('x', 'A', 1),
xA2 = LabeledSymbol('x', 'A', 2);
Key lA3 = LabeledSymbol('l', 'A', 3), lA5 = LabeledSymbol('l', 'A', 5);
gtsam::noiseModel::Base::shared_ptr model2 = noiseModel::Unit::Create(2);
gtsam::noiseModel::Base::shared_ptr model3 = noiseModel::Unit::Create(3);
SharedDiagonal diagmodel2 = noiseModel::Unit::Create(2);
SharedDiagonal diagmodel4 = noiseModel::Unit::Create(4);
@ -218,6 +217,26 @@ TEST( testSummarization, example_from_ddf1 ) {
}
}
/* ************************************************************************* */
TEST( testSummarization, no_summarize_case ) {
// Checks a corner case in which no variables are being eliminated
gtsam::Key key = 7;
gtsam::KeySet saved_keys; saved_keys.insert(key);
NonlinearFactorGraph graph;
graph.add(PosePrior(key, Pose2(1.0, 2.0, 0.3), model3));
graph.add(PosePrior(key, Pose2(2.0, 3.0, 0.4), model3));
Values values;
values.insert(key, Pose2(0.0, 0.0, 0.1));
SummarizationMode mode = SEQUENTIAL_CHOLESKY;
GaussianFactorGraph actLinGraph; Ordering actOrdering;
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
Ordering expOrdering; expOrdering += key;
GaussianFactorGraph expLinGraph = *graph.linearize(values, expOrdering);
EXPECT(assert_equal(expOrdering, actOrdering));
EXPECT(assert_equal(expLinGraph, actLinGraph));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */