diff --git a/CMakeLists.txt b/CMakeLists.txt index a79e812ef..c637796cf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ endif() set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 2) set (GTSAM_VERSION_PATCH 0) -set (GTSAM_PRERELEASE_VERSION "a4") +set (GTSAM_PRERELEASE_VERSION "a5") math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") if (${GTSAM_VERSION_PATCH} EQUAL 0) diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index 3768ee2a3..3a02e6cab 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -28,7 +28,8 @@ // Header order is close to far #include #include -#include // for loading BAL datasets ! +#include // for loading BAL datasets ! +#include #include using namespace std; @@ -46,10 +47,9 @@ int main(int argc, char* argv[]) { if (argc > 1) filename = string(argv[1]); // Load the SfM data from file - SfmData mydata; - readBAL(filename, mydata); + SfmData mydata = SfmData::FromBalFile(filename); cout << boost::format("read %1% tracks on %2% cameras\n") % - mydata.number_tracks() % mydata.number_cameras(); + mydata.numberTracks() % mydata.numberCameras(); // Create a factor graph ExpressionFactorGraph graph; diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index ffb5b195b..6944177c1 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SFMExample.cpp + * @file SFMExample_bal.cpp * @brief Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file * @author Frank Dellaert */ @@ -20,7 +20,8 @@ #include #include #include -#include // for loading BAL datasets ! +#include // for loading BAL datasets ! +#include #include using namespace std; @@ -41,9 +42,8 @@ int main (int argc, char* argv[]) { if (argc>1) filename = string(argv[1]); // Load the SfM data from file - SfmData mydata; - readBAL(filename, mydata); - cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); + SfmData mydata = SfmData::FromBalFile(filename); + cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras(); // Create a factor graph NonlinearFactorGraph graph; diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index b79a9fa28..4d04dd16e 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -22,7 +22,8 @@ #include #include #include -#include // for loading BAL datasets ! +#include // for loading BAL datasets ! +#include #include @@ -45,10 +46,9 @@ int main(int argc, char* argv[]) { if (argc > 1) filename = string(argv[1]); // Load the SfM data from file - SfmData mydata; - readBAL(filename, mydata); + SfmData mydata = SfmData::FromBalFile(filename); cout << boost::format("read %1% tracks on %2% cameras\n") % - mydata.number_tracks() % mydata.number_cameras(); + mydata.numberTracks() % mydata.numberCameras(); // Create a factor graph NonlinearFactorGraph graph; @@ -131,7 +131,7 @@ int main(int argc, char* argv[]) { cout << "Time comparison by solving " << filename << " results:" << endl; cout << boost::format("%1% point tracks and %2% cameras\n") % - mydata.number_tracks() % mydata.number_cameras() + mydata.numberTracks() % mydata.numberCameras() << endl; tictoc_print_(); diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 8c23ae9e5..a5ee77495 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -18,6 +18,9 @@ #pragma once +#if BOOST_VERSION >= 107400 +#include +#endif #include #include #include diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index 24355c684..e615afe83 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -25,6 +25,7 @@ #include // includes for standard serialization types +#include #include #include #include diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 06b2856f8..0d6c5e976 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -225,13 +225,13 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood( /* ****************************************************************************/ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood( - size_t parent_value) const { + size_t frontal) const { if (nrFrontals() != 1) throw std::invalid_argument( "Single value likelihood can only be invoked on single-variable " "conditional"); DiscreteValues values; - values.emplace(keys_[0], parent_value); + values.emplace(keys_[0], frontal); return likelihood(values); } diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 48d94a383..cff1b69a6 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -177,7 +177,7 @@ class GTSAM_EXPORT DiscreteConditional const DiscreteValues& frontalValues) const; /** Single variable version of likelihood. */ - DecisionTreeFactor::shared_ptr likelihood(size_t parent_value) const; + DecisionTreeFactor::shared_ptr likelihood(size_t frontal) const; /** * sample diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 56e7248a3..b3a12a8d5 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -70,7 +70,7 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor { string dot( const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, bool showZero = true) const; - std::vector> enumerate() const; + std::vector> enumerate() const; string markdown(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; string markdown(const gtsam::KeyFormatter& keyFormatter, @@ -97,7 +97,7 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor { const gtsam::Ordering& orderedKeys); gtsam::DiscreteConditional operator*( const gtsam::DiscreteConditional& other) const; - DiscreteConditional marginal(gtsam::Key key) const; + gtsam::DiscreteConditional marginal(gtsam::Key key) const; void print(string s = "Discrete Conditional\n", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; @@ -269,13 +269,16 @@ class DiscreteFactorGraph { gtsam::DiscreteLookupDAG maxProduct(gtsam::Ordering::OrderingType type); gtsam::DiscreteLookupDAG maxProduct(const gtsam::Ordering& ordering); - gtsam::DiscreteBayesNet eliminateSequential(); - gtsam::DiscreteBayesNet eliminateSequential(const gtsam::Ordering& ordering); - std::pair + gtsam::DiscreteBayesNet* eliminateSequential(); + gtsam::DiscreteBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type); + gtsam::DiscreteBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + pair eliminatePartialSequential(const gtsam::Ordering& ordering); - gtsam::DiscreteBayesTree eliminateMultifrontal(); - gtsam::DiscreteBayesTree eliminateMultifrontal(const gtsam::Ordering& ordering); - std::pair + + gtsam::DiscreteBayesTree* eliminateMultifrontal(); + gtsam::DiscreteBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type); + gtsam::DiscreteBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); + pair eliminatePartialMultifrontal(const gtsam::Ordering& ordering); string dot( diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index cfc9c1bb5..19af676f7 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp @@ -150,7 +150,6 @@ TEST(DiscreteBayesNet, Dot) { fragment.add((Either | Tuberculosis, LungCancer) = "F T T T"); string actual = fragment.dot(); - cout << actual << endl; EXPECT(actual == "digraph {\n" " size=\"5,5\";\n" diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index 26356be3d..6635633a2 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -243,27 +243,27 @@ TEST(DiscreteBayesTree, Dot) { string actual = self.bayesTree->dot(); EXPECT(actual == "digraph G{\n" - "0[label=\"13,11,6,7\"];\n" + "0[label=\"13, 11, 6, 7\"];\n" "0->1\n" - "1[label=\"14 : 11,13\"];\n" + "1[label=\"14 : 11, 13\"];\n" "1->2\n" - "2[label=\"9,12 : 14\"];\n" + "2[label=\"9, 12 : 14\"];\n" "2->3\n" - "3[label=\"3 : 9,12\"];\n" + "3[label=\"3 : 9, 12\"];\n" "2->4\n" - "4[label=\"2 : 9,12\"];\n" + "4[label=\"2 : 9, 12\"];\n" "2->5\n" - "5[label=\"8 : 12,14\"];\n" + "5[label=\"8 : 12, 14\"];\n" "5->6\n" - "6[label=\"1 : 8,12\"];\n" + "6[label=\"1 : 8, 12\"];\n" "5->7\n" - "7[label=\"0 : 8,12\"];\n" + "7[label=\"0 : 8, 12\"];\n" "1->8\n" - "8[label=\"10 : 13,14\"];\n" + "8[label=\"10 : 13, 14\"];\n" "8->9\n" - "9[label=\"5 : 10,13\"];\n" + "9[label=\"5 : 10, 13\"];\n" "8->10\n" - "10[label=\"4 : 10,13\"];\n" + "10[label=\"4 : 10, 13\"];\n" "}"); } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index b240603fc..82b5ec91d 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -41,6 +41,9 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { public: enum { dimension = 3 }; + ///< shared pointer to stereo calibration object + using shared_ptr = boost::shared_ptr; + /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index f756cba5e..affce0819 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -37,6 +37,9 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { public: enum { dimension = 9 }; + ///< shared pointer to stereo calibration object + using shared_ptr = boost::shared_ptr; + /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index a9b09cf46..b583234fd 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -47,6 +47,9 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 { public: enum { dimension = 9 }; + ///< shared pointer to stereo calibration object + using shared_ptr = boost::shared_ptr; + /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index f07ca0a54..e93d313c8 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -52,6 +52,9 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { public: enum { dimension = 10 }; + ///< shared pointer to stereo calibration object + using shared_ptr = boost::shared_ptr; + /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index abd74e063..18bd88b52 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -49,16 +49,14 @@ namespace gtsam { - /** - * @brief A 3D rotation represented as a rotation matrix if the preprocessor - * symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it - * is defined. - * @addtogroup geometry - * \nosubgrouping - */ - class GTSAM_EXPORT Rot3 : public LieGroup { - - private: +/** + * @brief Rot3 is a 3D rotation represented as a rotation matrix if the + * preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion + * if it is defined. + * @addtogroup geometry + */ +class GTSAM_EXPORT Rot3 : public LieGroup { + private: #ifdef GTSAM_USE_QUATERNIONS /** Internal Eigen Quaternion */ @@ -67,8 +65,7 @@ namespace gtsam { SO3 rot_; #endif - public: - + public: /// @name Constructors and named constructors /// @{ @@ -83,7 +80,7 @@ namespace gtsam { */ Rot3(const Point3& col1, const Point3& col2, const Point3& col3); - /** constructor from a rotation matrix, as doubles in *row-major* order !!! */ + /// Construct from a rotation matrix, as doubles in *row-major* order !!! Rot3(double R11, double R12, double R13, double R21, double R22, double R23, double R31, double R32, double R33); @@ -567,6 +564,9 @@ namespace gtsam { #endif }; + /// std::vector of Rot3s, mainly for wrapper + using Rot3Vector = std::vector >; + /** * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' @@ -585,5 +585,6 @@ namespace gtsam { template<> struct traits : public internal::LieGroup {}; -} + +} // namespace gtsam diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 9b937fefb..b341c1d5a 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -95,7 +95,7 @@ namespace gtsam { /* ************************************************************************* */ template void BayesTree::dot(std::ostream& s, sharedClique clique, - const KeyFormatter& indexFormatter, + const KeyFormatter& keyFormatter, int parentnum) const { static int num = 0; bool first = true; @@ -104,10 +104,10 @@ namespace gtsam { std::string parent = out.str(); parent += "[label=\""; - for (Key index : clique->conditional_->frontals()) { - if (!first) parent += ","; + for (Key key : clique->conditional_->frontals()) { + if (!first) parent += ", "; first = false; - parent += indexFormatter(index); + parent += keyFormatter(key); } if (clique->parent()) { @@ -116,10 +116,10 @@ namespace gtsam { } first = true; - for (Key sep : clique->conditional_->parents()) { - if (!first) parent += ","; + for (Key parentKey : clique->conditional_->parents()) { + if (!first) parent += ", "; first = false; - parent += indexFormatter(sep); + parent += keyFormatter(parentKey); } parent += "\"];\n"; s << parent; @@ -127,7 +127,7 @@ namespace gtsam { for (sharedClique c : clique->children) { num++; - dot(s, c, indexFormatter, parentnum); + dot(s, c, keyFormatter, parentnum); } } diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index 5a661d5cf..e7b074ec4 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -4,6 +4,12 @@ namespace gtsam { +// Headers for overloaded methods below, break hierarchy :-/ +#include +#include +#include +#include + #include // Default keyformatter @@ -98,10 +104,41 @@ class Ordering { Ordering(); Ordering(const gtsam::Ordering& other); - template + template < + FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); + template < + FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + static gtsam::Ordering ColamdConstrainedLast( + const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast, + bool forceOrder = false); + + template < + FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + static gtsam::Ordering ColamdConstrainedFirst( + const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst, + bool forceOrder = false); + + template < + FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + static gtsam::Ordering Natural(const FACTOR_GRAPH& graph); + + template < + FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + static gtsam::Ordering Metis(const FACTOR_GRAPH& graph); + + template < + FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType, + const FACTOR_GRAPH& graph); + // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; @@ -135,12 +172,6 @@ class DotWriter { }; #include - -// Headers for overloaded methods below, break hierarchy :-/ -#include -#include -#include - class VariableIndex { // Standard Constructors and Named Constructors VariableIndex(); diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 8fd4f2c26..6dcf662a9 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -26,6 +26,9 @@ using namespace std; using namespace gtsam; +// In Wrappers we have no access to this so have a default ready +static std::mt19937_64 kRandomNumberGenerator(42); + namespace gtsam { // Instantiate base class @@ -37,28 +40,50 @@ namespace gtsam { return Base::equals(bn, tol); } - /* ************************************************************************* */ - VectorValues GaussianBayesNet::optimize() const - { - VectorValues soln; // no missing variables -> just create an empty vector - return optimize(soln); + /* ************************************************************************ */ + VectorValues GaussianBayesNet::optimize() const { + VectorValues solution; // no missing variables -> create an empty vector + return optimize(solution); } - /* ************************************************************************* */ - VectorValues GaussianBayesNet::optimize( - const VectorValues& solutionForMissing) const { - VectorValues soln(solutionForMissing); // possibly empty + VectorValues GaussianBayesNet::optimize(VectorValues solution) const { // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) - /** solve each node in turn in topological sort order (parents first)*/ - for (auto cg: boost::adaptors::reverse(*this)) { + // solve each node in reverse topological sort order (parents first) + for (auto cg : boost::adaptors::reverse(*this)) { // i^th part of R*x=y, x=inv(R)*y - // (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:)) - soln.insert(cg->solve(soln)); + // (Rii*xi + R_i*x(i+1:))./si = yi => + // xi = inv(Rii)*(yi.*si - R_i*x(i+1:)) + solution.insert(cg->solve(solution)); } - return soln; + return solution; } - /* ************************************************************************* */ + /* ************************************************************************ */ + VectorValues GaussianBayesNet::sample(std::mt19937_64* rng) const { + VectorValues result; // no missing variables -> create an empty vector + return sample(result, rng); + } + + VectorValues GaussianBayesNet::sample(VectorValues result, + std::mt19937_64* rng) const { + // sample each node in reverse topological sort order (parents first) + for (auto cg : boost::adaptors::reverse(*this)) { + const VectorValues sampled = cg->sample(result, rng); + result.insert(sampled); + } + return result; + } + + /* ************************************************************************ */ + VectorValues GaussianBayesNet::sample() const { + return sample(&kRandomNumberGenerator); + } + + VectorValues GaussianBayesNet::sample(VectorValues given) const { + return sample(given, &kRandomNumberGenerator); + } + + /* ************************************************************************ */ VectorValues GaussianBayesNet::optimizeGradientSearch() const { gttic(GaussianBayesTree_optimizeGradientSearch); diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 6d906d65e..940ffd882 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -88,11 +88,35 @@ namespace gtsam { /// @name Standard Interface /// @{ - /// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by back-substitution + /// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by + /// back-substitution VectorValues optimize() const; - /// Version of optimize for incomplete BayesNet, needs solution for missing variables - VectorValues optimize(const VectorValues& solutionForMissing) const; + /// Version of optimize for incomplete BayesNet, given missing variables + VectorValues optimize(const VectorValues given) const; + + /** + * Sample using ancestral sampling + * Example: + * std::mt19937_64 rng(42); + * auto sample = gbn.sample(&rng); + */ + VectorValues sample(std::mt19937_64* rng) const; + + /** + * Sample from an incomplete BayesNet, given missing variables + * Example: + * std::mt19937_64 rng(42); + * VectorValues given = ...; + * auto sample = gbn.sample(given, &rng); + */ + VectorValues sample(VectorValues given, std::mt19937_64* rng) const; + + /// Sample using ancestral sampling, use default rng + VectorValues sample() const; + + /// Sample from an incomplete BayesNet, use default rng + VectorValues sample(VectorValues given) const; /** * Return ordering corresponding to a topological sort. diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index d87c39eea..cf3f78b73 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #ifdef __GNUC__ @@ -34,6 +35,9 @@ #include #include +// In Wrappers we have no access to this so have a default ready +static std::mt19937_64 kRandomNumberGenerator(42); + using namespace std; namespace gtsam { @@ -43,19 +47,47 @@ namespace gtsam { Key key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas) : BaseFactor(key, R, d, sigmas), BaseConditional(1) {} - /* ************************************************************************* */ - GaussianConditional::GaussianConditional( - Key key, const Vector& d, const Matrix& R, - Key name1, const Matrix& S, const SharedDiagonal& sigmas) : - BaseFactor(key, R, name1, S, d, sigmas), BaseConditional(1) {} + /* ************************************************************************ */ + GaussianConditional::GaussianConditional(Key key, const Vector& d, + const Matrix& R, Key parent1, + const Matrix& S, + const SharedDiagonal& sigmas) + : BaseFactor(key, R, parent1, S, d, sigmas), BaseConditional(1) {} - /* ************************************************************************* */ - GaussianConditional::GaussianConditional( - Key key, const Vector& d, const Matrix& R, - Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas) : - BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {} + /* ************************************************************************ */ + GaussianConditional::GaussianConditional(Key key, const Vector& d, + const Matrix& R, Key parent1, + const Matrix& S, Key parent2, + const Matrix& T, + const SharedDiagonal& sigmas) + : BaseFactor(key, R, parent1, S, parent2, T, d, sigmas), + BaseConditional(1) {} - /* ************************************************************************* */ + /* ************************************************************************ */ + GaussianConditional GaussianConditional::FromMeanAndStddev( + Key key, const Matrix& A, Key parent, const Vector& b, double sigma) { + // |Rx + Sy - d| = |x-(Ay + b)|/sigma + const Matrix R = Matrix::Identity(b.size(), b.size()); + const Matrix S = -A; + const Vector d = b; + return GaussianConditional(key, d, R, parent, S, + noiseModel::Isotropic::Sigma(b.size(), sigma)); + } + + /* ************************************************************************ */ + GaussianConditional GaussianConditional::FromMeanAndStddev( + Key key, const Matrix& A1, Key parent1, const Matrix& A2, Key parent2, + const Vector& b, double sigma) { + // |Rx + Sy + Tz - d| = |x-(A1 y + A2 z + b)|/sigma + const Matrix R = Matrix::Identity(b.size(), b.size()); + const Matrix S = -A1; + const Matrix T = -A2; + const Vector d = b; + return GaussianConditional(key, d, R, parent1, S, parent2, T, + noiseModel::Isotropic::Sigma(b.size(), sigma)); + } + + /* ************************************************************************ */ void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const { cout << s << " Conditional density "; for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) { @@ -192,7 +224,88 @@ namespace gtsam { } } - /* ************************************************************************* */ + /* ************************************************************************ */ + JacobianFactor::shared_ptr GaussianConditional::likelihood( + const VectorValues& frontalValues) const { + // Error is |Rx - (d - Sy - Tz - ...)|^2 + // so when we instantiate x (which has to be completely known) we beget: + // |Sy + Tz + ... - (d - Rx)|^2 + // The noise model just transfers over! + + // Get frontalValues as vector + const Vector x = + frontalValues.vector(KeyVector(beginFrontals(), endFrontals())); + + // Copy the augmented Jacobian matrix: + auto newAb = Ab_; + + // Restrict view to parent blocks + newAb.firstBlock() += nrFrontals_; + + // Update right-hand-side (last column) + auto last = newAb.matrix().cols() - 1; + const auto RR = R().triangularView(); + newAb.matrix().col(last) -= RR * x; + + // The keys now do not include the frontal keys: + KeyVector newKeys; + newKeys.reserve(nrParents()); + for (auto&& key : parents()) newKeys.push_back(key); + + // Hopefully second newAb copy below is optimized out... + return boost::make_shared(newKeys, newAb, model_); + } + + /* **************************************************************************/ + JacobianFactor::shared_ptr GaussianConditional::likelihood( + const Vector& frontal) const { + if (nrFrontals() != 1) + throw std::invalid_argument( + "GaussianConditional Single value likelihood can only be invoked on " + "single-variable conditional"); + VectorValues values; + values.insert(keys_[0], frontal); + return likelihood(values); + } + + /* ************************************************************************ */ + VectorValues GaussianConditional::sample(const VectorValues& parentsValues, + std::mt19937_64* rng) const { + if (nrFrontals() != 1) { + throw std::invalid_argument( + "GaussianConditional::sample can only be called on single variable " + "conditionals"); + } + if (!model_) { + throw std::invalid_argument( + "GaussianConditional::sample can only be called if a diagonal noise " + "model was specified at construction."); + } + VectorValues solution = solve(parentsValues); + Key key = firstFrontalKey(); + const Vector& sigmas = model_->sigmas(); + solution[key] += Sampler::sampleDiagonal(sigmas, rng); + return solution; + } + + VectorValues GaussianConditional::sample(std::mt19937_64* rng) const { + if (nrParents() != 0) + throw std::invalid_argument( + "sample() can only be invoked on no-parent prior"); + VectorValues values; + return sample(values); + } + + /* ************************************************************************ */ + VectorValues GaussianConditional::sample() const { + return sample(&kRandomNumberGenerator); + } + + VectorValues GaussianConditional::sample(const VectorValues& given) const { + return sample(given, &kRandomNumberGenerator); + } + + /* ************************************************************************ */ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 void GTSAM_DEPRECATED GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index d93f65b42..6dd278536 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -26,12 +26,15 @@ #include #include +#include // for std::mt19937_64 + namespace gtsam { /** - * A conditional Gaussian functions as the node in a Bayes network + * A GaussianConditional functions as the node in a Bayes network. * It has a set of parents y,z, etc. and implements a probability density on x. * The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$ + * @addtogroup linear */ class GTSAM_EXPORT GaussianConditional : public JacobianFactor, @@ -43,6 +46,9 @@ namespace gtsam { typedef JacobianFactor BaseFactor; ///< Typedef to our factor base class typedef Conditional BaseConditional; ///< Typedef to our conditional base class + /// @name Constructors + /// @{ + /** default constructor needed for serialization */ GaussianConditional() {} @@ -51,13 +57,14 @@ namespace gtsam { const SharedDiagonal& sigmas = SharedDiagonal()); /** constructor with only one parent |Rx+Sy-d| */ - GaussianConditional(Key key, const Vector& d, const Matrix& R, - Key name1, const Matrix& S, const SharedDiagonal& sigmas = SharedDiagonal()); + GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1, + const Matrix& S, + const SharedDiagonal& sigmas = SharedDiagonal()); /** constructor with two parents |Rx+Sy+Tz-d| */ - GaussianConditional(Key key, const Vector& d, const Matrix& R, - Key name1, const Matrix& S, Key name2, const Matrix& T, - const SharedDiagonal& sigmas = SharedDiagonal()); + GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1, + const Matrix& S, Key parent2, const Matrix& T, + const SharedDiagonal& sigmas = SharedDiagonal()); /** Constructor with arbitrary number of frontals and parents. * @tparam TERMS A container whose value type is std::pair, specifying the @@ -76,6 +83,17 @@ namespace gtsam { const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); + /// Construct from mean A1 p1 + b and standard deviation. + static GaussianConditional FromMeanAndStddev(Key key, const Matrix& A, + Key parent, const Vector& b, + double sigma); + + /// Construct from mean A1 p1 + A2 p2 + b and standard deviation. + static GaussianConditional FromMeanAndStddev(Key key, // + const Matrix& A1, Key parent1, + const Matrix& A2, Key parent2, + const Vector& b, double sigma); + /** Combine several GaussianConditional into a single dense GC. The conditionals enumerated by * \c first and \c last must be in increasing order, meaning that the parents of any * conditional may not include a conditional coming before it. @@ -86,6 +104,10 @@ namespace gtsam { template static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional); + /// @} + /// @name Testable + /// @{ + /** print */ void print(const std::string& = "GaussianConditional", const KeyFormatter& formatter = DefaultKeyFormatter) const override; @@ -93,6 +115,10 @@ namespace gtsam { /** equals function */ bool equals(const GaussianFactor&cg, double tol = 1e-9) const override; + /// @} + /// @name Standard Interface + /// @{ + /** Return a view of the upper-triangular R block of the conditional */ constABlock R() const { return Ab_.range(0, nrFrontals()); } @@ -125,10 +151,46 @@ namespace gtsam { /** Performs transpose backsubstition in place on values */ void solveTransposeInPlace(VectorValues& gy) const; + /** Convert to a likelihood factor by providing value before bar. */ + JacobianFactor::shared_ptr likelihood( + const VectorValues& frontalValues) const; + + /** Single variable version of likelihood. */ + JacobianFactor::shared_ptr likelihood(const Vector& frontal) const; + + /** + * Sample from conditional, zero parent version + * Example: + * std::mt19937_64 rng(42); + * auto sample = gbn.sample(&rng); + */ + VectorValues sample(std::mt19937_64* rng) const; + + /** + * Sample from conditional, given missing variables + * Example: + * std::mt19937_64 rng(42); + * VectorValues given = ...; + * auto sample = gbn.sample(given, &rng); + */ + VectorValues sample(const VectorValues& parentsValues, + std::mt19937_64* rng) const; + + /// Sample, use default rng + VectorValues sample() const; + + /// Sample with given values, use default rng + VectorValues sample(const VectorValues& parentsValues) const; + + /// @} + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /// @name Deprecated + /// @{ /** Scale the values in \c gy according to the sigmas for the frontal variables in this * conditional. */ void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const; + /// @} #endif private: diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index d9cde9b91..2f7aa312b 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -23,9 +23,12 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, const Vector& mean, const double& sigma) - { - return GaussianDensity(key, mean / sigma, Matrix::Identity(mean.size(), mean.size()) / sigma); + GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, + const Vector& mean, + double sigma) { + return GaussianDensity(key, mean, + Matrix::Identity(mean.size(), mean.size()), + noiseModel::Isotropic::Sigma(mean.size(), sigma)); } /* ************************************************************************* */ @@ -35,8 +38,8 @@ namespace gtsam { for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; cout << endl; - gtsam::print(Matrix(R()), "R: "); - gtsam::print(Vector(d()), "d: "); + gtsam::print(mean(), "mean: "); + gtsam::print(covariance(), "covariance: "); if(model_) model_->print("Noise model: "); } diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 71af704ab..f078d5db6 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -24,11 +24,10 @@ namespace gtsam { /** - * A Gaussian density. - * - * It is implemented as a GaussianConditional without parents. + * A GaussianDensity is a GaussianConditional without parents. * The negative log-probability is given by \f$ |Rx - d|^2 \f$ * with \f$ \Lambda = \Sigma^{-1} = R^T R \f$ and \f$ \mu = R^{-1} d \f$ + * @addtogroup linear */ class GTSAM_EXPORT GaussianDensity : public GaussianConditional { @@ -52,8 +51,9 @@ namespace gtsam { GaussianDensity(Key key, const Vector& d, const Matrix& R, const SharedDiagonal& noiseModel = SharedDiagonal()) : GaussianConditional(key, d, R, noiseModel) {} - /// Construct using a mean and variance - static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean, const double& sigma); + /// Construct using a mean and standard deviation + static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean, + double sigma); /// print void print(const std::string& = "GaussianDensity", diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 4957dfa14..20d4c955b 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -22,14 +22,18 @@ namespace gtsam { /* ************************************************************************* */ Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, uint_fast64_t seed) - : model_(model), generator_(seed) {} + : model_(model), generator_(seed) { + if (!model) { + throw std::invalid_argument("Sampler::Sampler needs a non-null model."); + } +} /* ************************************************************************* */ Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed) : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {} /* ************************************************************************* */ -Vector Sampler::sampleDiagonal(const Vector& sigmas) const { +Vector Sampler::sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng) { size_t d = sigmas.size(); Vector result(d); for (size_t i = 0; i < d; i++) { @@ -39,14 +43,18 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) const { if (sigma == 0.0) { result(i) = 0.0; } else { - typedef std::normal_distribution Normal; - Normal dist(0.0, sigma); - result(i) = dist(generator_); + std::normal_distribution dist(0.0, sigma); + result(i) = dist(*rng); } } return result; } +/* ************************************************************************* */ +Vector Sampler::sampleDiagonal(const Vector& sigmas) const { + return sampleDiagonal(sigmas, &generator_); +} + /* ************************************************************************* */ Vector Sampler::sample() const { assert(model_.get()); diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index bb5098f34..5be8b445d 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -63,15 +63,9 @@ class GTSAM_EXPORT Sampler { /// @name access functions /// @{ - size_t dim() const { - assert(model_.get()); - return model_->dim(); - } + size_t dim() const { return model_->dim(); } - Vector sigmas() const { - assert(model_.get()); - return model_->sigmas(); - } + Vector sigmas() const { return model_->sigmas(); } const noiseModel::Diagonal::shared_ptr& model() const { return model_; } @@ -82,6 +76,8 @@ class GTSAM_EXPORT Sampler { /// sample from distribution Vector sample() const; + /// sample with given random number generator + static Vector sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng); /// @} protected: diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 6a2514b35..62996af27 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -33,7 +33,7 @@ namespace gtsam { using boost::adaptors::map_values; using boost::accumulate; - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues::VectorValues(const VectorValues& first, const VectorValues& second) { // Merge using predicate for comparing first of pair @@ -44,7 +44,7 @@ namespace gtsam { throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common."); } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues::VectorValues(const Vector& x, const Dims& dims) { using Pair = pair; size_t j = 0; @@ -61,7 +61,7 @@ namespace gtsam { } } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues::VectorValues(const Vector& x, const Scatter& scatter) { size_t j = 0; for (const SlotEntry& v : scatter) { @@ -74,7 +74,7 @@ namespace gtsam { } } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues VectorValues::Zero(const VectorValues& other) { VectorValues result; @@ -87,7 +87,7 @@ namespace gtsam { return result; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues::iterator VectorValues::insert(const std::pair& key_value) { std::pair result = values_.insert(key_value); if(!result.second) @@ -97,7 +97,7 @@ namespace gtsam { return result.first; } - /* ************************************************************************* */ + /* ************************************************************************ */ void VectorValues::update(const VectorValues& values) { iterator hint = begin(); @@ -115,7 +115,7 @@ namespace gtsam { } } - /* ************************************************************************* */ + /* ************************************************************************ */ void VectorValues::insert(const VectorValues& values) { size_t originalSize = size(); @@ -124,14 +124,14 @@ namespace gtsam { throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys."); } - /* ************************************************************************* */ + /* ************************************************************************ */ void VectorValues::setZero() { for(Vector& v: values_ | map_values) v.setZero(); } - /* ************************************************************************* */ + /* ************************************************************************ */ GTSAM_EXPORT ostream& operator<<(ostream& os, const VectorValues& v) { // Change print depending on whether we are using TBB #ifdef GTSAM_USE_TBB @@ -150,7 +150,7 @@ namespace gtsam { return os; } - /* ************************************************************************* */ + /* ************************************************************************ */ void VectorValues::print(const string& str, const KeyFormatter& formatter) const { cout << str << ": " << size() << " elements\n"; @@ -158,7 +158,7 @@ namespace gtsam { cout.flush(); } - /* ************************************************************************* */ + /* ************************************************************************ */ bool VectorValues::equals(const VectorValues& x, double tol) const { if(this->size() != x.size()) return false; @@ -170,7 +170,7 @@ namespace gtsam { return true; } - /* ************************************************************************* */ + /* ************************************************************************ */ Vector VectorValues::vector() const { // Count dimensions DenseIndex totalDim = 0; @@ -187,7 +187,7 @@ namespace gtsam { return result; } - /* ************************************************************************* */ + /* ************************************************************************ */ Vector VectorValues::vector(const Dims& keys) const { // Count dimensions @@ -203,12 +203,12 @@ namespace gtsam { return result; } - /* ************************************************************************* */ + /* ************************************************************************ */ void VectorValues::swap(VectorValues& other) { this->values_.swap(other.values_); } - /* ************************************************************************* */ + /* ************************************************************************ */ namespace internal { bool structureCompareOp(const boost::tuple()); } - /* ************************************************************************* */ + /* ************************************************************************ */ double VectorValues::dot(const VectorValues& v) const { if(this->size() != v.size()) @@ -244,12 +244,12 @@ namespace gtsam { return result; } - /* ************************************************************************* */ + /* ************************************************************************ */ double VectorValues::norm() const { return std::sqrt(this->squaredNorm()); } - /* ************************************************************************* */ + /* ************************************************************************ */ double VectorValues::squaredNorm() const { double sumSquares = 0.0; using boost::adaptors::map_values; @@ -258,7 +258,7 @@ namespace gtsam { return sumSquares; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues VectorValues::operator+(const VectorValues& c) const { if(this->size() != c.size()) @@ -278,13 +278,13 @@ namespace gtsam { return result; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues VectorValues::add(const VectorValues& c) const { return *this + c; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues& VectorValues::operator+=(const VectorValues& c) { if(this->size() != c.size()) @@ -301,13 +301,13 @@ namespace gtsam { return *this; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues& VectorValues::addInPlace(const VectorValues& c) { return *this += c; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues& VectorValues::addInPlace_(const VectorValues& c) { for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) { @@ -320,7 +320,7 @@ namespace gtsam { return *this; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues VectorValues::operator-(const VectorValues& c) const { if(this->size() != c.size()) @@ -340,13 +340,13 @@ namespace gtsam { return result; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues VectorValues::subtract(const VectorValues& c) const { return *this - c; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues operator*(const double a, const VectorValues &v) { VectorValues result; @@ -359,13 +359,13 @@ namespace gtsam { return result; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues VectorValues::scale(const double a) const { return a * *this; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues& VectorValues::operator*=(double alpha) { for(Vector& v: *this | map_values) @@ -373,12 +373,43 @@ namespace gtsam { return *this; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues& VectorValues::scaleInPlace(double alpha) { return *this *= alpha; } - /* ************************************************************************* */ + /* ************************************************************************ */ + string VectorValues::html(const KeyFormatter& keyFormatter) const { + stringstream ss; + + // Print out preamble. + ss << "
\n\n \n"; + + // Print out header row. + ss << " \n"; + + // Finish header and start body. + ss << " \n \n"; + + // Print out all rows. +#ifdef GTSAM_USE_TBB + // TBB uses un-ordered map, so inefficiently order them: + std::map ordered; + for (const auto& kv : *this) ordered.emplace(kv); + for (const auto& kv : ordered) { +#else + for (const auto& kv : *this) { +#endif + ss << " "; + ss << ""; + ss << "\n"; + } + ss << " \n
Variablevalue
" << keyFormatter(kv.first) << "" + << kv.second.transpose() << "
\n
"; + return ss.str(); + } + + /* ************************************************************************ */ } // \namespace gtsam diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 9e60ff2aa..1ff8c5c16 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -34,7 +34,7 @@ namespace gtsam { /** - * This class represents a collection of vector-valued variables associated + * VectorValues represents a collection of vector-valued variables associated * each with a unique integer index. It is typically used to store the variables * of a GaussianFactorGraph. Optimizing a GaussianFactorGraph or GaussianBayesNet * returns this class. @@ -69,7 +69,7 @@ namespace gtsam { * which is a view on the underlying data structure. * * This class is additionally used in gradient descent and dog leg to store the gradient. - * \nosubgrouping + * @addtogroup linear */ class GTSAM_EXPORT VectorValues { protected: @@ -344,11 +344,16 @@ namespace gtsam { /// @} - /// @} - /// @name Matlab syntactic sugar for linear algebra operations + /// @name Wrapper support /// @{ - //inline VectorValues scale(const double a, const VectorValues& c) const { return a * (*this); } + /** + * @brief Output as a html table. + * + * @param keyFormatter function that formats keys. + */ + std::string html( + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// @} diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index b079c3dd1..f1bc92f69 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -255,6 +255,7 @@ class VectorValues { // enabling serialization functionality void serialize() const; + string html() const; }; #include @@ -407,8 +408,10 @@ class GaussianFactorGraph { // Elimination and marginals gtsam::GaussianBayesNet* eliminateSequential(); + gtsam::GaussianBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type); gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); gtsam::GaussianBayesTree* eliminateMultifrontal(); + gtsam::GaussianBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type); gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); pair eliminatePartialSequential( const gtsam::Ordering& ordering); @@ -466,15 +469,36 @@ virtual class GaussianConditional : gtsam::JacobianFactor { GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, size_t name2, Matrix T); - // Standard Interface + // Named constructors + static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, + const Matrix& A, + gtsam::Key parent, + const Vector& b, + double sigma); + + static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, + const Matrix& A1, + gtsam::Key parent1, + const Matrix& A2, + gtsam::Key parent2, + const Vector& b, + double sigma); + // Testable void print(string s = "GaussianConditional", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianConditional& cg, double tol) const; + + // Standard Interface gtsam::Key firstFrontalKey() const; + gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; + gtsam::JacobianFactor* likelihood( + const gtsam::VectorValues& frontalValues) const; + gtsam::JacobianFactor* likelihood(Vector frontal) const; + gtsam::VectorValues sample(const gtsam::VectorValues& parents) const; + gtsam::VectorValues sample() const; // Advanced Interface - gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; void solveTransposeInPlace(gtsam::VectorValues& gy) const; @@ -488,14 +512,21 @@ virtual class GaussianConditional : gtsam::JacobianFactor { #include virtual class GaussianDensity : gtsam::GaussianConditional { - //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + // Constructors + GaussianDensity(gtsam::Key key, Vector d, Matrix R, + const gtsam::noiseModel::Diagonal* sigmas); - //Standard Interface + static gtsam::GaussianDensity FromMeanAndStddev(gtsam::Key key, + const Vector& mean, + double sigma); + + // Testable void print(string s = "GaussianDensity", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianDensity &cg, double tol) const; + bool equals(const gtsam::GaussianDensity& cg, double tol) const; + + // Standard Interface Vector mean() const; Matrix covariance() const; }; @@ -512,6 +543,21 @@ virtual class GaussianBayesNet { bool equals(const gtsam::GaussianBayesNet& other, double tol) const; size_t size() const; + // Standard interface + void push_back(gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + gtsam::GaussianConditional* front() const; + gtsam::GaussianConditional* back() const; + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(gtsam::VectorValues given) const; + gtsam::VectorValues optimizeGradientSearch() const; + + gtsam::VectorValues sample(gtsam::VectorValues given) const; + gtsam::VectorValues sample() const; + gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; + gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; + // FactorGraph derived interface gtsam::GaussianConditional* at(size_t idx) const; gtsam::KeySet keys() const; @@ -520,21 +566,12 @@ virtual class GaussianBayesNet { void saveGraph(const string& s) const; - gtsam::GaussianConditional* front() const; - gtsam::GaussianConditional* back() const; - void push_back(gtsam::GaussianConditional* conditional); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; - gtsam::VectorValues optimizeGradientSearch() const; + std::pair matrix() const; gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; gtsam::VectorValues gradientAtZero() const; double error(const gtsam::VectorValues& x) const; double determinant() const; double logDeterminant() const; - gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; - gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; string dot( const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, @@ -556,7 +593,12 @@ virtual class GaussianBayesTree { size_t size() const; bool empty() const; size_t numCachedSeparatorMarginals() const; - void saveGraph(string s) const; + + string dot(const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void saveGraph(string s, + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; gtsam::VectorValues optimize() const; gtsam::VectorValues optimizeGradientSearch() const; diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index f62da15dd..2b125265f 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -16,10 +16,12 @@ */ #include +#include #include #include #include #include +#include #include #include @@ -35,6 +37,7 @@ using namespace boost::assign; using namespace std::placeholders; using namespace std; using namespace gtsam; +using symbol_shorthand::X; static const Key _x_ = 11, _y_ = 22, _z_ = 33; @@ -138,6 +141,30 @@ TEST( GaussianBayesNet, optimize3 ) EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST(GaussianBayesNet, sample) { + GaussianBayesNet gbn; + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished(); + const Vector2 mean(20, 40), b(10, 10); + const double sigma = 0.01; + + gbn.add(GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma)); + gbn.add(GaussianDensity::FromMeanAndStddev(X(1), mean, sigma)); + + auto actual = gbn.sample(); + EXPECT_LONGS_EQUAL(2, actual.size()); + EXPECT(assert_equal(mean, actual[X(1)], 50 * sigma)); + EXPECT(assert_equal(A1 * mean + b, actual[X(0)], 50 * sigma)); + + // Use a specific random generator + std::mt19937_64 rng(4242); + auto actual3 = gbn.sample(&rng); + EXPECT_LONGS_EQUAL(2, actual.size()); + // regression is not repeatable across platforms/versions :-( + // EXPECT(assert_equal(Vector2(20.0129382, 40.0039798), actual[X(1)], 1e-5)); + // EXPECT(assert_equal(Vector2(110.032083, 230.039811), actual[X(0)], 1e-5)); +} + /* ************************************************************************* */ TEST(GaussianBayesNet, ordering) { diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index fae00e1e4..8525cf9e0 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -20,9 +20,10 @@ #include #include -#include +#include #include #include +#include #include #include @@ -38,6 +39,7 @@ using namespace gtsam; using namespace std; using namespace boost::assign; +using symbol_shorthand::X; static const double tol = 1e-5; @@ -316,5 +318,87 @@ TEST( GaussianConditional, isGaussianFactor ) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +// Test FromMeanAndStddev named constructors +TEST(GaussianConditional, FromMeanAndStddev) { + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished(); + Matrix A2 = (Matrix(2, 2) << 5., 6., 7., 8.).finished(); + const Vector2 b(20, 40), x0(1, 2), x1(3, 4), x2(5, 6); + const double sigma = 3; + + VectorValues values = map_list_of(X(0), x0)(X(1), x1)(X(2), x2); + + auto conditional1 = + GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma); + Vector2 e1 = (x0 - (A1 * x1 + b)) / sigma; + double expected1 = 0.5 * e1.dot(e1); + EXPECT_DOUBLES_EQUAL(expected1, conditional1.error(values), 1e-9); + + auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), A2, + X(2), b, sigma); + Vector2 e2 = (x0 - (A1 * x1 + A2 * x2 + b)) / sigma; + double expected2 = 0.5 * e2.dot(e2); + EXPECT_DOUBLES_EQUAL(expected2, conditional2.error(values), 1e-9); +} + +/* ************************************************************************* */ +// Test likelihood method (conversion to JacobianFactor) +TEST(GaussianConditional, likelihood) { + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished(); + const Vector2 b(20, 40), x0(1, 2); + const double sigma = 0.01; + + // |x0 - A1 x1 - b|^2 + auto conditional = + GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma); + + VectorValues frontalValues; + frontalValues.insert(X(0), x0); + auto actual1 = conditional.likelihood(frontalValues); + CHECK(actual1); + + // |(-A1) x1 - (b - x0)|^2 + JacobianFactor expected(X(1), -A1, b - x0, + noiseModel::Isotropic::Sigma(2, sigma)); + EXPECT(assert_equal(expected, *actual1, tol)); + + // Check single vector version + auto actual2 = conditional.likelihood(x0); + CHECK(actual2); + EXPECT(assert_equal(expected, *actual2, tol)); +} + +/* ************************************************************************* */ +// Test sampling +TEST(GaussianConditional, sample) { + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished(); + const Vector2 b(20, 40), x1(3, 4); + const double sigma = 0.01; + + auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma); + auto actual1 = density.sample(); + EXPECT_LONGS_EQUAL(1, actual1.size()); + EXPECT(assert_equal(b, actual1[X(0)], 50 * sigma)); + + VectorValues given; + given.insert(X(1), x1); + + auto conditional = + GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma); + auto actual2 = conditional.sample(given); + EXPECT_LONGS_EQUAL(1, actual2.size()); + EXPECT(assert_equal(A1 * x1 + b, actual2[X(0)], 50 * sigma)); + + // Use a specific random generator + std::mt19937_64 rng(4242); + auto actual3 = conditional.sample(given, &rng); + EXPECT_LONGS_EQUAL(1, actual2.size()); + // regression is not repeatable across platforms/versions :-( + // EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianDensity.cpp b/gtsam/linear/tests/testGaussianDensity.cpp index 29dc49591..14608e794 100644 --- a/gtsam/linear/tests/testGaussianDensity.cpp +++ b/gtsam/linear/tests/testGaussianDensity.cpp @@ -17,10 +17,13 @@ **/ #include +#include + #include using namespace gtsam; using namespace std; +using symbol_shorthand::X; /* ************************************************************************* */ TEST(GaussianDensity, constructor) @@ -37,6 +40,22 @@ TEST(GaussianDensity, constructor) EXPECT(assert_equal(s, copied.get_model()->sigmas())); } +/* ************************************************************************* */ +// Test FromMeanAndStddev named constructor +TEST(GaussianDensity, FromMeanAndStddev) { + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished(); + const Vector2 b(20, 40), x0(1, 2); + const double sigma = 3; + + VectorValues values; + values.insert(X(0), x0); + + auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma); + Vector2 e = (x0 - b) / sigma; + double expected = 0.5 * e.dot(e); + EXPECT_DOUBLES_EQUAL(expected, density.error(values), 1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index f97f96aaf..521cc2289 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include @@ -248,6 +248,33 @@ TEST(VectorValues, print) EXPECT(expected == actual.str()); } +/* ************************************************************************* */ +// Check html representation. +TEST(VectorValues, html) { + VectorValues vv; + using symbol_shorthand::X; + vv.insert(X(1), Vector2(2, 3.1)); + vv.insert(X(2), Vector2(4, 5.2)); + vv.insert(X(5), Vector2(6, 7.3)); + vv.insert(X(7), Vector2(8, 9.4)); + string expected = + "
\n" + "\n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + "
Variablevalue
x1 2 3.1
x2 4 5.2
x5 6 7.3
x7 8 9.4
\n" + "
"; + string actual = vv.html(); + EXPECT(actual == expected); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/navigation/ImuBias.cpp b/gtsam/navigation/ImuBias.cpp index 0dbc5736f..bc11f95f8 100644 --- a/gtsam/navigation/ImuBias.cpp +++ b/gtsam/navigation/ImuBias.cpp @@ -66,8 +66,8 @@ namespace imuBias { // } /// ostream operator std::ostream& operator<<(std::ostream& os, const ConstantBias& bias) { - os << "acc = " << Point3(bias.accelerometer()); - os << " gyro = " << Point3(bias.gyroscope()); + os << "acc = " << bias.accelerometer().transpose(); + os << " gyro = " << bias.gyroscope().transpose(); return os; } diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h index c6e1001c4..d88afd505 100644 --- a/gtsam/nonlinear/ISAM2Params.h +++ b/gtsam/nonlinear/ISAM2Params.h @@ -300,18 +300,10 @@ struct GTSAM_EXPORT ISAM2Params { RelinearizationThreshold getRelinearizeThreshold() const { return relinearizeThreshold; } - int getRelinearizeSkip() const { return relinearizeSkip; } - bool isEnableRelinearization() const { return enableRelinearization; } - bool isEvaluateNonlinearError() const { return evaluateNonlinearError; } std::string getFactorization() const { return factorizationTranslator(factorization); } - bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; } KeyFormatter getKeyFormatter() const { return keyFormatter; } - bool isEnableDetailedResults() const { return enableDetailedResults; } - bool isEnablePartialRelinearizationCheck() const { - return enablePartialRelinearizationCheck; - } void setOptimizationParams(OptimizationParams optimizationParams) { this->optimizationParams = optimizationParams; @@ -319,31 +311,12 @@ struct GTSAM_EXPORT ISAM2Params { void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { this->relinearizeThreshold = relinearizeThreshold; } - void setRelinearizeSkip(int relinearizeSkip) { - this->relinearizeSkip = relinearizeSkip; - } - void setEnableRelinearization(bool enableRelinearization) { - this->enableRelinearization = enableRelinearization; - } - void setEvaluateNonlinearError(bool evaluateNonlinearError) { - this->evaluateNonlinearError = evaluateNonlinearError; - } void setFactorization(const std::string& factorization) { this->factorization = factorizationTranslator(factorization); } - void setCacheLinearizedFactors(bool cacheLinearizedFactors) { - this->cacheLinearizedFactors = cacheLinearizedFactors; - } void setKeyFormatter(KeyFormatter keyFormatter) { this->keyFormatter = keyFormatter; } - void setEnableDetailedResults(bool enableDetailedResults) { - this->enableDetailedResults = enableDetailedResults; - } - void setEnablePartialRelinearizationCheck( - bool enablePartialRelinearizationCheck) { - this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; - } GaussianFactorGraph::Eliminate getEliminationFunction() const { return factorization == CHOLESKY diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index eedf421bc..da5e8b29c 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -98,11 +98,11 @@ class NonlinearFactorGraph { string dot( const gtsam::Values& values, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, - const GraphvizFormatting& formatting = GraphvizFormatting()); + const GraphvizFormatting& writer = GraphvizFormatting()); void saveGraph( const string& s, const gtsam::Values& values, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, - const GraphvizFormatting& formatting = GraphvizFormatting()) const; + const GraphvizFormatting& writer = GraphvizFormatting()) const; // enabling serialization functionality void serialize() const; @@ -588,21 +588,19 @@ class ISAM2Params { void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params); void setRelinearizeThreshold(double threshold); void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map); - int getRelinearizeSkip() const; - void setRelinearizeSkip(int relinearizeSkip); - bool isEnableRelinearization() const; - void setEnableRelinearization(bool enableRelinearization); - bool isEvaluateNonlinearError() const; - void setEvaluateNonlinearError(bool evaluateNonlinearError); string getFactorization() const; void setFactorization(string factorization); - bool isCacheLinearizedFactors() const; - void setCacheLinearizedFactors(bool cacheLinearizedFactors); - bool isEnableDetailedResults() const; - void setEnableDetailedResults(bool enableDetailedResults); - bool isEnablePartialRelinearizationCheck() const; - void setEnablePartialRelinearizationCheck( - bool enablePartialRelinearizationCheck); + + int relinearizeSkip; + bool enableRelinearization; + bool evaluateNonlinearError; + bool cacheLinearizedFactors; + bool enableDetailedResults; + bool enablePartialRelinearizationCheck; + bool findUnusedFactorSlots; + + enum Factorization { CHOLESKY, QR }; + Factorization factorization; }; class ISAM2Clique { diff --git a/gtsam/sam/sam.i b/gtsam/sam/sam.i index 370e1c3ea..7ba401f1e 100644 --- a/gtsam/sam/sam.i +++ b/gtsam/sam/sam.i @@ -22,10 +22,19 @@ virtual class RangeFactor : gtsam::NoiseModelFactor { void serialize() const; }; +// between points: +typedef gtsam::RangeFactor RangeFactor2; +typedef gtsam::RangeFactor RangeFactor3; + +// between pose and point: typedef gtsam::RangeFactor RangeFactor2D; -typedef gtsam::RangeFactor RangeFactor3D; typedef gtsam::RangeFactor RangeFactorPose2; + +// between poses: +typedef gtsam::RangeFactor RangeFactor3D; typedef gtsam::RangeFactor RangeFactorPose3; + +// more specialized types: typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; typedef gtsam::RangeFactor, gtsam::Point3> diff --git a/gtsam/sfm/SfmData.cpp b/gtsam/sfm/SfmData.cpp new file mode 100644 index 000000000..43b1f5911 --- /dev/null +++ b/gtsam/sfm/SfmData.cpp @@ -0,0 +1,460 @@ +/* ---------------------------------------------------------------------------- + + * 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 SfmData.cpp + * @date January 2022 + * @author Frank dellaert + * @brief Data structure for dealing with Structure from Motion data + */ + +#include +#include +#include + +#include +#include + +namespace gtsam { + +using std::cout; +using std::endl; + +using gtsam::symbol_shorthand::P; + +/* ************************************************************************** */ +void SfmData::print(const std::string &s) const { + std::cout << "Number of cameras = " << cameras.size() << std::endl; + std::cout << "Number of tracks = " << tracks.size() << std::endl; +} + +/* ************************************************************************** */ +bool SfmData::equals(const SfmData &sfmData, double tol) const { + // check number of cameras and tracks + if (cameras.size() != sfmData.cameras.size() || + tracks.size() != sfmData.tracks.size()) { + return false; + } + + // check each camera + for (size_t i = 0; i < cameras.size(); ++i) { + if (!camera(i).equals(sfmData.camera(i), tol)) { + return false; + } + } + + // check each track + for (size_t j = 0; j < tracks.size(); ++j) { + if (!track(j).equals(sfmData.track(j), tol)) { + return false; + } + } + + return true; +} + +/* ************************************************************************* */ +Rot3 openGLFixedRotation() { // this is due to different convention for + // cameras in gtsam and openGL + /* R = [ 1 0 0 + * 0 -1 0 + * 0 0 -1] + */ + Matrix3 R_mat = Matrix3::Zero(3, 3); + R_mat(0, 0) = 1.0; + R_mat(1, 1) = -1.0; + R_mat(2, 2) = -1.0; + return Rot3(R_mat); +} + +/* ************************************************************************* */ +Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) { + Rot3 R90 = openGLFixedRotation(); + Rot3 wRc = (R.inverse()).compose(R90); + + // Our camera-to-world translation wTc = -R'*t + return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz))); +} + +/* ************************************************************************* */ +Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) { + Rot3 R90 = openGLFixedRotation(); + Rot3 cRw_openGL = R90.compose(R.inverse()); + Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz)); + return Pose3(cRw_openGL, t_openGL); +} + +/* ************************************************************************* */ +Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) { + return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), + PoseGTSAM.z()); +} + +/* ************************************************************************** */ +SfmData SfmData::FromBundlerFile(const std::string &filename) { + // Load the data file + std::ifstream is(filename.c_str(), std::ifstream::in); + if (!is) { + throw std::runtime_error( + "Error in FromBundlerFile: can not find the file!!"); + } + + SfmData sfmData; + + // Ignore the first line + char aux[500]; + is.getline(aux, 500); + + // Get the number of camera poses and 3D points + size_t nrPoses, nrPoints; + is >> nrPoses >> nrPoints; + + // Get the information for the camera poses + for (size_t i = 0; i < nrPoses; i++) { + // Get the focal length and the radial distortion parameters + float f, k1, k2; + is >> f >> k1 >> k2; + Cal3Bundler K(f, k1, k2); + + // Get the rotation matrix + float r11, r12, r13; + float r21, r22, r23; + float r31, r32, r33; + is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33; + + // Bundler-OpenGL rotation matrix + Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33); + + // Check for all-zero R, in which case quit + if (r11 == 0 && r12 == 0 && r13 == 0) { + throw std::runtime_error( + "Error in FromBundlerFile: zero rotation matrix"); + } + + // Get the translation vector + float tx, ty, tz; + is >> tx >> ty >> tz; + + Pose3 pose = openGL2gtsam(R, tx, ty, tz); + + sfmData.cameras.emplace_back(pose, K); + } + + // Get the information for the 3D points + sfmData.tracks.reserve(nrPoints); + for (size_t j = 0; j < nrPoints; j++) { + SfmTrack track; + + // Get the 3D position + float x, y, z; + is >> x >> y >> z; + track.p = Point3(x, y, z); + + // Get the color information + float r, g, b; + is >> r >> g >> b; + track.r = r / 255.f; + track.g = g / 255.f; + track.b = b / 255.f; + + // Now get the visibility information + size_t nvisible = 0; + is >> nvisible; + + track.measurements.reserve(nvisible); + track.siftIndices.reserve(nvisible); + for (size_t k = 0; k < nvisible; k++) { + size_t cam_idx = 0, point_idx = 0; + float u, v; + is >> cam_idx >> point_idx >> u >> v; + track.measurements.emplace_back(cam_idx, Point2(u, -v)); + track.siftIndices.emplace_back(cam_idx, point_idx); + } + + sfmData.tracks.push_back(track); + } + + return sfmData; +} + +/* ************************************************************************** */ +SfmData SfmData::FromBalFile(const std::string &filename) { + // Load the data file + std::ifstream is(filename.c_str(), std::ifstream::in); + if (!is) { + throw std::runtime_error("Error in FromBalFile: can not find the file!!"); + } + + SfmData sfmData; + + // Get the number of camera poses and 3D points + size_t nrPoses, nrPoints, nrObservations; + is >> nrPoses >> nrPoints >> nrObservations; + + sfmData.tracks.resize(nrPoints); + + // Get the information for the observations + for (size_t k = 0; k < nrObservations; k++) { + size_t i = 0, j = 0; + float u, v; + is >> i >> j >> u >> v; + sfmData.tracks[j].measurements.emplace_back(i, Point2(u, -v)); + } + + // Get the information for the camera poses + for (size_t i = 0; i < nrPoses; i++) { + // Get the Rodrigues vector + float wx, wy, wz; + is >> wx >> wy >> wz; + Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix + + // Get the translation vector + float tx, ty, tz; + is >> tx >> ty >> tz; + + Pose3 pose = openGL2gtsam(R, tx, ty, tz); + + // Get the focal length and the radial distortion parameters + float f, k1, k2; + is >> f >> k1 >> k2; + Cal3Bundler K(f, k1, k2); + + sfmData.cameras.emplace_back(pose, K); + } + + // Get the information for the 3D points + for (size_t j = 0; j < nrPoints; j++) { + // Get the 3D position + float x, y, z; + is >> x >> y >> z; + SfmTrack &track = sfmData.tracks[j]; + track.p = Point3(x, y, z); + track.r = 0.4f; + track.g = 0.4f; + track.b = 0.4f; + } + + return sfmData; +} + +/* ************************************************************************** */ +bool writeBAL(const std::string &filename, const SfmData &data) { + // Open the output file + std::ofstream os; + os.open(filename.c_str()); + os.precision(20); + if (!os.is_open()) { + cout << "Error in writeBAL: can not open the file!!" << endl; + return false; + } + + // Write the number of camera poses and 3D points + size_t nrObservations = 0; + for (size_t j = 0; j < data.tracks.size(); j++) { + nrObservations += data.tracks[j].numberMeasurements(); + } + + // Write observations + os << data.cameras.size() << " " << data.tracks.size() << " " + << nrObservations << endl; + os << endl; + + for (size_t j = 0; j < data.tracks.size(); j++) { // for each 3D point j + const SfmTrack &track = data.tracks[j]; + + for (size_t k = 0; k < track.numberMeasurements(); + k++) { // for each observation of the 3D point j + size_t i = track.measurements[k].first; // camera id + double u0 = data.cameras[i].calibration().px(); + double v0 = data.cameras[i].calibration().py(); + + if (u0 != 0 || v0 != 0) { + cout << "writeBAL has not been tested for calibration with nonzero " + "(u0,v0)" + << endl; + } + + double pixelBALx = track.measurements[k].second.x() - + u0; // center of image is the origin + double pixelBALy = -(track.measurements[k].second.y() - + v0); // center of image is the origin + Point2 pixelMeasurement(pixelBALx, pixelBALy); + os << i /*camera id*/ << " " << j /*point id*/ << " " + << pixelMeasurement.x() /*u of the pixel*/ << " " + << pixelMeasurement.y() /*v of the pixel*/ << endl; + } + } + os << endl; + + // Write cameras + for (size_t i = 0; i < data.cameras.size(); i++) { // for each camera + Pose3 poseGTSAM = data.cameras[i].pose(); + Cal3Bundler cameraCalibration = data.cameras[i].calibration(); + Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); + os << Rot3::Logmap(poseOpenGL.rotation()) << endl; + os << poseOpenGL.translation().x() << endl; + os << poseOpenGL.translation().y() << endl; + os << poseOpenGL.translation().z() << endl; + os << cameraCalibration.fx() << endl; + os << cameraCalibration.k1() << endl; + os << cameraCalibration.k2() << endl; + os << endl; + } + + // Write the points + for (size_t j = 0; j < data.tracks.size(); j++) { // for each 3D point j + Point3 point = data.tracks[j].p; + os << point.x() << endl; + os << point.y() << endl; + os << point.z() << endl; + os << endl; + } + + os.close(); + return true; +} + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +bool readBundler(const std::string &filename, SfmData &data) { + try { + data = SfmData::FromBundlerFile(filename); + return true; + } catch (const std::exception & /* e */) { + return false; + } +} +bool readBAL(const std::string &filename, SfmData &data) { + try { + data = SfmData::FromBalFile(filename); + return true; + } catch (const std::exception & /* e */) { + return false; + } +} +#endif + +SfmData readBal(const std::string &filename) { + return SfmData::FromBalFile(filename); +} + +/* ************************************************************************** */ +bool writeBALfromValues(const std::string &filename, const SfmData &data, + const Values &values) { + using Camera = PinholeCamera; + SfmData dataValues = data; + + // Store poses or cameras in SfmData + size_t nrPoses = values.count(); + if (nrPoses == dataValues.cameras.size()) { // we only estimated camera poses + for (size_t i = 0; i < dataValues.cameras.size(); i++) { // for each camera + Pose3 pose = values.at(i); + Cal3Bundler K = dataValues.cameras[i].calibration(); + Camera camera(pose, K); + dataValues.cameras[i] = camera; + } + } else { + size_t nrCameras = values.count(); + if (nrCameras == dataValues.cameras.size()) { // we only estimated camera + // poses and calibration + for (size_t i = 0; i < nrCameras; i++) { // for each camera + Key cameraKey = i; // symbol('c',i); + Camera camera = values.at(cameraKey); + dataValues.cameras[i] = camera; + } + } else { + cout << "writeBALfromValues: different number of cameras in " + "SfM_dataValues (#cameras " + << dataValues.cameras.size() << ") and values (#cameras " << nrPoses + << ", #poses " << nrCameras << ")!!" << endl; + return false; + } + } + + // Store 3D points in SfmData + size_t nrPoints = values.count(), nrTracks = dataValues.tracks.size(); + if (nrPoints != nrTracks) { + cout << "writeBALfromValues: different number of points in " + "SfM_dataValues (#points= " + << nrTracks << ") and values (#points " << nrPoints << ")!!" << endl; + } + + for (size_t j = 0; j < nrTracks; j++) { // for each point + Key pointKey = P(j); + if (values.exists(pointKey)) { + Point3 point = values.at(pointKey); + dataValues.tracks[j].p = point; + } else { + dataValues.tracks[j].r = 1.0; + dataValues.tracks[j].g = 0.0; + dataValues.tracks[j].b = 0.0; + dataValues.tracks[j].p = Point3(0, 0, 0); + } + } + + // Write SfmData to file + return writeBAL(filename, dataValues); +} + +/* ************************************************************************** */ +NonlinearFactorGraph SfmData::generalSfmFactors( + const SharedNoiseModel &model) const { + using ProjectionFactor = GeneralSFMFactor; + NonlinearFactorGraph factors; + + size_t j = 0; + for (const SfmTrack &track : tracks) { + for (const SfmMeasurement &m : track.measurements) { + size_t i = m.first; + Point2 uv = m.second; + factors.emplace_shared(uv, model, i, P(j)); + } + j += 1; + } + + return factors; +} + +/* ************************************************************************** */ +NonlinearFactorGraph SfmData::sfmFactorGraph( + const SharedNoiseModel &model, boost::optional fixedCamera, + boost::optional fixedPoint) const { + using ProjectionFactor = GeneralSFMFactor; + NonlinearFactorGraph graph = generalSfmFactors(model); + using noiseModel::Constrained; + if (fixedCamera) { + graph.addPrior(*fixedCamera, cameras[0], Constrained::All(9)); + } + if (fixedPoint) { + graph.addPrior(P(*fixedPoint), tracks[0].p, Constrained::All(3)); + } + return graph; +} + +/* ************************************************************************** */ +Values initialCamerasEstimate(const SfmData &db) { + Values initial; + size_t i = 0; // NO POINTS: j = 0; + for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera); + return initial; +} + +/* ************************************************************************** */ +Values initialCamerasAndPointsEstimate(const SfmData &db) { + Values initial; + size_t i = 0, j = 0; + for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera); + for (const SfmTrack &track : db.tracks) initial.insert(P(j++), track.p); + return initial; +} + +/* ************************************************************************** */ + +} // namespace gtsam diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h new file mode 100644 index 000000000..afce12205 --- /dev/null +++ b/gtsam/sfm/SfmData.h @@ -0,0 +1,236 @@ +/* ---------------------------------------------------------------------------- + + * 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 SfmData.h + * @date January 2022 + * @author Frank dellaert + * @brief Data structure for dealing with Structure from Motion data + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +namespace gtsam { + +/// Define the structure for the camera poses +typedef PinholeCamera SfmCamera; + +/** + * @brief SfmData stores a bunch of SfmTracks + * @addtogroup sfm + */ +struct GTSAM_EXPORT SfmData { + std::vector cameras; ///< Set of cameras + + std::vector tracks; ///< Sparse set of points + + /// @name Create from file + /// @{ + + /** + * @brief Parses a bundler output file and return result as SfmData instance. + * @param filename The name of the bundler file + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ + static SfmData FromBundlerFile(const std::string& filename); + + /** + * @brief Parse a "Bundle Adjustment in the Large" (BAL) file and return + * result as SfmData instance. + * @param filename The name of the BAL file. + * @return SfM structure where the data is stored. + */ + static SfmData FromBalFile(const std::string& filename); + + /// @} + /// @name Standard Interface + /// @{ + + /// Add a track to SfmData + void addTrack(const SfmTrack& t) { tracks.push_back(t); } + + /// Add a camera to SfmData + void addCamera(const SfmCamera& cam) { cameras.push_back(cam); } + + /// The number of reconstructed 3D points + size_t numberTracks() const { return tracks.size(); } + + /// The number of cameras + size_t numberCameras() const { return cameras.size(); } + + /// The track formed by series of landmark measurements + SfmTrack track(size_t idx) const { return tracks[idx]; } + + /// The camera pose at frame index `idx` + SfmCamera camera(size_t idx) const { return cameras[idx]; } + + /** + * @brief Create projection factors using keys i and P(j) + * + * @param model a noise model for projection errors + * @return NonlinearFactorGraph + */ + NonlinearFactorGraph generalSfmFactors( + const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, + 1.0)) const; + + /** + * @brief Create factor graph with projection factors and gauge fix. + * + * Note: pose keys are simply integer indices, points use Symbol('p', j). + * + * @param model a noise model for projection errors + * @param fixedCamera which camera to fix, if any (use boost::none if none) + * @param fixedPoint which point to fix, if any (use boost::none if none) + * @return NonlinearFactorGraph + */ + NonlinearFactorGraph sfmFactorGraph( + const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, 1.0), + boost::optional fixedCamera = 0, + boost::optional fixedPoint = 0) const; + + /// @} + /// @name Testable + /// @{ + + /// print + void print(const std::string& s = "") const; + + /// assert equality up to a tolerance + bool equals(const SfmData& sfmData, double tol = 1e-9) const; + + /// @} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /// @name Deprecated + /// @{ + void GTSAM_DEPRECATED add_track(const SfmTrack& t) { tracks.push_back(t); } + void GTSAM_DEPRECATED add_camera(const SfmCamera& cam) { + cameras.push_back(cam); + } + size_t GTSAM_DEPRECATED number_tracks() const { return tracks.size(); } + size_t GTSAM_DEPRECATED number_cameras() const { return cameras.size(); } + /// @} +#endif + /// @name Serialization + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(cameras); + ar& BOOST_SERIALIZATION_NVP(tracks); + } + + /// @} +}; + +/// traits +template <> +struct traits : public Testable {}; + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +GTSAM_EXPORT bool GTSAM_DEPRECATED readBundler(const std::string& filename, + SfmData& data); +GTSAM_EXPORT bool GTSAM_DEPRECATED readBAL(const std::string& filename, + SfmData& data); +#endif + +/** + * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and + * returns the data as a SfmData structure. Mainly used by wrapped code. + * @param filename The name of the BAL file. + * @return SfM structure where the data is stored. + */ +GTSAM_EXPORT SfmData readBal(const std::string& filename); + +/** + * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file + * from a SfmData structure + * @param filename The name of the BAL file to write + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ +GTSAM_EXPORT bool writeBAL(const std::string& filename, const SfmData& data); + +/** + * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file + * from a SfmData structure and a value structure (measurements are the same as + * the SfM input data, while camera poses and values are read from Values) + * @param filename The name of the BAL file to write + * @param data SfM structure where the data is stored + * @param values structure where the graph values are stored (values can be + * either Pose3 or PinholeCamera for the cameras, and should be + * Point3 for the 3D points). Note: assumes that the keys are "i" for pose i + * and "Symbol::('p',j)" for landmark j. + * @return true if the parsing was successful, false otherwise + */ +GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, + const SfmData& data, const Values& values); + +/** + * @brief This function converts an openGL camera pose to an GTSAM camera pose + * @param R rotation in openGL + * @param tx x component of the translation in openGL + * @param ty y component of the translation in openGL + * @param tz z component of the translation in openGL + * @return Pose3 in GTSAM format + */ +GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz); + +/** + * @brief This function converts a GTSAM camera pose to an openGL camera pose + * @param R rotation in GTSAM + * @param tx x component of the translation in GTSAM + * @param ty y component of the translation in GTSAM + * @param tz z component of the translation in GTSAM + * @return Pose3 in openGL format + */ +GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz); + +/** + * @brief This function converts a GTSAM camera pose to an openGL camera pose + * @param PoseGTSAM pose in GTSAM format + * @return Pose3 in openGL format + */ +GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM); + +/** + * @brief This function creates initial values for cameras from db. + * + * No symbol is used, so camera keys are simply integer indices. + * + * @param SfmData + * @return Values + */ +GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db); + +/** + * @brief This function creates initial values for cameras and points from db + * + * Note: Pose keys are simply integer indices, points use Symbol('p', j). + * + * @param SfmData + * @return Values + */ +GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); + +} // namespace gtsam diff --git a/gtsam/sfm/SfmTrack.cpp b/gtsam/sfm/SfmTrack.cpp new file mode 100644 index 000000000..d571e7c35 --- /dev/null +++ b/gtsam/sfm/SfmTrack.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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 SfmTrack.cpp + * @date January 2022 + * @author Frank Dellaert + * @brief A simple data structure for a track in Structure from Motion + */ + +#include + +#include + +namespace gtsam { + +void SfmTrack::print(const std::string& s) const { + std::cout << "Track with " << measurements.size(); + std::cout << " measurements of point " << p << std::endl; +} + +bool SfmTrack::equals(const SfmTrack& sfmTrack, double tol) const { + // check the 3D point + if (!p.isApprox(sfmTrack.p)) { + return false; + } + + // check the RGB values + if (r != sfmTrack.r || g != sfmTrack.g || b != sfmTrack.b) { + return false; + } + + // compare size of vectors for measurements and siftIndices + if (numberMeasurements() != sfmTrack.numberMeasurements() || + siftIndices.size() != sfmTrack.siftIndices.size()) { + return false; + } + + // compare measurements (order sensitive) + for (size_t idx = 0; idx < numberMeasurements(); ++idx) { + SfmMeasurement measurement = measurements[idx]; + SfmMeasurement otherMeasurement = sfmTrack.measurements[idx]; + + if (measurement.first != otherMeasurement.first || + !measurement.second.isApprox(otherMeasurement.second)) { + return false; + } + } + + // compare sift indices (order sensitive) + for (size_t idx = 0; idx < siftIndices.size(); ++idx) { + SiftIndex index = siftIndices[idx]; + SiftIndex otherIndex = sfmTrack.siftIndices[idx]; + + if (index.first != otherIndex.first || index.second != otherIndex.second) { + return false; + } + } + + return true; +} + +} // namespace gtsam diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h new file mode 100644 index 000000000..37731b32a --- /dev/null +++ b/gtsam/sfm/SfmTrack.h @@ -0,0 +1,133 @@ +/* ---------------------------------------------------------------------------- + + * 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 SfmTrack.h + * @date January 2022 + * @author Frank Dellaert + * @brief A simple data structure for a track in Structure from Motion + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace gtsam { + +/// A measurement with its camera index +typedef std::pair SfmMeasurement; + +/// Sift index for SfmTrack +typedef std::pair SiftIndex; + +/** + * @brief An SfmTrack stores SfM measurements grouped in a track + * @addtogroup sfm + */ +struct GTSAM_EXPORT SfmTrack { + Point3 p; ///< 3D position of the point + float r, g, b; ///< RGB color of the 3D point + + /// The 2D image projections (id,(u,v)) + std::vector measurements; + + /// The feature descriptors + std::vector siftIndices; + + /// @name Constructors + /// @{ + + explicit SfmTrack(float r = 0, float g = 0, float b = 0) + : p(0, 0, 0), r(r), g(g), b(b) {} + + explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0, + float b = 0) + : p(pt), r(r), g(g), b(b) {} + + /// @} + /// @name Standard Interface + /// @{ + + /// Add measurement (camera_idx, Point2) to track + void addMeasurement(size_t idx, const gtsam::Point2& m) { + measurements.emplace_back(idx, m); + } + + /// Total number of measurements in this track + size_t numberMeasurements() const { return measurements.size(); } + + /// Get the measurement (camera index, Point2) at pose index `idx` + const SfmMeasurement& measurement(size_t idx) const { + return measurements[idx]; + } + + /// Get the SIFT feature index corresponding to the measurement at `idx` + const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; } + + /// Get 3D point + const Point3& point3() const { return p; } + + /// Get RGB values describing 3d point + Point3 rgb() const { return Point3(r, g, b); } + + /// @} + /// @name Testable + /// @{ + + /// print + void print(const std::string& s = "") const; + + /// assert equality up to a tolerance + bool equals(const SfmTrack& sfmTrack, double tol = 1e-9) const; + + /// @} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /// @name Deprecated + /// @{ + void GTSAM_DEPRECATED add_measurement(size_t idx, const gtsam::Point2& m) { + measurements.emplace_back(idx, m); + } + + size_t GTSAM_DEPRECATED number_measurements() const { + return measurements.size(); + } + /// @} +#endif + /// @name Serialization + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(p); + ar& BOOST_SERIALIZATION_NVP(r); + ar& BOOST_SERIALIZATION_NVP(g); + ar& BOOST_SERIALIZATION_NVP(b); + ar& BOOST_SERIALIZATION_NVP(measurements); + ar& BOOST_SERIALIZATION_NVP(siftIndices); + } + /// @} +}; + +template +struct traits; + +template <> +struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index de12de478..a5c31534c 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -165,7 +165,7 @@ class GTSAM_EXPORT ShonanAveraging { size_t nrUnknowns() const { return nrUnknowns_; } /// Return number of measurements - size_t nrMeasurements() const { return measurements_.size(); } + size_t numberMeasurements() const { return measurements_.size(); } /// k^th binary measurement const BinaryMeasurement &measurement(size_t k) const { diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 705892e60..bf9a73ac5 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -4,7 +4,62 @@ namespace gtsam { -// ##### +#include +class SfmTrack { + SfmTrack(); + SfmTrack(const gtsam::Point3& pt); + const Point3& point3() const; + + double r; + double g; + double b; + + std::vector> measurements; + + size_t numberMeasurements() const; + pair measurement(size_t idx) const; + pair siftIndex(size_t idx) const; + void addMeasurement(size_t idx, const gtsam::Point2& m); + + // enabling serialization functionality + void serialize() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmTrack& expected, double tol) const; +}; + +#include +class SfmData { + SfmData(); + static gtsam::SfmData FromBundlerFile(string filename); + static gtsam::SfmData FromBalFile(string filename); + + void addTrack(const gtsam::SfmTrack& t); + void addCamera(const gtsam::SfmCamera& cam); + size_t numberTracks() const; + size_t numberCameras() const; + gtsam::SfmTrack track(size_t idx) const; + gtsam::PinholeCamera camera(size_t idx) const; + + gtsam::NonlinearFactorGraph generalSfmFactors( + const gtsam::SharedNoiseModel& model = + gtsam::noiseModel::Isotropic::Sigma(2, 1.0)) const; + gtsam::NonlinearFactorGraph sfmFactorGraph( + const gtsam::SharedNoiseModel& model = + gtsam::noiseModel::Isotropic::Sigma(2, 1.0), + size_t fixedCamera = 0, size_t fixedPoint = 0) const; + + // enabling serialization functionality + void serialize() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmData& expected, double tol) const; +}; + +gtsam::SfmData readBal(string filename); +bool writeBAL(string filename, gtsam::SfmData& data); +gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); +gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); #include @@ -92,7 +147,7 @@ class ShonanAveraging2 { // Query properties size_t nrUnknowns() const; - size_t nrMeasurements() const; + size_t numberMeasurements() const; gtsam::Rot2 measured(size_t i); gtsam::KeyVector keys(size_t i); @@ -140,7 +195,7 @@ class ShonanAveraging3 { // Query properties size_t nrUnknowns() const; - size_t nrMeasurements() const; + size_t numberMeasurements() const; gtsam::Rot3 measured(size_t i); gtsam::KeyVector keys(size_t i); diff --git a/gtsam/sfm/tests/testSfmData.cpp b/gtsam/sfm/tests/testSfmData.cpp new file mode 100644 index 000000000..7bd5d27e7 --- /dev/null +++ b/gtsam/sfm/tests/testSfmData.cpp @@ -0,0 +1,214 @@ +/* ---------------------------------------------------------------------------- + + * 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 TestSfmData.cpp + * @date January 2022 + * @author Frank dellaert + * @brief tests for SfmData class and associated utilites + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +using gtsam::symbol_shorthand::P; + +namespace gtsam { +GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name); +GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); +} // namespace gtsam + +/* ************************************************************************* */ +TEST(dataSet, Balbianello) { + // The structure where we will save the SfM data + const string filename = findExampleDataFile("Balbianello"); + SfmData sfmData = SfmData::FromBundlerFile(filename); + + // Check number of things + EXPECT_LONGS_EQUAL(5, sfmData.numberCameras()); + EXPECT_LONGS_EQUAL(544, sfmData.numberTracks()); + const SfmTrack& track0 = sfmData.tracks[0]; + EXPECT_LONGS_EQUAL(3, track0.numberMeasurements()); + + // Check projection of a given point + EXPECT_LONGS_EQUAL(0, track0.measurements[0].first); + const SfmCamera& camera0 = sfmData.cameras[0]; + Point2 expected = camera0.project(track0.p), + actual = track0.measurements[0].second; + EXPECT(assert_equal(expected, actual, 1)); + + // We share *one* noiseModel between all projection factors + auto model = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Convert to NonlinearFactorGraph + NonlinearFactorGraph graph = sfmData.sfmFactorGraph(model); + EXPECT_LONGS_EQUAL(1419, graph.size()); // regression + + // Get initial estimate + Values values = initialCamerasAndPointsEstimate(sfmData); + EXPECT_LONGS_EQUAL(549, values.size()); // regression +} + +/* ************************************************************************* */ +TEST(dataSet, readBAL_Dubrovnik) { + // The structure where we will save the SfM data + const string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfmData sfmData = SfmData::FromBalFile(filename); + + // Check number of things + EXPECT_LONGS_EQUAL(3, sfmData.numberCameras()); + EXPECT_LONGS_EQUAL(7, sfmData.numberTracks()); + const SfmTrack& track0 = sfmData.tracks[0]; + EXPECT_LONGS_EQUAL(3, track0.numberMeasurements()); + + // Check projection of a given point + EXPECT_LONGS_EQUAL(0, track0.measurements[0].first); + const SfmCamera& camera0 = sfmData.cameras[0]; + Point2 expected = camera0.project(track0.p), + actual = track0.measurements[0].second; + EXPECT(assert_equal(expected, actual, 12)); +} + +/* ************************************************************************* */ +TEST(dataSet, openGL2gtsam) { + Vector3 rotVec(0.2, 0.7, 1.1); + Rot3 R = Rot3::Expmap(rotVec); + Point3 t = Point3(0.0, 0.0, 0.0); + Pose3 poseGTSAM = Pose3(R, t); + + Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z()); + + Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); // columns! + Rot3 cRw(r1.x(), r2.x(), r3.x(), -r1.y(), -r2.y(), -r3.y(), -r1.z(), -r2.z(), + -r3.z()); + Rot3 wRc = cRw.inverse(); + Pose3 actual = Pose3(wRc, t); + + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(dataSet, gtsam2openGL) { + Vector3 rotVec(0.2, 0.7, 1.1); + Rot3 R = Rot3::Expmap(rotVec); + Point3 t = Point3(1.0, 20.0, 10.0); + Pose3 actual = Pose3(R, t); + Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z()); + + Pose3 expected = gtsam2openGL(poseGTSAM); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(dataSet, writeBAL_Dubrovnik) { + const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); + SfmData readData = SfmData::FromBalFile(filenameToRead); + + // Write readData to file filenameToWrite + const string filenameToWrite = createRewrittenFileName(filenameToRead); + CHECK(writeBAL(filenameToWrite, readData)); + + // Read what we wrote + SfmData writtenData = SfmData::FromBalFile(filenameToWrite); + + // Check that what we read is the same as what we wrote + EXPECT_LONGS_EQUAL(readData.numberCameras(), writtenData.numberCameras()); + EXPECT_LONGS_EQUAL(readData.numberTracks(), writtenData.numberTracks()); + + for (size_t i = 0; i < readData.numberCameras(); i++) { + PinholeCamera expectedCamera = writtenData.cameras[i]; + PinholeCamera actualCamera = readData.cameras[i]; + EXPECT(assert_equal(expectedCamera, actualCamera)); + } + + for (size_t j = 0; j < readData.numberTracks(); j++) { + // check point + SfmTrack expectedTrack = writtenData.tracks[j]; + SfmTrack actualTrack = readData.tracks[j]; + Point3 expectedPoint = expectedTrack.p; + Point3 actualPoint = actualTrack.p; + EXPECT(assert_equal(expectedPoint, actualPoint)); + + // check rgb + Point3 expectedRGB = + Point3(expectedTrack.r, expectedTrack.g, expectedTrack.b); + Point3 actualRGB = Point3(actualTrack.r, actualTrack.g, actualTrack.b); + EXPECT(assert_equal(expectedRGB, actualRGB)); + + // check measurements + for (size_t k = 0; k < actualTrack.numberMeasurements(); k++) { + EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first, + actualTrack.measurements[k].first); + EXPECT(assert_equal(expectedTrack.measurements[k].second, + actualTrack.measurements[k].second)); + } + } +} + +/* ************************************************************************* */ +TEST(dataSet, writeBALfromValues_Dubrovnik) { + const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); + SfmData readData = SfmData::FromBalFile(filenameToRead); + + Pose3 poseChange = + Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.3, 0.1, 0.3)); + + Values values; + for (size_t i = 0; i < readData.numberCameras(); i++) { // for each camera + Pose3 pose = poseChange.compose(readData.cameras[i].pose()); + values.insert(i, pose); + } + for (size_t j = 0; j < readData.numberTracks(); j++) { // for each point + Point3 point = poseChange.transformFrom(readData.tracks[j].p); + values.insert(P(j), point); + } + + // Write values and readData to a file + const string filenameToWrite = createRewrittenFileName(filenameToRead); + writeBALfromValues(filenameToWrite, readData, values); + + // Read the file we wrote + SfmData writtenData = SfmData::FromBalFile(filenameToWrite); + + // Check that the reprojection errors are the same and the poses are correct + // Check number of things + EXPECT_LONGS_EQUAL(3, writtenData.numberCameras()); + EXPECT_LONGS_EQUAL(7, writtenData.numberTracks()); + const SfmTrack& track0 = writtenData.tracks[0]; + EXPECT_LONGS_EQUAL(3, track0.numberMeasurements()); + + // Check projection of a given point + EXPECT_LONGS_EQUAL(0, track0.measurements[0].first); + const SfmCamera& camera0 = writtenData.cameras[0]; + Point2 expected = camera0.project(track0.p), + actual = track0.measurements[0].second; + EXPECT(assert_equal(expected, actual, 12)); + + Pose3 expectedPose = camera0.pose(); + Pose3 actualPose = values.at(0); + EXPECT(assert_equal(expectedPose, actualPose, 1e-7)); + + Point3 expectedPoint = track0.p; + Point3 actualPoint = values.at(P(0)); + EXPECT(assert_equal(expectedPoint, actualPoint, 1e-6)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index f17a9e421..4a60c8ba0 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -54,6 +54,8 @@ class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1 { Eigen::Matrix vecM_; ///< vectorized matrix to approximate public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// Constructor FrobeniusPrior(Key j, const MatrixNN& M, const SharedNoiseModel& model = nullptr) @@ -106,6 +108,8 @@ class GTSAM_EXPORT FrobeniusBetweenFactor : public NoiseModelFactor2 { enum { Dim = Rot::VectorN2::RowsAtCompileTime }; public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// @name Constructor /// @{ diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index ba4d12a25..20f12dbce 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -39,6 +39,9 @@ protected: public: + /** default constructor - only use for serialization */ + PoseRotationPrior() {} + /** standard constructor */ PoseRotationPrior(Key key, const Rotation& rot_z, const SharedNoiseModel& model) : Base(model, key), measured_(rot_z) {} diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 209c1196d..16712c429 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -146,7 +146,7 @@ protected: */ template void add(const SFM_TRACK& trackToAdd) { - for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { + for (size_t k = 0; k < trackToAdd.numberMeasurements(); k++) { this->measured_.push_back(trackToAdd.measurements[k].second); this->keys_.push_back(trackToAdd.measurements[k].first); } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 0684063de..a2b96efab 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -54,8 +54,6 @@ using namespace std; namespace fs = boost::filesystem; using gtsam::symbol_shorthand::L; -using gtsam::symbol_shorthand::P; -using gtsam::symbol_shorthand::X; #define LINESIZE 81920 @@ -208,15 +206,15 @@ std::map parseVariables(const std::string &filename, /* ************************************************************************* */ // Interpret noise parameters according to flags -static SharedNoiseModel -createNoiseModel(const Vector6 v, bool smart, NoiseFormat noiseFormat, - KernelFunctionType kernelFunctionType) { +static SharedNoiseModel createNoiseModel( + const Vector6 &v, bool smart, NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { if (noiseFormat == NoiseFormatAUTO) { // Try to guess covariance matrix layout - if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 && // + if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 && // v(3) != 0.0 && v(4) == 0.0 && v(5) == 0.0) { noiseFormat = NoiseFormatGRAPH; - } else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 && // + } else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 && // v(3) != 0.0 && v(4) == 0.0 && v(5) != 0.0) { noiseFormat = NoiseFormatCOV; } else { @@ -945,352 +943,6 @@ GraphAndValues load3D(const string &filename) { return make_pair(graph, initial); } -/* ************************************************************************* */ -Rot3 openGLFixedRotation() { // this is due to different convention for - // cameras in gtsam and openGL - /* R = [ 1 0 0 - * 0 -1 0 - * 0 0 -1] - */ - Matrix3 R_mat = Matrix3::Zero(3, 3); - R_mat(0, 0) = 1.0; - R_mat(1, 1) = -1.0; - R_mat(2, 2) = -1.0; - return Rot3(R_mat); -} - -/* ************************************************************************* */ -Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) { - Rot3 R90 = openGLFixedRotation(); - Rot3 wRc = (R.inverse()).compose(R90); - - // Our camera-to-world translation wTc = -R'*t - return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz))); -} - -/* ************************************************************************* */ -Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) { - Rot3 R90 = openGLFixedRotation(); - Rot3 cRw_openGL = R90.compose(R.inverse()); - Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz)); - return Pose3(cRw_openGL, t_openGL); -} - -/* ************************************************************************* */ -Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) { - return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), - PoseGTSAM.z()); -} - -/* ************************************************************************* */ -bool readBundler(const string &filename, SfmData &data) { - // Load the data file - ifstream is(filename.c_str(), ifstream::in); - if (!is) { - cout << "Error in readBundler: can not find the file!!" << endl; - return false; - } - - // Ignore the first line - char aux[500]; - is.getline(aux, 500); - - // Get the number of camera poses and 3D points - size_t nrPoses, nrPoints; - is >> nrPoses >> nrPoints; - - // Get the information for the camera poses - for (size_t i = 0; i < nrPoses; i++) { - // Get the focal length and the radial distortion parameters - float f, k1, k2; - is >> f >> k1 >> k2; - Cal3Bundler K(f, k1, k2); - - // Get the rotation matrix - float r11, r12, r13; - float r21, r22, r23; - float r31, r32, r33; - is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33; - - // Bundler-OpenGL rotation matrix - Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33); - - // Check for all-zero R, in which case quit - if (r11 == 0 && r12 == 0 && r13 == 0) { - cout << "Error in readBundler: zero rotation matrix for pose " << i - << endl; - return false; - } - - // Get the translation vector - float tx, ty, tz; - is >> tx >> ty >> tz; - - Pose3 pose = openGL2gtsam(R, tx, ty, tz); - - data.cameras.emplace_back(pose, K); - } - - // Get the information for the 3D points - data.tracks.reserve(nrPoints); - for (size_t j = 0; j < nrPoints; j++) { - SfmTrack track; - - // Get the 3D position - float x, y, z; - is >> x >> y >> z; - track.p = Point3(x, y, z); - - // Get the color information - float r, g, b; - is >> r >> g >> b; - track.r = r / 255.f; - track.g = g / 255.f; - track.b = b / 255.f; - - // Now get the visibility information - size_t nvisible = 0; - is >> nvisible; - - track.measurements.reserve(nvisible); - track.siftIndices.reserve(nvisible); - for (size_t k = 0; k < nvisible; k++) { - size_t cam_idx = 0, point_idx = 0; - float u, v; - is >> cam_idx >> point_idx >> u >> v; - track.measurements.emplace_back(cam_idx, Point2(u, -v)); - track.siftIndices.emplace_back(cam_idx, point_idx); - } - - data.tracks.push_back(track); - } - - is.close(); - return true; -} - -/* ************************************************************************* */ -bool readBAL(const string &filename, SfmData &data) { - // Load the data file - ifstream is(filename.c_str(), ifstream::in); - if (!is) { - cout << "Error in readBAL: can not find the file!!" << endl; - return false; - } - - // Get the number of camera poses and 3D points - size_t nrPoses, nrPoints, nrObservations; - is >> nrPoses >> nrPoints >> nrObservations; - - data.tracks.resize(nrPoints); - - // Get the information for the observations - for (size_t k = 0; k < nrObservations; k++) { - size_t i = 0, j = 0; - float u, v; - is >> i >> j >> u >> v; - data.tracks[j].measurements.emplace_back(i, Point2(u, -v)); - } - - // Get the information for the camera poses - for (size_t i = 0; i < nrPoses; i++) { - // Get the Rodrigues vector - float wx, wy, wz; - is >> wx >> wy >> wz; - Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix - - // Get the translation vector - float tx, ty, tz; - is >> tx >> ty >> tz; - - Pose3 pose = openGL2gtsam(R, tx, ty, tz); - - // Get the focal length and the radial distortion parameters - float f, k1, k2; - is >> f >> k1 >> k2; - Cal3Bundler K(f, k1, k2); - - data.cameras.emplace_back(pose, K); - } - - // Get the information for the 3D points - for (size_t j = 0; j < nrPoints; j++) { - // Get the 3D position - float x, y, z; - is >> x >> y >> z; - SfmTrack &track = data.tracks[j]; - track.p = Point3(x, y, z); - track.r = 0.4f; - track.g = 0.4f; - track.b = 0.4f; - } - - is.close(); - return true; -} - -/* ************************************************************************* */ -SfmData readBal(const string &filename) { - SfmData data; - readBAL(filename, data); - return data; -} - -/* ************************************************************************* */ -bool writeBAL(const string &filename, SfmData &data) { - // Open the output file - ofstream os; - os.open(filename.c_str()); - os.precision(20); - if (!os.is_open()) { - cout << "Error in writeBAL: can not open the file!!" << endl; - return false; - } - - // Write the number of camera poses and 3D points - size_t nrObservations = 0; - for (size_t j = 0; j < data.number_tracks(); j++) { - nrObservations += data.tracks[j].number_measurements(); - } - - // Write observations - os << data.number_cameras() << " " << data.number_tracks() << " " - << nrObservations << endl; - os << endl; - - for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j - const SfmTrack &track = data.tracks[j]; - - for (size_t k = 0; k < track.number_measurements(); - k++) { // for each observation of the 3D point j - size_t i = track.measurements[k].first; // camera id - double u0 = data.cameras[i].calibration().px(); - double v0 = data.cameras[i].calibration().py(); - - if (u0 != 0 || v0 != 0) { - cout << "writeBAL has not been tested for calibration with nonzero " - "(u0,v0)" - << endl; - } - - double pixelBALx = track.measurements[k].second.x() - - u0; // center of image is the origin - double pixelBALy = -(track.measurements[k].second.y() - - v0); // center of image is the origin - Point2 pixelMeasurement(pixelBALx, pixelBALy); - os << i /*camera id*/ << " " << j /*point id*/ << " " - << pixelMeasurement.x() /*u of the pixel*/ << " " - << pixelMeasurement.y() /*v of the pixel*/ << endl; - } - } - os << endl; - - // Write cameras - for (size_t i = 0; i < data.number_cameras(); i++) { // for each camera - Pose3 poseGTSAM = data.cameras[i].pose(); - Cal3Bundler cameraCalibration = data.cameras[i].calibration(); - Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); - os << Rot3::Logmap(poseOpenGL.rotation()) << endl; - os << poseOpenGL.translation().x() << endl; - os << poseOpenGL.translation().y() << endl; - os << poseOpenGL.translation().z() << endl; - os << cameraCalibration.fx() << endl; - os << cameraCalibration.k1() << endl; - os << cameraCalibration.k2() << endl; - os << endl; - } - - // Write the points - for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j - Point3 point = data.tracks[j].p; - os << point.x() << endl; - os << point.y() << endl; - os << point.z() << endl; - os << endl; - } - - os.close(); - return true; -} - -bool writeBALfromValues(const string &filename, const SfmData &data, - Values &values) { - using Camera = PinholeCamera; - SfmData dataValues = data; - - // Store poses or cameras in SfmData - size_t nrPoses = values.count(); - if (nrPoses == - dataValues.number_cameras()) { // we only estimated camera poses - for (size_t i = 0; i < dataValues.number_cameras(); - i++) { // for each camera - Pose3 pose = values.at(X(i)); - Cal3Bundler K = dataValues.cameras[i].calibration(); - Camera camera(pose, K); - dataValues.cameras[i] = camera; - } - } else { - size_t nrCameras = values.count(); - if (nrCameras == dataValues.number_cameras()) { // we only estimated camera - // poses and calibration - for (size_t i = 0; i < nrCameras; i++) { // for each camera - Key cameraKey = i; // symbol('c',i); - Camera camera = values.at(cameraKey); - dataValues.cameras[i] = camera; - } - } else { - cout << "writeBALfromValues: different number of cameras in " - "SfM_dataValues (#cameras " - << dataValues.number_cameras() << ") and values (#cameras " - << nrPoses << ", #poses " << nrCameras << ")!!" << endl; - return false; - } - } - - // Store 3D points in SfmData - size_t nrPoints = values.count(), - nrTracks = dataValues.number_tracks(); - if (nrPoints != nrTracks) { - cout << "writeBALfromValues: different number of points in " - "SfM_dataValues (#points= " - << nrTracks << ") and values (#points " << nrPoints << ")!!" << endl; - } - - for (size_t j = 0; j < nrTracks; j++) { // for each point - Key pointKey = P(j); - if (values.exists(pointKey)) { - Point3 point = values.at(pointKey); - dataValues.tracks[j].p = point; - } else { - dataValues.tracks[j].r = 1.0; - dataValues.tracks[j].g = 0.0; - dataValues.tracks[j].b = 0.0; - dataValues.tracks[j].p = Point3(0, 0, 0); - } - } - - // Write SfmData to file - return writeBAL(filename, dataValues); -} - -Values initialCamerasEstimate(const SfmData &db) { - Values initial; - size_t i = 0; // NO POINTS: j = 0; - for (const SfmCamera &camera : db.cameras) - initial.insert(i++, camera); - return initial; -} - -Values initialCamerasAndPointsEstimate(const SfmData &db) { - Values initial; - size_t i = 0, j = 0; - for (const SfmCamera &camera : db.cameras) - initial.insert((i++), camera); - for (const SfmTrack &track : db.tracks) - initial.insert(P(j++), track.p); - return initial; -} - // Wrapper-friendly versions of parseFactors and parseFactors BetweenFactorPose2s parse2DFactors(const std::string &filename, diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index db5d7d76a..dc450a9f7 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -167,10 +168,11 @@ GTSAM_EXPORT GraphAndValues load2D( * @param kernelFunctionType whether to wrap the noise model in a robust kernel * @return graph and initial values */ -GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, - SharedNoiseModel model = SharedNoiseModel(), size_t maxIndex = 0, bool addNoise = - false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, // - KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); +GTSAM_EXPORT GraphAndValues +load2D(const std::string& filename, SharedNoiseModel model = SharedNoiseModel(), + size_t maxIndex = 0, bool addNoise = false, bool smart = true, + NoiseFormat noiseFormat = NoiseFormatAUTO, // + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /** save 2d graph */ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, @@ -185,8 +187,9 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, * @param kernelFunctionType whether to wrap the noise model in a robust kernel * @return graph and initial values */ -GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D = false, - KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); +GTSAM_EXPORT GraphAndValues +readG2o(const std::string& g2oFile, const bool is3D = false, + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /** * @brief This function writes a g2o file from @@ -206,286 +209,6 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, /// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); -/// A measurement with its camera index -typedef std::pair SfmMeasurement; - -/// Sift index for SfmTrack -typedef std::pair SiftIndex; - -/// Define the structure for the 3D points -struct SfmTrack { - SfmTrack(float r = 0, float g = 0, float b = 0): p(0,0,0), r(r), g(g), b(b) {} - SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0, float b = 0) : p(pt), r(r), g(g), b(b) {} - - Point3 p; ///< 3D position of the point - float r, g, b; ///< RGB color of the 3D point - std::vector measurements; ///< The 2D image projections (id,(u,v)) - std::vector siftIndices; - - /// Get RGB values describing 3d point - const Point3 rgb() const { return Point3(r, g, b); } - - /// Total number of measurements in this track - size_t number_measurements() const { - return measurements.size(); - } - /// Get the measurement (camera index, Point2) at pose index `idx` - SfmMeasurement measurement(size_t idx) const { - return measurements[idx]; - } - /// Get the SIFT feature index corresponding to the measurement at `idx` - SiftIndex siftIndex(size_t idx) const { - return siftIndices[idx]; - } - /// Get 3D point - const Point3& point3() const { - return p; - } - /// Add measurement (camera_idx, Point2) to track - void add_measurement(size_t idx, const gtsam::Point2& m) { - measurements.emplace_back(idx, m); - } - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(p); - ar & BOOST_SERIALIZATION_NVP(r); - ar & BOOST_SERIALIZATION_NVP(g); - ar & BOOST_SERIALIZATION_NVP(b); - ar & BOOST_SERIALIZATION_NVP(measurements); - ar & BOOST_SERIALIZATION_NVP(siftIndices); - } - - /// assert equality up to a tolerance - bool equals(const SfmTrack &sfmTrack, double tol = 1e-9) const { - // check the 3D point - if (!p.isApprox(sfmTrack.p)) { - return false; - } - - // check the RGB values - if (r!=sfmTrack.r || g!=sfmTrack.g || b!=sfmTrack.b) { - return false; - } - - // compare size of vectors for measurements and siftIndices - if (number_measurements() != sfmTrack.number_measurements() || - siftIndices.size() != sfmTrack.siftIndices.size()) { - return false; - } - - // compare measurements (order sensitive) - for (size_t idx = 0; idx < number_measurements(); ++idx) { - SfmMeasurement measurement = measurements[idx]; - SfmMeasurement otherMeasurement = sfmTrack.measurements[idx]; - - if (measurement.first != otherMeasurement.first || - !measurement.second.isApprox(otherMeasurement.second)) { - return false; - } - } - - // compare sift indices (order sensitive) - for (size_t idx = 0; idx < siftIndices.size(); ++idx) { - SiftIndex index = siftIndices[idx]; - SiftIndex otherIndex = sfmTrack.siftIndices[idx]; - - if (index.first != otherIndex.first || - index.second != otherIndex.second) { - return false; - } - } - - return true; - } - - /// print - void print(const std::string& s = "") const { - std::cout << "Track with " << measurements.size(); - std::cout << " measurements of point " << p << std::endl; - } -}; - -/* ************************************************************************* */ -/// traits -template<> -struct traits : public Testable { -}; - - -/// Define the structure for the camera poses -typedef PinholeCamera SfmCamera; - -/// Define the structure for SfM data -struct SfmData { - std::vector cameras; ///< Set of cameras - std::vector tracks; ///< Sparse set of points - size_t number_cameras() const { - return cameras.size(); - } - /// The number of reconstructed 3D points - size_t number_tracks() const { - return tracks.size(); - } - /// The camera pose at frame index `idx` - SfmCamera camera(size_t idx) const { - return cameras[idx]; - } - /// The track formed by series of landmark measurements - SfmTrack track(size_t idx) const { - return tracks[idx]; - } - /// Add a track to SfmData - void add_track(const SfmTrack& t) { - tracks.push_back(t); - } - /// Add a camera to SfmData - void add_camera(const SfmCamera& cam) { - cameras.push_back(cam); - } - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(cameras); - ar & BOOST_SERIALIZATION_NVP(tracks); - } - - /// @} - /// @name Testable - /// @{ - - /// assert equality up to a tolerance - bool equals(const SfmData &sfmData, double tol = 1e-9) const { - // check number of cameras and tracks - if (number_cameras() != sfmData.number_cameras() || - number_tracks() != sfmData.number_tracks()) { - return false; - } - - // check each camera - for (size_t i = 0; i < number_cameras(); ++i) { - if (!camera(i).equals(sfmData.camera(i), tol)) { - return false; - } - } - - // check each track - for (size_t j = 0; j < number_tracks(); ++j) { - if (!track(j).equals(sfmData.track(j), tol)) { - return false; - } - } - - return true; - } - - /// print - void print(const std::string& s = "") const { - std::cout << "Number of cameras = " << number_cameras() << std::endl; - std::cout << "Number of tracks = " << number_tracks() << std::endl; - } -}; - -/* ************************************************************************* */ -/// traits -template<> -struct traits : public Testable { -}; - -/** - * @brief This function parses a bundler output file and stores the data into a - * SfmData structure - * @param filename The name of the bundler file - * @param data SfM structure where the data is stored - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData &data); - -/** - * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a - * SfmData structure - * @param filename The name of the BAL file - * @param data SfM structure where the data is stored - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData &data); - -/** - * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data - * as a SfmData structure. Mainly used by wrapped code. - * @param filename The name of the BAL file. - * @return SfM structure where the data is stored. - */ -GTSAM_EXPORT SfmData readBal(const std::string& filename); - -/** - * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a - * SfmData structure - * @param filename The name of the BAL file to write - * @param data SfM structure where the data is stored - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData &data); - -/** - * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a - * SfmData structure and a value structure (measurements are the same as the SfM input data, - * while camera poses and values are read from Values) - * @param filename The name of the BAL file to write - * @param data SfM structure where the data is stored - * @param values structure where the graph values are stored (values can be either Pose3 or PinholeCamera for the - * cameras, and should be Point3 for the 3D points). Note that the current version - * assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1 - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, - const SfmData &data, Values& values); - -/** - * @brief This function converts an openGL camera pose to an GTSAM camera pose - * @param R rotation in openGL - * @param tx x component of the translation in openGL - * @param ty y component of the translation in openGL - * @param tz z component of the translation in openGL - * @return Pose3 in GTSAM format - */ -GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz); - -/** - * @brief This function converts a GTSAM camera pose to an openGL camera pose - * @param R rotation in GTSAM - * @param tx x component of the translation in GTSAM - * @param ty y component of the translation in GTSAM - * @param tz z component of the translation in GTSAM - * @return Pose3 in openGL format - */ -GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz); - -/** - * @brief This function converts a GTSAM camera pose to an openGL camera pose - * @param PoseGTSAM pose in GTSAM format - * @return Pose3 in openGL format - */ -GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM); - -/** - * @brief This function creates initial values for cameras from db - * @param SfmData - * @return Values - */ -GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db); - -/** - * @brief This function creates initial values for cameras and points from db - * @param SfmData - * @return Values - */ -GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); - // Wrapper-friendly versions of parseFactors and parseFactors using BetweenFactorPose2s = std::vector::shared_ptr>; GTSAM_EXPORT BetweenFactorPose2s diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 602b2afe3..4e943253e 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -209,61 +209,27 @@ virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor { #include -class SfmTrack { - SfmTrack(); - SfmTrack(const gtsam::Point3& pt); - const Point3& point3() const; - - double r; - double g; - double b; - - std::vector> measurements; - - size_t number_measurements() const; - pair measurement(size_t idx) const; - pair siftIndex(size_t idx) const; - void add_measurement(size_t idx, const gtsam::Point2& m); - - // enabling serialization functionality - void serialize() const; - - // enabling function to compare objects - bool equals(const gtsam::SfmTrack& expected, double tol) const; +enum NoiseFormat { + NoiseFormatG2O, + NoiseFormatTORO, + NoiseFormatGRAPH, + NoiseFormatCOV, + NoiseFormatAUTO }; -class SfmData { - SfmData(); - size_t number_cameras() const; - size_t number_tracks() const; - gtsam::PinholeCamera camera(size_t idx) const; - gtsam::SfmTrack track(size_t idx) const; - void add_track(const gtsam::SfmTrack& t); - void add_camera(const gtsam::SfmCamera& cam); - - // enabling serialization functionality - void serialize() const; - - // enabling function to compare objects - bool equals(const gtsam::SfmData& expected, double tol) const; +enum KernelFunctionType { + KernelFunctionTypeNONE, + KernelFunctionTypeHUBER, + KernelFunctionTypeTUKEY }; -gtsam::SfmData readBal(string filename); -bool writeBAL(string filename, gtsam::SfmData& data); -gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); -gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); +pair load2D( + string filename, gtsam::noiseModel::Diagonal* model = nullptr, + size_t maxIndex = 0, bool addNoise = false, bool smart = true, + gtsam::NoiseFormat noiseFormat = gtsam::NoiseFormat::NoiseFormatAUTO, + gtsam::KernelFunctionType kernelFunctionType = + gtsam::KernelFunctionType::KernelFunctionTypeNONE); -pair load2D( - string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, - bool addNoise, bool smart); -pair load2D( - string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, - bool addNoise); -pair load2D( - string filename, gtsam::noiseModel::Diagonal* model, int maxIndex); -pair load2D( - string filename, gtsam::noiseModel::Diagonal* model); -pair load2D(string filename); void save2D(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, string filename); @@ -290,9 +256,10 @@ gtsam::BetweenFactorPose3s parse3DFactors(string filename); pair load3D(string filename); -pair readG2o(string filename); -pair readG2o(string filename, - bool is3D); +pair readG2o( + string filename, const bool is3D = false, + gtsam::KernelFunctionType kernelFunctionType = + gtsam::KernelFunctionType::KernelFunctionTypeNONE); void writeG2o(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& estimate, string filename); @@ -323,6 +290,8 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor { KarcherMeanFactor(const gtsam::KeyVector& keys); }; +gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations); + #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, size_t d); diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index aad9124c5..be638d51a 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -151,27 +151,6 @@ TEST(dataSet, load2DVictoriaPark) { EXPECT_LONGS_EQUAL(L(5), graph->at(4)->keys()[1]); } -/* ************************************************************************* */ -TEST( dataSet, Balbianello) -{ - ///< The structure where we will save the SfM data - const string filename = findExampleDataFile("Balbianello"); - SfmData mydata; - CHECK(readBundler(filename, mydata)); - - // Check number of things - EXPECT_LONGS_EQUAL(5,mydata.number_cameras()); - EXPECT_LONGS_EQUAL(544,mydata.number_tracks()); - const SfmTrack& track0 = mydata.tracks[0]; - EXPECT_LONGS_EQUAL(3,track0.number_measurements()); - - // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); - const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; - EXPECT(assert_equal(expected,actual,1)); -} - /* ************************************************************************* */ TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); @@ -461,160 +440,6 @@ TEST( dataSet, writeG2o3DNonDiagonalNoise) EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4)); } -/* ************************************************************************* */ -TEST( dataSet, readBAL_Dubrovnik) -{ - ///< The structure where we will save the SfM data - const string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData mydata; - CHECK(readBAL(filename, mydata)); - - // Check number of things - EXPECT_LONGS_EQUAL(3,mydata.number_cameras()); - EXPECT_LONGS_EQUAL(7,mydata.number_tracks()); - const SfmTrack& track0 = mydata.tracks[0]; - EXPECT_LONGS_EQUAL(3,track0.number_measurements()); - - // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); - const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; - EXPECT(assert_equal(expected,actual,12)); -} - -/* ************************************************************************* */ -TEST( dataSet, openGL2gtsam) -{ - Vector3 rotVec(0.2, 0.7, 1.1); - Rot3 R = Rot3::Expmap(rotVec); - Point3 t = Point3(0.0,0.0,0.0); - Pose3 poseGTSAM = Pose3(R,t); - - Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z()); - - Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); //columns! - Rot3 cRw( - r1.x(), r2.x(), r3.x(), - -r1.y(), -r2.y(), -r3.y(), - -r1.z(), -r2.z(), -r3.z()); - Rot3 wRc = cRw.inverse(); - Pose3 actual = Pose3(wRc,t); - - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( dataSet, gtsam2openGL) -{ - Vector3 rotVec(0.2, 0.7, 1.1); - Rot3 R = Rot3::Expmap(rotVec); - Point3 t = Point3(1.0,20.0,10.0); - Pose3 actual = Pose3(R,t); - Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z()); - - Pose3 expected = gtsam2openGL(poseGTSAM); - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( dataSet, writeBAL_Dubrovnik) -{ - ///< Read a file using the unit tested readBAL - const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData readData; - readBAL(filenameToRead, readData); - - // Write readData to file filenameToWrite - const string filenameToWrite = createRewrittenFileName(filenameToRead); - CHECK(writeBAL(filenameToWrite, readData)); - - // Read what we wrote - SfmData writtenData; - CHECK(readBAL(filenameToWrite, writtenData)); - - // Check that what we read is the same as what we wrote - EXPECT_LONGS_EQUAL(readData.number_cameras(),writtenData.number_cameras()); - EXPECT_LONGS_EQUAL(readData.number_tracks(),writtenData.number_tracks()); - - for (size_t i = 0; i < readData.number_cameras(); i++){ - PinholeCamera expectedCamera = writtenData.cameras[i]; - PinholeCamera actualCamera = readData.cameras[i]; - EXPECT(assert_equal(expectedCamera,actualCamera)); - } - - for (size_t j = 0; j < readData.number_tracks(); j++){ - // check point - SfmTrack expectedTrack = writtenData.tracks[j]; - SfmTrack actualTrack = readData.tracks[j]; - Point3 expectedPoint = expectedTrack.p; - Point3 actualPoint = actualTrack.p; - EXPECT(assert_equal(expectedPoint,actualPoint)); - - // check rgb - Point3 expectedRGB = Point3( expectedTrack.r, expectedTrack.g, expectedTrack.b ); - Point3 actualRGB = Point3( actualTrack.r, actualTrack.g, actualTrack.b); - EXPECT(assert_equal(expectedRGB,actualRGB)); - - // check measurements - for (size_t k = 0; k < actualTrack.number_measurements(); k++){ - EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first); - EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second)); - } - } -} - - -/* ************************************************************************* */ -TEST( dataSet, writeBALfromValues_Dubrovnik){ - - ///< Read a file using the unit tested readBAL - const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData readData; - readBAL(filenameToRead, readData); - - Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.3,0.1,0.3)); - - Values value; - for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera - Pose3 pose = poseChange.compose(readData.cameras[i].pose()); - value.insert(X(i), pose); - } - for(size_t j=0; j < readData.number_tracks(); j++){ // for each point - Point3 point = poseChange.transformFrom( readData.tracks[j].p ); - value.insert(P(j), point); - } - - // Write values and readData to a file - const string filenameToWrite = createRewrittenFileName(filenameToRead); - writeBALfromValues(filenameToWrite, readData, value); - - // Read the file we wrote - SfmData writtenData; - readBAL(filenameToWrite, writtenData); - - // Check that the reprojection errors are the same and the poses are correct - // Check number of things - EXPECT_LONGS_EQUAL(3,writtenData.number_cameras()); - EXPECT_LONGS_EQUAL(7,writtenData.number_tracks()); - const SfmTrack& track0 = writtenData.tracks[0]; - EXPECT_LONGS_EQUAL(3,track0.number_measurements()); - - // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); - const SfmCamera& camera0 = writtenData.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; - EXPECT(assert_equal(expected,actual,12)); - - Pose3 expectedPose = camera0.pose(); - Pose3 actualPose = value.at(X(0)); - EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); - - Point3 expectedPoint = track0.p; - Point3 actualPoint = value.at(P(0)); - EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); -} - - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 03775a70f..ef22bad2a 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include using namespace std::placeholders; @@ -34,8 +35,7 @@ gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse(); namespace example1 { const string filename = findExampleDataFile("18pointExample1.txt"); -SfmData data; -bool readOK = readBAL(filename, data); +SfmData data = SfmData::FromBalFile(filename); Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Point3 c1Tc2 = data.cameras[1].pose().translation(); // TODO: maybe default value not good; assert with 0th @@ -53,8 +53,6 @@ Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); } //************************************************************************* TEST(EssentialMatrixFactor, testData) { - CHECK(readOK); - // Check E matrix Matrix expected(3, 3); expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0; @@ -538,8 +536,7 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { namespace example2 { const string filename = findExampleDataFile("5pointExample2.txt"); -SfmData data; -bool readOK = readBAL(filename, data); +SfmData data = SfmData::FromBalFile(filename); Rot3 aRb = data.cameras[1].pose().rotation(); Point3 aTb = data.cameras[1].pose().translation(); EssentialMatrix trueE(aRb, Unit3(aTb)); @@ -632,14 +629,14 @@ TEST(EssentialMatrixFactor2, extraMinimization) { // We start with a factor graph and add constraints to it // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; - for (size_t i = 0; i < data.number_tracks(); i++) + for (size_t i = 0; i < data.numberTracks(); i++) graph.emplace_shared(100, i, pA(i), pB(i), model2, K); // Check error at ground truth Values truth; truth.insert(100, trueE); - for (size_t i = 0; i < data.number_tracks(); i++) { + for (size_t i = 0; i < data.numberTracks(); i++) { Point3 P1 = data.tracks[i].p; truth.insert(i, double(baseline / P1.z())); } @@ -654,7 +651,7 @@ TEST(EssentialMatrixFactor2, extraMinimization) { // Check result EssentialMatrix actual = result.at(100); EXPECT(assert_equal(trueE, actual, 1e-1)); - for (size_t i = 0; i < data.number_tracks(); i++) + for (size_t i = 0; i < data.numberTracks(); i++) EXPECT_DOUBLES_EQUAL(truth.at(i), result.at(i), 1e-1); // Check error at result diff --git a/gtsam/slam/tests/testSerializationDataset.cpp b/gtsam/slam/tests/testSerializationDataset.cpp index 6ef82f07f..dcac3d47e 100644 --- a/gtsam/slam/tests/testSerializationDataset.cpp +++ b/gtsam/slam/tests/testSerializationDataset.cpp @@ -16,6 +16,7 @@ * @date Jan 1, 2021 */ +#include #include #include @@ -29,8 +30,7 @@ using namespace gtsam::serializationTestHelpers; TEST(dataSet, sfmDataSerialization) { // Test the serialization of SfmData const string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData mydata; - CHECK(readBAL(filename, mydata)); + SfmData mydata = SfmData::FromBalFile(filename); // round-trip equality check on serialization and subsequent deserialization EXPECT(equalsObj(mydata)); @@ -42,8 +42,7 @@ TEST(dataSet, sfmDataSerialization) { TEST(dataSet, sfmTrackSerialization) { // Test the serialization of SfmTrack const string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData mydata; - CHECK(readBAL(filename, mydata)); + SfmData mydata = SfmData::FromBalFile(filename); SfmTrack track = mydata.track(0); diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 8c9345147..dd66e7a73 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -566,7 +566,13 @@ virtual class FixedLagSmoother { gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; double smootherLag() const; - gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps); + gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors, + const gtsam::Values &newTheta, + const gtsam::FixedLagSmootherKeyTimestampMap ×tamps); + gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors, + const gtsam::Values &newTheta, + const gtsam::FixedLagSmootherKeyTimestampMap ×tamps, + const gtsam::FactorIndices &factorsToRemove); gtsam::Values calculateEstimate() const; }; @@ -576,6 +582,8 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { BatchFixedLagSmoother(double smootherLag); BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); + void print(string s = "BatchFixedLagSmoother:\n") const; + gtsam::LevenbergMarquardtParams params() const; template diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 79c05a01a..4079dbb23 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -113,6 +113,9 @@ public: /// Get results of latest isam2 update const ISAM2Result& getISAM2Result() const{ return isamResult_; } + /// Get the iSAM2 object which is used for the inference internally + const ISAM2& getISAM2() const { return isam_; } + protected: /** Create default parameters */ diff --git a/matlab/+gtsam/VisualISAMInitialize.m b/matlab/+gtsam/VisualISAMInitialize.m index 9b834e3e1..560503345 100644 --- a/matlab/+gtsam/VisualISAMInitialize.m +++ b/matlab/+gtsam/VisualISAMInitialize.m @@ -7,7 +7,7 @@ import gtsam.* %% Initialize iSAM params = gtsam.ISAM2Params; if options.alwaysRelinearize - params.setRelinearizeSkip(1); + params.relinearizeSkip = 1; end isam = ISAM2(params); diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 749ad870a..1755e2075 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -68,6 +68,8 @@ set(interface_files ${GTSAM_SOURCE_DIR}/gtsam/gtsam.i ${GTSAM_SOURCE_DIR}/gtsam/base/base.i ${GTSAM_SOURCE_DIR}/gtsam/basis/basis.i + ${PROJECT_SOURCE_DIR}/gtsam/inference/inference.i + ${PROJECT_SOURCE_DIR}/gtsam/discrete/discrete.i ${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i ${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i diff --git a/matlab/gtsam_examples/IMUKittiExampleGPS.m b/matlab/gtsam_examples/IMUKittiExampleGPS.m index 530c3382c..b350618e5 100755 --- a/matlab/gtsam_examples/IMUKittiExampleGPS.m +++ b/matlab/gtsam_examples/IMUKittiExampleGPS.m @@ -52,7 +52,7 @@ IMU_params.setOmegaCoriolis(w_coriolis); %% Solver object isamParams = ISAM2Params; isamParams.setFactorization('CHOLESKY'); -isamParams.setRelinearizeSkip(10); +isamParams.relinearizeSkip = 10; isam = gtsam.ISAM2(isamParams); newFactors = NonlinearFactorGraph; newValues = Values; diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison.m b/matlab/unstable_examples/+imuSimulator/IMUComparison.m index ccc975d84..b753916c6 100644 --- a/matlab/unstable_examples/+imuSimulator/IMUComparison.m +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison.m @@ -46,7 +46,7 @@ posesIMUbody(1).R = poses(1).R; %% Solver object isamParams = ISAM2Params; -isamParams.setRelinearizeSkip(1); +isamParams.relinearizeSkip = 1; isam = gtsam.ISAM2(isamParams); initialValues = Values; diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m index 6adc8e9dc..689d8a3f5 100644 --- a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m @@ -34,7 +34,7 @@ poses(1).R = currentPoseGlobal.rotation.matrix; %% Solver object isamParams = ISAM2Params; -isamParams.setRelinearizeSkip(1); +isamParams.relinearizeSkip = 1; isam = gtsam.ISAM2(isamParams); sigma_init_x = 1.0; diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index 61dc78d96..dd276e2c1 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -119,7 +119,7 @@ h = figure; % Solver object isamParams = ISAM2Params; isamParams.setFactorization('CHOLESKY'); -isamParams.setRelinearizeSkip(10); +isamParams.relinearizeSkip = 10; isam = gtsam.ISAM2(isamParams); newFactors = NonlinearFactorGraph; newValues = Values; diff --git a/matlab/unstable_examples/IMUKittiExampleAdvanced.m b/matlab/unstable_examples/IMUKittiExampleAdvanced.m index cb13eacee..b09764ec0 100644 --- a/matlab/unstable_examples/IMUKittiExampleAdvanced.m +++ b/matlab/unstable_examples/IMUKittiExampleAdvanced.m @@ -82,7 +82,7 @@ w_coriolis = [0;0;0]; %% Solver object isamParams = ISAM2Params; isamParams.setFactorization('QR'); -isamParams.setRelinearizeSkip(1); +isamParams.relinearizeSkip = 1; isam = gtsam.ISAM2(isamParams); newFactors = NonlinearFactorGraph; newValues = Values; diff --git a/matlab/unstable_examples/IMUKittiExampleVO.m b/matlab/unstable_examples/IMUKittiExampleVO.m index f35d36512..4183e439a 100644 --- a/matlab/unstable_examples/IMUKittiExampleVO.m +++ b/matlab/unstable_examples/IMUKittiExampleVO.m @@ -58,7 +58,7 @@ w_coriolis = [0;0;0]; %% Solver object isamParams = ISAM2Params; isamParams.setFactorization('CHOLESKY'); -isamParams.setRelinearizeSkip(10); +isamParams.relinearizeSkip = 10; isam = gtsam.ISAM2(isamParams); newFactors = NonlinearFactorGraph; newValues = Values; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 56062c5be..85ddc7b6d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -45,6 +45,7 @@ set(ignore gtsam::Point3Pairs gtsam::Pose3Pairs gtsam::Pose3Vector + gtsam::Rot3Vector gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 gtsam::DiscreteKey diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py index 8b6b4fdf0..d00f633ba 100644 --- a/python/gtsam/examples/IMUKittiExampleGPS.py +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -203,7 +203,7 @@ def optimize(gps_measurements: List[GpsMeasurement], # Set ISAM2 parameters and create ISAM2 solver object isam_params = gtsam.ISAM2Params() isam_params.setFactorization("CHOLESKY") - isam_params.setRelinearizeSkip(10) + isam_params.relinearizeSkip = 10 isam = gtsam.ISAM2(isam_params) diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index c70dbfa6f..3a8de0317 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -111,7 +111,7 @@ def Pose2SLAM_ISAM2_example(): # update calls are required to perform the relinearization. parameters = gtsam.ISAM2Params() parameters.setRelinearizeThreshold(0.1) - parameters.setRelinearizeSkip(1) + parameters.relinearizeSkip = 1 isam = gtsam.ISAM2(parameters) # Create the ground truth odometry measurements of the robot during the trajectory. diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index 59b9fb2ac..cb71813c5 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -140,7 +140,7 @@ def Pose3_ISAM2_example(): # update calls are required to perform the relinearization. parameters = gtsam.ISAM2Params() parameters.setRelinearizeThreshold(0.1) - parameters.setRelinearizeSkip(1) + parameters.relinearizeSkip = 1 isam = gtsam.ISAM2(parameters) # Create the ground truth poses of the robot trajectory. diff --git a/python/gtsam/examples/SFMExample.py b/python/gtsam/examples/SFMExample.py index f0c4c82ba..87bb3cb87 100644 --- a/python/gtsam/examples/SFMExample.py +++ b/python/gtsam/examples/SFMExample.py @@ -8,7 +8,6 @@ See LICENSE for the license information A structure-from-motion problem on a simulated dataset """ -from __future__ import print_function import gtsam import matplotlib.pyplot as plt @@ -89,7 +88,7 @@ def main(): point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) factor = PriorFactorPoint3(L(0), points[0], point_noise) graph.push_back(factor) - graph.print_('Factor Graph:\n') + graph.print('Factor Graph:\n') # Create the data structure to hold the initial estimate to the solution # Intentionally initialize the variables off from the ground truth @@ -100,7 +99,7 @@ def main(): for j, point in enumerate(points): transformed_point = point + 0.1*np.random.randn(3) initial_estimate.insert(L(j), transformed_point) - initial_estimate.print_('Initial Estimates:\n') + initial_estimate.print('Initial Estimates:\n') # Optimize the graph and print results params = gtsam.DoglegParams() @@ -108,7 +107,7 @@ def main(): optimizer = DoglegOptimizer(graph, initial_estimate, params) print('Optimizing:') result = optimizer.optimize() - result.print_('Final results:\n') + result.print('Final results:\n') print('initial error = {}'.format(graph.error(initial_estimate))) print('final error = {}'.format(graph.error(result))) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index f3e48c3c3..3d71590a9 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -15,9 +15,9 @@ import logging import sys import gtsam -from gtsam import (GeneralSFMFactorCal3Bundler, +from gtsam import (GeneralSFMFactorCal3Bundler, SfmData, PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3) -from gtsam.symbol_shorthand import C, P # type: ignore +from gtsam.symbol_shorthand import P # type: ignore from gtsam.utils import plot # type: ignore from matplotlib import pyplot as plt @@ -26,13 +26,12 @@ logging.basicConfig(stream=sys.stdout, level=logging.INFO) DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre" -def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None: +def plot_scene(scene_data: SfmData, result: gtsam.Values) -> None: """Plot the SFM results.""" plot_vals = gtsam.Values() - for cam_idx in range(scene_data.number_cameras()): - plot_vals.insert(C(cam_idx), - result.atPinholeCameraCal3Bundler(C(cam_idx)).pose()) - for j in range(scene_data.number_tracks()): + for i in range(scene_data.numberCameras()): + plot_vals.insert(i, result.atPinholeCameraCal3Bundler(i).pose()) + for j in range(scene_data.numberTracks()): plot_vals.insert(P(j), result.atPoint3(P(j))) plot.plot_3d_points(0, plot_vals, linespec="g.") @@ -46,9 +45,9 @@ def run(args: argparse.Namespace) -> None: input_file = args.input_file # Load the SfM data from file - scene_data = gtsam.readBal(input_file) - logging.info("read %d tracks on %d cameras\n", scene_data.number_tracks(), - scene_data.number_cameras()) + scene_data = SfmData.FromBalFile(input_file) + logging.info("read %d tracks on %d cameras\n", scene_data.numberTracks(), + scene_data.numberCameras()) # Create a factor graph graph = gtsam.NonlinearFactorGraph() @@ -57,19 +56,19 @@ def run(args: argparse.Namespace) -> None: noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v # Add measurements to the factor graph - for j in range(scene_data.number_tracks()): + for j in range(scene_data.numberTracks()): track = scene_data.track(j) # SfmTrack # retrieve the SfmMeasurement objects - for m_idx in range(track.number_measurements()): + for m_idx in range(track.numberMeasurements()): # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(m_idx) # note use of shorthand symbols C and P - graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) + graph.add(GeneralSFMFactorCal3Bundler(uv, noise, i, P(j))) # Add a prior on pose x1. This indirectly specifies where the origin is. graph.push_back( PriorFactorPinholeCameraCal3Bundler( - C(0), scene_data.camera(0), + 0, scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1))) # Also add a prior on the position of the first landmark to fix the scale graph.push_back( @@ -82,13 +81,13 @@ def run(args: argparse.Namespace) -> None: i = 0 # add each PinholeCameraCal3Bundler - for cam_idx in range(scene_data.number_cameras()): - camera = scene_data.camera(cam_idx) - initial.insert(C(i), camera) + for i in range(scene_data.numberCameras()): + camera = scene_data.camera(i) + initial.insert(i, camera) i += 1 # add each SfmTrack - for j in range(scene_data.number_tracks()): + for j in range(scene_data.numberTracks()): track = scene_data.track(j) initial.insert(P(j), track.point3()) diff --git a/python/gtsam/examples/SimpleRotation.py b/python/gtsam/examples/SimpleRotation.py index 0fef261f8..3d5fd9e45 100644 --- a/python/gtsam/examples/SimpleRotation.py +++ b/python/gtsam/examples/SimpleRotation.py @@ -31,7 +31,7 @@ def main(): - A measurement model with the correct dimensionality for the factor """ prior = gtsam.Rot2.fromAngle(np.deg2rad(30)) - prior.print_('goal angle') + prior.print('goal angle') model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1)) key = X(1) factor = gtsam.PriorFactorRot2(key, prior, model) @@ -48,7 +48,7 @@ def main(): """ graph = gtsam.NonlinearFactorGraph() graph.push_back(factor) - graph.print_('full graph') + graph.print('full graph') """ Step 3: Create an initial estimate @@ -65,7 +65,7 @@ def main(): """ initial = gtsam.Values() initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20))) - initial.print_('initial estimate') + initial.print('initial estimate') """ Step 4: Optimize @@ -77,7 +77,7 @@ def main(): with the final state of the optimization. """ result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize() - result.print_('final result') + result.print('final result') if __name__ == '__main__': diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py index bacf510ec..4b480fab7 100644 --- a/python/gtsam/examples/VisualISAM2Example.py +++ b/python/gtsam/examples/VisualISAM2Example.py @@ -81,7 +81,7 @@ def visual_ISAM2_example(): # will approach the batch result. parameters = gtsam.ISAM2Params() parameters.setRelinearizeThreshold(0.01) - parameters.setRelinearizeSkip(1) + parameters.relinearizeSkip = 1 isam = gtsam.ISAM2(parameters) # Create a Factor Graph and Values to hold the new data diff --git a/python/gtsam/examples/VisualISAMExample.py b/python/gtsam/examples/VisualISAMExample.py index f99d3f3e6..9691b3c46 100644 --- a/python/gtsam/examples/VisualISAMExample.py +++ b/python/gtsam/examples/VisualISAMExample.py @@ -10,8 +10,6 @@ A visualSLAM example for the structure-from-motion problem on a simulated datase This version uses iSAM to solve the problem incrementally """ -from __future__ import print_function - import numpy as np import gtsam from gtsam.examples import SFMdata @@ -94,7 +92,7 @@ def main(): current_estimate = isam.estimate() print('*' * 50) print('Frame {}:'.format(i)) - current_estimate.print_('Current estimate: ') + current_estimate.print('Current estimate: ') # Clear the factor graph and values for the next iteration graph.resize(0) diff --git a/python/gtsam/preamble/inference.h b/python/gtsam/preamble/inference.h index 320e0ac71..4106c794a 100644 --- a/python/gtsam/preamble/inference.h +++ b/python/gtsam/preamble/inference.h @@ -11,5 +11,4 @@ * mutations on Python side will not be reflected on C++. */ -#include - +#include \ No newline at end of file diff --git a/python/gtsam/preamble/slam.h b/python/gtsam/preamble/slam.h index 34dbb4b7a..f7bf5863c 100644 --- a/python/gtsam/preamble/slam.h +++ b/python/gtsam/preamble/slam.h @@ -15,3 +15,4 @@ PYBIND11_MAKE_OPAQUE( std::vector > >); PYBIND11_MAKE_OPAQUE( std::vector > >); +PYBIND11_MAKE_OPAQUE(gtsam::Rot3Vector); diff --git a/python/gtsam/specializations/slam.h b/python/gtsam/specializations/slam.h index 198485a72..6a439c370 100644 --- a/python/gtsam/specializations/slam.h +++ b/python/gtsam/specializations/slam.h @@ -12,8 +12,9 @@ */ py::bind_vector< - std::vector > > >( + std::vector>>>( m_, "BetweenFactorPose3s"); py::bind_vector< - std::vector > > >( + std::vector>>>( m_, "BetweenFactorPose2s"); +py::bind_vector(m_, "Rot3Vector"); diff --git a/python/gtsam/tests/test_DiscreteBayesNet.py b/python/gtsam/tests/test_DiscreteBayesNet.py index 3ae3b625c..74191dcc7 100644 --- a/python/gtsam/tests/test_DiscreteBayesNet.py +++ b/python/gtsam/tests/test_DiscreteBayesNet.py @@ -12,7 +12,9 @@ Author: Frank Dellaert # pylint: disable=no-name-in-module, invalid-name import unittest +import textwrap +import gtsam from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph, DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering) from gtsam.utils.test_case import GtsamTestCase @@ -126,6 +128,39 @@ class TestDiscreteBayesNet(GtsamTestCase): actual = fragment.sample(given) self.assertEqual(len(actual), 5) + def test_dot(self): + """Check that dot works with position hints.""" + fragment = DiscreteBayesNet() + fragment.add(Either, [Tuberculosis, LungCancer], "F T T T") + MyAsia = gtsam.symbol('a', 0), 2 # use a symbol! + fragment.add(Tuberculosis, [MyAsia], "99/1 95/5") + fragment.add(LungCancer, [Smoking], "99/1 90/10") + + # Make sure we can *update* position hints + writer = gtsam.DotWriter() + ph: dict = writer.positionHints + ph.update({'a': 2}) # hint at symbol position + writer.positionHints = ph + + # Check the output of dot + actual = fragment.dot(writer=writer) + expected_result = """\ + digraph { + size="5,5"; + + var3[label="3"]; + var4[label="4"]; + var5[label="5"]; + var6[label="6"]; + vara0[label="a0", pos="0,2!"]; + + var4->var6 + vara0->var3 + var3->var5 + var6->var5 + }""" + self.assertEqual(actual, textwrap.dedent(expected_result)) + if __name__ == "__main__": unittest.main() diff --git a/python/gtsam/tests/test_GaussianBayesNet.py b/python/gtsam/tests/test_GaussianBayesNet.py new file mode 100644 index 000000000..e4d396cfe --- /dev/null +++ b/python/gtsam/tests/test_GaussianBayesNet.py @@ -0,0 +1,53 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for Gaussian Bayes Nets. +Author: Frank Dellaert +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import gtsam +import numpy as np +from gtsam import GaussianBayesNet, GaussianConditional +from gtsam.utils.test_case import GtsamTestCase + +# some keys +_x_ = 11 +_y_ = 22 +_z_ = 33 + + +def smallBayesNet(): + """Create a small Bayes Net for testing""" + bayesNet = GaussianBayesNet() + I_1x1 = np.eye(1, dtype=float) + bayesNet.push_back(GaussianConditional( + _x_, [9.0], I_1x1, _y_, I_1x1)) + bayesNet.push_back(GaussianConditional(_y_, [5.0], I_1x1)) + return bayesNet + + +class TestGaussianBayesNet(GtsamTestCase): + """Tests for Gaussian Bayes nets.""" + + def test_matrix(self): + """Test matrix method""" + R, d = smallBayesNet().matrix() # get matrix and RHS + R1 = np.array([ + [1.0, 1.0], + [0.0, 1.0]]) + d1 = np.array([9.0, 5.0]) + np.testing.assert_equal(R, R1) + np.testing.assert_equal(d, d1) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/gtsam/tests/test_GraphvizFormatting.py b/python/gtsam/tests/test_GraphvizFormatting.py index 5962366ef..ecdc23b45 100644 --- a/python/gtsam/tests/test_GraphvizFormatting.py +++ b/python/gtsam/tests/test_GraphvizFormatting.py @@ -78,7 +78,7 @@ class TestGraphvizFormatting(GtsamTestCase): graphviz_formatting.paperHorizontalAxis = gtsam.GraphvizFormatting.Axis.X graphviz_formatting.paperVerticalAxis = gtsam.GraphvizFormatting.Axis.Y self.assertEqual(self.graph.dot(self.values, - formatting=graphviz_formatting), + writer=graphviz_formatting), textwrap.dedent(expected_result)) def test_factor_points(self): @@ -100,7 +100,7 @@ class TestGraphvizFormatting(GtsamTestCase): graphviz_formatting.plotFactorPoints = False self.assertEqual(self.graph.dot(self.values, - formatting=graphviz_formatting), + writer=graphviz_formatting), textwrap.dedent(expected_result)) def test_width_height(self): @@ -127,7 +127,7 @@ class TestGraphvizFormatting(GtsamTestCase): graphviz_formatting.figureHeightInches = 10 self.assertEqual(self.graph.dot(self.values, - formatting=graphviz_formatting), + writer=graphviz_formatting), textwrap.dedent(expected_result)) diff --git a/python/gtsam/tests/test_KarcherMeanFactor.py b/python/gtsam/tests/test_KarcherMeanFactor.py index a315a506c..f4ec64283 100644 --- a/python/gtsam/tests/test_KarcherMeanFactor.py +++ b/python/gtsam/tests/test_KarcherMeanFactor.py @@ -15,27 +15,15 @@ import unittest import gtsam import numpy as np +from gtsam import Rot3 from gtsam.utils.test_case import GtsamTestCase KEY = 0 MODEL = gtsam.noiseModel.Unit.Create(3) -def find_Karcher_mean_Rot3(rotations): - """Find the Karcher mean of given values.""" - # Cost function C(R) = \sum PriorFactor(R_i)::error(R) - # No closed form solution. - graph = gtsam.NonlinearFactorGraph() - for R in rotations: - graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) - initial = gtsam.Values() - initial.insert(KEY, gtsam.Rot3()) - result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() - return result.atRot3(KEY) - - # Rot3 version -R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0])) +R = Rot3.Expmap(np.array([0.1, 0, 0])) class TestKarcherMean(GtsamTestCase): @@ -43,11 +31,23 @@ class TestKarcherMean(GtsamTestCase): def test_find(self): # Check that optimizing for Karcher mean (which minimizes Between distance) # gets correct result. - rotations = {R, R.inverse()} - expected = gtsam.Rot3() - actual = find_Karcher_mean_Rot3(rotations) + rotations = gtsam.Rot3Vector([R, R.inverse()]) + expected = Rot3() + actual = gtsam.FindKarcherMean(rotations) self.gtsamAssertEquals(expected, actual) + def test_find_karcher_mean_identity(self): + """Averaging 3 identity rotations should yield the identity.""" + a1Rb1 = Rot3() + a2Rb2 = Rot3() + a3Rb3 = Rot3() + + aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3]) + aRb_expected = Rot3() + + aRb = gtsam.FindKarcherMean(aRb_list) + self.gtsamAssertEquals(aRb, aRb_expected) + def test_factor(self): """Check that the InnerConstraint factor leaves the mean unchanged.""" # Make a graph with two variables, one between, and one InnerConstraint @@ -66,11 +66,11 @@ class TestKarcherMean(GtsamTestCase): initial = gtsam.Values() initial.insert(1, R.inverse()) initial.insert(2, R) - expected = find_Karcher_mean_Rot3([R, R.inverse()]) + expected = Rot3() result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() - actual = find_Karcher_mean_Rot3( - [result.atRot3(1), result.atRot3(2)]) + actual = gtsam.FindKarcherMean( + gtsam.Rot3Vector([result.atRot3(1), result.atRot3(2)])) self.gtsamAssertEquals(expected, actual) self.gtsamAssertEquals( R12, result.atRot3(1).between(result.atRot3(2))) diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index 9c965ddc5..18c9ef722 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -14,9 +14,9 @@ from __future__ import print_function import unittest -import numpy as np - import gtsam +import numpy as np +from gtsam import Point2, Point3, SfmData, SfmTrack from gtsam.utils.test_case import GtsamTestCase @@ -25,55 +25,97 @@ class TestSfmData(GtsamTestCase): def setUp(self): """Initialize SfmData and SfmTrack""" - self.data = gtsam.SfmData() + self.data = SfmData() # initialize SfmTrack with 3D point - self.tracks = gtsam.SfmTrack() + self.tracks = SfmTrack() def test_tracks(self): """Test functions in SfmTrack""" # measurement is of format (camera_idx, imgPoint) # create arbitrary camera indices for two cameras - i1, i2 = 4,5 + i1, i2 = 4, 5 + # create arbitrary image measurements for cameras i1 and i2 - uv_i1 = gtsam.Point2(12.6, 82) + uv_i1 = Point2(12.6, 82) + # translating point uv_i1 along X-axis - uv_i2 = gtsam.Point2(24.88, 82) + uv_i2 = Point2(24.88, 82) + # add measurements to the track - self.tracks.add_measurement(i1, uv_i1) - self.tracks.add_measurement(i2, uv_i2) + self.tracks.addMeasurement(i1, uv_i1) + self.tracks.addMeasurement(i2, uv_i2) + # Number of measurements in the track is 2 - self.assertEqual(self.tracks.number_measurements(), 2) + self.assertEqual(self.tracks.numberMeasurements(), 2) + # camera_idx in the first measurement of the track corresponds to i1 cam_idx, img_measurement = self.tracks.measurement(0) self.assertEqual(cam_idx, i1) np.testing.assert_array_almost_equal( - gtsam.Point3(0.,0.,0.), + Point3(0., 0., 0.), self.tracks.point3() ) - def test_data(self): """Test functions in SfmData""" # Create new track with 3 measurements - i1, i2, i3 = 3,5,6 - uv_i1 = gtsam.Point2(21.23, 45.64) + i1, i2, i3 = 3, 5, 6 + uv_i1 = Point2(21.23, 45.64) + # translating along X-axis - uv_i2 = gtsam.Point2(45.7, 45.64) - uv_i3 = gtsam.Point2(68.35, 45.64) + uv_i2 = Point2(45.7, 45.64) + uv_i3 = Point2(68.35, 45.64) + # add measurements and arbitrary point to the track measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)] - pt = gtsam.Point3(1.0, 6.0, 2.0) - track2 = gtsam.SfmTrack(pt) - track2.add_measurement(i1, uv_i1) - track2.add_measurement(i2, uv_i2) - track2.add_measurement(i3, uv_i3) - self.data.add_track(self.tracks) - self.data.add_track(track2) + pt = Point3(1.0, 6.0, 2.0) + track2 = SfmTrack(pt) + track2.addMeasurement(i1, uv_i1) + track2.addMeasurement(i2, uv_i2) + track2.addMeasurement(i3, uv_i3) + self.data.addTrack(self.tracks) + self.data.addTrack(track2) + # Number of tracks in SfmData is 2 - self.assertEqual(self.data.number_tracks(), 2) + self.assertEqual(self.data.numberTracks(), 2) + # camera idx of first measurement of second track corresponds to i1 cam_idx, img_measurement = self.data.track(1).measurement(0) self.assertEqual(cam_idx, i1) + def test_Balbianello(self): + """ Check that we can successfully read a bundler file and create a + factor graph from it + """ + # The structure where we will save the SfM data + filename = gtsam.findExampleDataFile("Balbianello.out") + sfm_data = SfmData.FromBundlerFile(filename) + + # Check number of things + self.assertEqual(5, sfm_data.numberCameras()) + self.assertEqual(544, sfm_data.numberTracks()) + track0 = sfm_data.track(0) + self.assertEqual(3, track0.numberMeasurements()) + + # Check projection of a given point + self.assertEqual(0, track0.measurement(0)[0]) + camera0 = sfm_data.camera(0) + expected = camera0.project(track0.point3()) + actual = track0.measurement(0)[1] + self.gtsamAssertEquals(expected, actual, 1) + + # We share *one* noiseModel between all projection factors + model = gtsam.noiseModel.Isotropic.Sigma( + 2, 1.0) # one pixel in u and v + + # Convert to NonlinearFactorGraph + graph = sfm_data.sfmFactorGraph(model) + self.assertEqual(1419, graph.size()) # regression + + # Get initial estimate + values = gtsam.initialCamerasAndPointsEstimate(sfm_data) + self.assertEqual(549, values.size()) # regression + + if __name__ == '__main__': unittest.main() diff --git a/python/gtsam/tests/test_pickle.py b/python/gtsam/tests/test_pickle.py index 0acbf6765..a6a5745bc 100644 --- a/python/gtsam/tests/test_pickle.py +++ b/python/gtsam/tests/test_pickle.py @@ -37,8 +37,8 @@ class TestPickle(GtsamTestCase): def test_sfmTrack_roundtrip(self): obj = SfmTrack(Point3(1, 1, 0)) - obj.add_measurement(0, Point2(-1, 5)) - obj.add_measurement(1, Point2(6, 2)) + obj.addMeasurement(0, Point2(-1, 5)) + obj.addMeasurement(1, Point2(6, 2)) self.assertEqualityOnPickleRoundtrip(obj) def test_unit3_roundtrip(self): diff --git a/python/gtsam/utils/visual_data_generator.py b/python/gtsam/utils/visual_data_generator.py index 51852760a..972f25477 100644 --- a/python/gtsam/utils/visual_data_generator.py +++ b/python/gtsam/utils/visual_data_generator.py @@ -1,12 +1,12 @@ from __future__ import print_function -from typing import Tuple import math -import numpy as np from math import pi +from typing import Tuple import gtsam -from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2 +import numpy as np +from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point3, Pose3 class Options: @@ -36,7 +36,7 @@ class GroundTruth: self.cameras = [Pose3()] * nrCameras self.points = [Point3(0, 0, 0)] * nrPoints - def print_(self, s="") -> None: + def print(self, s: str = "") -> None: print(s) print("K = ", self.K) print("Cameras: ", len(self.cameras)) @@ -88,7 +88,8 @@ def generate_data(options) -> Tuple[Data, GroundTruth]: r = 10 for j in range(len(truth.points)): theta = j * 2 * pi / nrPoints - truth.points[j] = Point3(r * math.cos(theta), r * math.sin(theta), 0) + truth.points[j] = Point3( + r * math.cos(theta), r * math.sin(theta), 0) else: # 3D landmarks as vertices of a cube truth.points = [ Point3(10, 10, 10), Point3(-10, 10, 10), diff --git a/python/gtsam/utils/visual_isam.py b/python/gtsam/utils/visual_isam.py index a8fed4b23..4ebd8accd 100644 --- a/python/gtsam/utils/visual_isam.py +++ b/python/gtsam/utils/visual_isam.py @@ -17,7 +17,7 @@ def initialize(data, truth, options): # Initialize iSAM params = gtsam.ISAM2Params() if options.alwaysRelinearize: - params.setRelinearizeSkip(1) + params.relinearizeSkip = 1 isam = gtsam.ISAM2(params=params) # Add constraints/priors diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index b8b6cf284..a321aa25d 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -112,54 +112,50 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) C4 x7 : x6 ************************************************************************* */ -TEST( GaussianBayesTree, balanced_smoother_marginals ) -{ +TEST(GaussianBayesTree, balanced_smoother_marginals) { // Create smoother with 7 nodes GaussianFactorGraph smoother = createSmoother(7); // Create the Bayes tree Ordering ordering; - ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); + ordering += X(1), X(3), X(5), X(7), X(2), X(6), X(4); GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); VectorValues actualSolution = bayesTree.optimize(); VectorValues expectedSolution = VectorValues::Zero(actualSolution); - EXPECT(assert_equal(expectedSolution,actualSolution,tol)); + EXPECT(assert_equal(expectedSolution, actualSolution, tol)); - LONGS_EQUAL(4, (long)bayesTree.size()); + LONGS_EQUAL(4, bayesTree.size()); - double tol=1e-5; + double tol = 1e-5; // Check marginal on x1 - JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1); JacobianFactor actual1 = *bayesTree.marginalFactor(X(1)); - Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1); - Matrix actualCovarianceX1; - GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky); - actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse(); - EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol)); - EXPECT(assert_equal(expected1,actual1,tol)); + Matrix expectedCovX1 = I_2x2 * (sigmax1 * sigmax1); + auto m = bayesTree.marginalFactor(X(1), EliminateCholesky); + Matrix actualCovarianceX1 = m->information().inverse(); + EXPECT(assert_equal(expectedCovX1, actualCovarianceX1, tol)); // Check marginal on x2 - double sigx2 = 0.68712938; // FIXME: this should be corrected analytically - JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2); + double sigmax2 = 0.68712938; // FIXME: this should be corrected analytically JacobianFactor actual2 = *bayesTree.marginalFactor(X(2)); - EXPECT(assert_equal(expected2,actual2,tol)); + Matrix expectedCovX2 = I_2x2 * (sigmax2 * sigmax2); + EXPECT(assert_equal(expectedCovX2, actual2.information().inverse(), tol)); // Check marginal on x3 - JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3); JacobianFactor actual3 = *bayesTree.marginalFactor(X(3)); - EXPECT(assert_equal(expected3,actual3,tol)); + Matrix expectedCovX3 = I_2x2 * (sigmax3 * sigmax3); + EXPECT(assert_equal(expectedCovX3, actual3.information().inverse(), tol)); // Check marginal on x4 - JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4); JacobianFactor actual4 = *bayesTree.marginalFactor(X(4)); - EXPECT(assert_equal(expected4,actual4,tol)); + Matrix expectedCovX4 = I_2x2 * (sigmax4 * sigmax4); + EXPECT(assert_equal(expectedCovX4, actual4.information().inverse(), tol)); // Check marginal on x7 (should be equal to x1) - JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7); JacobianFactor actual7 = *bayesTree.marginalFactor(X(7)); - EXPECT(assert_equal(expected7,actual7,tol)); + Matrix expectedCovX7 = I_2x2 * (sigmax7 * sigmax7); + EXPECT(assert_equal(expectedCovX7, actual7.information().inverse(), tol)); } /* ************************************************************************* */ diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index 05b4c7f66..fa27e1370 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -15,6 +15,7 @@ * @brief test general SFM class, with nonlinear optimization and BAL files */ +#include #include #include #include @@ -42,14 +43,12 @@ using symbol_shorthand::P; /* ************************************************************************* */ TEST(PinholeCamera, BAL) { string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData db; - bool success = readBAL(filename, db); - if (!success) throw runtime_error("Could not access file!"); + SfmData db = SfmData::FromBalFile(filename); SharedNoiseModel unit2 = noiseModel::Unit::Create(2); NonlinearFactorGraph graph; - for (size_t j = 0; j < db.number_tracks(); j++) { + for (size_t j = 0; j < db.numberTracks(); j++) { for (const SfmMeasurement& m: db.tracks[j].measurements) graph.emplace_shared(m.second, unit2, m.first, P(j)); } diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 2915a375e..4fe643047 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -18,6 +18,7 @@ #include #include +#include #include using namespace std; @@ -42,9 +43,7 @@ Unit3 GetDirectionFromPoses(const Values& poses, // sets up an optimization problem for the three unknown translations. TEST(TranslationRecovery, BAL) { const string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData db; - bool success = readBAL(filename, db); - if (!success) throw runtime_error("Could not access file!"); + SfmData db = SfmData::FromBalFile(filename); // Get camera poses, as Values size_t j = 0; diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 4a58a57a6..c1f36abd0 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -36,7 +36,7 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; - for (size_t j = 0; j < db.number_tracks(); j++) { + for (size_t j = 0; j < db.numberTracks(); j++) { for (const SfmMeasurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index 548c4de70..79d7432c8 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -16,7 +16,7 @@ * @date July 5, 2015 */ -#include +#include #include #include #include @@ -54,9 +54,7 @@ SfmData preamble(int argc, char* argv[]) { filename = argv[argc - 1]; else filename = findExampleDataFile("dubrovnik-16-22106-pre"); - bool success = readBAL(filename, db); - if (!success) throw runtime_error("Could not access file!"); - return db; + return SfmData::FromBalFile(filename); } // Create ordering and optimize @@ -73,8 +71,8 @@ int optimize(const SfmData& db, const NonlinearFactorGraph& graph, if (gUseSchur) { // Create Schur-complement ordering Ordering ordering; - for (size_t j = 0; j < db.number_tracks(); j++) ordering.push_back(P(j)); - for (size_t i = 0; i < db.number_cameras(); i++) { + for (size_t j = 0; j < db.numberTracks(); j++) ordering.push_back(P(j)); + for (size_t i = 0; i < db.numberCameras(); i++) { ordering.push_back(C(i)); if (separateCalibration) ordering.push_back(K(i)); } diff --git a/timing/timeSFMBALautodiff.cpp b/timing/timeSFMBALautodiff.cpp index 2d0f4a1fe..1a7e35929 100644 --- a/timing/timeSFMBALautodiff.cpp +++ b/timing/timeSFMBALautodiff.cpp @@ -44,7 +44,7 @@ int main(int argc, char* argv[]) { // Build graph NonlinearFactorGraph graph; - for (size_t j = 0; j < db.number_tracks(); j++) { + for (size_t j = 0; j < db.numberTracks(); j++) { for (const SfmMeasurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; @@ -59,7 +59,7 @@ int main(int argc, char* argv[]) { Values initial; size_t i = 0, j = 0; for (const SfmCamera& camera: db.cameras) { - // readBAL converts to GTSAM format, so we need to convert back ! + // SfmData::FromBalFile converts to GTSAM format, so we need to convert back ! Pose3 openGLpose = gtsam2openGL(camera.pose()); Vector9 v9; v9 << Pose3::Logmap(openGLpose), camera.calibration(); diff --git a/timing/timeSFMBALcamTnav.cpp b/timing/timeSFMBALcamTnav.cpp index 355defed9..a564a3a35 100644 --- a/timing/timeSFMBALcamTnav.cpp +++ b/timing/timeSFMBALcamTnav.cpp @@ -33,7 +33,7 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; - for (size_t j = 0; j < db.number_tracks(); j++) { + for (size_t j = 0; j < db.numberTracks(); j++) { for (const SfmMeasurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; diff --git a/timing/timeSFMBALnavTcam.cpp b/timing/timeSFMBALnavTcam.cpp index e602ef241..5299c8552 100644 --- a/timing/timeSFMBALnavTcam.cpp +++ b/timing/timeSFMBALnavTcam.cpp @@ -33,7 +33,7 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; - for (size_t j = 0; j < db.number_tracks(); j++) { + for (size_t j = 0; j < db.numberTracks(); j++) { Point3_ nav_point_(P(j)); for (const SfmMeasurement& m: db.tracks[j].measurements) { size_t i = m.first; diff --git a/timing/timeSFMBALsmart.cpp b/timing/timeSFMBALsmart.cpp index a69d895a5..fe2f7b925 100644 --- a/timing/timeSFMBALsmart.cpp +++ b/timing/timeSFMBALsmart.cpp @@ -35,7 +35,7 @@ int main(int argc, char* argv[]) { // Add smart factors to graph NonlinearFactorGraph graph; - for (size_t j = 0; j < db.number_tracks(); j++) { + for (size_t j = 0; j < db.numberTracks(); j++) { auto smartFactor = boost::make_shared(gNoiseModel); for (const SfmMeasurement& m : db.tracks[j].measurements) { size_t i = m.first; diff --git a/wrap/gtwrap/matlab_wrapper/mixins.py b/wrap/gtwrap/matlab_wrapper/mixins.py index f4a7988fd..4c2b005b7 100644 --- a/wrap/gtwrap/matlab_wrapper/mixins.py +++ b/wrap/gtwrap/matlab_wrapper/mixins.py @@ -26,25 +26,30 @@ class CheckMixin: return True return False + def can_be_pointer(self, arg_type: parser.Type): + """ + Determine if the `arg_type` can have a pointer to it. + + E.g. `Pose3` can have `Pose3*` but + `Matrix` should not have `Matrix*`. + """ + return (arg_type.typename.name not in self.not_ptr_type + and arg_type.typename.name not in self.ignore_namespace + and arg_type.typename.name != 'string') + def is_shared_ptr(self, arg_type: parser.Type): """ Determine if the `interface_parser.Type` should be treated as a shared pointer in the wrapper. """ - return arg_type.is_shared_ptr or ( - arg_type.typename.name not in self.not_ptr_type - and arg_type.typename.name not in self.ignore_namespace - and arg_type.typename.name != 'string') + return arg_type.is_shared_ptr def is_ptr(self, arg_type: parser.Type): """ Determine if the `interface_parser.Type` should be treated as a raw pointer in the wrapper. """ - return arg_type.is_ptr or ( - arg_type.typename.name not in self.not_ptr_type - and arg_type.typename.name not in self.ignore_namespace - and arg_type.typename.name != 'string') + return arg_type.is_ptr def is_ref(self, arg_type: parser.Type): """ diff --git a/wrap/gtwrap/matlab_wrapper/wrapper.py b/wrap/gtwrap/matlab_wrapper/wrapper.py index 42610999d..e690cd213 100755 --- a/wrap/gtwrap/matlab_wrapper/wrapper.py +++ b/wrap/gtwrap/matlab_wrapper/wrapper.py @@ -147,11 +147,13 @@ class MatlabWrapper(CheckMixin, FormatMixin): """ def args_copy(args): return ArgumentList([copy.copy(arg) for arg in args.list()]) + def method_copy(method): method2 = copy.copy(method) method2.args = args_copy(method.args) method2.args.backup = method.args.backup return method2 + if save_backup: method.args.backup = args_copy(method.args) method = method_copy(method) @@ -162,7 +164,8 @@ class MatlabWrapper(CheckMixin, FormatMixin): method.args.list().remove(arg) return [ methodWithArg, - *MatlabWrapper._expand_default_arguments(method, save_backup=False) + *MatlabWrapper._expand_default_arguments(method, + save_backup=False) ] break assert all(arg.default is None for arg in method.args.list()), \ @@ -180,9 +183,12 @@ class MatlabWrapper(CheckMixin, FormatMixin): if method_index is None: method_map[method.name] = len(method_out) - method_out.append(MatlabWrapper._expand_default_arguments(method)) + method_out.append( + MatlabWrapper._expand_default_arguments(method)) else: - method_out[method_index] += MatlabWrapper._expand_default_arguments(method) + method_out[ + method_index] += MatlabWrapper._expand_default_arguments( + method) return method_out @@ -337,43 +343,42 @@ class MatlabWrapper(CheckMixin, FormatMixin): body_args = '' for arg in args.list(): + ctype_camel = self._format_type_name(arg.ctype.typename, + separator='') + ctype_sep = self._format_type_name(arg.ctype.typename) + if self.is_ref(arg.ctype): # and not constructor: - ctype_camel = self._format_type_name(arg.ctype.typename, - separator='') - body_args += textwrap.indent(textwrap.dedent('''\ - {ctype}& {name} = *unwrap_shared_ptr< {ctype} >(in[{id}], "ptr_{ctype_camel}"); - '''.format(ctype=self._format_type_name(arg.ctype.typename), - ctype_camel=ctype_camel, - name=arg.name, - id=arg_id)), - prefix=' ') + arg_type = "{ctype}&".format(ctype=ctype_sep) + unwrap = '*unwrap_shared_ptr< {ctype} >(in[{id}], "ptr_{ctype_camel}");'.format( + ctype=ctype_sep, ctype_camel=ctype_camel, id=arg_id) - elif (self.is_shared_ptr(arg.ctype) or self.is_ptr(arg.ctype)) and \ + elif self.is_ptr(arg.ctype) and \ arg.ctype.typename.name not in self.ignore_namespace: - if arg.ctype.is_shared_ptr: - call_type = arg.ctype.is_shared_ptr - else: - call_type = arg.ctype.is_ptr - body_args += textwrap.indent(textwrap.dedent('''\ - {std_boost}::shared_ptr<{ctype_sep}> {name} = unwrap_shared_ptr< {ctype_sep} >(in[{id}], "ptr_{ctype}"); - '''.format(std_boost='boost' if constructor else 'boost', - ctype_sep=self._format_type_name( - arg.ctype.typename), - ctype=self._format_type_name(arg.ctype.typename, - separator=''), - name=arg.name, - id=arg_id)), - prefix=' ') + arg_type = "{ctype_sep}*".format(ctype_sep=ctype_sep) + unwrap = 'unwrap_ptr< {ctype_sep} >(in[{id}], "ptr_{ctype}");'.format( + ctype_sep=ctype_sep, ctype=ctype_camel, id=arg_id) + + elif (self.is_shared_ptr(arg.ctype) or self.can_be_pointer(arg.ctype)) and \ + arg.ctype.typename.name not in self.ignore_namespace: + call_type = arg.ctype.is_shared_ptr + + arg_type = "{std_boost}::shared_ptr<{ctype_sep}>".format( + std_boost='boost' if constructor else 'boost', + ctype_sep=ctype_sep) + unwrap = 'unwrap_shared_ptr< {ctype_sep} >(in[{id}], "ptr_{ctype}");'.format( + ctype_sep=ctype_sep, ctype=ctype_camel, id=arg_id) else: - body_args += textwrap.indent(textwrap.dedent('''\ - {ctype} {name} = unwrap< {ctype} >(in[{id}]); - '''.format(ctype=arg.ctype.typename.name, - name=arg.name, - id=arg_id)), - prefix=' ') + arg_type = "{ctype}".format(ctype=arg.ctype.typename.name) + unwrap = 'unwrap< {ctype} >(in[{id}]);'.format( + ctype=arg.ctype.typename.name, id=arg_id) + body_args += textwrap.indent(textwrap.dedent('''\ + {arg_type} {name} = {unwrap} + '''.format(arg_type=arg_type, name=arg.name, + unwrap=unwrap)), + prefix=' ') arg_id += 1 params = '' @@ -383,12 +388,14 @@ class MatlabWrapper(CheckMixin, FormatMixin): if params != '': params += ',' - if (arg.default is not None) and (arg.name not in explicit_arg_names): + if (arg.default is not None) and (arg.name + not in explicit_arg_names): params += arg.default continue - if (not self.is_ref(arg.ctype)) and (self.is_shared_ptr(arg.ctype)) and (self.is_ptr( - arg.ctype)) and (arg.ctype.typename.name not in self.ignore_namespace): + if not self.is_ref(arg.ctype) and (self.is_shared_ptr(arg.ctype) or \ + self.is_ptr(arg.ctype) or self.can_be_pointer(arg.ctype))and \ + arg.ctype.typename.name not in self.ignore_namespace: if arg.ctype.is_shared_ptr: call_type = arg.ctype.is_shared_ptr else: @@ -601,7 +608,8 @@ class MatlabWrapper(CheckMixin, FormatMixin): if not isinstance(ctors, Iterable): ctors = [ctors] - ctors = sum((MatlabWrapper._expand_default_arguments(ctor) for ctor in ctors), []) + ctors = sum((MatlabWrapper._expand_default_arguments(ctor) + for ctor in ctors), []) methods_wrap = textwrap.indent(textwrap.dedent("""\ methods @@ -885,10 +893,10 @@ class MatlabWrapper(CheckMixin, FormatMixin): wrapper=self._wrapper_name(), id=self._update_wrapper_id( (namespace_name, instantiated_class, - static_overload.name, static_overload)), + static_overload.name, static_overload)), class_name=instantiated_class.name, end_statement=end_statement), - prefix=' ') + prefix=' ') # If the arguments don't match any of the checks above, # throw an error with the class and method name. @@ -1079,7 +1087,8 @@ class MatlabWrapper(CheckMixin, FormatMixin): pair_value = 'first' if func_id == 0 else 'second' new_line = '\n' if func_id == 0 else '' - if self.is_shared_ptr(return_type) or self.is_ptr(return_type): + if self.is_shared_ptr(return_type) or self.is_ptr(return_type) or \ + self.can_be_pointer(return_type): shared_obj = 'pairResult.' + pair_value if not (return_type.is_shared_ptr or return_type.is_ptr): @@ -1145,7 +1154,8 @@ class MatlabWrapper(CheckMixin, FormatMixin): if return_1_name != 'void': if return_count == 1: - if self.is_shared_ptr(return_1) or self.is_ptr(return_1): + if self.is_shared_ptr(return_1) or self.is_ptr(return_1) or \ + self.can_be_pointer(return_1): sep_method_name = partial(self._format_type_name, return_1.typename, include_namespace=True) diff --git a/wrap/matlab.h b/wrap/matlab.h index bcdef3c8d..fbed0b2e2 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -477,6 +477,14 @@ boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& pro return *spp; } +template +Class* unwrap_ptr(const mxArray* obj, const string& propertyName) { + + mxArray* mxh = mxGetProperty(obj,0, propertyName.c_str()); + Class* x = reinterpret_cast (mxGetData(mxh)); + return x; +} + //// throw an error if unwrap_shared_ptr is attempted for an Eigen Vector //template <> //Vector unwrap_shared_ptr(const mxArray* obj, const string& propertyName) { diff --git a/wrap/tests/expected/matlab/ForwardKinematicsFactor.m b/wrap/tests/expected/matlab/ForwardKinematicsFactor.m index 46aa41392..e5efdce19 100644 --- a/wrap/tests/expected/matlab/ForwardKinematicsFactor.m +++ b/wrap/tests/expected/matlab/ForwardKinematicsFactor.m @@ -11,9 +11,9 @@ classdef ForwardKinematicsFactor < gtsam.BetweenFactor if nargin == 2 my_ptr = varargin{2}; else - my_ptr = inheritance_wrapper(36, varargin{2}); + my_ptr = inheritance_wrapper(52, varargin{2}); end - base_ptr = inheritance_wrapper(35, my_ptr); + base_ptr = inheritance_wrapper(51, my_ptr); else error('Arguments do not match any overload of ForwardKinematicsFactor constructor'); end @@ -22,7 +22,7 @@ classdef ForwardKinematicsFactor < gtsam.BetweenFactor end function delete(obj) - inheritance_wrapper(37, obj.ptr_ForwardKinematicsFactor); + inheritance_wrapper(53, obj.ptr_ForwardKinematicsFactor); end function display(obj), obj.print(''); end diff --git a/wrap/tests/expected/matlab/functions_wrapper.cpp b/wrap/tests/expected/matlab/functions_wrapper.cpp index 17b5fb494..61286d84f 100644 --- a/wrap/tests/expected/matlab/functions_wrapper.cpp +++ b/wrap/tests/expected/matlab/functions_wrapper.cpp @@ -86,7 +86,7 @@ void load2D_2(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("load2D",nargout,nargin,2); string filename = unwrap< string >(in[0]); - boost::shared_ptr model = unwrap_shared_ptr< gtsam::noiseModel::Diagonal >(in[1], "ptr_gtsamnoiseModelDiagonal"); + gtsam::noiseModel::Diagonal* model = unwrap_ptr< gtsam::noiseModel::Diagonal >(in[1], "ptr_gtsamnoiseModelDiagonal"); auto pairResult = load2D(filename,model); out[0] = wrap_shared_ptr(pairResult.first,"gtsam.NonlinearFactorGraph", false); out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Values", false); diff --git a/wrap/tests/expected/matlab/geometry_wrapper.cpp b/wrap/tests/expected/matlab/geometry_wrapper.cpp index ee1f04359..4a3ad1d68 100644 --- a/wrap/tests/expected/matlab/geometry_wrapper.cpp +++ b/wrap/tests/expected/matlab/geometry_wrapper.cpp @@ -151,7 +151,7 @@ void gtsamPoint2_argChar_7(int nargout, mxArray *out[], int nargin, const mxArra { checkArguments("argChar",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); - boost::shared_ptr a = unwrap_shared_ptr< char >(in[1], "ptr_char"); + char* a = unwrap_ptr< char >(in[1], "ptr_char"); obj->argChar(a); } @@ -175,7 +175,7 @@ void gtsamPoint2_argChar_10(int nargout, mxArray *out[], int nargin, const mxArr { checkArguments("argChar",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); - boost::shared_ptr a = unwrap_shared_ptr< char >(in[1], "ptr_char"); + char* a = unwrap_ptr< char >(in[1], "ptr_char"); obj->argChar(a); } diff --git a/wrap/tests/expected/matlab/inheritance_wrapper.cpp b/wrap/tests/expected/matlab/inheritance_wrapper.cpp index 0cf17eedd..9c45ca55f 100644 --- a/wrap/tests/expected/matlab/inheritance_wrapper.cpp +++ b/wrap/tests/expected/matlab/inheritance_wrapper.cpp @@ -9,6 +9,7 @@ typedef MyTemplate MyTemplatePoint2; typedef MyTemplate MyTemplateMatrix; +typedef MyTemplate MyTemplateA; typedef std::set*> Collector_MyBase; static Collector_MyBase collector_MyBase; @@ -16,6 +17,8 @@ typedef std::set*> Collector_MyTemplatePoint static Collector_MyTemplatePoint2 collector_MyTemplatePoint2; typedef std::set*> Collector_MyTemplateMatrix; static Collector_MyTemplateMatrix collector_MyTemplateMatrix; +typedef std::set*> Collector_MyTemplateA; +static Collector_MyTemplateA collector_MyTemplateA; typedef std::set*> Collector_ForwardKinematicsFactor; static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor; @@ -44,6 +47,12 @@ void _deleteAllObjects() collector_MyTemplateMatrix.erase(iter++); anyDeleted = true; } } + { for(Collector_MyTemplateA::iterator iter = collector_MyTemplateA.begin(); + iter != collector_MyTemplateA.end(); ) { + delete *iter; + collector_MyTemplateA.erase(iter++); + anyDeleted = true; + } } { for(Collector_ForwardKinematicsFactor::iterator iter = collector_ForwardKinematicsFactor.begin(); iter != collector_ForwardKinematicsFactor.end(); ) { delete *iter; @@ -67,6 +76,7 @@ void _inheritance_RTTIRegister() { types.insert(std::make_pair(typeid(MyBase).name(), "MyBase")); types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2")); types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), "MyTemplateMatrix")); + types.insert(std::make_pair(typeid(MyTemplateA).name(), "MyTemplateA")); types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor")); @@ -462,7 +472,157 @@ void MyTemplateMatrix_Level_34(int nargout, mxArray *out[], int nargin, const mx out[0] = wrap_shared_ptr(boost::make_shared>(MyTemplate::Level(K)),"MyTemplateMatrix", false); } -void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyTemplateA_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_MyTemplateA.insert(self); + + typedef boost::shared_ptr SharedBase; + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); +} + +void MyTemplateA_upcastFromVoid_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast>(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; +} + +void MyTemplateA_constructor_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + + Shared *self = new Shared(new MyTemplate()); + collector_MyTemplateA.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; + + typedef boost::shared_ptr SharedBase; + out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast(mxGetData(out[1])) = new SharedBase(*self); +} + +void MyTemplateA_deconstructor_38(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr> Shared; + checkArguments("delete_MyTemplateA",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_MyTemplateA::iterator item; + item = collector_MyTemplateA.find(self); + if(item != collector_MyTemplateA.end()) { + collector_MyTemplateA.erase(item); + } + delete self; +} + +void MyTemplateA_accept_T_39(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("accept_T",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr>(in[0], "ptr_MyTemplateA"); + A& value = *unwrap_shared_ptr< A >(in[1], "ptr_A"); + obj->accept_T(value); +} + +void MyTemplateA_accept_Tptr_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("accept_Tptr",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr>(in[0], "ptr_MyTemplateA"); + boost::shared_ptr value = unwrap_shared_ptr< A >(in[1], "ptr_A"); + obj->accept_Tptr(value); +} + +void MyTemplateA_create_MixedPtrs_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("create_MixedPtrs",nargout,nargin-1,0); + auto obj = unwrap_shared_ptr>(in[0], "ptr_MyTemplateA"); + auto pairResult = obj->create_MixedPtrs(); + out[0] = wrap_shared_ptr(boost::make_shared(pairResult.first),"A", false); + out[1] = wrap_shared_ptr(pairResult.second,"A", false); +} + +void MyTemplateA_create_ptrs_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("create_ptrs",nargout,nargin-1,0); + auto obj = unwrap_shared_ptr>(in[0], "ptr_MyTemplateA"); + auto pairResult = obj->create_ptrs(); + out[0] = wrap_shared_ptr(pairResult.first,"A", false); + out[1] = wrap_shared_ptr(pairResult.second,"A", false); +} + +void MyTemplateA_return_T_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("return_T",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr>(in[0], "ptr_MyTemplateA"); + A* value = unwrap_ptr< A >(in[1], "ptr_A"); + out[0] = wrap_shared_ptr(boost::make_shared(obj->return_T(value)),"A", false); +} + +void MyTemplateA_return_Tptr_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("return_Tptr",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr>(in[0], "ptr_MyTemplateA"); + boost::shared_ptr value = unwrap_shared_ptr< A >(in[1], "ptr_A"); + out[0] = wrap_shared_ptr(obj->return_Tptr(value),"A", false); +} + +void MyTemplateA_return_ptrs_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("return_ptrs",nargout,nargin-1,2); + auto obj = unwrap_shared_ptr>(in[0], "ptr_MyTemplateA"); + boost::shared_ptr p1 = unwrap_shared_ptr< A >(in[1], "ptr_A"); + boost::shared_ptr p2 = unwrap_shared_ptr< A >(in[2], "ptr_A"); + auto pairResult = obj->return_ptrs(p1,p2); + out[0] = wrap_shared_ptr(pairResult.first,"A", false); + out[1] = wrap_shared_ptr(pairResult.second,"A", false); +} + +void MyTemplateA_templatedMethod_46(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("templatedMethodMatrix",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr>(in[0], "ptr_MyTemplateA"); + Matrix t = unwrap< Matrix >(in[1]); + out[0] = wrap< Matrix >(obj->templatedMethod(t)); +} + +void MyTemplateA_templatedMethod_47(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("templatedMethodPoint2",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr>(in[0], "ptr_MyTemplateA"); + Point2 t = unwrap< Point2 >(in[1]); + out[0] = wrap< Point2 >(obj->templatedMethod(t)); +} + +void MyTemplateA_templatedMethod_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("templatedMethodPoint3",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr>(in[0], "ptr_MyTemplateA"); + Point3 t = unwrap< Point3 >(in[1]); + out[0] = wrap< Point3 >(obj->templatedMethod(t)); +} + +void MyTemplateA_templatedMethod_49(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("templatedMethodVector",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr>(in[0], "ptr_MyTemplateA"); + Vector t = unwrap< Vector >(in[1]); + out[0] = wrap< Vector >(obj->templatedMethod(t)); +} + +void MyTemplateA_Level_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("MyTemplate.Level",nargout,nargin,1); + A& K = *unwrap_shared_ptr< A >(in[0], "ptr_A"); + out[0] = wrap_shared_ptr(boost::make_shared>(MyTemplate::Level(K)),"MyTemplateA", false); +} + +void ForwardKinematicsFactor_collectorInsertAndMakeBase_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -475,7 +635,7 @@ void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); } -void ForwardKinematicsFactor_upcastFromVoid_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { +void ForwardKinematicsFactor_upcastFromVoid_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); @@ -484,7 +644,7 @@ void ForwardKinematicsFactor_upcastFromVoid_36(int nargout, mxArray *out[], int *reinterpret_cast(mxGetData(out[0])) = self; } -void ForwardKinematicsFactor_deconstructor_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void ForwardKinematicsFactor_deconstructor_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; checkArguments("delete_ForwardKinematicsFactor",nargout,nargin,1); @@ -615,13 +775,61 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) MyTemplateMatrix_Level_34(nargout, out, nargin-1, in+1); break; case 35: - ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1); + MyTemplateA_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1); break; case 36: - ForwardKinematicsFactor_upcastFromVoid_36(nargout, out, nargin-1, in+1); + MyTemplateA_upcastFromVoid_36(nargout, out, nargin-1, in+1); break; case 37: - ForwardKinematicsFactor_deconstructor_37(nargout, out, nargin-1, in+1); + MyTemplateA_constructor_37(nargout, out, nargin-1, in+1); + break; + case 38: + MyTemplateA_deconstructor_38(nargout, out, nargin-1, in+1); + break; + case 39: + MyTemplateA_accept_T_39(nargout, out, nargin-1, in+1); + break; + case 40: + MyTemplateA_accept_Tptr_40(nargout, out, nargin-1, in+1); + break; + case 41: + MyTemplateA_create_MixedPtrs_41(nargout, out, nargin-1, in+1); + break; + case 42: + MyTemplateA_create_ptrs_42(nargout, out, nargin-1, in+1); + break; + case 43: + MyTemplateA_return_T_43(nargout, out, nargin-1, in+1); + break; + case 44: + MyTemplateA_return_Tptr_44(nargout, out, nargin-1, in+1); + break; + case 45: + MyTemplateA_return_ptrs_45(nargout, out, nargin-1, in+1); + break; + case 46: + MyTemplateA_templatedMethod_46(nargout, out, nargin-1, in+1); + break; + case 47: + MyTemplateA_templatedMethod_47(nargout, out, nargin-1, in+1); + break; + case 48: + MyTemplateA_templatedMethod_48(nargout, out, nargin-1, in+1); + break; + case 49: + MyTemplateA_templatedMethod_49(nargout, out, nargin-1, in+1); + break; + case 50: + MyTemplateA_Level_50(nargout, out, nargin-1, in+1); + break; + case 51: + ForwardKinematicsFactor_collectorInsertAndMakeBase_51(nargout, out, nargin-1, in+1); + break; + case 52: + ForwardKinematicsFactor_upcastFromVoid_52(nargout, out, nargin-1, in+1); + break; + case 53: + ForwardKinematicsFactor_deconstructor_53(nargout, out, nargin-1, in+1); break; } } catch(const std::exception& e) { diff --git a/wrap/tests/expected/python/inheritance_pybind.cpp b/wrap/tests/expected/python/inheritance_pybind.cpp index d6cd77ca0..fdb29b5ce 100644 --- a/wrap/tests/expected/python/inheritance_pybind.cpp +++ b/wrap/tests/expected/python/inheritance_pybind.cpp @@ -54,6 +54,21 @@ PYBIND11_MODULE(inheritance_py, m_) { .def("return_ptrs",[](MyTemplate* self, const std::shared_ptr p1, const std::shared_ptr p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2")) .def_static("Level",[](const gtsam::Matrix& K){return MyTemplate::Level(K);}, py::arg("K")); + py::class_, MyBase, std::shared_ptr>>(m_, "MyTemplateA") + .def(py::init<>()) + .def("templatedMethodPoint2",[](MyTemplate* self, const gtsam::Point2& t){return self->templatedMethod(t);}, py::arg("t")) + .def("templatedMethodPoint3",[](MyTemplate* self, const gtsam::Point3& t){return self->templatedMethod(t);}, py::arg("t")) + .def("templatedMethodVector",[](MyTemplate* self, const gtsam::Vector& t){return self->templatedMethod(t);}, py::arg("t")) + .def("templatedMethodMatrix",[](MyTemplate* self, const gtsam::Matrix& t){return self->templatedMethod(t);}, py::arg("t")) + .def("accept_T",[](MyTemplate* self, const A& value){ self->accept_T(value);}, py::arg("value")) + .def("accept_Tptr",[](MyTemplate* self, std::shared_ptr value){ self->accept_Tptr(value);}, py::arg("value")) + .def("return_Tptr",[](MyTemplate* self, std::shared_ptr value){return self->return_Tptr(value);}, py::arg("value")) + .def("return_T",[](MyTemplate* self, A* value){return self->return_T(value);}, py::arg("value")) + .def("create_ptrs",[](MyTemplate* self){return self->create_ptrs();}) + .def("create_MixedPtrs",[](MyTemplate* self){return self->create_MixedPtrs();}) + .def("return_ptrs",[](MyTemplate* self, std::shared_ptr p1, std::shared_ptr p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2")) + .def_static("Level",[](const A& K){return MyTemplate::Level(K);}, py::arg("K")); + py::class_, std::shared_ptr>(m_, "ForwardKinematicsFactor"); diff --git a/wrap/tests/fixtures/inheritance.i b/wrap/tests/fixtures/inheritance.i index ddf9745df..e63f8e689 100644 --- a/wrap/tests/fixtures/inheritance.i +++ b/wrap/tests/fixtures/inheritance.i @@ -4,7 +4,7 @@ virtual class MyBase { }; // A templated class -template +template virtual class MyTemplate : MyBase { MyTemplate();