Merge pull request #1099 from borglab/release/4.2a5
						commit
						a74c7dd03c
					
				|  | @ -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) | ||||
|  |  | |||
|  | @ -28,7 +28,8 @@ | |||
| // Header order is close to far
 | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/slam/dataset.h>  // for loading BAL datasets ! | ||||
| #include <gtsam/sfm/SfmData.h>  // for loading BAL datasets ! | ||||
| #include <gtsam/slam/dataset.h> | ||||
| #include <vector> | ||||
| 
 | ||||
| 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; | ||||
|  |  | |||
|  | @ -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 <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/slam/GeneralSFMFactor.h> | ||||
| #include <gtsam/slam/dataset.h> // for loading BAL datasets ! | ||||
| #include <gtsam/sfm/SfmData.h> // for loading BAL datasets ! | ||||
| #include <gtsam/slam/dataset.h> | ||||
| #include <vector> | ||||
| 
 | ||||
| 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; | ||||
|  |  | |||
|  | @ -22,7 +22,8 @@ | |||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/slam/GeneralSFMFactor.h> | ||||
| #include <gtsam/slam/dataset.h>  // for loading BAL datasets ! | ||||
| #include <gtsam/sfm/SfmData.h>  // for loading BAL datasets ! | ||||
| #include <gtsam/slam/dataset.h> | ||||
| 
 | ||||
| #include <gtsam/base/timing.h> | ||||
| 
 | ||||
|  | @ -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_(); | ||||
|  |  | |||
|  | @ -18,6 +18,9 @@ | |||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #if BOOST_VERSION >= 107400 | ||||
| #include <boost/serialization/library_version_type.hpp> | ||||
| #endif | ||||
| #include <boost/serialization/nvp.hpp> | ||||
| #include <boost/serialization/set.hpp> | ||||
| #include <gtsam/base/FastDefaultAllocator.h> | ||||
|  |  | |||
|  | @ -25,6 +25,7 @@ | |||
| #include <string> | ||||
| 
 | ||||
| // includes for standard serialization types
 | ||||
| #include <boost/serialization/version.hpp> | ||||
| #include <boost/serialization/optional.hpp> | ||||
| #include <boost/serialization/shared_ptr.hpp> | ||||
| #include <boost/serialization/vector.hpp> | ||||
|  |  | |||
|  | @ -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); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -70,7 +70,7 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor { | |||
|   string dot( | ||||
|       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, | ||||
|       bool showZero = true) const; | ||||
|   std::vector<std::pair<DiscreteValues, double>> enumerate() const; | ||||
|   std::vector<std::pair<gtsam::DiscreteValues, double>> 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, gtsam::DiscreteFactorGraph> | ||||
|   gtsam::DiscreteBayesNet* eliminateSequential(); | ||||
|   gtsam::DiscreteBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type); | ||||
|   gtsam::DiscreteBayesNet* eliminateSequential(const gtsam::Ordering& ordering); | ||||
|   pair<gtsam::DiscreteBayesNet*, gtsam::DiscreteFactorGraph*> | ||||
|       eliminatePartialSequential(const gtsam::Ordering& ordering); | ||||
|   gtsam::DiscreteBayesTree eliminateMultifrontal(); | ||||
|   gtsam::DiscreteBayesTree eliminateMultifrontal(const gtsam::Ordering& ordering); | ||||
|   std::pair<gtsam::DiscreteBayesTree, gtsam::DiscreteFactorGraph> | ||||
| 
 | ||||
|   gtsam::DiscreteBayesTree* eliminateMultifrontal(); | ||||
|   gtsam::DiscreteBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type);   | ||||
|   gtsam::DiscreteBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);   | ||||
|   pair<gtsam::DiscreteBayesTree*, gtsam::DiscreteFactorGraph*> | ||||
|       eliminatePartialMultifrontal(const gtsam::Ordering& ordering); | ||||
| 
 | ||||
|   string dot( | ||||
|  |  | |||
|  | @ -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" | ||||
|  |  | |||
|  | @ -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" | ||||
|          "}"); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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<Cal3Bundler>; | ||||
| 
 | ||||
|   /// @name Standard Constructors
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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<Cal3DS2>; | ||||
| 
 | ||||
|   /// @name Standard Constructors
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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<Cal3DS2_Base>; | ||||
| 
 | ||||
|   /// @name Standard Constructors
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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<Cal3Unified>; | ||||
| 
 | ||||
