Added a generic prior factor for specific components of a variable
Alex Cunningham
parent
040493474f
commit
611b5061cd
|
|
@ -0,0 +1,139 @@
|
|||
/**
|
||||
* @file PartialPriorFactor.h
|
||||
* @brief A simple prior factor that allows for setting a prior only on a part of linear parameters
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A class for a soft partial prior on any Lie type, with a mask over Expmap
|
||||
* parameters. Note that this will use Logmap() to find a tangent space parameterization
|
||||
* for the variable attached, so this may fail for highly nonlinear manifolds.
|
||||
*
|
||||
* The prior vector used in this factor is stored in compressed form, such that
|
||||
* it only contains values for measurements that are to be compared, and they are in
|
||||
* the same order as T::Logmap(). The mask will determine which components to extract
|
||||
* in the error function.
|
||||
*
|
||||
* It takes two template parameters:
|
||||
* Key (typically TypedSymbol) is used to look up T's in a Values
|
||||
* Values where the T's are stored, typically LieValues<Key> or a TupleValues<...>
|
||||
*
|
||||
* For practical use, it would be good to subclass this factor and have the class type
|
||||
* construct the mask.
|
||||
*/
|
||||
template<class VALUES, class KEY>
|
||||
class PartialPriorFactor: public NonlinearFactor1<VALUES, KEY> {
|
||||
|
||||
public:
|
||||
typedef typename KEY::Value T;
|
||||
|
||||
protected:
|
||||
|
||||
typedef NonlinearFactor1<VALUES, KEY> Base;
|
||||
typedef PartialPriorFactor<VALUES, KEY> This;
|
||||
|
||||
Vector prior_; /// measurement on logmap parameters, in compressed form
|
||||
std::vector<bool> mask_; /// flags to mask all parameters not measured
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor - only use for serialization */
|
||||
PartialPriorFactor() {}
|
||||
|
||||
// shorthand for a smart pointer to a factor
|
||||
typedef typename boost::shared_ptr<PartialPriorFactor> shared_ptr;
|
||||
|
||||
virtual ~PartialPriorFactor() {}
|
||||
|
||||
/** Full Constructor: requires mask and vector - not for typical use */
|
||||
PartialPriorFactor(const KEY& key, const std::vector<bool>& mask,
|
||||
const Vector& prior, const SharedGaussian& model) :
|
||||
Base(model, key), prior_(prior), mask_(mask) {
|
||||
assert(mask_.size() == T::Dim()); // NOTE: assumes constant size variable
|
||||
assert(nrTrue() == model->dim());
|
||||
assert(nrTrue() == prior_.size());
|
||||
}
|
||||
|
||||
/** Single Element Constructor: acts on a single parameter specified by idx */
|
||||
PartialPriorFactor(const KEY& key, size_t idx, double prior, const SharedGaussian& model) :
|
||||
Base(model, key), prior_(Vector_(1, prior)), mask_(T::Dim(), false) {
|
||||
assert(model->dim() == 1);
|
||||
mask_[idx] = true;
|
||||
assert(nrTrue() == 1);
|
||||
}
|
||||
|
||||
/** Indices Constructor: specify the mask with a set of indices */
|
||||
PartialPriorFactor(const KEY& key, const std::vector<size_t>& mask, const Vector& prior,
|
||||
const SharedGaussian& model) :
|
||||
Base(model, key), prior_(prior), mask_(T::Dim(), false) {
|
||||
assert((size_t)prior_.size() == mask.size());
|
||||
assert(model->dim() == (size_t) prior.size());
|
||||
for (size_t i=0; i<mask.size(); ++i) {
|
||||
assert(mask[i] < mask_.size());
|
||||
mask_[mask[i]] = true;
|
||||
}
|
||||
assert(nrTrue() == this->dim());
|
||||
}
|
||||
|
||||
/** implement functions needed for Testable */
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s) const {
|
||||
Base::print(s);
|
||||
gtsam::print(prior_, "prior");
|
||||
}
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
|
||||
const This *e = dynamic_cast<const This*> (&expected);
|
||||
return e != NULL && Base::equals(*e, tol) &&
|
||||
gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) &&
|
||||
this->mask_ == e->mask_;
|
||||
}
|
||||
|
||||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/** vector of errors */
|
||||
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
|
||||
if (H) (*H) = zeros(this->dim(), p.dim());
|
||||
Vector full_logmap = T::Logmap(p);
|
||||
Vector masked_logmap = zero(this->dim());
|
||||
size_t masked_idx=0;
|
||||
for (size_t i=0;i<mask_.size();++i)
|
||||
if (mask_[i]) {
|
||||
masked_logmap(masked_idx) = full_logmap(i);
|
||||
if (H) (*H)(masked_idx, i) = 1.0;
|
||||
++masked_idx;
|
||||
}
|
||||
return masked_logmap - prior_;
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
/** counts true elements in the mask */
|
||||
size_t nrTrue() const {
|
||||
size_t result=0;
|
||||
for (size_t i=0; i<mask_.size(); ++i)
|
||||
if (mask_[i]) ++result;
|
||||
return result;
|
||||
}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("NonlinearFactor1",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(prior_);
|
||||
ar & BOOST_SERIALIZATION_NVP(mask_);
|
||||
}
|
||||
}; // \class PartialPriorFactor
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -15,19 +15,15 @@
|
|||
**/
|
||||
#pragma once
|
||||
|
||||
#include <ostream>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A class for a soft prior on any Lie type
|
||||
* It takes three template parameters:
|
||||
* T is the Lie group type for which the prior is define
|
||||
* It takes two template parameters:
|
||||
* Key (typically TypedSymbol) is used to look up T's in a Values
|
||||
* Values where the T's are stored, typically LieValues<Key,T> or a TupleValues<...>
|
||||
* Values where the T's are stored, typically LieValues<Key> or a TupleValues<...>
|
||||
* The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
|
||||
* a simple type like int will not work
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
#include <iostream>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/assign/std/list.hpp>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
using namespace boost;
|
||||
using namespace boost::assign;
|
||||
|
||||
|
|
@ -29,6 +30,7 @@ using namespace boost::assign;
|
|||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/slam/pose3SLAM.h>
|
||||
#include <gtsam/slam/PartialPriorFactor.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -36,6 +38,10 @@ using namespace gtsam;
|
|||
// common measurement covariance
|
||||
static Matrix covariance = eye(6);
|
||||
|
||||
const double tol=1e-5;
|
||||
|
||||
using namespace pose3SLAM;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// test optimization with 6 poses arranged in a hexagon and a loop closure
|
||||
TEST(Pose3Graph, optimizeCircle) {
|
||||
|
|
@ -70,7 +76,6 @@ TEST(Pose3Graph, optimizeCircle) {
|
|||
// Choose an ordering and optimize
|
||||
shared_ptr<Ordering> ordering(new Ordering);
|
||||
*ordering += "x0","x1","x2","x3","x4","x5";
|
||||
typedef NonlinearOptimizer<Pose3Graph, Pose3Values> Optimizer;
|
||||
NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15);
|
||||
Optimizer optimizer0(fg, initial, ordering, params);
|
||||
Optimizer optimizer = optimizer0.levenbergMarquardt();
|
||||
|
|
@ -84,6 +89,68 @@ TEST(Pose3Graph, optimizeCircle) {
|
|||
CHECK(assert_equal(_0T1,actual[5].between(actual[0]),1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3Graph, partial_prior_height) {
|
||||
typedef PartialPriorFactor<Values, Key> Partial;
|
||||
|
||||
// reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3)
|
||||
|
||||
// height prior - single element interface
|
||||
Key key(1);
|
||||
double exp_height = 5.0;
|
||||
SharedDiagonal model = noiseModel::Unit::Create(1);
|
||||
Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height));
|
||||
Partial height(key, 5, exp_height, model);
|
||||
Matrix actA;
|
||||
EXPECT(assert_equal(Vector_(1,-2.0), height.evaluateError(init, actA), tol));
|
||||
Matrix expA = Matrix_(1, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
|
||||
EXPECT(assert_equal(expA, actA));
|
||||
|
||||
Graph graph;
|
||||
// graph.add(height); // FAIL - on compile, can't initialize a reference?
|
||||
graph.push_back(boost::shared_ptr<Partial>(new Partial(height)));
|
||||
|
||||
Values values;
|
||||
values.insert(key, init);
|
||||
|
||||
// linearization
|
||||
EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
|
||||
|
||||
Values actual = *Optimizer::optimizeLM(graph, values);
|
||||
EXPECT(assert_equal(expected, actual[key], tol));
|
||||
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3Graph, partial_prior_xy) {
|
||||
typedef PartialPriorFactor<Values, Key> Partial;
|
||||
|
||||
// XY prior - full mask interface
|
||||
Key key(1);
|
||||
Vector exp_xy = Vector_(2, 3.0, 4.0);
|
||||
SharedDiagonal model = noiseModel::Unit::Create(2);
|
||||
Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0));
|
||||
vector<size_t> mask; mask += 3, 4;
|
||||
Partial priorXY(key, mask, exp_xy, model);
|
||||
Matrix actA;
|
||||
EXPECT(assert_equal(Vector_(2,-2.0,-6.0), priorXY.evaluateError(init, actA), tol));
|
||||
Matrix expA = Matrix_(2, 6,
|
||||
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
|
||||
EXPECT(assert_equal(expA, actA));
|
||||
|
||||
Graph graph;
|
||||
// graph.add(priorXY); // FAIL - on compile, can't initialize a reference?
|
||||
graph.push_back(Partial::shared_ptr(new Partial(priorXY)));
|
||||
|
||||
Values values;
|
||||
values.insert(key, init);
|
||||
|
||||
Values actual = *Optimizer::optimizeLM(graph, values);
|
||||
EXPECT(assert_equal(expected, actual[key], tol));
|
||||
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue