diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 07801df7a..c9a960a89 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -20,6 +20,8 @@ #pragma once #include // Configuration from CMake #include +#include +#include #ifndef OPTIONALJACOBIAN_NOBOOST #include @@ -96,6 +98,24 @@ public: usurp(dynamic->data()); } + /** + * @brief Constructor from an Eigen::Ref *value*. Will not usurp if dimension is wrong + * @note This is important so we don't overwrite someone else's memory! + */ + template + OptionalJacobian(Eigen::Ref dynamic_ref) : + map_(nullptr) { + if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) { + usurp(dynamic_ref.data()); + } else { + throw std::invalid_argument( + std::string("OptionalJacobian called with wrong dimensions or " + "storage order.\n" + "Expected: ") + + "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")"); + } + } + #ifndef OPTIONALJACOBIAN_NOBOOST /// Constructor with boost::none just makes empty diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index a2ceac834..9769715a1 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -160,7 +160,7 @@ namespace gtsam { const typename Base::LabelFormatter& labelFormatter = &DefaultFormatter) const { auto valueFormatter = [](const double& v) { - return (boost::format("%4.4g") % v).str(); + return (boost::format("%4.8g") % v).str(); }; Base::print(s, labelFormatter, valueFormatter); } diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 826e54b95..99f29b8e5 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -59,7 +59,7 @@ namespace gtsam { /** constant stored in this leaf */ Y constant_; - /** The number of assignments contained within this leaf + /** The number of assignments contained within this leaf. * Particularly useful when leaves have been pruned. */ size_t nrAssignments_; @@ -68,7 +68,7 @@ namespace gtsam { Leaf(const Y& constant, size_t nrAssignments = 1) : constant_(constant), nrAssignments_(nrAssignments) {} - /** return the constant */ + /// Return the constant const Y& constant() const { return constant_; } @@ -81,19 +81,19 @@ namespace gtsam { return constant_ == q.constant_; } - /// polymorphic equality: is q is a leaf, could be + /// polymorphic equality: is q a leaf and is it the same as this leaf? bool sameLeaf(const Node& q) const override { return (q.isLeaf() && q.sameLeaf(*this)); } - /** equality up to tolerance */ + /// equality up to tolerance bool equals(const Node& q, const CompareFunc& compare) const override { const Leaf* other = dynamic_cast(&q); if (!other) return false; return compare(this->constant_, other->constant_); } - /** print */ + /// print void print(const std::string& s, const LabelFormatter& labelFormatter, const ValueFormatter& valueFormatter) const override { std::cout << s << " Leaf " << valueFormatter(constant_) << std::endl; @@ -122,8 +122,8 @@ namespace gtsam { /// Apply unary operator with assignment NodePtr apply(const UnaryAssignment& op, - const Assignment& choices) const override { - NodePtr f(new Leaf(op(choices, constant_), nrAssignments_)); + const Assignment& assignment) const override { + NodePtr f(new Leaf(op(assignment, constant_), nrAssignments_)); return f; } @@ -168,7 +168,10 @@ namespace gtsam { std::vector branches_; private: - /** incremental allSame */ + /** + * Incremental allSame. + * Records if all the branches are the same leaf. + */ size_t allSame_; using ChoicePtr = boost::shared_ptr; @@ -181,9 +184,9 @@ namespace gtsam { #endif } - /** If all branches of a choice node f are the same, just return a branch */ + /// If all branches of a choice node f are the same, just return a branch. static NodePtr Unique(const ChoicePtr& f) { -#ifndef DT_NO_PRUNING +#ifndef GTSAM_DT_NO_PRUNING if (f->allSame_) { assert(f->branches().size() > 0); NodePtr f0 = f->branches_[0]; @@ -205,15 +208,13 @@ namespace gtsam { bool isLeaf() const override { return false; } - /** Constructor, given choice label and mandatory expected branch count */ + /// Constructor, given choice label and mandatory expected branch count. Choice(const L& label, size_t count) : label_(label), allSame_(true) { branches_.reserve(count); } - /** - * Construct from applying binary op to two Choice nodes - */ + /// Construct from applying binary op to two Choice nodes. Choice(const Choice& f, const Choice& g, const Binary& op) : allSame_(true) { // Choose what to do based on label @@ -241,6 +242,7 @@ namespace gtsam { } } + /// Return the label of this choice node. const L& label() const { return label_; } @@ -262,7 +264,7 @@ namespace gtsam { branches_.push_back(node); } - /** print (as a tree) */ + /// print (as a tree). void print(const std::string& s, const LabelFormatter& labelFormatter, const ValueFormatter& valueFormatter) const override { std::cout << s << " Choice("; @@ -308,7 +310,7 @@ namespace gtsam { return (q.isLeaf() && q.sameLeaf(*this)); } - /** equality */ + /// equality bool equals(const Node& q, const CompareFunc& compare) const override { const Choice* other = dynamic_cast(&q); if (!other) return false; @@ -321,7 +323,7 @@ namespace gtsam { return true; } - /** evaluate */ + /// evaluate const Y& operator()(const Assignment& x) const override { #ifndef NDEBUG typename Assignment::const_iterator it = x.find(label_); @@ -336,13 +338,13 @@ namespace gtsam { return (*child)(x); } - /** - * Construct from applying unary op to a Choice node - */ + /// Construct from applying unary op to a Choice node. Choice(const L& label, const Choice& f, const Unary& op) : label_(label), allSame_(true) { branches_.reserve(f.branches_.size()); // reserve space - for (const NodePtr& branch : f.branches_) push_back(branch->apply(op)); + for (const NodePtr& branch : f.branches_) { + push_back(branch->apply(op)); + } } /** @@ -353,28 +355,28 @@ namespace gtsam { * @param f The original choice node to apply the op on. * @param op Function to apply on the choice node. Takes Assignment and * value as arguments. - * @param choices The Assignment that will go to op. + * @param assignment The Assignment that will go to op. */ Choice(const L& label, const Choice& f, const UnaryAssignment& op, - const Assignment& choices) + const Assignment& assignment) : label_(label), allSame_(true) { branches_.reserve(f.branches_.size()); // reserve space - Assignment choices_ = choices; + Assignment assignment_ = assignment; for (size_t i = 0; i < f.branches_.size(); i++) { - choices_[label_] = i; // Set assignment for label to i + assignment_[label_] = i; // Set assignment for label to i const NodePtr branch = f.branches_[i]; - push_back(branch->apply(op, choices_)); + push_back(branch->apply(op, assignment_)); - // Remove the choice so we are backtracking - auto choice_it = choices_.find(label_); - choices_.erase(choice_it); + // Remove the assignment so we are backtracking + auto assignment_it = assignment_.find(label_); + assignment_.erase(assignment_it); } } - /** apply unary operator */ + /// apply unary operator. NodePtr apply(const Unary& op) const override { auto r = boost::make_shared(label_, *this, op); return Unique(r); @@ -382,8 +384,8 @@ namespace gtsam { /// Apply unary operator with assignment NodePtr apply(const UnaryAssignment& op, - const Assignment& choices) const override { - auto r = boost::make_shared(label_, *this, op, choices); + const Assignment& assignment) const override { + auto r = boost::make_shared(label_, *this, op, assignment); return Unique(r); } @@ -678,7 +680,16 @@ namespace gtsam { } /****************************************************************************/ - // Functor performing depth-first visit without Assignment argument. + /** + * Functor performing depth-first visit to each leaf with the leaf value as + * the argument. + * + * NOTE: We differentiate between leaves and assignments. Concretely, a 3 + * binary variable tree will have 2^3=8 assignments, but based on pruning, it + * can have less than 8 leaves. For example, if a tree has all assignment + * values as 1, then pruning will cause the tree to have only 1 leaf yet 8 + * assignments. + */ template struct Visit { using F = std::function; @@ -707,33 +718,74 @@ namespace gtsam { } /****************************************************************************/ - // Functor performing depth-first visit with Assignment argument. + /** + * Functor performing depth-first visit to each leaf with the Leaf object + * passed as an argument. + * + * NOTE: We differentiate between leaves and assignments. Concretely, a 3 + * binary variable tree will have 2^3=8 assignments, but based on pruning, it + * can have <8 leaves. For example, if a tree has all assignment values as 1, + * then pruning will cause the tree to have only 1 leaf yet 8 assignments. + */ + template + struct VisitLeaf { + using F = std::function::Leaf&)>; + explicit VisitLeaf(F f) : f(f) {} ///< Construct from folding function. + F f; ///< folding function object. + + /// Do a depth-first visit on the tree rooted at node. + void operator()(const typename DecisionTree::NodePtr& node) const { + using Leaf = typename DecisionTree::Leaf; + if (auto leaf = boost::dynamic_pointer_cast(node)) + return f(*leaf); + + using Choice = typename DecisionTree::Choice; + auto choice = boost::dynamic_pointer_cast(node); + if (!choice) + throw std::invalid_argument("DecisionTree::VisitLeaf: Invalid NodePtr"); + for (auto&& branch : choice->branches()) (*this)(branch); // recurse! + } + }; + + template + template + void DecisionTree::visitLeaf(Func f) const { + VisitLeaf visit(f); + visit(root_); + } + + /****************************************************************************/ + /** + * Functor performing depth-first visit to each leaf with the leaf's + * `Assignment` and value passed as arguments. + * + * NOTE: Follows the same pruning semantics as `visit`. + */ template struct VisitWith { - using Choices = Assignment; - using F = std::function; + using F = std::function&, const Y&)>; explicit VisitWith(F f) : f(f) {} ///< Construct from folding function. - Choices choices; ///< Assignment, mutating through recursion. - F f; ///< folding function object. + Assignment assignment; ///< Assignment, mutating through recursion. + F f; ///< folding function object. /// Do a depth-first visit on the tree rooted at node. void operator()(const typename DecisionTree::NodePtr& node) { using Leaf = typename DecisionTree::Leaf; if (auto leaf = boost::dynamic_pointer_cast(node)) - return f(choices, leaf->constant()); + return f(assignment, leaf->constant()); using Choice = typename DecisionTree::Choice; auto choice = boost::dynamic_pointer_cast(node); if (!choice) throw std::invalid_argument("DecisionTree::VisitWith: Invalid NodePtr"); for (size_t i = 0; i < choice->nrChoices(); i++) { - choices[choice->label()] = i; // Set assignment for label to i + assignment[choice->label()] = i; // Set assignment for label to i (*this)(choice->branches()[i]); // recurse! // Remove the choice so we are backtracking - auto choice_it = choices.find(choice->label()); - choices.erase(choice_it); + auto choice_it = assignment.find(choice->label()); + assignment.erase(choice_it); } } }; @@ -763,12 +815,26 @@ namespace gtsam { } /****************************************************************************/ - // labels is just done with a visit + /** + * Get (partial) labels by performing a visit. + * + * This method performs a depth-first search to go to every leaf and records + * the keys assignment which leads to that leaf. Since the tree can be pruned, + * there might be a leaf at a lower depth which results in a partial + * assignment (i.e. not all keys are specified). + * + * E.g. given a tree with 3 keys, there may be a branch where the 3rd key has + * the same values for all the leaves. This leads to the branch being pruned + * so we get a leaf which is arrived at by just the first 2 keys and their + * assignments. + */ template std::set DecisionTree::labels() const { std::set unique; - auto f = [&](const Assignment& choices, const Y&) { - for (auto&& kv : choices) unique.insert(kv.first); + auto f = [&](const Assignment& assignment, const Y&) { + for (auto&& kv : assignment) { + unique.insert(kv.first); + } }; visitWith(f); return unique; @@ -817,8 +883,8 @@ namespace gtsam { throw std::runtime_error( "DecisionTree::apply(unary op) undefined for empty tree."); } - Assignment choices; - return DecisionTree(root_->apply(op, choices)); + Assignment assignment; + return DecisionTree(root_->apply(op, assignment)); } /****************************************************************************/ diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index c0a2a7a1c..1f45d320b 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -105,7 +105,7 @@ namespace gtsam { virtual const Y& operator()(const Assignment& x) const = 0; virtual Ptr apply(const Unary& op) const = 0; virtual Ptr apply(const UnaryAssignment& op, - const Assignment& choices) const = 0; + const Assignment& assignment) const = 0; virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0; virtual Ptr apply_g_op_fL(const Leaf&, const Binary&) const = 0; virtual Ptr apply_g_op_fC(const Choice&, const Binary&) const = 0; @@ -153,7 +153,7 @@ namespace gtsam { /** Create a constant */ explicit DecisionTree(const Y& y); - /** Create a new leaf function splitting on a variable */ + /// Create tree with 2 assignments `y1`, `y2`, splitting on variable `label` DecisionTree(const L& label, const Y& y1, const Y& y2); /** Allow Label+Cardinality for convenience */ @@ -219,9 +219,8 @@ namespace gtsam { /// @name Standard Interface /// @{ - /** Make virtual */ - virtual ~DecisionTree() { - } + /// Make virtual + virtual ~DecisionTree() {} /// Check if tree is empty. bool empty() const { return !root_; } @@ -234,11 +233,13 @@ namespace gtsam { /** * @brief Visit all leaves in depth-first fashion. - * - * @param f side-effect taking a value. - * - * @note Due to pruning, leaves might not exhaust choices. - * + * + * @param f (side-effect) Function taking a value. + * + * @note Due to pruning, the number of leaves may not be the same as the + * number of assignments. E.g. if we have a tree on 2 binary variables with + * all values being 1, then there are 2^2=4 assignments, but only 1 leaf. + * * Example: * int sum = 0; * auto visitor = [&](int y) { sum += y; }; @@ -249,14 +250,33 @@ namespace gtsam { /** * @brief Visit all leaves in depth-first fashion. - * - * @param f side-effect taking an assignment and a value. - * - * @note Due to pruning, leaves might not exhaust choices. - * + * + * @param f (side-effect) Function taking the leaf node pointer. + * + * @note Due to pruning, the number of leaves may not be the same as the + * number of assignments. E.g. if we have a tree on 2 binary variables with + * all values being 1, then there are 2^2=4 assignments, but only 1 leaf. + * * Example: * int sum = 0; - * auto visitor = [&](const Assignment& choices, int y) { sum += y; }; + * auto visitor = [&](int y) { sum += y; }; + * tree.visitWith(visitor); + */ + template + void visitLeaf(Func f) const; + + /** + * @brief Visit all leaves in depth-first fashion. + * + * @param f (side-effect) Function taking an assignment and a value. + * + * @note Due to pruning, the number of leaves may not be the same as the + * number of assignments. E.g. if we have a tree on 2 binary variables with + * all values being 1, then there are 2^2=4 assignments, but only 1 leaf. + * + * Example: + * int sum = 0; + * auto visitor = [&](const Assignment& assignment, int y) { sum += y; }; * tree.visitWith(visitor); */ template @@ -275,7 +295,7 @@ namespace gtsam { * * @note X is always passed by value. * @note Due to pruning, leaves might not exhaust choices. - * + * * Example: * auto add = [](const double& y, double x) { return y + x; }; * double sum = tree.fold(add, 0.0); diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index acd4d4af2..4e16fc689 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -287,12 +287,16 @@ namespace gtsam { cardinalities_(keys.cardinalities()) {} /* ************************************************************************ */ - DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrLeaves) const { - const size_t N = maxNrLeaves; + DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const { + const size_t N = maxNrAssignments; // Get the probabilities in the decision tree so we can threshold. std::vector probabilities; - this->visit([&](const double& prob) { probabilities.emplace_back(prob); }); + this->visitLeaf([&](const Leaf& leaf) { + size_t nrAssignments = leaf.nrAssignments(); + double prob = leaf.constant(); + probabilities.insert(probabilities.end(), nrAssignments, prob); + }); // The number of probabilities can be lower than max_leaves if (probabilities.size() <= N) { diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 1f3d69292..86fa44649 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -174,13 +174,21 @@ namespace gtsam { * @brief Prune the decision tree of discrete variables. * * Pruning will set the leaves to be "pruned" to 0 indicating a 0 - * probability. - * A leaf is pruned if it is not in the top `maxNrLeaves` values. + * probability. An assignment is pruned if it is not in the top + * `maxNrAssignments` values. * - * @param maxNrLeaves The maximum number of leaves to keep. + * A violation can occur if there are more + * duplicate values than `maxNrAssignments`. A violation here is the need to + * un-prune the decision tree (e.g. all assignment values are 1.0). We could + * have another case where some subset of duplicates exist (e.g. for a tree + * with 8 assignments we have 1, 1, 1, 1, 0.8, 0.7, 0.6, 0.5), but this is + * not a violation since the for `maxNrAssignments=5` the top values are (1, + * 0.8). + * + * @param maxNrAssignments The maximum number of assignments to keep. * @return DecisionTreeFactor */ - DecisionTreeFactor prune(size_t maxNrLeaves) const; + DecisionTreeFactor prune(size_t maxNrAssignments) const; /// @} /// @name Wrapper support diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index c800321d6..6a3fb2388 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -20,7 +20,7 @@ #include // make sure we have traits #include // headers first to make sure no missing headers -//#define DT_NO_PRUNING +//#define GTSAM_DT_NO_PRUNING #include #include // for convert only #define DISABLE_TIMING diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 14cf307a5..5ccbcf916 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -18,7 +18,7 @@ */ // #define DT_DEBUG_MEMORY -// #define DT_NO_PRUNING +// #define GTSAM_DT_NO_PRUNING #define DISABLE_DOT #include diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index 83b586bbb..84e45a0f5 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -113,18 +113,32 @@ TEST(DecisionTreeFactor, Prune) { DecisionTreeFactor f(A & B & C, "1 5 3 7 2 6 4 8"); // Only keep the leaves with the top 5 values. - size_t maxNrLeaves = 5; - auto pruned5 = f.prune(maxNrLeaves); + size_t maxNrAssignments = 5; + auto pruned5 = f.prune(maxNrAssignments); // Pruned leaves should be 0 DecisionTreeFactor expected(A & B & C, "0 5 0 7 0 6 4 8"); EXPECT(assert_equal(expected, pruned5)); // Check for more extreme pruning where we only keep the top 2 leaves - maxNrLeaves = 2; - auto pruned2 = f.prune(maxNrLeaves); + maxNrAssignments = 2; + auto pruned2 = f.prune(maxNrAssignments); DecisionTreeFactor expected2(A & B & C, "0 0 0 7 0 0 0 8"); EXPECT(assert_equal(expected2, pruned2)); + + DiscreteKey D(4, 2); + DecisionTreeFactor factor( + D & C & B & A, + "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " + "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0"); + + DecisionTreeFactor expected3( + D & C & B & A, + "0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 " + "0.999952870000 1.0 1.0 1.0 1.0"); + maxNrAssignments = 5; + auto pruned3 = factor.prune(maxNrAssignments); + EXPECT(assert_equal(expected3, pruned3)); } /* ************************************************************************* */ @@ -133,7 +147,7 @@ TEST(DecisionTreeFactor, DotWithNames) { DecisionTreeFactor f(A & B, "1 2 3 4 5 6"); auto formatter = [](Key key) { return key == 12 ? "A" : "B"; }; - for (bool showZero:{true, false}) { + for (bool showZero:{true, false}) { string actual = f.dot(formatter, showZero); // pretty weak test, as ids are pointers and not stable across platforms. string expected = "digraph G {"; @@ -215,4 +229,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 0a7d869ec..3d9621aff 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -415,16 +415,16 @@ TEST(DiscreteFactorGraph, DotWithNames) { "graph {\n" " size=\"5,5\";\n" "\n" - " varC[label=\"C\"];\n" - " varA[label=\"A\"];\n" - " varB[label=\"B\"];\n" + " var0[label=\"C\"];\n" + " var1[label=\"A\"];\n" + " var2[label=\"B\"];\n" "\n" " factor0[label=\"\", shape=point];\n" - " varC--factor0;\n" - " varA--factor0;\n" + " var0--factor0;\n" + " var1--factor0;\n" " factor1[label=\"\", shape=point];\n" - " varC--factor1;\n" - " varB--factor1;\n" + " var0--factor1;\n" + " var2--factor1;\n" "}\n"; EXPECT(actual == expected); } diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index d8060cfcf..06c32526b 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -113,6 +113,18 @@ list circleCircleIntersection(Point2 c1, double r1, Point2 c2, return circleCircleIntersection(c1, c2, fh); } +Point2Pair means(const std::vector &abPointPairs) { + const size_t n = abPointPairs.size(); + if (n == 0) throw std::invalid_argument("Point2::mean input Point2Pair vector is empty"); + Point2 aSum(0, 0), bSum(0, 0); + for (const Point2Pair &abPair : abPointPairs) { + aSum += abPair.first; + bSum += abPair.second; + } + const double f = 1.0 / n; + return {aSum * f, bSum * f}; +} + /* ************************************************************************* */ ostream &operator<<(ostream &os, const gtsam::Point2Pair &p) { os << p.first << " <-> " << p.second; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index cdb9f4480..d8b6daca8 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -71,6 +71,9 @@ GTSAM_EXPORT boost::optional circleCircleIntersection(double R_d, double * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well. */ GTSAM_EXPORT std::list circleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh); + +/// Calculate the two means of a set of Point2 pairs +GTSAM_EXPORT Point2Pair means(const std::vector &abPointPairs); /** * @brief Intersect 2 circles diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 96bdce69b..b37674b92 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -365,7 +365,7 @@ boost::optional Pose2::Align(const Matrix& a, const Matrix& b) { "Pose2:Align expects 2*N matrices of equal shape."); } Point2Pairs ab_pairs; - for (size_t j=0; j < a.cols(); j++) { + for (Eigen::Index j = 0; j < a.cols(); j++) { ab_pairs.emplace_back(a.col(j), b.col(j)); } return Pose2::Align(ab_pairs); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 0d75113a2..466c5a42a 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -353,6 +353,10 @@ GTSAM_EXPORT boost::optional GTSAM_DEPRECATED align(const Point2Pairs& pairs); #endif +// Convenience typedef +using Pose2Pair = std::pair; +using Pose2Pairs = std::vector; + template <> struct traits : public internal::LieGroup {}; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index b88d812a4..2da51a625 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -473,7 +473,7 @@ boost::optional Pose3::Align(const Matrix& a, const Matrix& b) { "Pose3:Align expects 3*N matrices of equal shape."); } Point3Pairs abPointPairs; - for (size_t j=0; j < a.cols(); j++) { + for (Eigen::Index j = 0; j < a.cols(); j++) { abPointPairs.emplace_back(a.col(j), b.col(j)); } return Pose3::Align(abPointPairs); diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 283147e4c..9bf631e50 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -129,6 +129,19 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) { } } +/* ************************************************************************* */ +Rot2 Rot2::ClosestTo(const Matrix2& M) { + Eigen::JacobiSVD svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV); + const Matrix2& U = svd.matrixU(); + const Matrix2& V = svd.matrixV(); + const double det = (U * V.transpose()).determinant(); + Matrix2 M_prime = (U * Vector2(1, det).asDiagonal() * V.transpose()); + + double c = M_prime(0, 0); + double s = M_prime(1, 0); + return Rot2::fromCosSin(c, s); +} + /* ************************************************************************* */ } // gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index ec30c6657..2690ca248 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -14,6 +14,7 @@ * @brief 2D rotation * @date Dec 9, 2009 * @author Frank Dellaert + * @author John Lambert */ #pragma once @@ -209,6 +210,9 @@ namespace gtsam { /** return 2*2 transpose (inverse) rotation matrix */ Matrix2 transpose() const; + /** Find closest valid rotation matrix, given a 2x2 matrix */ + static Rot2 ClosestTo(const Matrix2& M); + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp new file mode 100644 index 000000000..4ed3351f8 --- /dev/null +++ b/gtsam/geometry/Similarity2.cpp @@ -0,0 +1,242 @@ +/* ---------------------------------------------------------------------------- + + * 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 Similarity2.cpp + * @brief Implementation of Similarity2 transform + * @author John Lambert, Varun Agrawal + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +using std::vector; + +namespace internal { + +/// Subtract centroids from point pairs. +static Point2Pairs SubtractCentroids(const Point2Pairs& abPointPairs, + const Point2Pair& centroids) { + Point2Pairs d_abPointPairs; + for (const Point2Pair& abPair : abPointPairs) { + Point2 da = abPair.first - centroids.first; + Point2 db = abPair.second - centroids.second; + d_abPointPairs.emplace_back(da, db); + } + return d_abPointPairs; +} + +/// Form inner products x and y and calculate scale. +static double CalculateScale(const Point2Pairs& d_abPointPairs, + const Rot2& aRb) { + double x = 0, y = 0; + Point2 da, db; + + for (const Point2Pair& d_abPair : d_abPointPairs) { + std::tie(da, db) = d_abPair; + const Vector2 da_prime = aRb * db; + y += da.transpose() * da_prime; + x += da_prime.transpose() * da_prime; + } + const double s = y / x; + return s; +} + +/// Form outer product H. +static Matrix2 CalculateH(const Point2Pairs& d_abPointPairs) { + Matrix2 H = Z_2x2; + for (const Point2Pair& d_abPair : d_abPointPairs) { + H += d_abPair.first * d_abPair.second.transpose(); + } + return H; +} + +/** + * @brief This method estimates the similarity transform from differences point + * pairs, given a known or estimated rotation and point centroids. + * + * @param d_abPointPairs + * @param aRb + * @param centroids + * @return Similarity2 + */ +static Similarity2 Align(const Point2Pairs& d_abPointPairs, const Rot2& aRb, + const Point2Pair& centroids) { + const double s = CalculateScale(d_abPointPairs, aRb); + // dividing aTb by s is required because the registration cost function + // minimizes ||a - sRb - t||, whereas Sim(2) computes s(Rb + t) + const Point2 aTb = (centroids.first - s * (aRb * centroids.second)) / s; + return Similarity2(aRb, aTb, s); +} + +/** + * @brief This method estimates the similarity transform from point pairs, + * given a known or estimated rotation. + * Refer to: + * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf + * Chapter 3 + * + * @param abPointPairs + * @param aRb + * @return Similarity2 + */ +static Similarity2 AlignGivenR(const Point2Pairs& abPointPairs, + const Rot2& aRb) { + auto centroids = means(abPointPairs); + auto d_abPointPairs = internal::SubtractCentroids(abPointPairs, centroids); + return internal::Align(d_abPointPairs, aRb, centroids); +} +} // namespace internal + +Similarity2::Similarity2() : t_(0, 0), s_(1) {} + +Similarity2::Similarity2(double s) : t_(0, 0), s_(s) {} + +Similarity2::Similarity2(const Rot2& R, const Point2& t, double s) + : R_(R), t_(t), s_(s) {} + +Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s) + : R_(Rot2::ClosestTo(R)), t_(t), s_(s) {} + +Similarity2::Similarity2(const Matrix3& T) + : R_(Rot2::ClosestTo(T.topLeftCorner<2, 2>())), + t_(T.topRightCorner<2, 1>()), + s_(1.0 / T(2, 2)) {} + +bool Similarity2::equals(const Similarity2& other, double tol) const { + return R_.equals(other.R_, tol) && + traits::Equals(t_, other.t_, tol) && s_ < (other.s_ + tol) && + s_ > (other.s_ - tol); +} + +bool Similarity2::operator==(const Similarity2& other) const { + return R_.matrix() == other.R_.matrix() && t_ == other.t_ && s_ == other.s_; +} + +void Similarity2::print(const std::string& s) const { + std::cout << std::endl; + std::cout << s; + rotation().print("\nR:\n"); + std::cout << "t: " << translation().transpose() << " s: " << scale() + << std::endl; +} + +Similarity2 Similarity2::identity() { return Similarity2(); } + +Similarity2 Similarity2::operator*(const Similarity2& S) const { + return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_); +} + +Similarity2 Similarity2::inverse() const { + const Rot2 Rt = R_.inverse(); + const Point2 sRt = Rt * (-s_ * t_); + return Similarity2(Rt, sRt, 1.0 / s_); +} + +Point2 Similarity2::transformFrom(const Point2& p) const { + const Point2 q = R_ * p + t_; + return s_ * q; +} + +Pose2 Similarity2::transformFrom(const Pose2& T) const { + Rot2 R = R_.compose(T.rotation()); + Point2 t = Point2(s_ * (R_ * T.translation() + t_)); + return Pose2(R, t); +} + +Point2 Similarity2::operator*(const Point2& p) const { + return transformFrom(p); +} + +Similarity2 Similarity2::Align(const Point2Pairs& abPointPairs) { + // Refer to Chapter 3 of + // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf + if (abPointPairs.size() < 2) + throw std::runtime_error("input should have at least 2 pairs of points"); + auto centroids = means(abPointPairs); + auto d_abPointPairs = internal::SubtractCentroids(abPointPairs, centroids); + Matrix2 H = internal::CalculateH(d_abPointPairs); + // ClosestTo finds rotation matrix closest to H in Frobenius sense + Rot2 aRb = Rot2::ClosestTo(H); + return internal::Align(d_abPointPairs, aRb, centroids); +} + +Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) { + const size_t n = abPosePairs.size(); + if (n < 2) + throw std::runtime_error("input should have at least 2 pairs of poses"); + + // calculate rotation + vector rotations; + Point2Pairs abPointPairs; + rotations.reserve(n); + abPointPairs.reserve(n); + // Below denotes the pose of the i'th object/camera/etc + // in frame "a" or frame "b". + Pose2 aTi, bTi; + for (const Pose2Pair& abPair : abPosePairs) { + std::tie(aTi, bTi) = abPair; + const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse()); + rotations.emplace_back(aRb); + abPointPairs.emplace_back(aTi.translation(), bTi.translation()); + } + const Rot2 aRb_estimate = FindKarcherMean(rotations); + + return internal::AlignGivenR(abPointPairs, aRb_estimate); +} + +Vector4 Similarity2::Logmap(const Similarity2& S, // + OptionalJacobian<4, 4> Hm) { + const Vector2 u = S.t_; + const Vector1 w = Rot2::Logmap(S.R_); + const double s = log(S.s_); + Vector4 result; + result << u, w, s; + if (Hm) { + throw std::runtime_error("Similarity2::Logmap: derivative not implemented"); + } + return result; +} + +Similarity2 Similarity2::Expmap(const Vector4& v, // + OptionalJacobian<4, 4> Hm) { + const Vector2 t = v.head<2>(); + const Rot2 R = Rot2::Expmap(v.segment<1>(2)); + const double s = v[3]; + if (Hm) { + throw std::runtime_error("Similarity2::Expmap: derivative not implemented"); + } + return Similarity2(R, t, s); +} + +Matrix4 Similarity2::AdjointMap() const { + throw std::runtime_error("Similarity2::AdjointMap not implemented"); +} + +std::ostream& operator<<(std::ostream& os, const Similarity2& p) { + os << "[" << p.rotation().theta() << " " << p.translation().transpose() << " " + << p.scale() << "]\';"; + return os; +} + +Matrix3 Similarity2::matrix() const { + Matrix3 T; + T.topRows<2>() << R_.matrix(), t_; + T.bottomRows<1>() << 0, 0, 1.0 / s_; + return T; +} + +} // namespace gtsam diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h new file mode 100644 index 000000000..05f10d149 --- /dev/null +++ b/gtsam/geometry/Similarity2.h @@ -0,0 +1,200 @@ +/* ---------------------------------------------------------------------------- + + * 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 Similarity2.h + * @brief Implementation of Similarity2 transform + * @author John Lambert, Varun Agrawal + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +// Forward declarations +class Pose2; + +/** + * 2D similarity transform + */ +class GTSAM_EXPORT Similarity2 : public LieGroup { + /// @name Pose Concept + /// @{ + typedef Rot2 Rotation; + typedef Point2 Translation; + /// @} + + private: + Rot2 R_; + Point2 t_; + double s_; + + public: + /// @name Constructors + /// @{ + + /// Default constructor + Similarity2(); + + /// Construct pure scaling + Similarity2(double s); + + /// Construct from GTSAM types + Similarity2(const Rot2& R, const Point2& t, double s); + + /// Construct from Eigen types + Similarity2(const Matrix2& R, const Vector2& t, double s); + + /// Construct from matrix [R t; 0 s^-1] + Similarity2(const Matrix3& T); + + /// @} + /// @name Testable + /// @{ + + /// Compare with tolerance + bool equals(const Similarity2& sim, double tol) const; + + /// Exact equality + bool operator==(const Similarity2& other) const; + + /// Print with optional string + void print(const std::string& s) const; + + friend std::ostream& operator<<(std::ostream& os, const Similarity2& p); + + /// @} + /// @name Group + /// @{ + + /// Return an identity transform + static Similarity2 identity(); + + /// Composition + Similarity2 operator*(const Similarity2& S) const; + + /// Return the inverse + Similarity2 inverse() const; + + /// @} + /// @name Group action on Point2 + /// @{ + + /// Action on a point p is s*(R*p+t) + Point2 transformFrom(const Point2& p) const; + + /** + * Action on a pose T. + * |Rs ts| |R t| |Rs*R Rs*t+ts| + * |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim2 object. + * To retrieve a Pose2, we normalized the scale value into 1. + * |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)| + * | 0 1/s | = | 0 1 | + * + * This group action satisfies the compatibility condition. + * For more details, refer to: https://en.wikipedia.org/wiki/Group_action + */ + Pose2 transformFrom(const Pose2& T) const; + + /* syntactic sugar for transformFrom */ + Point2 operator*(const Point2& p) const; + + /** + * Create Similarity2 by aligning at least two point pairs + */ + static Similarity2 Align(const Point2Pairs& abPointPairs); + + /** + * Create the Similarity2 object that aligns at least two pose pairs. + * Each pair is of the form (aTi, bTi). + * Given a list of pairs in frame a, and a list of pairs in frame b, + Align() + * will compute the best-fit Similarity2 aSb transformation to align them. + * First, the rotation aRb will be computed as the average (Karcher mean) + of + * many estimates aRb (from each pair). Afterwards, the scale factor will + be computed + * using the algorithm described here: + * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf + */ + static Similarity2 Align(const std::vector& abPosePairs); + + /// @} + /// @name Lie Group + /// @{ + + /** + * Log map at the identity + * \f$ [t_x, t_y, \delta, \lambda] \f$ + */ + static Vector4 Logmap(const Similarity2& S, // + OptionalJacobian<4, 4> Hm = boost::none); + + /// Exponential map at the identity + static Similarity2 Expmap(const Vector4& v, // + OptionalJacobian<4, 4> Hm = boost::none); + + /// Chart at the origin + struct ChartAtOrigin { + static Similarity2 Retract(const Vector4& v, + ChartJacobian H = boost::none) { + return Similarity2::Expmap(v, H); + } + static Vector4 Local(const Similarity2& other, + ChartJacobian H = boost::none) { + return Similarity2::Logmap(other, H); + } + }; + + /// Project from one tangent space to another + Matrix4 AdjointMap() const; + + using LieGroup::inverse; + + /// @} + /// @name Standard interface + /// @{ + + /// Calculate 4*4 matrix group equivalent + Matrix3 matrix() const; + + /// Return a GTSAM rotation + Rot2 rotation() const { return R_; } + + /// Return a GTSAM translation + Point2 translation() const { return t_; } + + /// Return the scale + double scale() const { return s_; } + + /// Dimensionality of tangent space = 4 DOF - used to autodetect sizes + inline static size_t Dim() { return 4; } + + /// Dimensionality of tangent space = 4 DOF + inline size_t dim() const { return 4; } + + /// @} +}; + +template <> +struct traits : public internal::LieGroup {}; + +template <> +struct traits : public internal::LieGroup {}; + +} // namespace gtsam diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index e8d6e7510..7fde974c5 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -26,7 +26,7 @@ namespace gtsam { using std::vector; -namespace { +namespace internal { /// Subtract centroids from point pairs. static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs, const Point3Pair ¢roids) { @@ -81,10 +81,10 @@ static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb, static Similarity3 alignGivenR(const Point3Pairs &abPointPairs, const Rot3 &aRb) { auto centroids = means(abPointPairs); - auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); + auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids); return align(d_abPointPairs, aRb, centroids); } -} // namespace +} // namespace internal Similarity3::Similarity3() : t_(0,0,0), s_(1) { @@ -165,11 +165,11 @@ Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) { if (abPointPairs.size() < 3) throw std::runtime_error("input should have at least 3 pairs of points"); auto centroids = means(abPointPairs); - auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); - Matrix3 H = calculateH(d_abPointPairs); + auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids); + Matrix3 H = internal::calculateH(d_abPointPairs); // ClosestTo finds rotation matrix closest to H in Frobenius sense Rot3 aRb = Rot3::ClosestTo(H); - return align(d_abPointPairs, aRb, centroids); + return internal::align(d_abPointPairs, aRb, centroids); } Similarity3 Similarity3::Align(const vector &abPosePairs) { @@ -192,7 +192,7 @@ Similarity3 Similarity3::Align(const vector &abPosePairs) { } const Rot3 aRb_estimate = FindKarcherMean(rotations); - return alignGivenR(abPointPairs, aRb_estimate); + return internal::alignGivenR(abPointPairs, aRb_estimate); } Matrix4 Similarity3::wedge(const Vector7 &xi) { @@ -283,15 +283,11 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) { return os; } -const Matrix4 Similarity3::matrix() const { +Matrix4 Similarity3::matrix() const { Matrix4 T; T.topRows<3>() << R_.matrix(), t_; T.bottomRows<1>() << 0, 0, 0, 1.0 / s_; return T; } -Similarity3::operator Pose3() const { - return Pose3(R_, s_ * t_); -} - } // namespace gtsam diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index 0ef787b05..845d4c810 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -18,13 +18,12 @@ #pragma once -#include -#include -#include #include #include #include - +#include +#include +#include namespace gtsam { @@ -34,108 +33,106 @@ class Pose3; /** * 3D similarity transform */ -class Similarity3: public LieGroup { - +class GTSAM_EXPORT Similarity3 : public LieGroup { /// @name Pose Concept /// @{ typedef Rot3 Rotation; typedef Point3 Translation; /// @} -private: + private: Rot3 R_; Point3 t_; double s_; -public: - + public: /// @name Constructors /// @{ /// Default constructor - GTSAM_EXPORT Similarity3(); + Similarity3(); /// Construct pure scaling - GTSAM_EXPORT Similarity3(double s); + Similarity3(double s); /// Construct from GTSAM types - GTSAM_EXPORT Similarity3(const Rot3& R, const Point3& t, double s); + Similarity3(const Rot3& R, const Point3& t, double s); /// Construct from Eigen types - GTSAM_EXPORT Similarity3(const Matrix3& R, const Vector3& t, double s); + Similarity3(const Matrix3& R, const Vector3& t, double s); /// Construct from matrix [R t; 0 s^-1] - GTSAM_EXPORT Similarity3(const Matrix4& T); + Similarity3(const Matrix4& T); /// @} /// @name Testable /// @{ /// Compare with tolerance - GTSAM_EXPORT bool equals(const Similarity3& sim, double tol) const; + bool equals(const Similarity3& sim, double tol) const; /// Exact equality - GTSAM_EXPORT bool operator==(const Similarity3& other) const; + bool operator==(const Similarity3& other) const; /// Print with optional string - GTSAM_EXPORT void print(const std::string& s) const; + void print(const std::string& s) const; - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p); + friend std::ostream& operator<<(std::ostream& os, const Similarity3& p); /// @} /// @name Group /// @{ /// Return an identity transform - GTSAM_EXPORT static Similarity3 identity(); + static Similarity3 identity(); /// Composition - GTSAM_EXPORT Similarity3 operator*(const Similarity3& S) const; + Similarity3 operator*(const Similarity3& S) const; /// Return the inverse - GTSAM_EXPORT Similarity3 inverse() const; + Similarity3 inverse() const; /// @} /// @name Group action on Point3 /// @{ /// Action on a point p is s*(R*p+t) - GTSAM_EXPORT Point3 transformFrom(const Point3& p, // - OptionalJacobian<3, 7> H1 = boost::none, // - OptionalJacobian<3, 3> H2 = boost::none) const; + Point3 transformFrom(const Point3& p, // + OptionalJacobian<3, 7> H1 = boost::none, // + OptionalJacobian<3, 3> H2 = boost::none) const; - /** + /** * Action on a pose T. - * |Rs ts| |R t| |Rs*R Rs*t+ts| + * |Rs ts| |R t| |Rs*R Rs*t+ts| * |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim3 object. * To retrieve a Pose3, we normalized the scale value into 1. * |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)| * | 0 1/s | = | 0 1 | - * - * This group action satisfies the compatibility condition. + * + * This group action satisfies the compatibility condition. * For more details, refer to: https://en.wikipedia.org/wiki/Group_action */ - GTSAM_EXPORT Pose3 transformFrom(const Pose3& T) const; + Pose3 transformFrom(const Pose3& T) const; /** syntactic sugar for transformFrom */ - GTSAM_EXPORT Point3 operator*(const Point3& p) const; + Point3 operator*(const Point3& p) const; /** * Create Similarity3 by aligning at least three point pairs */ - GTSAM_EXPORT static Similarity3 Align(const std::vector& abPointPairs); - + static Similarity3 Align(const std::vector& abPointPairs); + /** * Create the Similarity3 object that aligns at least two pose pairs. * Each pair is of the form (aTi, bTi). * Given a list of pairs in frame a, and a list of pairs in frame b, Align() * will compute the best-fit Similarity3 aSb transformation to align them. * First, the rotation aRb will be computed as the average (Karcher mean) of - * many estimates aRb (from each pair). Afterwards, the scale factor will be computed - * using the algorithm described here: + * many estimates aRb (from each pair). Afterwards, the scale factor will be + * computed using the algorithm described here: * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf */ - GTSAM_EXPORT static Similarity3 Align(const std::vector& abPosePairs); + static Similarity3 Align(const std::vector& abPosePairs); /// @} /// @name Lie Group @@ -144,20 +141,22 @@ public: /** Log map at the identity * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$ */ - GTSAM_EXPORT static Vector7 Logmap(const Similarity3& s, // - OptionalJacobian<7, 7> Hm = boost::none); + static Vector7 Logmap(const Similarity3& s, // + OptionalJacobian<7, 7> Hm = boost::none); /** Exponential map at the identity */ - GTSAM_EXPORT static Similarity3 Expmap(const Vector7& v, // - OptionalJacobian<7, 7> Hm = boost::none); + static Similarity3 Expmap(const Vector7& v, // + OptionalJacobian<7, 7> Hm = boost::none); /// Chart at the origin struct ChartAtOrigin { - static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) { + static Similarity3 Retract(const Vector7& v, + ChartJacobian H = boost::none) { return Similarity3::Expmap(v, H); } - static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) { + static Vector7 Local(const Similarity3& other, + ChartJacobian H = boost::none) { return Similarity3::Logmap(other, H); } }; @@ -170,67 +169,53 @@ public: * @return 4*4 element of Lie algebra that can be exponentiated * TODO(frank): rename to Hat, make part of traits */ - GTSAM_EXPORT static Matrix4 wedge(const Vector7& xi); + static Matrix4 wedge(const Vector7& xi); /// Project from one tangent space to another - GTSAM_EXPORT Matrix7 AdjointMap() const; + Matrix7 AdjointMap() const; /// @} /// @name Standard interface /// @{ /// Calculate 4*4 matrix group equivalent - GTSAM_EXPORT const Matrix4 matrix() const; + Matrix4 matrix() const; /// Return a GTSAM rotation - const Rot3& rotation() const { - return R_; - } + Rot3 rotation() const { return R_; } /// Return a GTSAM translation - const Point3& translation() const { - return t_; - } + Point3 translation() const { return t_; } /// Return the scale - double scale() const { - return s_; - } - - /// Convert to a rigid body pose (R, s*t) - /// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast. - GTSAM_EXPORT operator Pose3() const; + double scale() const { return s_; } /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes - inline static size_t Dim() { - return 7; - } + inline static size_t Dim() { return 7; } /// Dimensionality of tangent space = 7 DOF - inline size_t dim() const { - return 7; - } + inline size_t dim() const { return 7; } /// @} /// @name Helper functions /// @{ -private: + private: /// Calculate expmap and logmap coefficients. static Matrix3 GetV(Vector3 w, double lambda); /// @} }; -template<> +template <> inline Matrix wedge(const Vector& xi) { return Similarity3::wedge(xi); } -template<> +template <> struct traits : public internal::LieGroup {}; -template<> +template <> struct traits : public internal::LieGroup {}; -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 858270c00..0dc23c160 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -547,6 +547,12 @@ class EssentialMatrix { // Standard Constructors EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); + // Constructors from Pose3 + gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_); + + gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_, + Eigen::Ref H); + // Testable void print(string s = "") const; bool equals(const gtsam::EssentialMatrix& pose, double tol) const; @@ -584,7 +590,13 @@ class Cal3_S2 { // Action on Point2 gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // Standard Interface double fx() const; @@ -623,7 +635,13 @@ virtual class Cal3DS2_Base { // Action on Point2 gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // enabling serialization functionality void serialize() const; @@ -680,7 +698,13 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base { // Note: the signature of this functions differ from the functions // with equal name in the base class. gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // enabling serialization functionality void serialize() const; @@ -706,7 +730,13 @@ class Cal3Fisheye { // Action on Point2 gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // Standard Interface double fx() const; @@ -769,7 +799,13 @@ class Cal3Bundler { // Action on Point2 gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // Standard Interface double fx() const; @@ -807,12 +843,25 @@ class CalibratedCamera { // Action on Point3 gtsam::Point2 project(const gtsam::Point3& point) const; + gtsam::Point2 project(const gtsam::Point3& point, + Eigen::Ref Dcamera, + Eigen::Ref Dpoint); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + gtsam::Point3 backproject(const gtsam::Point2& p, double depth, + Eigen::Ref Dresult_dpose, + Eigen::Ref Dresult_dp, + Eigen::Ref Dresult_ddepth); + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); // Standard Interface gtsam::Pose3 pose() const; double range(const gtsam::Point3& point) const; + double range(const gtsam::Point3& point, Eigen::Ref Dcamera, + Eigen::Ref Dpoint); double range(const gtsam::Pose3& pose) const; + double range(const gtsam::Pose3& point, Eigen::Ref Dcamera, + Eigen::Ref Dpose); double range(const gtsam::CalibratedCamera& camera) const; // enabling serialization functionality @@ -824,6 +873,7 @@ template class PinholeCamera { // Standard Constructors and Named Constructors PinholeCamera(); + PinholeCamera(const gtsam::PinholeCamera other); PinholeCamera(const gtsam::Pose3& pose); PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, @@ -850,14 +900,123 @@ class PinholeCamera { static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); pair projectSafe(const gtsam::Point3& pw) const; gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point2 project(const gtsam::Point3& point, + Eigen::Ref Dpose, + Eigen::Ref Dpoint, + Eigen::Ref Dcal); gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + gtsam::Point3 backproject(const gtsam::Point2& p, double depth, + Eigen::Ref Dresult_dpose, + Eigen::Ref Dresult_dp, + Eigen::Ref Dresult_ddepth, + Eigen::Ref Dresult_dcal); + + gtsam::Point2 reprojectionError(const gtsam::Point3& pw, const gtsam::Point2& measured, + Eigen::Ref Dpose, + Eigen::Ref Dpoint, + Eigen::Ref Dcal); + double range(const gtsam::Point3& point); + double range(const gtsam::Point3& point, Eigen::Ref Dcamera, + Eigen::Ref Dpoint); double range(const gtsam::Pose3& pose); + double range(const gtsam::Pose3& pose, Eigen::Ref Dcamera, + Eigen::Ref Dpose); // enabling serialization functionality void serialize() const; }; +// Forward declaration of PinholeCameraCalX is defined here. +#include +// Some typedefs for common camera types +// PinholeCameraCal3_S2 is the same as SimpleCamera above +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; + +#include +template +class PinholePose { + // Standard Constructors and Named Constructors + PinholePose(); + PinholePose(const gtsam::PinholePose other); + PinholePose(const gtsam::Pose3& pose); + PinholePose(const gtsam::Pose3& pose, const CALIBRATION* K); + static This Level(const gtsam::Pose2& pose, double height); + static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const CALIBRATION* K); + + // Testable + void print(string s = "PinholePose") const; + bool equals(const This& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + CALIBRATION calibration() const; + + // Manifold + This retract(Vector d) const; + Vector localCoordinates(const This& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point2 project(const gtsam::Point3& point, + Eigen::Ref Dpose, + Eigen::Ref Dpoint, + Eigen::Ref Dcal); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + gtsam::Point3 backproject(const gtsam::Point2& p, double depth, + Eigen::Ref Dresult_dpose, + Eigen::Ref Dresult_dp, + Eigen::Ref Dresult_ddepth, + Eigen::Ref Dresult_dcal); + double range(const gtsam::Point3& point); + double range(const gtsam::Point3& point, Eigen::Ref Dcamera, + Eigen::Ref Dpoint); + double range(const gtsam::Pose3& pose); + double range(const gtsam::Pose3& pose, Eigen::Ref Dcamera, + Eigen::Ref Dpose); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::PinholePose PinholePoseCal3_S2; +typedef gtsam::PinholePose PinholePoseCal3DS2; +typedef gtsam::PinholePose PinholePoseCal3Unified; +typedef gtsam::PinholePose PinholePoseCal3Bundler; +typedef gtsam::PinholePose PinholePoseCal3Fisheye; + +#include +class Similarity2 { + // Standard Constructors + Similarity2(); + Similarity2(double s); + Similarity2(const gtsam::Rot2& R, const gtsam::Point2& t, double s); + Similarity2(const Matrix& R, const Vector& t, double s); + Similarity2(const Matrix& T); + + gtsam::Point2 transformFrom(const gtsam::Point2& p) const; + gtsam::Pose2 transformFrom(const gtsam::Pose2& T); + + static gtsam::Similarity2 Align(const gtsam::Point2Pairs& abPointPairs); + static gtsam::Similarity2 Align(const gtsam::Pose2Pairs& abPosePairs); + + // Standard Interface + bool equals(const gtsam::Similarity2& sim, double tol) const; + Matrix matrix() const; + gtsam::Rot2& rotation(); + gtsam::Point2& translation(); + double scale() const; +}; + #include class Similarity3 { // Standard Constructors @@ -874,22 +1033,13 @@ class Similarity3 { static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs); // Standard Interface - const Matrix matrix() const; - const gtsam::Rot3& rotation(); - const gtsam::Point3& translation(); + bool equals(const gtsam::Similarity3& sim, double tol) const; + Matrix matrix() const; + gtsam::Rot3& rotation(); + gtsam::Point3& translation(); double scale() const; }; -// Forward declaration of PinholeCameraCalX is defined here. -#include -// Some typedefs for common camera types -// PinholeCameraCal3_S2 is the same as SimpleCamera above -typedef gtsam::PinholeCamera PinholeCameraCal3_S2; -typedef gtsam::PinholeCamera PinholeCameraCal3DS2; -typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; -typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; - template class CameraSet { CameraSet(); @@ -921,9 +1071,18 @@ class StereoCamera { static size_t Dim(); // Transformations and measurement functions - gtsam::StereoPoint2 project(const gtsam::Point3& point); + gtsam::StereoPoint2 project(const gtsam::Point3& point) const; gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; + // project with Jacobian + gtsam::StereoPoint2 project2(const gtsam::Point3& point, + Eigen::Ref H1, + Eigen::Ref H2) const; + + gtsam::Point3 backproject2(const gtsam::StereoPoint2& p, + Eigen::Ref H1, + Eigen::Ref H2) const; + // enabling serialization functionality void serialize() const; }; diff --git a/gtsam/geometry/tests/testSimilarity2.cpp b/gtsam/geometry/tests/testSimilarity2.cpp new file mode 100644 index 000000000..dd4fd0efd --- /dev/null +++ b/gtsam/geometry/tests/testSimilarity2.cpp @@ -0,0 +1,66 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSimilarity2.cpp + * @brief Unit tests for Similarity2 class + * @author Varun Agrawal + */ + +#include +#include +#include +#include +#include + +#include + +using namespace std::placeholders; +using namespace gtsam; +using namespace std; + +GTSAM_CONCEPT_TESTABLE_INST(Similarity2) + +static const Point2 P(0.2, 0.7); +static const Rot2 R = Rot2::fromAngle(0.3); +static const double s = 4; + +const double degree = M_PI / 180; + +//****************************************************************************** +TEST(Similarity2, Concepts) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(Similarity2, Constructors) { + Similarity2 sim2_Construct1; + Similarity2 sim2_Construct2(s); + Similarity2 sim2_Construct3(R, P, s); + Similarity2 sim2_Construct4(R.matrix(), P, s); +} + +//****************************************************************************** +TEST(Similarity2, Getters) { + Similarity2 sim2_default; + EXPECT(assert_equal(Rot2(), sim2_default.rotation())); + EXPECT(assert_equal(Point2(0, 0), sim2_default.translation())); + EXPECT_DOUBLES_EQUAL(1.0, sim2_default.scale(), 1e-9); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index 428422072..7a134f6ef 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -458,18 +458,18 @@ TEST(Similarity3, Optimization2) { Values result; result = LevenbergMarquardtOptimizer(graph, initial).optimize(); //result.print("Optimized Estimate\n"); - Pose3 p1, p2, p3, p4, p5; - p1 = Pose3(result.at(X(1))); - p2 = Pose3(result.at(X(2))); - p3 = Pose3(result.at(X(3))); - p4 = Pose3(result.at(X(4))); - p5 = Pose3(result.at(X(5))); + Similarity3 p1, p2, p3, p4, p5; + p1 = result.at(X(1)); + p2 = result.at(X(2)); + p3 = result.at(X(3)); + p4 = result.at(X(4)); + p5 = result.at(X(5)); - //p1.print("Pose1"); - //p2.print("Pose2"); - //p3.print("Pose3"); - //p4.print("Pose4"); - //p5.print("Pose5"); + //p1.print("Similarity1"); + //p2.print("Similarity2"); + //p3.print("Similarity3"); + //p4.print("Similarity4"); + //p5.print("Similarity5"); Similarity3 expected(0.7); EXPECT(assert_equal(expected, result.at(X(5)), 0.4)); diff --git a/gtsam/inference/BayesNet-inst.h b/gtsam/inference/BayesNet-inst.h index afde5498d..e792b5c03 100644 --- a/gtsam/inference/BayesNet-inst.h +++ b/gtsam/inference/BayesNet-inst.h @@ -53,8 +53,9 @@ void BayesNet::dot(std::ostream& os, auto frontals = conditional->frontals(); const Key me = frontals.front(); auto parents = conditional->parents(); - for (const Key& p : parents) - os << " var" << keyFormatter(p) << "->var" << keyFormatter(me) << "\n"; + for (const Key& p : parents) { + os << " var" << p << "->var" << me << "\n"; + } } os << "}"; diff --git a/gtsam/inference/DotWriter.cpp b/gtsam/inference/DotWriter.cpp index ad5330575..eac0c90f9 100644 --- a/gtsam/inference/DotWriter.cpp +++ b/gtsam/inference/DotWriter.cpp @@ -43,7 +43,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter, const boost::optional& position, ostream* os) const { // Label the node with the label from the KeyFormatter - *os << " var" << keyFormatter(key) << "[label=\"" << keyFormatter(key) + *os << " var" << key << "[label=\"" << keyFormatter(key) << "\""; if (position) { *os << ", pos=\"" << position->x() << "," << position->y() << "!\""; @@ -65,13 +65,13 @@ void DotWriter::DrawFactor(size_t i, const boost::optional& position, static void ConnectVariables(Key key1, Key key2, const KeyFormatter& keyFormatter, ostream* os) { - *os << " var" << keyFormatter(key1) << "--" - << "var" << keyFormatter(key2) << ";\n"; + *os << " var" << key1 << "--" + << "var" << key2 << ";\n"; } static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter, size_t i, ostream* os) { - *os << " var" << keyFormatter(key) << "--" + *os << " var" << key << "--" << "factor" << i << ";\n"; } diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index c3d7d64db..d9cfc1f3c 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -54,23 +54,31 @@ namespace noiseModel { // clang-format on namespace mEstimator { -//--------------------------------------------------------------------------------------- - +/** + * Pure virtual class for all robust error function classes. + * + * It provides the machinery for block vs scalar reweighting strategies, in + * addition to defining the interface of derived classes. + */ class GTSAM_EXPORT Base { public: + /** the rows can be weighted independently according to the error + * or uniformly with the norm of the right hand side */ enum ReweightScheme { Scalar, Block }; typedef boost::shared_ptr shared_ptr; protected: - /** the rows can be weighted independently according to the error - * or uniformly with the norm of the right hand side */ + /// Strategy for reweighting \sa ReweightScheme ReweightScheme reweight_; public: Base(const ReweightScheme reweight = Block) : reweight_(reweight) {} virtual ~Base() {} - /* + /// Returns the reweight scheme, as explained in ReweightScheme + ReweightScheme reweightScheme() const { return reweight_; } + + /** * This method is responsible for returning the total penalty for a given * amount of error. For example, this method is responsible for implementing * the quadratic function for an L2 penalty, the absolute value function for @@ -80,16 +88,20 @@ class GTSAM_EXPORT Base { * error vector, then it prevents implementations of asymmeric loss * functions. It would be better for this function to accept the vector and * internally call the norm if necessary. + * + * This returns \rho(x) in \ref mEstimator */ - virtual double loss(double distance) const { return 0; }; + virtual double loss(double distance) const { return 0; } - /* + /** * This method is responsible for returning the weight function for a given * amount of error. The weight function is related to the analytic derivative * of the loss function. See * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf * for details. This method is required when optimizing cost functions with * robust penalties using iteratively re-weighted least squares. + * + * This returns w(x) in \ref mEstimator */ virtual double weight(double distance) const = 0; @@ -124,7 +136,15 @@ class GTSAM_EXPORT Base { } }; -/// Null class should behave as Gaussian +/** "Null" robust loss function, equivalent to a Gaussian pdf noise model, or + * plain least-squares (non-robust). + * + * This model has no additional parameters. + * + * - Loss \rho(x) = 0.5 x² + * - Derivative \phi(x) = x + * - Weight w(x) = \phi(x)/x = 1 + */ class GTSAM_EXPORT Null : public Base { public: typedef boost::shared_ptr shared_ptr; @@ -146,7 +166,14 @@ class GTSAM_EXPORT Null : public Base { } }; -/// Fair implements the "Fair" robust error model (Zhang97ivc) +/** Implementation of the "Fair" robust error model (Zhang97ivc) + * + * This model has a scalar parameter "c". + * + * - Loss \rho(x) = c² (|x|/c - log(1+|x|/c)) + * - Derivative \phi(x) = x/(1+|x|/c) + * - Weight w(x) = \phi(x)/x = 1/(1+|x|/c) + */ class GTSAM_EXPORT Fair : public Base { protected: double c_; @@ -160,6 +187,7 @@ class GTSAM_EXPORT Fair : public Base { void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double c, const ReweightScheme reweight = Block); + double modelParameter() const { return c_; } private: /** Serialization function */ @@ -171,7 +199,14 @@ class GTSAM_EXPORT Fair : public Base { } }; -/// Huber implements the "Huber" robust error model (Zhang97ivc) +/** The "Huber" robust error model (Zhang97ivc). + * + * This model has a scalar parameter "k". + * + * - Loss \rho(x) = 0.5 x² if |x| shared_ptr; @@ -293,6 +359,7 @@ class GTSAM_EXPORT GemanMcClure : public Base { void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); + double modelParameter() const { return c_; } protected: double c_; @@ -307,11 +374,18 @@ class GTSAM_EXPORT GemanMcClure : public Base { } }; -/// DCS implements the Dynamic Covariance Scaling robust error model -/// from the paper Robust Map Optimization (Agarwal13icra). -/// -/// Under the special condition of the parameter c == 1.0 and not -/// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure. +/** DCS implements the Dynamic Covariance Scaling robust error model + * from the paper Robust Map Optimization (Agarwal13icra). + * + * Under the special condition of the parameter c == 1.0 and not + * forcing the output weight s <= 1.0, DCS is similar to Geman-McClure. + * + * This model has a scalar parameter "c" (with "units" of squared error). + * + * - Loss \rho(x) = (c²x² + cx⁴)/(x²+c)² (for any "x") + * - Derivative \phi(x) = 2c²x/(x²+c)² + * - Weight w(x) = \phi(x)/x = 2c²/(x²+c)² if x²>c, 1 otherwise + */ class GTSAM_EXPORT DCS : public Base { public: typedef boost::shared_ptr shared_ptr; @@ -323,6 +397,7 @@ class GTSAM_EXPORT DCS : public Base { void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); + double modelParameter() const { return c_; } protected: double c_; @@ -337,12 +412,19 @@ class GTSAM_EXPORT DCS : public Base { } }; -/// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of -/// width 2*k, centered at the origin. The resulting penalty within the dead -/// zone is always zero, and grows quadratically outside the dead zone. In this -/// sense, the L2WithDeadZone penalty is "robust to inliers", rather than being -/// robust to outliers. This penalty can be used to create barrier functions in -/// a general way. +/** L2WithDeadZone implements a standard L2 penalty, but with a dead zone of + * width 2*k, centered at the origin. The resulting penalty within the dead + * zone is always zero, and grows quadratically outside the dead zone. In this + * sense, the L2WithDeadZone penalty is "robust to inliers", rather than being + * robust to outliers. This penalty can be used to create barrier functions in + * a general way. + * + * This model has a scalar parameter "k". + * + * - Loss \rho(x) = 0 if |x|k, (k+x) if x<-k + * - Weight w(x) = \phi(x)/x = 0 if |x|k, (k+x)/x if x<-k + */ class GTSAM_EXPORT L2WithDeadZone : public Base { protected: double k_; @@ -356,6 +438,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); + double modelParameter() const { return k_; } private: /** Serialization function */ diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index f1bc92f69..943b661d8 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -671,6 +671,10 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { DummyPreconditionerParameters(); }; +virtual class BlockJacobiPreconditionerParameters : gtsam::PreconditionerParameters { + BlockJacobiPreconditionerParameters(); +}; + #include virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { PCGSolverParameters(); diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 10c33d101..c7d82975a 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -90,7 +90,11 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation /** * Add a single Gyroscope measurement to the preintegration. - * @param measureOmedga Measured angular velocity (in body frame) + * Measurements are taken to be in the sensor + * frame and conversion to the body frame is handled by `body_P_sensor` in + * `PreintegratedRotationParams` (if provided). + * + * @param measuredOmega Measured angular velocity (as given by the sensor) * @param deltaT Time step */ void integrateMeasurement(const Vector3& measuredOmega, double deltaT); diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 068a17ca4..213f5f223 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -208,8 +208,11 @@ public: /** * Add a single IMU measurement to the preintegration. - * @param measuredAcc Measured acceleration (in body frame, as given by the - * sensor) + * Both accelerometer and gyroscope measurements are taken to be in the sensor + * frame and conversion to the body frame is handled by `body_P_sensor` in + * `PreintegrationParams`. + * + * @param measuredAcc Measured acceleration (as given by the sensor) * @param measuredOmega Measured angular velocity (as given by the sensor) * @param dt Time interval between two consecutive IMU measurements */ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 35207e452..740827162 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -121,7 +121,11 @@ public: /** * Add a single IMU measurement to the preintegration. - * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) + * Both accelerometer and gyroscope measurements are taken to be in the sensor + * frame and conversion to the body frame is handled by `body_P_sensor` in + * `PreintegrationParams`. + * + * @param measuredAcc Measured acceleration (as given by the sensor) * @param measuredOmega Measured angular velocity (as given by the sensor) * @param dt Time interval between this and the last IMU measurement */ diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index c94e1d3d5..5607add16 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -27,7 +27,6 @@ #include #include -using namespace std::placeholders; using namespace std; using namespace gtsam; using namespace GeographicLib; @@ -71,7 +70,7 @@ TEST( GPSFactor, Constructor ) { EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T); // Use the factor to calculate the derivative @@ -100,7 +99,7 @@ TEST( GPSFactor2, Constructor ) { EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T); // Use the factor to calculate the derivative diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index e2a623710..971803dbf 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -26,7 +26,6 @@ #include -using namespace std::placeholders; using namespace std; using namespace gtsam; using namespace GeographicLib; @@ -64,7 +63,7 @@ TEST( MagFactor, unrotate ) { Matrix H; Point3 expected(22735.5, 314.502, 44202.5); EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6)); } @@ -75,27 +74,27 @@ TEST( MagFactor, Factors ) { // MagFactor MagFactor f(1, measured, s, dir, bias, model); - EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); - EXPECT( assert_equal((Matrix)numericalDerivative11 // + EXPECT(assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); + EXPECT(assert_equal((Matrix)numericalDerivative11 // (std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7)); -// MagFactor1 + // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); - EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT(assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); + EXPECT(assert_equal(numericalDerivative11 // (std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7)); -// MagFactor2 + // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); - EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT(assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); + EXPECT(assert_equal(numericalDerivative11 // (std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),// H1, 1e-7)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),// H2, 1e-7)); -// MagFactor2 + // MagFactor3 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 028545d01..3c5aa9cab 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -121,7 +121,7 @@ public: /** Optimize the bayes tree */ VectorValues optimize() const; - + protected: /** Compute the Bayes Tree as a helper function to the constructor */ diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index c745f7bd9..a490162ac 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -94,7 +94,6 @@ namespace gtsam { Vector evaluateError(const T& x, boost::optional H = boost::none) const override { if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); // manifold equivalent of z-x -> Local(x,z) - // TODO(ASL) Add Jacobians. return -traits::Local(x, prior_); } diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index dfcb7e174..0370c5cee 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -279,10 +279,11 @@ namespace gtsam { template struct handle { ValueType operator()(Key j, const Value* const pointer) { - try { + auto ptr = dynamic_cast*>(pointer); + if (ptr) { // value returns a const ValueType&, and the return makes a copy !!!!! - return dynamic_cast&>(*pointer).value(); - } catch (std::bad_cast&) { + return ptr->value(); + } else { throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType)); } } @@ -294,11 +295,12 @@ namespace gtsam { // Handle dynamic matrices template struct handle_matrix, true> { - Eigen::Matrix operator()(Key j, const Value* const pointer) { - try { + inline Eigen::Matrix operator()(Key j, const Value* const pointer) { + auto ptr = dynamic_cast>*>(pointer); + if (ptr) { // value returns a const Matrix&, and the return makes a copy !!!!! - return dynamic_cast>&>(*pointer).value(); - } catch (std::bad_cast&) { + return ptr->value(); + } else { // If a fixed matrix was stored, we end up here as well. throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix)); } @@ -308,16 +310,18 @@ namespace gtsam { // Handle fixed matrices template struct handle_matrix, false> { - Eigen::Matrix operator()(Key j, const Value* const pointer) { - try { + inline Eigen::Matrix operator()(Key j, const Value* const pointer) { + auto ptr = dynamic_cast>*>(pointer); + if (ptr) { // value returns a const MatrixMN&, and the return makes a copy !!!!! - return dynamic_cast>&>(*pointer).value(); - } catch (std::bad_cast&) { + return ptr->value(); + } else { Matrix A; - try { - // Check if a dynamic matrix was stored - A = handle_matrix()(j, pointer); // will throw if not.... - } catch (const ValuesIncorrectType&) { + // Check if a dynamic matrix was stored + auto ptr = dynamic_cast*>(pointer); + if (ptr) { + A = ptr->value(); + } else { // Or a dynamic vector A = handle_matrix()(j, pointer); // will throw if not.... } @@ -364,10 +368,10 @@ namespace gtsam { if(item != values_.end()) { // dynamic cast the type and throw exception if incorrect - const Value& value = *item->second; - try { - return dynamic_cast&>(value).value(); - } catch (std::bad_cast &) { + auto ptr = dynamic_cast*>(item->second); + if (ptr) { + return ptr->value(); + } else { // NOTE(abe): clang warns about potential side effects if done in typeid const Value* value = item->second; throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 326b84d16..30181e08d 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -226,6 +226,10 @@ class Values { void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); void insert(size_t j, double c); @@ -245,6 +249,10 @@ class Values { void insert(size_t j, const gtsam::ParameterMatrix<14>& X); void insert(size_t j, const gtsam::ParameterMatrix<15>& X); + template + void insert(size_t j, const T& val); + void update(size_t j, const gtsam::Point2& point2); void update(size_t j, const gtsam::Point3& point3); void update(size_t j, const gtsam::Rot2& rot2); @@ -265,6 +273,10 @@ class Values { void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); @@ -306,6 +318,10 @@ class Values { void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert_or_assign(size_t j, const gtsam::NavState& nav_state); void insert_or_assign(size_t j, Vector vector); @@ -347,6 +363,10 @@ class Values { gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::PinholePose, + gtsam::PinholePose, + gtsam::PinholePose, + gtsam::PinholePose, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h index afce12205..430e107ad 100644 --- a/gtsam/sfm/SfmData.h +++ b/gtsam/sfm/SfmData.h @@ -77,10 +77,14 @@ struct GTSAM_EXPORT SfmData { size_t numberCameras() const { return cameras.size(); } /// The track formed by series of landmark measurements - SfmTrack track(size_t idx) const { return tracks[idx]; } + const 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]; } + const SfmCamera& camera(size_t idx) const { return cameras[idx]; } + + /// Getters + const std::vector& cameraList() const { return cameras; } + const std::vector& trackList() const { return tracks; } /** * @brief Create projection factors using keys i and P(j) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index bf9a73ac5..19e21b10c 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -9,6 +9,8 @@ class SfmTrack { SfmTrack(); SfmTrack(const gtsam::Point3& pt); const Point3& point3() const; + + Point3 p; double r; double g; @@ -34,12 +36,15 @@ class SfmData { static gtsam::SfmData FromBundlerFile(string filename); static gtsam::SfmData FromBalFile(string filename); + std::vector& trackList() const; + std::vector>& cameraList() const; + void addTrack(const gtsam::SfmTrack& t); void addCamera(const gtsam::SfmCamera& cam); size_t numberTracks() const; size_t numberCameras() const; - gtsam::SfmTrack track(size_t idx) const; - gtsam::PinholeCamera camera(size_t idx) const; + gtsam::SfmTrack& track(size_t idx) const; + gtsam::PinholeCamera& camera(size_t idx) const; gtsam::NonlinearFactorGraph generalSfmFactors( const gtsam::SharedNoiseModel& model = @@ -91,6 +96,13 @@ class BinaryMeasurementsUnit3 { void push_back(const gtsam::BinaryMeasurement& measurement); }; +class BinaryMeasurementsRot3 { + BinaryMeasurementsRot3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + #include // TODO(frank): copy/pasta below until we have integer template paremeters in @@ -184,6 +196,10 @@ class ShonanAveraging2 { }; class ShonanAveraging3 { + ShonanAveraging3( + const std::vector>& measurements, + const gtsam::ShonanAveragingParameters3& parameters = + gtsam::ShonanAveragingParameters3()); ShonanAveraging3(string g2oFile); ShonanAveraging3(string g2oFile, const gtsam::ShonanAveragingParameters3& parameters); diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h index c81a9adc5..00f741705 100644 --- a/gtsam/slam/KarcherMeanFactor-inl.h +++ b/gtsam/slam/KarcherMeanFactor-inl.h @@ -40,8 +40,7 @@ T FindKarcherMeanImpl(const vector& rotations) { return result.at(kKey); } -template ::value >::type > +template T FindKarcherMean(const std::vector& rotations) { return FindKarcherMeanImpl(rotations); } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index dc450a9f7..7c4d7fea7 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename, size_t maxIndex = 0); using BinaryMeasurementsUnit3 = std::vector>; +using BinaryMeasurementsRot3 = std::vector>; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 inline boost::optional GTSAM_DEPRECATED diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 4e943253e..8e1e06d5b 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -90,6 +90,22 @@ typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Unified; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3_S2; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3DS2; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3Bundler; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3Fisheye; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3Unified; + template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp index 2dc083cb2..d1a60e346 100644 --- a/gtsam/slam/tests/testPriorFactor.cpp +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -5,12 +5,16 @@ * @date Nov 4, 2014 */ -#include -#include #include +#include +#include +#include +#include using namespace std; +using namespace std::placeholders; using namespace gtsam; +using namespace imuBias; /* ************************************************************************* */ @@ -23,16 +27,44 @@ TEST(PriorFactor, ConstructorScalar) { // Constructor vector3 TEST(PriorFactor, ConstructorVector3) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); - PriorFactor factor(1, Vector3(1,2,3), model); + PriorFactor factor(1, Vector3(1, 2, 3), model); } // Constructor dynamic sized vector TEST(PriorFactor, ConstructorDynamicSizeVector) { - Vector v(5); v << 1, 2, 3, 4, 5; + Vector v(5); + v << 1, 2, 3, 4, 5; SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); PriorFactor factor(1, v, model); } +Vector callEvaluateError(const PriorFactor& factor, + const ConstantBias& bias) { + return factor.evaluateError(bias); +} + +// Test for imuBias::ConstantBias +TEST(PriorFactor, ConstantBias) { + Vector3 biasAcc(1, 2, 3); + Vector3 biasGyro(0.1, 0.2, 0.3); + ConstantBias bias(biasAcc, biasGyro); + + PriorFactor factor(1, bias, + noiseModel::Isotropic::Sigma(6, 0.1)); + Values values; + values.insert(1, bias); + + EXPECT_DOUBLES_EQUAL(0.0, factor.error(values), 1e-8); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); + + ConstantBias incorrectBias( + (Vector6() << 1.1, 2.1, 3.1, 0.2, 0.3, 0.4).finished()); + values.clear(); + values.insert(1, incorrectBias); + EXPECT_DOUBLES_EQUAL(3.0, factor.error(values), 1e-8); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index 2e13be10e..7795d5b89 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -104,16 +104,16 @@ TEST(SymbolicBayesNet, Dot) { "digraph {\n" " size=\"5,5\";\n" "\n" - " vara1[label=\"a1\", pos=\"1,2!\", shape=box];\n" - " vara2[label=\"a2\", pos=\"2,2!\", shape=box];\n" - " varx1[label=\"x1\", pos=\"1,1!\"];\n" - " varx2[label=\"x2\", pos=\"2,1!\"];\n" - " varx3[label=\"x3\", pos=\"3,1!\"];\n" + " var6989586621679009793[label=\"a1\", pos=\"1,2!\", shape=box];\n" + " var6989586621679009794[label=\"a2\", pos=\"2,2!\", shape=box];\n" + " var8646911284551352321[label=\"x1\", pos=\"1,1!\"];\n" + " var8646911284551352322[label=\"x2\", pos=\"2,1!\"];\n" + " var8646911284551352323[label=\"x3\", pos=\"3,1!\"];\n" "\n" - " varx1->varx2\n" - " vara1->varx2\n" - " varx2->varx3\n" - " vara2->varx3\n" + " var8646911284551352321->var8646911284551352322\n" + " var6989586621679009793->var8646911284551352322\n" + " var8646911284551352322->var8646911284551352323\n" + " var6989586621679009794->var8646911284551352323\n" "}"); } diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index f5869b145..0c8dc314b 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -48,6 +48,7 @@ set(ignore gtsam::Rot3Vector gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 + gtsam::BinaryMeasurementsRot3 gtsam::DiscreteKey gtsam::KeyPairDoubleMap) @@ -98,11 +99,23 @@ set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam) copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" "${GTSAM_MODULE_PATH}") -# Hack to get python test files copied every time they are modified +# Hack to get python test and util files copied every time they are modified file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py") foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY) endforeach() +file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py") +foreach(util_file ${GTSAM_PYTHON_UTIL_FILES}) + configure_file(${util_file} "${GTSAM_MODULE_PATH}/utils/${test_file}" COPYONLY) +endforeach() +file(GLOB GTSAM_PYTHON_PREAMBLE_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/preamble/*.h") +foreach(util_file ${GTSAM_PYTHON_PREAMBLE_FILES}) + configure_file(${util_file} "${GTSAM_MODULE_PATH}/preamble/${test_file}" COPYONLY) +endforeach() +file(GLOB GTSAM_PYTHON_SPECIALIZATION_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/specializations/*.h") +foreach(util_file ${GTSAM_PYTHON_SPECIALIZATION_FILES}) + configure_file(${util_file} "${GTSAM_MODULE_PATH}/specializations/${test_file}" COPYONLY) +endforeach() # Common directory for data/datasets stored with the package. # This will store the data in the Python site package directly. @@ -125,6 +138,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::KeyVector gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::BinaryMeasurementsUnit3 + gtsam::BinaryMeasurementsRot3 gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler gtsam::CameraSetCal3Unified @@ -160,7 +174,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) # Hack to get python test files copied every time they are modified file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py") - foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) + foreach(test_file ${GTSAM_UNSTABLE_PYTHON_TEST_FILES}) configure_file(${test_file} "${GTSAM_UNSTABLE_MODULE_PATH}/tests/${test_file}" COPYONLY) endforeach() diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index 35fe2a577..bd0441d06 100644 --- a/python/gtsam/preamble/geometry.h +++ b/python/gtsam/preamble/geometry.h @@ -23,8 +23,8 @@ PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); +PYBIND11_MAKE_OPAQUE(gtsam::Pose2Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE( - gtsam::CameraSet>); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index a34e73058..8ff0ea82e 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -9,4 +9,18 @@ * automatic STL binding, such that the raw objects can be accessed in Python. * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. - */ \ No newline at end of file + */ + +// Including can cause some serious hard-to-debug bugs!!! +// #include +#include + +PYBIND11_MAKE_OPAQUE( + std::vector); + +PYBIND11_MAKE_OPAQUE( + std::vector); +PYBIND11_MAKE_OPAQUE( + std::vector>); +PYBIND11_MAKE_OPAQUE( + std::vector>); diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index a492ce8eb..5a0ea35ef 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -16,6 +16,7 @@ py::bind_vector< m_, "Point2Vector"); py::bind_vector>(m_, "Point2Pairs"); py::bind_vector>(m_, "Point3Pairs"); +py::bind_vector>(m_, "Pose2Pairs"); py::bind_vector>(m_, "Pose3Pairs"); py::bind_vector>(m_, "Pose3Vector"); py::bind_vector>>( diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 6de15217f..90a2b8417 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -13,4 +13,19 @@ py::bind_vector > >( m_, "BinaryMeasurementsUnit3"); +py::bind_vector > >( + m_, "BinaryMeasurementsRot3"); py::bind_map(m_, "KeyPairDoubleMap"); + +py::bind_vector< + std::vector >( + m_, "SfmTracks"); + +py::bind_vector< + std::vector >( + m_, "SfmCameras"); + +py::bind_vector< + std::vector>>( + m_, "SfmMeasurementVector" + ); diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index bafbacfa4..630109d66 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -139,6 +139,17 @@ class TestCal3Unified(GtsamTestCase): self.gtsamAssertEquals(z, np.zeros(2)) self.gtsamAssertEquals(H @ H.T, 4*np.eye(2)) + Dcal = np.zeros((2, 10), order='F') + Dp = np.zeros((2, 2), order='F') + camera.calibrate(img_point, Dcal, Dp) + + self.gtsamAssertEquals(Dcal, np.array( + [[ 0., 0., 0., -1., 0., 0., 0., 0., 0., 0.], + [ 0., 0., 0., 0., -1., 0., 0., 0., 0., 0.]])) + self.gtsamAssertEquals(Dp, np.array( + [[ 1., -0.], + [-0., 1.]])) + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation(self): """Estimate spatial point from image measurements""" diff --git a/python/gtsam/tests/test_DiscreteBayesNet.py b/python/gtsam/tests/test_DiscreteBayesNet.py index 74191dcc7..10c5db612 100644 --- a/python/gtsam/tests/test_DiscreteBayesNet.py +++ b/python/gtsam/tests/test_DiscreteBayesNet.py @@ -11,12 +11,12 @@ Author: Frank Dellaert # pylint: disable=no-name-in-module, invalid-name -import unittest import textwrap +import unittest import gtsam -from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph, - DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering) +from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteDistribution, + DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering) from gtsam.utils.test_case import GtsamTestCase # Some keys: @@ -152,10 +152,10 @@ class TestDiscreteBayesNet(GtsamTestCase): var4[label="4"]; var5[label="5"]; var6[label="6"]; - vara0[label="a0", pos="0,2!"]; + var6989586621679009792[label="a0", pos="0,2!"]; var4->var6 - vara0->var3 + var6989586621679009792->var3 var3->var5 var6->var5 }""" diff --git a/python/gtsam/tests/test_PinholeCamera.py b/python/gtsam/tests/test_PinholeCamera.py new file mode 100644 index 000000000..392d48d3f --- /dev/null +++ b/python/gtsam/tests/test_PinholeCamera.py @@ -0,0 +1,46 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +PinholeCamera unit tests. +Author: Fan Jiang +""" +import unittest +from math import pi + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestPinholeCamera(GtsamTestCase): + """ + Tests if we can correctly get the camera Jacobians in Python + """ + def test_jacobian(self): + cam1 = gtsam.PinholeCameraCal3Bundler() + + # order is important because Eigen is column major! + Dpose = np.zeros((2, 6), order='F') + Dpoint = np.zeros((2, 3), order='F') + Dcal = np.zeros((2, 3), order='F') + cam1.project(np.array([1, 1, 1]), Dpose, Dpoint, Dcal) + + self.gtsamAssertEquals(Dpoint, np.array([[1, 0, -1], [0, 1, -1]])) + + self.gtsamAssertEquals( + Dpose, + np.array([ + [1., -2., 1., -1., 0., 1.], # + [2., -1., -1., 0., -1., 1.] + ])) + + self.gtsamAssertEquals(Dcal, np.array([[1., 2., 4.], [1., 2., 4.]])) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam/tests/test_Sim2.py b/python/gtsam/tests/test_Sim2.py new file mode 100644 index 000000000..ea809b965 --- /dev/null +++ b/python/gtsam/tests/test_Sim2.py @@ -0,0 +1,194 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Sim3 unit tests. +Author: John Lambert +""" +# pylint: disable=no-name-in-module +import unittest + +import numpy as np +from gtsam import Pose2, Pose2Pairs, Rot2, Similarity2 +from gtsam.utils.test_case import GtsamTestCase + + +class TestSim2(GtsamTestCase): + """Test selected Sim2 methods.""" + + def test_align_poses_along_straight_line(self) -> None: + """Test Align Pose2Pairs method. + + Scenario: + 3 object poses + same scale (no gauge ambiguity) + world frame has poses rotated about 180 degrees. + world and egovehicle frame translated by 15 meters w.r.t. each other + """ + R180 = Rot2.fromDegrees(180) + + # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) + # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame + eTo0 = Pose2(Rot2(), np.array([5, 0])) + eTo1 = Pose2(Rot2(), np.array([10, 0])) + eTo2 = Pose2(Rot2(), np.array([15, 0])) + + eToi_list = [eTo0, eTo1, eTo2] + + # Create destination poses + # (same three objects, but instead living in the world "w" frame) + wTo0 = Pose2(R180, np.array([-10, 0])) + wTo1 = Pose2(R180, np.array([-5, 0])) + wTo2 = Pose2(R180, np.array([0, 0])) + + wToi_list = [wTo0, wTo1, wTo2] + + we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + wSe = Similarity2.Align(we_pairs) + + for wToi, eToi in zip(wToi_list, eToi_list): + self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) + + def test_align_poses_along_straight_line_gauge(self): + """Test if Align Pose3Pairs method can account for gauge ambiguity. + + Scenario: + 3 object poses + with gauge ambiguity (2x scale) + world frame has poses rotated by 90 degrees. + world and egovehicle frame translated by 11 meters w.r.t. each other + """ + R90 = Rot2.fromDegrees(90) + + # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) + # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame + eTo0 = Pose2(Rot2(), np.array([1, 0])) + eTo1 = Pose2(Rot2(), np.array([2, 0])) + eTo2 = Pose2(Rot2(), np.array([4, 0])) + + eToi_list = [eTo0, eTo1, eTo2] + + # Create destination poses + # (same three objects, but instead living in the world/city "w" frame) + wTo0 = Pose2(R90, np.array([0, 12])) + wTo1 = Pose2(R90, np.array([0, 14])) + wTo2 = Pose2(R90, np.array([0, 18])) + + wToi_list = [wTo0, wTo1, wTo2] + + we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + wSe = Similarity2.Align(we_pairs) + + for wToi, eToi in zip(wToi_list, eToi_list): + self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) + + def test_align_poses_scaled_squares(self): + """Test if Align Pose2Pairs method can account for gauge ambiguity. + + Make sure a big and small square can be aligned. + The u's represent a big square (10x10), and v's represents a small square (4x4). + + Scenario: + 4 object poses + with gauge ambiguity (2.5x scale) + """ + # 0, 90, 180, and 270 degrees yaw + R0 = Rot2.fromDegrees(0) + R90 = Rot2.fromDegrees(90) + R180 = Rot2.fromDegrees(180) + R270 = Rot2.fromDegrees(270) + + aTi0 = Pose2(R0, np.array([2, 3])) + aTi1 = Pose2(R90, np.array([12, 3])) + aTi2 = Pose2(R180, np.array([12, 13])) + aTi3 = Pose2(R270, np.array([2, 13])) + + aTi_list = [aTi0, aTi1, aTi2, aTi3] + + bTi0 = Pose2(R0, np.array([4, 3])) + bTi1 = Pose2(R90, np.array([8, 3])) + bTi2 = Pose2(R180, np.array([8, 7])) + bTi3 = Pose2(R270, np.array([4, 7])) + + bTi_list = [bTi0, bTi1, bTi2, bTi3] + + ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + aSb = Similarity2.Align(ab_pairs) + + for aTi, bTi in zip(aTi_list, bTi_list): + self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi)) + + def test_constructor(self) -> None: + """Sim(2) to perform p_b = bSa * p_a""" + bRa = Rot2() + bta = np.array([1, 2]) + bsa = 3.0 + bSa = Similarity2(R=bRa, t=bta, s=bsa) + self.assertIsInstance(bSa, Similarity2) + np.testing.assert_allclose(bSa.rotation().matrix(), bRa.matrix()) + np.testing.assert_allclose(bSa.translation(), bta) + np.testing.assert_allclose(bSa.scale(), bsa) + + def test_is_eq(self) -> None: + """Ensure object equality works properly (are equal).""" + bSa = Similarity2(R=Rot2(), t=np.array([1, 2]), s=3.0) + bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3) + self.gtsamAssertEquals(bSa, bSa_) + + def test_not_eq_translation(self) -> None: + """Ensure object equality works properly (not equal translation).""" + bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0) + bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3) + self.assertNotEqual(bSa, bSa_) + + def test_not_eq_rotation(self) -> None: + """Ensure object equality works properly (not equal rotation).""" + bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0) + bSa_ = Similarity2(R=Rot2.fromDegrees(180), t=np.array([2.0, 1.0]), s=3) + self.assertNotEqual(bSa, bSa_) + + def test_not_eq_scale(self) -> None: + """Ensure object equality works properly (not equal scale).""" + bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0) + bSa_ = Similarity2(R=Rot2(), t=np.array([2.0, 1.0]), s=1.0) + self.assertNotEqual(bSa, bSa_) + + def test_rotation(self) -> None: + """Ensure rotation component is returned properly.""" + R = Rot2.fromDegrees(90) + t = np.array([1, 2]) + bSa = Similarity2(R=R, t=t, s=3.0) + + # evaluates to [[0, -1], [1, 0]] + expected_R = Rot2.fromDegrees(90) + np.testing.assert_allclose(expected_R.matrix(), bSa.rotation().matrix()) + + def test_translation(self) -> None: + """Ensure translation component is returned properly.""" + R = Rot2.fromDegrees(90) + t = np.array([1, 2]) + bSa = Similarity2(R=R, t=t, s=3.0) + + expected_t = np.array([1, 2]) + np.testing.assert_allclose(expected_t, bSa.translation()) + + def test_scale(self) -> None: + """Ensure the scale factor is returned properly.""" + bRa = Rot2() + bta = np.array([1, 2]) + bsa = 3.0 + bSa = Similarity2(R=bRa, t=bta, s=bsa) + self.assertEqual(bSa.scale(), 3.0) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 05a6e7f45..e1a88d616 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -335,21 +335,21 @@ TEST(NonlinearFactorGraph, dot) { "graph {\n" " size=\"5,5\";\n" "\n" - " varl1[label=\"l1\"];\n" - " varx1[label=\"x1\"];\n" - " varx2[label=\"x2\"];\n" + " var7782220156096217089[label=\"l1\"];\n" + " var8646911284551352321[label=\"x1\"];\n" + " var8646911284551352322[label=\"x2\"];\n" "\n" " factor0[label=\"\", shape=point];\n" - " varx1--factor0;\n" + " var8646911284551352321--factor0;\n" " factor1[label=\"\", shape=point];\n" - " varx1--factor1;\n" - " varx2--factor1;\n" + " var8646911284551352321--factor1;\n" + " var8646911284551352322--factor1;\n" " factor2[label=\"\", shape=point];\n" - " varx1--factor2;\n" - " varl1--factor2;\n" + " var8646911284551352321--factor2;\n" + " var7782220156096217089--factor2;\n" " factor3[label=\"\", shape=point];\n" - " varx2--factor3;\n" - " varl1--factor3;\n" + " var8646911284551352322--factor3;\n" + " var7782220156096217089--factor3;\n" "}\n"; const NonlinearFactorGraph fg = createNonlinearFactorGraph(); @@ -363,21 +363,21 @@ TEST(NonlinearFactorGraph, dot_extra) { "graph {\n" " size=\"5,5\";\n" "\n" - " varl1[label=\"l1\", pos=\"0,0!\"];\n" - " varx1[label=\"x1\", pos=\"1,0!\"];\n" - " varx2[label=\"x2\", pos=\"1,1.5!\"];\n" + " var7782220156096217089[label=\"l1\", pos=\"0,0!\"];\n" + " var8646911284551352321[label=\"x1\", pos=\"1,0!\"];\n" + " var8646911284551352322[label=\"x2\", pos=\"1,1.5!\"];\n" "\n" " factor0[label=\"\", shape=point];\n" - " varx1--factor0;\n" + " var8646911284551352321--factor0;\n" " factor1[label=\"\", shape=point];\n" - " varx1--factor1;\n" - " varx2--factor1;\n" + " var8646911284551352321--factor1;\n" + " var8646911284551352322--factor1;\n" " factor2[label=\"\", shape=point];\n" - " varx1--factor2;\n" - " varl1--factor2;\n" + " var8646911284551352321--factor2;\n" + " var7782220156096217089--factor2;\n" " factor3[label=\"\", shape=point];\n" - " varx2--factor3;\n" - " varl1--factor3;\n" + " var8646911284551352322--factor3;\n" + " var7782220156096217089--factor3;\n" "}\n"; const NonlinearFactorGraph fg = createNonlinearFactorGraph();