|   /// @name Standard Constructors
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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<Rot3,3> { | ||||
| 
 | ||||
|   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<Rot3, 3> { | ||||
|  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<Rot3, Eigen::aligned_allocator<Rot3> >; | ||||
| 
 | ||||
|   /**
 | ||||
|    * [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<const Rot3> : public internal::LieGroup<Rot3> {}; | ||||
| } | ||||
|    | ||||
| }  // namespace gtsam
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -95,7 +95,7 @@ namespace gtsam { | |||
|   /* ************************************************************************* */ | ||||
|   template <class CLIQUE> | ||||
|   void BayesTree<CLIQUE>::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); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -4,6 +4,12 @@ | |||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| // Headers for overloaded methods below, break hierarchy :-/ | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/symbolic/SymbolicFactorGraph.h> | ||||
| #include <gtsam/discrete/DiscreteFactorGraph.h> | ||||
| 
 | ||||
| #include <gtsam/inference/Key.h> | ||||
| 
 | ||||
| // Default keyformatter | ||||
|  | @ -98,10 +104,41 @@ class Ordering { | |||
|   Ordering(); | ||||
|   Ordering(const gtsam::Ordering& other); | ||||
| 
 | ||||
|   template <FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, | ||||
|                             gtsam::GaussianFactorGraph}> | ||||
|   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 <gtsam/inference/VariableIndex.h> | ||||
| 
 | ||||
| // Headers for overloaded methods below, break hierarchy :-/ | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/symbolic/SymbolicFactorGraph.h> | ||||
| 
 | ||||
| class VariableIndex { | ||||
|   // Standard Constructors and Named Constructors | ||||
|   VariableIndex(); | ||||
|  |  | |||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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. | ||||
|  |  | |||
|  | @ -18,6 +18,7 @@ | |||
| #include <gtsam/linear/linearExceptions.h> | ||||
| #include <gtsam/linear/GaussianConditional.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <gtsam/linear/Sampler.h> | ||||
| 
 | ||||
| #include <boost/format.hpp> | ||||
| #ifdef __GNUC__ | ||||
|  | @ -34,6 +35,9 @@ | |||
| #include <list> | ||||
| #include <string> | ||||
| 
 | ||||
| // 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<Eigen::Upper>(); | ||||
|     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<JacobianFactor>(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 { | ||||
|  |  | |||
|  | @ -26,12 +26,15 @@ | |||
| #include <gtsam/inference/Conditional.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| 
 | ||||
| #include <random> // 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<BaseFactor, This> 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<Key, Matrix>, 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<typename ITERATOR> | ||||
|     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: | ||||
|  |  | |||
|  | @ -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: "); | ||||
|   } | ||||
|  |  | |||
|  | @ -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", | ||||
|  |  | |||
|  | @ -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<double> Normal; | ||||
|       Normal dist(0.0, sigma); | ||||
|       result(i) = dist(generator_); | ||||
|       std::normal_distribution<double> 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()); | ||||
|  |  | |||
|  | @ -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: | ||||
|  |  | |||
|  | @ -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<const Key, size_t>; | ||||
|     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, Vector>& key_value) { | ||||
|     std::pair<iterator, bool> 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<VectorValues::value_type, | ||||
|  | @ -219,14 +219,14 @@ namespace gtsam { | |||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   /* ************************************************************************ */ | ||||
|   bool VectorValues::hasSameStructure(const VectorValues other) const | ||||
|   { | ||||
|     return accumulate(combine(*this, other) | ||||
|       | transformed(internal::structureCompareOp), true, logical_and<bool>()); | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   /* ************************************************************************ */ | ||||
|   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 << "<div>\n<table class='VectorValues'>\n  <thead>\n"; | ||||
| 
 | ||||
|     // Print out header row.
 | ||||
|     ss << "    <tr><th>Variable</th><th>value</th></tr>\n"; | ||||
| 
 | ||||
|     // Finish header and start body.
 | ||||
|     ss << "  </thead>\n  <tbody>\n"; | ||||
| 
 | ||||
|     // Print out all rows.
 | ||||
| #ifdef GTSAM_USE_TBB | ||||
|     // TBB uses un-ordered map, so inefficiently order them:
 | ||||
|     std::map<Key, Vector> ordered; | ||||
|     for (const auto& kv : *this) ordered.emplace(kv); | ||||
|     for (const auto& kv : ordered) { | ||||
| #else | ||||
|     for (const auto& kv : *this) { | ||||
| #endif | ||||
|       ss << "    <tr>"; | ||||
|       ss << "<th>" << keyFormatter(kv.first) << "</th><td>" | ||||
|          << kv.second.transpose() << "</td>"; | ||||
|       ss << "</tr>\n"; | ||||
|     } | ||||
|     ss << "  </tbody>\n</table>\n</div>"; | ||||
|     return ss.str(); | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************ */ | ||||
| 
 | ||||
| } // \namespace gtsam
 | ||||
|  |  | |||
|  | @ -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; | ||||
| 
 | ||||
|     /// @}
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -255,6 +255,7 @@ class VectorValues { | |||
| 
 | ||||
|   // enabling serialization functionality | ||||
|   void serialize() const; | ||||
|   string html() const; | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam/linear/GaussianFactor.h> | ||||
|  | @ -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<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> 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 <gtsam/linear/GaussianDensity.h> | ||||
| 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, Vector> 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; | ||||
|  |  | |||
|  | @ -16,10 +16,12 @@ | |||
|  */ | ||||
| 
 | ||||
| #include <gtsam/linear/GaussianBayesNet.h> | ||||
| #include <gtsam/linear/GaussianDensity.h> | ||||
| #include <gtsam/linear/JacobianFactor.h> | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <boost/tuple/tuple.hpp> | ||||
|  | @ -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) | ||||
| { | ||||
|  |  | |||
|  | @ -20,9 +20,10 @@ | |||
| 
 | ||||
| #include <gtsam/base/Matrix.h> | ||||
| #include <gtsam/base/VerticalBlockMatrix.h> | ||||
| #include <gtsam/inference/Key.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/linear/JacobianFactor.h> | ||||
| #include <gtsam/linear/GaussianConditional.h> | ||||
| #include <gtsam/linear/GaussianDensity.h> | ||||
| #include <gtsam/linear/GaussianBayesNet.h> | ||||
| 
 | ||||
| #include <boost/assign/std/list.hpp> | ||||
|  | @ -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); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -17,10 +17,13 @@ | |||
|  **/ | ||||
| 
 | ||||
| #include <gtsam/linear/GaussianDensity.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| 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);} | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -17,7 +17,7 @@ | |||
| 
 | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <gtsam/inference/LabeledSymbol.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
|  | @ -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 = | ||||
|       "<div>\n" | ||||
|       "<table class='VectorValues'>\n" | ||||
|       "  <thead>\n" | ||||
|       "    <tr><th>Variable</th><th>value</th></tr>\n" | ||||
|       "  </thead>\n" | ||||
|       "  <tbody>\n" | ||||
|       "    <tr><th>x1</th><td>  2 3.1</td></tr>\n" | ||||
|       "    <tr><th>x2</th><td>  4 5.2</td></tr>\n" | ||||
|       "    <tr><th>x5</th><td>  6 7.3</td></tr>\n" | ||||
|       "    <tr><th>x7</th><td>  8 9.4</td></tr>\n" | ||||
|       "  </tbody>\n" | ||||
|       "</table>\n" | ||||
|       "</div>"; | ||||
|   string actual = vv.html(); | ||||
|   EXPECT(actual == expected); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { TestResult tr; return TestRegistry::runAllTests(tr); } | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -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; | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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 { | ||||
|  |  | |||
|  | @ -22,10 +22,19 @@ virtual class RangeFactor : gtsam::NoiseModelFactor { | |||
|   void serialize() const; | ||||
| }; | ||||
| 
 | ||||
| // between points: | ||||
| typedef gtsam::RangeFactor<gtsam::Point2, gtsam::Point2> RangeFactor2; | ||||
| typedef gtsam::RangeFactor<gtsam::Point3, gtsam::Point3> RangeFactor3; | ||||
| 
 | ||||
| // between pose and point: | ||||
| typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D; | ||||
| typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D; | ||||
| typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2; | ||||
| 
 | ||||
| // between poses: | ||||
| typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D; | ||||
| typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3; | ||||
| 
 | ||||
| // more specialized types: | ||||
| typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> | ||||
|     RangeFactorCalibratedCameraPoint; | ||||
| typedef gtsam::RangeFactor<gtsam::PinholeCamera<gtsam::Cal3_S2>, gtsam::Point3> | ||||
|  |  | |||
|  | @ -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 <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/sfm/SfmData.h> | ||||
| #include <gtsam/slam/GeneralSFMFactor.h> | ||||
| 
 | ||||
| #include <fstream> | ||||
| #include <iostream> | ||||
| 
 | ||||
| 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<Cal3Bundler>; | ||||
|   SfmData dataValues = data; | ||||
| 
 | ||||
|   // Store poses or cameras in SfmData
 | ||||
|   size_t nrPoses = values.count<Pose3>(); | ||||
|   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<Pose3>(i); | ||||
|       Cal3Bundler K = dataValues.cameras[i].calibration(); | ||||
|       Camera camera(pose, K); | ||||
|       dataValues.cameras[i] = camera; | ||||
|     } | ||||
|   } else { | ||||
|     size_t nrCameras = values.count<Camera>(); | ||||
|     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<Camera>(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<Point3>(), 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<Point3>(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<SfmCamera, Point3>; | ||||
|   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<ProjectionFactor>(uv, model, i, P(j)); | ||||
|     } | ||||
|     j += 1; | ||||
|   } | ||||
| 
 | ||||
|   return factors; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************** */ | ||||
| NonlinearFactorGraph SfmData::sfmFactorGraph( | ||||
|     const SharedNoiseModel &model, boost::optional<size_t> fixedCamera, | ||||
|     boost::optional<size_t> fixedPoint) const { | ||||
|   using ProjectionFactor = GeneralSFMFactor<SfmCamera, Point3>; | ||||
|   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
 | ||||
|  | @ -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 <gtsam/geometry/Cal3Bundler.h> | ||||
| #include <gtsam/geometry/PinholeCamera.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/nonlinear/Values.h> | ||||
| #include <gtsam/sfm/SfmTrack.h> | ||||
| 
 | ||||
| #include <string> | ||||
| #include <vector> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /// Define the structure for the camera poses
 | ||||
| typedef PinholeCamera<Cal3Bundler> SfmCamera; | ||||
| 
 | ||||
| /**
 | ||||
|  * @brief SfmData stores a bunch of SfmTracks | ||||
|  * @addtogroup sfm | ||||
|  */ | ||||
| struct GTSAM_EXPORT SfmData { | ||||
|   std::vector<SfmCamera> cameras;  ///< Set of cameras
 | ||||
| 
 | ||||
|   std::vector<SfmTrack> 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<size_t> fixedCamera = 0, | ||||
|       boost::optional<size_t> 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 <class Archive> | ||||
|   void serialize(Archive& ar, const unsigned int /*version*/) { | ||||
|     ar& BOOST_SERIALIZATION_NVP(cameras); | ||||
|     ar& BOOST_SERIALIZATION_NVP(tracks); | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
| }; | ||||
| 
 | ||||
| /// traits
 | ||||
| template <> | ||||
| struct traits<SfmData> : public Testable<SfmData> {}; | ||||
| 
 | ||||
| #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<Cal3Bundler> 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
 | ||||
|  | @ -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 <gtsam/sfm/SfmTrack.h> | ||||
| 
 | ||||
| #include <iostream> | ||||
| 
 | ||||
| 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
 | ||||
|  | @ -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 <gtsam/base/serialization.h> | ||||
| #include <gtsam/geometry/Point2.h> | ||||
| #include <gtsam/geometry/Point3.h> | ||||
| 
 | ||||
| #include <string> | ||||
| #include <utility> | ||||
| #include <vector> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /// A measurement with its camera index
 | ||||
| typedef std::pair<size_t, Point2> SfmMeasurement; | ||||
| 
 | ||||
| /// Sift index for SfmTrack
 | ||||
| typedef std::pair<size_t, size_t> 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<SfmMeasurement> measurements; | ||||
| 
 | ||||
|   /// The feature descriptors
 | ||||
|   std::vector<SiftIndex> 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 <class ARCHIVE> | ||||
|   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 <typename T> | ||||
| struct traits; | ||||
| 
 | ||||
| template <> | ||||
| struct traits<SfmTrack> : public Testable<SfmTrack> {}; | ||||
| 
 | ||||
| }  // namespace gtsam
 | ||||
|  | @ -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<Rot> &measurement(size_t k) const { | ||||
|  |  | |||
|  | @ -4,7 +4,62 @@ | |||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| // ##### | ||||
| #include <gtsam/sfm/SfmTrack.h> | ||||
| class SfmTrack { | ||||
|   SfmTrack(); | ||||
|   SfmTrack(const gtsam::Point3& pt); | ||||
|   const Point3& point3() const; | ||||
| 
 | ||||
|   double r; | ||||
|   double g; | ||||
|   double b; | ||||
| 
 | ||||
|   std::vector<pair<size_t, gtsam::Point2>> measurements; | ||||
| 
 | ||||
|   size_t numberMeasurements() const; | ||||
|   pair<size_t, gtsam::Point2> measurement(size_t idx) const; | ||||
|   pair<size_t, size_t> 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 <gtsam/sfm/SfmData.h> | ||||
| 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<gtsam::Cal3Bundler> 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 <gtsam/sfm/ShonanFactor.h> | ||||
| 
 | ||||
|  | @ -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); | ||||
| 
 | ||||
|  |  | |||
|  | @ -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 <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/sfm/SfmData.h> | ||||
| 
 | ||||
| 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<Cal3Bundler> expectedCamera = writtenData.cameras[i]; | ||||
|     PinholeCamera<Cal3Bundler> 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<Pose3>(0); | ||||
|   EXPECT(assert_equal(expectedPose, actualPose, 1e-7)); | ||||
| 
 | ||||
|   Point3 expectedPoint = track0.p; | ||||
|   Point3 actualPoint = values.at<Point3>(P(0)); | ||||
|   EXPECT(assert_equal(expectedPoint, actualPoint, 1e-6)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|   return TestRegistry::runAllTests(tr); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
|  | @ -54,6 +54,8 @@ class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1<Rot> { | |||
|   Eigen::Matrix<double, Dim, 1> 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<Rot, Rot> { | |||
|   enum { Dim = Rot::VectorN2::RowsAtCompileTime }; | ||||
| 
 | ||||
|  public: | ||||
|   EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||||
| 
 | ||||
|   /// @name Constructor
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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) {} | ||||
|  |  | |||
|  | @ -146,7 +146,7 @@ protected: | |||
|    */ | ||||
|   template<class SFM_TRACK> | ||||
|   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); | ||||
|     } | ||||
|  |  | |||
|  | @ -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<size_t, Point2> parseVariables<Point2>(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<Cal3Bundler>; | ||||
|   SfmData dataValues = data; | ||||
| 
 | ||||
|   // Store poses or cameras in SfmData
 | ||||
|   size_t nrPoses = values.count<Pose3>(); | ||||
|   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<Pose3>(X(i)); | ||||
|       Cal3Bundler K = dataValues.cameras[i].calibration(); | ||||
|       Camera camera(pose, K); | ||||
|       dataValues.cameras[i] = camera; | ||||
|     } | ||||
|   } else { | ||||
|     size_t nrCameras = values.count<Camera>(); | ||||
|     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<Camera>(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<Point3>(), | ||||
|          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<Point3>(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<Pose2> and parseFactors<Pose2>
 | ||||
| BetweenFactorPose2s | ||||
| parse2DFactors(const std::string &filename, | ||||
|  |  | |||
|  | @ -22,6 +22,7 @@ | |||
| 
 | ||||
| #include <gtsam/sfm/BinaryMeasurement.h> | ||||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| #include <gtsam/sfm/SfmData.h> | ||||
| #include <gtsam/geometry/Cal3Bundler.h> | ||||
| #include <gtsam/geometry/PinholeCamera.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
|  | @ -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<size_t, Point2> SfmMeasurement; | ||||
| 
 | ||||
| /// Sift index for SfmTrack
 | ||||
| typedef std::pair<size_t, size_t> 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<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
 | ||||
|   std::vector<SiftIndex> 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<class ARCHIVE> | ||||
|   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<SfmTrack> : public Testable<SfmTrack> { | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
| /// Define the structure for the camera poses
 | ||||
| typedef PinholeCamera<Cal3Bundler> SfmCamera; | ||||
| 
 | ||||
| /// Define the structure for SfM data
 | ||||
| struct SfmData { | ||||
|   std::vector<SfmCamera> cameras; ///< Set of cameras
 | ||||
|   std::vector<SfmTrack> 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<class Archive> | ||||
|   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<SfmData> : public Testable<SfmData> { | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * @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<Cal3Bundler> 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<Pose2> and parseFactors<Pose2>
 | ||||
| using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>; | ||||
| GTSAM_EXPORT BetweenFactorPose2s | ||||
|  |  | |||
|  | @ -209,61 +209,27 @@ virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor { | |||
| 
 | ||||
| #include <gtsam/slam/dataset.h> | ||||
| 
 | ||||
| class SfmTrack { | ||||
|   SfmTrack(); | ||||
|   SfmTrack(const gtsam::Point3& pt); | ||||
|   const Point3& point3() const; | ||||
| 
 | ||||
|   double r; | ||||
|   double g; | ||||
|   double b; | ||||
| 
 | ||||
|   std::vector<pair<size_t, gtsam::Point2>> measurements; | ||||
| 
 | ||||
|   size_t number_measurements() const; | ||||
|   pair<size_t, gtsam::Point2> measurement(size_t idx) const; | ||||
|   pair<size_t, size_t> 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<gtsam::Cal3Bundler> 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<gtsam::NonlinearFactorGraph*, gtsam::Values*> 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<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D( | ||||
|     string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, | ||||
|     bool addNoise, bool smart); | ||||
| pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D( | ||||
|     string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, | ||||
|     bool addNoise); | ||||
| pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D( | ||||
|     string filename, gtsam::noiseModel::Diagonal* model, int maxIndex); | ||||
| pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D( | ||||
|     string filename, gtsam::noiseModel::Diagonal* model); | ||||
| pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> 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<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename); | ||||
| 
 | ||||
| pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename); | ||||
| pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, | ||||
|                                                            bool is3D); | ||||
| pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> 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/slam/FrobeniusFactor.h> | ||||
| gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, | ||||
|                                                 size_t d); | ||||
|  |  | |||
|  | @ -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<Cal3Bundler> expectedCamera = writtenData.cameras[i]; | ||||
|     PinholeCamera<Cal3Bundler> 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<Pose3>(X(0)); | ||||
|   EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); | ||||
| 
 | ||||
|   Point3 expectedPoint = track0.p; | ||||
|   Point3 actualPoint = value.at<Point3>(P(0)); | ||||
|   EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { TestResult tr; return TestRegistry::runAllTests(tr); } | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -15,6 +15,7 @@ | |||
| #include <gtsam/nonlinear/expressionTesting.h> | ||||
| #include <gtsam/nonlinear/factorTesting.h> | ||||
| #include <gtsam/slam/EssentialMatrixFactor.h> | ||||
| #include <gtsam/sfm/SfmData.h> | ||||
| #include <gtsam/slam/dataset.h> | ||||
| 
 | ||||
| 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<EssentialMatrixFactor2>(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<EssentialMatrix>(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<double>(i), result.at<double>(i), 1e-1); | ||||
| 
 | ||||
|   // Check error at result
 | ||||
|  |  | |||
|  | @ -16,6 +16,7 @@ | |||
|  * @date Jan 1, 2021 | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/sfm/SfmData.h> | ||||
| #include <gtsam/slam/dataset.h> | ||||
| 
 | ||||
| #include <gtsam/base/serializationTestHelpers.h> | ||||
|  | @ -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); | ||||
| 
 | ||||
|  |  | |||
|  | @ -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 <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, | ||||
|                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, | ||||
|  | @ -589,7 +597,12 @@ virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { | |||
|   IncrementalFixedLagSmoother(double smootherLag); | ||||
|   IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); | ||||
| 
 | ||||
|   void print(string s = "IncrementalFixedLagSmoother:\n") const; | ||||
| 
 | ||||
|   gtsam::ISAM2Params params() const; | ||||
| 
 | ||||
|   gtsam::NonlinearFactorGraph getFactors() const; | ||||
|   gtsam::ISAM2 getISAM2() const; | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.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 */ | ||||
|  |  | |||
|  | @ -7,7 +7,7 @@ import gtsam.* | |||
| %% Initialize iSAM | ||||
| params = gtsam.ISAM2Params; | ||||
| if options.alwaysRelinearize | ||||
|     params.setRelinearizeSkip(1); | ||||
|     params.relinearizeSkip = 1; | ||||
| end | ||||
| isam = ISAM2(params); | ||||
| 
 | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -45,6 +45,7 @@ set(ignore | |||
|     gtsam::Point3Pairs | ||||
|     gtsam::Pose3Pairs | ||||
|     gtsam::Pose3Vector | ||||
|     gtsam::Rot3Vector | ||||
|     gtsam::KeyVector | ||||
|     gtsam::BinaryMeasurementsUnit3 | ||||
|     gtsam::DiscreteKey | ||||
|  |  | |||
|  | @ -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) | ||||
| 
 | ||||
|  |  | |||
|  | @ -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. | ||||
|  |  | |||
|  | @ -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. | ||||
|  |  | |||
|  | @ -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))) | ||||
| 
 | ||||
|  |  | |||
|  | @ -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()) | ||||
| 
 | ||||
|  |  | |||
|  | @ -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__': | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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) | ||||
|  |  | |||
|  | @ -11,5 +11,4 @@ | |||
|  * mutations on Python side will not be reflected on C++. | ||||
|  */ | ||||
| 
 | ||||
| #include <pybind11/stl.h> | ||||
| 
 | ||||
| #include <pybind11/stl.h> | ||||
|  | @ -15,3 +15,4 @@ PYBIND11_MAKE_OPAQUE( | |||
|     std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >); | ||||
| PYBIND11_MAKE_OPAQUE( | ||||
|     std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >); | ||||
| PYBIND11_MAKE_OPAQUE(gtsam::Rot3Vector); | ||||
|  |  | |||
|  | @ -12,8 +12,9 @@ | |||
|  */ | ||||
| 
 | ||||
| py::bind_vector< | ||||
|     std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >( | ||||
|     std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3>>>>( | ||||
|     m_, "BetweenFactorPose3s"); | ||||
| py::bind_vector< | ||||
|     std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >( | ||||
|     std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>>>>( | ||||
|     m_, "BetweenFactorPose2s"); | ||||
| py::bind_vector<gtsam::Rot3Vector>(m_, "Rot3Vector"); | ||||
|  |  | |||
|  | @ -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() | ||||
|  |  | |||
|  | @ -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() | ||||
|  | @ -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)) | ||||
| 
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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))) | ||||
|  |  | |||
|  | @ -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() | ||||
|  |  | |||
|  | @ -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): | ||||
|  |  | |||
|  | @ -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), | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -15,6 +15,7 @@ | |||
|  * @brief test general SFM class, with nonlinear optimization and BAL files | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/sfm/SfmData.h> | ||||
| #include <gtsam/slam/dataset.h> | ||||
| #include <gtsam/slam/GeneralSFMFactor.h> | ||||
| #include <gtsam/geometry/Point3.h> | ||||
|  | @ -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<sfmFactor>(m.second, unit2, m.first, P(j)); | ||||
|   } | ||||
|  |  | |||
|  | @ -18,6 +18,7 @@ | |||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/sfm/TranslationRecovery.h> | ||||
| #include <gtsam/sfm/SfmData.h> | ||||
| #include <gtsam/slam/dataset.h> | ||||
| 
 | ||||
