12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'develop' into fix/ellipses

release/4.3a0
Frank Dellaert 2022-05-10 22:58:19 -04:00
parent de1827ca40 fb1da007f8
commit ce2cf723ad
54 changed files with 1622 additions and 324 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/base/OptionalJacobian.h
View File

@ -20,6 +20,8 @@
#pragma once #pragma once
#include <gtsam/config.h> // Configuration from CMake #include <gtsam/config.h> // Configuration from CMake
#include <Eigen/Dense> #include <Eigen/Dense>
#include <stdexcept>
#include <string>
#ifndef OPTIONALJACOBIAN_NOBOOST #ifndef OPTIONALJACOBIAN_NOBOOST
#include <boost/optional.hpp> #include <boost/optional.hpp>
@ -96,6 +98,24 @@ public:
usurp(dynamic->data()); 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<class MATRIX>
OptionalJacobian(Eigen::Ref<MATRIX> 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 #ifndef OPTIONALJACOBIAN_NOBOOST
/// Constructor with boost::none just makes empty /// Constructor with boost::none just makes empty

2
gtsam/discrete/AlgebraicDecisionTree.h
View File

@ -160,7 +160,7 @@ namespace gtsam {
const typename Base::LabelFormatter& labelFormatter = const typename Base::LabelFormatter& labelFormatter =
&DefaultFormatter) const { &DefaultFormatter) const {
auto valueFormatter = [](const double& v) { auto valueFormatter = [](const double& v) {
return (boost::format("%4.4g") % v).str(); return (boost::format("%4.8g") % v).str();
}; };
Base::print(s, labelFormatter, valueFormatter); Base::print(s, labelFormatter, valueFormatter);
} }

158
gtsam/discrete/DecisionTree-inl.h
View File

