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

Formatting

release/4.3a0
dellaert 2014-11-25 08:47:25 +01:00
parent 97d6088467
commit ae681cebb1
2 changed files with 354 additions and 310 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

193
gtsam/nonlinear/NonlinearEquality.h
View File

@ -26,16 +26,16 @@
namespace gtsam { namespace gtsam {
/** /**
* Template default compare function that assumes a testable T * Template default compare function that assumes a testable T
*/ */
template<class T> template<class T>
bool compare(const T& a, const T& b) { bool compare(const T& a, const T& b) {
GTSAM_CONCEPT_TESTABLE_TYPE(T); GTSAM_CONCEPT_TESTABLE_TYPE(T);
return a.equals(b); return a.equals(b);
} }
/** /**
* An equality factor that forces either one variable to a constant, * An equality factor that forces either one variable to a constant,
* or a set of variables to be equal to each other. * or a set of variables to be equal to each other.
* *
@ -47,13 +47,13 @@ namespace gtsam {
* *
* \nosubgrouping * \nosubgrouping
*/ */
template<class VALUE> template<class VALUE>
class NonlinearEquality: public NoiseModelFactor1<VALUE> { class NonlinearEquality: public NoiseModelFactor1<VALUE> {
public: public:
typedef VALUE T; typedef VALUE T;
private: private:
// feasible value // feasible value
T feasible_; T feasible_;
@ -70,18 +70,19 @@ namespace gtsam {
// typedef to base class // typedef to base class
typedef NoiseModelFactor1<VALUE> Base; typedef NoiseModelFactor1<VALUE> Base;
public: public:
/** /**
* Function that compares two values * Function that compares two values
*/ */
bool (*compare_)(const T& a, const T& b); bool (*compare_)(const T& a, const T& b);
/** default constructor - only for serialization */ /** default constructor - only for serialization */
NonlinearEquality() {} NonlinearEquality() {
}
virtual ~NonlinearEquality() {} virtual ~NonlinearEquality() {
}
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -89,36 +90,38 @@ namespace gtsam {
/** /**
* Constructor - forces exact evaluation * Constructor - forces exact evaluation
*/ */
NonlinearEquality(Key j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) : NonlinearEquality(Key j, const T& feasible,
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), bool (*_compare)(const T&, const T&) = compare<T>) :
allow_error_(false), error_gain_(0.0), Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(
compare_(_compare) { feasible), allow_error_(false), error_gain_(0.0), compare_(_compare) {
} }
/** /**
* Constructor - allows inexact evaluation * Constructor - allows inexact evaluation
*/ */
NonlinearEquality(Key j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) : NonlinearEquality(Key j, const T& feasible, double error_gain,
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), bool (*_compare)(const T&, const T&) = compare<T>) :
allow_error_(true), error_gain_(error_gain), Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(
compare_(_compare) { feasible), allow_error_(true), error_gain_(error_gain), compare_(
_compare) {
} }
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n"; std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n";
gtsam::print(feasible_,"Feasible Point:\n"); gtsam::print(feasible_, "Feasible Point:\n");
std::cout << "Variable Dimension: " << feasible_.dim() << std::endl; std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
} }
/** Check if two factors are equal */ /** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&f); const This* e = dynamic_cast<const This*>(&f);
return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) && return e && Base::equals(f) && feasible_.equals(e->feasible_, tol)
fabs(error_gain_ - e->error_gain_) < tol; && fabs(error_gain_ - e->error_gain_) < tol;
} }
/// @} /// @}
@ -130,24 +133,29 @@ namespace gtsam {
const T& xj = c.at<T>(this->key()); const T& xj = c.at<T>(this->key());
Vector e = this->unwhitenedError(c); Vector e = this->unwhitenedError(c);
if (allow_error_ || !compare_(xj, feasible_)) { if (allow_error_ || !compare_(xj, feasible_)) {
return error_gain_ * dot(e,e); return error_gain_ * dot(e, e);
} else { } else {
return 0.0; return 0.0;
} }
} }
/** error function */ /** error function */
Vector evaluateError(const T& xj, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const T& xj,
boost::optional<Matrix&> H = boost::none) const {
size_t nj = feasible_.dim(); size_t nj = feasible_.dim();
if (allow_error_) { if (allow_error_) {
if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare if (H)
*H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
return xj.localCoordinates(feasible_); return xj.localCoordinates(feasible_);
} else if (compare_(feasible_,xj)) { } else if (compare_(feasible_, xj)) {
if (H) *H = eye(nj); if (H)
*H = eye(nj);
return zero(nj); // set error to zero if equal return zero(nj); // set error to zero if equal
} else { } else {
if (H) throw std::invalid_argument( if (H)
"Linearization point not feasible for " + DefaultKeyFormatter(this->key()) + "!"); throw std::invalid_argument(
"Linearization point not feasible for "
+ DefaultKeyFormatter(this->key()) + "!");
return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
} }
} }
@ -158,148 +166,171 @@ namespace gtsam {
Matrix A; Matrix A;
Vector b = evaluateError(xj, A); Vector b = evaluateError(xj, A);
SharedDiagonal model = noiseModel::Constrained::All(b.size()); SharedDiagonal model = noiseModel::Constrained::All(b.size());
return GaussianFactor::shared_ptr(new JacobianFactor(this->key(), A, b, model)); return GaussianFactor::shared_ptr(
new JacobianFactor(this->key(), A, b, model));
} }
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/// @} /// @}
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor1", ar
& boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(feasible_); ar & BOOST_SERIALIZATION_NVP(feasible_);
ar & BOOST_SERIALIZATION_NVP(allow_error_); ar & BOOST_SERIALIZATION_NVP(allow_error_);
ar & BOOST_SERIALIZATION_NVP(error_gain_); ar & BOOST_SERIALIZATION_NVP(error_gain_);
} }
}; // \class NonlinearEquality };
// \class NonlinearEquality
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Simple unary equality constraint - fixes a value for a variable * Simple unary equality constraint - fixes a value for a variable
*/ */
template<class VALUE> template<class VALUE>
class NonlinearEquality1 : public NoiseModelFactor1<VALUE> { class NonlinearEquality1: public NoiseModelFactor1<VALUE> {
public: public:
typedef VALUE X; typedef VALUE X;
protected: protected:
typedef NoiseModelFactor1<VALUE> Base; typedef NoiseModelFactor1<VALUE> Base;
typedef NonlinearEquality1<VALUE> This; typedef NonlinearEquality1<VALUE> This;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
NonlinearEquality1() {} NonlinearEquality1() {
}
X value_; /// fixed value for variable X value_; /// fixed value for variable
GTSAM_CONCEPT_MANIFOLD_TYPE(X); GTSAM_CONCEPT_MANIFOLD_TYPE(X)
GTSAM_CONCEPT_TESTABLE_TYPE(X); ;GTSAM_CONCEPT_TESTABLE_TYPE(X)
;
public: public:
typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr; typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
///TODO: comment ///TODO: comment
NonlinearEquality1(const X& value, Key key1, double mu = 1000.0) NonlinearEquality1(const X& value, Key key1, double mu = 1000.0) :
: Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {} Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(
value) {
}
virtual ~NonlinearEquality1() {} virtual ~NonlinearEquality1() {
}
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** g(x) with optional derivative */ /** g(x) with optional derivative */
Vector evaluateError(const X& x1, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const X& x1,
if (H) (*H) = eye(x1.dim()); boost::optional<Matrix&> H = boost::none) const {
if (H)
(*H) = eye(x1.dim());
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
return value_.localCoordinates(x1); return value_.localCoordinates(x1);
} }
/** Print */ /** Print */
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { virtual void print(const std::string& s = "",
std::cout << s << ": NonlinearEquality1(" const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
<< keyFormatter(this->key()) << "),"<< "\n"; std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
<< ")," << "\n";
this->noiseModel_->print(); this->noiseModel_->print();
value_.print("Value"); value_.print("Value");
} }
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor1", ar
& boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(value_); ar & BOOST_SERIALIZATION_NVP(value_);
} }
}; // \NonlinearEquality1 };
// \NonlinearEquality1
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Simple binary equality constraint - this constraint forces two factors to * Simple binary equality constraint - this constraint forces two factors to
* be the same. * be the same.
*/ */
template<class VALUE> template<class VALUE>
class NonlinearEquality2 : public NoiseModelFactor2<VALUE, VALUE> { class NonlinearEquality2: public NoiseModelFactor2<VALUE, VALUE> {
public: public:
typedef VALUE X; typedef VALUE X;
protected: protected:
typedef NoiseModelFactor2<VALUE, VALUE> Base; typedef NoiseModelFactor2<VALUE, VALUE> Base;
typedef NonlinearEquality2<VALUE> This; typedef NonlinearEquality2<VALUE> This;
GTSAM_CONCEPT_MANIFOLD_TYPE(X); GTSAM_CONCEPT_MANIFOLD_TYPE(X)
;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
NonlinearEquality2() {} NonlinearEquality2() {
}
public: public:
typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr; typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr;
///TODO: comment ///TODO: comment
NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) :
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {} Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {
virtual ~NonlinearEquality2() {} }
virtual ~NonlinearEquality2() {
}
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** g(x) with optional derivative2 */ /** g(x) with optional derivative2 */
Vector evaluateError(const X& x1, const X& x2, Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 =
boost::optional<Matrix&> H1 = boost::none, boost::none, boost::optional<Matrix&> H2 = boost::none) const {
boost::optional<Matrix&> H2 = boost::none) const {
const size_t p = X::Dim(); const size_t p = X::Dim();
if (H1) *H1 = -eye(p); if (H1)
if (H2) *H2 = eye(p); *H1 = -eye(p);
if (H2)
*H2 = eye(p);
return x1.localCoordinates(x2); return x1.localCoordinates(x2);
} }
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor2", ar
& boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
} }
}; // \NonlinearEquality2 };
// \NonlinearEquality2
} // namespace gtsam }// namespace gtsam

175
tests/testNonlinearEquality.cpp
View File

@ -42,9 +42,9 @@ typedef PriorFactor<Pose2> PosePrior;
typedef NonlinearEquality<Pose2> PoseNLE; typedef NonlinearEquality<Pose2> PoseNLE;
typedef boost::shared_ptr<PoseNLE> shared_poseNLE; typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
static Symbol key('x',1); static Symbol key('x', 1);
/* ************************************************************************* */ //******************************************************************************
TEST ( NonlinearEquality, linearization ) { TEST ( NonlinearEquality, linearization ) {
Pose2 value = Pose2(2.1, 1.0, 2.0); Pose2 value = Pose2(2.1, 1.0, 2.0);
Values linearize; Values linearize;
@ -60,10 +60,10 @@ TEST ( NonlinearEquality, linearization ) {
EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF)); EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF));
} }
/* ********************************************************************** */ //******************************************************************************
TEST ( NonlinearEquality, linearization_pose ) { TEST ( NonlinearEquality, linearization_pose ) {
Symbol key('x',1); Symbol key('x', 1);
Pose2 value; Pose2 value;
Values config; Values config;
config.insert(key, value); config.insert(key, value);
@ -75,7 +75,7 @@ TEST ( NonlinearEquality, linearization_pose ) {
EXPECT(true); EXPECT(true);
} }
/* ********************************************************************** */ //******************************************************************************
TEST ( NonlinearEquality, linearization_fail ) { TEST ( NonlinearEquality, linearization_fail ) {
Pose2 value = Pose2(2.1, 1.0, 2.0); Pose2 value = Pose2(2.1, 1.0, 2.0);
Pose2 wrong = Pose2(2.1, 3.0, 4.0); Pose2 wrong = Pose2(2.1, 3.0, 4.0);
@ -89,12 +89,11 @@ TEST ( NonlinearEquality, linearization_fail ) {
CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument); CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument);
} }
/* ********************************************************************** */ //******************************************************************************
TEST ( NonlinearEquality, linearization_fail_pose ) { TEST ( NonlinearEquality, linearization_fail_pose ) {
Symbol key('x',1); Symbol key('x', 1);
Pose2 value(2.0, 1.0, 2.0), Pose2 value(2.0, 1.0, 2.0), wrong(2.0, 3.0, 4.0);
wrong(2.0, 3.0, 4.0);
Values bad_linearize; Values bad_linearize;
bad_linearize.insert(key, wrong); bad_linearize.insert(key, wrong);
@ -105,12 +104,11 @@ TEST ( NonlinearEquality, linearization_fail_pose ) {
CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument); CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument);
} }
/* ********************************************************************** */ //******************************************************************************
TEST ( NonlinearEquality, linearization_fail_pose_origin ) { TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
Symbol key('x',1); Symbol key('x', 1);
Pose2 value, Pose2 value, wrong(2.0, 3.0, 4.0);
wrong(2.0, 3.0, 4.0);
Values bad_linearize; Values bad_linearize;
bad_linearize.insert(key, wrong); bad_linearize.insert(key, wrong);
@ -121,7 +119,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument); CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument);
} }
/* ************************************************************************* */ //******************************************************************************
TEST ( NonlinearEquality, error ) { TEST ( NonlinearEquality, error ) {
Pose2 value = Pose2(2.1, 1.0, 2.0); Pose2 value = Pose2(2.1, 1.0, 2.0);
Pose2 wrong = Pose2(2.1, 3.0, 4.0); Pose2 wrong = Pose2(2.1, 3.0, 4.0);
@ -137,10 +135,11 @@ TEST ( NonlinearEquality, error ) {
EXPECT(assert_equal(actual, zero(3))); EXPECT(assert_equal(actual, zero(3)));
actual = nle->unwhitenedError(bad_linearize); actual = nle->unwhitenedError(bad_linearize);
EXPECT(assert_equal(actual, repeat(3, std::numeric_limits<double>::infinity()))); EXPECT(
assert_equal(actual, repeat(3, std::numeric_limits<double>::infinity())));
} }
/* ************************************************************************* */ //******************************************************************************
TEST ( NonlinearEquality, equals ) { TEST ( NonlinearEquality, equals ) {
Pose2 value1 = Pose2(2.1, 1.0, 2.0); Pose2 value1 = Pose2(2.1, 1.0, 2.0);
Pose2 value2 = Pose2(2.1, 3.0, 4.0); Pose2 value2 = Pose2(2.1, 3.0, 4.0);
@ -151,14 +150,17 @@ TEST ( NonlinearEquality, equals ) {
shared_poseNLE nle3(new PoseNLE(key, value2)); shared_poseNLE nle3(new PoseNLE(key, value2));
// verify // verify
EXPECT(nle1->equals(*nle2)); // basic equality = true EXPECT(nle1->equals(*nle2));
EXPECT(nle2->equals(*nle1)); // test symmetry of equals() // basic equality = true
EXPECT(!nle1->equals(*nle3)); // test config EXPECT(nle2->equals(*nle1));
// test symmetry of equals()
EXPECT(!nle1->equals(*nle3));
// test config
} }
/* ************************************************************************* */ //******************************************************************************
TEST ( NonlinearEquality, allow_error_pose ) { TEST ( NonlinearEquality, allow_error_pose ) {
Symbol key1('x',1); Symbol key1('x', 1);
Pose2 feasible1(1.0, 2.0, 3.0); Pose2 feasible1(1.0, 2.0, 3.0);
double error_gain = 500.0; double error_gain = 500.0;
PoseNLE nle(key1, feasible1, error_gain); PoseNLE nle(key1, feasible1, error_gain);
@ -177,16 +179,17 @@ TEST ( NonlinearEquality, allow_error_pose ) {
// check linearization // check linearization
GaussianFactor::shared_ptr actLinFactor = nle.linearize(config); GaussianFactor::shared_ptr actLinFactor = nle.linearize(config);
Matrix A1 = eye(3,3); Matrix A1 = eye(3, 3);
Vector b = expVec; Vector b = expVec;
SharedDiagonal model = noiseModel::Constrained::All(3); SharedDiagonal model = noiseModel::Constrained::All(3);
GaussianFactor::shared_ptr expLinFactor(new JacobianFactor(key1, A1, b, model)); GaussianFactor::shared_ptr expLinFactor(
new JacobianFactor(key1, A1, b, model));
EXPECT(assert_equal(*expLinFactor, *actLinFactor, 1e-5)); EXPECT(assert_equal(*expLinFactor, *actLinFactor, 1e-5));
} }
/* ************************************************************************* */ //******************************************************************************
TEST ( NonlinearEquality, allow_error_optimize ) { TEST ( NonlinearEquality, allow_error_optimize ) {
Symbol key1('x',1); Symbol key1('x', 1);
Pose2 feasible1(1.0, 2.0, 3.0); Pose2 feasible1(1.0, 2.0, 3.0);
double error_gain = 500.0; double error_gain = 500.0;
PoseNLE nle(key1, feasible1, error_gain); PoseNLE nle(key1, feasible1, error_gain);
@ -211,11 +214,11 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
EXPECT(assert_equal(expected, result)); EXPECT(assert_equal(expected, result));
} }
/* ************************************************************************* */ //******************************************************************************
TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
// create a hard constraint // create a hard constraint
Symbol key1('x',1); Symbol key1('x', 1);
Pose2 feasible1(1.0, 2.0, 3.0); Pose2 feasible1(1.0, 2.0, 3.0);
// initialize away from the ideal // initialize away from the ideal
@ -245,14 +248,14 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */ //******************************************************************************
static SharedDiagonal hard_model = noiseModel::Constrained::All(2); static SharedDiagonal hard_model = noiseModel::Constrained::All(2);
static SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); static SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
/* ************************************************************************* */ //******************************************************************************
TEST( testNonlinearEqualityConstraint, unary_basics ) { TEST( testNonlinearEqualityConstraint, unary_basics ) {
Point2 pt(1.0, 2.0); Point2 pt(1.0, 2.0);
Symbol key1('x',1); Symbol key1('x', 1);
double mu = 1000.0; double mu = 1000.0;
eq2D::UnaryEqualityConstraint constraint(pt, key, mu); eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
@ -267,38 +270,42 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) {
Point2 ptBad1(2.0, 2.0); Point2 ptBad1(2.0, 2.0);
config2.insert(key, ptBad1); config2.insert(key, ptBad1);
EXPECT(constraint.active(config2)); EXPECT(constraint.active(config2));
EXPECT(assert_equal(Vector2(1.0, 0.0), constraint.evaluateError(ptBad1), tol)); EXPECT(
EXPECT(assert_equal(Vector2(1.0, 0.0), constraint.unwhitenedError(config2), tol)); assert_equal(Vector2(1.0, 0.0), constraint.evaluateError(ptBad1), tol));
EXPECT(
assert_equal(Vector2(1.0, 0.0), constraint.unwhitenedError(config2), tol));
EXPECT_DOUBLES_EQUAL(500.0, constraint.error(config2), tol); EXPECT_DOUBLES_EQUAL(500.0, constraint.error(config2), tol);
} }
/* ************************************************************************* */ //******************************************************************************
TEST( testNonlinearEqualityConstraint, unary_linearization ) { TEST( testNonlinearEqualityConstraint, unary_linearization ) {
Point2 pt(1.0, 2.0); Point2 pt(1.0, 2.0);
Symbol key1('x',1); Symbol key1('x', 1);
double mu = 1000.0; double mu = 1000.0;
eq2D::UnaryEqualityConstraint constraint(pt, key, mu); eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
Values config1; Values config1;
config1.insert(key, pt); config1.insert(key, pt);
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
GaussianFactor::shared_ptr expected1(new JacobianFactor(key, eye(2,2), zero(2), hard_model)); GaussianFactor::shared_ptr expected1(
new JacobianFactor(key, eye(2, 2), zero(2), hard_model));
EXPECT(assert_equal(*expected1, *actual1, tol)); EXPECT(assert_equal(*expected1, *actual1, tol));
Values config2; Values config2;
Point2 ptBad(2.0, 2.0); Point2 ptBad(2.0, 2.0);
config2.insert(key, ptBad); config2.insert(key, ptBad);
GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2);
GaussianFactor::shared_ptr expected2(new JacobianFactor(key, eye(2,2), Vector2(-1.0,0.0), hard_model)); GaussianFactor::shared_ptr expected2(
new JacobianFactor(key, eye(2, 2), Vector2(-1.0, 0.0), hard_model));
EXPECT(assert_equal(*expected2, *actual2, tol)); EXPECT(assert_equal(*expected2, *actual2, tol));
} }
/* ************************************************************************* */ //******************************************************************************
TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
// create a single-node graph with a soft and hard constraint to // create a single-node graph with a soft and hard constraint to
// ensure that the hard constraint overrides the soft constraint // ensure that the hard constraint overrides the soft constraint
Point2 truth_pt(1.0, 2.0); Point2 truth_pt(1.0, 2.0);
Symbol key('x',1); Symbol key('x', 1);
double mu = 10.0; double mu = 10.0;
eq2D::UnaryEqualityConstraint::shared_ptr constraint( eq2D::UnaryEqualityConstraint::shared_ptr constraint(
new eq2D::UnaryEqualityConstraint(truth_pt, key, mu)); new eq2D::UnaryEqualityConstraint(truth_pt, key, mu));
@ -326,10 +333,10 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
EXPECT(assert_equal(expected, actual, tol)); EXPECT(assert_equal(expected, actual, tol));
} }
/* ************************************************************************* */ //******************************************************************************
TEST( testNonlinearEqualityConstraint, odo_basics ) { TEST( testNonlinearEqualityConstraint, odo_basics ) {
Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
Symbol key1('x',1), key2('x',2); Symbol key1('x', 1), key2('x', 2);
double mu = 1000.0; double mu = 1000.0;
eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
@ -347,15 +354,17 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) {
config2.insert(key1, x1bad); config2.insert(key1, x1bad);
config2.insert(key2, x2bad); config2.insert(key2, x2bad);
EXPECT(constraint.active(config2)); EXPECT(constraint.active(config2));
EXPECT(assert_equal(Vector2(-1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); EXPECT(
EXPECT(assert_equal(Vector2(-1.0, -1.0), constraint.unwhitenedError(config2), tol)); assert_equal(Vector2(-1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol));
EXPECT(
assert_equal(Vector2(-1.0, -1.0), constraint.unwhitenedError(config2), tol));
EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol); EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol);
} }
/* ************************************************************************* */ //******************************************************************************
TEST( testNonlinearEqualityConstraint, odo_linearization ) { TEST( testNonlinearEqualityConstraint, odo_linearization ) {
Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
Symbol key1('x',1), key2('x',2); Symbol key1('x', 1), key2('x', 2);
double mu = 1000.0; double mu = 1000.0;
eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
@ -364,8 +373,8 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
config1.insert(key2, x2); config1.insert(key2, x2);
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
GaussianFactor::shared_ptr expected1( GaussianFactor::shared_ptr expected1(
new JacobianFactor(key1, -eye(2,2), key2, new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), zero(2),
eye(2,2), zero(2), hard_model)); hard_model));
EXPECT(assert_equal(*expected1, *actual1, tol)); EXPECT(assert_equal(*expected1, *actual1, tol));
Values config2; Values config2;
@ -375,18 +384,18 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
config2.insert(key2, x2bad); config2.insert(key2, x2bad);
GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2);
GaussianFactor::shared_ptr expected2( GaussianFactor::shared_ptr expected2(
new JacobianFactor(key1, -eye(2,2), key2, new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), Vector2(1.0, 1.0),
eye(2,2), Vector2(1.0, 1.0), hard_model)); hard_model));
EXPECT(assert_equal(*expected2, *actual2, tol)); EXPECT(assert_equal(*expected2, *actual2, tol));
} }
/* ************************************************************************* */ //******************************************************************************
TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
// create a two-node graph, connected by an odometry constraint, with // create a two-node graph, connected by an odometry constraint, with
// a hard prior on one variable, and a conflicting soft prior // a hard prior on one variable, and a conflicting soft prior
// on the other variable - the constraints should override the soft constraint // on the other variable - the constraints should override the soft constraint
Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0); Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0);
Symbol key1('x',1), key2('x',2); Symbol key1('x', 1), key2('x', 2);
// hard prior on x1 // hard prior on x1
eq2D::UnaryEqualityConstraint::shared_ptr constraint1( eq2D::UnaryEqualityConstraint::shared_ptr constraint1(
@ -399,8 +408,8 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
// odometry constraint // odometry constraint
eq2D::OdoEqualityConstraint::shared_ptr constraint2( eq2D::OdoEqualityConstraint::shared_ptr constraint2(
new eq2D::OdoEqualityConstraint( new eq2D::OdoEqualityConstraint(truth_pt1.between(truth_pt2), key1,
truth_pt1.between(truth_pt2), key1, key2)); key2));
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.push_back(constraint1); graph.push_back(constraint1);
@ -418,7 +427,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
CHECK(assert_equal(expected, actual, tol)); CHECK(assert_equal(expected, actual, tol));
} }
/* ********************************************************************* */ //******************************************************************************
TEST (testNonlinearEqualityConstraint, two_pose ) { TEST (testNonlinearEqualityConstraint, two_pose ) {
/* /*
* Determining a ground truth linear system * Determining a ground truth linear system
@ -428,19 +437,18 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
Symbol x1('x',1), x2('x',2); Symbol x1('x', 1), x2('x', 2);
Symbol l1('l',1), l2('l',2); Symbol l1('l', 1), l2('l', 2);
Point2 pt_x1(1.0, 1.0), Point2 pt_x1(1.0, 1.0), pt_x2(5.0, 6.0);
pt_x2(5.0, 6.0);
graph += eq2D::UnaryEqualityConstraint(pt_x1, x1); graph += eq2D::UnaryEqualityConstraint(pt_x1, x1);
graph += eq2D::UnaryEqualityConstraint(pt_x2, x2); graph += eq2D::UnaryEqualityConstraint(pt_x2, x2);
Point2 z1(0.0, 5.0); Point2 z1(0.0, 5.0);
SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1)); SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1));
graph += simulated2D::Measurement(z1, sigma, x1,l1); graph += simulated2D::Measurement(z1, sigma, x1, l1);
Point2 z2(-4.0, 0.0); Point2 z2(-4.0, 0.0);
graph += simulated2D::Measurement(z2, sigma, x2,l2); graph += simulated2D::Measurement(z2, sigma, x2, l2);
graph += eq2D::PointEqualityConstraint(l1, l2); graph += eq2D::PointEqualityConstraint(l1, l2);
@ -450,7 +458,8 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
initialEstimate.insert(l1, Point2(1.0, 6.0)); // ground truth initialEstimate.insert(l1, Point2(1.0, 6.0)); // ground truth
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
Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); Values actual =
LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
Values expected; Values expected;
expected.insert(x1, pt_x1); expected.insert(x1, pt_x1);
@ -460,14 +469,14 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
CHECK(assert_equal(expected, actual, 1e-5)); CHECK(assert_equal(expected, actual, 1e-5));
} }
/* ********************************************************************* */ //******************************************************************************
TEST (testNonlinearEqualityConstraint, map_warp ) { TEST (testNonlinearEqualityConstraint, map_warp ) {
// get a graph // get a graph
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
// keys // keys
Symbol x1('x',1), x2('x',2); Symbol x1('x', 1), x2('x', 2);
Symbol l1('l',1), l2('l',2); Symbol l1('l', 1), l2('l', 2);
// constant constraint on x1 // constant constraint on x1
Point2 pose1(1.0, 1.0); Point2 pose1(1.0, 1.0);
@ -488,13 +497,14 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
// create an initial estimate // create an initial estimate
Values initialEstimate; Values initialEstimate;
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
// optimize // optimize
Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); Values actual =
LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
Values expected; Values expected;
expected.insert(x1, Point2(1.0, 1.0)); expected.insert(x1, Point2(1.0, 1.0));
@ -506,8 +516,8 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
// make a realistic calibration matrix // make a realistic calibration matrix
static double fov = 60; // degrees static double fov = 60; // degrees
static int w=640,h=480; static int w = 640, h = 480;
static Cal3_S2 K(fov,w,h); static Cal3_S2 K(fov, w, h);
static boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K)); static boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
// typedefs for visual SLAM example // typedefs for visual SLAM example
@ -516,14 +526,12 @@ typedef NonlinearFactorGraph VGraph;
// factors for visual slam // factors for visual slam
typedef NonlinearEquality2<Point3> Point3Equality; typedef NonlinearEquality2<Point3> Point3Equality;
/* ********************************************************************* */ //******************************************************************************
TEST (testNonlinearEqualityConstraint, stereo_constrained ) { TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
// create initial estimates // create initial estimates
Rot3 faceDownY((Matrix)(Matrix(3,3) << Rot3 faceDownY(
1.0, 0.0, 0.0, (Matrix) (Matrix(3, 3) << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0).finished());
0.0, 0.0, 1.0,
0.0, -1.0, 0.0).finished());
Pose3 pose1(faceDownY, Point3()); // origin, left camera Pose3 pose1(faceDownY, Point3()); // origin, left camera
SimpleCamera camera1(pose1, K); SimpleCamera camera1(pose1, K);
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
@ -531,8 +539,8 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away
// keys // keys
Symbol x1('x',1), x2('x',2); Symbol x1('x', 1), x2('x', 2);
Symbol l1('l',1), l2('l',2); Symbol l1('l', 1), l2('l', 2);
// create graph // create graph
VGraph graph; VGraph graph;
@ -543,8 +551,10 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
// create factors // create factors
SharedDiagonal vmodel = noiseModel::Unit::Create(2); SharedDiagonal vmodel = noiseModel::Unit::Create(2);
graph += GenericProjectionFactor<Pose3,Point3,Cal3_S2>(camera1.project(landmark), vmodel, x1, l1, shK); graph += GenericProjectionFactor<Pose3, Point3, Cal3_S2>(
graph += GenericProjectionFactor<Pose3,Point3,Cal3_S2>(camera2.project(landmark), vmodel, x2, l2, shK); camera1.project(landmark), vmodel, x1, l1, shK);
graph += GenericProjectionFactor<Pose3, Point3, Cal3_S2>(
camera2.project(landmark), vmodel, x2, l2, shK);
// add equality constraint // add equality constraint
graph += Point3Equality(l1, l2); graph += Point3Equality(l1, l2);
@ -573,6 +583,9 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
CHECK(assert_equal(truthValues, actual, 1e-5)); CHECK(assert_equal(truthValues, actual, 1e-5));
} }
/* ************************************************************************* */ //******************************************************************************
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
/* ************************************************************************* */ TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************