diff --git a/.cproject b/.cproject index f52a49e73..075558734 100644 --- a/.cproject +++ b/.cproject @@ -1484,6 +1484,14 @@ true true + + make + -j5 + testSimpleCamera.run + true + true + true + make -j2 diff --git a/gtsam.h b/gtsam.h index 755402554..8b569a91e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -671,7 +671,7 @@ class VariableIndex { size_t size() const; size_t nFactors() const; size_t nEntries() const; - void permute(const gtsam::Permutation& permutation); + void permuteInPlace(const gtsam::Permutation& permutation); };*/ //************************************************************************* diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 7f566a115..bb83cec16 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -49,10 +49,11 @@ namespace gtsam { } /* ************************************************************************* */ - void DecisionTreeFactor::print(const string& s) const { + void DecisionTreeFactor::print(const string& s, + const IndexFormatter& formatter) const { cout << s; - IndexFactor::print("IndexFactor:"); - Potentials::print("Potentials:"); + IndexFactor::print("IndexFactor:",formatter); + Potentials::print("Potentials:",formatter); } /* ************************************************************************* */ diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 89bc090dc..64f37c174 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -72,7 +72,8 @@ namespace gtsam { bool equals(const DecisionTreeFactor& other, double tol = 1e-9) const; // print - void print(const std::string& s = "DecisionTreeFactor:\n") const; + virtual void print(const std::string& s = "DecisionTreeFactor:\n", + const IndexFormatter& formatter = DefaultIndexFormatter) const; /// @} /// @name Standard Interface diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index b9a87dea9..2a2260d97 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -51,10 +51,11 @@ namespace gtsam { } /* ************************************************************************* */ - void Potentials::print(const string&s) const { + void Potentials::print(const string& s, + const IndexFormatter& formatter) const { cout << s << "\n Cardinalities: "; BOOST_FOREACH(const DiscreteKey& key, cardinalities_) - cout << key.first << "=" << key.second << " "; + cout << formatter(key.first) << "=" << formatter(key.second) << " "; cout << endl; ADT::print(" "); } diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 367a323c2..3ca222b5f 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -65,7 +65,8 @@ namespace gtsam { // Testable bool equals(const Potentials& other, double tol = 1e-9) const; - void print(const std::string& s = "Potentials: ") const; + void print(const std::string& s = "Potentials: ", + const IndexFormatter& formatter = DefaultIndexFormatter) const; size_t cardinality(Index j) const { return cardinalities_.at(j);} diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp new file mode 100644 index 000000000..d441716cb --- /dev/null +++ b/gtsam/geometry/SimpleCamera.cpp @@ -0,0 +1,49 @@ +/* ---------------------------------------------------------------------------- + + * 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 SimpleCamera.cpp + * @brief A simple camera class with a Cal3_S2 calibration + * @date June 30, 2012 + * @author Frank Dellaert + */ + +#include +#include + +namespace gtsam { + + SimpleCamera simpleCamera(const Matrix& P) { + + // P = [A|a] = s K cRw [I|-T], with s the unknown scale + Matrix A = P.topLeftCorner(3, 3); + Vector a = P.col(3); + + // do RQ decomposition to get s*K and cRw angles + Matrix sK; + Vector xyz; + boost::tie(sK, xyz) = RQ(A); + + // Recover scale factor s and K + double s = sK(2, 2); + Matrix K = sK / s; + + // Recover cRw itself, and its inverse + Rot3 cRw = Rot3::RzRyRx(xyz); + Rot3 wRc = cRw.inverse(); + + // Now, recover T from a = - s K cRw T = - A T + Vector T = -(A.inverse() * a); + return SimpleCamera(Pose3(wRc, T), + Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2))); + } + +} diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index d3b0cdbdc..abe42f1a0 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -22,5 +22,10 @@ #include namespace gtsam { + + /// A simple camera class with a Cal3_S2 calibration typedef PinholeCamera SimpleCamera; + + /// Recover camera from 3*4 camera matrix + SimpleCamera simpleCamera(const Matrix& P); } diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index fd82557a3..8995a3a14 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -24,8 +24,8 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2) GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2) -Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2); -Point2 p(1, -2); +static Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2); +static Point2 p(1, -2); /* ************************************************************************* */ TEST( Cal3_S2, easy_constructor) diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index c1fba6b2c..17a0b1eca 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -36,37 +36,20 @@ using namespace gtsam; using namespace gtsam::serializationTestHelpers; /* ************************************************************************* */ -// Export all classes derived from Value -BOOST_CLASS_EXPORT(gtsam::Cal3_S2) -BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo) -BOOST_CLASS_EXPORT(gtsam::Cal3Bundler) -BOOST_CLASS_EXPORT(gtsam::CalibratedCamera) -BOOST_CLASS_EXPORT(gtsam::Point2) -BOOST_CLASS_EXPORT(gtsam::Point3) -BOOST_CLASS_EXPORT(gtsam::Pose2) -BOOST_CLASS_EXPORT(gtsam::Pose3) -BOOST_CLASS_EXPORT(gtsam::Rot2) -BOOST_CLASS_EXPORT(gtsam::Rot3) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::StereoPoint2) +static Point3 pt3(1.0, 2.0, 3.0); +static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +static Pose3 pose3(rt3, pt3); -/* ************************************************************************* */ -Point3 pt3(1.0, 2.0, 3.0); -Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); -Pose3 pose3(rt3, pt3); +static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); +static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); +static Cal3Bundler cal3(1.0, 2.0, 3.0); +static Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); +static Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4)); +static CalibratedCamera cal5(Pose3(rt3, pt3)); -Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); -Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); -Cal3Bundler cal3(1.0, 2.0, 3.0); -Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); -Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4)); -CalibratedCamera cal5(Pose3(rt3, pt3)); - -PinholeCamera cam1(pose3, cal1); -StereoCamera cam2(pose3, cal4ptr); -StereoPoint2 spt(1.0, 2.0, 3.0); +static PinholeCamera cam1(pose3, cal1); +static StereoCamera cam2(pose3, cal4ptr); +static StereoPoint2 spt(1.0, 2.0, 3.0); /* ************************************************************************* */ TEST (Serialization, text_geometry) { diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 6cc00de8a..a1647fa47 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -133,6 +133,26 @@ TEST( SimpleCamera, Dproject_point_pose) CHECK(assert_equal(Dpoint, numerical_point,1e-7)); } +/* ************************************************************************* */ +TEST( SimpleCamera, simpleCamera) +{ + Cal3_S2 K(468.2,427.2,91.2,300,200); + Rot3 R( + 0.41380,0.90915,0.04708, + -0.57338,0.22011,0.78917, + 0.70711,-0.35355,0.61237); + Point3 T(1000,2000,1500); + SimpleCamera expected(Pose3(R.inverse(),T),K); + // H&Z example, 2nd edition, page 163 + Matrix P = Matrix_(3,4, + 3.53553e2, 3.39645e2, 2.77744e2, -1.44946e6, + -1.03528e2, 2.33212e1, 4.59607e2, -6.32525e5, + 7.07107e-1, -3.53553e-1,6.12372e-1, -9.18559e2); + SimpleCamera actual = simpleCamera(P); + // Note precision of numbers given in book + CHECK(assert_equal(expected, actual,1e-1)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 2446ff122..29a8344d4 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -74,21 +74,21 @@ TEST( StereoCamera, project) /* ************************************************************************* */ -Pose3 camera1(Matrix_(3,3, +static Pose3 camera1(Matrix_(3,3, 1., 0., 0., 0.,-1., 0., 0., 0.,-1. ), Point3(0,0,6.25)); -Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); -StereoCamera stereoCam(Pose3(), K); +static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); +static StereoCamera stereoCam(Pose3(), K); // point X Y Z in meters -Point3 p(0, 0, 5); +static Point3 p(0, 0, 5); /* ************************************************************************* */ -StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); } +static StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); } TEST( StereoCamera, Dproject_stereo_pose) { Matrix expected = numericalDerivative21(project_,stereoCam, p); diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 0aa322e87..3a5b9b0da 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -26,7 +26,7 @@ namespace gtsam { -/** + /** * A permutation reorders variables, for example to reduce fill-in during * elimination. To save computation, the permutation can be applied to * the necessary data structures only once, then multiple computations @@ -151,8 +151,8 @@ public: */ Permutation::shared_ptr inverse() const; - const_iterator begin() const { return rangeIndices_.begin(); } /// permuted(permutation, container); - * permuted[index1]; - * permuted[index2]; - * which is equivalent to: - * container[permutation[index1]]; - * container[permutation[index2]]; - * but more concise. - */ -template -class Permuted { - Permutation permutation_; - CONTAINER& container_; -public: - typedef typename CONTAINER::iterator::value_type value_type; - - /** Construct as a permuted view on the Container. The permutation is copied - * but only a reference to the container is stored. - */ - Permuted(const Permutation& permutation, CONTAINER& container) : permutation_(permutation), container_(container) {} - - /** Construct as a view on the Container with an identity permutation. Only - * a reference to the container is stored. - */ - Permuted(CONTAINER& container) : permutation_(Permutation::Identity(container.size())), container_(container) {} - - /** Print */ - void print(const std::string& str = "") const { - std::cout << str; - permutation_.print(" permutation: "); - container_.print(" container: "); - } - - /** Access the container through the permutation */ - value_type& operator[](size_t index) { return container_[permutation_[index]]; } - - /** Access the container through the permutation (const version) */ - const value_type& operator[](size_t index) const { return container_[permutation_[index]]; } - - /** Assignment operator for cloning in ISAM2 */ - Permuted operator=(const Permuted& other) { - permutation_ = other.permutation_; - container_ = other.container_; - return *this; - } - - /** Permute this view by applying a permutation to the underlying permutation */ - void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); } - - /** Access the underlying container */ - CONTAINER* operator->() { return &container_; } - - /** Access the underlying container (const version) */ - const CONTAINER* operator->() const { return &container_; } - - /** Size of the underlying container */ - size_t size() const { return container_.size(); } - - /** Access to the underlying container */ - CONTAINER& container() { return container_; } - - /** Access to the underlying container (const version) */ - const CONTAINER& container() const { return container_; } - - /** Access the underlying permutation */ - Permutation& permutation() { return permutation_; } - const Permutation& permutation() const { return permutation_; } -}; - - } diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp index b58a69b6d..00f3439a0 100644 --- a/gtsam/inference/SymbolicFactorGraph.cpp +++ b/gtsam/inference/SymbolicFactorGraph.cpp @@ -85,8 +85,8 @@ namespace gtsam { "IndexFactor::CombineAndEliminate called on factors with no variables."); vector newKeys(keys.begin(), keys.end()); - return make_pair(new IndexConditional(newKeys, nrFrontals), - new IndexFactor(newKeys.begin() + nrFrontals, newKeys.end())); + return make_pair(boost::make_shared(newKeys, nrFrontals), + boost::make_shared(newKeys.begin() + nrFrontals, newKeys.end())); } /* ************************************************************************* */ diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index c7ef90e9f..a29d5e432 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -18,37 +18,12 @@ #include #include +#include namespace gtsam { using namespace std; -/* ************************************************************************* */ -VariableIndex::VariableIndex(const VariableIndex& other) : - index_(indexUnpermuted_) { - *this = other; -} - -/* ************************************************************************* */ -VariableIndex& VariableIndex::operator=(const VariableIndex& rhs) { - index_ = rhs.index_; - nFactors_ = rhs.nFactors_; - nEntries_ = rhs.nEntries_; - return *this; -} - -/* ************************************************************************* */ -void VariableIndex::permute(const Permutation& permutation) { -#ifndef NDEBUG - // Assert that the permutation does not leave behind any non-empty variables, - // otherwise the nFactors and nEntries counts would be incorrect. - for(Index j=0; jindex_.size(); ++j) - if(find(permutation.begin(), permutation.end(), j) == permutation.end()) - assert(this->operator[](j).empty()); -#endif - index_.permute(permutation); -} - /* ************************************************************************* */ bool VariableIndex::equals(const VariableIndex& other, double tol) const { if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) { @@ -66,17 +41,13 @@ bool VariableIndex::equals(const VariableIndex& other, double tol) const { /* ************************************************************************* */ void VariableIndex::print(const string& str) const { - cout << str << "\n"; + cout << str; cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; - Index var = 0; - BOOST_FOREACH(const Factors& variable, index_.container()) { - Permutation::const_iterator rvar = find(index_.permutation().begin(), index_.permutation().end(), var); - assert(rvar != index_.permutation().end()); - cout << "var " << (rvar-index_.permutation().begin()) << ":"; - BOOST_FOREACH(const size_t factor, variable) + for(Index var = 0; var < size(); ++var) { + cout << "var " << var << ":"; + BOOST_FOREACH(const size_t factor, index_[var]) cout << " " << factor; cout << "\n"; - ++ var; } cout << flush; } @@ -85,7 +56,7 @@ void VariableIndex::print(const string& str) const { void VariableIndex::outputMetisFormat(ostream& os) const { os << size() << " " << nFactors() << "\n"; // run over variables, which will be hyper-edges. - BOOST_FOREACH(const Factors& variable, index_.container()) { + BOOST_FOREACH(const Factors& variable, index_) { // every variable is a hyper-edge covering its factors BOOST_FOREACH(const size_t factor, variable) os << (factor+1) << " "; // base 1 @@ -94,4 +65,15 @@ void VariableIndex::outputMetisFormat(ostream& os) const { os << flush; } +/* ************************************************************************* */ +void VariableIndex::permuteInPlace(const Permutation& permutation) { + // Create new index and move references to data into it in permuted order + vector newIndex(this->size()); + for(Index i = 0; i < newIndex.size(); ++i) + newIndex[i].swap(this->index_[permutation[i]]); + + // Move reference to entire index into the VariableIndex + index_.swap(newIndex); +} + } diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index c9efc6b22..be47cbd5a 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -22,10 +22,12 @@ #include #include -#include +#include namespace gtsam { + class Permutation; + /** * The VariableIndex class computes and stores the block column structure of a * factor graph. The factor graph stores a collection of factors, each of @@ -44,8 +46,7 @@ public: typedef Factors::const_iterator Factor_const_iterator; protected: - std::vector indexUnpermuted_; - Permuted > index_; // Permuted view of indexUnpermuted. + std::vector index_; size_t nFactors_; // Number of factors in the original factor graph. size_t nEntries_; // Sum of involved variable counts of each factor. @@ -55,7 +56,7 @@ public: /// @{ /** Default constructor, creates an empty VariableIndex */ - VariableIndex() : index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {} + VariableIndex() : nFactors_(0), nEntries_(0) {} /** * Create a VariableIndex that computes and stores the block column structure @@ -70,16 +71,6 @@ public: */ template VariableIndex(const FactorGraph& factorGraph); - /** - * Copy constructor - */ - VariableIndex(const VariableIndex& other); - - /** - * Assignment operator - */ - VariableIndex& operator=(const VariableIndex& rhs); - /// @} /// @name Standard Interface /// @{ @@ -120,9 +111,6 @@ public: /// @name Advanced Interface /// @{ - /** Access a list of factors by variable */ - Factors& operator[](Index variable) { checkVar(variable); return index_[variable]; } - /** * Augment the variable index with new factors. This can be used when * solving problems incrementally. @@ -137,11 +125,8 @@ public: */ template void remove(const CONTAINER& indices, const FactorGraph& factors); - /** - * Apply a variable permutation. Does not rearrange data, just permutes - * future lookups by variable. - */ - void permute(const Permutation& permutation); + /// Permute the variables in the VariableIndex according to the given permutation + void permuteInPlace(const Permutation& permutation); protected: Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); } /// void fill(const FactorGraph& factorGraph); /// @} @@ -183,7 +168,7 @@ void VariableIndex::fill(const FactorGraph& factorGraph) { /* ************************************************************************* */ template VariableIndex::VariableIndex(const FactorGraph& factorGraph) : - index_(indexUnpermuted_), nFactors_(0), nEntries_(0) { + nFactors_(0), nEntries_(0) { // If the factor graph is empty, return an empty index because inside this // if block we assume at least one factor. @@ -200,8 +185,7 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph) : } // Allocate array - index_.container().resize(maxVar+1); - index_.permutation() = Permutation::Identity(maxVar+1); + index_.resize(maxVar+1); fill(factorGraph); } @@ -210,7 +194,7 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph) : /* ************************************************************************* */ template VariableIndex::VariableIndex(const FactorGraph& factorGraph, Index nVariables) : - indexUnpermuted_(nVariables), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) { + index_(nVariables), nFactors_(0), nEntries_(0) { fill(factorGraph); } @@ -232,11 +216,7 @@ void VariableIndex::augment(const FactorGraph& factors) { } // Allocate index - Index originalSize = index_.size(); - index_.container().resize(std::max(index_.size(), maxVar+1)); - index_.permutation().resize(index_.container().size()); - for(Index var=originalSize; var SymbolicISAM; /* ************************************************************************* */ // Some numbers that should be consistent among all smoother tests -double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 = +static double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 = 0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1; /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 40d40a3a7..e167d8ad8 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -182,34 +182,18 @@ JacobianFactor::shared_ptr GaussianConditional::toFactor() const { return JacobianFactor::shared_ptr(new JacobianFactor(*this)); } -/* ************************************************************************* */ -template -inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES& x) { - - // Helper function to solve-in-place on a VectorValues or Permuted, - // called by GaussianConditional::solveInPlace(VectorValues&) and by - // GaussianConditional::solveInPlace(Permuted&). - - static const bool debug = false; - if(debug) conditional.print("Solving conditional in place"); - Vector xS = internal::extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents()); - xS = conditional.get_d() - conditional.get_S() * xS; - Vector soln = conditional.get_R().triangularView().solve(xS); - if(debug) { - gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on "); - gtsam::print(soln, "full back-substitution solution: "); - } - internal::writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals()); -} - /* ************************************************************************* */ void GaussianConditional::solveInPlace(VectorValues& x) const { - doSolveInPlace(*this, x); // Call helper version above -} - -/* ************************************************************************* */ -void GaussianConditional::solveInPlace(Permuted& x) const { - doSolveInPlace(*this, x); // Call helper version above + static const bool debug = false; + if(debug) this->print("Solving conditional in place"); + Vector xS = internal::extractVectorValuesSlices(x, this->beginParents(), this->endParents()); + xS = this->get_d() - this->get_S() * xS; + Vector soln = this->get_R().triangularView().solve(xS); + if(debug) { + gtsam::print(Matrix(this->get_R()), "Calling backSubstituteUpper on "); + gtsam::print(soln, "full back-substitution solution: "); + } + internal::writeVectorValuesSlices(soln, x, this->beginFrontals(), this->endFrontals()); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 5b7801156..cde4f266f 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -196,23 +196,6 @@ public: */ void solveInPlace(VectorValues& x) const; - /** - * Solves a conditional Gaussian and writes the solution into the entries of - * \c x for each frontal variable of the conditional (version for permuted - * VectorValues). The parents are assumed to have already been solved in - * and their values are read from \c x. This function works for multiple - * frontal variables. - * - * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2, - * where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator - * variables of this conditional, this solve function computes - * \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution. - * - * @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the - * solution \f$ x_f \f$ will be written. - */ - void solveInPlace(Permuted& x) const; - // functions for transpose backsubstitution /** diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 660eb25f7..c5f726252 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -84,7 +84,7 @@ namespace gtsam { protected: - SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix + noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix std::vector firstNonzeroBlocks_; AbMatrix matrix_; // the full matrix corresponding to the factor BlockAb Ab_; // the block view of the full matrix diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 9d39b5674..01799148a 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -19,6 +19,8 @@ #pragma once #include +#include +#include #include #include diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 1413d1517..15925c644 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -17,10 +17,12 @@ */ #include +#include #include using namespace std; -using namespace gtsam; + +namespace gtsam { /* ************************************************************************* */ VectorValues::VectorValues(const VectorValues& other) { @@ -166,20 +168,24 @@ void VectorValues::operator+=(const VectorValues& c) { } /* ************************************************************************* */ -VectorValues& VectorValues::operator=(const Permuted& rhs) { - if(this->size() != rhs.size()) - throw std::invalid_argument("VectorValues assignment from Permuted requires pre-allocation, see documentation."); - for(size_t j=0; jsize(); ++j) { - if(exists(j)) { - SubVector& l(this->at(j)); - const SubVector& r(rhs[j]); - if(l.rows() != r.rows()) - throw std::invalid_argument("VectorValues assignment from Permuted requires pre-allocation, see documentation."); - l = r; - } else { - if(rhs.container().exists(rhs.permutation()[j])) - throw std::invalid_argument("VectorValues assignment from Permuted requires pre-allocation, see documentation."); - } - } - return *this; +VectorValues VectorValues::permute(const Permutation& permutation) const { + // Create result and allocate space + VectorValues lhs; + lhs.values_.resize(this->dim()); + lhs.maps_.reserve(this->size()); + + // Copy values from this VectorValues to the permuted VectorValues + size_t lhsPos = 0; + for(size_t i = 0; i < this->size(); ++i) { + // Map the next LHS subvector to the next slice of the LHS vector + lhs.maps_.push_back(SubVector(lhs.values_, lhsPos, this->at(permutation[i]).size())); + // Copy the data from the RHS subvector to the LHS subvector + lhs.maps_[i] = this->at(permutation[i]); + // Increment lhs position + lhsPos += lhs.maps_[i].size(); + } + + return lhs; } + +} \ No newline at end of file diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 04c280577..463080db9 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -19,7 +19,6 @@ #include #include -#include #include #include @@ -29,6 +28,9 @@ namespace gtsam { + // Forward declarations + class Permutation; + /** * This class represents a collection of vector-valued variables associated * each with a unique integer index. It is typically used to store the variables @@ -288,10 +290,11 @@ namespace gtsam { */ void operator+=(const VectorValues& c); - /** Assignment operator from Permuted, requires the dimensions - * of the assignee to already be properly pre-allocated. - */ - VectorValues& operator=(const Permuted& rhs); + /** + * Permute the entries of this VectorValues, returns a new VectorValues as + * the result. + */ + VectorValues permute(const Permutation& permutation) const; /// @} diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 93030b598..8b0c5e239 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -267,55 +267,6 @@ TEST( GaussianConditional, solve_multifrontal ) } -/* ************************************************************************* */ -TEST( GaussianConditional, solve_multifrontal_permuted ) -{ - // create full system, 3 variables, 2 frontals, all 2 dim - Matrix full_matrix = Matrix_(4, 7, - 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, - 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, - 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3, - 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4); - - // 3 variables, all dim=2 - vector dims; dims += 2, 2, 2, 1; - GaussianConditional::rsd_type matrices(full_matrix, dims.begin(), dims.end()); - Vector sigmas = ones(4); - vector cgdims; cgdims += _x_, _x1_, _l1_; - GaussianConditional cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas); - - EXPECT(assert_equal(Vector_(4, 0.1, 0.2, 0.3, 0.4), cg.get_d())); - - // partial solution - Vector sl1 = Vector_(2, 9.0, 10.0); - - // elimination order; _x_, _x1_, _l1_ - VectorValues actualUnpermuted(vector(3, 2)); - Permutation permutation(3); - permutation[0] = 2; - permutation[1] = 0; - permutation[2] = 1; - Permuted actual(permutation, actualUnpermuted); - actual[_x_] = Vector_(2, 0.1, 0.2); // rhs - actual[_x1_] = Vector_(2, 0.3, 0.4); // rhs - actual[_l1_] = sl1; // parent - - VectorValues expectedUnpermuted(vector(3, 2)); - Permuted expected(permutation, expectedUnpermuted); - expected[_x_] = Vector_(2, -3.1,-3.4); - expected[_x1_] = Vector_(2, -11.9,-13.2); - expected[_l1_] = sl1; - - // verify indices/size - EXPECT_LONGS_EQUAL(3, cg.size()); - EXPECT_LONGS_EQUAL(4, cg.dim()); - - // solve and verify - cg.solveInPlace(actual); - EXPECT(assert_equal(expected.container(), actual.container(), tol)); - -} - /* ************************************************************************* */ TEST( GaussianConditional, solveTranspose ) { static const Index _y_=1; diff --git a/gtsam/linear/tests/testGaussianJunctionTree.cpp b/gtsam/linear/tests/testGaussianJunctionTree.cpp index b49d7fff0..c4ddc360b 100644 --- a/gtsam/linear/tests/testGaussianJunctionTree.cpp +++ b/gtsam/linear/tests/testGaussianJunctionTree.cpp @@ -35,7 +35,7 @@ using namespace gtsam; static const Index x2=0, x1=1, x3=2, x4=3; -GaussianFactorGraph createChain() { +static GaussianFactorGraph createChain() { typedef GaussianFactorGraph::sharedFactor Factor; SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 0.5); diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index a646773f2..e8a53c504 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -47,11 +47,11 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* ************************************************************************* */ // example noise models -noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); -noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); -noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); -noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1)); -noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); +static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); +static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); +static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); +static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1)); +static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); /* ************************************************************************* */ TEST (Serialization, noiseModels) { diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index a92b81ddc..17230854a 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -421,52 +421,31 @@ TEST(VectorValues, hasSameStructure) { EXPECT(!v1.hasSameStructure(VectorValues())); } + /* ************************************************************************* */ -TEST(VectorValues, permuted_combined) { - Vector v1 = Vector_(3, 1.0,2.0,3.0); - Vector v2 = Vector_(2, 4.0,5.0); - Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0); +TEST(VectorValues, permute) { - vector dims(3); dims[0]=3; dims[1]=2; dims[2]=4; - VectorValues combined(dims); - combined[0] = v1; - combined[1] = v2; - combined[2] = v3; + VectorValues original; + original.insert(0, Vector_(1, 1.0)); + original.insert(1, Vector_(2, 2.0, 3.0)); + original.insert(2, Vector_(2, 4.0, 5.0)); + original.insert(3, Vector_(2, 6.0, 7.0)); - Permutation perm1(3); - perm1[0] = 1; - perm1[1] = 2; - perm1[2] = 0; + VectorValues expected; + expected.insert(0, Vector_(2, 4.0, 5.0)); // from 2 + expected.insert(1, Vector_(1, 1.0)); // from 0 + expected.insert(2, Vector_(2, 6.0, 7.0)); // from 3 + expected.insert(3, Vector_(2, 2.0, 3.0)); // from 1 - Permutation perm2(3); - perm2[0] = 1; - perm2[1] = 2; - perm2[2] = 0; + Permutation permutation(4); + permutation[0] = 2; + permutation[1] = 0; + permutation[2] = 3; + permutation[3] = 1; - Permuted permuted1(combined); - CHECK(assert_equal(v1, permuted1[0])) - CHECK(assert_equal(v2, permuted1[1])) - CHECK(assert_equal(v3, permuted1[2])) + VectorValues actual = original.permute(permutation); - permuted1.permute(perm1); - CHECK(assert_equal(v1, permuted1[2])) - CHECK(assert_equal(v2, permuted1[0])) - CHECK(assert_equal(v3, permuted1[1])) - - permuted1.permute(perm2); - CHECK(assert_equal(v1, permuted1[1])) - CHECK(assert_equal(v2, permuted1[2])) - CHECK(assert_equal(v3, permuted1[0])) - - Permuted permuted2(perm1, combined); - CHECK(assert_equal(v1, permuted2[2])) - CHECK(assert_equal(v2, permuted2[0])) - CHECK(assert_equal(v3, permuted2[1])) - - permuted2.permute(perm2); - CHECK(assert_equal(v1, permuted2[1])) - CHECK(assert_equal(v2, permuted2[2])) - CHECK(assert_equal(v3, permuted2[0])) + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 8b3bd4a0d..3c373e0ba 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -29,8 +29,8 @@ namespace gtsam { /* ************************************************************************* */ void ISAM2::Impl::AddVariables( - const Values& newTheta, Values& theta, Permuted& delta, - Permuted& deltaNewton, Permuted& deltaGradSearch, vector& replacedKeys, + const Values& newTheta, Values& theta, VectorValues& delta, + VectorValues& deltaNewton, VectorValues& deltaGradSearch, vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); @@ -40,28 +40,21 @@ void ISAM2::Impl::AddVariables( std::vector dims(newTheta.dims(*newTheta.orderingArbitrary())); if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl; const size_t newDim = accumulate(dims.begin(), dims.end(), 0); - const size_t originalDim = delta->dim(); - const size_t originalnVars = delta->size(); - delta.container().append(dims); - delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); - delta.permutation().resize(originalnVars + newTheta.size()); - deltaNewton.container().append(dims); - deltaNewton.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); - deltaNewton.permutation().resize(originalnVars + newTheta.size()); - deltaGradSearch.container().append(dims); - deltaGradSearch.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); - deltaGradSearch.permutation().resize(originalnVars + newTheta.size()); + const size_t originalDim = delta.dim(); + const size_t originalnVars = delta.size(); + delta.append(dims); + delta.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); + deltaNewton.append(dims); + deltaNewton.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); + deltaGradSearch.append(dims); + deltaGradSearch.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); { Index nextVar = originalnVars; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { - delta.permutation()[nextVar] = nextVar; - deltaNewton.permutation()[nextVar] = nextVar; - deltaGradSearch.permutation()[nextVar] = nextVar; ordering.insert(key_value.key, nextVar); if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl; ++ nextVar; } - assert(delta.permutation().size() == delta.container().size()); assert(ordering.nVars() == delta.size()); assert(ordering.size() == delta.size()); } @@ -82,7 +75,7 @@ FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const N } /* ************************************************************************* */ -FastSet ISAM2::Impl::CheckRelinearizationFull(const Permuted& delta, const Ordering& ordering, +FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { FastSet relinKeys; @@ -110,7 +103,7 @@ FastSet ISAM2::Impl::CheckRelinearizationFull(const Permuted& relinKeys, double threshold, const Permuted& delta, const ISAM2Clique::shared_ptr& clique) { +void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) { // Check the current clique for relinearization bool relinearize = false; @@ -131,7 +124,7 @@ void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double thres } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const Permuted& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) { +void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const VectorValues& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) { // Check the current clique for relinearization bool relinearize = false; @@ -163,7 +156,7 @@ void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted& delta, const Ordering& ordering, +FastSet ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { FastSet relinKeys; @@ -201,13 +194,13 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, } /* ************************************************************************* */ -void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted& delta, const Ordering& ordering, - const vector& mask, boost::optional&> invalidateIfDebug, const KeyFormatter& keyFormatter) { +void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const Ordering& ordering, + const vector& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) { // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions // if we try to re-use them. #ifdef NDEBUG - invalidateIfDebug = boost::optional&>(); + invalidateIfDebug = boost::none; #endif assert(values.size() == ordering.nVars()); @@ -304,7 +297,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, toc(4,"ccolamd permutations"); tic(5,"permute affected variable index"); - affectedFactorsIndex.permute(*affectedColamd); + affectedFactorsIndex.permuteInPlace(*affectedColamd); toc(5,"permute affected variable index"); tic(6,"permute affected factors"); @@ -354,25 +347,13 @@ inline static void optimizeInPlace(const boost::shared_ptr& clique, } /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold) { +size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, VectorValues& delta, double wildfireThreshold) { size_t lastBacksubVariableCount; if (wildfireThreshold <= 0.0) { // Threshold is zero or less, so do a full recalculation - // Collect dimensions and allocate new VectorValues - vector dims(delta.size()); - for(size_t j=0; jdim(j); - VectorValues newDelta(dims); - - // Optimize full solution delta - internal::optimizeInPlace(root, newDelta); - - // Copy solution into delta - delta.permutation() = Permutation::Identity(delta.size()); - delta.container() = newDelta; - + internal::optimizeInPlace(root, delta); lastBacksubVariableCount = delta.size(); } else { @@ -380,8 +361,8 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std: lastBacksubVariableCount = optimizeWildfire(root, wildfireThreshold, replacedKeys, delta); // modifies delta_ #ifndef NDEBUG - for(size_t j=0; j)).all()); + for(size_t j=0; j)).all()); #endif } @@ -394,7 +375,7 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std: /* ************************************************************************* */ namespace internal { void updateDoglegDeltas(const boost::shared_ptr& clique, std::vector& replacedKeys, - const VectorValues& grad, Permuted& deltaNewton, Permuted& RgProd, size_t& varsUpdated) { + const VectorValues& grad, VectorValues& deltaNewton, VectorValues& RgProd, size_t& varsUpdated) { // Check if any frontal or separator keys were recalculated, if so, we need // update deltas and recurse to children, but if not, we do not need to @@ -433,7 +414,7 @@ void updateDoglegDeltas(const boost::shared_ptr& clique, std::vecto /* ************************************************************************* */ size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, - Permuted& deltaNewton, Permuted& RgProd) { + VectorValues& deltaNewton, VectorValues& RgProd) { // Get gradient VectorValues grad = *allocateVectorValues(isam); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 0aafb3f35..fdb39d855 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -46,10 +46,10 @@ struct ISAM2::Impl { * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables * @param keyFormatter Formatter for printing nonlinear keys during debugging */ - static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, - Permuted& deltaNewton, Permuted& deltaGradSearch, std::vector& replacedKeys, + static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta, + VectorValues& deltaNewton, VectorValues& deltaGradSearch, std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); - + /** * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol * in each NonlinearFactor, obtains the index by calling ordering[symbol]. @@ -68,7 +68,7 @@ struct ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationFull(const Permuted& delta, const Ordering& ordering, + static FastSet CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** @@ -82,7 +82,7 @@ struct ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted& delta, const Ordering& ordering, + static FastSet CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** @@ -115,9 +115,9 @@ struct ISAM2::Impl { * recalculate its delta. * @param keyFormatter Formatter for printing nonlinear keys during debugging */ - static void ExpmapMasked(Values& values, const Permuted& delta, + static void ExpmapMasked(Values& values, const VectorValues& delta, const Ordering& ordering, const std::vector& mask, - boost::optional&> invalidateIfDebug = boost::optional&>(), + boost::optional invalidateIfDebug = boost::none, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** @@ -137,10 +137,10 @@ struct ISAM2::Impl { static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode, bool useQR); - static size_t UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); + static size_t UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, VectorValues& delta, double wildfireThreshold); static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, - Permuted& deltaNewton, Permuted& RgProd); + VectorValues& deltaNewton, VectorValues& RgProd); }; diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 3520ffb84..ba0071b51 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -37,7 +37,7 @@ VALUE ISAM2::calculateEstimate(Key key) const { namespace internal { template void optimizeWildfire(const boost::shared_ptr& clique, double threshold, - std::vector& changed, const std::vector& replaced, Permuted& delta, int& count) { + std::vector& changed, const std::vector& replaced, VectorValues& delta, int& count) { // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the // cliques in the children need to be processed @@ -114,7 +114,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, /* ************************************************************************* */ template -int optimizeWildfire(const boost::shared_ptr& root, double threshold, const std::vector& keys, Permuted& delta) { +int optimizeWildfire(const boost::shared_ptr& root, double threshold, const std::vector& keys, VectorValues& delta) { std::vector changed(keys.size(), false); int count = 0; // starting from the root, call optimize on each conditional diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 07e0f518d..0c3c198aa 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -41,7 +41,6 @@ static const double batchThreshold = 0.65; /* ************************************************************************* */ ISAM2::ISAM2(const ISAM2Params& params): - delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_), deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) { if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; @@ -49,15 +48,13 @@ ISAM2::ISAM2(const ISAM2Params& params): /* ************************************************************************* */ ISAM2::ISAM2(): - delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_), deltaDoglegUptodate_(true), deltaUptodate_(true) { if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; } /* ************************************************************************* */ -ISAM2::ISAM2(const ISAM2& other): - delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_) { +ISAM2::ISAM2(const ISAM2& other) { *this = other; } @@ -308,12 +305,12 @@ boost::shared_ptr > ISAM2::recalculate( // Reorder tic(2,"permute global variable index"); - variableIndex_.permute(*colamd); + variableIndex_.permuteInPlace(*colamd); toc(2,"permute global variable index"); tic(3,"permute delta"); - delta_.permute(*colamd); - deltaNewton_.permute(*colamd); - RgProd_.permute(*colamd); + delta_ = delta_.permute(*colamd); + deltaNewton_ = deltaNewton_.permute(*colamd); + RgProd_ = RgProd_.permute(*colamd); toc(3,"permute delta"); tic(4,"permute ordering"); ordering_.permuteWithInverse(*colamdInverse); @@ -429,12 +426,12 @@ boost::shared_ptr > ISAM2::recalculate( // re-eliminate. The reordered variables are also mentioned in the // orphans and the leftover cached factors. tic(3,"permute global variable index"); - variableIndex_.permute(partialSolveResult.fullReordering); + variableIndex_.permuteInPlace(partialSolveResult.fullReordering); toc(3,"permute global variable index"); tic(4,"permute delta"); - delta_.permute(partialSolveResult.fullReordering); - deltaNewton_.permute(partialSolveResult.fullReordering); - RgProd_.permute(partialSolveResult.fullReordering); + delta_ = delta_.permute(partialSolveResult.fullReordering); + deltaNewton_ = deltaNewton_.permute(partialSolveResult.fullReordering); + RgProd_ = RgProd_.permute(partialSolveResult.fullReordering); toc(4,"permute delta"); tic(5,"permute ordering"); ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); @@ -723,8 +720,7 @@ void ISAM2::updateDelta(bool forceFullSolve) const { tic(2, "Copy dx_d"); // Update Delta and linear step doglegDelta_ = doglegResult.Delta; - delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation - delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution + delta_ = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution toc(2, "Copy dx_d"); } @@ -739,7 +735,7 @@ Values ISAM2::calculateEstimate() const { Values ret(theta_); toc(1, "Copy Values"); tic(2, "getDelta"); - const Permuted& delta(getDelta()); + const VectorValues& delta(getDelta()); toc(2, "getDelta"); tic(3, "Expmap"); vector mask(ordering_.nVars(), true); @@ -756,7 +752,7 @@ Values ISAM2::calculateBestEstimate() const { } /* ************************************************************************* */ -const Permuted& ISAM2::getDelta() const { +const VectorValues& ISAM2::getDelta() const { if(!deltaUptodate_) updateDelta(); return delta_; @@ -829,7 +825,7 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) { tic(3, "Compute minimizing step size"); // Compute minimizing step size - double RgNormSq = isam.RgProd_.container().vector().squaredNorm(); + double RgNormSq = isam.RgProd_.vector().squaredNorm(); double step = -gradientSqNorm / RgNormSq; toc(3, "Compute minimizing step size"); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 5804640df..2fd3e0b23 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -206,7 +206,7 @@ struct ISAM2Result { * factors passed as \c newFactors to ISAM2::update(). These indices may be * used later to refer to the factors in order to remove them. */ - FastVector newFactorsIndices; + std::vector newFactorsIndices; /** A struct holding detailed results, which must be enabled with * ISAM2Params::enableDetailedResults. @@ -347,26 +347,16 @@ protected: /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */ VariableIndex variableIndex_; - /** The linear delta from the last linear solution, an update to the estimate in theta */ - VectorValues deltaUnpermuted_; + /** The linear delta from the last linear solution, an update to the estimate in theta + * + * This is \c mutable because it is a "cached" variable - it is not updated + * until either requested with getDelta() or calculateEstimate(), or needed + * during update() to evaluate whether to relinearize variables. + */ + mutable VectorValues delta_; - /** The permutation through which the deltaUnpermuted_ is - * referenced. - * - * Permuting Vector entries would be slow, so for performance we - * instead maintain this permutation through which we access the linear delta - * indirectly - * - * This is \c mutable because it is a "cached" variable - it is not updated - * until either requested with getDelta() or calculateEstimate(), or needed - * during update() to evaluate whether to relinearize variables. - */ - mutable Permuted delta_; - - VectorValues deltaNewtonUnpermuted_; - mutable Permuted deltaNewton_; - VectorValues RgProdUnpermuted_; - mutable Permuted RgProd_; + mutable VectorValues deltaNewton_; + mutable VectorValues RgProd_; mutable bool deltaDoglegUptodate_; /** Indicates whether the current delta is up-to-date, only used @@ -497,7 +487,7 @@ public: Values calculateBestEstimate() const; /** Access the current delta, computed during the last call to update */ - const Permuted& getDelta() const; + const VectorValues& getDelta() const; /** Access the set of nonlinear factors */ const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } @@ -555,7 +545,7 @@ void optimizeInPlace(const ISAM2& isam, VectorValues& delta); /// @return The number of variables that were solved for template int optimizeWildfire(const boost::shared_ptr& root, - double threshold, const std::vector& replaced, Permuted& delta); + double threshold, const std::vector& replaced, VectorValues& delta); /** * Optimize along the gradient direction, with a closed-form computation to diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index bb66943eb..58099d149 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -120,7 +120,7 @@ namespace gtsam { /* ************************************************************************* */ void Values::insert(Key j, const Value& val) { Key key = j; // Non-const duplicate to deal with non-const insert argument - std::pair insertResult = values_.insert(key, val.clone_()); + std::pair insertResult = values_.insert(key, val.clone_()); if(!insertResult.second) throw ValuesKeyAlreadyExists(j); } diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index bc023f115..57e2c0a34 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -47,13 +47,13 @@ typedef PinholeCamera PinholeCal3DS2; typedef PinholeCamera PinholeCal3Bundler; /* ************************************************************************* */ -Point3 pt3(1.0, 2.0, 3.0); -Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); -Pose3 pose3(rt3, pt3); +static Point3 pt3(1.0, 2.0, 3.0); +static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +static Pose3 pose3(rt3, pt3); -Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); -Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); -Cal3Bundler cal3(1.0, 2.0, 3.0); +static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); +static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); +static Cal3Bundler cal3(1.0, 2.0, 3.0); TEST (Serialization, TemplatedValues) { Values values; diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 986538448..b38dae990 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -28,18 +28,18 @@ using namespace std; using namespace gtsam; -Pose3 camera1(Matrix_(3,3, +static Pose3 camera1(Matrix_(3,3, 1., 0., 0., 0.,-1., 0., 0., 0.,-1. ), Point3(0,0,6.25)); -Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); -StereoCamera stereoCam(Pose3(), K); +static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); +static StereoCamera stereoCam(Pose3(), K); // point X Y Z in meters -Point3 p(0, 0, 5); +static Point3 p(0, 0, 5); static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); // Convenience for named keys diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index 41905f874..ac412fa7f 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -20,10 +20,11 @@ namespace gtsam { } /* ************************************************************************* */ - void AllDiff::print(const std::string& s) const { + void AllDiff::print(const std::string& s, + const IndexFormatter& formatter) const { std::cout << s << "AllDiff on "; BOOST_FOREACH (Index dkey, keys_) - std::cout << dkey << " "; + std::cout << formatter(dkey) << " "; std::cout << std::endl; } diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index fb5a47a59..1a560ace2 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -34,7 +34,8 @@ namespace gtsam { AllDiff(const DiscreteKeys& dkeys); // print - virtual void print(const std::string& s = "") const; + virtual void print(const std::string& s = "", + const IndexFormatter& formatter = DefaultIndexFormatter) const; /// Calculate value = expensive ! virtual double operator()(const Values& values) const; diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index a2e260bcd..9ed2f79f1 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -33,9 +33,10 @@ namespace gtsam { } // print - virtual void print(const std::string& s = "") const { - std::cout << s << "BinaryAllDiff on " << keys_[0] << " and " << keys_[1] - << std::endl; + virtual void print(const std::string& s = "", + const IndexFormatter& formatter = DefaultIndexFormatter) const { + std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " + << formatter(keys_[1]) << std::endl; } /// Calculate value diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index e43065f3b..dbc05e3f6 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -15,9 +15,10 @@ namespace gtsam { using namespace std; /* ************************************************************************* */ - void Domain::print(const string& s) const { -// cout << s << ": Domain on " << keys_[0] << " (j=" << keys_[0] -// << ") with values"; + void Domain::print(const string& s, + const IndexFormatter& formatter) const { +// cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" << +// formatter(keys_[0]) << ") with values"; // BOOST_FOREACH (size_t v,values_) cout << " " << v; // cout << endl; BOOST_FOREACH (size_t v,values_) cout << v; diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index a4f0c8054..85b35fe8c 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -66,7 +66,8 @@ namespace gtsam { } // print - virtual void print(const std::string& s = "") const; + virtual void print(const std::string& s = "", + const IndexFormatter& formatter = DefaultIndexFormatter) const; bool contains(size_t value) const { return values_.count(value)>0; diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 6f6d5a3ff..81133e7f7 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -16,8 +16,9 @@ namespace gtsam { using namespace std; /* ************************************************************************* */ - void SingleValue::print(const string& s) const { - cout << s << "SingleValue on " << "j=" << keys_[0] + void SingleValue::print(const string& s, + const IndexFormatter& formatter) const { + cout << s << "SingleValue on " << "j=" << formatter(keys_[0]) << " with value " << value_ << endl; } diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index b229d8b79..1f6e362aa 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -42,7 +42,8 @@ namespace gtsam { } // print - virtual void print(const std::string& s = "") const; + virtual void print(const std::string& s = "", + const IndexFormatter& formatter = DefaultIndexFormatter) const; /// Calculate value virtual double operator()(const Values& values) const; diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp index bf9273ad6..6559754d5 100644 --- a/gtsam_unstable/discrete/tests/testScheduler.cpp +++ b/gtsam_unstable/discrete/tests/testScheduler.cpp @@ -149,7 +149,7 @@ TEST( schedulingExample, test) /* ************************************************************************* */ TEST( schedulingExample, smallFromFile) { - string path("../../../gtsam_unstable/discrete/examples/"); + string path(TOPSRCDIR "/gtsam_unstable/discrete/examples/"); Scheduler s(2, path + "small.csv"); // add areas diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 6ae4a7b20..7ec4e5317 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -41,10 +41,10 @@ using symbol_shorthand::L; /* ************************************************************************* */ // Some numbers that should be consistent among all smoother tests -double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 = +static double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 = 0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1; -const double tol = 1e-4; +static const double tol = 1e-4; /* ************************************************************************* */ TEST_UNSAFE( ISAM, iSAM_smoother ) diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 2a5bf7962..994077777 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -139,112 +139,69 @@ TEST_UNSAFE(ISAM2, AddVariables) { // Create initial state Values theta; - theta.insert((0), Pose2(.1, .2, .3)); + theta.insert(0, Pose2(.1, .2, .3)); theta.insert(100, Point2(.4, .5)); Values newTheta; - newTheta.insert((1), Pose2(.6, .7, .8)); + newTheta.insert(1, Pose2(.6, .7, .8)); - VectorValues deltaUnpermuted; - deltaUnpermuted.insert(0, Vector_(3, .1, .2, .3)); - deltaUnpermuted.insert(1, Vector_(2, .4, .5)); + VectorValues delta; + delta.insert(0, Vector_(3, .1, .2, .3)); + delta.insert(1, Vector_(2, .4, .5)); - Permutation permutation(2); - permutation[0] = 1; - permutation[1] = 0; + VectorValues deltaNewton; + deltaNewton.insert(0, Vector_(3, .1, .2, .3)); + deltaNewton.insert(1, Vector_(2, .4, .5)); - Permuted delta(permutation, deltaUnpermuted); - - VectorValues deltaNewtonUnpermuted; - deltaNewtonUnpermuted.insert(0, Vector_(3, .1, .2, .3)); - deltaNewtonUnpermuted.insert(1, Vector_(2, .4, .5)); - - Permutation permutationNewton(2); - permutationNewton[0] = 1; - permutationNewton[1] = 0; - - Permuted deltaNewton(permutationNewton, deltaNewtonUnpermuted); - - VectorValues deltaRgUnpermuted; - deltaRgUnpermuted.insert(0, Vector_(3, .1, .2, .3)); - deltaRgUnpermuted.insert(1, Vector_(2, .4, .5)); - - Permutation permutationRg(2); - permutationRg[0] = 1; - permutationRg[1] = 0; - - Permuted deltaRg(permutationRg, deltaRgUnpermuted); + VectorValues deltaRg; + deltaRg.insert(0, Vector_(3, .1, .2, .3)); + deltaRg.insert(1, Vector_(2, .4, .5)); vector replacedKeys(2, false); - Ordering ordering; ordering += 100, (0); + Ordering ordering; ordering += 100, 0; ISAM2::Nodes nodes(2); // Verify initial state LONGS_EQUAL(0, ordering[100]); - LONGS_EQUAL(1, ordering[(0)]); - EXPECT(assert_equal(deltaUnpermuted[1], delta[ordering[100]])); - EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[(0)]])); + LONGS_EQUAL(1, ordering[0]); + EXPECT(assert_equal(delta[0], delta[ordering[100]])); + EXPECT(assert_equal(delta[1], delta[ordering[0]])); // Create expected state Values thetaExpected; - thetaExpected.insert((0), Pose2(.1, .2, .3)); + thetaExpected.insert(0, Pose2(.1, .2, .3)); thetaExpected.insert(100, Point2(.4, .5)); - thetaExpected.insert((1), Pose2(.6, .7, .8)); + thetaExpected.insert(1, Pose2(.6, .7, .8)); - VectorValues deltaUnpermutedExpected; - deltaUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaUnpermutedExpected.insert(1, Vector_(2, .4, .5)); - deltaUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); + VectorValues deltaExpected; + deltaExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaExpected.insert(1, Vector_(2, .4, .5)); + deltaExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - Permutation permutationExpected(3); - permutationExpected[0] = 1; - permutationExpected[1] = 0; - permutationExpected[2] = 2; + VectorValues deltaNewtonExpected; + deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaNewtonExpected.insert(1, Vector_(2, .4, .5)); + deltaNewtonExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - Permuted deltaExpected(permutationExpected, deltaUnpermutedExpected); - - VectorValues deltaNewtonUnpermutedExpected; - deltaNewtonUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaNewtonUnpermutedExpected.insert(1, Vector_(2, .4, .5)); - deltaNewtonUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - - Permutation permutationNewtonExpected(3); - permutationNewtonExpected[0] = 1; - permutationNewtonExpected[1] = 0; - permutationNewtonExpected[2] = 2; - - Permuted deltaNewtonExpected(permutationNewtonExpected, deltaNewtonUnpermutedExpected); - - VectorValues deltaRgUnpermutedExpected; - deltaRgUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaRgUnpermutedExpected.insert(1, Vector_(2, .4, .5)); - deltaRgUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - - Permutation permutationRgExpected(3); - permutationRgExpected[0] = 1; - permutationRgExpected[1] = 0; - permutationRgExpected[2] = 2; - - Permuted deltaRgExpected(permutationRgExpected, deltaRgUnpermutedExpected); + VectorValues deltaRgExpected; + deltaRgExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaRgExpected.insert(1, Vector_(2, .4, .5)); + deltaRgExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); vector replacedKeysExpected(3, false); - Ordering orderingExpected; orderingExpected += 100, (0), (1); + Ordering orderingExpected; orderingExpected += 100, 0, 1; - ISAM2::Nodes nodesExpected( - 3, ISAM2::sharedClique()); + ISAM2::Nodes nodesExpected(3, ISAM2::sharedClique()); // Expand initial state ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes); EXPECT(assert_equal(thetaExpected, theta)); - EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted)); - EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation())); - EXPECT(assert_equal(deltaNewtonUnpermutedExpected, deltaNewtonUnpermuted)); - EXPECT(assert_equal(deltaNewtonExpected.permutation(), deltaNewton.permutation())); - EXPECT(assert_equal(deltaRgUnpermutedExpected, deltaRgUnpermuted)); - EXPECT(assert_equal(deltaRgExpected.permutation(), deltaRg.permutation())); + EXPECT(assert_equal(deltaExpected, delta)); + EXPECT(assert_equal(deltaNewtonExpected, deltaNewton)); + EXPECT(assert_equal(deltaRgExpected, deltaRg)); EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); EXPECT(assert_equal(orderingExpected, ordering)); } diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 0f412a0b5..883011da2 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -52,7 +52,7 @@ using symbol_shorthand::L; C3 x1 : x2 C4 x7 : x6 */ -TEST( GaussianJunctionTree, constructor2 ) +TEST( GaussianJunctionTreeB, constructor2 ) { // create a graph Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); @@ -88,7 +88,7 @@ TEST( GaussianJunctionTree, constructor2 ) } /* ************************************************************************* */ -TEST( GaussianJunctionTree, optimizeMultiFrontal ) +TEST( GaussianJunctionTreeB, optimizeMultiFrontal ) { // create a graph GaussianFactorGraph fg; @@ -108,7 +108,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal ) } /* ************************************************************************* */ -TEST( GaussianJunctionTree, optimizeMultiFrontal2) +TEST( GaussianJunctionTreeB, optimizeMultiFrontal2) { // create a graph example::Graph nlfg = createNonlinearFactorGraph(); @@ -126,7 +126,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2) } /* ************************************************************************* */ -TEST(GaussianJunctionTree, slamlike) { +TEST(GaussianJunctionTreeB, slamlike) { Values init; planarSLAM::Graph newfactors; planarSLAM::Graph fullgraph; @@ -188,7 +188,7 @@ TEST(GaussianJunctionTree, slamlike) { } /* ************************************************************************* */ -TEST(GaussianJunctionTree, simpleMarginal) { +TEST(GaussianJunctionTreeB, simpleMarginal) { typedef BayesTree GaussianBayesTree; diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 4679dd740..1efc40cad 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -37,7 +37,7 @@ typedef PriorFactor PosePrior; typedef NonlinearEquality PoseNLE; typedef boost::shared_ptr shared_poseNLE; -Symbol key('x',1); +static Symbol key('x',1); /* ************************************************************************* */ TEST ( NonlinearEquality, linearization ) { @@ -241,8 +241,8 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { } /* ************************************************************************* */ -SharedDiagonal hard_model = noiseModel::Constrained::All(2); -SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); +static SharedDiagonal hard_model = noiseModel::Constrained::All(2); +static SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, unary_basics ) { @@ -504,10 +504,10 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { } // make a realistic calibration matrix -double fov = 60; // degrees -size_t w=640,h=480; -Cal3_S2 K(fov,w,h); -boost::shared_ptr shK(new Cal3_S2(K)); +static double fov = 60; // degrees +static size_t w=640,h=480; +static Cal3_S2 K(fov,w,h); +static boost::shared_ptr shK(new Cal3_S2(K)); // typedefs for visual SLAM example typedef visualSLAM::Graph VGraph; diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 7650925d1..d9094111e 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -174,10 +174,10 @@ BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoF BOOST_CLASS_EXPORT(gtsam::Pose3) BOOST_CLASS_EXPORT(gtsam::Point3) -Point3 pt3(1.0, 2.0, 3.0); -Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); -Pose3 pose3(rt3, pt3); -Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); +static Point3 pt3(1.0, 2.0, 3.0); +static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +static Pose3 pose3(rt3, pt3); +static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); /* ************************************************************************* */ TEST (Serialization, visual_system) { diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 16dfbb8e5..757b488ba 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -4,7 +4,7 @@ find_package(Boost 1.42 COMPONENTS system filesystem thread REQUIRED) # Build the executable itself file(GLOB wrap_srcs "*.cpp") -list(REMOVE_ITEM wrap_srcs wrap.cpp) +list(REMOVE_ITEM wrap_srcs ${CMAKE_CURRENT_SOURCE_DIR}/wrap.cpp) add_library(wrap_lib STATIC ${wrap_srcs}) add_executable(wrap wrap.cpp) target_link_libraries(wrap wrap_lib ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}) @@ -19,7 +19,6 @@ install(FILES matlab.h DESTINATION include/wrap) # Build tests if (GTSAM_BUILD_TESTS) - add_definitions(-DTOPSRCDIR="${CMAKE_SOURCE_DIR}") set(wrap_local_libs wrap_lib ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}) gtsam_add_subdir_tests("wrap" "${wrap_local_libs}" "${wrap_local_libs}" "") endif(GTSAM_BUILD_TESTS)