diff --git a/.cproject b/.cproject index e5b4e113a..46f9a60fb 100644 --- a/.cproject +++ b/.cproject @@ -1,7 +1,5 @@ - - - + @@ -453,6 +451,14 @@ true true + + make + -j5 + testVelocityConstraint3.run + true + true + true + make -j5 @@ -613,54 +619,6 @@ true true - - make - -j5 - testPlanarSLAM.run - true - true - true - - - make - -j5 - testPose2SLAM.run - true - true - true - - - make - -j5 - testPose3SLAM.run - true - true - true - - - make - -j5 - testSimulated2D.run - true - true - true - - - make - -j5 - testSimulated2DOriented.run - true - true - true - - - make - -j5 - testVisualSLAM.run - true - true - true - make -j5 @@ -669,14 +627,6 @@ true true - - make - -j5 - testSerializationSLAM.run - true - true - true - make -j5 @@ -685,6 +635,14 @@ true true + + make + -j5 + testSerialization.run + true + true + true + make -j5 @@ -917,6 +875,14 @@ true true + + make + -j5 + testBayesTreeOperations.run + true + true + true + make -j5 @@ -1149,6 +1115,14 @@ true true + + make + -j5 + testSerializationSLAM.run + true + true + true + make -j5 @@ -1613,6 +1587,14 @@ true true + + make + -j5 + testRot3Q.run + true + true + true + make -j2 diff --git a/CMakeLists.txt b/CMakeLists.txt index e4b6efb0e..bdbd25abf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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() diff --git a/gtsam.h b/gtsam.h index 0c393a433..45c701ee1 100644 --- a/gtsam.h +++ b/gtsam.h @@ -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 clone() const; - * Templates + * Class Templates * - Basic templates are supported either with an explicit list of types to instantiate, * e.g. template 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 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 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 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 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 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 RangeFactorPosePoint2; @@ -1999,6 +2008,7 @@ typedef gtsam::RangeFactor RangeFactor template 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 BearingFactor2D; @@ -2008,6 +2018,7 @@ typedef gtsam::BearingFactor BearingFa template 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 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 GenericProjectionFactorCal3_S2; typedef gtsam::GenericProjectionFactor 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 GenericStereoFactor3D; @@ -2066,6 +2079,7 @@ typedef gtsam::GenericStereoFactor GenericStereoFac template 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 PoseTranslationPrior2D; @@ -2075,6 +2089,7 @@ typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; template 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 PoseRotationPrior2D; @@ -2092,6 +2107,51 @@ pair load2D(string filename, pair load2D_robust(string filename, gtsam::noiseModel::Base* model); +//************************************************************************* +// Serialization +//************************************************************************* +#include + +// 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 //************************************************************************* diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 429920e1c..053a89da8 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -32,8 +32,6 @@ #include #include - - namespace gtsam { /* @@ -106,8 +104,8 @@ namespace gtsam { for (size_t j=0;j +#include +#include + +// includes for standard serialization types +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +// Serialization directly to strings in compressed format +template +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 3d5bf4d71..b6593bd9f 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -22,46 +22,14 @@ #include #include -// includes for standard serialization types -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include +#include // whether to print the serialized text to stdout const bool verbose = false; -namespace gtsam { namespace serializationTestHelpers { +namespace gtsam { +namespace serializationTestHelpers { - /* ************************************************************************* */ - // Serialization testing code. - /* ************************************************************************* */ - -template -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 -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 @@ -97,22 +65,6 @@ bool equalsDereferenced(const T& input) { return input->equals(*output); } -/* ************************************************************************* */ -template -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 -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 void roundtripXML(const T& input, T& output) { @@ -148,22 +100,6 @@ bool equalsDereferencedXML(const T& input = T()) { return input->equals(*output); } -/* ************************************************************************* */ -template -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 -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 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 + diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index 6816346e8..1d5ad695a 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -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) ; diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 444f5d759..27a962e70 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -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 diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 850f84b01..74ea16638 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -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 diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 95c7582c6..df31f0803 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -14,10 +14,11 @@ * @brief 3D Point */ -#include #include #include +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 H1, boost::optional 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 H1, boost::optional 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 diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 7e0c59ba9..cf30bd1ff 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -21,13 +21,14 @@ #pragma once -#include -#include - #include #include #include +#include + +#include + 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 diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index adaa2ce69..faec92a6b 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -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 diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 9d7444233..234e2cad6 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -290,6 +290,9 @@ namespace gtsam { */ static std::pair 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; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 591e5c8a0..45012770f 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -116,11 +116,11 @@ namespace gtsam { } /** Compose - make a new rotation by adding angles */ - inline Rot2 compose(const Rot2& R1, boost::optional H1 = + inline Rot2 compose(const Rot2& R, boost::optional H1 = boost::none, boost::optional 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 H1 = + inline Rot2 between(const Rot2& R, boost::optional H1 = boost::none, boost::optional 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_); } /// @} diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index e9c7c6b64..6adcd8a1b 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -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; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index b63499d6d..cb6660dae 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -380,6 +380,15 @@ pair 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 diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index afab3900c..72d50484b 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -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 diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 446255803..7c73b28a2 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -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); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 07152df36..b50e1b9d9 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -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); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index ed8f850d2..5f4a68d3b 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -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);} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index f59455e2f..b50ad1253 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -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 /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index e8733c24d..d44b8f34c 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -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 /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 9f672e56f..d5592ed2a 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -799,5 +799,6 @@ namespace gtsam { } } -} -/// namespace gtsam + /* ************************************************************************* */ + +} // \namespace gtsam diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 8f57531ae..9e487d0a2 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -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(); diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 70954152f..0d3b10e31 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -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 bn; - this->removeTop(newFactors.keys(), bn, orphans); + 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 diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp index 332a3688b..b986154df 100644 --- a/gtsam/inference/Key.cpp +++ b/gtsam/inference/Key.cpp @@ -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 diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index 87ec08393..21f75e8d4 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -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); } diff --git a/gtsam/inference/tests/testPermutation.cpp b/gtsam/inference/tests/testPermutation.cpp index 98e94b4f7..5aa83422f 100644 --- a/gtsam/inference/tests/testPermutation.cpp +++ b/gtsam/inference/tests/testPermutation.cpp @@ -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)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 9b063f0ce..a209c3d44 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -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_; diff --git a/gtsam/nonlinear/summarization.cpp b/gtsam/nonlinear/summarization.cpp index 5722f9c2c..acd222d43 100644 --- a/gtsam/nonlinear/summarization.cpp +++ b/gtsam/nonlinear/summarization.cpp @@ -18,18 +18,13 @@ std::pair 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 ordering_constraints; diff --git a/gtsam/nonlinear/summarization.h b/gtsam/nonlinear/summarization.h index faa2003ef..c25d5d934 100644 --- a/gtsam/nonlinear/summarization.h +++ b/gtsam/nonlinear/summarization.h @@ -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 diff --git a/gtsam/slam/serialization.cpp b/gtsam/slam/serialization.cpp new file mode 100644 index 000000000..304b10b6d --- /dev/null +++ b/gtsam/slam/serialization.cpp @@ -0,0 +1,313 @@ +/** + * @file serialization.cpp + * + * @date Jun 12, 2013 + * @author Alex Cunningham + */ + +#include +#include + +//#include +#include +#include +#include +//#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +#include +#include +#include +//#include + +using namespace gtsam; + +// Creating as many permutations of factors as possible +typedef PriorFactor PriorFactorLieVector; +typedef PriorFactor PriorFactorLieMatrix; +typedef PriorFactor PriorFactorPoint2; +typedef PriorFactor PriorFactorStereoPoint2; +typedef PriorFactor PriorFactorPoint3; +typedef PriorFactor PriorFactorRot2; +typedef PriorFactor PriorFactorRot3; +typedef PriorFactor PriorFactorPose2; +typedef PriorFactor PriorFactorPose3; +typedef PriorFactor PriorFactorCal3_S2; +typedef PriorFactor PriorFactorCal3DS2; +typedef PriorFactor PriorFactorCalibratedCamera; +typedef PriorFactor PriorFactorSimpleCamera; +typedef PriorFactor PriorFactorStereoCamera; + +typedef BetweenFactor BetweenFactorLieVector; +typedef BetweenFactor BetweenFactorLieMatrix; +typedef BetweenFactor BetweenFactorPoint2; +typedef BetweenFactor BetweenFactorPoint3; +typedef BetweenFactor BetweenFactorRot2; +typedef BetweenFactor BetweenFactorRot3; +typedef BetweenFactor BetweenFactorPose2; +typedef BetweenFactor BetweenFactorPose3; + +typedef NonlinearEquality NonlinearEqualityLieVector; +typedef NonlinearEquality NonlinearEqualityLieMatrix; +typedef NonlinearEquality NonlinearEqualityPoint2; +typedef NonlinearEquality NonlinearEqualityStereoPoint2; +typedef NonlinearEquality NonlinearEqualityPoint3; +typedef NonlinearEquality NonlinearEqualityRot2; +typedef NonlinearEquality NonlinearEqualityRot3; +typedef NonlinearEquality NonlinearEqualityPose2; +typedef NonlinearEquality NonlinearEqualityPose3; +typedef NonlinearEquality NonlinearEqualityCal3_S2; +typedef NonlinearEquality NonlinearEqualityCal3DS2; +typedef NonlinearEquality NonlinearEqualityCalibratedCamera; +typedef NonlinearEquality NonlinearEqualitySimpleCamera; +typedef NonlinearEquality NonlinearEqualityStereoCamera; + +typedef RangeFactor RangeFactorPosePoint2; +typedef RangeFactor RangeFactorPosePoint3; +typedef RangeFactor RangeFactorPose2; +typedef RangeFactor RangeFactorPose3; +typedef RangeFactor RangeFactorCalibratedCameraPoint; +typedef RangeFactor RangeFactorSimpleCameraPoint; +typedef RangeFactor RangeFactorCalibratedCamera; +typedef RangeFactor RangeFactorSimpleCamera; + +typedef BearingFactor BearingFactor2D; +typedef BearingFactor BearingFactor3D; + +typedef BearingRangeFactor BearingRangeFactor2D; +typedef BearingRangeFactor BearingRangeFactor3D; + +typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; +typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; + +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +//typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; + +typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; + +typedef gtsam::GenericStereoFactor 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(graph); +} + +/* ************************************************************************* */ +NonlinearFactorGraph::shared_ptr gtsam::deserializeGraph(const std::string& serialized_graph) { + NonlinearFactorGraph::shared_ptr result(new NonlinearFactorGraph()); + deserialize(serialized_graph, *result); + return result; +} + +/* ************************************************************************* */ +std::string gtsam::serializeGraphXML(const NonlinearFactorGraph& graph, const std::string& name) { + return serializeXML(graph, name); +} + +/* ************************************************************************* */ +NonlinearFactorGraph::shared_ptr gtsam::deserializeGraphXML(const std::string& serialized_graph, + const std::string& name) { + NonlinearFactorGraph::shared_ptr result(new NonlinearFactorGraph()); + deserializeXML(serialized_graph, *result, name); + return result; +} + +/* ************************************************************************* */ +std::string gtsam::serializeValues(const Values& values) { + return serialize(values); +} + +/* ************************************************************************* */ +Values::shared_ptr gtsam::deserializeValues(const std::string& serialized_values) { + Values::shared_ptr result(new Values()); + deserialize(serialized_values, *result); + return result; +} + +/* ************************************************************************* */ +std::string gtsam::serializeValuesXML(const Values& values, const std::string& name) { + return serializeXML(values, name); +} + +/* ************************************************************************* */ +Values::shared_ptr gtsam::deserializeValuesXML(const std::string& serialized_values, + const std::string& name) { + Values::shared_ptr result(new Values()); + deserializeXML(serialized_values, *result, name); + return result; +} + +/* ************************************************************************* */ +bool gtsam::serializeGraphToFile(const NonlinearFactorGraph& graph, const std::string& fname) { + return serializeToFile(graph, fname); +} + +/* ************************************************************************* */ +bool gtsam::serializeGraphToXMLFile(const NonlinearFactorGraph& graph, + const std::string& fname, const std::string& name) { + return serializeToXMLFile(graph, fname, name); +} + +/* ************************************************************************* */ +bool gtsam::serializeValuesToFile(const Values& values, const std::string& fname) { + return serializeToFile(values, fname); +} + +/* ************************************************************************* */ +bool gtsam::serializeValuesToXMLFile(const Values& values, + const std::string& fname, const std::string& name) { + return serializeToXMLFile(values, fname, name); +} + +/* ************************************************************************* */ +NonlinearFactorGraph::shared_ptr gtsam::deserializeGraphFromFile(const std::string& fname) { + NonlinearFactorGraph::shared_ptr result(new NonlinearFactorGraph()); + if (!deserializeFromFile(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(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(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(fname, *result, name)) + throw std::invalid_argument("Failed to open file" + fname); + return result; +} + +/* ************************************************************************* */ + + diff --git a/gtsam/slam/serialization.h b/gtsam/slam/serialization.h new file mode 100644 index 000000000..08451fa0c --- /dev/null +++ b/gtsam/slam/serialization.h @@ -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 + +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 + + diff --git a/gtsam/slam/tests/testSerialization.cpp b/gtsam/slam/tests/testSerialization.cpp new file mode 100644 index 000000000..616b5b6b7 --- /dev/null +++ b/gtsam/slam/tests/testSerialization.cpp @@ -0,0 +1,122 @@ +/** + * @file testSerialization.cpp + * + * @brief Tests for serialization global functions using boost.serialization + * + * @date Jun 12, 2013 + * @author Alex Cunningham + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +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(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); + graph.add(BetweenFactor(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); + graph.add(BearingRangeFactor(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); } +/* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 5a442560e..dd2671530 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -232,7 +232,7 @@ double PoseRTV::range(const PoseRTV& other, boost::optional H1, boost::optional 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()); } /* ************************************************************************* */ diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index 002d24ad4..65fc681c4 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -118,7 +118,7 @@ int main(int argc, char** argv) { newFactors.add(BetweenFactor(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 oldKeys; if(time >= lag+deltaT) { oldKeys.push_back(1000 * (time-lag-deltaT)); diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index bc22cb024..85b592b98 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -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& 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; } diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index b03f52890..78347a077 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -139,7 +139,7 @@ std::pair 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 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 diff --git a/gtsam_unstable/geometry/SimWall2D.h b/gtsam_unstable/geometry/SimWall2D.h index d3e86296a..cf2142af8 100644 --- a/gtsam_unstable/geometry/SimWall2D.h +++ b/gtsam_unstable/geometry/SimWall2D.h @@ -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; /** diff --git a/gtsam_unstable/linear/bayesTreeOperations.cpp b/gtsam_unstable/linear/bayesTreeOperations.cpp index e889b3cbb..c2e84dec3 100644 --- a/gtsam_unstable/linear/bayesTreeOperations.cpp +++ b/gtsam_unstable/linear/bayesTreeOperations.cpp @@ -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 diff --git a/gtsam_unstable/linear/bayesTreeOperations.h b/gtsam_unstable/linear/bayesTreeOperations.h index 471b38376..3f85dc12c 100644 --- a/gtsam_unstable/linear/bayesTreeOperations.h +++ b/gtsam_unstable/linear/bayesTreeOperations.h @@ -67,13 +67,35 @@ GTSAM_UNSTABLE_EXPORT std::deque 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 +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(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 +GaussianFactorGraph liquefy(const BAYESTREE& bayesTree, bool splitConditionals = false) { + return liquefy(bayesTree.root(), splitConditionals); +} } // \namespace gtsam diff --git a/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp b/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp index b1c833f2c..efc5bba55 100644 --- a/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp +++ b/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp @@ -10,6 +10,9 @@ #include #include +#include + +#include 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); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index cbd1096a9..002c9e3e6 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -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 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 AHRS::aid( @@ -169,10 +182,14 @@ std::pair 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_)); } diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index 284fe2fc6..1d7eb85f5 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -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 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 aid( const Mechanization_bRn2& mech, KalmanFilter::State state, const Vector& f, bool Farrell=0); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index ca95843af..312814d15 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -12,35 +12,43 @@ namespace gtsam { /* ************************************************************************* */ Mechanization_bRn2 Mechanization_bRn2::initializeVector(const std::list& U, - const std::list& F, const double g_e) { + const std::list& 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 { - // normalize b_g and attribute remainder to biases - b_g = g_e * meanF/meanF.norm(); - x_a = -(meanF - b_g); + 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(); } + // 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); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index b81e685d4..711a44bf9 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -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& U, - const std::list& F, const double g_e = 0); + const std::list& 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 diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 7d469d704..de7f97351 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -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 diff --git a/matlab/gtsam_tests/testGraphValuesSerialization.m b/matlab/gtsam_tests/testGraphValuesSerialization.m new file mode 100644 index 000000000..5583dde76 --- /dev/null +++ b/matlab/gtsam_tests/testGraphValuesSerialization.m @@ -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)); \ No newline at end of file diff --git a/matlab/gtsam_tests/testPlanarSLAMExample.m b/matlab/gtsam_tests/testPlanarSLAMExample.m index 2a736a710..7694a1a0b 100644 --- a/matlab/gtsam_tests/testPlanarSLAMExample.m +++ b/matlab/gtsam_tests/testPlanarSLAMExample.m @@ -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)); - - diff --git a/matlab/gtsam_tests/testPose2SLAMExample.m b/matlab/gtsam_tests/testPose2SLAMExample.m index 127bcd41c..8c70c27e7 100644 --- a/matlab/gtsam_tests/testPose2SLAMExample.m +++ b/matlab/gtsam_tests/testPose2SLAMExample.m @@ -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)); - diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 113e594fd..1f8125b81 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -30,5 +30,8 @@ testStereoVOExample display 'Starting: testVisualISAMExample' testVisualISAMExample +display 'Starting: testGraphValuesSerialization' +testVisualISAMExample + % end of tests display 'Tests complete!' diff --git a/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m b/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m new file mode 100644 index 000000000..d1d225545 --- /dev/null +++ b/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m @@ -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 + + + diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index b29b444ec..c9bda40dc 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -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) diff --git a/tests/simulated2D.cpp b/tests/simulated2D.cpp deleted file mode 100644 index 199a6756b..000000000 --- a/tests/simulated2D.cpp +++ /dev/null @@ -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 - -namespace simulated2D { - - static Matrix I = gtsam::eye(2); - - /* ************************************************************************* */ - Point2 prior(const Point2& x, boost::optional H) { - if (H) *H = I; - return x; - } - - /* ************************************************************************* */ - Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1, - boost::optional H2) { - if (H1) *H1 = -I; - if (H2) *H2 = I; - return x2 - x1; - } - - /* ************************************************************************* */ - Point2 mea(const Point2& x, const Point2& l, boost::optional H1, - boost::optional H2) { - if (H1) *H1 = -I; - if (H2) *H2 = I; - return l - x; - } - -/* ************************************************************************* */ - -} // namespace simulated2D - diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 3075d4a4a..f7a9a9284 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -90,7 +90,10 @@ namespace simulated2D { } /// Prior on a single pose, optionally returns derivative - Point2 prior(const Point2& x, boost::optional H = boost::none); + Point2 prior(const Point2& x, boost::optional 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 H1 = - boost::none, boost::optional H2 = boost::none); + boost::none, boost::optional 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 H1 = - boost::none, boost::optional H2 = boost::none); + boost::none, boost::optional 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 diff --git a/tests/simulated2DOriented.cpp b/tests/simulated2DOriented.cpp deleted file mode 100644 index c7d1d8469..000000000 --- a/tests/simulated2DOriented.cpp +++ /dev/null @@ -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 - -namespace simulated2DOriented { - - static Matrix I = gtsam::eye(3); - - /* ************************************************************************* */ - Pose2 prior(const Pose2& x, boost::optional H) { - if (H) *H = I; - return x; - } - - /* ************************************************************************* */ - Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional H1, - boost::optional H2) { - return x1.between(x2, H1, H2); - } - -/* ************************************************************************* */ - -} // namespace simulated2DOriented - diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index a18ace2e3..b05fb45c1 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -62,7 +62,10 @@ namespace simulated2DOriented { } /// Prior on a single pose, optional derivative version - Pose2 prior(const Pose2& x, boost::optional H = boost::none); + Pose2 prior(const Pose2& x, boost::optional 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 H1 = - boost::none, boost::optional H2 = boost::none); + boost::none, boost::optional H2 = boost::none) { + return x1.between(x2, H1, H2); + } /// Unary factor encoding a soft prior on a vector template diff --git a/tests/simulated3D.cpp b/tests/simulated3D.cpp deleted file mode 100644 index c7ba2a9b0..000000000 --- a/tests/simulated3D.cpp +++ /dev/null @@ -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 - -namespace gtsam { - -namespace simulated3D { - -Point3 prior (const Point3& x, boost::optional H) { - if (H) *H = eye(3); - return x; -} - -Point3 odo(const Point3& x1, const Point3& x2, - boost::optional H1, boost::optional 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 H1, boost::optional H2) { - if (H1) *H1 = -1 * eye(3); - if (H2) *H2 = eye(3); - return l - x; -} - -}} // namespace gtsam::simulated3D diff --git a/tests/simulated3D.h b/tests/simulated3D.h index 8aa0dc469..46945558a 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -38,21 +38,32 @@ namespace simulated3D { /** * Prior on a single pose */ -Point3 prior(const Point3& x, boost::optional H = boost::none); +Point3 prior(const Point3& x, boost::optional H = boost::none) { + if (H) *H = eye(3); + return x; +} /** * odometry between two poses */ Point3 odo(const Point3& x1, const Point3& x2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none); + boost::optional 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 H1 = boost::none, - boost::optional H2 = boost::none); + boost::optional 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 diff --git a/tests/smallExample.cpp b/tests/smallExample.cpp deleted file mode 100644 index 6074109a8..000000000 --- a/tests/smallExample.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include - -#include - -#include -#include - -#include -#include - -using namespace std; - -namespace gtsam { -namespace example { - - using namespace gtsam::noiseModel; - - typedef boost::shared_ptr 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 sharedNonlinearFactorGraph() { - // Create - boost::shared_ptr 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(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 sharedNoisyValues() { - boost::shared_ptr 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(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(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(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(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(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(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 z_; - - UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) : - gtsam::NoiseModelFactor1(model, key), z_(z) { - } - - Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { - if (A) *A = H(x); - return (h(x) - z_).vector(); - } - - }; - - } - - /* ************************************************************************* */ - boost::shared_ptr sharedReallyNonlinearFactorGraph() { - boost::shared_ptr fg(new Graph); - Vector z = Vector_(2, 1.0, 0.0); - double sigma = 0.1; - boost::shared_ptr factor( - new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); - fg->push_back(factor); - return fg; - } - - Graph createReallyNonlinearFactorGraph() { - return *sharedReallyNonlinearFactorGraph(); - } - - /* ************************************************************************* */ - pair 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, Ordering> createSmoother(int T, boost::optional 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(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(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(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 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 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 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 diff --git a/tests/smallExample.h b/tests/smallExample.h index 7652d96aa..ea9f30b54 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -22,132 +22,632 @@ #pragma once #include +#include #include #include namespace gtsam { - namespace example { +namespace example { - typedef NonlinearFactorGraph Graph; +typedef NonlinearFactorGraph Graph; - /** - * Create small example for non-linear factor graph - */ - boost::shared_ptr sharedNonlinearFactorGraph(); - Graph createNonlinearFactorGraph(); +/** + * Create small example for non-linear factor graph + */ +boost::shared_ptr sharedNonlinearFactorGraph(); +Graph createNonlinearFactorGraph(); - /** - * Create values structure to go with it - * The ground truth values structure for the example above - */ - Values createValues(); +/** + * Create values structure to go with it + * The ground truth values structure for the example above + */ +Values createValues(); - /** Vector Values equivalent */ - VectorValues createVectorValues(); +/** Vector Values equivalent */ +VectorValues createVectorValues(); - /** - * create a noisy values structure for a nonlinear factor graph - */ - boost::shared_ptr sharedNoisyValues(); - Values createNoisyValues(); +/** + * create a noisy values structure for a nonlinear factor graph + */ +boost::shared_ptr sharedNoisyValues(); +Values createNoisyValues(); - /** - * Zero delta config - */ - VectorValues createZeroDelta(const Ordering& ordering); +/** + * Zero delta config + */ +VectorValues createZeroDelta(const Ordering& ordering); - /** - * Delta config that, when added to noisyValues, returns the ground truth - */ - VectorValues createCorrectDelta(const Ordering& ordering); +/** + * Delta config that, when added to noisyValues, returns the ground truth + */ +VectorValues createCorrectDelta(const Ordering& ordering); - /** - * create a linear factor graph - * The non-linear graph above evaluated at NoisyValues - */ - GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering); +/** + * create a linear factor graph + * The non-linear graph above evaluated at NoisyValues + */ +GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering); - /** - * create small Chordal Bayes Net x <- y - */ - GaussianBayesNet createSmallGaussianBayesNet(); +/** + * create small Chordal Bayes Net x <- y + */ +GaussianBayesNet createSmallGaussianBayesNet(); - /** - * Create really non-linear factor graph (cos/sin) - */ - boost::shared_ptr - sharedReallyNonlinearFactorGraph(); - Graph createReallyNonlinearFactorGraph(); +/** + * Create really non-linear factor graph (cos/sin) + */ +boost::shared_ptr +sharedReallyNonlinearFactorGraph(); +Graph createReallyNonlinearFactorGraph(); - /** - * Create a full nonlinear smoother - * @param T number of time-steps - */ - std::pair createNonlinearSmoother(int T); +/** + * Create a full nonlinear smoother + * @param T number of time-steps + */ +std::pair createNonlinearSmoother(int T); - /** - * Create a Kalman smoother by linearizing a non-linear factor graph - * @param T number of time-steps - */ - std::pair, Ordering> createSmoother(int T, boost::optional ordering = boost::none); +/** + * Create a Kalman smoother by linearizing a non-linear factor graph + * @param T number of time-steps + */ +std::pair, Ordering> createSmoother(int T, boost::optional ordering = boost::none); - /* ******************************************************* */ - // Linear Constrained Examples - /* ******************************************************* */ +/* ******************************************************* */ +// Linear Constrained Examples +/* ******************************************************* */ - /** - * Creates a simple constrained graph with one linear factor and - * one binary equality constraint that sets x = y - */ - GaussianFactorGraph createSimpleConstraintGraph(); - VectorValues createSimpleConstraintValues(); +/** + * Creates a simple constrained graph with one linear factor and + * one binary equality constraint that sets x = y + */ +GaussianFactorGraph createSimpleConstraintGraph(); +VectorValues createSimpleConstraintValues(); - /** - * Creates a simple constrained graph with one linear factor and - * one binary constraint. - */ - GaussianFactorGraph createSingleConstraintGraph(); - VectorValues createSingleConstraintValues(); +/** + * Creates a simple constrained graph with one linear factor and + * one binary constraint. + */ +GaussianFactorGraph createSingleConstraintGraph(); +VectorValues createSingleConstraintValues(); - /** - * Creates a constrained graph with a linear factor and two - * binary constraints that share a node - */ - GaussianFactorGraph createMultiConstraintGraph(); - VectorValues createMultiConstraintValues(); +/** + * Creates a constrained graph with a linear factor and two + * binary constraints that share a node + */ +GaussianFactorGraph createMultiConstraintGraph(); +VectorValues createMultiConstraintValues(); - /* ******************************************************* */ - // Planar graph with easy subtree for SubgraphPreconditioner - /* ******************************************************* */ +/* ******************************************************* */ +// Planar graph with easy subtree for SubgraphPreconditioner +/* ******************************************************* */ - /* - * Create factor graph with N^2 nodes, for example for N=3 - * x13-x23-x33 - * | | | - * x12-x22-x32 - * | | | - * -x11-x21-x31 - * with x11 clamped at (1,1), and others related by 2D odometry. - */ - boost::tuple planarGraph(size_t N); +/* + * Create factor graph with N^2 nodes, for example for N=3 + * x13-x23-x33 + * | | | + * x12-x22-x32 + * | | | + * -x11-x21-x31 + * with x11 clamped at (1,1), and others related by 2D odometry. + */ +boost::tuple planarGraph(size_t N); - /* - * Create canonical ordering for planar graph that also works for tree - * With x11 the root, e.g. for N=3 - * x33 x23 x13 x32 x22 x12 x31 x21 x11 - */ - Ordering planarOrdering(size_t N); +/* + * Create canonical ordering for planar graph that also works for tree + * With x11 the root, e.g. for N=3 + * x33 x23 x13 x32 x22 x12 x31 x21 x11 + */ +Ordering planarOrdering(size_t N); - /* - * Split graph into tree and loop closing constraints, e.g., with N=3 - * x13-x23-x33 - * | - * x12-x22-x32 - * | - * -x11-x21-x31 - */ - std::pair splitOffPlanarTree( - size_t N, const GaussianFactorGraph& original); +/* + * Split graph into tree and loop closing constraints, e.g., with N=3 + * x13-x23-x33 + * | + * x12-x22-x32 + * | + * -x11-x21-x31 + */ +std::pair splitOffPlanarTree( + size_t N, const GaussianFactorGraph& original); - } // example +} // example } // gtsam + + +// Implementations +namespace gtsam { +namespace example { + +// using namespace gtsam::noiseModel; + +namespace impl { +typedef boost::shared_ptr 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 sharedNonlinearFactorGraph() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + // Create + boost::shared_ptr 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(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 sharedNoisyValues() { + using symbol_shorthand::X; + using symbol_shorthand::L; + boost::shared_ptr 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(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(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(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(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(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(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 z_; + + UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) : + gtsam::NoiseModelFactor1(model, key), z_(z) { + } + + Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { + if (A) *A = H(x); + return (h(x) - z_).vector(); + } + +}; + +} + +/* ************************************************************************* */ +boost::shared_ptr sharedReallyNonlinearFactorGraph() { + using symbol_shorthand::X; + using symbol_shorthand::L; + boost::shared_ptr fg(new Graph); + Vector z = Vector_(2, 1.0, 0.0); + double sigma = 0.1; + boost::shared_ptr factor( + new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); + fg->push_back(factor); + return fg; +} + +Graph createReallyNonlinearFactorGraph() { + return *sharedReallyNonlinearFactorGraph(); +} + +/* ************************************************************************* */ +std::pair 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, Ordering> createSmoother(int T, boost::optional 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(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(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(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 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 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 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 diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index f9229eac6..57a1e5cb3 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -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)); EXPECT(equalsBinary(genericStereoFactor3D)); - } - - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testSummarization.cpp b/tests/testSummarization.cpp index 2f3cfdf03..bf7342202 100644 --- a/tests/testSummarization.cpp +++ b/tests/testSummarization.cpp @@ -34,17 +34,16 @@ typedef gtsam::PriorFactor PosePrior; typedef gtsam::BetweenFactor PoseBetween; typedef gtsam::BearingRangeFactor 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); } /* ************************************************************************* */