diff --git a/.cproject b/.cproject index dd5f6dc09..5eb64684d 100644 --- a/.cproject +++ b/.cproject @@ -1,5 +1,7 @@ - + + + @@ -1505,90 +1507,82 @@ true true - - make - -j2 - tests/testStereoCamera.run - true - true - true - - - make - -j2 - tests/testRot3M.run - true - true - true - - - make - -j2 - tests/testPoint3.run - true - true - true - - - make - -j2 - tests/testCalibratedCamera.run - true - true - true - - - make - -j2 - tests/timeRot3.run - true - true - true - - - make - -j2 - tests/timePose3.run - true - true - true - - - make - -j2 - tests/timeStereoCamera.run - true - true - true - - - make - -j1 VERBOSE=1 - geometry.testHomography2.run - true - false - true - - + make -j5 - geometry.testPoint2.run + testStereoCamera.run true true true - + make -j5 - geometry.testPose2.run + testRot3M.run true true true - + make -j5 - geometry.testPose3.run + testPoint3.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + timeRot3.run + true + true + true + + + make + -j5 + timePose3.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run true true true @@ -1753,6 +1747,38 @@ true true + + make + -j5 + timeMatrixOps + true + true + true + + + make + -j5 + timeTest + true + true + true + + + make + -j5 + timeVirtual + true + true + true + + + make + -j5 + timeVirtual2 + true + true + true + make -j5 diff --git a/gtsam.h b/gtsam.h index 12f7942fc..901ff4ca4 100644 --- a/gtsam.h +++ b/gtsam.h @@ -84,38 +84,44 @@ * - TODO: Handle gtsam::Rot3M conversions to quaternions */ -/*namespace std { +namespace std { + #include template - //Module.cpp needs to be changed to allow lowercase classnames class vector { + //Do we need these? //Capacity - size_t size() const; + /*size_t size() const; size_t max_size() const; - void resize(size_t sz, T c = T()); + //void resize(size_t sz); size_t capacity() const; bool empty() const; void reserve(size_t n); //Element acces - T& at(size_t n); - const T& at(size_t n) const; - T& front(); - const T& front() const; - T& back(); - const T& back() const; + T* at(size_t n); + T* front(); + T* back(); //Modifiers void assign(size_t n, const T& u); void push_back(const T& x); - void pop_back(); + void pop_back();*/ }; -}*/ + //typedef std::vector + + #include + template + class list + { + + + }; + +} namespace gtsam { -//typedef std::vector IndexVector; - //************************************************************************* // base //************************************************************************* @@ -255,6 +261,7 @@ virtual class Point2 : gtsam::Value { double y() const; Vector vector() const; double dist(const gtsam::Point2& p2) const; + double norm() const; }; virtual class StereoPoint2 : gtsam::Value { @@ -383,7 +390,7 @@ virtual class Rot3 : gtsam::Value { // Group static gtsam::Rot3 identity(); - gtsam::Rot3 inverse() const; + gtsam::Rot3 inverse() const; gtsam::Rot3 compose(const gtsam::Rot3& p2) const; gtsam::Rot3 between(const gtsam::Rot3& p2) const; @@ -777,7 +784,7 @@ virtual class BayesNet { // Standard interface size_t size() const; void printStats(string s) const; - void saveGraph(string s) const; + void saveGraph(string s) const; CONDITIONAL* front() const; CONDITIONAL* back() const; void push_back(CONDITIONAL* conditional); @@ -840,6 +847,7 @@ virtual class BayesTreeClique { #include typedef gtsam::BayesNet SymbolicBayesNetBase; + virtual class SymbolicBayesNet : gtsam::SymbolicBayesNetBase { // Standard Constructors and Named Constructors SymbolicBayesNet(); @@ -1350,13 +1358,13 @@ virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParamete void setReset(size_t value); void setEpsilon_rel(double value); void setEpsilon_abs(double value); - void print(string s); + void print(); }; #include virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { SubgraphSolverParameters(); - void print(string s) const; + void print() const; }; class SubgraphSolver { @@ -1936,6 +1944,8 @@ template virtual class GenericProjectionFactor : gtsam::NonlinearFactor { GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, size_t poseKey, size_t pointKey, const CALIBRATION* k); + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor); gtsam::Point2 measured() const; CALIBRATION* calibration() const; }; @@ -1999,6 +2009,7 @@ namespace utilities { void perturbPoint3(gtsam::Values& values, double sigma, int seed); void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); + void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); } //\namespace utilities diff --git a/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt index 011992810..a4c30d5a4 100644 --- a/gtsam/base/CMakeLists.txt +++ b/gtsam/base/CMakeLists.txt @@ -9,8 +9,11 @@ set(base_local_libs # Files to exclude from compilation of tests and timing scripts set(base_excluded_files -# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test - "" # Add to this list, with full path, to exclude +# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test + "${CMAKE_CURRENT_SOURCE_DIR}/tests/timeTest.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tests/timeVirtual.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tests/timeVirtual2.cpp" +# "" # Add to this list, with full path, to exclude ) # Build tests diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index eb52011a1..8946d231b 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -31,6 +31,9 @@ namespace gtsam { */ struct LieMatrix : public Matrix, public DerivedValue { + /// @name Constructors + /// @{ + /** default constructor - should be unnecessary */ LieMatrix() {} @@ -48,10 +51,9 @@ struct LieMatrix : public Matrix, public DerivedValue { /** Specify arguments directly, as in Matrix_() - always force these to be doubles */ LieMatrix(size_t m, size_t n, ...); - /** get the underlying vector */ - inline Matrix matrix() const { - return static_cast(*this); - } + /// @} + /// @name Testable interface + /// @{ /** print @param s optional string naming the object */ inline void print(const std::string& name="") const { @@ -62,8 +64,19 @@ struct LieMatrix : public Matrix, public DerivedValue { inline bool equals(const LieMatrix& expected, double tol=1e-5) const { return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); } + + /// @} + /// @name Standard Interface + /// @{ - // Manifold requirements + /** get the underlying vector */ + inline Matrix matrix() const { + return static_cast(*this); + } + + /// @} + /// @name Manifold interface + /// @{ /** Returns dimensionality of the tangent space */ inline size_t dim() const { return this->size(); } @@ -72,22 +85,26 @@ struct LieMatrix : public Matrix, public DerivedValue { * tangent space vector correspond to the matrix entries arranged in * *row-major* order. */ inline LieMatrix retract(const Vector& v) const { - if(v.size() != this->rows() * this->cols()) + if(v.size() != this->size()) throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size"); - return LieMatrix(*this + Eigen::Map(&v(0), this->cols(), this->rows()).transpose()); + return LieMatrix(*this + + Eigen::Map >( + &v(0), this->rows(), this->cols())); } /** @return the local coordinates of another object. The elements of the * tangent space vector correspond to the matrix entries arranged in * *row-major* order. */ inline Vector localCoordinates(const LieMatrix& t2) const { - Vector result(this->rows() * this->cols()); - Eigen::Map(&result(0), this->cols(), this->rows()).transpose() = - (t2 - *this); + Vector result(this->size()); + Eigen::Map >( + &result(0), this->rows(), this->cols()) = t2 - *this; return result; } - // Group requirements + /// @} + /// @name Group interface + /// @{ /** identity - NOTE: no known size at compile time - so zero length */ inline static LieMatrix identity() { @@ -125,7 +142,9 @@ struct LieMatrix : public Matrix, public DerivedValue { return LieMatrix(-(*this)); } - // Lie functions + /// @} + /// @name Lie group interface + /// @{ /** Expmap around identity */ static inline LieMatrix Expmap(const Vector& v) { @@ -135,6 +154,8 @@ struct LieMatrix : public Matrix, public DerivedValue { /** Logmap around identity - just returns with default cast back */ static inline Vector Logmap(const LieMatrix& p) { return Eigen::Map(&p(0,0), p.dim()); } + + /// @} private: diff --git a/gtsam/base/tests/timeMatrixOps.cpp b/gtsam/base/tests/timeMatrixOps.cpp index d749a4e39..e924c8402 100644 --- a/gtsam/base/tests/timeMatrixOps.cpp +++ b/gtsam/base/tests/timeMatrixOps.cpp @@ -38,8 +38,8 @@ using namespace boost::lambda; static boost::variate_generator > rng(boost::mt19937(), boost::uniform_real<>(-1.0, 0.0)); //typedef ublas::matrix matrix; //typedef ublas::matrix_range matrix_range; -typedef Eigen::Matrix matrix; -typedef Eigen::Block matrix_block; +//typedef Eigen::Matrix matrix; +//typedef Eigen::Block matrix_block; //using ublas::range; //using ublas::triangular_matrix; @@ -57,10 +57,10 @@ int main(int argc, char* argv[]) { assert(m > n); boost::variate_generator > rni(boost::mt19937(), boost::uniform_int(0,m-1)); boost::variate_generator > rnj(boost::mt19937(), boost::uniform_int(0,n-1)); - matrix mat(m,n); - matrix_block full = mat.block(0, 0, m, n); - matrix_block top = mat.block(0, 0, n, n); - matrix_block block = mat.block(m/4, n/4, m-m/2, n-n/2); + gtsam::Matrix mat((int)m,(int)n); + gtsam::SubMatrix full = mat.block(0, 0, m, n); + gtsam::SubMatrix top = mat.block(0, 0, n, n); + gtsam::SubMatrix block = mat.block(m/4, n/4, m-m/2, n-n/2); cout << format(" Basic: %1%x%2%\n") % m % n; cout << format(" Full: mat(%1%:%2%, %3%:%4%)\n") % 0 % m % 0 % n; diff --git a/gtsam/base/tests/timeTest.cpp b/gtsam/base/tests/timeTest.cpp index 807e95508..707f24b26 100644 --- a/gtsam/base/tests/timeTest.cpp +++ b/gtsam/base/tests/timeTest.cpp @@ -19,6 +19,7 @@ int main(int argc, char *argv[]) { + // FIXME: ticPush_ does not exist ticPush_("1", "top 1"); ticPush_("1", "sub 1"); gttic_("sub sub a"); diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 6d4cb192f..88e59507c 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -85,13 +85,16 @@ namespace gtsam { #ifdef _MSC_VER -// Define some common g++ functions and macros that MSVC does not have +// Define some common g++ functions and macros we use that MSVC does not have #include namespace std { - using boost::math::isfinite; - using boost::math::isnan; - using boost::math::isinf; + template inline int isfinite(T a) { + return (int)boost::math::isfinite(a); } + template inline int isnan(T a) { + return (int)boost::math::isnan(a); } + template inline int isinf(T a) { + return (int)boost::math::isinf(a); } } #include diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index a93f3e290..c2c225ee3 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -109,30 +109,33 @@ TEST(Pose2, expmap3) { } /* ************************************************************************* */ -TEST(Pose2, expmap0) { - Pose2 pose(M_PI/2.0, Point2(1, 2)); -//#ifdef SLOW_BUT_CORRECT_EXPMAP - Pose2 expected(1.01491, 2.01013, 1.5888); -//#else -// Pose2 expected(M_PI/2.0+0.018, Point2(1.015, 2.01)); -//#endif - Pose2 actual = pose * (Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018))); +TEST(Pose2, expmap0a) { + Pose2 expected(0.0101345, -0.0149092, 0.018); + Pose2 actual = Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)); EXPECT(assert_equal(expected, actual, 1e-5)); } /* ************************************************************************* */ -TEST(Pose2, expmap0_full) { - Pose2 pose(M_PI/2.0, Point2(1, 2)); - Pose2 expected(1.01491, 2.01013, 1.5888); - Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)); +TEST(Pose2, expmap0b) { + // a quarter turn + Pose2 expected(1.0, 1.0, M_PI/2); + Pose2 actual = Pose2::Expmap(Vector_(3, M_PI/2, 0.0, M_PI/2)); EXPECT(assert_equal(expected, actual, 1e-5)); } /* ************************************************************************* */ -TEST(Pose2, expmap0_full2) { - Pose2 pose(M_PI/2.0, Point2(1, 2)); - Pose2 expected(1.01491, 2.01013, 1.5888); - Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)); +TEST(Pose2, expmap0c) { + // a half turn + Pose2 expected(0.0, 2.0, M_PI); + Pose2 actual = Pose2::Expmap(Vector_(3, M_PI, 0.0, M_PI)); + EXPECT(assert_equal(expected, actual, 1e-5)); +} + +/* ************************************************************************* */ +TEST(Pose2, expmap0d) { + // a full turn + Pose2 expected(0, 0, 0); + Pose2 actual = Pose2::Expmap(Vector_(3, 2*M_PI, 0.0, 2*M_PI)); EXPECT(assert_equal(expected, actual, 1e-5)); } diff --git a/gtsam/linear/CMakeLists.txt b/gtsam/linear/CMakeLists.txt index 88417e268..3db38b953 100644 --- a/gtsam/linear/CMakeLists.txt +++ b/gtsam/linear/CMakeLists.txt @@ -13,8 +13,10 @@ set(linear_local_libs # Files to exclude from compilation of tests and timing scripts set(linear_excluded_files -# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test - "" # Add to this list, with full path, to exclude +# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test + "${CMAKE_CURRENT_SOURCE_DIR}/tests/timeSLAMlike.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tests/timeFactorOverhead.cpp" +# "" # Add to this list, with full path, to exclude ) # Build tests diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 1f01565ce..d1b3b2c7e 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -61,14 +61,14 @@ public: inline void setEpsilon_rel(double value) { epsilon_rel_ = value; } inline void setEpsilon_abs(double value) { epsilon_abs_ = value; } - virtual void print(const std::string &s="") const { + virtual void print() const { Base::print(); std::cout << "ConjugateGradientParameters" << std::endl - << "minIter: " << minIterations_ << std::endl - << "maxIter: " << maxIterations_ << std::endl - << "resetIter: " << reset_ << std::endl - << "eps_rel: " << epsilon_rel_ << std::endl - << "eps_abs: " << epsilon_abs_ << std::endl; + << "minIter: " << minIterations_ << std::endl + << "maxIter: " << maxIterations_ << std::endl + << "resetIter: " << reset_ << std::endl + << "eps_rel: " << epsilon_rel_ << std::endl + << "eps_abs: " << epsilon_abs_ << std::endl; } }; diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index fb948bf69..81c989840 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -48,11 +48,10 @@ namespace gtsam { void setKernel(const std::string &s) ; void setVerbosity(const std::string &s) ; - void print() const { + virtual void print() const { std::cout << "IterativeOptimizationParameters" << std::endl - << "kernel: " << kernelTranslator(kernel_) << std::endl - << "verbosity: " << verbosityTranslator(verbosity_) << std::endl - << std::endl; + << "kernel: " << kernelTranslator(kernel_) << std::endl + << "verbosity: " << verbosityTranslator(verbosity_) << std::endl; } static Kernel kernelTranslator(const std::string &s); diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 4df74aa16..889a82862 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -23,7 +23,7 @@ class SubgraphSolverParameters : public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; SubgraphSolverParameters() : Base() {} - virtual void print(const std::string &s="") const { Base::print(s); } + virtual void print() const { Base::print(); } }; /** diff --git a/gtsam/linear/tests/timeSLAMlike.cpp b/gtsam/linear/tests/timeSLAMlike.cpp index 3b66b6fd4..340e16a79 100644 --- a/gtsam/linear/tests/timeSLAMlike.cpp +++ b/gtsam/linear/tests/timeSLAMlike.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 3d51ef3ac..0479b06a5 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -18,9 +18,12 @@ */ #include +#include #include #include #include +#include +#include #include using namespace std; @@ -43,24 +46,135 @@ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& key } /* ************************************************************************* */ -void NonlinearFactorGraph::saveGraph(std::ostream &stm, const KeyFormatter& keyFormatter) const { +void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, + const GraphvizFormatting& graphvizFormatting, + const KeyFormatter& keyFormatter) const +{ stm << "graph {\n"; + stm << " size=\"" << graphvizFormatting.figureWidthInches << "," << + graphvizFormatting.figureHeightInches << "\";\n\n"; + + FastSet keys = this->keys(); + + // Local utility function to extract x and y coordinates + struct { boost::optional operator()( + const Value& value, const GraphvizFormatting& graphvizFormatting) + { + if(const Pose2* p = dynamic_cast(&value)) { + double x, y; + switch (graphvizFormatting.paperHorizontalAxis) { + case GraphvizFormatting::X: x = p->x(); break; + case GraphvizFormatting::Y: x = p->y(); break; + case GraphvizFormatting::Z: x = 0.0; break; + case GraphvizFormatting::NEGX: x = -p->x(); break; + case GraphvizFormatting::NEGY: x = -p->y(); break; + case GraphvizFormatting::NEGZ: x = 0.0; break; + } + switch (graphvizFormatting.paperVerticalAxis) { + case GraphvizFormatting::X: y = p->x(); break; + case GraphvizFormatting::Y: y = p->y(); break; + case GraphvizFormatting::Z: y = 0.0; break; + case GraphvizFormatting::NEGX: y = -p->x(); break; + case GraphvizFormatting::NEGY: y = -p->y(); break; + case GraphvizFormatting::NEGZ: y = 0.0; break; + } + return Point2(x,y); + } else if(const Pose3* p = dynamic_cast(&value)) { + double x, y; + switch (graphvizFormatting.paperHorizontalAxis) { + case GraphvizFormatting::X: x = p->x(); break; + case GraphvizFormatting::Y: x = p->y(); break; + case GraphvizFormatting::Z: x = p->z(); break; + case GraphvizFormatting::NEGX: x = -p->x(); break; + case GraphvizFormatting::NEGY: x = -p->y(); break; + case GraphvizFormatting::NEGZ: x = -p->z(); break; + } + switch (graphvizFormatting.paperVerticalAxis) { + case GraphvizFormatting::X: y = p->x(); break; + case GraphvizFormatting::Y: y = p->y(); break; + case GraphvizFormatting::Z: y = p->z(); break; + case GraphvizFormatting::NEGX: y = -p->x(); break; + case GraphvizFormatting::NEGY: y = -p->y(); break; + case GraphvizFormatting::NEGZ: y = -p->z(); break; + } + return Point2(x,y); + } else { + return boost::none; + } + }} getXY; + + // Find bounds + double minX = numeric_limits::infinity(), maxX = -numeric_limits::infinity(); + double minY = numeric_limits::infinity(), maxY = -numeric_limits::infinity(); + BOOST_FOREACH(Key key, keys) { + if(values.exists(key)) { + boost::optional xy = getXY(values.at(key), graphvizFormatting); + if(xy) { + if(xy->x() < minX) + minX = xy->x(); + if(xy->x() > maxX) + maxX = xy->x(); + if(xy->y() < minY) + minY = xy->y(); + if(xy->y() > maxY) + maxY = xy->y(); + } + } + } // Create nodes for each variable in the graph - BOOST_FOREACH(Key key, this->keys()) { + BOOST_FOREACH(Key key, keys) { // Label the node with the label from the KeyFormatter - stm << " var" << key << "[label=\"" << keyFormatter(key) << "\"];\n"; } + stm << " var" << key << "[label=\"" << keyFormatter(key) << "\""; + if(values.exists(key)) { + boost::optional xy = getXY(values.at(key), graphvizFormatting); + if(xy) + stm << ", pos=\"" << graphvizFormatting.scale*(xy->x() - minX) << "," << graphvizFormatting.scale*(xy->y() - minY) << "!\""; + } + stm << "];\n"; + } stm << "\n"; - // Create factors and variable connections - for(size_t i = 0; i < this->size(); ++i) { - // Make each factor a dot - stm << " factor" << i << "[label=\"\", shape=point];\n"; + if(graphvizFormatting.mergeSimilarFactors) { + // Remove duplicate factors + FastSet > structure; + BOOST_FOREACH(const sharedFactor& factor, *this) { + if(factor) { + vector factorKeys = factor->keys(); + std::sort(factorKeys.begin(), factorKeys.end()); + structure.insert(factorKeys); + } + } - // Make factor-variable connections - if(this->at(i)) { - BOOST_FOREACH(Key key, *this->at(i)) { - stm << " var" << key << "--" << "factor" << i << ";\n"; } } + // Create factors and variable connections + size_t i = 0; + BOOST_FOREACH(const vector& factorKeys, structure) { + // Make each factor a dot + stm << " factor" << i << "[label=\"\", shape=point"; + { + map::const_iterator pos = graphvizFormatting.factorPositions.find(i); + if(pos != graphvizFormatting.factorPositions.end()) + stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; + } + stm << "];\n"; + + // Make factor-variable connections + BOOST_FOREACH(Key key, factorKeys) { + stm << " var" << key << "--" << "factor" << i << ";\n"; } + + ++ i; + } + } else { + // Create factors and variable connections + for(size_t i = 0; i < this->size(); ++i) { + // Make each factor a dot + stm << " factor" << i << "[label=\"\", shape=point];\n"; + + // Make factor-variable connections + if(this->at(i)) { + BOOST_FOREACH(Key key, *this->at(i)) { + stm << " var" << key << "--" << "factor" << i << ";\n"; } } + } } stm << "}\n"; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 945189a41..d86831703 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -21,12 +21,35 @@ #pragma once +#include #include #include #include namespace gtsam { + /** + * Formatting options when saving in GraphViz format using + * NonlinearFactorGraph::saveGraph. + */ + struct GraphvizFormatting { + enum Axis { X, Y, Z, NEGX, NEGY, NEGZ }; ///< World axes to be assigned to paper axes + Axis paperHorizontalAxis; ///< The world axis assigned to the horizontal paper axis + Axis paperVerticalAxis; ///< The world axis assigned to the vertical paper axis + double figureWidthInches; ///< The figure width on paper in inches + double figureHeightInches; ///< The figure height on paper in inches + double scale; ///< Scale all positions to reduce / increase density + bool mergeSimilarFactors; ///< Merge multiple factors that have the same connectivity + std::map factorPositions; ///< (optional for each factor) Manually specify factor "dot" positions. + /// Default constructor sets up robot coordinates. Paper horizontal is robot Y, + /// paper vertical is robot X. Default figure size of 5x5 in. + GraphvizFormatting() : + paperHorizontalAxis(Y), paperVerticalAxis(X), + figureWidthInches(5), figureHeightInches(5), scale(1), + mergeSimilarFactors(false) {} + }; + + /** * A non-linear factor graph is a graph of non-Gaussian, i.e. non-linear factors, * which derive from NonlinearFactor. The values structures are typically (in SAM) more general @@ -47,7 +70,9 @@ namespace gtsam { void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Write the graph in GraphViz format for visualization */ - void saveGraph(std::ostream& stm, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void saveGraph(std::ostream& stm, const Values& values = Values(), + const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** return keys as an ordered set - ordering is by key value */ FastSet keys() const; diff --git a/matlab.h b/matlab.h index 17f96bcd6..c7017dc97 100644 --- a/matlab.h +++ b/matlab.h @@ -114,14 +114,14 @@ namespace gtsam { /// insert multiple projection factors for a single pose key void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, const Vector& J, const Matrix& Z, - const SharedNoiseModel& model, const shared_ptrK K) { + const SharedNoiseModel& model, const shared_ptrK K, const Pose3& body_P_sensor = Pose3()) { if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K"); if (Z.cols() != J.size()) throw std::invalid_argument( "addMeasurements: J and Z must have same number of entries"); for (int k = 0; k < Z.cols(); k++) { graph.push_back( boost::make_shared > - (Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K)); + (Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); } }