From 64eca2d5505b2fb472e33e34165bc0ed7ea81a18 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 10 Jan 2010 19:25:19 +0000 Subject: [PATCH] addConstraint --- cpp/NonlinearEquality.h | 153 +++++++++++++++++++++++----------------- cpp/Pose2Graph.h | 62 +++++++++------- cpp/Pose3Graph.h | 62 +++++++++------- cpp/testPose2Graph.cpp | 19 +---- cpp/testPose3Graph.cpp | 12 +--- 5 files changed, 165 insertions(+), 143 deletions(-) diff --git a/cpp/NonlinearEquality.h b/cpp/NonlinearEquality.h index 66bb51309..35097e394 100644 --- a/cpp/NonlinearEquality.h +++ b/cpp/NonlinearEquality.h @@ -12,81 +12,104 @@ namespace gtsam { -/** - * An equality factor that forces either one variable to a constant, - * or a set of variables to be equal to each other. - * Throws an error at linearization if the constraints are not met. - */ -template -class NonlinearEquality : public NonlinearFactor { -private: + /** + * Template default compare function that assumes Congig.get yields a testable T + */ + template + bool compare(const std::string& key, const Config& feasible, const Config& input) { + const T& t1 = feasible.get(key); + const T& t2 = input.get(key); + return t1.equals(t2); + } - // node to constrain - std::string key_; - - // config containing the necessary feasible point - Config feasible_; - - // dimension of the variable - size_t dim_; - -public: /** - * Function that compares a value from a config with - * another to determine whether a linearization point is - * a feasible point. - * @param key is the identifier for the key - * @param feasible is the value which is constrained - * @param input is the config to be tested for feasibility - * @return true if the linearization point is feasible + * An equality factor that forces either one variable to a constant, + * or a set of variables to be equal to each other. + * Throws an error at linearization if the constraints are not met. */ - bool (*compare_)(const std::string& key, const Config& feasible, const Config& input); + template + class NonlinearEquality: public NonlinearFactor { + private: - /** Constructor */ - NonlinearEquality(const std::string& key, - const Config& feasible, - size_t dim, - bool (*compare)(const std::string& key, - const Config& feasible, - const Config& input)) - : key_(key), dim_(dim), feasible_(feasible), compare_(compare) { + // node to constrain + std::string key_; - } + // config containing the necessary feasible point + Config feasible_; - void print(const std::string& s = "") const { - std::cout << "Constraint: " << s << " on [" << key_ << "]\n"; - feasible_.print("Feasible Point"); - std::cout << "Variable Dimension: " << dim_ << std::endl; - } + // dimension of the variable + size_t dim_; - /** Check if two factors are equal */ - bool equals(const Factor& f, double tol=1e-9) const { - const NonlinearEquality* p = dynamic_cast*> (&f); - if (p == NULL) return false; - if (key_ != p->key_) return false; - if (!compare_(key_, feasible_, p->feasible_)) return false; // only check the relevant value - return dim_ == p->dim_; - } + public: - /** error function */ - inline Vector error_vector(const Config& c) const { - if (!compare_(key_, feasible_, c)) - return repeat(dim_, std::numeric_limits::infinity()); // set error to infinity if not equal - else - return zero(dim_); // set error to zero if equal - } + /** + * Function that compares a value from a config with + * another to determine whether a linearization point is + * a feasible point. + * @param key is the identifier for the key + * @param feasible is the value which is constrained + * @param input is the config to be tested for feasibility + * @return true if the linearization point is feasible + */ + bool (*compare_)(const std::string& key, const Config& feasible, + const Config& input); + + /** Constructor */ + NonlinearEquality(const std::string& key, const Config& feasible, + size_t dim, bool(*compare)(const std::string& key, + const Config& feasible, const Config& input)) : + key_(key), dim_(dim), feasible_(feasible), compare_(compare) { - /** linearize a nonlinear constraint into a linear constraint */ - boost::shared_ptr linearize(const Config& c) const { - if (!compare_(key_, feasible_, c)) { - throw std::invalid_argument("Linearization point not feasible for " + key_ + "!"); - } else { - GaussianFactor::shared_ptr ret(new GaussianFactor(key_, eye(dim_), zero(dim_), 0.0)); - return ret; } - } -}; -} + /** + * Constructor with default compare + * Needs class T to have dim() + * and Config to have insert and get + */ + template + NonlinearEquality(const std::string& key, const T& feasible) : + key_(key), dim_(dim(feasible)), compare_(compare) { + feasible_.insert(key,feasible); + } + + void print(const std::string& s = "") const { + std::cout << "Constraint: " << s << " on [" << key_ << "]\n"; + feasible_.print("Feasible Point"); + std::cout << "Variable Dimension: " << dim_ << std::endl; + } + + /** Check if two factors are equal */ + bool equals(const Factor& f, double tol = 1e-9) const { + const NonlinearEquality* p = + dynamic_cast*> (&f); + if (p == NULL) return false; + if (key_ != p->key_) return false; + if (!compare_(key_, feasible_, p->feasible_)) return false; // only check the relevant value + return dim_ == p->dim_; + } + + /** error function */ + inline Vector error_vector(const Config& c) const { + if (!compare_(key_, feasible_, c)) + return repeat(dim_, std::numeric_limits::infinity()); // set error to infinity if not equal + else + return zero(dim_); // set error to zero if equal + } + + /** linearize a nonlinear constraint into a linear constraint */ + boost::shared_ptr linearize(const Config& c) const { + if (!compare_(key_, feasible_, c)) { + throw std::invalid_argument("Linearization point not feasible for " + + key_ + "!"); + } else { + GaussianFactor::shared_ptr ret(new GaussianFactor(key_, eye(dim_), + zero(dim_), 0.0)); + return ret; + } + } + }; // NonlinearEquality + +} // namespace gtsam diff --git a/cpp/Pose2Graph.h b/cpp/Pose2Graph.h index 76b84eadd..29645e883 100644 --- a/cpp/Pose2Graph.h +++ b/cpp/Pose2Graph.h @@ -8,39 +8,51 @@ #pragma once #include "NonlinearFactorGraph.h" +#include "NonlinearEquality.h" #include "Pose2Factor.h" #include "Pose2Config.h" -namespace gtsam{ +namespace gtsam { -/** - * Non-linear factor graph for visual SLAM - */ -class Pose2Graph : public gtsam::NonlinearFactorGraph { + /** + * Non-linear factor graph for visual SLAM + */ + class Pose2Graph: public gtsam::NonlinearFactorGraph { -public: + public: - /** default constructor is empty graph */ - Pose2Graph() {} + /** default constructor is empty graph */ + Pose2Graph() { + } - /** - * equals - */ - bool equals(const Pose2Graph& p, double tol=1e-9) const; + /** + * equals + */ + bool equals(const Pose2Graph& p, double tol = 1e-9) const; - /** - * Add a factor without having to do shared factor dance - */ - inline void add(const std::string& key1, const std::string& key2, - const Pose2& measured, const Matrix& covariance) { - push_back(sharedFactor(new Pose2Factor(key1, key2, measured, covariance))); - } + /** + * Add a factor without having to do shared factor dance + */ + inline void add(const std::string& key1, const std::string& key2, + const Pose2& measured, const Matrix& covariance) { + push_back(sharedFactor(new Pose2Factor(key1, key2, measured, covariance))); + } -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) {} -}; + /** + * Add an equality constraint on a pose + * @param key of pose + * @param pose which pose to constrain it to + */ + inline void addConstraint(const std::string& key, const Pose2& pose = Pose2()) { + push_back(sharedFactor(new NonlinearEquality(key, pose))); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + } + }; } // namespace gtsam diff --git a/cpp/Pose3Graph.h b/cpp/Pose3Graph.h index d6d4fba51..3105b0d6e 100644 --- a/cpp/Pose3Graph.h +++ b/cpp/Pose3Graph.h @@ -8,39 +8,51 @@ #pragma once #include "NonlinearFactorGraph.h" +#include "NonlinearEquality.h" #include "Pose3Factor.h" #include "Pose3Config.h" -namespace gtsam{ +namespace gtsam { -/** - * Non-linear factor graph for visual SLAM - */ -class Pose3Graph : public gtsam::NonlinearFactorGraph { + /** + * Non-linear factor graph for visual SLAM + */ + class Pose3Graph: public gtsam::NonlinearFactorGraph { -public: + public: - /** default constructor is empty graph */ - Pose3Graph() {} + /** default constructor is empty graph */ + Pose3Graph() { + } - /** - * equals - */ - bool equals(const Pose3Graph& p, double tol=1e-9) const; + /** + * equals + */ + bool equals(const Pose3Graph& p, double tol = 1e-9) const; - /** - * Add a factor without having to do shared factor dance - */ - inline void add(const std::string& key1, const std::string& key2, - const Pose3& measured, const Matrix& covariance) { - push_back(sharedFactor(new Pose3Factor(key1, key2, measured, covariance))); - } + /** + * Add a factor without having to do shared factor dance + */ + inline void add(const std::string& key1, const std::string& key2, + const Pose3& measured, const Matrix& covariance) { + push_back(sharedFactor(new Pose3Factor(key1, key2, measured, covariance))); + } -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) {} -}; + /** + * Add an equality constraint on a pose + * @param key of pose + * @param pose which pose to constrain it to + */ + inline void addConstraint(const std::string& key, const Pose3& pose = Pose3()) { + push_back(sharedFactor(new NonlinearEquality (key, pose))); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + } + }; } // namespace gtsam diff --git a/cpp/testPose2Graph.cpp b/cpp/testPose2Graph.cpp index 99a870ff0..bda87c0a0 100644 --- a/cpp/testPose2Graph.cpp +++ b/cpp/testPose2Graph.cpp @@ -11,7 +11,6 @@ using namespace boost::assign; #include #include "NonlinearOptimizer-inl.h" -#include "NonlinearEquality.h" #include "Ordering.h" #include "Pose2Config.h" #include "Pose2Graph.h" @@ -81,22 +80,12 @@ TEST( Pose2Graph, linerization ) CHECK(assert_equal(lfg_expected, lfg_linearized)); } -/* ************************************************************************* */ -bool poseCompare(const std::string& key, - const gtsam::Pose2Config& feasible, - const gtsam::Pose2Config& input) { - return feasible.get(key).equals(input.get(key)); -} - /* ************************************************************************* */ TEST(Pose2Graph, optimize) { // create a Pose graph with one equality constraint and one measurement Pose2Graph fg; - Pose2Config feasible; - feasible.insert("p0", Pose2(0,0,0)); - fg.push_back(Pose2Graph::sharedFactor( - new NonlinearEquality("p0", feasible, dim(Pose2()), poseCompare))); + fg.addConstraint("p0", Pose2(0,0,0)); fg.add("p0", "p1", Pose2(1,2,M_PI_2), covariance); // Create initial config @@ -118,7 +107,6 @@ TEST(Pose2Graph, optimize) { expected.insert("p0", Pose2(0,0,0)); expected.insert("p1", Pose2(1,2,M_PI_2)); CHECK(assert_equal(expected, *optimizer.config())); - } /* ************************************************************************* */ @@ -131,10 +119,7 @@ TEST(Pose2Graph, optimizeCircle) { // create a Pose graph with one equality constraint and one measurement Pose2Graph fg; - Pose2Config feasible; - feasible.insert("p0", p0); - fg.push_back(Pose2Graph::sharedFactor( - new NonlinearEquality("p0", feasible, dim(Pose2()), poseCompare))); + fg.addConstraint("p0", p0); Pose2 delta = between(p0,p1); fg.add("p0", "p1", delta, covariance); fg.add("p1", "p2", delta, covariance); diff --git a/cpp/testPose3Graph.cpp b/cpp/testPose3Graph.cpp index d5a98d4f3..fde8b98a2 100644 --- a/cpp/testPose3Graph.cpp +++ b/cpp/testPose3Graph.cpp @@ -23,13 +23,6 @@ using namespace gtsam; // common measurement covariance static Matrix covariance = eye(6); -/* ************************************************************************* */ -bool poseCompare(const std::string& key, - const gtsam::Pose3Config& feasible, - const gtsam::Pose3Config& input) { - return feasible.get(key).equals(input.get(key)); -} - /* ************************************************************************* */ // test optimization with 6 poses arranged in a hexagon and a loop closure TEST(Pose3Graph, optimizeCircle) { @@ -40,10 +33,7 @@ TEST(Pose3Graph, optimizeCircle) { // create a Pose graph with one equality constraint and one measurement Pose3Graph fg; - Pose3Config feasible; - feasible.insert("p0", p0); - fg.push_back(Pose3Graph::sharedFactor( - new NonlinearEquality("p0", feasible, dim(Pose3()), poseCompare))); + fg.addConstraint("p0", p0); Pose3 delta = between(p0,p1); fg.add("p0", "p1", delta, covariance); fg.add("p1", "p2", delta, covariance);