| 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; | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -16,7 +16,7 @@ | |||
|  * @date    July 5, 2015 | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/slam/dataset.h> | ||||
| #include <gtsam/sfm/SfmData.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/nonlinear/Values.h> | ||||
|  | @ -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)); | ||||
|     } | ||||
|  |  | |||
|  | @ -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(); | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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<SfmFactor>(gNoiseModel); | ||||
|     for (const SfmMeasurement& m : db.tracks[j].measurements) { | ||||
|       size_t i = m.first; | ||||
|  |  | |||
|  | @ -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): | ||||
|         """ | ||||
|  |  | |||
|  | @ -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) | ||||
|  |  | |||
|  | @ -477,6 +477,14 @@ boost::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& pro | |||
|   return *spp; | ||||
| } | ||||
| 
 | ||||
| template <typename Class> | ||||
| Class* unwrap_ptr(const mxArray* obj, const string& propertyName) { | ||||
| 
 | ||||
|   mxArray* mxh = mxGetProperty(obj,0, propertyName.c_str()); | ||||
|   Class* x = reinterpret_cast<Class*> (mxGetData(mxh)); | ||||
|   return x; | ||||
| } | ||||
| 
 | ||||
| //// throw an error if unwrap_shared_ptr is attempted for an Eigen Vector
 | ||||
| //template <>
 | ||||
| //Vector unwrap_shared_ptr<Vector>(const mxArray* obj, const string& propertyName) {
 | ||||
|  |  | |||
|  | @ -11,9 +11,9 @@ classdef ForwardKinematicsFactor < gtsam.BetweenFactor<gtsam.Pose3> | |||
|         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<gtsam.Pose3> | |||
|     end | ||||
| 
 | ||||
|     function delete(obj) | ||||
|       inheritance_wrapper(37, obj.ptr_ForwardKinematicsFactor); | ||||
|       inheritance_wrapper(53, obj.ptr_ForwardKinematicsFactor); | ||||
|     end | ||||
| 
 | ||||
|     function display(obj), obj.print(''); end | ||||
|  |  | |||
|  | @ -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<gtsam::noiseModel::Diagonal> 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); | ||||
|  |  | |||
|  | @ -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<gtsam::Point2>(in[0], "ptr_gtsamPoint2"); | ||||
|   boost::shared_ptr<char> 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<gtsam::Point2>(in[0], "ptr_gtsamPoint2"); | ||||
|   boost::shared_ptr<char> a = unwrap_shared_ptr< char >(in[1], "ptr_char"); | ||||
|   char* a = unwrap_ptr< char >(in[1], "ptr_char"); | ||||
|   obj->argChar(a); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -9,6 +9,7 @@ | |||
| 
 | ||||
| typedef MyTemplate<gtsam::Point2> MyTemplatePoint2; | ||||
| typedef MyTemplate<gtsam::Matrix> MyTemplateMatrix; | ||||
| typedef MyTemplate<A> MyTemplateA; | ||||
| 
 | ||||
| typedef std::set<boost::shared_ptr<MyBase>*> Collector_MyBase; | ||||
| static Collector_MyBase collector_MyBase; | ||||
|  | @ -16,6 +17,8 @@ typedef std::set<boost::shared_ptr<MyTemplatePoint2>*> Collector_MyTemplatePoint | |||
| static Collector_MyTemplatePoint2 collector_MyTemplatePoint2; | ||||
| typedef std::set<boost::shared_ptr<MyTemplateMatrix>*> Collector_MyTemplateMatrix; | ||||
| static Collector_MyTemplateMatrix collector_MyTemplateMatrix; | ||||
| typedef std::set<boost::shared_ptr<MyTemplateA>*> Collector_MyTemplateA; | ||||
| static Collector_MyTemplateA collector_MyTemplateA; | ||||
| typedef std::set<boost::shared_ptr<ForwardKinematicsFactor>*> 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<Matrix>>(MyTemplate<gtsam::Matrix>::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<MyTemplate<A>> Shared; | ||||
| 
 | ||||
|   Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0])); | ||||
|   collector_MyTemplateA.insert(self); | ||||
| 
 | ||||
|   typedef boost::shared_ptr<MyBase> SharedBase; | ||||
|   out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); | ||||
|   *reinterpret_cast<SharedBase**>(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<MyTemplate<A>> Shared; | ||||
|   boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0])); | ||||
|   out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); | ||||
|   Shared *self = new Shared(boost::static_pointer_cast<MyTemplate<A>>(*asVoid)); | ||||
|   *reinterpret_cast<Shared**>(mxGetData(out[0])) = self; | ||||
| } | ||||
| 
 | ||||
| void MyTemplateA_constructor_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) | ||||
| { | ||||
|   mexAtExit(&_deleteAllObjects); | ||||
|   typedef boost::shared_ptr<MyTemplate<A>> Shared; | ||||
| 
 | ||||
|   Shared *self = new Shared(new MyTemplate<A>()); | ||||
|   collector_MyTemplateA.insert(self); | ||||
|   out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); | ||||
|   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self; | ||||
| 
 | ||||
|   typedef boost::shared_ptr<MyBase> SharedBase; | ||||
|   out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); | ||||
|   *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self); | ||||
| } | ||||
| 
 | ||||
| void MyTemplateA_deconstructor_38(int nargout, mxArray *out[], int nargin, const mxArray *in[]) | ||||
| { | ||||
|   typedef boost::shared_ptr<MyTemplate<A>> Shared; | ||||
|   checkArguments("delete_MyTemplateA",nargout,nargin,1); | ||||
|   Shared *self = *reinterpret_cast<Shared**>(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<MyTemplate<A>>(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<MyTemplate<A>>(in[0], "ptr_MyTemplateA"); | ||||
|   boost::shared_ptr<A> 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<MyTemplate<A>>(in[0], "ptr_MyTemplateA"); | ||||
|   auto pairResult = obj->create_MixedPtrs(); | ||||
|   out[0] = wrap_shared_ptr(boost::make_shared<A>(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<MyTemplate<A>>(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<MyTemplate<A>>(in[0], "ptr_MyTemplateA"); | ||||
|   A* value = unwrap_ptr< A >(in[1], "ptr_A"); | ||||
|   out[0] = wrap_shared_ptr(boost::make_shared<A>(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<MyTemplate<A>>(in[0], "ptr_MyTemplateA"); | ||||
|   boost::shared_ptr<A> 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<MyTemplate<A>>(in[0], "ptr_MyTemplateA"); | ||||
|   boost::shared_ptr<A> p1 = unwrap_shared_ptr< A >(in[1], "ptr_A"); | ||||
|   boost::shared_ptr<A> 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<MyTemplate<A>>(in[0], "ptr_MyTemplateA"); | ||||
|   Matrix t = unwrap< Matrix >(in[1]); | ||||
|   out[0] = wrap< Matrix >(obj->templatedMethod<gtsam::Matrix>(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<MyTemplate<A>>(in[0], "ptr_MyTemplateA"); | ||||
|   Point2 t = unwrap< Point2 >(in[1]); | ||||
|   out[0] = wrap< Point2 >(obj->templatedMethod<gtsam::Point2>(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<MyTemplate<A>>(in[0], "ptr_MyTemplateA"); | ||||
|   Point3 t = unwrap< Point3 >(in[1]); | ||||
|   out[0] = wrap< Point3 >(obj->templatedMethod<gtsam::Point3>(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<MyTemplate<A>>(in[0], "ptr_MyTemplateA"); | ||||
|   Vector t = unwrap< Vector >(in[1]); | ||||
|   out[0] = wrap< Vector >(obj->templatedMethod<gtsam::Vector>(t)); | ||||
| } | ||||
| 
 | ||||
| void MyTemplateA_Level_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) | ||||
| { | ||||
|   checkArguments("MyTemplate<A>.Level",nargout,nargin,1); | ||||
|   A& K = *unwrap_shared_ptr< A >(in[0], "ptr_A"); | ||||
|   out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<A>>(MyTemplate<A>::Level(K)),"MyTemplateA", false); | ||||
| } | ||||
| 
 | ||||
| void ForwardKinematicsFactor_collectorInsertAndMakeBase_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) | ||||
| { | ||||
|   mexAtExit(&_deleteAllObjects); | ||||
|   typedef boost::shared_ptr<ForwardKinematicsFactor> Shared; | ||||
|  | @ -475,7 +635,7 @@ void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray | |||
|   *reinterpret_cast<SharedBase**>(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<ForwardKinematicsFactor> Shared; | ||||
|   boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0])); | ||||
|  | @ -484,7 +644,7 @@ void ForwardKinematicsFactor_upcastFromVoid_36(int nargout, mxArray *out[], int | |||
|   *reinterpret_cast<Shared**>(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<ForwardKinematicsFactor> 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) { | ||||
|  |  | |||
|  | @ -54,6 +54,21 @@ PYBIND11_MODULE(inheritance_py, m_) { | |||
|         .def("return_ptrs",[](MyTemplate<gtsam::Matrix>* self, const std::shared_ptr<gtsam::Matrix> p1, const std::shared_ptr<gtsam::Matrix> p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2")) | ||||
|         .def_static("Level",[](const gtsam::Matrix& K){return MyTemplate<gtsam::Matrix>::Level(K);}, py::arg("K")); | ||||
| 
 | ||||
|     py::class_<MyTemplate<A>, MyBase, std::shared_ptr<MyTemplate<A>>>(m_, "MyTemplateA") | ||||
|         .def(py::init<>()) | ||||
|         .def("templatedMethodPoint2",[](MyTemplate<A>* self, const gtsam::Point2& t){return self->templatedMethod<gtsam::Point2>(t);}, py::arg("t")) | ||||
|         .def("templatedMethodPoint3",[](MyTemplate<A>* self, const gtsam::Point3& t){return self->templatedMethod<gtsam::Point3>(t);}, py::arg("t")) | ||||
|         .def("templatedMethodVector",[](MyTemplate<A>* self, const gtsam::Vector& t){return self->templatedMethod<gtsam::Vector>(t);}, py::arg("t")) | ||||
|         .def("templatedMethodMatrix",[](MyTemplate<A>* self, const gtsam::Matrix& t){return self->templatedMethod<gtsam::Matrix>(t);}, py::arg("t")) | ||||
|         .def("accept_T",[](MyTemplate<A>* self, const A& value){ self->accept_T(value);}, py::arg("value")) | ||||
|         .def("accept_Tptr",[](MyTemplate<A>* self, std::shared_ptr<A> value){ self->accept_Tptr(value);}, py::arg("value")) | ||||
|         .def("return_Tptr",[](MyTemplate<A>* self, std::shared_ptr<A> value){return self->return_Tptr(value);}, py::arg("value")) | ||||
|         .def("return_T",[](MyTemplate<A>* self, A* value){return self->return_T(value);}, py::arg("value")) | ||||
|         .def("create_ptrs",[](MyTemplate<A>* self){return self->create_ptrs();}) | ||||
|         .def("create_MixedPtrs",[](MyTemplate<A>* self){return self->create_MixedPtrs();}) | ||||
|         .def("return_ptrs",[](MyTemplate<A>* self, std::shared_ptr<A> p1, std::shared_ptr<A> p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2")) | ||||
|         .def_static("Level",[](const A& K){return MyTemplate<A>::Level(K);}, py::arg("K")); | ||||
| 
 | ||||
|     py::class_<ForwardKinematicsFactor, gtsam::BetweenFactor<gtsam::Pose3>, std::shared_ptr<ForwardKinematicsFactor>>(m_, "ForwardKinematicsFactor"); | ||||
| 
 | ||||
| 
 | ||||
|  |  | |||
Some files were not shown because too many files have changed in this diff Show More
		Loading…
	
		Reference in New Issue