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.release/4.3a0
Frank Dellaert
parent
0d580032c5
commit
cd3e3d8a86
|
|
@ -138,7 +138,7 @@ ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::elimina
|
|||
}
|
||||
|
||||
// construct the updated factor
|
||||
boost::shared_ptr<MutableLinearFactor> new_factor(new MutableLinearFactor);
|
||||
boost::shared_ptr<LinearFactor> new_factor(new LinearFactor);
|
||||
string cur_key; Matrix M;
|
||||
FOREACH_PAIR(cur_key, M, blocks) {
|
||||
new_factor->insert(cur_key, M);
|
||||
|
|
|
|||
10
cpp/Factor.h
10
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 <set>
|
||||
#include <list>
|
||||
#include <boost/utility.hpp> // for noncopyable
|
||||
#include "Testable.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -46,7 +48,7 @@ namespace gtsam {
|
|||
* provide the appropriate values at the appropriate time.
|
||||
*/
|
||||
template <class Config>
|
||||
class Factor : boost::noncopyable
|
||||
class Factor : boost::noncopyable, public Testable< Factor<Config> >
|
||||
{
|
||||
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
|
||||
|
|
|
|||
|
|
@ -21,14 +21,29 @@ using namespace gtsam;
|
|||
typedef pair<const string, Matrix>& 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<shared_ptr> & 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<mf; i++) b(pos+i) = bf(i);
|
||||
|
||||
// update the matrices
|
||||
append_factor(factor,m,pos);
|
||||
|
||||
pos += mf;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -44,36 +59,39 @@ void LinearFactor::print(const string& s) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Check if two linear factors are equal
|
||||
bool LinearFactor::equals(const LinearFactor& lf, double tol) const {
|
||||
bool LinearFactor::equals(const Factor<VectorConfig>& f, double tol) const {
|
||||
|
||||
//const LinearFactor* lf = dynamic_cast<const LinearFactor*>(&f);
|
||||
//if (lf == NULL) return false;
|
||||
const LinearFactor* lf = dynamic_cast<const LinearFactor*>(&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<Matrix,Vector> 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<shared_ptr> & 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<mf; i++) b(pos+i) = bf(i);
|
||||
|
||||
// update the matrices
|
||||
append_factor(factor,m,pos);
|
||||
|
||||
pos += mf;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* Note, in place !!!!
|
||||
* Do incomplete QR factorization for the first n columns
|
||||
|
|
@ -183,10 +175,10 @@ MutableLinearFactor::MutableLinearFactor(const vector<shared_ptr> & factors)
|
|||
*/
|
||||
/* ************************************************************************* */
|
||||
pair<ConditionalGaussian::shared_ptr, LinearFactor::shared_ptr>
|
||||
MutableLinearFactor::eliminate(const string& key)
|
||||
LinearFactor::eliminate(const string& key)
|
||||
{
|
||||
// start empty remaining factor to be returned
|
||||
boost::shared_ptr<MutableLinearFactor> lf(new MutableLinearFactor);
|
||||
boost::shared_ptr<LinearFactor> lf(new LinearFactor);
|
||||
|
||||
// find the matrix associated with key
|
||||
iterator it = As.find(key);
|
||||
|
|
|
|||
|
|
@ -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<std::string, Matrix> 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<shared_ptr> & factors);
|
||||
|
||||
// Implementing Testable virtual functions
|
||||
|
||||
void print(const std::string& s = "") const;
|
||||
bool equals(const Factor<VectorConfig>& 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, Vector> 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<shared_ptr> & 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
|
||||
|
|
|
|||
|
|
@ -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<MutableLinearFactor>
|
||||
boost::shared_ptr<LinearFactor>
|
||||
LinearFactorGraph::combine_factors(const string& key)
|
||||
{
|
||||
LinearFactorSet found = find_factors_and_remove(key);
|
||||
boost::shared_ptr<MutableLinearFactor> lf(new MutableLinearFactor(found));
|
||||
boost::shared_ptr<LinearFactor> 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<MutableLinearFactor> joint_factor = combine_factors(key);
|
||||
boost::shared_ptr<LinearFactor> joint_factor = combine_factors(key);
|
||||
|
||||
// eliminate that joint factor
|
||||
try {
|
||||
|
|
@ -235,7 +235,7 @@ pair<Matrix,Vector> 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);
|
||||
|
|
|
|||
|
|
@ -73,7 +73,7 @@ namespace gtsam {
|
|||
* @param key the key for the given node
|
||||
* @return the combined linear factor
|
||||
*/
|
||||
boost::shared_ptr<MutableLinearFactor>
|
||||
boost::shared_ptr<LinearFactor>
|
||||
combine_factors(const std::string& key);
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@ NonlinearFactor1::NonlinearFactor1(const Vector& z,
|
|||
/* ************************************************************************* */
|
||||
void NonlinearFactor1::print(const string& s) const {
|
||||
cout << "NonLinearFactor1 " << s << endl;
|
||||
NonlinearFactor<VectorConfig>::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<VectorConfig>& f, double tol) const {
|
||||
bool NonlinearFactor1::equals(const Factor<VectorConfig>& f, double tol) const {
|
||||
const NonlinearFactor1* p = dynamic_cast<const NonlinearFactor1*> (&f);
|
||||
if (p == NULL) return false;
|
||||
return NonlinearFactor<VectorConfig>::equals(*p, tol)
|
||||
|
|
@ -74,6 +75,7 @@ NonlinearFactor2::NonlinearFactor2(const Vector& z,
|
|||
/* ************************************************************************* */
|
||||
void NonlinearFactor2::print(const string& s) const {
|
||||
cout << "NonLinearFactor2 " << s << endl;
|
||||
NonlinearFactor<VectorConfig>::print(s);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -94,7 +96,7 @@ LinearFactor::shared_ptr NonlinearFactor2::linearize(const VectorConfig& c) cons
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool NonlinearFactor2::equals(const NonlinearFactor<VectorConfig>& f, double tol) const {
|
||||
bool NonlinearFactor2::equals(const Factor<VectorConfig>& f, double tol) const {
|
||||
const NonlinearFactor2* p = dynamic_cast<const NonlinearFactor2*> (&f);
|
||||
if (p == NULL) return false;
|
||||
return NonlinearFactor<VectorConfig>::equals(*p, tol)
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
#include <boost/serialization/list.hpp>
|
||||
|
||||
#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<Config>& f, double tol) const {
|
||||
const NonlinearFactor<Config>* p = dynamic_cast<const NonlinearFactor<Config>*> (&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<LinearFactor> 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<VectorConfig>& 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<LinearFactor> linearize(const VectorConfig& c) const;
|
||||
|
||||
/** Check if two factors are equal */
|
||||
bool equals(const NonlinearFactor<VectorConfig>& 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<VectorConfig>& 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<LinearFactor> linearize(const VectorConfig& c) const;
|
||||
|
||||
/** Check if two factors are equal */
|
||||
bool equals(const NonlinearFactor<VectorConfig>& f, double tol=1e-9) const;
|
||||
|
||||
std::string dump() const{return "";};
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -493,7 +493,7 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
|
|||
// // make a joint linear factor
|
||||
// set<LinearFactor::shared_ptr> f1_set;
|
||||
// f1_set.insert(f1);
|
||||
// boost::shared_ptr<MutableLinearFactor> joined(new MutableLinearFactor(f1_set));
|
||||
// boost::shared_ptr<LinearFactor> joined(new LinearFactor(f1_set));
|
||||
//
|
||||
// // create a sample graph
|
||||
// ConstrainedLinearFactorGraph graph;
|
||||
|
|
|
|||
|
|
@ -2,11 +2,10 @@
|
|||
* @file testLinearFactor.cpp
|
||||
* @brief Unit tests for Linear Factor
|
||||
* @author Christian Potthast
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
/*STL/C++*/
|
||||
#include <iostream>
|
||||
using namespace std;
|
||||
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -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<LinearFactor::shared_ptr> 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<pair<string, Matrix> > 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<LinearFactor::shared_ptr> 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);}
|
||||
|
|
|
|||
Loading…
Reference in New Issue