diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h new file mode 100644 index 000000000..d1ab0bb1d --- /dev/null +++ b/gtsam/slam/PartialPriorFactor.h @@ -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 + +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 or a TupleValues<...> + * + * For practical use, it would be good to subclass this factor and have the class type + * construct the mask. + */ + template + class PartialPriorFactor: public NonlinearFactor1 { + + public: + typedef typename KEY::Value T; + + protected: + + typedef NonlinearFactor1 Base; + typedef PartialPriorFactor This; + + Vector prior_; /// measurement on logmap parameters, in compressed form + std::vector 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 shared_ptr; + + virtual ~PartialPriorFactor() {} + + /** Full Constructor: requires mask and vector - not for typical use */ + PartialPriorFactor(const KEY& key, const std::vector& 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& 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; idim()); + } + + /** 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& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&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 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 + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(prior_); + ar & BOOST_SERIALIZATION_NVP(mask_); + } + }; // \class PartialPriorFactor + +} /// namespace gtsam diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 48e512555..b23245ca9 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -15,19 +15,15 @@ **/ #pragma once -#include -#include #include -#include 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 or a TupleValues<...> + * Values where the T's are stored, typically LieValues 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 */ diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 256b6152e..e07f632b6 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -19,6 +19,7 @@ #include #include #include +#include using namespace boost; using namespace boost::assign; @@ -29,6 +30,7 @@ using namespace boost::assign; #define GTSAM_MAGIC_KEY #include +#include 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(new Ordering); *ordering += "x0","x1","x2","x3","x4","x5"; - typedef NonlinearOptimizer 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 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(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 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 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;