@ -59,7 +59,7 @@ namespace gtsam {
/** constant stored in this leaf */ /** constant stored in this leaf */
Y constant_; 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. * Particularly useful when leaves have been pruned.
*/ */
size_t nrAssignments_; size_t nrAssignments_;
@ -68,7 +68,7 @@ namespace gtsam {
Leaf(const Y& constant, size_t nrAssignments = 1) Leaf(const Y& constant, size_t nrAssignments = 1)
: constant_(constant), nrAssignments_(nrAssignments) {} : constant_(constant), nrAssignments_(nrAssignments) {}
/** return the constant */ /// Return the constant
const Y& constant() const { const Y& constant() const {
return constant_; return constant_;
} }
@ -81,19 +81,19 @@ namespace gtsam {
return constant_ == q.constant_; 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 { bool sameLeaf(const Node& q) const override {
return (q.isLeaf() && q.sameLeaf(*this)); return (q.isLeaf() && q.sameLeaf(*this));
} }
/** equality up to tolerance */ /// equality up to tolerance
bool equals(const Node& q, const CompareFunc& compare) const override { bool equals(const Node& q, const CompareFunc& compare) const override {
const Leaf* other = dynamic_cast<const Leaf*>(&q); const Leaf* other = dynamic_cast<const Leaf*>(&q);
if (!other) return false; if (!other) return false;
return compare(this->constant_, other->constant_); return compare(this->constant_, other->constant_);
} }
/** print */ /// print
void print(const std::string& s, const LabelFormatter& labelFormatter, void print(const std::string& s, const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter) const override { const ValueFormatter& valueFormatter) const override {
std::cout << s << " Leaf " << valueFormatter(constant_) << std::endl; std::cout << s << " Leaf " << valueFormatter(constant_) << std::endl;
@ -122,8 +122,8 @@ namespace gtsam {
/// Apply unary operator with assignment /// Apply unary operator with assignment
NodePtr apply(const UnaryAssignment& op, NodePtr apply(const UnaryAssignment& op,
const Assignment<L>& choices) const override { const Assignment<L>& assignment) const override {
NodePtr f(new Leaf(op(choices, constant_), nrAssignments_)); NodePtr f(new Leaf(op(assignment, constant_), nrAssignments_));
return f; return f;
} }
@ -168,7 +168,10 @@ namespace gtsam {
std::vector<NodePtr> branches_; std::vector<NodePtr> branches_;
private: private:
/** incremental allSame */ /**
* Incremental allSame.
* Records if all the branches are the same leaf.
*/
size_t allSame_; size_t allSame_;
using ChoicePtr = boost::shared_ptr<const Choice>; using ChoicePtr = boost::shared_ptr<const Choice>;
@ -181,9 +184,9 @@ namespace gtsam {
#endif #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) { static NodePtr Unique(const ChoicePtr& f) {
#ifndef DT_NO_PRUNING #ifndef GTSAM_DT_NO_PRUNING
if (f->allSame_) { if (f->allSame_) {
assert(f->branches().size() > 0); assert(f->branches().size() > 0);
NodePtr f0 = f->branches_[0]; NodePtr f0 = f->branches_[0];
@ -205,15 +208,13 @@ namespace gtsam {
bool isLeaf() const override { return false; } 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) : Choice(const L& label, size_t count) :
label_(label), allSame_(true) { label_(label), allSame_(true) {
branches_.reserve(count); 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) : Choice(const Choice& f, const Choice& g, const Binary& op) :
allSame_(true) { allSame_(true) {
// Choose what to do based on label // Choose what to do based on label
@ -241,6 +242,7 @@ namespace gtsam {
} }
} }
/// Return the label of this choice node.
const L& label() const { const L& label() const {
return label_; return label_;
} }
@ -262,7 +264,7 @@ namespace gtsam {
branches_.push_back(node); branches_.push_back(node);
} }
/** print (as a tree) */ /// print (as a tree).
void print(const std::string& s, const LabelFormatter& labelFormatter, void print(const std::string& s, const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter) const override { const ValueFormatter& valueFormatter) const override {
std::cout << s << " Choice("; std::cout << s << " Choice(";
@ -308,7 +310,7 @@ namespace gtsam {
return (q.isLeaf() && q.sameLeaf(*this)); return (q.isLeaf() && q.sameLeaf(*this));
} }
/** equality */ /// equality
bool equals(const Node& q, const CompareFunc& compare) const override { bool equals(const Node& q, const CompareFunc& compare) const override {
const Choice* other = dynamic_cast<const Choice*>(&q); const Choice* other = dynamic_cast<const Choice*>(&q);
if (!other) return false; if (!other) return false;
@ -321,7 +323,7 @@ namespace gtsam {
return true; return true;
} }
/** evaluate */ /// evaluate
const Y& operator()(const Assignment<L>& x) const override { const Y& operator()(const Assignment<L>& x) const override {
#ifndef NDEBUG #ifndef NDEBUG
typename Assignment<L>::const_iterator it = x.find(label_); typename Assignment<L>::const_iterator it = x.find(label_);
@ -336,13 +338,13 @@ namespace gtsam {
return (*child)(x); 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) : Choice(const L& label, const Choice& f, const Unary& op) :
label_(label), allSame_(true) { label_(label), allSame_(true) {
branches_.reserve(f.branches_.size()); // reserve space 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 f The original choice node to apply the op on.
* @param op Function to apply on the choice node. Takes Assignment and * @param op Function to apply on the choice node. Takes Assignment and
* value as arguments. * 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, Choice(const L& label, const Choice& f, const UnaryAssignment& op,
const Assignment<L>& choices) const Assignment<L>& assignment)
: label_(label), allSame_(true) { : label_(label), allSame_(true) {
branches_.reserve(f.branches_.size()); // reserve space branches_.reserve(f.branches_.size()); // reserve space
Assignment<L> choices_ = choices; Assignment<L> assignment_ = assignment;
for (size_t i = 0; i < f.branches_.size(); i++) { 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]; 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 // Remove the assignment so we are backtracking
auto choice_it = choices_.find(label_); auto assignment_it = assignment_.find(label_);
choices_.erase(choice_it); assignment_.erase(assignment_it);
} }
} }
/** apply unary operator */ /// apply unary operator.
NodePtr apply(const Unary& op) const override { NodePtr apply(const Unary& op) const override {
auto r = boost::make_shared<Choice>(label_, *this, op); auto r = boost::make_shared<Choice>(label_, *this, op);
return Unique(r); return Unique(r);
@ -382,8 +384,8 @@ namespace gtsam {
/// Apply unary operator with assignment /// Apply unary operator with assignment
NodePtr apply(const UnaryAssignment& op, NodePtr apply(const UnaryAssignment& op,
const Assignment<L>& choices) const override { const Assignment<L>& assignment) const override {
auto r = boost::make_shared<Choice>(label_, *this, op, choices); auto r = boost::make_shared<Choice>(label_, *this, op, assignment);
return Unique(r); return Unique(r);
} }
@ -678,7 +680,16 @@ namespace gtsam {
} }
/****************************************************************************/ /****************************************************************************/
// Functor performing depth-first visit without Assignment<L> 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 <typename L, typename Y> template <typename L, typename Y>
struct Visit { struct Visit {
using F = std::function<void(const Y&)>; using F = std::function<void(const Y&)>;
@ -707,33 +718,74 @@ namespace gtsam {
} }
/****************************************************************************/ /****************************************************************************/
// Functor performing depth-first visit with Assignment<L> 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 <typename L, typename Y>
struct VisitLeaf {
using F = std::function<void(const typename DecisionTree<L, Y>::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<L, Y>::NodePtr& node) const {
using Leaf = typename DecisionTree<L, Y>::Leaf;
if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node))
return f(*leaf);
using Choice = typename DecisionTree<L, Y>::Choice;
auto choice = boost::dynamic_pointer_cast<const Choice>(node);
if (!choice)
throw std::invalid_argument("DecisionTree::VisitLeaf: Invalid NodePtr");
for (auto&& branch : choice->branches()) (*this)(branch); // recurse!
}
};
template <typename L, typename Y>
template <typename Func>
void DecisionTree<L, Y>::visitLeaf(Func f) const {
VisitLeaf<L, Y> visit(f);
visit(root_);
}
/****************************************************************************/
/**
* Functor performing depth-first visit to each leaf with the leaf's
* `Assignment<L>` and value passed as arguments.
*
* NOTE: Follows the same pruning semantics as `visit`.
*/
template <typename L, typename Y> template <typename L, typename Y>
struct VisitWith { struct VisitWith {
using Choices = Assignment<L>; using F = std::function<void(const Assignment<L>&, const Y&)>;
using F = std::function<void(const Choices&, const Y&)>;
explicit VisitWith(F f) : f(f) {} ///< Construct from folding function. explicit VisitWith(F f) : f(f) {} ///< Construct from folding function.
Choices choices; ///< Assignment, mutating through recursion. Assignment<L> assignment; ///< Assignment, mutating through recursion.
F f; ///< folding function object. F f; ///< folding function object.
/// Do a depth-first visit on the tree rooted at node. /// Do a depth-first visit on the tree rooted at node.
void operator()(const typename DecisionTree<L, Y>::NodePtr& node) { void operator()(const typename DecisionTree<L, Y>::NodePtr& node) {
using Leaf = typename DecisionTree<L, Y>::Leaf; using Leaf = typename DecisionTree<L, Y>::Leaf;
if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node)) if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node))
return f(choices, leaf->constant()); return f(assignment, leaf->constant());
using Choice = typename DecisionTree<L, Y>::Choice; using Choice = typename DecisionTree<L, Y>::Choice;
auto choice = boost::dynamic_pointer_cast<const Choice>(node); auto choice = boost::dynamic_pointer_cast<const Choice>(node);
if (!choice) if (!choice)
throw std::invalid_argument("DecisionTree::VisitWith: Invalid NodePtr"); throw std::invalid_argument("DecisionTree::VisitWith: Invalid NodePtr");
for (size_t i = 0; i < choice->nrChoices(); i++) { 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! (*this)(choice->branches()[i]); // recurse!
// Remove the choice so we are backtracking // Remove the choice so we are backtracking
auto choice_it = choices.find(choice->label()); auto choice_it = assignment.find(choice->label());
choices.erase(choice_it); 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 <typename L, typename Y> template <typename L, typename Y>
std::set<L> DecisionTree<L, Y>::labels() const { std::set<L> DecisionTree<L, Y>::labels() const {
std::set<L> unique; std::set<L> unique;
auto f = [&](const Assignment<L>& choices, const Y&) { auto f = [&](const Assignment<L>& assignment, const Y&) {
for (auto&& kv : choices) unique.insert(kv.first); for (auto&& kv : assignment) {
unique.insert(kv.first);
}
}; };
visitWith(f); visitWith(f);
return unique; return unique;
@ -817,8 +883,8 @@ namespace gtsam {
throw std::runtime_error( throw std::runtime_error(
"DecisionTree::apply(unary op) undefined for empty tree."); "DecisionTree::apply(unary op) undefined for empty tree.");
} }
Assignment<L> choices; Assignment<L> assignment;
return DecisionTree(root_->apply(op, choices)); return DecisionTree(root_->apply(op, assignment));
} }
/****************************************************************************/ /****************************************************************************/

40
gtsam/discrete/DecisionTree.h
View File

@ -105,7 +105,7 @@ namespace gtsam {
virtual const Y& operator()(const Assignment<L>& x) const = 0; virtual const Y& operator()(const Assignment<L>& x) const = 0;
virtual Ptr apply(const Unary& op) const = 0; virtual Ptr apply(const Unary& op) const = 0;
virtual Ptr apply(const UnaryAssignment& op, virtual Ptr apply(const UnaryAssignment& op,
const Assignment<L>& choices) const = 0; const Assignment<L>& assignment) const = 0;
virtual Ptr apply_f_op_g(const Node&, const Binary&) 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_fL(const Leaf&, const Binary&) const = 0;
virtual Ptr apply_g_op_fC(const Choice&, 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 */ /** Create a constant */
explicit DecisionTree(const Y& y); 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); DecisionTree(const L& label, const Y& y1, const Y& y2);
/** Allow Label+Cardinality for convenience */ /** Allow Label+Cardinality for convenience */
@ -219,9 +219,8 @@ namespace gtsam {
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/** Make virtual */ /// Make virtual
virtual ~DecisionTree() { virtual ~DecisionTree() {}
}
/// Check if tree is empty. /// Check if tree is empty.
bool empty() const { return !root_; } bool empty() const { return !root_; }
@ -235,9 +234,11 @@ namespace gtsam {
/** /**
* @brief Visit all leaves in depth-first fashion. * @brief Visit all leaves in depth-first fashion.
* *
* @param f side-effect taking a value. * @param f (side-effect) Function taking a value.
* *
* @note Due to pruning, leaves might not exhaust choices. * @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: * Example:
* int sum = 0; * int sum = 0;
@ -250,13 +251,32 @@ namespace gtsam {
/** /**
* @brief Visit all leaves in depth-first fashion. * @brief Visit all leaves in depth-first fashion.
* *
* @param f side-effect taking an assignment and a value. * @param f (side-effect) Function taking the leaf node pointer.
* *
* @note Due to pruning, leaves might not exhaust choices. * @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: * Example:
* int sum = 0; * int sum = 0;
* auto visitor = [&](const Assignment<L>& choices, int y) { sum += y; }; * auto visitor = [&](int y) { sum += y; };
* tree.visitWith(visitor);
*/
template <typename Func>
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<L>& assignment, int y) { sum += y; };
* tree.visitWith(visitor); * tree.visitWith(visitor);
*/ */
template <typename Func> template <typename Func>

10
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -287,12 +287,16 @@ namespace gtsam {
cardinalities_(keys.cardinalities()) {} cardinalities_(keys.cardinalities()) {}
/* ************************************************************************ */ /* ************************************************************************ */
DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrLeaves) const { DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const {
const size_t N = maxNrLeaves; const size_t N = maxNrAssignments;
// Get the probabilities in the decision tree so we can threshold. // Get the probabilities in the decision tree so we can threshold.
std::vector<double> probabilities; std::vector<double> 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 // The number of probabilities can be lower than max_leaves
if (probabilities.size() <= N) { if (probabilities.size() <= N) {

16
gtsam/discrete/DecisionTreeFactor.h
View File

@ -174,13 +174,21 @@ namespace gtsam {
* @brief Prune the decision tree of discrete variables. * @brief Prune the decision tree of discrete variables.
* *
* Pruning will set the leaves to be "pruned" to 0 indicating a 0 * Pruning will set the leaves to be "pruned" to 0 indicating a 0
* probability. * probability. An assignment is pruned if it is not in the top
* A leaf is pruned if it is not in the top `maxNrLeaves` values. * `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 * @return DecisionTreeFactor
*/ */
DecisionTreeFactor prune(size_t maxNrLeaves) const; DecisionTreeFactor prune(size_t maxNrAssignments) const;
/// @} /// @}
/// @name Wrapper support /// @name Wrapper support

2
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/discrete/DiscreteKey.h> // make sure we have traits #include <gtsam/discrete/DiscreteKey.h> // make sure we have traits
#include <gtsam/discrete/DiscreteValues.h> #include <gtsam/discrete/DiscreteValues.h>
// headers first to make sure no missing headers // headers first to make sure no missing headers
//#define DT_NO_PRUNING //#define GTSAM_DT_NO_PRUNING
#include <gtsam/discrete/AlgebraicDecisionTree.h> #include <gtsam/discrete/AlgebraicDecisionTree.h>
#include <gtsam/discrete/DecisionTree-inl.h> // for convert only #include <gtsam/discrete/DecisionTree-inl.h> // for convert only
#define DISABLE_TIMING #define DISABLE_TIMING

2
gtsam/discrete/tests/testDecisionTree.cpp
View File

@ -18,7 +18,7 @@
*/ */
// #define DT_DEBUG_MEMORY // #define DT_DEBUG_MEMORY
// #define DT_NO_PRUNING // #define GTSAM_DT_NO_PRUNING
#define DISABLE_DOT #define DISABLE_DOT
#include <gtsam/discrete/DecisionTree-inl.h> #include <gtsam/discrete/DecisionTree-inl.h>

23
gtsam/discrete/tests/testDecisionTreeFactor.cpp
View File

@ -113,18 +113,32 @@ TEST(DecisionTreeFactor, Prune) {
DecisionTreeFactor f(A & B & C, "1 5 3 7 2 6 4 8"); DecisionTreeFactor f(A & B & C, "1 5 3 7 2 6 4 8");
// Only keep the leaves with the top 5 values. // Only keep the leaves with the top 5 values.
size_t maxNrLeaves = 5; size_t maxNrAssignments = 5;
auto pruned5 = f.prune(maxNrLeaves); auto pruned5 = f.prune(maxNrAssignments);
// Pruned leaves should be 0 // Pruned leaves should be 0
DecisionTreeFactor expected(A & B & C, "0 5 0 7 0 6 4 8"); DecisionTreeFactor expected(A & B & C, "0 5 0 7 0 6 4 8");
EXPECT(assert_equal(expected, pruned5)); EXPECT(assert_equal(expected, pruned5));
// Check for more extreme pruning where we only keep the top 2 leaves // Check for more extreme pruning where we only keep the top 2 leaves
maxNrLeaves = 2; maxNrAssignments = 2;
auto pruned2 = f.prune(maxNrLeaves); auto pruned2 = f.prune(maxNrAssignments);
DecisionTreeFactor expected2(A & B & C, "0 0 0 7 0 0 0 8"); DecisionTreeFactor expected2(A & B & C, "0 0 0 7 0 0 0 8");
EXPECT(assert_equal(expected2, pruned2)); 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -215,4 +229,3 @@ int main() {
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }
/* ************************************************************************* */ /* ************************************************************************* */

14
gtsam/discrete/tests/testDiscreteFactorGraph.cpp
View File

@ -415,16 +415,16 @@ TEST(DiscreteFactorGraph, DotWithNames) {
"graph {\n" "graph {\n"
" size=\"5,5\";\n" " size=\"5,5\";\n"
"\n" "\n"
" varC[label=\"C\"];\n" " var0[label=\"C\"];\n"
" varA[label=\"A\"];\n" " var1[label=\"A\"];\n"
" varB[label=\"B\"];\n" " var2[label=\"B\"];\n"
"\n" "\n"
" factor0[label=\"\", shape=point];\n" " factor0[label=\"\", shape=point];\n"
" varC--factor0;\n" " var0--factor0;\n"
" varA--factor0;\n" " var1--factor0;\n"
" factor1[label=\"\", shape=point];\n" " factor1[label=\"\", shape=point];\n"
" varC--factor1;\n" " var0--factor1;\n"
" varB--factor1;\n" " var2--factor1;\n"
"}\n"; "}\n";
EXPECT(actual == expected); EXPECT(actual == expected);
} }

12
gtsam/geometry/Point2.cpp
View File

@ -113,6 +113,18 @@ list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2,
return circleCircleIntersection(c1, c2, fh); return circleCircleIntersection(c1, c2, fh);
} }
Point2Pair means(const std::vector<Point2Pair> &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) { ostream &operator<<(ostream &os, const gtsam::Point2Pair &p) {
os << p.first << " <-> " << p.second; os << p.first << " <-> " << p.second;

3
gtsam/geometry/Point2.h
View File

@ -72,6 +72,9 @@ GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double
*/ */
GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh); GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
/// Calculate the two means of a set of Point2 pairs
GTSAM_EXPORT Point2Pair means(const std::vector<Point2Pair> &abPointPairs);
/** /**
* @brief Intersect 2 circles * @brief Intersect 2 circles
* @param c1 center of first circle * @param c1 center of first circle

2
gtsam/geometry/Pose2.cpp
View File

@ -365,7 +365,7 @@ boost::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
"Pose2:Align expects 2*N matrices of equal shape."); "Pose2:Align expects 2*N matrices of equal shape.");
} }
Point2Pairs ab_pairs; 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)); ab_pairs.emplace_back(a.col(j), b.col(j));
} }
return Pose2::Align(ab_pairs); return Pose2::Align(ab_pairs);

4
gtsam/geometry/Pose2.h
View File

@ -353,6 +353,10 @@ GTSAM_EXPORT boost::optional<Pose2>
GTSAM_DEPRECATED align(const Point2Pairs& pairs); GTSAM_DEPRECATED align(const Point2Pairs& pairs);
#endif #endif
// Convenience typedef
using Pose2Pair = std::pair<Pose2, Pose2>;
using Pose2Pairs = std::vector<Pose2Pair>;
template <> template <>
struct traits<Pose2> : public internal::LieGroup<Pose2> {}; struct traits<Pose2> : public internal::LieGroup<Pose2> {};

2
gtsam/geometry/Pose3.cpp
View File

@ -473,7 +473,7 @@ boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
"Pose3:Align expects 3*N matrices of equal shape."); "Pose3:Align expects 3*N matrices of equal shape.");
} }
Point3Pairs abPointPairs; 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)); abPointPairs.emplace_back(a.col(j), b.col(j));
} }
return Pose3::Align(abPointPairs); return Pose3::Align(abPointPairs);

13
gtsam/geometry/Rot2.cpp
View File

@ -129,6 +129,19 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
} }
} }
/* ************************************************************************* */
Rot2 Rot2::ClosestTo(const Matrix2& M) {
Eigen::JacobiSVD<Matrix2> 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 } // gtsam

4
gtsam/geometry/Rot2.h
View File

@ -14,6 +14,7 @@
* @brief 2D rotation * @brief 2D rotation
* @date Dec 9, 2009 * @date Dec 9, 2009
* @author Frank Dellaert * @author Frank Dellaert
* @author John Lambert
*/ */
#pragma once #pragma once
@ -209,6 +210,9 @@ namespace gtsam {
/** return 2*2 transpose (inverse) rotation matrix */ /** return 2*2 transpose (inverse) rotation matrix */
Matrix2 transpose() const; Matrix2 transpose() const;
/** Find closest valid rotation matrix, given a 2x2 matrix */
static Rot2 ClosestTo(const Matrix2& M);
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;

242
gtsam/geometry/Similarity2.cpp Normal file
View File

@ -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 <gtsam/base/Manifold.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Similarity2.h>
#include <gtsam/slam/KarcherMeanFactor-inl.h>
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<Point2>::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<Rot2> 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<Rot2>(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

200
gtsam/geometry/Similarity2.h Normal file
View File

@ -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 <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/dllexport.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Rot2.h>
namespace gtsam {
// Forward declarations
class Pose2;
/**
* 2D similarity transform
*/
class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
/// @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<Pose2Pair>& 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<Similarity2, 4>::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<Similarity2> : public internal::LieGroup<Similarity2> {};
template <>
struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
} // namespace gtsam

20
gtsam/geometry/Similarity3.cpp
View File

@ -26,7 +26,7 @@ namespace gtsam {
using std::vector; using std::vector;
namespace { namespace internal {
/// Subtract centroids from point pairs. /// Subtract centroids from point pairs.
static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs, static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
const Point3Pair &centroids) { const Point3Pair &centroids) {
@ -81,10 +81,10 @@ static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb,
static Similarity3 alignGivenR(const Point3Pairs &abPointPairs, static Similarity3 alignGivenR(const Point3Pairs &abPointPairs,
const Rot3 &aRb) { const Rot3 &aRb) {
auto centroids = means(abPointPairs); auto centroids = means(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
return align(d_abPointPairs, aRb, centroids); return align(d_abPointPairs, aRb, centroids);
} }
} // namespace } // namespace internal
Similarity3::Similarity3() : Similarity3::Similarity3() :
t_(0,0,0), s_(1) { t_(0,0,0), s_(1) {
@ -165,11 +165,11 @@ Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
if (abPointPairs.size() < 3) if (abPointPairs.size() < 3)
throw std::runtime_error("input should have at least 3 pairs of points"); throw std::runtime_error("input should have at least 3 pairs of points");
auto centroids = means(abPointPairs); auto centroids = means(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
Matrix3 H = calculateH(d_abPointPairs); Matrix3 H = internal::calculateH(d_abPointPairs);
// ClosestTo finds rotation matrix closest to H in Frobenius sense // ClosestTo finds rotation matrix closest to H in Frobenius sense
Rot3 aRb = Rot3::ClosestTo(H); Rot3 aRb = Rot3::ClosestTo(H);
return align(d_abPointPairs, aRb, centroids); return internal::align(d_abPointPairs, aRb, centroids);
} }
Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) { Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
@ -192,7 +192,7 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
} }
const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations); const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
return alignGivenR(abPointPairs, aRb_estimate); return internal::alignGivenR(abPointPairs, aRb_estimate);
} }
Matrix4 Similarity3::wedge(const Vector7 &xi) { Matrix4 Similarity3::wedge(const Vector7 &xi) {
@ -283,15 +283,11 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
return os; return os;
} }
const Matrix4 Similarity3::matrix() const { Matrix4 Similarity3::matrix() const {
Matrix4 T; Matrix4 T;
T.topRows<3>() << R_.matrix(), t_; T.topRows<3>() << R_.matrix(), t_;
T.bottomRows<1>() << 0, 0, 0, 1.0 / s_; T.bottomRows<1>() << 0, 0, 0, 1.0 / s_;
return T; return T;
} }
Similarity3::operator Pose3() const {
return Pose3(R_, s_ * t_);
}
} // namespace gtsam } // namespace gtsam

89
gtsam/geometry/Similarity3.h
View File

@ -18,13 +18,12 @@
#pragma once #pragma once
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Lie.h> #include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h> #include <gtsam/base/Manifold.h>
#include <gtsam/dllexport.h> #include <gtsam/dllexport.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
namespace gtsam { namespace gtsam {
@ -34,8 +33,7 @@ class Pose3;
/** /**
* 3D similarity transform * 3D similarity transform
*/ */
class Similarity3: public LieGroup<Similarity3, 7> { class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
/// @name Pose Concept /// @name Pose Concept
/// @{ /// @{
typedef Rot3 Rotation; typedef Rot3 Rotation;
@ -48,59 +46,58 @@ private:
double s_; double s_;
public: public:
/// @name Constructors /// @name Constructors
/// @{ /// @{
/// Default constructor /// Default constructor
GTSAM_EXPORT Similarity3(); Similarity3();
/// Construct pure scaling /// Construct pure scaling
GTSAM_EXPORT Similarity3(double s); Similarity3(double s);
/// Construct from GTSAM types /// 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 /// 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] /// Construct from matrix [R t; 0 s^-1]
GTSAM_EXPORT Similarity3(const Matrix4& T); Similarity3(const Matrix4& T);
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
/// Compare with tolerance /// Compare with tolerance
GTSAM_EXPORT bool equals(const Similarity3& sim, double tol) const; bool equals(const Similarity3& sim, double tol) const;
/// Exact equality /// Exact equality
GTSAM_EXPORT bool operator==(const Similarity3& other) const; bool operator==(const Similarity3& other) const;
/// Print with optional string /// 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 /// @name Group
/// @{ /// @{
/// Return an identity transform /// Return an identity transform
GTSAM_EXPORT static Similarity3 identity(); static Similarity3 identity();
/// Composition /// Composition
GTSAM_EXPORT Similarity3 operator*(const Similarity3& S) const; Similarity3 operator*(const Similarity3& S) const;
/// Return the inverse /// Return the inverse
GTSAM_EXPORT Similarity3 inverse() const; Similarity3 inverse() const;
/// @} /// @}
/// @name Group action on Point3 /// @name Group action on Point3
/// @{ /// @{
/// Action on a point p is s*(R*p+t) /// Action on a point p is s*(R*p+t)
GTSAM_EXPORT Point3 transformFrom(const Point3& p, // Point3 transformFrom(const Point3& p, //
OptionalJacobian<3, 7> H1 = boost::none, // OptionalJacobian<3, 7> H1 = boost::none, //
OptionalJacobian<3, 3> H2 = boost::none) const; OptionalJacobian<3, 3> H2 = boost::none) const;
@ -115,15 +112,15 @@ public:
* 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 * 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 */ /** 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 * Create Similarity3 by aligning at least three point pairs
*/ */
GTSAM_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs); static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
/** /**
* Create the Similarity3 object that aligns at least two pose pairs. * Create the Similarity3 object that aligns at least two pose pairs.
@ -131,11 +128,11 @@ public:
* Given a list of pairs in frame a, and a list of pairs in frame b, Align() * 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. * will compute the best-fit Similarity3 aSb transformation to align them.
* First, the rotation aRb will be computed as the average (Karcher mean) of * 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 * many estimates aRb (from each pair). Afterwards, the scale factor will be
* using the algorithm described here: * computed using the algorithm described here:
* http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
*/ */
GTSAM_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs); static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
/// @} /// @}
/// @name Lie Group /// @name Lie Group
@ -144,20 +141,22 @@ public:
/** Log map at the identity /** Log map at the identity
* \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$ * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$
*/ */
GTSAM_EXPORT static Vector7 Logmap(const Similarity3& s, // static Vector7 Logmap(const Similarity3& s, //
OptionalJacobian<7, 7> Hm = boost::none); OptionalJacobian<7, 7> Hm = boost::none);
/** Exponential map at the identity /** Exponential map at the identity
*/ */
GTSAM_EXPORT static Similarity3 Expmap(const Vector7& v, // static Similarity3 Expmap(const Vector7& v, //
OptionalJacobian<7, 7> Hm = boost::none); OptionalJacobian<7, 7> Hm = boost::none);
/// Chart at the origin /// Chart at the origin
struct ChartAtOrigin { 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); 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); return Similarity3::Logmap(other, H);
} }
}; };
@ -170,46 +169,32 @@ public:
* @return 4*4 element of Lie algebra that can be exponentiated * @return 4*4 element of Lie algebra that can be exponentiated
* TODO(frank): rename to Hat, make part of traits * 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 /// Project from one tangent space to another
GTSAM_EXPORT Matrix7 AdjointMap() const; Matrix7 AdjointMap() const;
/// @} /// @}
/// @name Standard interface /// @name Standard interface
/// @{ /// @{
/// Calculate 4*4 matrix group equivalent /// Calculate 4*4 matrix group equivalent
GTSAM_EXPORT const Matrix4 matrix() const; Matrix4 matrix() const;
/// Return a GTSAM rotation /// Return a GTSAM rotation
const Rot3& rotation() const { Rot3 rotation() const { return R_; }
return R_;
}
/// Return a GTSAM translation /// Return a GTSAM translation
const Point3& translation() const { Point3 translation() const { return t_; }
return t_;
}
/// Return the scale /// Return the scale
double scale() const { double scale() const { return s_; }
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;
/// Dimensionality of tangent space = 7 DOF - used to autodetect sizes /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
inline static size_t Dim() { inline static size_t Dim() { return 7; }
return 7;
}
/// Dimensionality of tangent space = 7 DOF /// Dimensionality of tangent space = 7 DOF
inline size_t dim() const { inline size_t dim() const { return 7; }
return 7;
}
/// @} /// @}
/// @name Helper functions /// @name Helper functions

187
gtsam/geometry/geometry.i
View File

@ -547,6 +547,12 @@ class EssentialMatrix {
// Standard Constructors // Standard Constructors
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); 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<Eigen::MatrixXd> H);
// Testable // Testable
void print(string s = "") const; void print(string s = "") const;
bool equals(const gtsam::EssentialMatrix& pose, double tol) const; bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
@ -584,7 +590,13 @@ class Cal3_S2 {
// Action on Point2 // Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p) const; gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
// Standard Interface // Standard Interface
double fx() const; double fx() const;
@ -623,7 +635,13 @@ virtual class Cal3DS2_Base {
// Action on Point2 // Action on Point2
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const; gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -680,7 +698,13 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
// Note: the signature of this functions differ from the functions // Note: the signature of this functions differ from the functions
// with equal name in the base class. // with equal name in the base class.
gtsam::Point2 calibrate(const gtsam::Point2& p) const; gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -706,7 +730,13 @@ class Cal3Fisheye {
// Action on Point2 // Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p) const; gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
// Standard Interface // Standard Interface
double fx() const; double fx() const;
@ -769,7 +799,13 @@ class Cal3Bundler {
// Action on Point2 // Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p) const; gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
// Standard Interface // Standard Interface
double fx() const; double fx() const;
@ -807,12 +843,25 @@ class CalibratedCamera {
// Action on Point3 // Action on Point3
gtsam::Point2 project(const gtsam::Point3& point) const; gtsam::Point2 project(const gtsam::Point3& point) const;
gtsam::Point2 project(const gtsam::Point3& point,
Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpoint);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth);
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
// Standard Interface // Standard Interface
gtsam::Pose3 pose() const; gtsam::Pose3 pose() const;
double range(const gtsam::Point3& point) const; double range(const gtsam::Point3& point) const;
double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpoint);
double range(const gtsam::Pose3& pose) const; double range(const gtsam::Pose3& pose) const;
double range(const gtsam::Pose3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpose);
double range(const gtsam::CalibratedCamera& camera) const; double range(const gtsam::CalibratedCamera& camera) const;
// enabling serialization functionality // enabling serialization functionality
@ -824,6 +873,7 @@ template <CALIBRATION>
class PinholeCamera { class PinholeCamera {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
PinholeCamera(); PinholeCamera();
PinholeCamera(const gtsam::PinholeCamera<CALIBRATION> other);
PinholeCamera(const gtsam::Pose3& pose); PinholeCamera(const gtsam::Pose3& pose);
PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K);
static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, static This Level(const CALIBRATION& K, const gtsam::Pose2& pose,
@ -850,14 +900,123 @@ class PinholeCamera {
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const; pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point); gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point2 project(const gtsam::Point3& point,
Eigen::Ref<Eigen::MatrixXd> Dpose,
Eigen::Ref<Eigen::MatrixXd> Dpoint,
Eigen::Ref<Eigen::MatrixXd> Dcal);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
gtsam::Point2 reprojectionError(const gtsam::Point3& pw, const gtsam::Point2& measured,
Eigen::Ref<Eigen::MatrixXd> Dpose,
Eigen::Ref<Eigen::MatrixXd> Dpoint,
Eigen::Ref<Eigen::MatrixXd> Dcal);
double range(const gtsam::Point3& point); double range(const gtsam::Point3& point);
double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpoint);
double range(const gtsam::Pose3& pose); double range(const gtsam::Pose3& pose);
double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpose);
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
}; };
// Forward declaration of PinholeCameraCalX is defined here.
#include <gtsam/geometry/SimpleCamera.h>
// Some typedefs for common camera types
// PinholeCameraCal3_S2 is the same as SimpleCamera above
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
#include <gtsam/geometry/PinholePose.h>
template <CALIBRATION>
class PinholePose {
// Standard Constructors and Named Constructors
PinholePose();
PinholePose(const gtsam::PinholePose<CALIBRATION> 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<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point2 project(const gtsam::Point3& point,
Eigen::Ref<Eigen::MatrixXd> Dpose,
Eigen::Ref<Eigen::MatrixXd> Dpoint,
Eigen::Ref<Eigen::MatrixXd> Dcal);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
double range(const gtsam::Point3& point);
double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpoint);
double range(const gtsam::Pose3& pose);
double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpose);
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::PinholePose<gtsam::Cal3_S2> PinholePoseCal3_S2;
typedef gtsam::PinholePose<gtsam::Cal3DS2> PinholePoseCal3DS2;
typedef gtsam::PinholePose<gtsam::Cal3Unified> PinholePoseCal3Unified;
typedef gtsam::PinholePose<gtsam::Cal3Bundler> PinholePoseCal3Bundler;
typedef gtsam::PinholePose<gtsam::Cal3Fisheye> PinholePoseCal3Fisheye;
#include <gtsam/geometry/Similarity2.h>
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 <gtsam/geometry/Similarity3.h> #include <gtsam/geometry/Similarity3.h>
class Similarity3 { class Similarity3 {
// Standard Constructors // Standard Constructors
@ -874,22 +1033,13 @@ class Similarity3 {
static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs); static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs);
// Standard Interface // Standard Interface
const Matrix matrix() const; bool equals(const gtsam::Similarity3& sim, double tol) const;
const gtsam::Rot3& rotation(); Matrix matrix() const;
const gtsam::Point3& translation(); gtsam::Rot3& rotation();
gtsam::Point3& translation();
double scale() const; double scale() const;
}; };
// Forward declaration of PinholeCameraCalX is defined here.
#include <gtsam/geometry/SimpleCamera.h>
// Some typedefs for common camera types
// PinholeCameraCal3_S2 is the same as SimpleCamera above
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
template <T> template <T>
class CameraSet { class CameraSet {
CameraSet(); CameraSet();
@ -921,9 +1071,18 @@ class StereoCamera {
static size_t Dim(); static size_t Dim();
// Transformations and measurement functions // 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; gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const;
// project with Jacobian
gtsam::StereoPoint2 project2(const gtsam::Point3& point,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> H2) const;
gtsam::Point3 backproject2(const gtsam::StereoPoint2& p,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> H2) const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
}; };

66
gtsam/geometry/tests/testSimilarity2.cpp Normal file
View File

@ -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 <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/Similarity2.h>
#include <functional>
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<Similarity2>));
BOOST_CONCEPT_ASSERT((IsManifold<Similarity2>));
BOOST_CONCEPT_ASSERT((IsLieGroup<Similarity2>));
}
//******************************************************************************
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);
}
//******************************************************************************

22
gtsam/geometry/tests/testSimilarity3.cpp
View File

@ -458,18 +458,18 @@ TEST(Similarity3, Optimization2) {
Values result; Values result;
result = LevenbergMarquardtOptimizer(graph, initial).optimize(); result = LevenbergMarquardtOptimizer(graph, initial).optimize();
//result.print("Optimized Estimate\n"); //result.print("Optimized Estimate\n");
Pose3 p1, p2, p3, p4, p5; Similarity3 p1, p2, p3, p4, p5;
p1 = Pose3(result.at<Similarity3>(X(1))); p1 = result.at<Similarity3>(X(1));
p2 = Pose3(result.at<Similarity3>(X(2))); p2 = result.at<Similarity3>(X(2));
p3 = Pose3(result.at<Similarity3>(X(3))); p3 = result.at<Similarity3>(X(3));
p4 = Pose3(result.at<Similarity3>(X(4))); p4 = result.at<Similarity3>(X(4));
p5 = Pose3(result.at<Similarity3>(X(5))); p5 = result.at<Similarity3>(X(5));
//p1.print("Pose1"); //p1.print("Similarity1");
//p2.print("Pose2"); //p2.print("Similarity2");
//p3.print("Pose3"); //p3.print("Similarity3");
//p4.print("Pose4"); //p4.print("Similarity4");
//p5.print("Pose5"); //p5.print("Similarity5");
Similarity3 expected(0.7); Similarity3 expected(0.7);
EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4)); EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4));

5
gtsam/inference/BayesNet-inst.h
View File

@ -53,8 +53,9 @@ void BayesNet<CONDITIONAL>::dot(std::ostream& os,
auto frontals = conditional->frontals(); auto frontals = conditional->frontals();
const Key me = frontals.front(); const Key me = frontals.front();
auto parents = conditional->parents(); auto parents = conditional->parents();
for (const Key& p : parents) for (const Key& p : parents) {
os << " var" << keyFormatter(p) << "->var" << keyFormatter(me) << "\n"; os << " var" << p << "->var" << me << "\n";
}
} }
os << "}"; os << "}";

8
gtsam/inference/DotWriter.cpp
View File

@ -43,7 +43,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& position, const boost::optional<Vector2>& position,
ostream* os) const { ostream* os) const {
// Label the node with the label from the KeyFormatter // Label the node with the label from the KeyFormatter
*os << " var" << keyFormatter(key) << "[label=\"" << keyFormatter(key) *os << " var" << key << "[label=\"" << keyFormatter(key)
<< "\""; << "\"";
if (position) { if (position) {
*os << ", pos=\"" << position->x() << "," << position->y() << "!\""; *os << ", pos=\"" << position->x() << "," << position->y() << "!\"";
@ -65,13 +65,13 @@ void DotWriter::DrawFactor(size_t i, const boost::optional<Vector2>& position,
static void ConnectVariables(Key key1, Key key2, static void ConnectVariables(Key key1, Key key2,
const KeyFormatter& keyFormatter, ostream* os) { const KeyFormatter& keyFormatter, ostream* os) {
*os << " var" << keyFormatter(key1) << "--" *os << " var" << key1 << "--"
<< "var" << keyFormatter(key2) << ";\n"; << "var" << key2 << ";\n";
} }
static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter, static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter,
size_t i, ostream* os) { size_t i, ostream* os) {
*os << " var" << keyFormatter(key) << "--" *os << " var" << key << "--"
<< "factor" << i << ";\n"; << "factor" << i << ";\n";
} }

153
gtsam/linear/LossFunctions.h
View File

@ -54,23 +54,31 @@ namespace noiseModel {
// clang-format on // clang-format on
namespace mEstimator { 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 { class GTSAM_EXPORT Base {
public: 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 }; enum ReweightScheme { Scalar, Block };
typedef boost::shared_ptr<Base> shared_ptr; typedef boost::shared_ptr<Base> shared_ptr;
protected: protected:
/** the rows can be weighted independently according to the error /// Strategy for reweighting \sa ReweightScheme
* or uniformly with the norm of the right hand side */
ReweightScheme reweight_; ReweightScheme reweight_;
public: public:
Base(const ReweightScheme reweight = Block) : reweight_(reweight) {} Base(const ReweightScheme reweight = Block) : reweight_(reweight) {}
virtual ~Base() {} 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 * This method is responsible for returning the total penalty for a given
* amount of error. For example, this method is responsible for implementing * amount of error. For example, this method is responsible for implementing
* the quadratic function for an L2 penalty, the absolute value function for * 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 * error vector, then it prevents implementations of asymmeric loss
* functions. It would be better for this function to accept the vector and * functions. It would be better for this function to accept the vector and
* internally call the norm if necessary. * 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 * This method is responsible for returning the weight function for a given
* amount of error. The weight function is related to the analytic derivative * amount of error. The weight function is related to the analytic derivative
* of the loss function. See * of the loss function. See
* https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
* for details. This method is required when optimizing cost functions with * for details. This method is required when optimizing cost functions with
* robust penalties using iteratively re-weighted least squares. * robust penalties using iteratively re-weighted least squares.
*
* This returns w(x) in \ref mEstimator
*/ */
virtual double weight(double distance) const = 0; 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 { class GTSAM_EXPORT Null : public Base {
public: public:
typedef boost::shared_ptr<Null> shared_ptr; typedef boost::shared_ptr<Null> 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 { class GTSAM_EXPORT Fair : public Base {
protected: protected:
double c_; double c_;
@ -160,6 +187,7 @@ class GTSAM_EXPORT Fair : public Base {
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double c, const ReweightScheme reweight = Block); static shared_ptr Create(double c, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
private: private:
/** Serialization function */ /** 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|<k, 0.5 k² + k|x-k| otherwise
* - Derivative \phi(x) = x if |x|<k, k sgn(x) otherwise
* - Weight w(x) = \phi(x)/x = 1 if |x|<k, k/|x| otherwise
*/
class GTSAM_EXPORT Huber : public Base { class GTSAM_EXPORT Huber : public Base {
protected: protected:
double k_; double k_;
@ -185,6 +220,7 @@ class GTSAM_EXPORT Huber : public Base {
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return k_; }
private: private:
/** Serialization function */ /** Serialization function */
@ -196,12 +232,19 @@ class GTSAM_EXPORT Huber : public Base {
} }
}; };
/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed /** Implementation of the "Cauchy" robust error model (Lee2013IROS).
/// by: * Contributed by:
/// Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for * Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
/// Information Technology, Karlsruhe, Germany. * Information Technology, Karlsruhe, Germany.
/// oberlaender@fzi.de * oberlaender@fzi.de
/// Thanks Jan! * Thanks Jan!
*
* This model has a scalar parameter "k".
*
* - Loss \rho(x) = 0.5 k² log(1+x²/k²)
* - Derivative \phi(x) = (k²x)/(x²+k²)
* - Weight w(x) = \phi(x)/x = k²/(x²+k²)
*/
class GTSAM_EXPORT Cauchy : public Base { class GTSAM_EXPORT Cauchy : public Base {
protected: protected:
double k_, ksquared_; double k_, ksquared_;
@ -215,6 +258,7 @@ class GTSAM_EXPORT Cauchy : public Base {
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return k_; }
private: private:
/** Serialization function */ /** Serialization function */
@ -223,10 +267,18 @@ class GTSAM_EXPORT Cauchy : public Base {
void serialize(ARCHIVE &ar, const unsigned int /*version*/) { void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar &BOOST_SERIALIZATION_NVP(k_); ar &BOOST_SERIALIZATION_NVP(k_);
ar &BOOST_SERIALIZATION_NVP(ksquared_);
} }
}; };
/// Tukey implements the "Tukey" robust error model (Zhang97ivc) /** Implementation of the "Tukey" robust error model (Zhang97ivc).
*
* This model has a scalar parameter "c".
*
* - Loss \rho(x) = c² (1 - (1-x²/c²)³)/6 if |x|<c, c²/6 otherwise
* - Derivative \phi(x) = x(1-x²/c²)² if |x|<c, 0 otherwise
* - Weight w(x) = \phi(x)/x = (1-x²/c²)² if |x|<c, 0 otherwise
*/
class GTSAM_EXPORT Tukey : public Base { class GTSAM_EXPORT Tukey : public Base {
protected: protected:
double c_, csquared_; double c_, csquared_;
@ -240,6 +292,7 @@ class GTSAM_EXPORT Tukey : public Base {
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
private: private:
/** Serialization function */ /** Serialization function */
@ -251,7 +304,14 @@ class GTSAM_EXPORT Tukey : public Base {
} }
}; };
/// Welsch implements the "Welsch" robust error model (Zhang97ivc) /** Implementation of the "Welsch" robust error model (Zhang97ivc).
*
* This model has a scalar parameter "c".
*
* - Loss \rho(x) = -0.5 c² (exp(-x²/c²) - 1)
* - Derivative \phi(x) = x exp(-x²/c²)
* - Weight w(x) = \phi(x)/x = exp(-x²/c²)
*/
class GTSAM_EXPORT Welsch : public Base { class GTSAM_EXPORT Welsch : public Base {
protected: protected:
double c_, csquared_; double c_, csquared_;
@ -265,6 +325,7 @@ class GTSAM_EXPORT Welsch : public Base {
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
private: private:
/** Serialization function */ /** Serialization function */
@ -273,15 +334,20 @@ class GTSAM_EXPORT Welsch : public Base {
void serialize(ARCHIVE &ar, const unsigned int /*version*/) { void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar &BOOST_SERIALIZATION_NVP(c_); ar &BOOST_SERIALIZATION_NVP(c_);
ar &BOOST_SERIALIZATION_NVP(csquared_);
} }
}; };
/// GemanMcClure implements the "Geman-McClure" robust error model /** Implementation of the "Geman-McClure" robust error model (Zhang97ivc).
/// (Zhang97ivc). *
/// * Note that Geman-McClure weight function uses the parameter c == 1.0,
/// Note that Geman-McClure weight function uses the parameter c == 1.0, * but here it's allowed to use different values, so we actually have
/// but here it's allowed to use different values, so we actually have * the generalized Geman-McClure from (Agarwal15phd).
/// the generalized Geman-McClure from (Agarwal15phd). *
* - Loss \rho(x) = 0.5 (c²x²)/(c²+x²)
* - Derivative \phi(x) = xc⁴/(c²+x²)²
* - Weight w(x) = \phi(x)/x = c⁴/(c²+x²)²
*/
class GTSAM_EXPORT GemanMcClure : public Base { class GTSAM_EXPORT GemanMcClure : public Base {
public: public:
typedef boost::shared_ptr<GemanMcClure> shared_ptr; typedef boost::shared_ptr<GemanMcClure> shared_ptr;
@ -293,6 +359,7 @@ class GTSAM_EXPORT GemanMcClure : public Base {
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
protected: protected:
double c_; double c_;
@ -307,11 +374,18 @@ class GTSAM_EXPORT GemanMcClure : public Base {
} }
}; };
/// DCS implements the Dynamic Covariance Scaling robust error model /** DCS implements the Dynamic Covariance Scaling robust error model
/// from the paper Robust Map Optimization (Agarwal13icra). * from the paper Robust Map Optimization (Agarwal13icra).
/// *
/// Under the special condition of the parameter c == 1.0 and not * 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. * 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 { class GTSAM_EXPORT DCS : public Base {
public: public:
typedef boost::shared_ptr<DCS> shared_ptr; typedef boost::shared_ptr<DCS> shared_ptr;
@ -323,6 +397,7 @@ class GTSAM_EXPORT DCS : public Base {
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
protected: protected:
double c_; double c_;
@ -337,12 +412,19 @@ class GTSAM_EXPORT DCS : public Base {
} }
}; };
/// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of /** 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 * 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 * zone is always zero, and grows quadratically outside the dead zone. In this
/// sense, the L2WithDeadZone penalty is "robust to inliers", rather than being * sense, the L2WithDeadZone penalty is "robust to inliers", rather than being
/// robust to outliers. This penalty can be used to create barrier functions in * robust to outliers. This penalty can be used to create barrier functions in
/// a general way. * a general way.
*
* This model has a scalar parameter "k".
*
* - Loss \rho(x) = 0 if |x|<k, 0.5(k-|x|)² otherwise
* - Derivative \phi(x) = 0 if |x|<k, (-k+x) if x>k, (k+x) if x<-k
* - Weight w(x) = \phi(x)/x = 0 if |x|<k, (-k+x)/x if x>k, (k+x)/x if x<-k
*/
class GTSAM_EXPORT L2WithDeadZone : public Base { class GTSAM_EXPORT L2WithDeadZone : public Base {
protected: protected:
double k_; double k_;
@ -356,6 +438,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base {
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return k_; }
private: private:
/** Serialization function */ /** Serialization function */

4
gtsam/linear/linear.i
View File

@ -671,6 +671,10 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
DummyPreconditionerParameters(); DummyPreconditionerParameters();
}; };
virtual class BlockJacobiPreconditionerParameters : gtsam::PreconditionerParameters {
BlockJacobiPreconditionerParameters();
};
#include <gtsam/linear/PCGSolver.h> #include <gtsam/linear/PCGSolver.h>
virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
PCGSolverParameters(); PCGSolverParameters();

6
gtsam/navigation/AHRSFactor.h
View File

@ -90,7 +90,11 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
/** /**
* Add a single Gyroscope measurement to the preintegration. * 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 * @param deltaT Time step
*/ */
void integrateMeasurement(const Vector3& measuredOmega, double deltaT); void integrateMeasurement(const Vector3& measuredOmega, double deltaT);

7
gtsam/navigation/CombinedImuFactor.h
View File

@ -208,8 +208,11 @@ public:
/** /**
* Add a single IMU measurement to the preintegration. * Add a single IMU measurement to the preintegration.
* @param measuredAcc Measured acceleration (in body frame, as given by the * Both accelerometer and gyroscope measurements are taken to be in the sensor
* 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 measuredOmega Measured angular velocity (as given by the sensor)
* @param dt Time interval between two consecutive IMU measurements * @param dt Time interval between two consecutive IMU measurements
*/ */

6
gtsam/navigation/ImuFactor.h
View File

@ -121,7 +121,11 @@ public:
/** /**
* Add a single IMU measurement to the preintegration. * 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 measuredOmega Measured angular velocity (as given by the sensor)
* @param dt Time interval between this and the last IMU measurement * @param dt Time interval between this and the last IMU measurement
*/ */

1
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -27,7 +27,6 @@
#include <GeographicLib/Config.h> #include <GeographicLib/Config.h>
#include <GeographicLib/LocalCartesian.hpp> #include <GeographicLib/LocalCartesian.hpp>
using namespace std::placeholders;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace GeographicLib; using namespace GeographicLib;

3
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -26,7 +26,6 @@
#include <GeographicLib/LocalCartesian.hpp> #include <GeographicLib/LocalCartesian.hpp>
using namespace std::placeholders;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace GeographicLib; using namespace GeographicLib;
@ -95,7 +94,7 @@ TEST( MagFactor, Factors ) {
(std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),// (std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),//
H2, 1e-7)); H2, 1e-7));
// MagFactor2 // MagFactor3
MagFactor3 f3(1, 2, 3, measured, nRb, model); 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(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> // EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //

1
gtsam/nonlinear/PriorFactor.h
View File

@ -94,7 +94,6 @@ namespace gtsam {
Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override { Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override {
if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x)); if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
// manifold equivalent of z-x -> Local(x,z) // manifold equivalent of z-x -> Local(x,z)
// TODO(ASL) Add Jacobians.
return -traits<T>::Local(x, prior_); return -traits<T>::Local(x, prior_);
} }

40
gtsam/nonlinear/Values-inl.h
View File

@ -279,10 +279,11 @@ namespace gtsam {
template <typename ValueType> template <typename ValueType>
struct handle { struct handle {
ValueType operator()(Key j, const Value* const pointer) { ValueType operator()(Key j, const Value* const pointer) {
try { auto ptr = dynamic_cast<const GenericValue<ValueType>*>(pointer);
if (ptr) {
// value returns a const ValueType&, and the return makes a copy !!!!! // value returns a const ValueType&, and the return makes a copy !!!!!
return dynamic_cast<const GenericValue<ValueType>&>(*pointer).value(); return ptr->value();
} catch (std::bad_cast&) { } else {
throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType)); throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
} }
} }
@ -294,11 +295,12 @@ namespace gtsam {
// Handle dynamic matrices // Handle dynamic matrices
template <int M, int N> template <int M, int N>
struct handle_matrix<Eigen::Matrix<double, M, N>, true> { struct handle_matrix<Eigen::Matrix<double, M, N>, true> {
Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) { inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
try { auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
if (ptr) {
// value returns a const Matrix&, and the return makes a copy !!!!! // value returns a const Matrix&, and the return makes a copy !!!!!
return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value(); return ptr->value();
} catch (std::bad_cast&) { } else {
// If a fixed matrix was stored, we end up here as well. // If a fixed matrix was stored, we end up here as well.
throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>)); throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>));
} }
@ -308,16 +310,18 @@ namespace gtsam {
// Handle fixed matrices // Handle fixed matrices
template <int M, int N> template <int M, int N>
struct handle_matrix<Eigen::Matrix<double, M, N>, false> { struct handle_matrix<Eigen::Matrix<double, M, N>, false> {
Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) { inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
try { auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
if (ptr) {
// value returns a const MatrixMN&, and the return makes a copy !!!!! // value returns a const MatrixMN&, and the return makes a copy !!!!!
return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value(); return ptr->value();
} catch (std::bad_cast&) { } else {
Matrix A; Matrix A;
try {
// Check if a dynamic matrix was stored // Check if a dynamic matrix was stored
A = handle_matrix<Eigen::MatrixXd, true>()(j, pointer); // will throw if not.... auto ptr = dynamic_cast<const GenericValue<Eigen::MatrixXd>*>(pointer);
} catch (const ValuesIncorrectType&) { if (ptr) {
A = ptr->value();
} else {
// Or a dynamic vector // Or a dynamic vector
A = handle_matrix<Eigen::VectorXd, true>()(j, pointer); // will throw if not.... A = handle_matrix<Eigen::VectorXd, true>()(j, pointer); // will throw if not....
} }
@ -364,10 +368,10 @@ namespace gtsam {
if(item != values_.end()) { if(item != values_.end()) {
// dynamic cast the type and throw exception if incorrect // dynamic cast the type and throw exception if incorrect
const Value& value = *item->second; auto ptr = dynamic_cast<const GenericValue<ValueType>*>(item->second);
try { if (ptr) {
return dynamic_cast<const GenericValue<ValueType>&>(value).value(); return ptr->value();
} catch (std::bad_cast &) { } else {
// NOTE(abe): clang warns about potential side effects if done in typeid // NOTE(abe): clang warns about potential side effects if done in typeid
const Value* value = item->second; const Value* value = item->second;
throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));

20
gtsam/nonlinear/nonlinear.i
View File

@ -226,6 +226,10 @@ class Values {
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera); void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera); void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera); void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); 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, const gtsam::NavState& nav_state);
void insert(size_t j, double c); 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<14>& X);
void insert(size_t j, const gtsam::ParameterMatrix<15>& X); void insert(size_t j, const gtsam::ParameterMatrix<15>& X);
template <T = {gtsam::Point2,
gtsam::Point3}>
void insert(size_t j, const T& val);
void update(size_t j, const gtsam::Point2& point2); 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::Point3& point3);
void update(size_t j, const gtsam::Rot2& rot2); void update(size_t j, const gtsam::Rot2& rot2);
@ -265,6 +273,10 @@ class Values {
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera); void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera); void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera); void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); 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, const gtsam::NavState& nav_state);
void update(size_t j, Vector vector); void update(size_t j, Vector vector);
@ -306,6 +318,10 @@ class Values {
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); 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, const gtsam::NavState& nav_state);
void insert_or_assign(size_t j, Vector vector); void insert_or_assign(size_t j, Vector vector);
@ -347,6 +363,10 @@ class Values {
gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>, gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>, gtsam::PinholeCamera<gtsam::Cal3Unified>,
gtsam::PinholePose<gtsam::Cal3_S2>,
gtsam::PinholePose<gtsam::Cal3Bundler>,
gtsam::PinholePose<gtsam::Cal3Fisheye>,
gtsam::PinholePose<gtsam::Cal3Unified>,
gtsam::imuBias::ConstantBias, gtsam::imuBias::ConstantBias,
gtsam::NavState, gtsam::NavState,
Vector, Vector,

8
gtsam/sfm/SfmData.h
View File

@ -77,10 +77,14 @@ struct GTSAM_EXPORT SfmData {
size_t numberCameras() const { return cameras.size(); } size_t numberCameras() const { return cameras.size(); }
/// The track formed by series of landmark measurements /// 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` /// 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<SfmCamera>& cameraList() const { return cameras; }
const std::vector<SfmTrack>& trackList() const { return tracks; }
/** /**
* @brief Create projection factors using keys i and P(j) * @brief Create projection factors using keys i and P(j)

20
gtsam/sfm/sfm.i
View File

@ -10,6 +10,8 @@ class SfmTrack {
SfmTrack(const gtsam::Point3& pt); SfmTrack(const gtsam::Point3& pt);
const Point3& point3() const; const Point3& point3() const;
Point3 p;
double r; double r;
double g; double g;
double b; double b;
@ -34,12 +36,15 @@ class SfmData {
static gtsam::SfmData FromBundlerFile(string filename); static gtsam::SfmData FromBundlerFile(string filename);
static gtsam::SfmData FromBalFile(string filename); static gtsam::SfmData FromBalFile(string filename);
std::vector<gtsam::SfmTrack>& trackList() const;
std::vector<gtsam::PinholeCamera<gtsam::Cal3Bundler>>& cameraList() const;
void addTrack(const gtsam::SfmTrack& t); void addTrack(const gtsam::SfmTrack& t);
void addCamera(const gtsam::SfmCamera& cam); void addCamera(const gtsam::SfmCamera& cam);
size_t numberTracks() const; size_t numberTracks() const;
size_t numberCameras() const; size_t numberCameras() const;
gtsam::SfmTrack track(size_t idx) const; gtsam::SfmTrack& track(size_t idx) const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const; gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera(size_t idx) const;
gtsam::NonlinearFactorGraph generalSfmFactors( gtsam::NonlinearFactorGraph generalSfmFactors(
const gtsam::SharedNoiseModel& model = const gtsam::SharedNoiseModel& model =
@ -91,6 +96,13 @@ class BinaryMeasurementsUnit3 {
void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement); void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
}; };
class BinaryMeasurementsRot3 {
BinaryMeasurementsRot3();
size_t size() const;
gtsam::BinaryMeasurement<gtsam::Rot3> at(size_t idx) const;
void push_back(const gtsam::BinaryMeasurement<gtsam::Rot3>& measurement);
};
#include <gtsam/sfm/ShonanAveraging.h> #include <gtsam/sfm/ShonanAveraging.h>
// TODO(frank): copy/pasta below until we have integer template paremeters in // TODO(frank): copy/pasta below until we have integer template paremeters in
@ -184,6 +196,10 @@ class ShonanAveraging2 {
}; };
class ShonanAveraging3 { class ShonanAveraging3 {
ShonanAveraging3(
const std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>& measurements,
const gtsam::ShonanAveragingParameters3& parameters =
gtsam::ShonanAveragingParameters3());
ShonanAveraging3(string g2oFile); ShonanAveraging3(string g2oFile);
ShonanAveraging3(string g2oFile, ShonanAveraging3(string g2oFile,
const gtsam::ShonanAveragingParameters3& parameters); const gtsam::ShonanAveragingParameters3& parameters);

3
gtsam/slam/KarcherMeanFactor-inl.h
View File

@ -40,8 +40,7 @@ T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
return result.at<T>(kKey); return result.at<T>(kKey);
} }
template <class T, template <class T>
typename = typename std::enable_if< std::is_same<gtsam::Rot3, T>::value >::type >
T FindKarcherMean(const std::vector<T>& rotations) { T FindKarcherMean(const std::vector<T>& rotations) {
return FindKarcherMeanImpl(rotations); return FindKarcherMeanImpl(rotations);
} }

1
gtsam/slam/dataset.h
View File

@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename,
size_t maxIndex = 0); size_t maxIndex = 0);
using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>; using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
inline boost::optional<IndexedPose> GTSAM_DEPRECATED inline boost::optional<IndexedPose> GTSAM_DEPRECATED

16
gtsam/slam/slam.i
View File

@ -90,6 +90,22 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Unified>,
gtsam::Point3> gtsam::Point3>
GeneralSFMFactorCal3Unified; GeneralSFMFactorCal3Unified;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3_S2>,
gtsam::Point3>
GeneralSFMFactorPoseCal3_S2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3DS2>,
gtsam::Point3>
GeneralSFMFactorPoseCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Bundler>,
gtsam::Point3>
GeneralSFMFactorPoseCal3Bundler;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Fisheye>,
gtsam::Point3>
GeneralSFMFactorPoseCal3Fisheye;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Unified>,
gtsam::Point3>
GeneralSFMFactorPoseCal3Unified;
template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified}> gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {

38
gtsam/slam/tests/testPriorFactor.cpp
View File

@ -5,12 +5,16 @@
* @date Nov 4, 2014 * @date Nov 4, 2014
*/ */
#include <gtsam/base/Vector.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Vector.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/nonlinear/factorTesting.h>
using namespace std; using namespace std;
using namespace std::placeholders;
using namespace gtsam; using namespace gtsam;
using namespace imuBias;
/* ************************************************************************* */ /* ************************************************************************* */
@ -28,11 +32,39 @@ TEST(PriorFactor, ConstructorVector3) {
// Constructor dynamic sized vector // Constructor dynamic sized vector
TEST(PriorFactor, ConstructorDynamicSizeVector) { 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); SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
PriorFactor<Vector> factor(1, v, model); PriorFactor<Vector> factor(1, v, model);
} }
Vector callEvaluateError(const PriorFactor<ConstantBias>& 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<ConstantBias> 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() { int main() {
TestResult tr; TestResult tr;

18
gtsam/symbolic/tests/testSymbolicBayesNet.cpp
View File

@ -104,16 +104,16 @@ TEST(SymbolicBayesNet, Dot) {
"digraph {\n" "digraph {\n"
" size=\"5,5\";\n" " size=\"5,5\";\n"
"\n" "\n"
" vara1[label=\"a1\", pos=\"1,2!\", shape=box];\n" " var6989586621679009793[label=\"a1\", pos=\"1,2!\", shape=box];\n"
" vara2[label=\"a2\", pos=\"2,2!\", shape=box];\n" " var6989586621679009794[label=\"a2\", pos=\"2,2!\", shape=box];\n"
" varx1[label=\"x1\", pos=\"1,1!\"];\n" " var8646911284551352321[label=\"x1\", pos=\"1,1!\"];\n"
" varx2[label=\"x2\", pos=\"2,1!\"];\n" " var8646911284551352322[label=\"x2\", pos=\"2,1!\"];\n"
" varx3[label=\"x3\", pos=\"3,1!\"];\n" " var8646911284551352323[label=\"x3\", pos=\"3,1!\"];\n"
"\n" "\n"
" varx1->varx2\n" " var8646911284551352321->var8646911284551352322\n"
" vara1->varx2\n" " var6989586621679009793->var8646911284551352322\n"
" varx2->varx3\n" " var8646911284551352322->var8646911284551352323\n"
" vara2->varx3\n" " var6989586621679009794->var8646911284551352323\n"
"}"); "}");
} }

18
python/CMakeLists.txt
View File

@ -48,6 +48,7 @@ set(ignore
gtsam::Rot3Vector gtsam::Rot3Vector
gtsam::KeyVector gtsam::KeyVector
gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsUnit3
gtsam::BinaryMeasurementsRot3
gtsam::DiscreteKey gtsam::DiscreteKey
gtsam::KeyPairDoubleMap) gtsam::KeyPairDoubleMap)
@ -98,11 +99,23 @@ set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam)
copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam"
"${GTSAM_MODULE_PATH}") "${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") file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py")
foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY) configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY)
endforeach() 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. # Common directory for data/datasets stored with the package.
# This will store the data in the Python site package directly. # This will store the data in the Python site package directly.
@ -125,6 +138,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
gtsam::KeyVector gtsam::KeyVector
gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsUnit3
gtsam::BinaryMeasurementsRot3
gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3_S2
gtsam::CameraSetCal3Bundler gtsam::CameraSetCal3Bundler
gtsam::CameraSetCal3Unified gtsam::CameraSetCal3Unified
@ -160,7 +174,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
# Hack to get python test files copied every time they are modified # 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") 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) configure_file(${test_file} "${GTSAM_UNSTABLE_MODULE_PATH}/tests/${test_file}" COPYONLY)
endforeach() endforeach()

4
python/gtsam/preamble/geometry.h
View File

@ -23,8 +23,8 @@ PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>); std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>);
PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Pose2Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>); PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
PYBIND11_MAKE_OPAQUE( PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>);

14
python/gtsam/preamble/sfm.h
View File

@ -10,3 +10,17 @@
* Without this they will be automatically converted to a Python object, and all * Without this they will be automatically converted to a Python object, and all
* mutations on Python side will not be reflected on C++. * mutations on Python side will not be reflected on C++.
*/ */
// Including <stl.h> can cause some serious hard-to-debug bugs!!!
// #include <pybind11/stl.h>
#include <pybind11/stl_bind.h>
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::SfmTrack>);
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::SfmCamera>);
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::BinaryMeasurement<gtsam::Unit3>>);
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>);

1
python/gtsam/specializations/geometry.h
View File

@ -16,6 +16,7 @@ py::bind_vector<
m_, "Point2Vector"); m_, "Point2Vector");
py::bind_vector<std::vector<gtsam::Point2Pair>>(m_, "Point2Pairs"); py::bind_vector<std::vector<gtsam::Point2Pair>>(m_, "Point2Pairs");
py::bind_vector<std::vector<gtsam::Point3Pair>>(m_, "Point3Pairs"); py::bind_vector<std::vector<gtsam::Point3Pair>>(m_, "Point3Pairs");
py::bind_vector<std::vector<gtsam::Pose2Pair>>(m_, "Pose2Pairs");
py::bind_vector<std::vector<gtsam::Pose3Pair>>(m_, "Pose3Pairs"); py::bind_vector<std::vector<gtsam::Pose3Pair>>(m_, "Pose3Pairs");
py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector"); py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>( py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>(

15
python/gtsam/specializations/sfm.h
View File

@ -13,4 +13,19 @@
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >( py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
m_, "BinaryMeasurementsUnit3"); m_, "BinaryMeasurementsUnit3");
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >(
m_, "BinaryMeasurementsRot3");
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap"); py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
py::bind_vector<
std::vector<gtsam::SfmTrack> >(
m_, "SfmTracks");
py::bind_vector<
std::vector<gtsam::SfmCamera> >(
m_, "SfmCameras");
py::bind_vector<
std::vector<std::pair<size_t, gtsam::Point2>>>(
m_, "SfmMeasurementVector"
);

11
python/gtsam/tests/test_Cal3Unified.py
View File

@ -139,6 +139,17 @@ class TestCal3Unified(GtsamTestCase):
self.gtsamAssertEquals(z, np.zeros(2)) self.gtsamAssertEquals(z, np.zeros(2))
self.gtsamAssertEquals(H @ H.T, 4*np.eye(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.") @unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
def test_triangulation(self): def test_triangulation(self):
"""Estimate spatial point from image measurements""" """Estimate spatial point from image measurements"""

10
python/gtsam/tests/test_DiscreteBayesNet.py
View File

@ -11,12 +11,12 @@ Author: Frank Dellaert
# pylint: disable=no-name-in-module, invalid-name # pylint: disable=no-name-in-module, invalid-name
import unittest
import textwrap import textwrap
import unittest
import gtsam import gtsam
from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph, from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteDistribution,
DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering) DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering)
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
# Some keys: # Some keys:
@ -152,10 +152,10 @@ class TestDiscreteBayesNet(GtsamTestCase):
var4[label="4"]; var4[label="4"];
var5[label="5"]; var5[label="5"];
var6[label="6"]; var6[label="6"];
vara0[label="a0", pos="0,2!"]; var6989586621679009792[label="a0", pos="0,2!"];
var4->var6 var4->var6
vara0->var3 var6989586621679009792->var3
var3->var5 var3->var5
var6->var5 var6->var5
}""" }"""

46
python/gtsam/tests/test_PinholeCamera.py Normal file
View File

@ -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()

194
python/gtsam/tests/test_Sim2.py Normal file
View File

@ -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()

40
tests/testNonlinearFactorGraph.cpp
View File

@ -335,21 +335,21 @@ TEST(NonlinearFactorGraph, dot) {
"graph {\n" "graph {\n"
" size=\"5,5\";\n" " size=\"5,5\";\n"
"\n" "\n"
" varl1[label=\"l1\"];\n" " var7782220156096217089[label=\"l1\"];\n"
" varx1[label=\"x1\"];\n" " var8646911284551352321[label=\"x1\"];\n"
" varx2[label=\"x2\"];\n" " var8646911284551352322[label=\"x2\"];\n"
"\n" "\n"
" factor0[label=\"\", shape=point];\n" " factor0[label=\"\", shape=point];\n"
" varx1--factor0;\n" " var8646911284551352321--factor0;\n"
" factor1[label=\"\", shape=point];\n" " factor1[label=\"\", shape=point];\n"
" varx1--factor1;\n" " var8646911284551352321--factor1;\n"
" varx2--factor1;\n" " var8646911284551352322--factor1;\n"
" factor2[label=\"\", shape=point];\n" " factor2[label=\"\", shape=point];\n"
" varx1--factor2;\n" " var8646911284551352321--factor2;\n"
" varl1--factor2;\n" " var7782220156096217089--factor2;\n"
" factor3[label=\"\", shape=point];\n" " factor3[label=\"\", shape=point];\n"
" varx2--factor3;\n" " var8646911284551352322--factor3;\n"
" varl1--factor3;\n" " var7782220156096217089--factor3;\n"
"}\n"; "}\n";
const NonlinearFactorGraph fg = createNonlinearFactorGraph(); const NonlinearFactorGraph fg = createNonlinearFactorGraph();
@ -363,21 +363,21 @@ TEST(NonlinearFactorGraph, dot_extra) {
"graph {\n" "graph {\n"
" size=\"5,5\";\n" " size=\"5,5\";\n"
"\n" "\n"
" varl1[label=\"l1\", pos=\"0,0!\"];\n" " var7782220156096217089[label=\"l1\", pos=\"0,0!\"];\n"
" varx1[label=\"x1\", pos=\"1,0!\"];\n" " var8646911284551352321[label=\"x1\", pos=\"1,0!\"];\n"
" varx2[label=\"x2\", pos=\"1,1.5!\"];\n" " var8646911284551352322[label=\"x2\", pos=\"1,1.5!\"];\n"
"\n" "\n"
" factor0[label=\"\", shape=point];\n" " factor0[label=\"\", shape=point];\n"
" varx1--factor0;\n" " var8646911284551352321--factor0;\n"
" factor1[label=\"\", shape=point];\n" " factor1[label=\"\", shape=point];\n"
" varx1--factor1;\n" " var8646911284551352321--factor1;\n"
" varx2--factor1;\n" " var8646911284551352322--factor1;\n"
" factor2[label=\"\", shape=point];\n" " factor2[label=\"\", shape=point];\n"
" varx1--factor2;\n" " var8646911284551352321--factor2;\n"
" varl1--factor2;\n" " var7782220156096217089--factor2;\n"
" factor3[label=\"\", shape=point];\n" " factor3[label=\"\", shape=point];\n"
" varx2--factor3;\n" " var8646911284551352322--factor3;\n"
" varl1--factor3;\n" " var7782220156096217089--factor3;\n"
"}\n"; "}\n";
const NonlinearFactorGraph fg = createNonlinearFactorGraph(); const NonlinearFactorGraph fg = createNonlinearFactorGraph();