Formatting
dellaert
parent
915c760524
commit
084de3350e
|
|
@ -26,280 +26,311 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Template default compare function that assumes a testable T
|
||||
*/
|
||||
template<class T>
|
||||
bool compare(const T& a, const T& b) {
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(T);
|
||||
return a.equals(b);
|
||||
}
|
||||
|
||||
/**
|
||||
* An equality factor that forces either one variable to a constant,
|
||||
* or a set of variables to be equal to each other.
|
||||
*
|
||||
* Depending on flag, throws an error at linearization if the constraints are not met.
|
||||
*
|
||||
* Switchable implementation:
|
||||
* - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain
|
||||
* - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error
|
||||
*
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class VALUE>
|
||||
class NonlinearEquality: public NoiseModelFactor1<VALUE> {
|
||||
|
||||
public:
|
||||
typedef VALUE T;
|
||||
|
||||
private:
|
||||
|
||||
// feasible value
|
||||
T feasible_;
|
||||
|
||||
// error handling flag
|
||||
bool allow_error_;
|
||||
|
||||
// error gain in allow error case
|
||||
double error_gain_;
|
||||
|
||||
// typedef to this class
|
||||
typedef NonlinearEquality<VALUE> This;
|
||||
|
||||
// typedef to base class
|
||||
typedef NoiseModelFactor1<VALUE> Base;
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* Template default compare function that assumes a testable T
|
||||
* Function that compares two values
|
||||
*/
|
||||
template<class T>
|
||||
bool compare(const T& a, const T& b) {
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(T);
|
||||
return a.equals(b);
|
||||
bool (*compare_)(const T& a, const T& b);
|
||||
|
||||
/** default constructor - only for serialization */
|
||||
NonlinearEquality() {
|
||||
}
|
||||
|
||||
virtual ~NonlinearEquality() {
|
||||
}
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Constructor - forces exact evaluation
|
||||
*/
|
||||
NonlinearEquality(Key j, const T& feasible,
|
||||
bool (*_compare)(const T&, const T&) = compare<T>) :
|
||||
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(
|
||||
feasible), allow_error_(false), error_gain_(0.0), compare_(_compare) {
|
||||
}
|
||||
|
||||
/**
|
||||
* An equality factor that forces either one variable to a constant,
|
||||
* or a set of variables to be equal to each other.
|
||||
*
|
||||
* Depending on flag, throws an error at linearization if the constraints are not met.
|
||||
*
|
||||
* Switchable implementation:
|
||||
* - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain
|
||||
* - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error
|
||||
*
|
||||
* \nosubgrouping
|
||||
* Constructor - allows inexact evaluation
|
||||
*/
|
||||
template<class VALUE>
|
||||
class NonlinearEquality: public NoiseModelFactor1<VALUE> {
|
||||
NonlinearEquality(Key j, const T& feasible, double error_gain,
|
||||
bool (*_compare)(const T&, const T&) = compare<T>) :
|
||||
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(
|
||||
feasible), allow_error_(true), error_gain_(error_gain), compare_(
|
||||
_compare) {
|
||||
}
|
||||
|
||||
public:
|
||||
typedef VALUE T;
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
private:
|
||||
virtual void print(const std::string& s = "",
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n";
|
||||
gtsam::print(feasible_, "Feasible Point:\n");
|
||||
std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
|
||||
}
|
||||
|
||||
// feasible value
|
||||
T feasible_;
|
||||
/** Check if two factors are equal */
|
||||
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
|
||||
const This* e = dynamic_cast<const This*>(&f);
|
||||
return e && Base::equals(f) && feasible_.equals(e->feasible_, tol)
|
||||
&& fabs(error_gain_ - e->error_gain_) < tol;
|
||||
}
|
||||
|
||||
// error handling flag
|
||||
bool allow_error_;
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
// error gain in allow error case
|
||||
double error_gain_;
|
||||
|
||||
// typedef to this class
|
||||
typedef NonlinearEquality<VALUE> This;
|
||||
|
||||
// typedef to base class
|
||||
typedef NoiseModelFactor1<VALUE> Base;
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* Function that compares two values
|
||||
*/
|
||||
bool (*compare_)(const T& a, const T& b);
|
||||
|
||||
|
||||
/** default constructor - only for serialization */
|
||||
NonlinearEquality() {}
|
||||
|
||||
virtual ~NonlinearEquality() {}
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Constructor - forces exact evaluation
|
||||
*/
|
||||
NonlinearEquality(Key j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) :
|
||||
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
|
||||
allow_error_(false), error_gain_(0.0),
|
||||
compare_(_compare) {
|
||||
/** actual error function calculation */
|
||||
virtual double error(const Values& c) const {
|
||||
const T& xj = c.at<T>(this->key());
|
||||
Vector e = this->unwhitenedError(c);
|
||||
if (allow_error_ || !compare_(xj, feasible_)) {
|
||||
return error_gain_ * dot(e, e);
|
||||
} else {
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor - allows inexact evaluation
|
||||
*/
|
||||
NonlinearEquality(Key j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) :
|
||||
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
|
||||
allow_error_(true), error_gain_(error_gain),
|
||||
compare_(_compare) {
|
||||
/** error function */
|
||||
Vector evaluateError(const T& xj,
|
||||
boost::optional<Matrix&> H = boost::none) const {
|
||||
size_t nj = feasible_.dim();
|
||||
if (allow_error_) {
|
||||
if (H)
|
||||
*H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
|
||||
return xj.localCoordinates(feasible_);
|
||||
} else if (compare_(feasible_, xj)) {
|
||||
if (H)
|
||||
*H = eye(nj);
|
||||
return zero(nj); // set error to zero if equal
|
||||
} else {
|
||||
if (H)
|
||||
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
|
||||
}
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
// Linearize is over-written, because base linearization tries to whiten
|
||||
virtual GaussianFactor::shared_ptr linearize(const Values& x) const {
|
||||
const T& xj = x.at<T>(this->key());
|
||||
Matrix A;
|
||||
Vector b = evaluateError(xj, A);
|
||||
SharedDiagonal model = noiseModel::Constrained::All(b.size());
|
||||
return GaussianFactor::shared_ptr(
|
||||
new JacobianFactor(this->key(), A, b, model));
|
||||
}
|
||||
|
||||
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n";
|
||||
gtsam::print(feasible_,"Feasible Point:\n");
|
||||
std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
|
||||
}
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
/** Check if two factors are equal */
|
||||
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
|
||||
const This* e = dynamic_cast<const This*>(&f);
|
||||
return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) &&
|
||||
fabs(error_gain_ - e->error_gain_) < tol;
|
||||
}
|
||||
/// @}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
private:
|
||||
|
||||
/** actual error function calculation */
|
||||
virtual double error(const Values& c) const {
|
||||
const T& xj = c.at<T>(this->key());
|
||||
Vector e = this->unwhitenedError(c);
|
||||
if (allow_error_ || !compare_(xj, feasible_)) {
|
||||
return error_gain_ * dot(e,e);
|
||||
} else {
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar
|
||||
& boost::serialization::make_nvp("NoiseModelFactor1",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(feasible_);
|
||||
ar & BOOST_SERIALIZATION_NVP(allow_error_);
|
||||
ar & BOOST_SERIALIZATION_NVP(error_gain_);
|
||||
}
|
||||
|
||||
/** error function */
|
||||
Vector evaluateError(const T& xj, boost::optional<Matrix&> H = boost::none) const {
|
||||
size_t nj = feasible_.dim();
|
||||
if (allow_error_) {
|
||||
if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
|
||||
return xj.localCoordinates(feasible_);
|
||||
} else if (compare_(feasible_,xj)) {
|
||||
if (H) *H = eye(nj);
|
||||
return zero(nj); // set error to zero if equal
|
||||
} else {
|
||||
if (H) 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
|
||||
}
|
||||
}
|
||||
};
|
||||
// \class NonlinearEquality
|
||||
|
||||
// Linearize is over-written, because base linearization tries to whiten
|
||||
virtual GaussianFactor::shared_ptr linearize(const Values& x) const {
|
||||
const T& xj = x.at<T>(this->key());
|
||||
Matrix A;
|
||||
Vector b = evaluateError(xj, A);
|
||||
SharedDiagonal model = noiseModel::Constrained::All(b.size());
|
||||
return GaussianFactor::shared_ptr(new JacobianFactor(this->key(), A, b, model));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Simple unary equality constraint - fixes a value for a variable
|
||||
*/
|
||||
template<class VALUE>
|
||||
class NonlinearEquality1: public NoiseModelFactor1<VALUE> {
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
||||
public:
|
||||
typedef VALUE X;
|
||||
|
||||
/// @}
|
||||
protected:
|
||||
typedef NoiseModelFactor1<VALUE> Base;
|
||||
typedef NonlinearEquality1<VALUE> This;
|
||||
|
||||
private:
|
||||
/** default constructor to allow for serialization */
|
||||
NonlinearEquality1() {
|
||||
}
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor1",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(feasible_);
|
||||
ar & BOOST_SERIALIZATION_NVP(allow_error_);
|
||||
ar & BOOST_SERIALIZATION_NVP(error_gain_);
|
||||
}
|
||||
X value_; /// fixed value for variable
|
||||
|
||||
}; // \class NonlinearEquality
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(X)
|
||||
;GTSAM_CONCEPT_TESTABLE_TYPE(X)
|
||||
;
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Simple unary equality constraint - fixes a value for a variable
|
||||
*/
|
||||
template<class VALUE>
|
||||
class NonlinearEquality1 : public NoiseModelFactor1<VALUE> {
|
||||
public:
|
||||
|
||||
public:
|
||||
typedef VALUE X;
|
||||
typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
|
||||
|
||||
protected:
|
||||
typedef NoiseModelFactor1<VALUE> Base;
|
||||
typedef NonlinearEquality1<VALUE> This;
|
||||
///TODO: comment
|
||||
NonlinearEquality1(const X& value, Key key1, double mu = 1000.0) :
|
||||
Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(
|
||||
value) {
|
||||
}
|
||||
|
||||
/** default constructor to allow for serialization */
|
||||
NonlinearEquality1() {}
|
||||
virtual ~NonlinearEquality1() {
|
||||
}
|
||||
|
||||
X value_; /// fixed value for variable
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(X);
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(X);
|
||||
/** g(x) with optional derivative */
|
||||
Vector evaluateError(const X& x1,
|
||||
boost::optional<Matrix&> H = boost::none) const {
|
||||
if (H)
|
||||
(*H) = eye(x1.dim());
|
||||
// manifold equivalent of h(x)-z -> log(z,h(x))
|
||||
return value_.localCoordinates(x1);
|
||||
}
|
||||
|
||||
public:
|
||||
/** Print */
|
||||
virtual void print(const std::string& s = "",
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
|
||||
<< ")," << "\n";
|
||||
this->noiseModel_->print();
|
||||
value_.print("Value");
|
||||
}
|
||||
|
||||
typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
|
||||
private:
|
||||
|
||||
///TODO: comment
|
||||
NonlinearEquality1(const X& value, Key key1, double mu = 1000.0)
|
||||
: Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar
|
||||
& boost::serialization::make_nvp("NoiseModelFactor1",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(value_);
|
||||
}
|
||||
};
|
||||
// \NonlinearEquality1
|
||||
|
||||
virtual ~NonlinearEquality1() {}
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Simple binary equality constraint - this constraint forces two factors to
|
||||
* be the same.
|
||||
*/
|
||||
template<class VALUE>
|
||||
class NonlinearEquality2: public NoiseModelFactor2<VALUE, VALUE> {
|
||||
public:
|
||||
typedef VALUE X;
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
||||
protected:
|
||||
typedef NoiseModelFactor2<VALUE, VALUE> Base;
|
||||
typedef NonlinearEquality2<VALUE> This;
|
||||
|
||||
/** g(x) with optional derivative */
|
||||
Vector evaluateError(const X& x1, boost::optional<Matrix&> H = boost::none) const {
|
||||
if (H) (*H) = eye(x1.dim());
|
||||
// manifold equivalent of h(x)-z -> log(z,h(x))
|
||||
return value_.localCoordinates(x1);
|
||||
}
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(X)
|
||||
;
|
||||
|
||||
/** Print */
|
||||
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
std::cout << s << ": NonlinearEquality1("
|
||||
<< keyFormatter(this->key()) << "),"<< "\n";
|
||||
this->noiseModel_->print();
|
||||
value_.print("Value");
|
||||
}
|
||||
/** default constructor to allow for serialization */
|
||||
NonlinearEquality2() {
|
||||
}
|
||||
|
||||
private:
|
||||
public:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor1",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(value_);
|
||||
}
|
||||
}; // \NonlinearEquality1
|
||||
typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr;
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Simple binary equality constraint - this constraint forces two factors to
|
||||
* be the same.
|
||||
*/
|
||||
template<class VALUE>
|
||||
class NonlinearEquality2 : public NoiseModelFactor2<VALUE, VALUE> {
|
||||
public:
|
||||
typedef VALUE X;
|
||||
///TODO: comment
|
||||
NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) :
|
||||
Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {
|
||||
}
|
||||
virtual ~NonlinearEquality2() {
|
||||
}
|
||||
|
||||
protected:
|
||||
typedef NoiseModelFactor2<VALUE, VALUE> Base;
|
||||
typedef NonlinearEquality2<VALUE> This;
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(X);
|
||||
/** g(x) with optional derivative2 */
|
||||
Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 =
|
||||
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
|
||||
const size_t p = X::Dim();
|
||||
if (H1)
|
||||
*H1 = -eye(p);
|
||||
if (H2)
|
||||
*H2 = eye(p);
|
||||
return x1.localCoordinates(x2);
|
||||
}
|
||||
|
||||
/** default constructor to allow for serialization */
|
||||
NonlinearEquality2() {}
|
||||
private:
|
||||
|
||||
public:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar
|
||||
& boost::serialization::make_nvp("NoiseModelFactor2",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
};
|
||||
// \NonlinearEquality2
|
||||
|
||||
typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr;
|
||||
|
||||
///TODO: comment
|
||||
NonlinearEquality2(Key key1, Key key2, double mu = 1000.0)
|
||||
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
|
||||
virtual ~NonlinearEquality2() {}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
||||
|
||||
/** g(x) with optional derivative2 */
|
||||
Vector evaluateError(const X& x1, const X& x2,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const {
|
||||
const size_t p = X::Dim();
|
||||
if (H1) *H1 = -eye(p);
|
||||
if (H2) *H2 = eye(p);
|
||||
return x1.localCoordinates(x2);
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor2",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
}; // \NonlinearEquality2
|
||||
|
||||
} // namespace gtsam
|
||||
}// namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -42,9 +42,9 @@ typedef PriorFactor<Pose2> PosePrior;
|
|||
typedef NonlinearEquality<Pose2> PoseNLE;
|
||||
typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
|
||||
|
||||
static Symbol key('x',1);
|
||||
static Symbol key('x', 1);
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST ( NonlinearEquality, linearization ) {
|
||||
Pose2 value = Pose2(2.1, 1.0, 2.0);
|
||||
Values linearize;
|
||||
|
|
@ -60,10 +60,10 @@ TEST ( NonlinearEquality, linearization ) {
|
|||
EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF));
|
||||
}
|
||||
|
||||
/* ********************************************************************** */
|
||||
//******************************************************************************
|
||||
TEST ( NonlinearEquality, linearization_pose ) {
|
||||
|
||||
Symbol key('x',1);
|
||||
Symbol key('x', 1);
|
||||
Pose2 value;
|
||||
Values config;
|
||||
config.insert(key, value);
|
||||
|
|
@ -75,7 +75,7 @@ TEST ( NonlinearEquality, linearization_pose ) {
|
|||
EXPECT(true);
|
||||
}
|
||||
|
||||
/* ********************************************************************** */
|
||||
//******************************************************************************
|
||||
TEST ( NonlinearEquality, linearization_fail ) {
|
||||
Pose2 value = Pose2(2.1, 1.0, 2.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);
|
||||
}
|
||||
|
||||
/* ********************************************************************** */
|
||||
//******************************************************************************
|
||||
TEST ( NonlinearEquality, linearization_fail_pose ) {
|
||||
|
||||
Symbol key('x',1);
|
||||
Pose2 value(2.0, 1.0, 2.0),
|
||||
wrong(2.0, 3.0, 4.0);
|
||||
Symbol key('x', 1);
|
||||
Pose2 value(2.0, 1.0, 2.0), wrong(2.0, 3.0, 4.0);
|
||||
Values bad_linearize;
|
||||
bad_linearize.insert(key, wrong);
|
||||
|
||||
|
|
@ -105,12 +104,11 @@ TEST ( NonlinearEquality, linearization_fail_pose ) {
|
|||
CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument);
|
||||
}
|
||||
|
||||
/* ********************************************************************** */
|
||||
//******************************************************************************
|
||||
TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
|
||||
|
||||
Symbol key('x',1);
|
||||
Pose2 value,
|
||||
wrong(2.0, 3.0, 4.0);
|
||||
Symbol key('x', 1);
|
||||
Pose2 value, wrong(2.0, 3.0, 4.0);
|
||||
Values bad_linearize;
|
||||
bad_linearize.insert(key, wrong);
|
||||
|
||||
|
|
@ -121,7 +119,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
|
|||
CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST ( NonlinearEquality, error ) {
|
||||
Pose2 value = Pose2(2.1, 1.0, 2.0);
|
||||
Pose2 wrong = Pose2(2.1, 3.0, 4.0);
|
||||
|
|
@ -137,10 +135,11 @@ TEST ( NonlinearEquality, error ) {
|
|||
EXPECT(assert_equal(actual, zero(3)));
|
||||
|
||||
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 ) {
|
||||
Pose2 value1 = Pose2(2.1, 1.0, 2.0);
|
||||
Pose2 value2 = Pose2(2.1, 3.0, 4.0);
|
||||
|
|
@ -151,14 +150,17 @@ TEST ( NonlinearEquality, equals ) {
|
|||
shared_poseNLE nle3(new PoseNLE(key, value2));
|
||||
|
||||
// verify
|
||||
EXPECT(nle1->equals(*nle2)); // basic equality = true
|
||||
EXPECT(nle2->equals(*nle1)); // test symmetry of equals()
|
||||
EXPECT(!nle1->equals(*nle3)); // test config
|
||||
EXPECT(nle1->equals(*nle2));
|
||||
// basic equality = true
|
||||
EXPECT(nle2->equals(*nle1));
|
||||
// test symmetry of equals()
|
||||
EXPECT(!nle1->equals(*nle3));
|
||||
// test config
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST ( NonlinearEquality, allow_error_pose ) {
|
||||
Symbol key1('x',1);
|
||||
Symbol key1('x', 1);
|
||||
Pose2 feasible1(1.0, 2.0, 3.0);
|
||||
double error_gain = 500.0;
|
||||
PoseNLE nle(key1, feasible1, error_gain);
|
||||
|
|
@ -177,16 +179,17 @@ TEST ( NonlinearEquality, allow_error_pose ) {
|
|||
|
||||
// check linearization
|
||||
GaussianFactor::shared_ptr actLinFactor = nle.linearize(config);
|
||||
Matrix A1 = eye(3,3);
|
||||
Matrix A1 = eye(3, 3);
|
||||
Vector b = expVec;
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST ( NonlinearEquality, allow_error_optimize ) {
|
||||
Symbol key1('x',1);
|
||||
Symbol key1('x', 1);
|
||||
Pose2 feasible1(1.0, 2.0, 3.0);
|
||||
double error_gain = 500.0;
|
||||
PoseNLE nle(key1, feasible1, error_gain);
|
||||
|
|
@ -211,11 +214,11 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
|
|||
EXPECT(assert_equal(expected, result));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
|
||||
|
||||
// create a hard constraint
|
||||
Symbol key1('x',1);
|
||||
Symbol key1('x', 1);
|
||||
Pose2 feasible1(1.0, 2.0, 3.0);
|
||||
|
||||
// initialize away from the ideal
|
||||
|
|
@ -245,14 +248,14 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
|
|||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
static SharedDiagonal hard_model = noiseModel::Constrained::All(2);
|
||||
static SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST( testNonlinearEqualityConstraint, unary_basics ) {
|
||||
Point2 pt(1.0, 2.0);
|
||||
Symbol key1('x',1);
|
||||
Symbol key1('x', 1);
|
||||
double mu = 1000.0;
|
||||
eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
|
||||
|
||||
|
|
@ -267,38 +270,42 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) {
|
|||
Point2 ptBad1(2.0, 2.0);
|
||||
config2.insert(key, ptBad1);
|
||||
EXPECT(constraint.active(config2));
|
||||
EXPECT(assert_equal(Vector2(1.0, 0.0), constraint.evaluateError(ptBad1), tol));
|
||||
EXPECT(assert_equal(Vector2(1.0, 0.0), constraint.unwhitenedError(config2), tol));
|
||||
EXPECT(
|
||||
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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST( testNonlinearEqualityConstraint, unary_linearization ) {
|
||||
Point2 pt(1.0, 2.0);
|
||||
Symbol key1('x',1);
|
||||
Symbol key1('x', 1);
|
||||
double mu = 1000.0;
|
||||
eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
|
||||
|
||||
Values config1;
|
||||
config1.insert(key, pt);
|
||||
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));
|
||||
|
||||
Values config2;
|
||||
Point2 ptBad(2.0, 2.0);
|
||||
config2.insert(key, ptBad);
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
|
||||
// create a single-node graph with a soft and hard constraint to
|
||||
// ensure that the hard constraint overrides the soft constraint
|
||||
Point2 truth_pt(1.0, 2.0);
|
||||
Symbol key('x',1);
|
||||
Symbol key('x', 1);
|
||||
double mu = 10.0;
|
||||
eq2D::UnaryEqualityConstraint::shared_ptr constraint(
|
||||
new eq2D::UnaryEqualityConstraint(truth_pt, key, mu));
|
||||
|
|
@ -326,10 +333,10 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
|
|||
EXPECT(assert_equal(expected, actual, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST( testNonlinearEqualityConstraint, odo_basics ) {
|
||||
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;
|
||||
eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
|
||||
|
||||
|
|
@ -347,15 +354,17 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) {
|
|||
config2.insert(key1, x1bad);
|
||||
config2.insert(key2, x2bad);
|
||||
EXPECT(constraint.active(config2));
|
||||
EXPECT(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(
|
||||
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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST( testNonlinearEqualityConstraint, odo_linearization ) {
|
||||
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;
|
||||
eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
|
||||
|
||||
|
|
@ -364,8 +373,8 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
|
|||
config1.insert(key2, x2);
|
||||
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
|
||||
GaussianFactor::shared_ptr expected1(
|
||||
new JacobianFactor(key1, -eye(2,2), key2,
|
||||
eye(2,2), zero(2), hard_model));
|
||||
new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), zero(2),
|
||||
hard_model));
|
||||
EXPECT(assert_equal(*expected1, *actual1, tol));
|
||||
|
||||
Values config2;
|
||||
|
|
@ -375,18 +384,18 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
|
|||
config2.insert(key2, x2bad);
|
||||
GaussianFactor::shared_ptr actual2 = constraint.linearize(config2);
|
||||
GaussianFactor::shared_ptr expected2(
|
||||
new JacobianFactor(key1, -eye(2,2), key2,
|
||||
eye(2,2), Vector2(1.0, 1.0), hard_model));
|
||||
new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), Vector2(1.0, 1.0),
|
||||
hard_model));
|
||||
EXPECT(assert_equal(*expected2, *actual2, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
|
||||
// create a two-node graph, connected by an odometry constraint, with
|
||||
// a hard prior on one variable, and a conflicting soft prior
|
||||
// on the other variable - the constraints should override the soft constraint
|
||||
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
|
||||
eq2D::UnaryEqualityConstraint::shared_ptr constraint1(
|
||||
|
|
@ -399,8 +408,8 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
|
|||
|
||||
// odometry constraint
|
||||
eq2D::OdoEqualityConstraint::shared_ptr constraint2(
|
||||
new eq2D::OdoEqualityConstraint(
|
||||
truth_pt1.between(truth_pt2), key1, key2));
|
||||
new eq2D::OdoEqualityConstraint(truth_pt1.between(truth_pt2), key1,
|
||||
key2));
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(constraint1);
|
||||
|
|
@ -418,7 +427,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
|
|||
CHECK(assert_equal(expected, actual, tol));
|
||||
}
|
||||
|
||||
/* ********************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST (testNonlinearEqualityConstraint, two_pose ) {
|
||||
/*
|
||||
* Determining a ground truth linear system
|
||||
|
|
@ -428,19 +437,18 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
|
|||
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
Symbol x1('x',1), x2('x',2);
|
||||
Symbol l1('l',1), l2('l',2);
|
||||
Point2 pt_x1(1.0, 1.0),
|
||||
pt_x2(5.0, 6.0);
|
||||
Symbol x1('x', 1), x2('x', 2);
|
||||
Symbol l1('l', 1), l2('l', 2);
|
||||
Point2 pt_x1(1.0, 1.0), pt_x2(5.0, 6.0);
|
||||
graph += eq2D::UnaryEqualityConstraint(pt_x1, x1);
|
||||
graph += eq2D::UnaryEqualityConstraint(pt_x2, x2);
|
||||
|
||||
Point2 z1(0.0, 5.0);
|
||||
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);
|
||||
graph += simulated2D::Measurement(z2, sigma, x2,l2);
|
||||
graph += simulated2D::Measurement(z2, sigma, x2, 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(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;
|
||||
expected.insert(x1, pt_x1);
|
||||
|
|
@ -460,14 +469,14 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
|
|||
CHECK(assert_equal(expected, actual, 1e-5));
|
||||
}
|
||||
|
||||
/* ********************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST (testNonlinearEqualityConstraint, map_warp ) {
|
||||
// get a graph
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
// keys
|
||||
Symbol x1('x',1), x2('x',2);
|
||||
Symbol l1('l',1), l2('l',2);
|
||||
Symbol x1('x', 1), x2('x', 2);
|
||||
Symbol l1('l', 1), l2('l', 2);
|
||||
|
||||
// constant constraint on x1
|
||||
Point2 pose1(1.0, 1.0);
|
||||
|
|
@ -488,13 +497,14 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
|
|||
|
||||
// create an initial estimate
|
||||
Values initialEstimate;
|
||||
initialEstimate.insert(x1, Point2( 1.0, 1.0));
|
||||
initialEstimate.insert(l1, Point2( 1.0, 6.0));
|
||||
initialEstimate.insert(x1, Point2(1.0, 1.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(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
|
||||
Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
|
||||
Values actual =
|
||||
LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
|
||||
|
||||
Values expected;
|
||||
expected.insert(x1, Point2(1.0, 1.0));
|
||||
|
|
@ -506,8 +516,8 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
|
|||
|
||||
// make a realistic calibration matrix
|
||||
static double fov = 60; // degrees
|
||||
static int w=640,h=480;
|
||||
static Cal3_S2 K(fov,w,h);
|
||||
static int w = 640, h = 480;
|
||||
static Cal3_S2 K(fov, w, h);
|
||||
static boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
|
||||
|
||||
// typedefs for visual SLAM example
|
||||
|
|
@ -516,14 +526,12 @@ typedef NonlinearFactorGraph VGraph;
|
|||
// factors for visual slam
|
||||
typedef NonlinearEquality2<Point3> Point3Equality;
|
||||
|
||||
/* ********************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
|
||||
|
||||
// create initial estimates
|
||||
Rot3 faceDownY((Matrix)(Matrix(3,3) <<
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 0.0, 1.0,
|
||||
0.0, -1.0, 0.0).finished());
|
||||
Rot3 faceDownY(
|
||||
(Matrix) (Matrix(3, 3) << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0).finished());
|
||||
Pose3 pose1(faceDownY, Point3()); // origin, left camera
|
||||
SimpleCamera camera1(pose1, K);
|
||||
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
|
||||
|
||||
// keys
|
||||
Symbol x1('x',1), x2('x',2);
|
||||
Symbol l1('l',1), l2('l',2);
|
||||
Symbol x1('x', 1), x2('x', 2);
|
||||
Symbol l1('l', 1), l2('l', 2);
|
||||
|
||||
// create graph
|
||||
VGraph graph;
|
||||
|
|
@ -543,8 +551,10 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
|
|||
|
||||
// create factors
|
||||
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>(camera2.project(landmark), vmodel, x2, l2, shK);
|
||||
graph += GenericProjectionFactor<Pose3, Point3, Cal3_S2>(
|
||||
camera1.project(landmark), vmodel, x1, l1, shK);
|
||||
graph += GenericProjectionFactor<Pose3, Point3, Cal3_S2>(
|
||||
camera2.project(landmark), vmodel, x2, l2, shK);
|
||||
|
||||
// add equality constraint
|
||||
graph += Point3Equality(l1, l2);
|
||||
|
|
@ -573,6 +583,9 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
|
|||
CHECK(assert_equal(truthValues, actual, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
//******************************************************************************
|
||||
|
|
|
|||
Loading…
Reference in New Issue