diff --git a/.cproject b/.cproject index 1d226ae91..d6793bf22 100644 --- a/.cproject +++ b/.cproject @@ -1,4 +1,4 @@ - + @@ -298,20 +298,6 @@ - -make -install -true -true -true - - -make -check -true -true -true - make -k @@ -322,6 +308,7 @@ make + testSimpleCamera.run true true @@ -337,7 +324,6 @@ make - testVSLAMFactor.run true true @@ -345,21 +331,22 @@ make + testCalibratedCamera.run true true true - + make - -testGaussianConditional.run +testConditionalGaussian.run true true true make + testPose2.run true true @@ -367,6 +354,7 @@ make + testRot3.run true true @@ -374,29 +362,29 @@ make - testNonlinearOptimizer.run true true true - + make -testGaussianFactor.run + +testLinearFactor.run true true true - + make -testGaussianFactorGraph.run + +testLinearFactorGraph.run true true true make - testNonlinearFactorGraph.run true true @@ -404,6 +392,7 @@ make + testPose3.run true true @@ -411,7 +400,6 @@ make - testVectorConfig.run true true @@ -419,7 +407,6 @@ make - testPoint2.run true true @@ -427,27 +414,31 @@ make + testNonlinearFactor.run true true true - + make -timeGaussianFactor.run + +timeLinearFactor.run true true true - + make -timeGaussianFactorGraph.run + +timeLinearFactorGraph.run true true true make + testGaussianBayesNet.run true true @@ -455,7 +446,6 @@ make - testBayesTree.run true false @@ -463,6 +453,7 @@ make + testSymbolicBayesNet.run true false @@ -470,7 +461,6 @@ make - testSymbolicFactorGraph.run true false @@ -478,6 +468,7 @@ make + testVector.run true true @@ -485,6 +476,7 @@ make + testMatrix.run true true @@ -492,23 +484,29 @@ make - testInference.run true true true - + make -testVSLAMFactorGraph.run +testNonlinearEquality.run +true +true +true + + +make + +testVSLAMGraph.run true true true make - install true true @@ -516,7 +514,6 @@ make - clean true true @@ -524,7 +521,22 @@ make - +check +true +true +true + + +make + +install +true +true +true + + +make + check true true diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 4f0b2a25c..85a8a8aff 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -111,8 +111,8 @@ timeGaussianFactorGraph: LDFLAGS += smallExample.o -L.libs -lgtsam -L../CppUnitL # Nonlinear inference headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h -sources += NonlinearFactor.cpp -sources += NonlinearEquality.cpp +sources += NonlinearFactor.cpp +sources += NonlinearEquality.h check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp testNonlinearFactor_LDADD = libgtsam.la diff --git a/cpp/NonlinearEquality.cpp b/cpp/NonlinearEquality.cpp deleted file mode 100644 index 6fbaf0f6d..000000000 --- a/cpp/NonlinearEquality.cpp +++ /dev/null @@ -1,9 +0,0 @@ -/* - * @file NonlinearEquality.cpp - * @brief - * @author alexgc - */ - -#include "NonlinearEquality.h" - - diff --git a/cpp/NonlinearEquality.h b/cpp/NonlinearEquality.h index 5a1a504e9..a6fc98eec 100644 --- a/cpp/NonlinearEquality.h +++ b/cpp/NonlinearEquality.h @@ -6,6 +6,7 @@ #pragma once +#include #include "NonlinearFactor.h" namespace gtsam { @@ -16,10 +17,74 @@ namespace gtsam { * Throws an error at linearization if the constraints are not met. */ template -class NonlinearEquality : public NonlinearFactor{ +class NonlinearEquality : public NonlinearFactor { +private: + + // node to constrain + std::string key_; + + // config containing the necessary feasible point + Config feasible_; + + // dimension of the variable + size_t dim_; + public: - NonlinearEquality(); - virtual ~NonlinearEquality(); + + /** + * 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) { + + } + + 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 VectorConfig& c) const { + if (!compare_(key_, feasible_, c)) + return repeat(dim_, 1.0/0.0); // 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 VectorConfig& 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; + } + } }; } diff --git a/cpp/testNonlinearEquality.cpp b/cpp/testNonlinearEquality.cpp index 7aaa5efbe..7f0a81bf2 100644 --- a/cpp/testNonlinearEquality.cpp +++ b/cpp/testNonlinearEquality.cpp @@ -3,14 +3,100 @@ * @author Alex Cunningham */ - #include +#include "VectorConfig.h" #include "NonlinearEquality.h" -TEST ( NonlinearEquality, construction ) { +using namespace std; +using namespace gtsam; + +typedef boost::shared_ptr > shared_nle; + +bool vector_compare(const std::string& key, + const VectorConfig& feasible, + const VectorConfig& input) { + Vector feas, lin; + feas = feasible[key]; + lin = input[key]; + return equal_with_abs_tol(lin, feas, 1e-5); +} + +TEST ( NonlinearEquality, linearization ) { + string key = "x"; + Vector value = Vector_(2, 1.0, 2.0); + VectorConfig feasible, linearize; + feasible.insert(key, value); + linearize.insert(key, value); + + // create a nonlinear equality constraint + shared_nle nle(new NonlinearEquality(key, feasible, 2, *vector_compare)); + + // check linearize + GaussianFactor expLF(key, eye(2), zero(2), 0.0); + GaussianFactor::shared_ptr actualLF = nle->linearize(linearize); + CHECK(assert_equal(*actualLF, expLF)); +} + +TEST ( NonlinearEquality, linearization_fail ) { + string key = "x"; + Vector value = Vector_(2, 1.0, 2.0); + Vector wrong = Vector_(2, 3.0, 4.0); + VectorConfig feasible, bad_linearize; + feasible.insert(key, value); + bad_linearize.insert(key, wrong); + + // create a nonlinear equality constraint + shared_nle nle(new NonlinearEquality(key, feasible, 2, *vector_compare)); + + // check linearize to ensure that it fails for bad linearization points + try { + GaussianFactor::shared_ptr actualLF = nle->linearize(bad_linearize); + CHECK(false); + } catch (std::invalid_argument) { + CHECK(true); + } +} + +TEST ( NonlinearEquality, error ) { + string key = "x"; + Vector value = Vector_(2, 1.0, 2.0); + Vector wrong = Vector_(2, 3.0, 4.0); + VectorConfig feasible, bad_linearize; + feasible.insert(key, value); + bad_linearize.insert(key, wrong); + + // create a nonlinear equality constraint + shared_nle nle(new NonlinearEquality(key, feasible, 2, *vector_compare)); + + // check error function outputs + Vector actual = nle->error_vector(feasible); + CHECK(assert_equal(actual, zero(2))); + + actual = nle->error_vector(bad_linearize); + CHECK(assert_equal(actual, repeat(2, 1.0/0.0))); +} + +TEST ( NonlinearEquality, equals ) { + string key1 = "x"; + Vector value1 = Vector_(2, 1.0, 2.0); + Vector value2 = Vector_(2, 3.0, 4.0); + VectorConfig feasible1, feasible2; + feasible1.insert(key1, value1); + feasible2.insert(key1, value2); + + // create some constraints to compare + shared_nle nle1(new NonlinearEquality(key1, feasible1, 2, *vector_compare)); + shared_nle nle2(new NonlinearEquality(key1, feasible1, 2, *vector_compare)); + shared_nle nle3(new NonlinearEquality(key1, feasible2, 2, *vector_compare)); + + // verify + CHECK(nle1->equals(*nle2)); // basic equality = true + CHECK(nle2->equals(*nle1)); // test symmetry of equals() + CHECK(!nle1->equals(*nle3)); // test config } + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */