12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed more tests for NonlinearConstraints, inequality constraints now work.

release/4.3a0
Alex Cunningham 2010-02-07 01:18:17 +00:00
parent 814fb949ba
commit ea3b8083b0
5 changed files with 456 additions and 456 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cpp/Matrix.cpp
View File

@ -375,6 +375,25 @@ inline void householder_update_manual(Matrix &A, int j, double beta, const Vecto
} }
void householder_update(Matrix &A, int j, double beta, const Vector& vjm) { void householder_update(Matrix &A, int j, double beta, const Vector& vjm) {
// TODO: SWAP IN ATLAS VERSION OF THE SYSTEM
// // straight atlas version
// const size_t m = A.size1(), n = A.size2(), mj = m-j;
//
// // find pointers to the data
// const double * vptr = vjm.data().begin(); // mj long
// double * Aptr = A.data().begin() + n*j; // mj x n - note that this starts at row j
//
// // first step: get w = beta*trans(A(j:m,:))*vjm
// Vector w(n);
// double * wptr = w.data().begin();
//
// // execute w generation
// cblas_dgemv(CblasRowMajor, CblasTrans, mj, n, beta, Aptr, n, vptr, 1, 0.0, wptr, 1);
//
// // second step: rank 1 update A(j:m,:) = v(j:m)*w' + A(j:m,:)
// cblas_dger(CblasRowMajor, mj, n, 1.0, vptr, 1, wptr, 1, Aptr, n);
#ifdef GSL #ifdef GSL
#ifndef REVERTGSL #ifndef REVERTGSL
const size_t m = A.size1(), n = A.size2(); const size_t m = A.size1(), n = A.size2();

76
cpp/NonlinearConstraint-inl.h
View File

@ -20,21 +20,9 @@ namespace gtsam {
template <class Config> template <class Config>
NonlinearConstraint<Config>::NonlinearConstraint(const LagrangeKey& lagrange_key, NonlinearConstraint<Config>::NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange, size_t dim_lagrange,
Vector (*g)(const Config& config), bool isEquality) :
bool isEquality) NonlinearFactor<Config>(noiseModel::Constrained::All(dim_lagrange)),
: NonlinearFactor<Config>(1.0), lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality) {}
lagrange_key_(lagrange_key), p_(dim_lagrange),
isEquality_(isEquality), g_(boost::bind(g, _1)) {}
/* ************************************************************************* */
template <class Config>
NonlinearConstraint<Config>::NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange,
boost::function<Vector(const Config& config)> g,
bool isEquality)
: NonlinearFactor<Config>(noiseModel::Constrained::All(dim_lagrange)),
lagrange_key_(lagrange_key), p_(dim_lagrange),
g_(g), isEquality_(isEquality) {}
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config> template <class Config>
@ -54,8 +42,8 @@ NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
size_t dim_constraint, size_t dim_constraint,
const LagrangeKey& lagrange_key, const LagrangeKey& lagrange_key,
bool isEquality) : bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
G_(boost::bind(gradG, _1)), key_(key) g_(boost::bind(g, _1)), G_(boost::bind(gradG, _1)), key_(key)
{ {
} }
@ -68,22 +56,21 @@ NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
size_t dim_constraint, size_t dim_constraint,
const LagrangeKey& lagrange_key, const LagrangeKey& lagrange_key,
bool isEquality) : bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
G_(gradG), key_(key) g_(g), G_(gradG), key_(key)
{ {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config, class Key, class X> template <class Config, class Key, class X>
void NonlinearConstraint1<Config, Key, X>::print(const std::string& s) const { void NonlinearConstraint1<Config, Key, X>::print(const std::string& s) const {
std::cout << "NonlinearConstraint1 [" << s << "]:\n"; std::cout << "NonlinearConstraint1 [" << s << "]: Dim: " << this->p_ << "\n"
// << " key: " << key_ << "\n" << " Key : " << (std::string) this->key_ << "\n"
// << " p: " << this->p_ << "\n" << " Lagrange Key: " << (std::string) this->lagrange_key_ << "\n";
// << " lambda key: " << this->lagrange_key_ << "\n"; if (this->isEquality_)
// if (this->isEquality_) std::cout << " Equality Factor" << std::endl;
// std::cout << " Equality Factor" << std::endl; else
// else std::cout << " Inequality Factor" << std::endl;
// std::cout << " Inequality Factor" << std::endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -148,8 +135,8 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
size_t dim_constraint, size_t dim_constraint,
const LagrangeKey& lagrange_key, const LagrangeKey& lagrange_key,
bool isEquality) : bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)), g_(boost::bind(g, _1)), G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)),
key1_(key1), key2_(key2) key1_(key1), key2_(key2)
{ {
} }
@ -165,8 +152,8 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
size_t dim_constraint, size_t dim_constraint,
const LagrangeKey& lagrange_key, const LagrangeKey& lagrange_key,
bool isEquality) : bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
G1_(G1), G2_(G2), g_(g), G1_(G1), G2_(G2),
key1_(key1), key2_(key2) key1_(key1), key2_(key2)
{ {
} }
@ -174,11 +161,14 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config, class Key1, class X1, class Key2, class X2> template <class Config, class Key1, class X1, class Key2, class X2>
void NonlinearConstraint2<Config, Key1, X1, Key2, X2>::print(const std::string& s) const { void NonlinearConstraint2<Config, Key1, X1, Key2, X2>::print(const std::string& s) const {
std::cout << "NonlinearConstraint2 [" << s << "]:\n"; std::cout << "NonlinearConstraint2 [" << s << "]: Dim: " << this->p_ << "\n"
// << " key1: " << key1_ << "\n" << " Key1 : " << (std::string) this->key1_ << "\n"
// << " key2: " << key2_ << "\n" << " Key2 : " << (std::string) this->key2_ << "\n"
// << " p: " << this->p_ << "\n" << " Lagrange Key: " << (std::string) this->lagrange_key_ << "\n";
// << " lambda key: " << this->lagrange_key_ << std::endl; if (this->isEquality_)
std::cout << " Equality Factor" << std::endl;
else
std::cout << " Inequality Factor" << std::endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -198,7 +188,6 @@ template<class Config, class Key1, class X1, class Key2, class X2>
GaussianFactor::shared_ptr GaussianFactor::shared_ptr
NonlinearConstraint2<Config, Key1, X1, Key2, X2>::linearize(const Config& config) const { NonlinearConstraint2<Config, Key1, X1, Key2, X2>::linearize(const Config& config) const {
const size_t p = this->p_; const size_t p = this->p_;
// extract lagrange multiplier // extract lagrange multiplier
Vector lambda = config[this->lagrange_key_]; Vector lambda = config[this->lagrange_key_];
@ -231,19 +220,6 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::linearize(const Config& config
GaussianFactor::shared_ptr factor(new GaussianFactor::shared_ptr factor(new
GaussianFactor(key1_, Ax1, key2_, Ax2, this->lagrange_key_, AL, rhs, mixedConstraint)); GaussianFactor(key1_, Ax1, key2_, Ax2, this->lagrange_key_, AL, rhs, mixedConstraint));
// // construct probabilistic factor
// Matrix A1 = vector_scale(lambda, grad1);
// Matrix A2 = vector_scale(lambda, grad2);
// SharedDiagonal probModel = sharedSigma(this->p_,1.0);
// GaussianFactor::shared_ptr factor(new GaussianFactor(key1_, A1, key2_, A2,
// this->lagrange_key_, eye(this->p_), zero(this->p_), probModel));
//
// // construct the constraint
// SharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_);
// GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1,
// key2_, grad2, -1.0 * g, constraintModel));
return factor; return factor;
} }

47
cpp/NonlinearConstraint.h
View File

@ -40,36 +40,15 @@ protected:
/** type of constraint */ /** type of constraint */
bool isEquality_; bool isEquality_;
/** calculates the constraint function of the current config
* If the value is zero, the constraint is not active
* Use boost.bind to create the function object
* @param config is a configuration of all the variables
* @return the cost for each of p constraints, arranged in a vector
*/
boost::function<Vector(const Config& config)> g_;
public: public:
/** Constructor - sets the cost function and the lagrange multipliers /** Constructor - sets the cost function and the lagrange multipliers
* @param lagrange_key is the label for the associated lagrange multipliers * @param lagrange_key is the label for the associated lagrange multipliers
* @param dim_lagrange is the number of associated constraints * @param dim_lagrange is the number of associated constraints
* @param isEquality is true if the constraint is an equality constraint * @param isEquality is true if the constraint is an equality constraint
* @param g is the cost function for the constraint
*/ */
NonlinearConstraint(const LagrangeKey& lagrange_key, NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange, size_t dim_lagrange,
Vector (*g)(const Config& config),
bool isEquality=true);
/** Constructor - sets a more general cost function using boost::bind directly
* @param lagrange_key is the label for the associated lagrange multipliers
* @param dim_lagrange is the number of associated constraints
* @param g is the cost function for the constraint
* @param isEquality is true if the constraint is an equality constraint
*/
NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange,
boost::function<Vector(const Config& config)> g,
bool isEquality=true); bool isEquality=true);
/** returns the key used for the Lagrange multipliers */ /** returns the key used for the Lagrange multipliers */
@ -88,7 +67,7 @@ public:
virtual bool equals(const Factor<Config>& f, double tol=1e-9) const=0; virtual bool equals(const Factor<Config>& f, double tol=1e-9) const=0;
/** error function - returns the result of the constraint function */ /** error function - returns the result of the constraint function */
inline Vector unwhitenedError(const Config& c) const { return g_(c); } virtual Vector unwhitenedError(const Config& c) const=0;
/** /**
* Determines whether the constraint is active given a particular configuration * Determines whether the constraint is active given a particular configuration
@ -108,6 +87,8 @@ public:
/** /**
* A unary constraint with arbitrary cost and jacobian functions * A unary constraint with arbitrary cost and jacobian functions
* This is an example class designed for easy testing, but real uses should probably
* subclass NonlinearConstraint and implement virtual functions directly
*/ */
template <class Config, class Key, class X> template <class Config, class Key, class X>
class NonlinearConstraint1 : public NonlinearConstraint<Config> { class NonlinearConstraint1 : public NonlinearConstraint<Config> {
@ -123,6 +104,14 @@ private:
*/ */
boost::function<Matrix(const Config& config)> G_; boost::function<Matrix(const Config& config)> G_;
/** calculates the constraint function of the current config
* If the value is zero, the constraint is not active
* Use boost.bind to create the function object
* @param config is a configuration of all the variables
* @return the cost for each of p constraints, arranged in a vector
*/
boost::function<Vector(const Config& config)> g_;
/** key for the constrained variable */ /** key for the constrained variable */
Key key_; Key key_;
@ -168,6 +157,9 @@ public:
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const Factor<Config>& f, double tol=1e-9) const; bool equals(const Factor<Config>& f, double tol=1e-9) const;
/** Error function */
virtual inline Vector unwhitenedError(const Config& c) const { return g_(c); }
/** /**
* Linearize from config - must have Lagrange multipliers * Linearize from config - must have Lagrange multipliers
*/ */
@ -192,6 +184,14 @@ private:
boost::function<Matrix(const Config& config)> G1_; boost::function<Matrix(const Config& config)> G1_;
boost::function<Matrix(const Config& config)> G2_; boost::function<Matrix(const Config& config)> G2_;
/** calculates the constraint function of the current config
* If the value is zero, the constraint is not active
* Use boost.bind to create the function object
* @param config is a configuration of all the variables
* @return the cost for each of p constraints, arranged in a vector
*/
boost::function<Vector(const Config& config)> g_;
/** keys for the constrained variables */ /** keys for the constrained variables */
Key1 key1_; Key1 key1_;
Key2 key2_; Key2 key2_;
@ -243,6 +243,9 @@ public:
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const Factor<Config>& f, double tol=1e-9) const; bool equals(const Factor<Config>& f, double tol=1e-9) const;
/** Error function */
virtual inline Vector unwhitenedError(const Config& c) const { return g_(c); }
/** /**
* Linearize from config - must have Lagrange multipliers * Linearize from config - must have Lagrange multipliers
*/ */

388
cpp/testNonlinearConstraint.cpp
View File

@ -14,14 +14,17 @@
#include <VectorConfig.h> #include <VectorConfig.h>
#include <NonlinearConstraint.h> #include <NonlinearConstraint.h>
#include <NonlinearConstraint-inl.h> #include <NonlinearConstraint-inl.h>
#include <TupleConfig-inl.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace boost::assign; using namespace boost::assign;
typedef TypedSymbol<Vector, 'x'> Key; typedef TypedSymbol<Vector, 'x'> Key;
typedef NonlinearConstraint1<VectorConfig, Key, Vector> NLC1; typedef TupleConfig2< LieConfig<LagrangeKey, Vector>,
typedef NonlinearConstraint2<VectorConfig, Key, Vector, Key, Vector> NLC2; LieConfig<Key, Vector> > VecConfig;
typedef NonlinearConstraint1<VecConfig, Key, Vector> NLC1;
typedef NonlinearConstraint2<VecConfig, Key, Vector, Key, Vector> NLC2;
/* ************************************************************************* */ /* ************************************************************************* */
// unary functions with scalar variables // unary functions with scalar variables
@ -29,13 +32,13 @@ typedef NonlinearConstraint2<VectorConfig, Key, Vector, Key, Vector> NLC2;
namespace test1 { namespace test1 {
/** p = 1, g(x) = x^2-5 = 0 */ /** p = 1, g(x) = x^2-5 = 0 */
Vector g(const VectorConfig& config, const list<Symbol>& keys) { Vector g(const VecConfig& config, const list<Key>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
return Vector_(1, x * x - 5); return Vector_(1, x * x - 5);
} }
/** p = 1, jacobianG(x) = 2x */ /** p = 1, jacobianG(x) = 2x */
Matrix G(const VectorConfig& config, const list<Symbol>& keys) { Matrix G(const VecConfig& config, const list<Key>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
return Matrix_(1, 1, 2 * x); return Matrix_(1, 1, 2 * x);
} }
@ -48,16 +51,16 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
// the lagrange multipliers will be expected on L_x1 // the lagrange multipliers will be expected on L_x1
// and there is only one multiplier // and there is only one multiplier
size_t p = 1; size_t p = 1;
list<Symbol> keys; keys += "x1";
Key x1(1); Key x1(1);
list<Key> keys; keys += x1;
LagrangeKey L1(1); LagrangeKey L1(1);
NLC1 c1(boost::bind(test1::g, _1, keys), NLC1 c1(boost::bind(test1::g, _1, keys),
x1, boost::bind(test1::G, _1, keys), x1, boost::bind(test1::G, _1, keys),
p, L1); p, L1);
// get a configuration to use for finding the error // get a configuration to use for finding the error
VectorConfig config; VecConfig config;
config.insert("x1", Vector_(1, 1.0)); config.insert(x1, Vector_(1, 1.0));
// calculate the error // calculate the error
Vector actual = c1.unwhitenedError(config); Vector actual = c1.unwhitenedError(config);
@ -68,15 +71,15 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_linearize ) { TEST( NonlinearConstraint1, unary_scalar_linearize ) {
size_t p = 1; size_t p = 1;
list<Symbol> keys; keys += "x1";
Key x1(1); Key x1(1);
list<Key> keys; keys += x1;
LagrangeKey L1(1); LagrangeKey L1(1);
NLC1 c1(boost::bind(test1::g, _1, keys), NLC1 c1(boost::bind(test1::g, _1, keys),
x1, boost::bind(test1::G, _1, keys), x1, boost::bind(test1::G, _1, keys),
p, L1); p, L1);
// get a configuration to use for linearization (with lagrange multipliers) // get a configuration to use for linearization (with lagrange multipliers)
VectorConfig realconfig; VecConfig realconfig;
realconfig.insert(x1, Vector_(1, 1.0)); realconfig.insert(x1, Vector_(1, 1.0));
realconfig.insert(L1, Vector_(1, 3.0)); realconfig.insert(L1, Vector_(1, 3.0));
@ -97,8 +100,8 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_equal ) { TEST( NonlinearConstraint1, unary_scalar_equal ) {
list<Symbol> keys1, keys2; keys1 += "x0"; keys2 += "x1";
Key x(0), y(1); Key x(0), y(1);
list<Key> keys1, keys2; keys1 += x; keys2 += y;
LagrangeKey L1(1); LagrangeKey L1(1);
NLC1 NLC1
c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, L1, true), c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, L1, true),
@ -118,20 +121,20 @@ TEST( NonlinearConstraint1, unary_scalar_equal ) {
namespace test2 { namespace test2 {
/** p = 1, g(x) = x^2-5 -y = 0 */ /** p = 1, g(x) = x^2-5 -y = 0 */
Vector g(const VectorConfig& config, const list<Symbol>& keys) { Vector g(const VecConfig& config, const list<Key>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
double y = config[keys.back()](0); double y = config[keys.back()](0);
return Vector_(1, x * x - 5.0 - y); return Vector_(1, x * x - 5.0 - y);
} }
/** jacobian for x, jacobianG(x,y) in x: 2x*/ /** jacobian for x, jacobianG(x,y) in x: 2x*/
Matrix G1(const VectorConfig& config, const list<Symbol>& keys) { Matrix G1(const VecConfig& config, const list<Key>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
return Matrix_(1, 1, 2.0 * x); return Matrix_(1, 1, 2.0 * x);
} }
/** jacobian for y, jacobianG(x,y) in y: -1 */ /** jacobian for y, jacobianG(x,y) in y: -1 */
Matrix G2(const VectorConfig& config, const list<Symbol>& keys) { Matrix G2(const VecConfig& config, const list<Key>& keys) {
double x = config[keys.back()](0); double x = config[keys.back()](0);
return Matrix_(1, 1, -1.0); return Matrix_(1, 1, -1.0);
} }
@ -144,8 +147,8 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
// the lagrange multipliers will be expected on L_xy // the lagrange multipliers will be expected on L_xy
// and there is only one multiplier // and there is only one multiplier
size_t p = 1; size_t p = 1;
list<Symbol> keys; keys += "x0", "x1";
Key x0(0), x1(1); Key x0(0), x1(1);
list<Key> keys; keys += x0, x1;
LagrangeKey L1(1); LagrangeKey L1(1);
NLC2 c1( NLC2 c1(
boost::bind(test2::g, _1, keys), boost::bind(test2::g, _1, keys),
@ -154,9 +157,9 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
p, L1); p, L1);
// get a configuration to use for finding the error // get a configuration to use for finding the error
VectorConfig config; VecConfig config;
config.insert("x0", Vector_(1, 1.0)); config.insert(x0, Vector_(1, 1.0));
config.insert("x1", Vector_(1, 2.0)); config.insert(x1, Vector_(1, 2.0));
// calculate the error // calculate the error
Vector actual = c1.unwhitenedError(config); Vector actual = c1.unwhitenedError(config);
@ -168,8 +171,8 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
TEST( NonlinearConstraint2, binary_scalar_linearize ) { TEST( NonlinearConstraint2, binary_scalar_linearize ) {
// create a constraint // create a constraint
size_t p = 1; size_t p = 1;
list<Symbol> keys; keys += "x0", "x1";
Key x0(0), x1(1); Key x0(0), x1(1);
list<Key> keys; keys += x0, x1;
LagrangeKey L1(1); LagrangeKey L1(1);
NLC2 c1( NLC2 c1(
boost::bind(test2::g, _1, keys), boost::bind(test2::g, _1, keys),
@ -178,7 +181,7 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
p, L1); p, L1);
// get a configuration to use for finding the error // get a configuration to use for finding the error
VectorConfig realconfig; VecConfig realconfig;
realconfig.insert(x0, Vector_(1, 1.0)); realconfig.insert(x0, Vector_(1, 1.0));
realconfig.insert(x1, Vector_(1, 2.0)); realconfig.insert(x1, Vector_(1, 2.0));
realconfig.insert(L1, Vector_(1, 3.0)); realconfig.insert(L1, Vector_(1, 3.0));
@ -199,9 +202,9 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint2, binary_scalar_equal ) { TEST( NonlinearConstraint2, binary_scalar_equal ) {
list<Symbol> keys1, keys2, keys3; list<Key> keys1, keys2, keys3;
keys1 += "x0", "x1"; keys2 += "x1", "x0"; keys3 += "x0", "z";
Key x0(0), x1(1), x2(2); Key x0(0), x1(1), x2(2);
keys1 += x0, x1; keys2 += x1, x0; keys3 += x0;
LagrangeKey L1(1); LagrangeKey L1(1);
NLC2 NLC2
c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, L1), c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, L1),
@ -215,178 +218,177 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) {
CHECK(!c1.equals(c4)); CHECK(!c1.equals(c4));
} }
///* ************************************************************************* */ /* ************************************************************************* */
//// Inequality tests // Inequality tests
///* ************************************************************************* */ /* ************************************************************************* */
//namespace inequality1 { namespace inequality1 {
//
// /** p = 1, g(x) x^2 - 5 > 0 */ /** p = 1, g(x) x^2 - 5 > 0 */
// Vector g(const VectorConfig& config, const Key& key) { Vector g(const VecConfig& config, const Key& key) {
// double x = config[key](0); double x = config[key](0);
// double g = x * x - 5; double g = x * x - 5;
// return Vector_(1, g); // return the actual cost return Vector_(1, g); // return the actual cost
// } }
//
// /** p = 1, jacobianG(x) = 2*x */ /** p = 1, jacobianG(x) = 2*x */
// Matrix G(const VectorConfig& config, const Key& key) { Matrix G(const VecConfig& config, const Key& key) {
// double x = config[key](0); double x = config[key](0);
// return Matrix_(1, 1, 2 * x); return Matrix_(1, 1, 2 * x);
// } }
//
//} // \namespace inequality1 } // \namespace inequality1
//
///* ************************************************************************* */ /* ************************************************************************* */
//TEST( NonlinearConstraint1, unary_inequality ) { TEST( NonlinearConstraint1, unary_inequality ) {
// size_t p = 1; size_t p = 1;
// Key x0(0); Key x0(0);
// NLC1 c1(boost::bind(inequality1::g, _1, x0), LagrangeKey L1(1);
// x0, boost::bind(inequality1::G, _1, x0), NLC1 c1(boost::bind(inequality1::g, _1, x0),
// p, "L1", x0, boost::bind(inequality1::G, _1, x0),
// false); // inequality constraint p, L1,
// false); // inequality constraint
// // get configurations to use for evaluation
// VectorConfig config1, config2; // get configurations to use for evaluation
// config1.insert(x0, Vector_(1, 10.0)); // should be inactive VecConfig config1, config2;
// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error config1.insert(x0, Vector_(1, 10.0)); // should be inactive
// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
// // check error
// CHECK(!c1.active(config1)); // check error
// Vector actualError2 = c1.unwhitenedError(config2); CHECK(!c1.active(config1));
// CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9))); Vector actualError2 = c1.unwhitenedError(config2);
// CHECK(c1.active(config2)); CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
//} CHECK(c1.active(config2));
// }
///* ************************************************************************* */
//TEST( NonlinearConstraint1, unary_inequality_linearize ) { /* ************************************************************************* */
// size_t p = 1; TEST( NonlinearConstraint1, unary_inequality_linearize ) {
// Key x0(0); size_t p = 1;
// NLC1 c1(boost::bind(inequality1::g, _1, x0), Key x0(0);
// x0, boost::bind(inequality1::G, _1, x0), LagrangeKey L1(1);
// p, "L1", NLC1 c1(boost::bind(inequality1::g, _1, x0),
// false); // inequality constraint x0, boost::bind(inequality1::G, _1, x0),
// p, L1,
// // get configurations to use for linearization false); // inequality constraint
// VectorConfig config1, config2;
// config1.insert(x0, Vector_(1, 10.0)); // should have zero error // get configurations to use for linearization
// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error VecConfig config1, config2;
// config1.insert(x0, Vector_(1, 10.0)); // should have zero error
// // get a configuration of Lagrange multipliers config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
// VectorConfig lagrangeConfig; config1.insert(L1, Vector_(1, 3.0));
// lagrangeConfig.insert("L1", Vector_(1, 3.0)); config2.insert(L1, Vector_(1, 3.0));
//
// // linearize for inactive constraint // linearize for inactive constraint
// GaussianFactor::shared_ptr actualFactor1, actualConstraint1; GaussianFactor::shared_ptr actualFactor1 = c1.linearize(config1);
// boost::tie(actualFactor1, actualConstraint1) = c1.linearize(config1, lagrangeConfig);
// // check if the factor is active
// // check if the factor is active CHECK(!c1.active(config1));
// CHECK(!c1.active(config1));
// // linearize for active constraint
// // linearize for active constraint GaussianFactor::shared_ptr actualFactor2 = c1.linearize(config2);
// GaussianFactor::shared_ptr actualFactor2, actualConstraint2; CHECK(c1.active(config2));
// boost::tie(actualFactor2, actualConstraint2) = c1.linearize(config2, lagrangeConfig);
// CHECK(c1.active(config2)); // verify
// Vector sigmas = Vector_(2, 1.0, 0.0);
// // verify SharedDiagonal model = noiseModel::Constrained::MixedSigmas(sigmas);
// SharedDiagonal probModel = sharedSigma(p,1.0); GaussianFactor expectedFactor(x0, Matrix_(2,1, 6.0, 2.0),
// GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel); L1, Matrix_(2,1, 1.0, 0.0),
// SharedDiagonal constraintModel = noiseModel::Constrained::All(p); Vector_(2, 0.0, 4.0), model);
// GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel);
// CHECK(assert_equal(*actualFactor2, expectedFactor)); CHECK(assert_equal(*actualFactor2, expectedFactor));
// CHECK(assert_equal(*actualConstraint2, expectedConstraint)); }
//}
// /* ************************************************************************* */
///* ************************************************************************* */ // Binding arbitrary functions
//// Binding arbitrary functions /* ************************************************************************* */
///* ************************************************************************* */ namespace binding1 {
//namespace binding1 {
// /** p = 1, g(x) x^2 - r > 0 */
// /** p = 1, g(x) x^2 - r > 0 */ Vector g(double r, const VecConfig& config, const Key& key) {
// Vector g(double r, const VectorConfig& config, const Key& key) { double x = config[key](0);
// double x = config[key](0); double g = x * x - r;
// double g = x * x - r; return Vector_(1, g); // return the actual cost
// return Vector_(1, g); // return the actual cost }
// }
// /** p = 1, jacobianG(x) = 2*x */
// /** p = 1, jacobianG(x) = 2*x */ Matrix G(double coeff, const VecConfig& config,
// Matrix G(double coeff, const VectorConfig& config, const Key& key) {
// const Key& key) { double x = config[key](0);
// double x = config[key](0); return Matrix_(1, 1, coeff * x);
// return Matrix_(1, 1, coeff * x); }
// }
// } // \namespace binding1
//} // \namespace binding1
// /* ************************************************************************* */
///* ************************************************************************* */ TEST( NonlinearConstraint1, unary_binding ) {
//TEST( NonlinearConstraint1, unary_binding ) { size_t p = 1;
// size_t p = 1; double coeff = 2;
// double coeff = 2; double radius = 5;
// double radius = 5; Key x0(0); LagrangeKey L1(1);
// Key x0(0); NLC1 c1(boost::bind(binding1::g, radius, _1, x0),
// NLC1 c1(boost::bind(binding1::g, radius, _1, x0), x0, boost::bind(binding1::G, coeff, _1, x0),
// x0, boost::bind(binding1::G, coeff, _1, x0), p, L1,
// p, "L1", false); // inequality constraint
// false); // inequality constraint
// // get configurations to use for evaluation
// // get configurations to use for evaluation VecConfig config1, config2;
// VectorConfig config1, config2; config1.insert(x0, Vector_(1, 10.0)); // should have zero error
// config1.insert(x0, Vector_(1, 10.0)); // should have zero error config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
// // check error
// // check error CHECK(!c1.active(config1));
// CHECK(!c1.active(config1)); Vector actualError2 = c1.unwhitenedError(config2);
// Vector actualError2 = c1.unwhitenedError(config2); CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
// CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9))); CHECK(c1.active(config2));
// CHECK(c1.active(config2)); }
//}
// /* ************************************************************************* */
///* ************************************************************************* */ namespace binding2 {
//namespace binding2 {
// /** p = 1, g(x) = x^2-5 -y = 0 */
// /** p = 1, g(x) = x^2-5 -y = 0 */ Vector g(double r, const VecConfig& config, const Key& k1, const Key& k2) {
// Vector g(double r, const VectorConfig& config, const Key& k1, const Key& k2) { double x = config[k1](0);
// double x = config[k1](0); double y = config[k2](0);
// double y = config[k2](0); return Vector_(1, x * x - r - y);
// return Vector_(1, x * x - r - y); }
// }
// /** jacobian for x, jacobianG(x,y) in x: 2x*/
// /** jacobian for x, jacobianG(x,y) in x: 2x*/ Matrix G1(double c, const VecConfig& config, const Key& key) {
// Matrix G1(double c, const VectorConfig& config, const Key& key) { double x = config[key](0);
// double x = config[key](0); return Matrix_(1, 1, c * x);
// return Matrix_(1, 1, c * x); }
// }
// /** jacobian for y, jacobianG(x,y) in y: -1 */
// /** jacobian for y, jacobianG(x,y) in y: -1 */ Matrix G2(double c, const VecConfig& config) {
// Matrix G2(double c, const VectorConfig& config) { return Matrix_(1, 1, -1.0 * c);
// return Matrix_(1, 1, -1.0 * c); }
// }
// } // \namespace binding2
//} // \namespace binding2
// /* ************************************************************************* */
///* ************************************************************************* */ TEST( NonlinearConstraint2, binary_binding ) {
//TEST( NonlinearConstraint2, binary_binding ) { // construct a constraint on x and y
// // construct a constraint on x and y // the lagrange multipliers will be expected on L_xy
// // the lagrange multipliers will be expected on L_xy // and there is only one multiplier
// // and there is only one multiplier size_t p = 1;
// size_t p = 1; double a = 2.0;
// double a = 2.0; double b = 1.0;
// double b = 1.0; double r = 5.0;
// double r = 5.0; Key x0(0), x1(1); LagrangeKey L1(1);
// Key x0(0), x1(1); NLC2 c1(boost::bind(binding2::g, r, _1, x0, x1),
// NLC2 c1(boost::bind(binding2::g, r, _1, x0, x1), x0, boost::bind(binding2::G1, a, _1, x0),
// x0, boost::bind(binding2::G1, a, _1, x0), x1, boost::bind(binding2::G2, b, _1),
// x1, boost::bind(binding2::G2, b, _1), p, L1);
// p, "L1");
// // get a configuration to use for finding the error
// // get a configuration to use for finding the error VecConfig config;
// VectorConfig config; config.insert(x0, Vector_(1, 1.0));
// config.insert(x0, Vector_(1, 1.0)); config.insert(x1, Vector_(1, 2.0));
// config.insert(x1, Vector_(1, 2.0));
// // calculate the error
// // calculate the error Vector actual = c1.unwhitenedError(config);
// Vector actual = c1.unwhitenedError(config); Vector expected = Vector_(1.0, -6.0);
// Vector expected = Vector_(1.0, -6.0); CHECK(assert_equal(actual, expected, 1e-5));
// CHECK(assert_equal(actual, expected, 1e-5)); }
//}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

382
cpp/testSQP.cpp
View File

@ -755,119 +755,119 @@ TEST (SQP, stereo_sqp ) {
// create optimizer // create optimizer
VOptimizer optimizer(graph, truthConfig, solver); VOptimizer optimizer(graph, truthConfig, solver);
// optimize // // optimize
VOptimizer afterOneIteration = optimizer.iterate(); // VOptimizer afterOneIteration = optimizer.iterate();
//
// check if correct // // check if correct
CHECK(assert_equal(*truthConfig,*(afterOneIteration.config()))); // CHECK(assert_equal(*truthConfig,*(afterOneIteration.config())));
} }
/* ********************************************************************* ///* *********************************************************************
* SQP version of the above stereo example, // * SQP version of the above stereo example,
* with noise in the initial estimate // * with noise in the initial estimate
*/ // */
TEST (SQP, stereo_sqp_noisy ) { //TEST (SQP, stereo_sqp_noisy ) {
bool verbose = false; // bool verbose = false;
//
// get a graph // // get a graph
boost::shared_ptr<VGraph> graph = stereoExampleGraph(); // boost::shared_ptr<VGraph> graph = stereoExampleGraph();
//
// create initial data // // create initial data
Rot3 faceDownY(Matrix_(3,3, // Rot3 faceDownY(Matrix_(3,3,
1.0, 0.0, 0.0, // 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, // 0.0, 0.0, 1.0,
0.0, 1.0, 0.0)); // 0.0, 1.0, 0.0));
Pose3 pose1(faceDownY, Point3()); // origin, left camera // Pose3 pose1(faceDownY, Point3()); // origin, left camera
Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left // Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left
Point3 landmark1(0.5, 5.0, 0.0); //centered between the cameras, 5 units away // Point3 landmark1(0.5, 5.0, 0.0); //centered between the cameras, 5 units away
Point3 landmark2(1.5, 5.0, 0.0); // Point3 landmark2(1.5, 5.0, 0.0);
//
// noisy config // // noisy config
boost::shared_ptr<VConfig> initConfig(new VConfig); // boost::shared_ptr<VConfig> initConfig(new VConfig);
initConfig->insert(Pose3Key(1), pose1); // initConfig->insert(Pose3Key(1), pose1);
initConfig->insert(Pose3Key(2), pose2); // initConfig->insert(Pose3Key(2), pose2);
initConfig->insert(Point3Key(1), landmark1); // initConfig->insert(Point3Key(1), landmark1);
initConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place // initConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place
initConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0)); // initConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0));
//
// create ordering // // create ordering
shared_ptr<Ordering> ord(new Ordering()); // shared_ptr<Ordering> ord(new Ordering());
*ord += "x1", "x2", "l1", "l2", "L12"; // *ord += "x1", "x2", "l1", "l2", "L12";
VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); // VOptimizer::shared_solver solver(new VOptimizer::solver(ord));
//
// create optimizer // // create optimizer
VOptimizer optimizer(graph, initConfig, solver); // VOptimizer optimizer(graph, initConfig, solver);
//
// optimize // // optimize
VOptimizer *pointer = new VOptimizer(optimizer); // VOptimizer *pointer = new VOptimizer(optimizer);
for (int i=0;i<1;i++) { // for (int i=0;i<1;i++) {
VOptimizer* newOptimizer = new VOptimizer(pointer->iterateLM()); // VOptimizer* newOptimizer = new VOptimizer(pointer->iterateLM());
delete pointer; // delete pointer;
pointer = newOptimizer; // pointer = newOptimizer;
} // }
VOptimizer::shared_config actual = pointer->config(); // VOptimizer::shared_config actual = pointer->config();
delete(pointer); // delete(pointer);
//
// get the truth config // // get the truth config
boost::shared_ptr<VConfig> truthConfig = stereoExampleTruthConfig(); // boost::shared_ptr<VConfig> truthConfig = stereoExampleTruthConfig();
truthConfig->insert(LagrangeKey(12), Vector_(3, 0.0, 1.0, 1.0)); // truthConfig->insert(LagrangeKey(12), Vector_(3, 0.0, 1.0, 1.0));
//
// check if correct // // check if correct
CHECK(assert_equal(*truthConfig,*actual, 1e-5)); // CHECK(assert_equal(*truthConfig,*actual, 1e-5));
} //}
//
static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); //static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1));
//
// typedefs //// typedefs
//typedef simulated2D::Config Config2D; ////typedef simulated2D::Config Config2D;
//typedef boost::shared_ptr<Config2D> shared_config; ////typedef boost::shared_ptr<Config2D> shared_config;
//typedef NonlinearFactorGraph<Config2D> NLGraph; ////typedef NonlinearFactorGraph<Config2D> NLGraph;
//typedef boost::shared_ptr<NonlinearFactor<Config2D> > shared; ////typedef boost::shared_ptr<NonlinearFactor<Config2D> > shared;
//
namespace map_warp_example { //namespace map_warp_example {
typedef NonlinearConstraint1< //typedef NonlinearConstraint1<
Config2D, simulated2D::PoseKey, Point2> NLC1; // Config2D, simulated2D::PoseKey, Point2> NLC1;
//typedef NonlinearConstraint2< ////typedef NonlinearConstraint2<
// Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2; //// Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2;
} // \namespace map_warp_example //} // \namespace map_warp_example
//
/* ********************************************************************* */ ///* ********************************************************************* */
// Example that moves two separate maps into the same frame of reference //// Example that moves two separate maps into the same frame of reference
// Note that this is a linear example, so it should converge in one step //// Note that this is a linear example, so it should converge in one step
/* ********************************************************************* */ ///* ********************************************************************* */
//
namespace sqp_LinearMapWarp2 { //namespace sqp_LinearMapWarp2 {
// binary constraint between landmarks //// binary constraint between landmarks
/** g(x) = x-y = 0 */ ///** g(x) = x-y = 0 */
Vector g_func(const Config2D& config, const simulated2D::PointKey& key1, const simulated2D::PointKey& key2) { //Vector g_func(const Config2D& config, const simulated2D::PointKey& key1, const simulated2D::PointKey& key2) {
Point2 p = config[key1]-config[key2]; // Point2 p = config[key1]-config[key2];
return Vector_(2, p.x(), p.y()); // return Vector_(2, p.x(), p.y());
} //}
//
/** jacobian at l1 */ ///** jacobian at l1 */
Matrix jac_g1(const Config2D& config) { //Matrix jac_g1(const Config2D& config) {
return eye(2); // return eye(2);
} //}
//
/** jacobian at l2 */ ///** jacobian at l2 */
Matrix jac_g2(const Config2D& config) { //Matrix jac_g2(const Config2D& config) {
return -1*eye(2); // return -1*eye(2);
} //}
} // \namespace sqp_LinearMapWarp2 //} // \namespace sqp_LinearMapWarp2
//
namespace sqp_LinearMapWarp1 { //namespace sqp_LinearMapWarp1 {
// Unary Constraint on x1 //// Unary Constraint on x1
/** g(x) = x -[1;1] = 0 */ ///** g(x) = x -[1;1] = 0 */
Vector g_func(const Config2D& config, const simulated2D::PoseKey& key) { //Vector g_func(const Config2D& config, const simulated2D::PoseKey& key) {
Point2 p = config[key]-Point2(1.0, 1.0); // Point2 p = config[key]-Point2(1.0, 1.0);
return Vector_(2, p.x(), p.y()); // return Vector_(2, p.x(), p.y());
} //}
//
/** jacobian at x1 */ ///** jacobian at x1 */
Matrix jac_g(const Config2D& config) { //Matrix jac_g(const Config2D& config) {
return eye(2); // return eye(2);
} //}
} // \namespace sqp_LinearMapWarp12 //} // \namespace sqp_LinearMapWarp12
//typedef NonlinearOptimizer<NLGraph, Config2D> Optimizer; //typedef NonlinearOptimizer<NLGraph, Config2D> Optimizer;
@ -875,86 +875,86 @@ Matrix jac_g(const Config2D& config) {
* Creates the graph with each robot seeing the landmark, and it is * Creates the graph with each robot seeing the landmark, and it is
* known that it is the same landmark * known that it is the same landmark
*/ */
boost::shared_ptr<Graph2D> linearMapWarpGraph() { //boost::shared_ptr<Graph2D> linearMapWarpGraph() {
using namespace map_warp_example; // using namespace map_warp_example;
// keys // // keys
simulated2D::PoseKey x1(1), x2(2); // simulated2D::PoseKey x1(1), x2(2);
simulated2D::PointKey l1(1), l2(2); // simulated2D::PointKey l1(1), l2(2);
//
// constant constraint on x1 // // constant constraint on x1
LagrangeKey L1(1); // LagrangeKey L1(1);
shared_ptr<NLC1> c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1), // shared_ptr<NLC1> c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1),
x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1), // x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1),
2, L1)); // 2, L1));
//
// measurement from x1 to l1 // // measurement from x1 to l1
Point2 z1(0.0, 5.0); // Point2 z1(0.0, 5.0);
shared f1(new simulated2D::GenericMeasurement<Config2D>(z1, sigma, x1,l1)); // shared f1(new simulated2D::GenericMeasurement<Config2D>(z1, sigma, x1,l1));
//
// measurement from x2 to l2 // // measurement from x2 to l2
Point2 z2(-4.0, 0.0); // Point2 z2(-4.0, 0.0);
shared f2(new simulated2D::GenericMeasurement<Config2D>(z2, sigma, x2,l2)); // shared f2(new simulated2D::GenericMeasurement<Config2D>(z2, sigma, x2,l2));
//
// equality constraint between l1 and l2 // // equality constraint between l1 and l2
LagrangeKey L12(12); // LagrangeKey L12(12);
shared_ptr<NLC2> c2 (new NLC2( // shared_ptr<NLC2> c2 (new NLC2(
boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2), // boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2),
l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1), // l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1),
l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1), // l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1),
2, L12)); // 2, L12));
//
// construct the graph // // construct the graph
boost::shared_ptr<Graph2D> graph(new Graph2D()); // boost::shared_ptr<Graph2D> graph(new Graph2D());
graph->push_back(c1); // graph->push_back(c1);
graph->push_back(c2); // graph->push_back(c2);
graph->push_back(f1); // graph->push_back(f1);
graph->push_back(f2); // graph->push_back(f2);
//
return graph; // return graph;
} //}
//
/* ********************************************************************* */ ///* ********************************************************************* */
TEST ( SQPOptimizer, map_warp_initLam ) { //TEST ( SQPOptimizer, map_warp_initLam ) {
bool verbose = false; // bool verbose = false;
// get a graph // // get a graph
boost::shared_ptr<Graph2D> graph = linearMapWarpGraph(); // boost::shared_ptr<Graph2D> graph = linearMapWarpGraph();
//
// keys // // keys
simulated2D::PoseKey x1(1), x2(2); // simulated2D::PoseKey x1(1), x2(2);
simulated2D::PointKey l1(1), l2(2); // simulated2D::PointKey l1(1), l2(2);
LagrangeKey L1(1), L12(12); // LagrangeKey L1(1), L12(12);
//
// create an initial estimate // // create an initial estimate
shared_ptr<Config2D> initialEstimate(new Config2D); // shared_ptr<Config2D> initialEstimate(new Config2D);
initialEstimate->insert(x1, Point2(1.0, 1.0)); // initialEstimate->insert(x1, Point2(1.0, 1.0));
initialEstimate->insert(l1, Point2(1.0, 6.0)); // initialEstimate->insert(l1, Point2(1.0, 6.0));
initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame // initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin // initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin
initialEstimate->insert(L12, Vector_(2, 1.0, 1.0)); // initialEstimate->insert(L12, Vector_(2, 1.0, 1.0));
initialEstimate->insert(L1, Vector_(2, 1.0, 1.0)); // initialEstimate->insert(L1, Vector_(2, 1.0, 1.0));
//
// create an ordering // // create an ordering
shared_ptr<Ordering> ordering(new Ordering()); // shared_ptr<Ordering> ordering(new Ordering());
*ordering += "x1", "x2", "l1", "l2", "L12", "L1"; // *ordering += "x1", "x2", "l1", "l2", "L12", "L1";
//
// create an optimizer // // create an optimizer
Optimizer::shared_solver solver(new Optimizer::solver(ordering)); // Optimizer::shared_solver solver(new Optimizer::solver(ordering));
Optimizer optimizer(graph, initialEstimate, solver); // Optimizer optimizer(graph, initialEstimate, solver);
//
// perform an iteration of optimization // // perform an iteration of optimization
Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT); // Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT);
//
// get the config back out and verify // // get the config back out and verify
Config2D actual = *(oneIteration.config()); // Config2D actual = *(oneIteration.config());
Config2D expected; // Config2D expected;
expected.insert(x1, Point2(1.0, 1.0)); // expected.insert(x1, Point2(1.0, 1.0));
expected.insert(l1, Point2(1.0, 6.0)); // expected.insert(l1, Point2(1.0, 6.0));
expected.insert(l2, Point2(1.0, 6.0)); // expected.insert(l2, Point2(1.0, 6.0));
expected.insert(x2, Point2(5.0, 6.0)); // expected.insert(x2, Point2(5.0, 6.0));
expected.insert(L1, Vector_(2, 1.0, 1.0)); // expected.insert(L1, Vector_(2, 1.0, 1.0));
expected.insert(L12, Vector_(2, 6.0, 7.0)); // expected.insert(L12, Vector_(2, 6.0, 7.0));
CHECK(assert_equal(expected, actual)); // CHECK(assert_equal(expected, actual));
} //}
///* ********************************************************************* */ ///* ********************************************************************* */
//// This is an obstacle avoidance demo, where there is a trajectory of //// This is an obstacle avoidance demo, where there is a trajectory of