moved and enabled testKey, LinearApproxFactor creation/linearization works
Alex Cunningham
parent
c57c93a490
commit
c92026884b
|
|
@ -17,7 +17,7 @@ check_PROGRAMS =
|
|||
|
||||
# Lie Groups
|
||||
headers += Key.h LieValues.h LieValues-inl.h TupleValues.h TupleValues-inl.h
|
||||
check_PROGRAMS += tests/testLieValues
|
||||
check_PROGRAMS += tests/testLieValues tests/testKey
|
||||
|
||||
# Nonlinear nonlinear
|
||||
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
|
||||
|
|
|
|||
|
|
@ -7,7 +7,7 @@
|
|||
using namespace boost::assign;
|
||||
|
||||
#include <gtsam/CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -1,4 +1,4 @@
|
|||
/*
|
||||
/**
|
||||
* @file LinearApproxFactor.h
|
||||
* @brief A dummy factor that allows a linear factor to act as a nonlinear factor
|
||||
* @author Alex Cunningham
|
||||
|
|
@ -15,51 +15,83 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template <class Values, class Key>
|
||||
LinearApproxFactor<Values,Key>::LinearApproxFactor(GaussianFactor::shared_ptr lin_factor, const Values& lin_points)
|
||||
LinearApproxFactor<Values,Key>::LinearApproxFactor(
|
||||
GaussianFactor::shared_ptr lin_factor, const Ordering& ordering, const Values& lin_points)
|
||||
: Base(noiseModel::Unit::Create(lin_factor->get_model()->dim())),
|
||||
lin_factor_(lin_factor), lin_points_(lin_points)
|
||||
{
|
||||
// create the keys and store them
|
||||
BOOST_FOREACH(Symbol key, lin_factor->keys()) {
|
||||
nonlinearKeys_.push_back(Key(key.index()));
|
||||
this->keys_.push_back(key);
|
||||
b_(lin_factor->getb()), model_(lin_factor->get_model()), lin_points_(lin_points)
|
||||
{
|
||||
BOOST_FOREACH(const Ordering::Map::value_type& p, ordering) {
|
||||
Symbol key = p.first;
|
||||
varid_t var = p.second;
|
||||
|
||||
// check if actually in factor
|
||||
Factor::const_iterator it = lin_factor->find(var);
|
||||
if (it != lin_factor->end()) {
|
||||
// store matrix
|
||||
Matrix A = lin_factor->getA(it);
|
||||
matrices_.insert(make_pair(key, A));
|
||||
|
||||
// store keys
|
||||
nonlinearKeys_.push_back(Key(key.index()));
|
||||
this->keys_.push_back(key);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Values, class Key>
|
||||
Vector LinearApproxFactor<Values,Key>::unwhitenedError(const Values& c) const {
|
||||
// extract the points in the new config
|
||||
VectorValues delta;
|
||||
BOOST_FOREACH(const Key& key, nonlinearKeys_) {
|
||||
X newPt = c[key], linPt = lin_points_[key];
|
||||
Vector d = linPt.logmap(newPt);
|
||||
delta.insert(key, d);
|
||||
}
|
||||
// BOOST_FOREACH(const Key& key, nonlinearKeys_) {
|
||||
// X newPt = c[key], linPt = lin_points_[key];
|
||||
// Vector d = linPt.logmap(newPt);
|
||||
// delta.insert(key, d);
|
||||
// }
|
||||
|
||||
return lin_factor_->unweighted_error(delta);
|
||||
return zero(b_.size()); //FIXME: PLACEHOLDER!
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Values, class Key>
|
||||
boost::shared_ptr<GaussianFactor>
|
||||
LinearApproxFactor<Values,Key>::linearize(const Values& c) const {
|
||||
Vector b = lin_factor_->getb();
|
||||
SharedDiagonal model = lin_factor_->get_model();
|
||||
std::vector<std::pair<Symbol, Matrix> > terms;
|
||||
BOOST_FOREACH(Symbol key, lin_factor_->keys()) {
|
||||
terms.push_back(std::make_pair(key, lin_factor_->get_A(key)));
|
||||
}
|
||||
LinearApproxFactor<Values,Key>::linearize(const Values& c, const Ordering& ordering) const {
|
||||
|
||||
return boost::shared_ptr<GaussianFactor>(
|
||||
new GaussianFactor(terms, b, model));
|
||||
// sort by varid - only known at linearization time
|
||||
typedef std::map<varid_t, Matrix> VarMatrixMap;
|
||||
VarMatrixMap sorting_terms;
|
||||
BOOST_FOREACH(const SymbolMatrixMap::value_type& p, matrices_)
|
||||
sorting_terms.insert(std::make_pair(ordering[p.first], p.second));
|
||||
|
||||
// move into terms
|
||||
std::vector<std::pair<varid_t, Matrix> > terms;
|
||||
BOOST_FOREACH(const VarMatrixMap::value_type& p, sorting_terms)
|
||||
terms.push_back(p);
|
||||
|
||||
return boost::shared_ptr<GaussianFactor>(new GaussianFactor(terms, b_, model_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Values, class Key>
|
||||
Factor::shared_ptr
|
||||
LinearApproxFactor<Values,Key>::symbolic(const Ordering& ordering) const {
|
||||
std::vector<varid_t> key_ids(this->keys_.size());
|
||||
size_t i=0;
|
||||
BOOST_FOREACH(const Symbol& key, this->keys_)
|
||||
key_ids[i++] = ordering[key];
|
||||
std::sort(key_ids.begin(), key_ids.end());
|
||||
return boost::shared_ptr<Factor>(new Factor(key_ids.begin(), key_ids.end()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Values, class Key>
|
||||
void LinearApproxFactor<Values,Key>::print(const std::string& s) const {
|
||||
LinearApproxFactor<Values,Key>::Base::print(s);
|
||||
lin_factor_->print();
|
||||
BOOST_FOREACH(const SymbolMatrixMap::value_type& p, matrices_) {
|
||||
gtsam::print(p.second, (std::string) p.first);
|
||||
}
|
||||
gtsam::print(b_, std::string("b"));
|
||||
model_->print("model");
|
||||
}
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
/*
|
||||
/**
|
||||
* @file LinearApproxFactor.h
|
||||
* @brief A dummy factor that allows a linear factor to act as a nonlinear factor
|
||||
* @author Alex Cunningham
|
||||
|
|
@ -8,8 +8,6 @@
|
|||
|
||||
#include <vector>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -35,7 +33,12 @@ public:
|
|||
|
||||
protected:
|
||||
/** hold onto the factor itself */
|
||||
GaussianFactor::shared_ptr lin_factor_;
|
||||
// GaussianFactor::shared_ptr lin_factor_;
|
||||
// store components of a linear factor that can be reordered
|
||||
typedef std::map<Symbol, Matrix> SymbolMatrixMap;
|
||||
SymbolMatrixMap matrices_;
|
||||
Vector b_;
|
||||
SharedDiagonal model_;
|
||||
|
||||
/** linearization points for error calculation */
|
||||
Values lin_points_;
|
||||
|
|
@ -51,8 +54,14 @@ protected:
|
|||
|
||||
public:
|
||||
|
||||
/** use this constructor when starting with nonlinear keys */
|
||||
LinearApproxFactor(GaussianFactor::shared_ptr lin_factor, const Values& lin_points);
|
||||
/**
|
||||
* use this constructor when starting with nonlinear keys
|
||||
*
|
||||
* Note that you need to have the ordering used to construct the factor
|
||||
* initially in order to find the actual keys
|
||||
*/
|
||||
LinearApproxFactor(GaussianFactor::shared_ptr lin_factor,
|
||||
const Ordering& ordering, const Values& lin_points);
|
||||
|
||||
virtual ~LinearApproxFactor() {}
|
||||
|
||||
|
|
@ -61,11 +70,17 @@ public:
|
|||
|
||||
/**
|
||||
* linearize to a GaussianFactor
|
||||
* Just returns a copy of the existing factor
|
||||
* NOTE: copies to avoid actual factor getting destroyed
|
||||
* during elimination
|
||||
* Reconstructs the linear factor from components to ensure that
|
||||
* the ordering is correct
|
||||
*/
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& c) const;
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(
|
||||
const Values& c, const Ordering& ordering) const;
|
||||
|
||||
/**
|
||||
* Create a symbolic factor using the given ordering to determine the
|
||||
* variable indices.
|
||||
*/
|
||||
Factor::shared_ptr symbolic(const Ordering& ordering) const;
|
||||
|
||||
/** get access to nonlinear keys */
|
||||
KeyVector nonlinearKeys() const { return nonlinearKeys_; }
|
||||
|
|
@ -74,7 +89,7 @@ public:
|
|||
virtual void print(const std::string& s="") const;
|
||||
|
||||
/** access to b vector of gaussian */
|
||||
Vector get_b() const { return lin_factor_->getb(); }
|
||||
Vector get_b() const { return b_; }
|
||||
};
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
|
|
@ -22,7 +22,7 @@ check_PROGRAMS += testTupleValues
|
|||
#check_PROGRAMS += testNonlinearEqualityConstraint
|
||||
#check_PROGRAMS += testBoundingConstraint
|
||||
check_PROGRAMS += testTransformConstraint
|
||||
#check_PROGRAMS += testLinearApproxFactor
|
||||
check_PROGRAMS += testLinearApproxFactor
|
||||
|
||||
# only if serialization is available
|
||||
if ENABLE_SERIALIZATION
|
||||
|
|
|
|||
|
|
@ -18,22 +18,21 @@ typedef LinearApproxFactor<planarSLAM::Values,planarSLAM::PointKey> ApproxFactor
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST ( LinearApproxFactor, basic ) {
|
||||
Symbol key1('x', 1);
|
||||
Symbol key1('l', 1);
|
||||
Matrix A1 = eye(2);
|
||||
Vector b = repeat(2, 1.2);
|
||||
SharedDiagonal model = noiseModel::Unit::Create(2);
|
||||
GaussianFactor::shared_ptr lin_factor(new GaussianFactor(1, A1, b, model));
|
||||
GaussianFactor::shared_ptr lin_factor(new GaussianFactor(0, A1, b, model));
|
||||
Ordering ordering;
|
||||
ordering.push_back(key1);
|
||||
planarSLAM::Values lin_points;
|
||||
ApproxFactor f1(lin_factor, lin_points);
|
||||
ApproxFactor f1(lin_factor, ordering, lin_points);
|
||||
|
||||
EXPECT(f1.size() == 1);
|
||||
ApproxFactor::KeyVector expKeyVec;
|
||||
expKeyVec.push_back(planarSLAM::PointKey(key1.index()));
|
||||
EXPECT(assert_equal(expKeyVec, f1.nonlinearKeys()));
|
||||
|
||||
planarSLAM::Values config; // doesn't really need to have any data
|
||||
Ordering ordering;
|
||||
ordering.push_back(key1);
|
||||
GaussianFactor::shared_ptr actual = f1.linearize(config, ordering);
|
||||
|
||||
// Check the linearization
|
||||
|
|
|
|||
Loading…
Reference in New Issue