From cd3e3d8a862af79f53e5a7d915b64cd9cc2d5978 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 22 Oct 2009 17:23:24 +0000 Subject: [PATCH] Fairly substantial change: Factor now Testable, MutableLinearFactor gone The latter was prompted by the fact that assert_equal did not like mixing LinearFactor and MutableLinearFactor But MutableLinearFactor always was a bit of a kluge. We should eradicate all non-const on LinearFactor some other way. --- cpp/ConstrainedLinearFactorGraph.cpp | 2 +- cpp/Factor.h | 10 +- cpp/LinearFactor.cpp | 110 +++++++++----------- cpp/LinearFactor.h | 71 ++++--------- cpp/LinearFactorGraph.cpp | 8 +- cpp/LinearFactorGraph.h | 2 +- cpp/NonlinearFactor.cpp | 6 +- cpp/NonlinearFactor.h | 36 ++++--- cpp/Testable.h | 1 + cpp/testConstrainedLinearFactorGraph.cpp | 2 +- cpp/testLinearFactor.cpp | 125 ++++++++++++----------- 11 files changed, 172 insertions(+), 201 deletions(-) diff --git a/cpp/ConstrainedLinearFactorGraph.cpp b/cpp/ConstrainedLinearFactorGraph.cpp index 9df6bdf8b..62b7b1254 100644 --- a/cpp/ConstrainedLinearFactorGraph.cpp +++ b/cpp/ConstrainedLinearFactorGraph.cpp @@ -138,7 +138,7 @@ ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::elimina } // construct the updated factor - boost::shared_ptr new_factor(new MutableLinearFactor); + boost::shared_ptr new_factor(new LinearFactor); string cur_key; Matrix M; FOREACH_PAIR(cur_key, M, blocks) { new_factor->insert(cur_key, M); diff --git a/cpp/Factor.h b/cpp/Factor.h index 440e4b1a4..35aa9e084 100644 --- a/cpp/Factor.h +++ b/cpp/Factor.h @@ -3,6 +3,7 @@ * @brief A simple factor class to use in a factor graph * @brief factor * @author Kai Ni + * @author Frank Dellaert */ // \callgraph @@ -12,6 +13,7 @@ #include #include #include // for noncopyable +#include "Testable.h" namespace gtsam { @@ -46,7 +48,7 @@ namespace gtsam { * provide the appropriate values at the appropriate time. */ template - class Factor : boost::noncopyable + class Factor : boost::noncopyable, public Testable< Factor > { public: @@ -57,12 +59,6 @@ namespace gtsam { */ virtual double error(const Config& c) const = 0; - /** - * print - * @param s optional string naming the factor - */ - virtual void print(const std::string& s="") const = 0; - /** * equality up to tolerance * tricky to implement, see NonLinearFactor1 for an example diff --git a/cpp/LinearFactor.cpp b/cpp/LinearFactor.cpp index 20a3b4bd1..52bc9a450 100644 --- a/cpp/LinearFactor.cpp +++ b/cpp/LinearFactor.cpp @@ -21,14 +21,29 @@ using namespace gtsam; typedef pair& mypair; /* ************************************************************************* */ -// we might have multiple As, so iterate and subtract from b -double LinearFactor::error(const VectorConfig& c) const { - if (empty()) return 0; - Vector e = b; - string j; Matrix Aj; - FOREACH_PAIR(j, Aj, As) - e -= Vector(Aj * c[j]); - return 0.5 * inner_prod(trans(e),e); +LinearFactor::LinearFactor(const vector & factors) +{ + // Create RHS vector of the right size by adding together row counts + size_t m = 0; + BOOST_FOREACH(shared_ptr factor, factors) m += factor->numberOfRows(); + b = Vector(m); + + size_t pos = 0; // save last position inserted into the new rhs vector + + // iterate over all factors + BOOST_FOREACH(shared_ptr factor, factors){ + // number of rows for factor f + const size_t mf = factor->numberOfRows(); + + // copy the rhs vector from factor to b + const Vector bf = factor->get_b(); + for (size_t i=0; i& f, double tol) const { - //const LinearFactor* lf = dynamic_cast(&f); - //if (lf == NULL) return false; + const LinearFactor* lf = dynamic_cast(&f); + if (lf == NULL) return false; - if (empty()) return (lf.empty()); + if (empty()) return (lf->empty()); - const_iterator it1 = As.begin(), it2 = lf.As.begin(); - if(As.size() != lf.As.size()) goto fail; + const_iterator it1 = As.begin(), it2 = lf->As.begin(); + if(As.size() != lf->As.size()) return false; - for(; it1 != As.end(); it1++, it2++){ + for(; it1 != As.end(); it1++, it2++) { const string& j1 = it1->first, j2 = it2->first; const Matrix A1 = it1->second, A2 = it2->second; - if (j1 != j2) goto fail; - if (!equal_with_abs_tol(A1,A2,tol)) { - cout << "A1[" << j1 << "] != A2[" << j2 << "]" << endl; - goto fail; - } + if (j1 != j2) return false; + if (!equal_with_abs_tol(A1,A2,tol)) + return false; } - if( !(::equal_with_abs_tol(b, (lf.b),tol)) ) { - cout << "RHS disagree" << endl; - goto fail; - } - return true; - fail: - // they don't match, print out and fail - print(); - lf.print(); - return false; + if( !(::equal_with_abs_tol(b, (lf->b),tol)) ) + return false; + + return true; +} + +/* ************************************************************************* */ +// we might have multiple As, so iterate and subtract from b +double LinearFactor::error(const VectorConfig& c) const { + if (empty()) return 0; + Vector e = b; + string j; Matrix Aj; + FOREACH_PAIR(j, Aj, As) + e -= Vector(Aj * c[j]); + return 0.5 * inner_prod(trans(e),e); } /* ************************************************************************* */ @@ -119,7 +137,7 @@ pair LinearFactor::matrix(const Ordering& ordering) const { } /* ************************************************************************* */ -void MutableLinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m, const size_t pos) +void LinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m, const size_t pos) { // iterate over all matrices from the factor f LinearFactor::const_iterator it = f->begin(); @@ -148,32 +166,6 @@ void MutableLinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t } } -/* ************************************************************************* */ -MutableLinearFactor::MutableLinearFactor(const vector & factors) -{ - // Create RHS vector of the right size by adding together row counts - size_t m = 0; - BOOST_FOREACH(shared_ptr factor, factors) m += factor->numberOfRows(); - b = Vector(m); - - size_t pos = 0; // save last position inserted into the new rhs vector - - // iterate over all factors - BOOST_FOREACH(shared_ptr factor, factors){ - // number of rows for factor f - const size_t mf = factor->numberOfRows(); - - // copy the rhs vector from factor to b - const Vector bf = factor->get_b(); - for (size_t i=0; i & factors) */ /* ************************************************************************* */ pair -MutableLinearFactor::eliminate(const string& key) +LinearFactor::eliminate(const string& key) { // start empty remaining factor to be returned - boost::shared_ptr lf(new MutableLinearFactor); + boost::shared_ptr lf(new LinearFactor); // find the matrix associated with key iterator it = As.find(key); diff --git a/cpp/LinearFactor.h b/cpp/LinearFactor.h index cd1ecbc49..7aa96a202 100644 --- a/cpp/LinearFactor.h +++ b/cpp/LinearFactor.h @@ -26,8 +26,6 @@ namespace gtsam { -class MutableLinearFactor; - /** * Base Class for a linear factor. * LinearFactor is non-mutable (all methods const!). @@ -45,11 +43,12 @@ protected: std::map As; // linear matrices Vector b; // right-hand-side +public: + + // TODO: eradicate, as implies non-const LinearFactor() { } -public: - /** Construct Null factor */ CONSTRUCTOR LinearFactor(const Vector& b_in) : @@ -106,11 +105,21 @@ public: } } + /** + * Constructor that combines a set of factors + * @param factors Set of factors to combine + */ + CONSTRUCTOR + LinearFactor(const std::vector & factors); + + // Implementing Testable virtual functions + + void print(const std::string& s = "") const; + bool equals(const Factor& lf, double tol = 1e-9) const; + // Implementing Factor virtual functions double error(const VectorConfig& c) const; /** 0.5*(A*x-b)'*(A*x-b) */ - void print(const std::string& s = "") const; - bool equals(const LinearFactor& lf, double tol = 1e-9) const; std::string dump() const { return "";} std::size_t size() const { return As.size();} @@ -180,49 +189,9 @@ public: */ std::pair matrix(const Ordering& ordering) const; -}; // LinearFactor - -/* ************************************************************************* */ - -/** - * Mutable subclass of LinearFactor - * To isolate bugs - */ -class MutableLinearFactor: public LinearFactor { -public: - - CONSTRUCTOR - MutableLinearFactor() { - } - - /** - * Constructor that combines a set of factors - * @param factors Set of factors to combine - */ - CONSTRUCTOR - MutableLinearFactor(const std::vector & factors); - - /** Construct unary mutable factor */ - CONSTRUCTOR - MutableLinearFactor(const std::string& key1, const Matrix& A1, - const Vector& b_in) : - LinearFactor(key1, A1, b_in) { - } - - /** Construct binary mutable factor */ - CONSTRUCTOR - MutableLinearFactor(const std::string& key1, const Matrix& A1, - const std::string& key2, const Matrix& A2, const Vector& b_in) : - LinearFactor(key1, A1, key2, A2, b_in) { - } - - /** Construct ternary mutable factor */ - CONSTRUCTOR - MutableLinearFactor(const std::string& key1, const Matrix& A1, - const std::string& key2, const Matrix& A2, const std::string& key3, - const Matrix& A3, const Vector& b_in) : - LinearFactor(key1, A1, key2, A2, key3, A3, b_in) { - } + /* ************************************************************************* */ + // MUTABLE functions. FD:on the path to being eradicated + /* ************************************************************************* */ /** insert, copies A */ void insert(const std::string& key, const Matrix& A) { @@ -255,6 +224,8 @@ public: void append_factor(LinearFactor::shared_ptr f, const size_t m, const size_t pos); -}; +}; // LinearFactor + +/* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp index d884a1624..0d04ba5c5 100644 --- a/cpp/LinearFactorGraph.cpp +++ b/cpp/LinearFactorGraph.cpp @@ -82,11 +82,11 @@ LinearFactorGraph::find_factors_and_remove(const string& key) /* ************************************************************************* */ /* find factors and remove them from the factor graph: O(n) */ /* ************************************************************************* */ -boost::shared_ptr +boost::shared_ptr LinearFactorGraph::combine_factors(const string& key) { LinearFactorSet found = find_factors_and_remove(key); - boost::shared_ptr lf(new MutableLinearFactor(found)); + boost::shared_ptr lf(new LinearFactor(found)); return lf; } @@ -97,7 +97,7 @@ ConditionalGaussian::shared_ptr LinearFactorGraph::eliminate_one(const string& k { // combine the factors of all nodes connected to the variable to be eliminated // if no factors are connected to key, returns an empty factor - boost::shared_ptr joint_factor = combine_factors(key); + boost::shared_ptr joint_factor = combine_factors(key); // eliminate that joint factor try { @@ -235,7 +235,7 @@ pair LinearFactorGraph::matrix(const Ordering& ordering) const { found.push_back(factor); // combine them - MutableLinearFactor lf(found); + LinearFactor lf(found); // Return Matrix and Vector return lf.matrix(ordering); diff --git a/cpp/LinearFactorGraph.h b/cpp/LinearFactorGraph.h index 7aa244f06..24bf09347 100644 --- a/cpp/LinearFactorGraph.h +++ b/cpp/LinearFactorGraph.h @@ -73,7 +73,7 @@ namespace gtsam { * @param key the key for the given node * @return the combined linear factor */ - boost::shared_ptr + boost::shared_ptr combine_factors(const std::string& key); /** diff --git a/cpp/NonlinearFactor.cpp b/cpp/NonlinearFactor.cpp index f041ae536..76d9b372d 100644 --- a/cpp/NonlinearFactor.cpp +++ b/cpp/NonlinearFactor.cpp @@ -27,6 +27,7 @@ NonlinearFactor1::NonlinearFactor1(const Vector& z, /* ************************************************************************* */ void NonlinearFactor1::print(const string& s) const { cout << "NonLinearFactor1 " << s << endl; + NonlinearFactor::print(s); } /* ************************************************************************* */ @@ -47,7 +48,7 @@ LinearFactor::shared_ptr NonlinearFactor1::linearize(const VectorConfig& c) cons /* ************************************************************************* */ /** http://artis.imag.fr/~Xavier.Decoret/resources/C++/operator==.html */ /* ************************************************************************* */ -bool NonlinearFactor1::equals(const NonlinearFactor& f, double tol) const { +bool NonlinearFactor1::equals(const Factor& f, double tol) const { const NonlinearFactor1* p = dynamic_cast (&f); if (p == NULL) return false; return NonlinearFactor::equals(*p, tol) @@ -74,6 +75,7 @@ NonlinearFactor2::NonlinearFactor2(const Vector& z, /* ************************************************************************* */ void NonlinearFactor2::print(const string& s) const { cout << "NonLinearFactor2 " << s << endl; + NonlinearFactor::print(s); } /* ************************************************************************* */ @@ -94,7 +96,7 @@ LinearFactor::shared_ptr NonlinearFactor2::linearize(const VectorConfig& c) cons } /* ************************************************************************* */ -bool NonlinearFactor2::equals(const NonlinearFactor& f, double tol) const { +bool NonlinearFactor2::equals(const Factor& f, double tol) const { const NonlinearFactor2* p = dynamic_cast (&f); if (p == NULL) return false; return NonlinearFactor::equals(*p, tol) diff --git a/cpp/NonlinearFactor.h b/cpp/NonlinearFactor.h index 98ad62dd9..4f52ff1d6 100644 --- a/cpp/NonlinearFactor.h +++ b/cpp/NonlinearFactor.h @@ -18,6 +18,7 @@ #include #include "Factor.h" +#include "Vector.h" #include "Matrix.h" #include "LinearFactor.h" @@ -63,15 +64,26 @@ namespace gtsam { sigma_ = sigma; } + /** print */ + void print(const std::string& s = "") const { + std::cout << "NonLinearFactor " << s << std::endl; + gtsam::print(z_, " z = "); + std::cout << " sigma = " << sigma_ << std::endl; + } + + /** Check if two NonlinearFactor objects are equal */ + bool equals(const Factor& f, double tol) const { + const NonlinearFactor* p = dynamic_cast*> (&f); + if (p == NULL) return false; + return equal_with_abs_tol(z_,p->z_,tol) && fabs(sigma_ - p->sigma_)<=tol; + } + /** Vector of errors */ virtual Vector error_vector(const Config& c) const = 0; /** linearize to a LinearFactor */ virtual boost::shared_ptr linearize(const Config& c) const = 0; - /** print to cout */ - virtual void print(const std::string& s = "") const = 0; - /** get functions */ double sigma() const {return sigma_;} Vector measurement() const {return z_;} @@ -86,11 +98,6 @@ namespace gtsam { /** get the size of the factor */ std::size_t size() const{return keys_.size();} - /** Check if two NonlinearFactor objects are equal */ - bool equals(const NonlinearFactor& other, double tol=1e-9) const { - return equal_with_abs_tol(z_,other.z_,tol) && fabs(sigma_ - other.sigma_)<=tol; - } - /** dump the information of the factor into a string **/ std::string dump() const{return "";} @@ -132,6 +139,9 @@ namespace gtsam { void print(const std::string& s = "") const; + /** Check if two factors are equal */ + bool equals(const Factor& f, double tol=1e-9) const; + /** error function */ inline Vector error_vector(const VectorConfig& c) const { return z_ - h_(c[key1_]); @@ -140,9 +150,6 @@ namespace gtsam { /** linearize a non-linearFactor1 to get a linearFactor1 */ boost::shared_ptr linearize(const VectorConfig& c) const; - /** Check if two factors are equal */ - bool equals(const NonlinearFactor& f, double tol=1e-9) const; - std::string dump() const {return "";} }; @@ -173,8 +180,12 @@ namespace gtsam { const std::string& key2, // key of the second variable Matrix (*H2)(const Vector&, const Vector&));// derivative of h in second variable + /** Print */ void print(const std::string& s = "") const; + /** Check if two factors are equal */ + bool equals(const Factor& f, double tol=1e-9) const; + /** error function */ inline Vector error_vector(const VectorConfig& c) const { return z_ - h_(c[key1_], c[key2_]); @@ -183,9 +194,6 @@ namespace gtsam { /** Linearize a non-linearFactor2 to get a linearFactor2 */ boost::shared_ptr linearize(const VectorConfig& c) const; - /** Check if two factors are equal */ - bool equals(const NonlinearFactor& f, double tol=1e-9) const; - std::string dump() const{return "";}; }; diff --git a/cpp/Testable.h b/cpp/Testable.h index e03e05db3..f79d31d55 100644 --- a/cpp/Testable.h +++ b/cpp/Testable.h @@ -30,6 +30,7 @@ namespace gtsam { /** * equality up to tolerance * tricky to implement, see NonLinearFactor1 for an example + * equals is not supposed to print out *anything*, just return true|false */ virtual bool equals(const Derived& expected, double tol) const = 0; diff --git a/cpp/testConstrainedLinearFactorGraph.cpp b/cpp/testConstrainedLinearFactorGraph.cpp index 14289faa1..319750eab 100644 --- a/cpp/testConstrainedLinearFactorGraph.cpp +++ b/cpp/testConstrainedLinearFactorGraph.cpp @@ -493,7 +493,7 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint ) // // make a joint linear factor // set f1_set; // f1_set.insert(f1); -// boost::shared_ptr joined(new MutableLinearFactor(f1_set)); +// boost::shared_ptr joined(new LinearFactor(f1_set)); // // // create a sample graph // ConstrainedLinearFactorGraph graph; diff --git a/cpp/testLinearFactor.cpp b/cpp/testLinearFactor.cpp index 5bfd4dd23..ced65e7d7 100644 --- a/cpp/testLinearFactor.cpp +++ b/cpp/testLinearFactor.cpp @@ -2,11 +2,10 @@ * @file testLinearFactor.cpp * @brief Unit tests for Linear Factor * @author Christian Potthast + * @author Frank Dellaert **/ -/*STL/C++*/ #include -using namespace std; #include #include @@ -14,6 +13,7 @@ using namespace std; #include "Matrix.h" #include "smallExample.h" +using namespace std; using namespace gtsam; /* ************************************************************************* */ @@ -39,7 +39,7 @@ TEST( LinearFactor, linearFactor ) LinearFactor::shared_ptr lf = fg[1]; // check if the two factors are the same - CHECK( lf->equals(expected) ); + CHECK(assert_equal(expected,*lf)); } /* ************************************************************************* */ @@ -75,7 +75,7 @@ TEST( LinearFactor, linearFactor2 ) lfg.push_back(fg[2 - 1]); // combine in a factor - MutableLinearFactor combined(lfg); + LinearFactor combined(lfg); // the expected combined linear factor Matrix Ax2 = Matrix_(4, 2, // x2 @@ -105,7 +105,56 @@ TEST( LinearFactor, linearFactor2 ) b2(3) = -1; LinearFactor expected("x2", Ax2, "l1", Al1, "x1", Ax1, b2); - CHECK(combined.equals(expected)); + CHECK(assert_equal(expected,combined)); +} + +/* ************************************************************************* */ +TEST( NonlinearFactorGraph, linearFactor3){ + + Matrix A11(2,2); + A11(0,0) = 10.4545; A11(0,1) = 0; + A11(1,0) = 0; A11(1,1) = 10.4545; + Vector b(2); + b(0) = 2 ; b(1) = -1; + LinearFactor::shared_ptr f1(new LinearFactor("x1", A11, b)); + + A11(0,0) = 2; A11(0,1) = 0; + A11(1,0) = 0; A11(1,1) = -2; + b(0) = 4 ; b(1) = -5; + LinearFactor::shared_ptr f2(new LinearFactor("x1", A11, b)); + + A11(0,0) = 4; A11(0,1) = 0; + A11(1,0) = 0; A11(1,1) = -4; + b(0) = 3 ; b(1) = -88; + LinearFactor::shared_ptr f3(new LinearFactor("x1", A11, b)); + + A11(0,0) = 6; A11(0,1) = 0; + A11(1,0) = 0; A11(1,1) = 7; + b(0) = 5 ; b(1) = -6; + LinearFactor::shared_ptr f4(new LinearFactor("x1", A11, b)); + + vector lfg; + lfg.push_back(f1); + lfg.push_back(f2); + lfg.push_back(f3); + lfg.push_back(f4); + LinearFactor combined(lfg); + + Matrix A22(8,2); + A22(0,0) = 10.4545; A22(0,1) = 0; + A22(1,0) = 0; A22(1,1) = 10.4545; + A22(2,0) = 2; A22(2,1) = 0; + A22(3,0) = 0; A22(3,1) = -2; + A22(4,0) = 4; A22(4,1) = 0; + A22(5,0) = 0; A22(5,1) = -4; + A22(6,0) = 6; A22(6,1) = 0; + A22(7,0) = 0; A22(7,1) = 7; + Vector exb(8); + exb(0) = 2 ; exb(1) = -1; exb(2) = 4 ; exb(3) = -5; + exb(4) = 3 ; exb(5) = -88; exb(6) = 5 ; exb(7) = -6; + LinearFactor expected("x1", A22, exb); + + CHECK(assert_equal(expected,combined)); } /* ************************************************************************* */ @@ -141,7 +190,7 @@ TEST( LinearFactor, linearFactorN){ Vector_(2, 2.0, -1.0)))); - MutableLinearFactor combined(f); + LinearFactor combined(f); vector > meas; meas.push_back(make_pair("x1", Matrix_(8,2, @@ -184,55 +233,7 @@ TEST( LinearFactor, linearFactorN){ 10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0); LinearFactor expected(meas, b); - CHECK(combined.equals(expected)); -} - -TEST( NonlinearFactorGraph, linearFactor3){ - - Matrix A11(2,2); - A11(0,0) = 10.4545; A11(0,1) = 0; - A11(1,0) = 0; A11(1,1) = 10.4545; - Vector b(2); - b(0) = 2 ; b(1) = -1; - LinearFactor::shared_ptr f1(new LinearFactor("x1", A11, b)); - - A11(0,0) = 2; A11(0,1) = 0; - A11(1,0) = 0; A11(1,1) = -2; - b(0) = 4 ; b(1) = -5; - LinearFactor::shared_ptr f2(new LinearFactor("x1", A11, b)); - - A11(0,0) = 4; A11(0,1) = 0; - A11(1,0) = 0; A11(1,1) = -4; - b(0) = 3 ; b(1) = -88; - LinearFactor::shared_ptr f3(new LinearFactor("x1", A11, b)); - - A11(0,0) = 6; A11(0,1) = 0; - A11(1,0) = 0; A11(1,1) = 7; - b(0) = 5 ; b(1) = -6; - LinearFactor::shared_ptr f4(new LinearFactor("x1", A11, b)); - - vector lfg; - lfg.push_back(f1); - lfg.push_back(f2); - lfg.push_back(f3); - lfg.push_back(f4); - MutableLinearFactor combined(lfg); - - Matrix A22(8,2); - A22(0,0) = 10.4545; A22(0,1) = 0; - A22(1,0) = 0; A22(1,1) = 10.4545; - A22(2,0) = 2; A22(2,1) = 0; - A22(3,0) = 0; A22(3,1) = -2; - A22(4,0) = 4; A22(4,1) = 0; - A22(5,0) = 0; A22(5,1) = -4; - A22(6,0) = 6; A22(6,1) = 0; - A22(7,0) = 0; A22(7,1) = 7; - Vector exb(8); - exb(0) = 2 ; exb(1) = -1; exb(2) = 4 ; exb(3) = -5; - exb(4) = 3 ; exb(5) = -88; exb(6) = 5 ; exb(7) = -6; - LinearFactor::shared_ptr expected(new LinearFactor("x1", A22, exb)); - - CHECK( combined.equals(*expected) ); // currently fails on linux, see previous test's note + CHECK(assert_equal(expected,combined)); } /* ************************************************************************* */ @@ -288,7 +289,7 @@ TEST( LinearFactor, eliminate ) b2(2) = 2; b2(3) = -1; - MutableLinearFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2); + LinearFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2); // eliminate the combined factor @@ -363,7 +364,7 @@ TEST( LinearFactor, eliminate2 ) b2(2) = 2; b2(3) = -1; - MutableLinearFactor combined("x2", Ax2, "l1x1", Al1x1, b2); + LinearFactor combined("x2", Ax2, "l1x1", Al1x1, b2); // eliminate the combined factor ConditionalGaussian::shared_ptr actualCG; @@ -402,7 +403,7 @@ TEST( LinearFactor, eliminate2 ) //* ************************************************************************* */ TEST( LinearFactor, default_error ) { - MutableLinearFactor f; + LinearFactor f; VectorConfig c; double actual = f.error(c); CHECK(actual==0.0); @@ -412,7 +413,7 @@ TEST( LinearFactor, default_error ) TEST( LinearFactor, eliminate_empty ) { // create an empty factor - MutableLinearFactor f; + LinearFactor f; // eliminate the empty factor ConditionalGaussian::shared_ptr actualCG; @@ -423,7 +424,7 @@ TEST( LinearFactor, eliminate_empty ) ConditionalGaussian expectedCG; // expected remaining factor is still empty :-) - MutableLinearFactor expectedLF; + LinearFactor expectedLF; // check if the result matches CHECK(actualCG->equals(expectedCG)); @@ -434,7 +435,7 @@ TEST( LinearFactor, eliminate_empty ) TEST( LinearFactor, empty ) { // create an empty factor - MutableLinearFactor f; + LinearFactor f; CHECK(f.empty()==true); } @@ -497,7 +498,7 @@ TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian ) // actualLF.print(); LinearFactor expectedLF("x2",R11,"l1x1",S12,d); - CHECK(actualLF.equals(expectedLF)); + CHECK(assert_equal(expectedLF,actualLF)); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);}