Merge branch 'develop' into fix/ellipses
Frank Dellaert
commit
ce2cf723ad
|
|
@ -20,6 +20,8 @@
|
|||
#pragma once
|
||||
#include <gtsam/config.h> // Configuration from CMake
|
||||
#include <Eigen/Dense>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#ifndef OPTIONALJACOBIAN_NOBOOST
|
||||
#include <boost/optional.hpp>
|
||||
|
|
@ -96,6 +98,24 @@ public:
|
|||
usurp(dynamic->data());
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor from an Eigen::Ref *value*. Will not usurp if dimension is wrong
|
||||
* @note This is important so we don't overwrite someone else's memory!
|
||||
*/
|
||||
template<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
|
||||
|
||||
/// Constructor with boost::none just makes empty
|
||||
|
|
|
|||
|
|
@ -160,7 +160,7 @@ namespace gtsam {
|
|||
const typename Base::LabelFormatter& labelFormatter =
|
||||
&DefaultFormatter) const {
|
||||
auto valueFormatter = [](const double& v) {
|
||||
return (boost::format("%4.4g") % v).str();
|
||||
return (boost::format("%4.8g") % v).str();
|
||||
};
|
||||
Base::print(s, labelFormatter, valueFormatter);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -59,7 +59,7 @@ namespace gtsam {
|
|||
/** constant stored in this leaf */
|
||||
Y constant_;
|
||||
|
||||
/** The number of assignments contained within this leaf
|
||||
/** The number of assignments contained within this leaf.
|
||||
* Particularly useful when leaves have been pruned.
|
||||
*/
|
||||
size_t nrAssignments_;
|
||||
|
|
@ -68,7 +68,7 @@ namespace gtsam {
|
|||
Leaf(const Y& constant, size_t nrAssignments = 1)
|
||||
: constant_(constant), nrAssignments_(nrAssignments) {}
|
||||
|
||||
/** return the constant */
|
||||
/// Return the constant
|
||||
const Y& constant() const {
|
||||
return constant_;
|
||||
}
|
||||
|
|
@ -81,19 +81,19 @@ namespace gtsam {
|
|||
return constant_ == q.constant_;
|
||||
}
|
||||
|
||||
/// polymorphic equality: is q is a leaf, could be
|
||||
/// polymorphic equality: is q a leaf and is it the same as this leaf?
|
||||
bool sameLeaf(const Node& q) const override {
|
||||
return (q.isLeaf() && q.sameLeaf(*this));
|
||||
}
|
||||
|
||||
/** equality up to tolerance */
|
||||
/// equality up to tolerance
|
||||
bool equals(const Node& q, const CompareFunc& compare) const override {
|
||||
const Leaf* other = dynamic_cast<const Leaf*>(&q);
|
||||
if (!other) return false;
|
||||
return compare(this->constant_, other->constant_);
|
||||
}
|
||||
|
||||
/** print */
|
||||
/// print
|
||||
void print(const std::string& s, const LabelFormatter& labelFormatter,
|
||||
const ValueFormatter& valueFormatter) const override {
|
||||
std::cout << s << " Leaf " << valueFormatter(constant_) << std::endl;
|
||||
|
|
@ -122,8 +122,8 @@ namespace gtsam {
|
|||
|
||||
/// Apply unary operator with assignment
|
||||
NodePtr apply(const UnaryAssignment& op,
|
||||
const Assignment<L>& choices) const override {
|
||||
NodePtr f(new Leaf(op(choices, constant_), nrAssignments_));
|
||||
const Assignment<L>& assignment) const override {
|
||||
NodePtr f(new Leaf(op(assignment, constant_), nrAssignments_));
|
||||
return f;
|
||||
}
|
||||
|
||||
|
|
@ -168,7 +168,10 @@ namespace gtsam {
|
|||
std::vector<NodePtr> branches_;
|
||||
|
||||
private:
|
||||
/** incremental allSame */
|
||||
/**
|
||||
* Incremental allSame.
|
||||
* Records if all the branches are the same leaf.
|
||||
*/
|
||||
size_t allSame_;
|
||||
|
||||
using ChoicePtr = boost::shared_ptr<const Choice>;
|
||||
|
|
@ -181,9 +184,9 @@ namespace gtsam {
|
|||
#endif
|
||||
}
|
||||
|
||||
/** If all branches of a choice node f are the same, just return a branch */
|
||||
/// If all branches of a choice node f are the same, just return a branch.
|
||||
static NodePtr Unique(const ChoicePtr& f) {
|
||||
#ifndef DT_NO_PRUNING
|
||||
#ifndef GTSAM_DT_NO_PRUNING
|
||||
if (f->allSame_) {
|
||||
assert(f->branches().size() > 0);
|
||||
NodePtr f0 = f->branches_[0];
|
||||
|
|
@ -205,15 +208,13 @@ namespace gtsam {
|
|||
|
||||
bool isLeaf() const override { return false; }
|
||||
|
||||
/** Constructor, given choice label and mandatory expected branch count */
|
||||
/// Constructor, given choice label and mandatory expected branch count.
|
||||
Choice(const L& label, size_t count) :
|
||||
label_(label), allSame_(true) {
|
||||
branches_.reserve(count);
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct from applying binary op to two Choice nodes
|
||||
*/
|
||||
/// Construct from applying binary op to two Choice nodes.
|
||||
Choice(const Choice& f, const Choice& g, const Binary& op) :
|
||||
allSame_(true) {
|
||||
// Choose what to do based on label
|
||||
|
|
@ -241,6 +242,7 @@ namespace gtsam {
|
|||
}
|
||||
}
|
||||
|
||||
/// Return the label of this choice node.
|
||||
const L& label() const {
|
||||
return label_;
|
||||
}
|
||||
|
|
@ -262,7 +264,7 @@ namespace gtsam {
|
|||
branches_.push_back(node);
|
||||
}
|
||||
|
||||
/** print (as a tree) */
|
||||
/// print (as a tree).
|
||||
void print(const std::string& s, const LabelFormatter& labelFormatter,
|
||||
const ValueFormatter& valueFormatter) const override {
|
||||
std::cout << s << " Choice(";
|
||||
|
|
@ -308,7 +310,7 @@ namespace gtsam {
|
|||
return (q.isLeaf() && q.sameLeaf(*this));
|
||||
}
|
||||
|
||||
/** equality */
|
||||
/// equality
|
||||
bool equals(const Node& q, const CompareFunc& compare) const override {
|
||||
const Choice* other = dynamic_cast<const Choice*>(&q);
|
||||
if (!other) return false;
|
||||
|
|
@ -321,7 +323,7 @@ namespace gtsam {
|
|||
return true;
|
||||
}
|
||||
|
||||
/** evaluate */
|
||||
/// evaluate
|
||||
const Y& operator()(const Assignment<L>& x) const override {
|
||||
#ifndef NDEBUG
|
||||
typename Assignment<L>::const_iterator it = x.find(label_);
|
||||
|
|
@ -336,13 +338,13 @@ namespace gtsam {
|
|||
return (*child)(x);
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct from applying unary op to a Choice node
|
||||
*/
|
||||
/// Construct from applying unary op to a Choice node.
|
||||
Choice(const L& label, const Choice& f, const Unary& op) :
|
||||
label_(label), allSame_(true) {
|
||||
branches_.reserve(f.branches_.size()); // reserve space
|
||||
for (const NodePtr& branch : f.branches_) push_back(branch->apply(op));
|
||||
for (const NodePtr& branch : f.branches_) {
|
||||
push_back(branch->apply(op));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -353,28 +355,28 @@ namespace gtsam {
|
|||
* @param f The original choice node to apply the op on.
|
||||
* @param op Function to apply on the choice node. Takes Assignment and
|
||||
* value as arguments.
|
||||
* @param choices The Assignment that will go to op.
|
||||
* @param assignment The Assignment that will go to op.
|
||||
*/
|
||||
Choice(const L& label, const Choice& f, const UnaryAssignment& op,
|
||||
const Assignment<L>& choices)
|
||||
const Assignment<L>& assignment)
|
||||
: label_(label), allSame_(true) {
|
||||
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++) {
|
||||
choices_[label_] = i; // Set assignment for label to i
|
||||
assignment_[label_] = i; // Set assignment for label to i
|
||||
|
||||
const NodePtr branch = f.branches_[i];
|
||||
push_back(branch->apply(op, choices_));
|
||||
push_back(branch->apply(op, assignment_));
|
||||
|
||||
// Remove the choice so we are backtracking
|
||||
auto choice_it = choices_.find(label_);
|
||||
choices_.erase(choice_it);
|
||||
// Remove the assignment so we are backtracking
|
||||
auto assignment_it = assignment_.find(label_);
|
||||
assignment_.erase(assignment_it);
|
||||
}
|
||||
}
|
||||
|
||||
/** apply unary operator */
|
||||
/// apply unary operator.
|
||||
NodePtr apply(const Unary& op) const override {
|
||||
auto r = boost::make_shared<Choice>(label_, *this, op);
|
||||
return Unique(r);
|
||||
|
|
@ -382,8 +384,8 @@ namespace gtsam {
|
|||
|
||||
/// Apply unary operator with assignment
|
||||
NodePtr apply(const UnaryAssignment& op,
|
||||
const Assignment<L>& choices) const override {
|
||||
auto r = boost::make_shared<Choice>(label_, *this, op, choices);
|
||||
const Assignment<L>& assignment) const override {
|
||||
auto r = boost::make_shared<Choice>(label_, *this, op, assignment);
|
||||
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>
|
||||
struct Visit {
|
||||
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>
|
||||
struct VisitWith {
|
||||
using Choices = Assignment<L>;
|
||||
using F = std::function<void(const Choices&, const Y&)>;
|
||||
using F = std::function<void(const Assignment<L>&, const Y&)>;
|
||||
explicit VisitWith(F f) : f(f) {} ///< Construct from folding function.
|
||||
Choices choices; ///< Assignment, mutating through recursion.
|
||||
F f; ///< folding function object.
|
||||
Assignment<L> assignment; ///< Assignment, mutating through recursion.
|
||||
F f; ///< folding function object.
|
||||
|
||||
/// Do a depth-first visit on the tree rooted at node.
|
||||
void operator()(const typename DecisionTree<L, Y>::NodePtr& node) {
|
||||
using Leaf = typename DecisionTree<L, Y>::Leaf;
|
||||
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;
|
||||
auto choice = boost::dynamic_pointer_cast<const Choice>(node);
|
||||
if (!choice)
|
||||
throw std::invalid_argument("DecisionTree::VisitWith: Invalid NodePtr");
|
||||
for (size_t i = 0; i < choice->nrChoices(); i++) {
|
||||
choices[choice->label()] = i; // Set assignment for label to i
|
||||
assignment[choice->label()] = i; // Set assignment for label to i
|
||||
|
||||
(*this)(choice->branches()[i]); // recurse!
|
||||
|
||||
// Remove the choice so we are backtracking
|
||||
auto choice_it = choices.find(choice->label());
|
||||
choices.erase(choice_it);
|
||||
auto choice_it = assignment.find(choice->label());
|
||||
assignment.erase(choice_it);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
|
@ -763,12 +815,26 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/****************************************************************************/
|
||||
// labels is just done with a visit
|
||||
/**
|
||||
* Get (partial) labels by performing a visit.
|
||||
*
|
||||
* This method performs a depth-first search to go to every leaf and records
|
||||
* the keys assignment which leads to that leaf. Since the tree can be pruned,
|
||||
* there might be a leaf at a lower depth which results in a partial
|
||||
* assignment (i.e. not all keys are specified).
|
||||
*
|
||||
* E.g. given a tree with 3 keys, there may be a branch where the 3rd key has
|
||||
* the same values for all the leaves. This leads to the branch being pruned
|
||||
* so we get a leaf which is arrived at by just the first 2 keys and their
|
||||
* assignments.
|
||||
*/
|
||||
template <typename L, typename Y>
|
||||
std::set<L> DecisionTree<L, Y>::labels() const {
|
||||
std::set<L> unique;
|
||||
auto f = [&](const Assignment<L>& choices, const Y&) {
|
||||
for (auto&& kv : choices) unique.insert(kv.first);
|
||||
auto f = [&](const Assignment<L>& assignment, const Y&) {
|
||||
for (auto&& kv : assignment) {
|
||||
unique.insert(kv.first);
|
||||
}
|
||||
};
|
||||
visitWith(f);
|
||||
return unique;
|
||||
|
|
@ -817,8 +883,8 @@ namespace gtsam {
|
|||
throw std::runtime_error(
|
||||
"DecisionTree::apply(unary op) undefined for empty tree.");
|
||||
}
|
||||
Assignment<L> choices;
|
||||
return DecisionTree(root_->apply(op, choices));
|
||||
Assignment<L> assignment;
|
||||
return DecisionTree(root_->apply(op, assignment));
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
|
|
|
|||
|
|
@ -105,7 +105,7 @@ namespace gtsam {
|
|||
virtual const Y& operator()(const Assignment<L>& x) const = 0;
|
||||
virtual Ptr apply(const Unary& op) const = 0;
|
||||
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_g_op_fL(const Leaf&, const Binary&) const = 0;
|
||||
virtual Ptr apply_g_op_fC(const Choice&, const Binary&) const = 0;
|
||||
|
|
@ -153,7 +153,7 @@ namespace gtsam {
|
|||
/** Create a constant */
|
||||
explicit DecisionTree(const Y& y);
|
||||
|
||||
/** Create a new leaf function splitting on a variable */
|
||||
/// Create tree with 2 assignments `y1`, `y2`, splitting on variable `label`
|
||||
DecisionTree(const L& label, const Y& y1, const Y& y2);
|
||||
|
||||
/** Allow Label+Cardinality for convenience */
|
||||
|
|
@ -219,9 +219,8 @@ namespace gtsam {
|
|||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Make virtual */
|
||||
virtual ~DecisionTree() {
|
||||
}
|
||||
/// Make virtual
|
||||
virtual ~DecisionTree() {}
|
||||
|
||||
/// Check if tree is empty.
|
||||
bool empty() const { return !root_; }
|
||||
|
|
@ -234,11 +233,13 @@ namespace gtsam {
|
|||
|
||||
/**
|
||||
* @brief Visit all leaves in depth-first fashion.
|
||||
*
|
||||
* @param f side-effect taking a value.
|
||||
*
|
||||
* @note Due to pruning, leaves might not exhaust choices.
|
||||
*
|
||||
*
|
||||
* @param f (side-effect) Function taking a value.
|
||||
*
|
||||
* @note Due to pruning, the number of leaves may not be the same as the
|
||||
* number of assignments. E.g. if we have a tree on 2 binary variables with
|
||||
* all values being 1, then there are 2^2=4 assignments, but only 1 leaf.
|
||||
*
|
||||
* Example:
|
||||
* int sum = 0;
|
||||
* auto visitor = [&](int y) { sum += y; };
|
||||
|
|
@ -249,14 +250,33 @@ namespace gtsam {
|
|||
|
||||
/**
|
||||
* @brief Visit all leaves in depth-first fashion.
|
||||
*
|
||||
* @param f side-effect taking an assignment and a value.
|
||||
*
|
||||
* @note Due to pruning, leaves might not exhaust choices.
|
||||
*
|
||||
*
|
||||
* @param f (side-effect) Function taking the leaf node pointer.
|
||||
*
|
||||
* @note Due to pruning, the number of leaves may not be the same as the
|
||||
* number of assignments. E.g. if we have a tree on 2 binary variables with
|
||||
* all values being 1, then there are 2^2=4 assignments, but only 1 leaf.
|
||||
*
|
||||
* Example:
|
||||
* int sum = 0;
|
||||
* auto visitor = [&](const Assignment<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);
|
||||
*/
|
||||
template <typename Func>
|
||||
|
|
@ -275,7 +295,7 @@ namespace gtsam {
|
|||
*
|
||||
* @note X is always passed by value.
|
||||
* @note Due to pruning, leaves might not exhaust choices.
|
||||
*
|
||||
*
|
||||
* Example:
|
||||
* auto add = [](const double& y, double x) { return y + x; };
|
||||
* double sum = tree.fold(add, 0.0);
|
||||
|
|
|
|||
|
|
@ -287,12 +287,16 @@ namespace gtsam {
|
|||
cardinalities_(keys.cardinalities()) {}
|
||||
|
||||
/* ************************************************************************ */
|
||||
DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrLeaves) const {
|
||||
const size_t N = maxNrLeaves;
|
||||
DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const {
|
||||
const size_t N = maxNrAssignments;
|
||||
|
||||
// Get the probabilities in the decision tree so we can threshold.
|
||||
std::vector<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
|
||||
if (probabilities.size() <= N) {
|
||||
|
|
|
|||
|
|
@ -174,13 +174,21 @@ namespace gtsam {
|
|||
* @brief Prune the decision tree of discrete variables.
|
||||
*
|
||||
* Pruning will set the leaves to be "pruned" to 0 indicating a 0
|
||||
* probability.
|
||||
* A leaf is pruned if it is not in the top `maxNrLeaves` values.
|
||||
* probability. An assignment is pruned if it is not in the top
|
||||
* `maxNrAssignments` values.
|
||||
*
|
||||
* @param maxNrLeaves The maximum number of leaves to keep.
|
||||
* A violation can occur if there are more
|
||||
* duplicate values than `maxNrAssignments`. A violation here is the need to
|
||||
* un-prune the decision tree (e.g. all assignment values are 1.0). We could
|
||||
* have another case where some subset of duplicates exist (e.g. for a tree
|
||||
* with 8 assignments we have 1, 1, 1, 1, 0.8, 0.7, 0.6, 0.5), but this is
|
||||
* not a violation since the for `maxNrAssignments=5` the top values are (1,
|
||||
* 0.8).
|
||||
*
|
||||
* @param maxNrAssignments The maximum number of assignments to keep.
|
||||
* @return DecisionTreeFactor
|
||||
*/
|
||||
DecisionTreeFactor prune(size_t maxNrLeaves) const;
|
||||
DecisionTreeFactor prune(size_t maxNrAssignments) const;
|
||||
|
||||
/// @}
|
||||
/// @name Wrapper support
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@
|
|||
#include <gtsam/discrete/DiscreteKey.h> // make sure we have traits
|
||||
#include <gtsam/discrete/DiscreteValues.h>
|
||||
// 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/DecisionTree-inl.h> // for convert only
|
||||
#define DISABLE_TIMING
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@
|
|||
*/
|
||||
|
||||
// #define DT_DEBUG_MEMORY
|
||||
// #define DT_NO_PRUNING
|
||||
// #define GTSAM_DT_NO_PRUNING
|
||||
#define DISABLE_DOT
|
||||
#include <gtsam/discrete/DecisionTree-inl.h>
|
||||
|
||||
|
|
|
|||
|
|
@ -113,18 +113,32 @@ TEST(DecisionTreeFactor, Prune) {
|
|||
DecisionTreeFactor f(A & B & C, "1 5 3 7 2 6 4 8");
|
||||
|
||||
// Only keep the leaves with the top 5 values.
|
||||
size_t maxNrLeaves = 5;
|
||||
auto pruned5 = f.prune(maxNrLeaves);
|
||||
size_t maxNrAssignments = 5;
|
||||
auto pruned5 = f.prune(maxNrAssignments);
|
||||
|
||||
// Pruned leaves should be 0
|
||||
DecisionTreeFactor expected(A & B & C, "0 5 0 7 0 6 4 8");
|
||||
EXPECT(assert_equal(expected, pruned5));
|
||||
|
||||
// Check for more extreme pruning where we only keep the top 2 leaves
|
||||
maxNrLeaves = 2;
|
||||
auto pruned2 = f.prune(maxNrLeaves);
|
||||
maxNrAssignments = 2;
|
||||
auto pruned2 = f.prune(maxNrAssignments);
|
||||
DecisionTreeFactor expected2(A & B & C, "0 0 0 7 0 0 0 8");
|
||||
EXPECT(assert_equal(expected2, pruned2));
|
||||
|
||||
DiscreteKey D(4, 2);
|
||||
DecisionTreeFactor factor(
|
||||
D & C & B & A,
|
||||
"0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 "
|
||||
"0.0 0.0 0.99995287 1.0 1.0 1.0 1.0");
|
||||
|
||||
DecisionTreeFactor expected3(
|
||||
D & C & B & A,
|
||||
"0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 "
|
||||
"0.999952870000 1.0 1.0 1.0 1.0");
|
||||
maxNrAssignments = 5;
|
||||
auto pruned3 = factor.prune(maxNrAssignments);
|
||||
EXPECT(assert_equal(expected3, pruned3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -133,7 +147,7 @@ TEST(DecisionTreeFactor, DotWithNames) {
|
|||
DecisionTreeFactor f(A & B, "1 2 3 4 5 6");
|
||||
auto formatter = [](Key key) { return key == 12 ? "A" : "B"; };
|
||||
|
||||
for (bool showZero:{true, false}) {
|
||||
for (bool showZero:{true, false}) {
|
||||
string actual = f.dot(formatter, showZero);
|
||||
// pretty weak test, as ids are pointers and not stable across platforms.
|
||||
string expected = "digraph G {";
|
||||
|
|
@ -215,4 +229,3 @@ int main() {
|
|||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -415,16 +415,16 @@ TEST(DiscreteFactorGraph, DotWithNames) {
|
|||
"graph {\n"
|
||||
" size=\"5,5\";\n"
|
||||
"\n"
|
||||
" varC[label=\"C\"];\n"
|
||||
" varA[label=\"A\"];\n"
|
||||
" varB[label=\"B\"];\n"
|
||||
" var0[label=\"C\"];\n"
|
||||
" var1[label=\"A\"];\n"
|
||||
" var2[label=\"B\"];\n"
|
||||
"\n"
|
||||
" factor0[label=\"\", shape=point];\n"
|
||||
" varC--factor0;\n"
|
||||
" varA--factor0;\n"
|
||||
" var0--factor0;\n"
|
||||
" var1--factor0;\n"
|
||||
" factor1[label=\"\", shape=point];\n"
|
||||
" varC--factor1;\n"
|
||||
" varB--factor1;\n"
|
||||
" var0--factor1;\n"
|
||||
" var2--factor1;\n"
|
||||
"}\n";
|
||||
EXPECT(actual == expected);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -113,6 +113,18 @@ list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2,
|
|||
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) {
|
||||
os << p.first << " <-> " << p.second;
|
||||
|
|
|
|||
|
|
@ -71,6 +71,9 @@ GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double
|
|||
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
|
||||
*/
|
||||
GTSAM_EXPORT std::list<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
|
||||
|
|
|
|||
|
|
@ -365,7 +365,7 @@ boost::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
|
|||
"Pose2:Align expects 2*N matrices of equal shape.");
|
||||
}
|
||||
Point2Pairs ab_pairs;
|
||||
for (size_t j=0; j < a.cols(); j++) {
|
||||
for (Eigen::Index j = 0; j < a.cols(); j++) {
|
||||
ab_pairs.emplace_back(a.col(j), b.col(j));
|
||||
}
|
||||
return Pose2::Align(ab_pairs);
|
||||
|
|
|
|||
|
|
@ -353,6 +353,10 @@ GTSAM_EXPORT boost::optional<Pose2>
|
|||
GTSAM_DEPRECATED align(const Point2Pairs& pairs);
|
||||
#endif
|
||||
|
||||
// Convenience typedef
|
||||
using Pose2Pair = std::pair<Pose2, Pose2>;
|
||||
using Pose2Pairs = std::vector<Pose2Pair>;
|
||||
|
||||
template <>
|
||||
struct traits<Pose2> : public internal::LieGroup<Pose2> {};
|
||||
|
||||
|
|
|
|||
|
|
@ -473,7 +473,7 @@ boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
|
|||
"Pose3:Align expects 3*N matrices of equal shape.");
|
||||
}
|
||||
Point3Pairs abPointPairs;
|
||||
for (size_t j=0; j < a.cols(); j++) {
|
||||
for (Eigen::Index j = 0; j < a.cols(); j++) {
|
||||
abPointPairs.emplace_back(a.col(j), b.col(j));
|
||||
}
|
||||
return Pose3::Align(abPointPairs);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
* @brief 2D rotation
|
||||
* @date Dec 9, 2009
|
||||
* @author Frank Dellaert
|
||||
* @author John Lambert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -209,6 +210,9 @@ namespace gtsam {
|
|||
/** return 2*2 transpose (inverse) rotation matrix */
|
||||
Matrix2 transpose() const;
|
||||
|
||||
/** Find closest valid rotation matrix, given a 2x2 matrix */
|
||||
static Rot2 ClosestTo(const Matrix2& M);
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -26,7 +26,7 @@ namespace gtsam {
|
|||
|
||||
using std::vector;
|
||||
|
||||
namespace {
|
||||
namespace internal {
|
||||
/// Subtract centroids from point pairs.
|
||||
static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
|
||||
const Point3Pair ¢roids) {
|
||||
|
|
@ -81,10 +81,10 @@ static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb,
|
|||
static Similarity3 alignGivenR(const Point3Pairs &abPointPairs,
|
||||
const Rot3 &aRb) {
|
||||
auto centroids = means(abPointPairs);
|
||||
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
|
||||
auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
|
||||
return align(d_abPointPairs, aRb, centroids);
|
||||
}
|
||||
} // namespace
|
||||
} // namespace internal
|
||||
|
||||
Similarity3::Similarity3() :
|
||||
t_(0,0,0), s_(1) {
|
||||
|
|
@ -165,11 +165,11 @@ Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
|
|||
if (abPointPairs.size() < 3)
|
||||
throw std::runtime_error("input should have at least 3 pairs of points");
|
||||
auto centroids = means(abPointPairs);
|
||||
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
|
||||
Matrix3 H = calculateH(d_abPointPairs);
|
||||
auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
|
||||
Matrix3 H = internal::calculateH(d_abPointPairs);
|
||||
// ClosestTo finds rotation matrix closest to H in Frobenius sense
|
||||
Rot3 aRb = Rot3::ClosestTo(H);
|
||||
return align(d_abPointPairs, aRb, centroids);
|
||||
return internal::align(d_abPointPairs, aRb, centroids);
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
|
||||
|
|
@ -192,7 +192,7 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
|
|||
}
|
||||
const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
|
||||
|
||||
return alignGivenR(abPointPairs, aRb_estimate);
|
||||
return internal::alignGivenR(abPointPairs, aRb_estimate);
|
||||
}
|
||||
|
||||
Matrix4 Similarity3::wedge(const Vector7 &xi) {
|
||||
|
|
@ -283,15 +283,11 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
|
|||
return os;
|
||||
}
|
||||
|
||||
const Matrix4 Similarity3::matrix() const {
|
||||
Matrix4 Similarity3::matrix() const {
|
||||
Matrix4 T;
|
||||
T.topRows<3>() << R_.matrix(), t_;
|
||||
T.bottomRows<1>() << 0, 0, 0, 1.0 / s_;
|
||||
return T;
|
||||
}
|
||||
|
||||
Similarity3::operator Pose3() const {
|
||||
return Pose3(R_, s_ * t_);
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -18,13 +18,12 @@
|
|||
|
||||
#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/Manifold.h>
|
||||
#include <gtsam/dllexport.h>
|
||||
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -34,108 +33,106 @@ class Pose3;
|
|||
/**
|
||||
* 3D similarity transform
|
||||
*/
|
||||
class Similarity3: public LieGroup<Similarity3, 7> {
|
||||
|
||||
class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
|
||||
/// @name Pose Concept
|
||||
/// @{
|
||||
typedef Rot3 Rotation;
|
||||
typedef Point3 Translation;
|
||||
/// @}
|
||||
|
||||
private:
|
||||
private:
|
||||
Rot3 R_;
|
||||
Point3 t_;
|
||||
double s_;
|
||||
|
||||
public:
|
||||
|
||||
public:
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor
|
||||
GTSAM_EXPORT Similarity3();
|
||||
Similarity3();
|
||||
|
||||
/// Construct pure scaling
|
||||
GTSAM_EXPORT Similarity3(double s);
|
||||
Similarity3(double s);
|
||||
|
||||
/// Construct from GTSAM types
|
||||
GTSAM_EXPORT Similarity3(const Rot3& R, const Point3& t, double s);
|
||||
Similarity3(const Rot3& R, const Point3& t, double s);
|
||||
|
||||
/// Construct from Eigen types
|
||||
GTSAM_EXPORT Similarity3(const Matrix3& R, const Vector3& t, double s);
|
||||
Similarity3(const Matrix3& R, const Vector3& t, double s);
|
||||
|
||||
/// Construct from matrix [R t; 0 s^-1]
|
||||
GTSAM_EXPORT Similarity3(const Matrix4& T);
|
||||
Similarity3(const Matrix4& T);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// Compare with tolerance
|
||||
GTSAM_EXPORT bool equals(const Similarity3& sim, double tol) const;
|
||||
bool equals(const Similarity3& sim, double tol) const;
|
||||
|
||||
/// Exact equality
|
||||
GTSAM_EXPORT bool operator==(const Similarity3& other) const;
|
||||
bool operator==(const Similarity3& other) const;
|
||||
|
||||
/// Print with optional string
|
||||
GTSAM_EXPORT void print(const std::string& s) const;
|
||||
void print(const std::string& s) const;
|
||||
|
||||
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p);
|
||||
friend std::ostream& operator<<(std::ostream& os, const Similarity3& p);
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// Return an identity transform
|
||||
GTSAM_EXPORT static Similarity3 identity();
|
||||
static Similarity3 identity();
|
||||
|
||||
/// Composition
|
||||
GTSAM_EXPORT Similarity3 operator*(const Similarity3& S) const;
|
||||
Similarity3 operator*(const Similarity3& S) const;
|
||||
|
||||
/// Return the inverse
|
||||
GTSAM_EXPORT Similarity3 inverse() const;
|
||||
Similarity3 inverse() const;
|
||||
|
||||
/// @}
|
||||
/// @name Group action on Point3
|
||||
/// @{
|
||||
|
||||
/// Action on a point p is s*(R*p+t)
|
||||
GTSAM_EXPORT Point3 transformFrom(const Point3& p, //
|
||||
OptionalJacobian<3, 7> H1 = boost::none, //
|
||||
OptionalJacobian<3, 3> H2 = boost::none) const;
|
||||
Point3 transformFrom(const Point3& p, //
|
||||
OptionalJacobian<3, 7> H1 = boost::none, //
|
||||
OptionalJacobian<3, 3> H2 = boost::none) const;
|
||||
|
||||
/**
|
||||
/**
|
||||
* Action on a pose T.
|
||||
* |Rs ts| |R t| |Rs*R Rs*t+ts|
|
||||
* |Rs ts| |R t| |Rs*R Rs*t+ts|
|
||||
* |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim3 object.
|
||||
* To retrieve a Pose3, we normalized the scale value into 1.
|
||||
* |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)|
|
||||
* | 0 1/s | = | 0 1 |
|
||||
*
|
||||
* This group action satisfies the compatibility condition.
|
||||
*
|
||||
* This group action satisfies the compatibility condition.
|
||||
* For more details, refer to: https://en.wikipedia.org/wiki/Group_action
|
||||
*/
|
||||
GTSAM_EXPORT Pose3 transformFrom(const Pose3& T) const;
|
||||
Pose3 transformFrom(const Pose3& T) const;
|
||||
|
||||
/** syntactic sugar for transformFrom */
|
||||
GTSAM_EXPORT Point3 operator*(const Point3& p) const;
|
||||
Point3 operator*(const Point3& p) const;
|
||||
|
||||
/**
|
||||
* Create Similarity3 by aligning at least three point pairs
|
||||
*/
|
||||
GTSAM_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
|
||||
|
||||
static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
|
||||
|
||||
/**
|
||||
* Create the Similarity3 object that aligns at least two pose pairs.
|
||||
* Each pair is of the form (aTi, bTi).
|
||||
* Given a list of pairs in frame a, and a list of pairs in frame b, Align()
|
||||
* will compute the best-fit Similarity3 aSb transformation to align them.
|
||||
* First, the rotation aRb will be computed as the average (Karcher mean) of
|
||||
* many estimates aRb (from each pair). Afterwards, the scale factor will be computed
|
||||
* using the algorithm described here:
|
||||
* many estimates aRb (from each pair). Afterwards, the scale factor will be
|
||||
* computed using the algorithm described here:
|
||||
* http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
|
||||
*/
|
||||
GTSAM_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
|
||||
static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
|
|
@ -144,20 +141,22 @@ public:
|
|||
/** Log map at the identity
|
||||
* \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$
|
||||
*/
|
||||
GTSAM_EXPORT static Vector7 Logmap(const Similarity3& s, //
|
||||
OptionalJacobian<7, 7> Hm = boost::none);
|
||||
static Vector7 Logmap(const Similarity3& s, //
|
||||
OptionalJacobian<7, 7> Hm = boost::none);
|
||||
|
||||
/** Exponential map at the identity
|
||||
*/
|
||||
GTSAM_EXPORT static Similarity3 Expmap(const Vector7& v, //
|
||||
OptionalJacobian<7, 7> Hm = boost::none);
|
||||
static Similarity3 Expmap(const Vector7& v, //
|
||||
OptionalJacobian<7, 7> Hm = boost::none);
|
||||
|
||||
/// Chart at the origin
|
||||
struct ChartAtOrigin {
|
||||
static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) {
|
||||
static Similarity3 Retract(const Vector7& v,
|
||||
ChartJacobian H = boost::none) {
|
||||
return Similarity3::Expmap(v, H);
|
||||
}
|
||||
static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) {
|
||||
static Vector7 Local(const Similarity3& other,
|
||||
ChartJacobian H = boost::none) {
|
||||
return Similarity3::Logmap(other, H);
|
||||
}
|
||||
};
|
||||
|
|
@ -170,67 +169,53 @@ public:
|
|||
* @return 4*4 element of Lie algebra that can be exponentiated
|
||||
* TODO(frank): rename to Hat, make part of traits
|
||||
*/
|
||||
GTSAM_EXPORT static Matrix4 wedge(const Vector7& xi);
|
||||
static Matrix4 wedge(const Vector7& xi);
|
||||
|
||||
/// Project from one tangent space to another
|
||||
GTSAM_EXPORT Matrix7 AdjointMap() const;
|
||||
Matrix7 AdjointMap() const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard interface
|
||||
/// @{
|
||||
|
||||
/// Calculate 4*4 matrix group equivalent
|
||||
GTSAM_EXPORT const Matrix4 matrix() const;
|
||||
Matrix4 matrix() const;
|
||||
|
||||
/// Return a GTSAM rotation
|
||||
const Rot3& rotation() const {
|
||||
return R_;
|
||||
}
|
||||
Rot3 rotation() const { return R_; }
|
||||
|
||||
/// Return a GTSAM translation
|
||||
const Point3& translation() const {
|
||||
return t_;
|
||||
}
|
||||
Point3 translation() const { return t_; }
|
||||
|
||||
/// Return the scale
|
||||
double scale() const {
|
||||
return s_;
|
||||
}
|
||||
|
||||
/// Convert to a rigid body pose (R, s*t)
|
||||
/// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast.
|
||||
GTSAM_EXPORT operator Pose3() const;
|
||||
double scale() const { return s_; }
|
||||
|
||||
/// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
|
||||
inline static size_t Dim() {
|
||||
return 7;
|
||||
}
|
||||
inline static size_t Dim() { return 7; }
|
||||
|
||||
/// Dimensionality of tangent space = 7 DOF
|
||||
inline size_t dim() const {
|
||||
return 7;
|
||||
}
|
||||
inline size_t dim() const { return 7; }
|
||||
|
||||
/// @}
|
||||
/// @name Helper functions
|
||||
/// @{
|
||||
|
||||
private:
|
||||
private:
|
||||
/// Calculate expmap and logmap coefficients.
|
||||
static Matrix3 GetV(Vector3 w, double lambda);
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
||||
template<>
|
||||
template <>
|
||||
inline Matrix wedge<Similarity3>(const Vector& xi) {
|
||||
return Similarity3::wedge(xi);
|
||||
}
|
||||
|
||||
template<>
|
||||
template <>
|
||||
struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
|
||||
|
||||
template<>
|
||||
template <>
|
||||
struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
|
||||
|
||||
} // namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -547,6 +547,12 @@ class EssentialMatrix {
|
|||
// Standard Constructors
|
||||
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
|
||||
|
||||
// Constructors from Pose3
|
||||
gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_);
|
||||
|
||||
gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_,
|
||||
Eigen::Ref<Eigen::MatrixXd> H);
|
||||
|
||||
// Testable
|
||||
void print(string s = "") const;
|
||||
bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
|
||||
|
|
@ -584,7 +590,13 @@ class Cal3_S2 {
|
|||
|
||||
// Action on Point2
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dcal,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dp) 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
|
||||
double fx() const;
|
||||
|
|
@ -623,7 +635,13 @@ virtual class Cal3DS2_Base {
|
|||
|
||||
// Action on Point2
|
||||
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
|
||||
gtsam::Point2 uncalibrate(const gtsam::Point2& p,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dcal,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dp) 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
|
||||
void serialize() const;
|
||||
|
|
@ -680,7 +698,13 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
|
|||
// Note: the signature of this functions differ from the functions
|
||||
// with equal name in the base class.
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dcal,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dp) 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
|
||||
void serialize() const;
|
||||
|
|
@ -706,7 +730,13 @@ class Cal3Fisheye {
|
|||
|
||||
// Action on Point2
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dcal,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dp) 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
|
||||
double fx() const;
|
||||
|
|
@ -769,7 +799,13 @@ class Cal3Bundler {
|
|||
|
||||
// Action on Point2
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dcal,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dp) 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
|
||||
double fx() const;
|
||||
|
|
@ -807,12 +843,25 @@ class CalibratedCamera {
|
|||
|
||||
// Action on Point3
|
||||
gtsam::Point2 project(const gtsam::Point3& point) const;
|
||||
gtsam::Point2 project(const gtsam::Point3& point,
|
||||
Eigen::Ref<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);
|
||||
|
||||
// Standard Interface
|
||||
gtsam::Pose3 pose() 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& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dpose);
|
||||
double range(const gtsam::CalibratedCamera& camera) const;
|
||||
|
||||
// enabling serialization functionality
|
||||
|
|
@ -824,6 +873,7 @@ template <CALIBRATION>
|
|||
class PinholeCamera {
|
||||
// Standard Constructors and Named Constructors
|
||||
PinholeCamera();
|
||||
PinholeCamera(const gtsam::PinholeCamera<CALIBRATION> other);
|
||||
PinholeCamera(const gtsam::Pose3& pose);
|
||||
PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K);
|
||||
static This Level(const CALIBRATION& K, const gtsam::Pose2& pose,
|
||||
|
|
@ -850,14 +900,123 @@ class PinholeCamera {
|
|||
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
|
||||
pair<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);
|
||||
|
||||
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, 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;
|
||||
};
|
||||
|
||||
// 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>
|
||||
class Similarity3 {
|
||||
// Standard Constructors
|
||||
|
|
@ -874,22 +1033,13 @@ class Similarity3 {
|
|||
static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs);
|
||||
|
||||
// Standard Interface
|
||||
const Matrix matrix() const;
|
||||
const gtsam::Rot3& rotation();
|
||||
const gtsam::Point3& translation();
|
||||
bool equals(const gtsam::Similarity3& sim, double tol) const;
|
||||
Matrix matrix() const;
|
||||
gtsam::Rot3& rotation();
|
||||
gtsam::Point3& translation();
|
||||
double scale() const;
|
||||
};
|
||||
|
||||
// Forward declaration of PinholeCameraCalX is defined here.
|
||||
#include <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>
|
||||
class CameraSet {
|
||||
CameraSet();
|
||||
|
|
@ -921,9 +1071,18 @@ class StereoCamera {
|
|||
static size_t Dim();
|
||||
|
||||
// Transformations and measurement functions
|
||||
gtsam::StereoPoint2 project(const gtsam::Point3& point);
|
||||
gtsam::StereoPoint2 project(const gtsam::Point3& point) const;
|
||||
gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const;
|
||||
|
||||
// project with Jacobian
|
||||
gtsam::StereoPoint2 project2(const gtsam::Point3& point,
|
||||
Eigen::Ref<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
|
||||
void serialize() const;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
//******************************************************************************
|
||||
|
|
@ -458,18 +458,18 @@ TEST(Similarity3, Optimization2) {
|
|||
Values result;
|
||||
result = LevenbergMarquardtOptimizer(graph, initial).optimize();
|
||||
//result.print("Optimized Estimate\n");
|
||||
Pose3 p1, p2, p3, p4, p5;
|
||||
p1 = Pose3(result.at<Similarity3>(X(1)));
|
||||
p2 = Pose3(result.at<Similarity3>(X(2)));
|
||||
p3 = Pose3(result.at<Similarity3>(X(3)));
|
||||
p4 = Pose3(result.at<Similarity3>(X(4)));
|
||||
p5 = Pose3(result.at<Similarity3>(X(5)));
|
||||
Similarity3 p1, p2, p3, p4, p5;
|
||||
p1 = result.at<Similarity3>(X(1));
|
||||
p2 = result.at<Similarity3>(X(2));
|
||||
p3 = result.at<Similarity3>(X(3));
|
||||
p4 = result.at<Similarity3>(X(4));
|
||||
p5 = result.at<Similarity3>(X(5));
|
||||
|
||||
//p1.print("Pose1");
|
||||
//p2.print("Pose2");
|
||||
//p3.print("Pose3");
|
||||
//p4.print("Pose4");
|
||||
//p5.print("Pose5");
|
||||
//p1.print("Similarity1");
|
||||
//p2.print("Similarity2");
|
||||
//p3.print("Similarity3");
|
||||
//p4.print("Similarity4");
|
||||
//p5.print("Similarity5");
|
||||
|
||||
Similarity3 expected(0.7);
|
||||
EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4));
|
||||
|
|
|
|||
|
|
@ -53,8 +53,9 @@ void BayesNet<CONDITIONAL>::dot(std::ostream& os,
|
|||
auto frontals = conditional->frontals();
|
||||
const Key me = frontals.front();
|
||||
auto parents = conditional->parents();
|
||||
for (const Key& p : parents)
|
||||
os << " var" << keyFormatter(p) << "->var" << keyFormatter(me) << "\n";
|
||||
for (const Key& p : parents) {
|
||||
os << " var" << p << "->var" << me << "\n";
|
||||
}
|
||||
}
|
||||
|
||||
os << "}";
|
||||
|
|
|
|||
|
|
@ -43,7 +43,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter,
|
|||
const boost::optional<Vector2>& position,
|
||||
ostream* os) const {
|
||||
// Label the node with the label from the KeyFormatter
|
||||
*os << " var" << keyFormatter(key) << "[label=\"" << keyFormatter(key)
|
||||
*os << " var" << key << "[label=\"" << keyFormatter(key)
|
||||
<< "\"";
|
||||
if (position) {
|
||||
*os << ", pos=\"" << position->x() << "," << position->y() << "!\"";
|
||||
|
|
@ -65,13 +65,13 @@ void DotWriter::DrawFactor(size_t i, const boost::optional<Vector2>& position,
|
|||
|
||||
static void ConnectVariables(Key key1, Key key2,
|
||||
const KeyFormatter& keyFormatter, ostream* os) {
|
||||
*os << " var" << keyFormatter(key1) << "--"
|
||||
<< "var" << keyFormatter(key2) << ";\n";
|
||||
*os << " var" << key1 << "--"
|
||||
<< "var" << key2 << ";\n";
|
||||
}
|
||||
|
||||
static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter,
|
||||
size_t i, ostream* os) {
|
||||
*os << " var" << keyFormatter(key) << "--"
|
||||
*os << " var" << key << "--"
|
||||
<< "factor" << i << ";\n";
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -54,23 +54,31 @@ namespace noiseModel {
|
|||
// clang-format on
|
||||
namespace mEstimator {
|
||||
|
||||
//---------------------------------------------------------------------------------------
|
||||
|
||||
/**
|
||||
* Pure virtual class for all robust error function classes.
|
||||
*
|
||||
* It provides the machinery for block vs scalar reweighting strategies, in
|
||||
* addition to defining the interface of derived classes.
|
||||
*/
|
||||
class GTSAM_EXPORT Base {
|
||||
public:
|
||||
/** the rows can be weighted independently according to the error
|
||||
* or uniformly with the norm of the right hand side */
|
||||
enum ReweightScheme { Scalar, Block };
|
||||
typedef boost::shared_ptr<Base> shared_ptr;
|
||||
|
||||
protected:
|
||||
/** the rows can be weighted independently according to the error
|
||||
* or uniformly with the norm of the right hand side */
|
||||
/// Strategy for reweighting \sa ReweightScheme
|
||||
ReweightScheme reweight_;
|
||||
|
||||
public:
|
||||
Base(const ReweightScheme reweight = Block) : reweight_(reweight) {}
|
||||
virtual ~Base() {}
|
||||
|
||||
/*
|
||||
/// Returns the reweight scheme, as explained in ReweightScheme
|
||||
ReweightScheme reweightScheme() const { return reweight_; }
|
||||
|
||||
/**
|
||||
* This method is responsible for returning the total penalty for a given
|
||||
* amount of error. For example, this method is responsible for implementing
|
||||
* the quadratic function for an L2 penalty, the absolute value function for
|
||||
|
|
@ -80,16 +88,20 @@ class GTSAM_EXPORT Base {
|
|||
* error vector, then it prevents implementations of asymmeric loss
|
||||
* functions. It would be better for this function to accept the vector and
|
||||
* internally call the norm if necessary.
|
||||
*
|
||||
* This returns \rho(x) in \ref mEstimator
|
||||
*/
|
||||
virtual double loss(double distance) const { return 0; };
|
||||
virtual double loss(double distance) const { return 0; }
|
||||
|
||||
/*
|
||||
/**
|
||||
* This method is responsible for returning the weight function for a given
|
||||
* amount of error. The weight function is related to the analytic derivative
|
||||
* of the loss function. See
|
||||
* https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
|
||||
* for details. This method is required when optimizing cost functions with
|
||||
* robust penalties using iteratively re-weighted least squares.
|
||||
*
|
||||
* This returns w(x) in \ref mEstimator
|
||||
*/
|
||||
virtual double weight(double distance) const = 0;
|
||||
|
||||
|
|
@ -124,7 +136,15 @@ class GTSAM_EXPORT Base {
|
|||
}
|
||||
};
|
||||
|
||||
/// Null class should behave as Gaussian
|
||||
/** "Null" robust loss function, equivalent to a Gaussian pdf noise model, or
|
||||
* plain least-squares (non-robust).
|
||||
*
|
||||
* This model has no additional parameters.
|
||||
*
|
||||
* - Loss \rho(x) = 0.5 x²
|
||||
* - Derivative \phi(x) = x
|
||||
* - Weight w(x) = \phi(x)/x = 1
|
||||
*/
|
||||
class GTSAM_EXPORT Null : public Base {
|
||||
public:
|
||||
typedef boost::shared_ptr<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 {
|
||||
protected:
|
||||
double c_;
|
||||
|
|
@ -160,6 +187,7 @@ class GTSAM_EXPORT Fair : public Base {
|
|||
void print(const std::string &s) const override;
|
||||
bool equals(const Base &expected, double tol = 1e-8) const override;
|
||||
static shared_ptr Create(double c, const ReweightScheme reweight = Block);
|
||||
double modelParameter() const { return c_; }
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
@ -171,7 +199,14 @@ class GTSAM_EXPORT Fair : public Base {
|
|||
}
|
||||
};
|
||||
|
||||
/// Huber implements the "Huber" robust error model (Zhang97ivc)
|
||||
/** The "Huber" robust error model (Zhang97ivc).
|
||||
*
|
||||
* This model has a scalar parameter "k".
|
||||
*
|
||||
* - Loss \rho(x) = 0.5 x² if |x|<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 {
|
||||
protected:
|
||||
double k_;
|
||||
|
|
@ -185,6 +220,7 @@ class GTSAM_EXPORT Huber : public Base {
|
|||
void print(const std::string &s) const override;
|
||||
bool equals(const Base &expected, double tol = 1e-8) const override;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
|
||||
double modelParameter() const { return k_; }
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
@ -196,12 +232,19 @@ class GTSAM_EXPORT Huber : public Base {
|
|||
}
|
||||
};
|
||||
|
||||
/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed
|
||||
/// by:
|
||||
/// Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
|
||||
/// Information Technology, Karlsruhe, Germany.
|
||||
/// oberlaender@fzi.de
|
||||
/// Thanks Jan!
|
||||
/** Implementation of the "Cauchy" robust error model (Lee2013IROS).
|
||||
* Contributed by:
|
||||
* Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
|
||||
* Information Technology, Karlsruhe, Germany.
|
||||
* oberlaender@fzi.de
|
||||
* 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 {
|
||||
protected:
|
||||
double k_, ksquared_;
|
||||
|
|
@ -215,6 +258,7 @@ class GTSAM_EXPORT Cauchy : public Base {
|
|||
void print(const std::string &s) const override;
|
||||
bool equals(const Base &expected, double tol = 1e-8) const override;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
|
||||
double modelParameter() const { return k_; }
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
@ -223,10 +267,18 @@ class GTSAM_EXPORT Cauchy : public Base {
|
|||
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
|
||||
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
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 {
|
||||
protected:
|
||||
double c_, csquared_;
|
||||
|
|
@ -240,6 +292,7 @@ class GTSAM_EXPORT Tukey : public Base {
|
|||
void print(const std::string &s) const override;
|
||||
bool equals(const Base &expected, double tol = 1e-8) const override;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
|
||||
double modelParameter() const { return c_; }
|
||||
|
||||
private:
|
||||
/** 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 {
|
||||
protected:
|
||||
double c_, csquared_;
|
||||
|
|
@ -265,6 +325,7 @@ class GTSAM_EXPORT Welsch : public Base {
|
|||
void print(const std::string &s) const override;
|
||||
bool equals(const Base &expected, double tol = 1e-8) const override;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
|
||||
double modelParameter() const { return c_; }
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
@ -273,15 +334,20 @@ class GTSAM_EXPORT Welsch : public Base {
|
|||
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
|
||||
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
ar &BOOST_SERIALIZATION_NVP(c_);
|
||||
ar &BOOST_SERIALIZATION_NVP(csquared_);
|
||||
}
|
||||
};
|
||||
|
||||
/// GemanMcClure implements the "Geman-McClure" robust error model
|
||||
/// (Zhang97ivc).
|
||||
///
|
||||
/// 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
|
||||
/// the generalized Geman-McClure from (Agarwal15phd).
|
||||
/** Implementation of the "Geman-McClure" robust error model (Zhang97ivc).
|
||||
*
|
||||
* 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
|
||||
* 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 {
|
||||
public:
|
||||
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;
|
||||
bool equals(const Base &expected, double tol = 1e-8) const override;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
|
||||
double modelParameter() const { return c_; }
|
||||
|
||||
protected:
|
||||
double c_;
|
||||
|
|
@ -307,11 +374,18 @@ class GTSAM_EXPORT GemanMcClure : public Base {
|
|||
}
|
||||
};
|
||||
|
||||
/// DCS implements the Dynamic Covariance Scaling robust error model
|
||||
/// from the paper Robust Map Optimization (Agarwal13icra).
|
||||
///
|
||||
/// Under the special condition of the parameter c == 1.0 and not
|
||||
/// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure.
|
||||
/** DCS implements the Dynamic Covariance Scaling robust error model
|
||||
* from the paper Robust Map Optimization (Agarwal13icra).
|
||||
*
|
||||
* Under the special condition of the parameter c == 1.0 and not
|
||||
* forcing the output weight s <= 1.0, DCS is similar to Geman-McClure.
|
||||
*
|
||||
* This model has a scalar parameter "c" (with "units" of squared error).
|
||||
*
|
||||
* - Loss \rho(x) = (c²x² + cx⁴)/(x²+c)² (for any "x")
|
||||
* - Derivative \phi(x) = 2c²x/(x²+c)²
|
||||
* - Weight w(x) = \phi(x)/x = 2c²/(x²+c)² if x²>c, 1 otherwise
|
||||
*/
|
||||
class GTSAM_EXPORT DCS : public Base {
|
||||
public:
|
||||
typedef boost::shared_ptr<DCS> shared_ptr;
|
||||
|
|
@ -323,6 +397,7 @@ class GTSAM_EXPORT DCS : public Base {
|
|||
void print(const std::string &s) const override;
|
||||
bool equals(const Base &expected, double tol = 1e-8) const override;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
|
||||
double modelParameter() const { return c_; }
|
||||
|
||||
protected:
|
||||
double c_;
|
||||
|
|
@ -337,12 +412,19 @@ class GTSAM_EXPORT DCS : public Base {
|
|||
}
|
||||
};
|
||||
|
||||
/// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of
|
||||
/// width 2*k, centered at the origin. The resulting penalty within the dead
|
||||
/// zone is always zero, and grows quadratically outside the dead zone. In this
|
||||
/// sense, the L2WithDeadZone penalty is "robust to inliers", rather than being
|
||||
/// robust to outliers. This penalty can be used to create barrier functions in
|
||||
/// a general way.
|
||||
/** L2WithDeadZone implements a standard L2 penalty, but with a dead zone of
|
||||
* width 2*k, centered at the origin. The resulting penalty within the dead
|
||||
* zone is always zero, and grows quadratically outside the dead zone. In this
|
||||
* sense, the L2WithDeadZone penalty is "robust to inliers", rather than being
|
||||
* robust to outliers. This penalty can be used to create barrier functions in
|
||||
* a general way.
|
||||
*
|
||||
* This model has a scalar parameter "k".
|
||||
*
|
||||
* - Loss \rho(x) = 0 if |x|<k, 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 {
|
||||
protected:
|
||||
double k_;
|
||||
|
|
@ -356,6 +438,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base {
|
|||
void print(const std::string &s) const override;
|
||||
bool equals(const Base &expected, double tol = 1e-8) const override;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
|
||||
double modelParameter() const { return k_; }
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
|
|||
|
|
@ -671,6 +671,10 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
|
|||
DummyPreconditionerParameters();
|
||||
};
|
||||
|
||||
virtual class BlockJacobiPreconditionerParameters : gtsam::PreconditionerParameters {
|
||||
BlockJacobiPreconditionerParameters();
|
||||
};
|
||||
|
||||
#include <gtsam/linear/PCGSolver.h>
|
||||
virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
|
||||
PCGSolverParameters();
|
||||
|
|
|
|||
|
|
@ -90,7 +90,11 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
|
|||
|
||||
/**
|
||||
* Add a single Gyroscope measurement to the preintegration.
|
||||
* @param measureOmedga Measured angular velocity (in body frame)
|
||||
* Measurements are taken to be in the sensor
|
||||
* frame and conversion to the body frame is handled by `body_P_sensor` in
|
||||
* `PreintegratedRotationParams` (if provided).
|
||||
*
|
||||
* @param measuredOmega Measured angular velocity (as given by the sensor)
|
||||
* @param deltaT Time step
|
||||
*/
|
||||
void integrateMeasurement(const Vector3& measuredOmega, double deltaT);
|
||||
|
|
|
|||
|
|
@ -208,8 +208,11 @@ public:
|
|||
|
||||
/**
|
||||
* Add a single IMU measurement to the preintegration.
|
||||
* @param measuredAcc Measured acceleration (in body frame, as given by the
|
||||
* sensor)
|
||||
* Both accelerometer and gyroscope measurements are taken to be in the sensor
|
||||
* frame and conversion to the body frame is handled by `body_P_sensor` in
|
||||
* `PreintegrationParams`.
|
||||
*
|
||||
* @param measuredAcc Measured acceleration (as given by the sensor)
|
||||
* @param measuredOmega Measured angular velocity (as given by the sensor)
|
||||
* @param dt Time interval between two consecutive IMU measurements
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -121,7 +121,11 @@ public:
|
|||
|
||||
/**
|
||||
* Add a single IMU measurement to the preintegration.
|
||||
* @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
|
||||
* Both accelerometer and gyroscope measurements are taken to be in the sensor
|
||||
* frame and conversion to the body frame is handled by `body_P_sensor` in
|
||||
* `PreintegrationParams`.
|
||||
*
|
||||
* @param measuredAcc Measured acceleration (as given by the sensor)
|
||||
* @param measuredOmega Measured angular velocity (as given by the sensor)
|
||||
* @param dt Time interval between this and the last IMU measurement
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -27,7 +27,6 @@
|
|||
#include <GeographicLib/Config.h>
|
||||
#include <GeographicLib/LocalCartesian.hpp>
|
||||
|
||||
using namespace std::placeholders;
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace GeographicLib;
|
||||
|
|
@ -71,7 +70,7 @@ TEST( GPSFactor, Constructor ) {
|
|||
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
|
||||
|
||||
// Calculate numerical derivatives
|
||||
Matrix expectedH = numericalDerivative11<Vector,Pose3>(
|
||||
Matrix expectedH = numericalDerivative11<Vector, Pose3>(
|
||||
std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T);
|
||||
|
||||
// Use the factor to calculate the derivative
|
||||
|
|
@ -100,7 +99,7 @@ TEST( GPSFactor2, Constructor ) {
|
|||
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
|
||||
|
||||
// Calculate numerical derivatives
|
||||
Matrix expectedH = numericalDerivative11<Vector,NavState>(
|
||||
Matrix expectedH = numericalDerivative11<Vector, NavState>(
|
||||
std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T);
|
||||
|
||||
// Use the factor to calculate the derivative
|
||||
|
|
|
|||
|
|
@ -26,7 +26,6 @@
|
|||
|
||||
#include <GeographicLib/LocalCartesian.hpp>
|
||||
|
||||
using namespace std::placeholders;
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace GeographicLib;
|
||||
|
|
@ -64,7 +63,7 @@ TEST( MagFactor, unrotate ) {
|
|||
Matrix H;
|
||||
Point3 expected(22735.5, 314.502, 44202.5);
|
||||
EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
|
||||
EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> //
|
||||
EXPECT(assert_equal(numericalDerivative11<Point3, Rot2> //
|
||||
(std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6));
|
||||
}
|
||||
|
||||
|
|
@ -75,27 +74,27 @@ TEST( MagFactor, Factors ) {
|
|||
|
||||
// MagFactor
|
||||
MagFactor f(1, measured, s, dir, bias, model);
|
||||
EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
|
||||
EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
|
||||
EXPECT(assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
|
||||
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector, Rot2> //
|
||||
(std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7));
|
||||
|
||||
// MagFactor1
|
||||
// MagFactor1
|
||||
MagFactor1 f1(1, measured, s, dir, bias, model);
|
||||
EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
|
||||
EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
|
||||
EXPECT(assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
|
||||
EXPECT(assert_equal(numericalDerivative11<Vector, Rot3> //
|
||||
(std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7));
|
||||
|
||||
// MagFactor2
|
||||
// MagFactor2
|
||||
MagFactor2 f2(1, 2, measured, nRb, model);
|
||||
EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
|
||||
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
|
||||
EXPECT(assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
|
||||
EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
|
||||
(std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),//
|
||||
H1, 1e-7));
|
||||
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
|
||||
EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
|
||||
(std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),//
|
||||
H2, 1e-7));
|
||||
|
||||
// MagFactor2
|
||||
// MagFactor3
|
||||
MagFactor3 f3(1, 2, 3, measured, nRb, model);
|
||||
EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
|
||||
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
|
||||
|
|
|
|||
|
|
@ -121,7 +121,7 @@ public:
|
|||
|
||||
/** Optimize the bayes tree */
|
||||
VectorValues optimize() const;
|
||||
|
||||
|
||||
protected:
|
||||
|
||||
/** Compute the Bayes Tree as a helper function to the constructor */
|
||||
|
|
|
|||
|
|
@ -94,7 +94,6 @@ namespace gtsam {
|
|||
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));
|
||||
// manifold equivalent of z-x -> Local(x,z)
|
||||
// TODO(ASL) Add Jacobians.
|
||||
return -traits<T>::Local(x, prior_);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -279,10 +279,11 @@ namespace gtsam {
|
|||
template <typename ValueType>
|
||||
struct handle {
|
||||
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 !!!!!
|
||||
return dynamic_cast<const GenericValue<ValueType>&>(*pointer).value();
|
||||
} catch (std::bad_cast&) {
|
||||
return ptr->value();
|
||||
} else {
|
||||
throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
|
||||
}
|
||||
}
|
||||
|
|
@ -294,11 +295,12 @@ namespace gtsam {
|
|||
// Handle dynamic matrices
|
||||
template <int M, int N>
|
||||
struct handle_matrix<Eigen::Matrix<double, M, N>, true> {
|
||||
Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
|
||||
try {
|
||||
inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
|
||||
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 !!!!!
|
||||
return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value();
|
||||
} catch (std::bad_cast&) {
|
||||
return ptr->value();
|
||||
} else {
|
||||
// If a fixed matrix was stored, we end up here as well.
|
||||
throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>));
|
||||
}
|
||||
|
|
@ -308,16 +310,18 @@ namespace gtsam {
|
|||
// Handle fixed matrices
|
||||
template <int M, int N>
|
||||
struct handle_matrix<Eigen::Matrix<double, M, N>, false> {
|
||||
Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
|
||||
try {
|
||||
inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
|
||||
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 !!!!!
|
||||
return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value();
|
||||
} catch (std::bad_cast&) {
|
||||
return ptr->value();
|
||||
} else {
|
||||
Matrix A;
|
||||
try {
|
||||
// Check if a dynamic matrix was stored
|
||||
A = handle_matrix<Eigen::MatrixXd, true>()(j, pointer); // will throw if not....
|
||||
} catch (const ValuesIncorrectType&) {
|
||||
// Check if a dynamic matrix was stored
|
||||
auto ptr = dynamic_cast<const GenericValue<Eigen::MatrixXd>*>(pointer);
|
||||
if (ptr) {
|
||||
A = ptr->value();
|
||||
} else {
|
||||
// Or a dynamic vector
|
||||
A = handle_matrix<Eigen::VectorXd, true>()(j, pointer); // will throw if not....
|
||||
}
|
||||
|
|
@ -364,10 +368,10 @@ namespace gtsam {
|
|||
|
||||
if(item != values_.end()) {
|
||||
// dynamic cast the type and throw exception if incorrect
|
||||
const Value& value = *item->second;
|
||||
try {
|
||||
return dynamic_cast<const GenericValue<ValueType>&>(value).value();
|
||||
} catch (std::bad_cast &) {
|
||||
auto ptr = dynamic_cast<const GenericValue<ValueType>*>(item->second);
|
||||
if (ptr) {
|
||||
return ptr->value();
|
||||
} else {
|
||||
// NOTE(abe): clang warns about potential side effects if done in typeid
|
||||
const Value* value = item->second;
|
||||
throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
|
||||
|
|
|
|||
|
|
@ -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::Cal3Fisheye>& 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::NavState& nav_state);
|
||||
void insert(size_t j, double c);
|
||||
|
|
@ -245,6 +249,10 @@ class Values {
|
|||
void insert(size_t j, const gtsam::ParameterMatrix<14>& X);
|
||||
void insert(size_t j, const gtsam::ParameterMatrix<15>& X);
|
||||
|
||||
template <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::Point3& point3);
|
||||
void update(size_t j, const gtsam::Rot2& rot2);
|
||||
|
|
@ -265,6 +273,10 @@ class Values {
|
|||
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& 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::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::NavState& nav_state);
|
||||
void update(size_t j, Vector vector);
|
||||
|
|
@ -306,6 +318,10 @@ class Values {
|
|||
void insert_or_assign(size_t j, const gtsam::PinholeCamera<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::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::NavState& nav_state);
|
||||
void insert_or_assign(size_t j, Vector vector);
|
||||
|
|
@ -347,6 +363,10 @@ class Values {
|
|||
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
|
||||
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
|
||||
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::NavState,
|
||||
Vector,
|
||||
|
|
|
|||
|
|
@ -77,10 +77,14 @@ struct GTSAM_EXPORT SfmData {
|
|||
size_t numberCameras() const { return cameras.size(); }
|
||||
|
||||
/// The track formed by series of landmark measurements
|
||||
SfmTrack track(size_t idx) const { return tracks[idx]; }
|
||||
const SfmTrack& track(size_t idx) const { return tracks[idx]; }
|
||||
|
||||
/// The camera pose at frame index `idx`
|
||||
SfmCamera camera(size_t idx) const { return cameras[idx]; }
|
||||
const SfmCamera& camera(size_t idx) const { return cameras[idx]; }
|
||||
|
||||
/// Getters
|
||||
const std::vector<SfmCamera>& cameraList() const { return cameras; }
|
||||
const std::vector<SfmTrack>& trackList() const { return tracks; }
|
||||
|
||||
/**
|
||||
* @brief Create projection factors using keys i and P(j)
|
||||
|
|
|
|||
|
|
@ -9,6 +9,8 @@ class SfmTrack {
|
|||
SfmTrack();
|
||||
SfmTrack(const gtsam::Point3& pt);
|
||||
const Point3& point3() const;
|
||||
|
||||
Point3 p;
|
||||
|
||||
double r;
|
||||
double g;
|
||||
|
|
@ -34,12 +36,15 @@ class SfmData {
|
|||
static gtsam::SfmData FromBundlerFile(string filename);
|
||||
static gtsam::SfmData FromBalFile(string filename);
|
||||
|
||||
std::vector<gtsam::SfmTrack>& trackList() const;
|
||||
std::vector<gtsam::PinholeCamera<gtsam::Cal3Bundler>>& cameraList() const;
|
||||
|
||||
void addTrack(const gtsam::SfmTrack& t);
|
||||
void addCamera(const gtsam::SfmCamera& cam);
|
||||
size_t numberTracks() const;
|
||||
size_t numberCameras() const;
|
||||
gtsam::SfmTrack track(size_t idx) const;
|
||||
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
|
||||
gtsam::SfmTrack& track(size_t idx) const;
|
||||
gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera(size_t idx) const;
|
||||
|
||||
gtsam::NonlinearFactorGraph generalSfmFactors(
|
||||
const gtsam::SharedNoiseModel& model =
|
||||
|
|
@ -91,6 +96,13 @@ class BinaryMeasurementsUnit3 {
|
|||
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>
|
||||
|
||||
// TODO(frank): copy/pasta below until we have integer template paremeters in
|
||||
|
|
@ -184,6 +196,10 @@ class ShonanAveraging2 {
|
|||
};
|
||||
|
||||
class ShonanAveraging3 {
|
||||
ShonanAveraging3(
|
||||
const std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>& measurements,
|
||||
const gtsam::ShonanAveragingParameters3& parameters =
|
||||
gtsam::ShonanAveragingParameters3());
|
||||
ShonanAveraging3(string g2oFile);
|
||||
ShonanAveraging3(string g2oFile,
|
||||
const gtsam::ShonanAveragingParameters3& parameters);
|
||||
|
|
|
|||
|
|
@ -40,8 +40,7 @@ T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
|
|||
return result.at<T>(kKey);
|
||||
}
|
||||
|
||||
template <class T,
|
||||
typename = typename std::enable_if< std::is_same<gtsam::Rot3, T>::value >::type >
|
||||
template <class T>
|
||||
T FindKarcherMean(const std::vector<T>& rotations) {
|
||||
return FindKarcherMeanImpl(rotations);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename,
|
|||
size_t maxIndex = 0);
|
||||
|
||||
using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
|
||||
using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
|
||||
inline boost::optional<IndexedPose> GTSAM_DEPRECATED
|
||||
|
|
|
|||
|
|
@ -90,6 +90,22 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Unified>,
|
|||
gtsam::Point3>
|
||||
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,
|
||||
gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
|
||||
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
|
||||
|
|
|
|||
|
|
@ -5,12 +5,16 @@
|
|||
* @date Nov 4, 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/nonlinear/PriorFactor.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::placeholders;
|
||||
using namespace gtsam;
|
||||
using namespace imuBias;
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -23,16 +27,44 @@ TEST(PriorFactor, ConstructorScalar) {
|
|||
// Constructor vector3
|
||||
TEST(PriorFactor, ConstructorVector3) {
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
|
||||
PriorFactor<Vector3> factor(1, Vector3(1,2,3), model);
|
||||
PriorFactor<Vector3> factor(1, Vector3(1, 2, 3), model);
|
||||
}
|
||||
|
||||
// Constructor dynamic sized vector
|
||||
TEST(PriorFactor, ConstructorDynamicSizeVector) {
|
||||
Vector v(5); v << 1, 2, 3, 4, 5;
|
||||
Vector v(5);
|
||||
v << 1, 2, 3, 4, 5;
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
|
||||
PriorFactor<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() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -104,16 +104,16 @@ TEST(SymbolicBayesNet, Dot) {
|
|||
"digraph {\n"
|
||||
" size=\"5,5\";\n"
|
||||
"\n"
|
||||
" vara1[label=\"a1\", pos=\"1,2!\", shape=box];\n"
|
||||
" vara2[label=\"a2\", pos=\"2,2!\", shape=box];\n"
|
||||
" varx1[label=\"x1\", pos=\"1,1!\"];\n"
|
||||
" varx2[label=\"x2\", pos=\"2,1!\"];\n"
|
||||
" varx3[label=\"x3\", pos=\"3,1!\"];\n"
|
||||
" var6989586621679009793[label=\"a1\", pos=\"1,2!\", shape=box];\n"
|
||||
" var6989586621679009794[label=\"a2\", pos=\"2,2!\", shape=box];\n"
|
||||
" var8646911284551352321[label=\"x1\", pos=\"1,1!\"];\n"
|
||||
" var8646911284551352322[label=\"x2\", pos=\"2,1!\"];\n"
|
||||
" var8646911284551352323[label=\"x3\", pos=\"3,1!\"];\n"
|
||||
"\n"
|
||||
" varx1->varx2\n"
|
||||
" vara1->varx2\n"
|
||||
" varx2->varx3\n"
|
||||
" vara2->varx3\n"
|
||||
" var8646911284551352321->var8646911284551352322\n"
|
||||
" var6989586621679009793->var8646911284551352322\n"
|
||||
" var8646911284551352322->var8646911284551352323\n"
|
||||
" var6989586621679009794->var8646911284551352323\n"
|
||||
"}");
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -48,6 +48,7 @@ set(ignore
|
|||
gtsam::Rot3Vector
|
||||
gtsam::KeyVector
|
||||
gtsam::BinaryMeasurementsUnit3
|
||||
gtsam::BinaryMeasurementsRot3
|
||||
gtsam::DiscreteKey
|
||||
gtsam::KeyPairDoubleMap)
|
||||
|
||||
|
|
@ -98,11 +99,23 @@ set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam)
|
|||
copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam"
|
||||
"${GTSAM_MODULE_PATH}")
|
||||
|
||||
# Hack to get python test files copied every time they are modified
|
||||
# Hack to get python test and util files copied every time they are modified
|
||||
file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py")
|
||||
foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
|
||||
configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY)
|
||||
endforeach()
|
||||
file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py")
|
||||
foreach(util_file ${GTSAM_PYTHON_UTIL_FILES})
|
||||
configure_file(${util_file} "${GTSAM_MODULE_PATH}/utils/${test_file}" COPYONLY)
|
||||
endforeach()
|
||||
file(GLOB GTSAM_PYTHON_PREAMBLE_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/preamble/*.h")
|
||||
foreach(util_file ${GTSAM_PYTHON_PREAMBLE_FILES})
|
||||
configure_file(${util_file} "${GTSAM_MODULE_PATH}/preamble/${test_file}" COPYONLY)
|
||||
endforeach()
|
||||
file(GLOB GTSAM_PYTHON_SPECIALIZATION_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/specializations/*.h")
|
||||
foreach(util_file ${GTSAM_PYTHON_SPECIALIZATION_FILES})
|
||||
configure_file(${util_file} "${GTSAM_MODULE_PATH}/specializations/${test_file}" COPYONLY)
|
||||
endforeach()
|
||||
|
||||
# Common directory for data/datasets stored with the package.
|
||||
# This will store the data in the Python site package directly.
|
||||
|
|
@ -125,6 +138,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
|
|||
gtsam::KeyVector
|
||||
gtsam::FixedLagSmootherKeyTimestampMapValue
|
||||
gtsam::BinaryMeasurementsUnit3
|
||||
gtsam::BinaryMeasurementsRot3
|
||||
gtsam::CameraSetCal3_S2
|
||||
gtsam::CameraSetCal3Bundler
|
||||
gtsam::CameraSetCal3Unified
|
||||
|
|
@ -160,7 +174,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
|
|||
|
||||
# Hack to get python test files copied every time they are modified
|
||||
file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py")
|
||||
foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
|
||||
foreach(test_file ${GTSAM_UNSTABLE_PYTHON_TEST_FILES})
|
||||
configure_file(${test_file} "${GTSAM_UNSTABLE_MODULE_PATH}/tests/${test_file}" COPYONLY)
|
||||
endforeach()
|
||||
|
||||
|
|
|
|||
|
|
@ -23,8 +23,8 @@ PYBIND11_MAKE_OPAQUE(
|
|||
std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>);
|
||||
PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs);
|
||||
PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
|
||||
PYBIND11_MAKE_OPAQUE(gtsam::Pose2Pairs);
|
||||
PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
|
||||
PYBIND11_MAKE_OPAQUE(
|
||||
gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
|
||||
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
|
||||
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>);
|
||||
|
|
|
|||
|
|
@ -9,4 +9,18 @@
|
|||
* automatic STL binding, such that the raw objects can be accessed in Python.
|
||||
* Without this they will be automatically converted to a Python object, and all
|
||||
* mutations on Python side will not be reflected on C++.
|
||||
*/
|
||||
*/
|
||||
|
||||
// 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>>);
|
||||
|
|
|
|||
|
|
@ -16,6 +16,7 @@ py::bind_vector<
|
|||
m_, "Point2Vector");
|
||||
py::bind_vector<std::vector<gtsam::Point2Pair>>(m_, "Point2Pairs");
|
||||
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::Pose3>>(m_, "Pose3Vector");
|
||||
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>(
|
||||
|
|
|
|||
|
|
@ -13,4 +13,19 @@
|
|||
|
||||
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
|
||||
m_, "BinaryMeasurementsUnit3");
|
||||
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >(
|
||||
m_, "BinaryMeasurementsRot3");
|
||||
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"
|
||||
);
|
||||
|
|
|
|||
|
|
@ -139,6 +139,17 @@ class TestCal3Unified(GtsamTestCase):
|
|||
self.gtsamAssertEquals(z, np.zeros(2))
|
||||
self.gtsamAssertEquals(H @ H.T, 4*np.eye(2))
|
||||
|
||||
Dcal = np.zeros((2, 10), order='F')
|
||||
Dp = np.zeros((2, 2), order='F')
|
||||
camera.calibrate(img_point, Dcal, Dp)
|
||||
|
||||
self.gtsamAssertEquals(Dcal, np.array(
|
||||
[[ 0., 0., 0., -1., 0., 0., 0., 0., 0., 0.],
|
||||
[ 0., 0., 0., 0., -1., 0., 0., 0., 0., 0.]]))
|
||||
self.gtsamAssertEquals(Dp, np.array(
|
||||
[[ 1., -0.],
|
||||
[-0., 1.]]))
|
||||
|
||||
@unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
|
||||
def test_triangulation(self):
|
||||
"""Estimate spatial point from image measurements"""
|
||||
|
|
|
|||
|
|
@ -11,12 +11,12 @@ Author: Frank Dellaert
|
|||
|
||||
# pylint: disable=no-name-in-module, invalid-name
|
||||
|
||||
import unittest
|
||||
import textwrap
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph,
|
||||
DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering)
|
||||
from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteDistribution,
|
||||
DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering)
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
# Some keys:
|
||||
|
|
@ -152,10 +152,10 @@ class TestDiscreteBayesNet(GtsamTestCase):
|
|||
var4[label="4"];
|
||||
var5[label="5"];
|
||||
var6[label="6"];
|
||||
vara0[label="a0", pos="0,2!"];
|
||||
var6989586621679009792[label="a0", pos="0,2!"];
|
||||
|
||||
var4->var6
|
||||
vara0->var3
|
||||
var6989586621679009792->var3
|
||||
var3->var5
|
||||
var6->var5
|
||||
}"""
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
@ -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()
|
||||
|
|
@ -335,21 +335,21 @@ TEST(NonlinearFactorGraph, dot) {
|
|||
"graph {\n"
|
||||
" size=\"5,5\";\n"
|
||||
"\n"
|
||||
" varl1[label=\"l1\"];\n"
|
||||
" varx1[label=\"x1\"];\n"
|
||||
" varx2[label=\"x2\"];\n"
|
||||
" var7782220156096217089[label=\"l1\"];\n"
|
||||
" var8646911284551352321[label=\"x1\"];\n"
|
||||
" var8646911284551352322[label=\"x2\"];\n"
|
||||
"\n"
|
||||
" factor0[label=\"\", shape=point];\n"
|
||||
" varx1--factor0;\n"
|
||||
" var8646911284551352321--factor0;\n"
|
||||
" factor1[label=\"\", shape=point];\n"
|
||||
" varx1--factor1;\n"
|
||||
" varx2--factor1;\n"
|
||||
" var8646911284551352321--factor1;\n"
|
||||
" var8646911284551352322--factor1;\n"
|
||||
" factor2[label=\"\", shape=point];\n"
|
||||
" varx1--factor2;\n"
|
||||
" varl1--factor2;\n"
|
||||
" var8646911284551352321--factor2;\n"
|
||||
" var7782220156096217089--factor2;\n"
|
||||
" factor3[label=\"\", shape=point];\n"
|
||||
" varx2--factor3;\n"
|
||||
" varl1--factor3;\n"
|
||||
" var8646911284551352322--factor3;\n"
|
||||
" var7782220156096217089--factor3;\n"
|
||||
"}\n";
|
||||
|
||||
const NonlinearFactorGraph fg = createNonlinearFactorGraph();
|
||||
|
|
@ -363,21 +363,21 @@ TEST(NonlinearFactorGraph, dot_extra) {
|
|||
"graph {\n"
|
||||
" size=\"5,5\";\n"
|
||||
"\n"
|
||||
" varl1[label=\"l1\", pos=\"0,0!\"];\n"
|
||||
" varx1[label=\"x1\", pos=\"1,0!\"];\n"
|
||||
" varx2[label=\"x2\", pos=\"1,1.5!\"];\n"
|
||||
" var7782220156096217089[label=\"l1\", pos=\"0,0!\"];\n"
|
||||
" var8646911284551352321[label=\"x1\", pos=\"1,0!\"];\n"
|
||||
" var8646911284551352322[label=\"x2\", pos=\"1,1.5!\"];\n"
|
||||
"\n"
|
||||
" factor0[label=\"\", shape=point];\n"
|
||||
" varx1--factor0;\n"
|
||||
" var8646911284551352321--factor0;\n"
|
||||
" factor1[label=\"\", shape=point];\n"
|
||||
" varx1--factor1;\n"
|
||||
" varx2--factor1;\n"
|
||||
" var8646911284551352321--factor1;\n"
|
||||
" var8646911284551352322--factor1;\n"
|
||||
" factor2[label=\"\", shape=point];\n"
|
||||
" varx1--factor2;\n"
|
||||
" varl1--factor2;\n"
|
||||
" var8646911284551352321--factor2;\n"
|
||||
" var7782220156096217089--factor2;\n"
|
||||
" factor3[label=\"\", shape=point];\n"
|
||||
" varx2--factor3;\n"
|
||||
" varl1--factor3;\n"
|
||||
" var8646911284551352322--factor3;\n"
|
||||
" var7782220156096217089--factor3;\n"
|
||||
"}\n";
|
||||
|
||||
const NonlinearFactorGraph fg = createNonlinearFactorGraph();
|
||||
|
|
|
|||
Loading…
Reference in New Issue